152 lines
3.8 KiB
C++
152 lines
3.8 KiB
C++
|
/*
|
||
|
* Copyright (C) 2016 by Jonathan Naylor G4KLX
|
||
|
*
|
||
|
* This program is free software; you can redistribute it and/or modify
|
||
|
* it under the terms of the GNU General Public License as published by
|
||
|
* the Free Software Foundation; either version 2 of the License, or
|
||
|
* (at your option) any later version.
|
||
|
*
|
||
|
* This program is distributed in the hope that it will be useful,
|
||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
|
* GNU General Public License for more details.
|
||
|
*
|
||
|
* You should have received a copy of the GNU General Public License
|
||
|
* along with this program; if not, write to the Free Software
|
||
|
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
||
|
*/
|
||
|
|
||
|
#include "P25Data.h"
|
||
|
#include "Log.h"
|
||
|
|
||
|
const unsigned char BIT_MASK_TABLE[] = { 0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U };
|
||
|
|
||
|
#define WRITE_BIT(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7])
|
||
|
#define READ_BIT(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7])
|
||
|
|
||
|
CP25Data::CP25Data() :
|
||
|
m_source(0U),
|
||
|
m_group(true),
|
||
|
m_dest(0U)
|
||
|
{
|
||
|
}
|
||
|
|
||
|
CP25Data::~CP25Data()
|
||
|
{
|
||
|
}
|
||
|
|
||
|
void CP25Data::processHeader(unsigned char* data)
|
||
|
{
|
||
|
}
|
||
|
|
||
|
void CP25Data::processLDU1(unsigned char* data)
|
||
|
{
|
||
|
// XXX No FEC done yet
|
||
|
bool b[24U];
|
||
|
b[7U] = READ_BIT(data, 411U);
|
||
|
b[6U] = READ_BIT(data, 410U);
|
||
|
b[5U] = READ_BIT(data, 413U);
|
||
|
b[4U] = READ_BIT(data, 412U);
|
||
|
b[3U] = READ_BIT(data, 415U);
|
||
|
b[2U] = READ_BIT(data, 414U);
|
||
|
b[1U] = READ_BIT(data, 421U);
|
||
|
b[0U] = READ_BIT(data, 420U);
|
||
|
|
||
|
unsigned char format = 0U;
|
||
|
unsigned char mult = 1U;
|
||
|
for (unsigned int i = 0U; i < 8U; i++, mult <<= 1)
|
||
|
format += b[i] ? mult : 0U;
|
||
|
|
||
|
LogDebug("P25, LC_format = $%02X", format);
|
||
|
|
||
|
if (format == 0x03U) {
|
||
|
LogDebug("P25, non talk group destination");
|
||
|
m_group = false;
|
||
|
} else {
|
||
|
m_group = true;
|
||
|
|
||
|
b[15U] = READ_BIT(data, 613U); // 39
|
||
|
b[14U] = READ_BIT(data, 612U);
|
||
|
b[13U] = READ_BIT(data, 615U); // 37
|
||
|
b[12U] = READ_BIT(data, 614U);
|
||
|
b[11U] = READ_BIT(data, 621U); // 35
|
||
|
b[10U] = READ_BIT(data, 620U);
|
||
|
b[9U] = READ_BIT(data, 623U); // 33
|
||
|
b[8U] = READ_BIT(data, 622U);
|
||
|
b[7U] = READ_BIT(data, 625U); // 31
|
||
|
b[6U] = READ_BIT(data, 624U);
|
||
|
b[5U] = READ_BIT(data, 631U); // 29
|
||
|
b[4U] = READ_BIT(data, 630U);
|
||
|
b[3U] = READ_BIT(data, 633U); // 27
|
||
|
b[2U] = READ_BIT(data, 632U);
|
||
|
b[1U] = READ_BIT(data, 635U);
|
||
|
b[0U] = READ_BIT(data, 634U); // 24
|
||
|
|
||
|
mult = 1U;
|
||
|
for (unsigned int i = 0U; i < 16U; i++, mult <<= 1)
|
||
|
m_dest += b[i] ? mult : 0U;
|
||
|
|
||
|
LogDebug("P25, TG ID = %u", m_dest);
|
||
|
}
|
||
|
|
||
|
b[23U] = READ_BIT(data, 789U);
|
||
|
b[22U] = READ_BIT(data, 788U);
|
||
|
b[21U] = READ_BIT(data, 793U);
|
||
|
b[20U] = READ_BIT(data, 792U);
|
||
|
b[19U] = READ_BIT(data, 795U);
|
||
|
b[18U] = READ_BIT(data, 794U);
|
||
|
b[17U] = READ_BIT(data, 801U);
|
||
|
b[16U] = READ_BIT(data, 800U);
|
||
|
b[15U] = READ_BIT(data, 803U);
|
||
|
b[14U] = READ_BIT(data, 802U);
|
||
|
b[13U] = READ_BIT(data, 805U);
|
||
|
b[12U] = READ_BIT(data, 804U);
|
||
|
b[11U] = READ_BIT(data, 811U);
|
||
|
b[10U] = READ_BIT(data, 810U);
|
||
|
b[9U] = READ_BIT(data, 813U);
|
||
|
b[8U] = READ_BIT(data, 812U);
|
||
|
b[7U] = READ_BIT(data, 815U);
|
||
|
b[6U] = READ_BIT(data, 814U);
|
||
|
b[5U] = READ_BIT(data, 821U);
|
||
|
b[4U] = READ_BIT(data, 820U);
|
||
|
b[3U] = READ_BIT(data, 823U);
|
||
|
b[2U] = READ_BIT(data, 822U);
|
||
|
b[1U] = READ_BIT(data, 825U);
|
||
|
b[0U] = READ_BIT(data, 824U);
|
||
|
|
||
|
mult = 1U;
|
||
|
for (unsigned int i = 0U; i < 24U; i++, mult <<= 1)
|
||
|
m_source += b[i] ? mult : 0U;
|
||
|
|
||
|
LogDebug("P25, SRC ID = %u", m_source);
|
||
|
}
|
||
|
|
||
|
void CP25Data::processLDU2(unsigned char* data)
|
||
|
{
|
||
|
}
|
||
|
|
||
|
void CP25Data::processTerminator(unsigned char* data)
|
||
|
{
|
||
|
}
|
||
|
|
||
|
unsigned int CP25Data::getSource() const
|
||
|
{
|
||
|
return m_source;
|
||
|
}
|
||
|
|
||
|
bool CP25Data::getGroup() const
|
||
|
{
|
||
|
return m_group;
|
||
|
}
|
||
|
|
||
|
unsigned int CP25Data::getDest() const
|
||
|
{
|
||
|
return m_dest;
|
||
|
}
|
||
|
|
||
|
void CP25Data::reset()
|
||
|
{
|
||
|
m_source = 0U;
|
||
|
m_dest = 0U;
|
||
|
}
|