void CP25Control::setBusyBits(unsigned char* data, unsigned int ssOffset, bool b1, bool b2)
{
assert(data != NULL);
WRITE_BIT(data, ssOffset, b1);
WRITE_BIT(data, ssOffset + 1U, b2);
}
void CDMRTrellis::interleave(const signed char* dibits, unsigned char* data) const
{
for (unsigned int i = 0U; i < 98U; i++) {
unsigned int n = INTERLEAVE_TABLE[i];
bool b1, b2;
switch (dibits[n]) {
case +3:
b1 = false;
b2 = true;
break;
case +1:
b1 = false;
b2 = false;
break;
case -1:
b1 = true;
b2 = false;
break;
default:
b1 = true;
b2 = true;
break;
}
n = i * 2U + 0U;
if (n >= 98U) n += 68U;
WRITE_BIT(data, n, b1);
n = i * 2U + 1U;
if (n >= 98U) n += 68U;
WRITE_BIT(data, n, b2);
}
}
void CP25Control::addBusyBits(unsigned char* data, unsigned int length, bool b1, bool b2)
{
assert(data != NULL);
for (unsigned int ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += P25_SS_INCREMENT) {
unsigned int ss1Pos = ss0Pos + 1U;
WRITE_BIT(data, ss0Pos, b1);
WRITE_BIT(data, ss1Pos, b2);
}
}
unsigned int CP25Utils::encode(const unsigned char* in, unsigned char* out, unsigned int length)
{
assert(in != NULL);
assert(out != NULL);
// Move the SSx positions to the range needed
unsigned int ss0Pos = P25_SS0_START;
unsigned int ss1Pos = P25_SS1_START;
unsigned int n = 0U;
unsigned int pos = 0U;
while (n < length) {
if (pos == ss0Pos) {
ss0Pos += P25_SS_INCREMENT;
} else if (pos == ss1Pos) {
ss1Pos += P25_SS_INCREMENT;
} else {
bool b = READ_BIT(in, n);
WRITE_BIT(out, pos, b);
n++;
}
pos++;
}
return pos;
}
unsigned int CP25Utils::encode(const unsigned char* in, unsigned char* out, unsigned int start, unsigned int stop)
{
assert(in != NULL);
assert(out != NULL);
// Move the SSx positions to the range needed
unsigned int ss0Pos = P25_SS0_START;
unsigned int ss1Pos = P25_SS1_START;
while (ss0Pos < start) {
ss0Pos += P25_SS_INCREMENT;
ss1Pos += P25_SS_INCREMENT;
}
unsigned int n = 0U;
for (unsigned int i = start; i < stop; i++) {
if (i == ss0Pos) {
ss0Pos += P25_SS_INCREMENT;
} else if (i == ss1Pos) {
ss1Pos += P25_SS_INCREMENT;
} else {
bool b = READ_BIT(in, n);
WRITE_BIT(out, i, b);
n++;
}
}
return n;
}
void CDMRTrellis::tribitsToBits(const unsigned char* tribits, unsigned char* payload) const
{
for (unsigned int i = 0U; i < 48U; i++) {
unsigned char tribit = tribits[i];
bool b1 = (tribit & 0x04U) == 0x04U;
bool b2 = (tribit & 0x02U) == 0x02U;
bool b3 = (tribit & 0x01U) == 0x01U;
unsigned int n = 143U - i * 3U;
WRITE_BIT(payload, n, b1);
n--;
WRITE_BIT(payload, n, b2);
n--;
WRITE_BIT(payload, n, b3);
}
}
void ProbabilityCoder::WriteSymbol(unsigned int symbol){
if ( max_val <= 1 ){
assert(symbol == 1);
return;
}
unsigned int new_max=0;
for ( unsigned int a=1;a<=max_val;a<<=1){
WRITE_BIT(symbol&a);
if ( symbol&a ){
new_max = a;
}
}
max_val = new_max;
}
void CDMRRX::processCACH(const unsigned char* buffer)
{
bool word[7U];
word[0U] = (buffer[0U] & 0x80U) == 0x80U;
word[1U] = (buffer[0U] & 0x08U) == 0x08U;
word[2U] = (buffer[1U] & 0x80U) == 0x80U;
word[3U] = (buffer[1U] & 0x08U) == 0x08U;
word[4U] = (buffer[1U] & 0x02U) == 0x02U;
word[5U] = (buffer[2U] & 0x20U) == 0x20U;
word[6U] = (buffer[2U] & 0x02U) == 0x02U;
CHamming::decode743(word);
if (!word[2U] && word[3U])
m_shortN = 0U;
else if (word[2U] && !word[3U])
m_shortN = 3U;
for (unsigned int i = 0U; i < 17U; i++) {
unsigned int m = CACH_INTERLEAVE[i];
bool b = READ_BIT(buffer, m) != 0x00U;
unsigned int n = i + (m_shortN * 17U);
WRITE_BIT(m_shortLC, n, b);
}
if (word[2U] && !word[3U]) {
unsigned char lc[5U];
CDMRShortLC shortLC;
bool valid = shortLC.decode(m_shortLC, lc);
if (valid)
LogMessage(" [CACH] AT=%d TC=%d LCSS=%d%d %02X %02X %02X %02X %02X %02X %02X %02X %02X LC=%02X %02X %02X %02X %02X", word[0U] ? 1 : 0, word[1U] ? 1 : 0, word[2U] ? 1 : 0, word[3U] ? 1 : 0, m_shortLC[0U], m_shortLC[1U], m_shortLC[2U], m_shortLC[3U], m_shortLC[4U], m_shortLC[5U], m_shortLC[6U], m_shortLC[7U], m_shortLC[8U], lc[0U], lc[1U], lc[2U], lc[3U], lc[4U]);
else
LogMessage(" [CACH] AT=%d TC=%d LCSS=%d%d %02X %02X %02X %02X %02X %02X %02X %02X %02X <Invalid LC>", word[0U] ? 1 : 0, word[1U] ? 1 : 0, word[2U] ? 1 : 0, word[3U] ? 1 : 0, m_shortLC[0U], m_shortLC[1U], m_shortLC[2U], m_shortLC[3U], m_shortLC[4U], m_shortLC[5U], m_shortLC[6U], m_shortLC[7U], m_shortLC[8U]);
} else {
LogMessage(" [CACH] AT=%d TC=%d LCSS=%d%d", word[0U] ? 1 : 0, word[1U] ? 1 : 0, word[2U] ? 1 : 0, word[3U] ? 1 : 0);
}
m_shortN++;
if (m_shortN >= 4U)
m_shortN = 0U;
m_slotNo = word[1U] ? 2U : 1U;
}
void CBCH::encode(unsigned char* nid)
{
assert(nid != NULL);
int data[16];
for (int i = 0; i < 16; i++)
data[i] = READ_BIT(nid, i) ? 1 : 0;
int bb[63];
encode(data, bb);
for (int i = 0; i < (length - k); i++) {
bool b = bb[i] == 1;
WRITE_BIT(nid, i + 16U, b);
}
}
void CDMRRX::processBit(bool b)
{
WRITE_BIT(m_buffer, m_pos, b);
m_pos++;
if (m_pos == DMR_FRAME_LENGTH_BITS) {
// CUtils::dump("DMR Frame bytes", m_buffer, DMR_FRAME_LENGTH_BYTES);
switch (m_type) {
case SYNC_AUDIO: {
CAMBEFEC fec;
unsigned int ber = fec.regenerateDMR(m_buffer);
LogMessage("%u [Audio Sync] BER=%.1f%%", m_slotNo, float(ber) / 1.41F);
m_n = 0U;
if (m_socket != NULL && m_slotNo == m_udpSlot)
writeAMBE(m_buffer);
}
break;
case SYNC_DATA:
processDataSync(m_buffer);
break;
case SYNC_NONE:
processAudio(m_buffer);
break;
default:
break;
}
m_type = SYNC_NONE;
m_count++;
if (m_count >= 10U) {
if (m_idleBits == 0U) m_idleBits = 1U;
LogMessage("Signal lost, BER=%.1f%%", float(m_idleErrs * 100U) / float(m_idleBits));
m_receiving = false;
m_idleBits = 0U;
m_idleErrs = 0U;
}
}
if (m_pos == (DMR_FRAME_LENGTH_BITS + DMR_CACH_LENGTH_BITS)) {
// CUtils::dump("DMR CACH bytes", m_buffer + DMR_FRAME_LENGTH_BYTES, DMR_CACH_LENGTH_BYTES);
processCACH(m_buffer + DMR_FRAME_LENGTH_BYTES);
m_pos = 0U;
}
}
void *
break_outguess_prepare(short *dcts, int bits)
{
struct ogobj *ogob;
int i, j, max;
short val;
ogob = malloc(sizeof(struct ogobj));
if (ogob == NULL)
err(1, "malloc");
ogob->bits = bits;
max = sizeof(ogob->coeff) * 8;
j = 0;
for (i = 0; i < bits && j < max; i++) {
val = dcts[i];
WRITE_BIT(ogob->coeff, j, (val & 0x01));
j++;
}
return (ogob);
}
bool CP25Control::writeModem(unsigned char* data, unsigned int len)
{
assert(data != NULL);
bool sync = data[1U] == 0x01U;
if (data[0U] == TAG_LOST && m_rfState == RS_RF_AUDIO) {
if (m_rssi != 0U)
LogMessage("P25, transmission lost, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount);
else
LogMessage("P25, transmission lost, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits));
if (m_netState == RS_NET_IDLE)
m_display->clearP25();
writeNetwork(m_rfLDU, m_lastDUID, true);
writeNetwork(data + 2U, P25_DUID_TERM, true);
m_rfState = RS_RF_LISTENING;
m_rfTimeout.stop();
m_rfData.reset();
#if defined(DUMP_P25)
closeFile();
#endif
return false;
}
if (data[0U] == TAG_LOST && m_rfState == RS_RF_DATA) {
if (m_netState == RS_NET_IDLE)
m_display->clearP25();
m_rfState = RS_RF_LISTENING;
m_rfPDUCount = 0U;
m_rfPDUBits = 0U;
#if defined(DUMP_P25)
closeFile();
#endif
return false;
}
if (data[0U] == TAG_LOST) {
m_rfState = RS_RF_LISTENING;
return false;
}
if (!sync && m_rfState == RS_RF_LISTENING)
return false;
// Decode the NID
bool valid = m_nid.decode(data + 2U);
if (m_rfState == RS_RF_LISTENING && !valid)
return false;
unsigned char duid = m_nid.getDUID();
if (!valid) {
switch (m_lastDUID) {
case P25_DUID_HEADER:
case P25_DUID_LDU2:
duid = P25_DUID_LDU1;
break;
case P25_DUID_LDU1:
duid = P25_DUID_LDU2;
break;
case P25_DUID_PDU:
duid = P25_DUID_PDU;
break;
case P25_DUID_TSDU:
duid = P25_DUID_TSDU;
break;
default:
break;
}
}
// Have we got RSSI bytes on the end of a P25 LDU?
if (len == (P25_LDU_FRAME_LENGTH_BYTES + 4U)) {
uint16_t raw = 0U;
raw |= (data[218U] << 8) & 0xFF00U;
raw |= (data[219U] << 0) & 0x00FFU;
// Convert the raw RSSI to dBm
int rssi = m_rssiMapper->interpolate(raw);
if (rssi != 0)
LogDebug("P25, raw RSSI: %u, reported RSSI: %d dBm", raw, rssi);
// RSSI is always reported as positive
m_rssi = (rssi >= 0) ? rssi : -rssi;
if (m_rssi > m_minRSSI)
m_minRSSI = m_rssi;
if (m_rssi < m_maxRSSI)
m_maxRSSI = m_rssi;
m_aveRSSI += m_rssi;
m_rssiCount++;
}
if (duid == P25_DUID_LDU1) {
if (m_rfState == RS_RF_LISTENING) {
m_rfData.reset();
bool ret = m_rfData.decodeLDU1(data + 2U);
if (!ret) {
m_lastDUID = duid;
return false;
}
unsigned int srcId = m_rfData.getSrcId();
if (m_selfOnly) {
if (m_id > 99999999U) { // Check that the Config DMR-ID is bigger than 8 digits
if (srcId != m_id / 100U)
return false;
}
else if (m_id > 9999999U) { // Check that the Config DMR-ID is bigger than 7 digits
if (srcId != m_id / 10U)
return false;
}
else if (srcId != m_id) { // All other cases
return false;
}
}
if (!m_uidOverride) {
bool found = m_lookup->exists(srcId);
if (!found)
return false;
}
bool grp = m_rfData.getLCF() == P25_LCF_GROUP;
unsigned int dstId = m_rfData.getDstId();
std::string source = m_lookup->find(srcId);
LogMessage("P25, received RF voice transmission from %s to %s%u", source.c_str(), grp ? "TG " : "", dstId);
m_display->writeP25(source.c_str(), grp, dstId, "R");
m_rfState = RS_RF_AUDIO;
m_minRSSI = m_rssi;
m_maxRSSI = m_rssi;
m_aveRSSI = m_rssi;
m_rssiCount = 1U;
createRFHeader();
writeNetwork(data + 2U, P25_DUID_HEADER, false);
} else if (m_rfState == RS_RF_AUDIO) {
writeNetwork(m_rfLDU, m_lastDUID, false);
}
if (m_rfState == RS_RF_AUDIO) {
// Regenerate Sync
CSync::addP25Sync(data + 2U);
// Regenerate NID
m_nid.encode(data + 2U, P25_DUID_LDU1);
// Regenerate LDU1 Data
m_rfData.encodeLDU1(data + 2U);
// Regenerate the Low Speed Data
m_rfLSD.process(data + 2U);
// Regenerate Audio
unsigned int errors = m_audio.process(data + 2U);
LogDebug("P25, LDU1 audio, errs: %u/1233 (%.1f%%)", errors, float(errors) / 12.33F);
m_display->writeP25BER(float(errors) / 12.33F);
m_rfBits += 1233U;
m_rfErrs += errors;
m_rfFrames++;
m_lastDUID = duid;
// Add busy bits
addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true);
#if defined(DUMP_P25)
writeFile(data + 2U, len - 2U);
#endif
::memcpy(m_rfLDU, data + 2U, P25_LDU_FRAME_LENGTH_BYTES);
if (m_duplex) {
data[0U] = TAG_DATA;
data[1U] = 0x00U;
writeQueueRF(data, P25_LDU_FRAME_LENGTH_BYTES + 2U);
}
m_display->writeP25RSSI(m_rssi);
return true;
}
} else if (duid == P25_DUID_LDU2) {
if (m_rfState == RS_RF_AUDIO) {
writeNetwork(m_rfLDU, m_lastDUID, false);
// Regenerate Sync
CSync::addP25Sync(data + 2U);
// Regenerate NID
m_nid.encode(data + 2U, P25_DUID_LDU2);
// Add the dummy LDU2 data
m_rfData.encodeLDU2(data + 2U);
// Regenerate the Low Speed Data
m_rfLSD.process(data + 2U);
// Regenerate Audio
unsigned int errors = m_audio.process(data + 2U);
LogDebug("P25, LDU2 audio, errs: %u/1233 (%.1f%%)", errors, float(errors) / 12.33F);
m_display->writeP25BER(float(errors) / 12.33F);
m_rfBits += 1233U;
m_rfErrs += errors;
m_rfFrames++;
m_lastDUID = duid;
// Add busy bits
addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true);
#if defined(DUMP_P25)
writeFile(data + 2U, len - 2U);
#endif
::memcpy(m_rfLDU, data + 2U, P25_LDU_FRAME_LENGTH_BYTES);
if (m_duplex) {
data[0U] = TAG_DATA;
data[1U] = 0x00U;
writeQueueRF(data, P25_LDU_FRAME_LENGTH_BYTES + 2U);
}
m_display->writeP25RSSI(m_rssi);
return true;
}
} else if (duid == P25_DUID_TSDU) {
if (m_rfState != RS_RF_DATA) {
m_rfPDUCount = 0U;
m_rfPDUBits = 0U;
m_rfState = RS_RF_DATA;
m_rfDataFrames = 0U;
}
bool ret = m_rfData.decodeTSDU(data + 2U);
if (!ret) {
m_lastDUID = duid;
return false;
}
unsigned int srcId = m_rfData.getSrcId();
unsigned int dstId = m_rfData.getDstId();
unsigned char data[P25_TSDU_FRAME_LENGTH_BYTES + 2U];
switch (m_rfData.getLCF()) {
case P25_LCF_TSBK_CALL_ALERT:
LogMessage("P25, received RF TSDU transmission, CALL ALERT from %u to %u", srcId, dstId);
::memset(data + 2U, 0x00U, P25_TSDU_FRAME_LENGTH_BYTES);
// Regenerate Sync
CSync::addP25Sync(data + 2U);
// Regenerate NID
m_nid.encode(data + 2U, P25_DUID_TSDU);
// Regenerate TDULC Data
m_rfData.encodeTSDU(data + 2U);
// Add busy bits
addBusyBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, true, false);
// Set first busy bits to 1,1
setBusyBits(data + 2U, P25_SS0_START, true, true);
if (m_duplex) {
data[0U] = TAG_DATA;
data[1U] = 0x00U;
writeQueueRF(data, P25_TSDU_FRAME_LENGTH_BYTES + 2U);
}
break;
case P25_LCF_TSBK_ACK_RSP_FNE:
LogMessage("P25, received RF TSDU transmission, ACK RESPONSE FNE from %u to %u", srcId, dstId);
::memset(data + 2U, 0x00U, P25_TSDU_FRAME_LENGTH_BYTES);
// Regenerate Sync
CSync::addP25Sync(data + 2U);
// Regenerate NID
m_nid.encode(data + 2U, P25_DUID_TSDU);
// Regenerate TDULC Data
m_rfData.encodeTSDU(data + 2U);
// Add busy bits
addBusyBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, true, false);
// Set first busy bits to 1,1
setBusyBits(data + 2U, P25_SS0_START, true, true);
if (m_duplex) {
data[0U] = TAG_DATA;
data[1U] = 0x00U;
writeQueueRF(data, P25_TSDU_FRAME_LENGTH_BYTES + 2U);
}
break;
default:
LogMessage("P25, recieved RF TSDU transmission, unhandled LCF $%02X", m_rfData.getLCF());
break;
}
m_rfState = RS_RF_LISTENING;
return true;
} else if (duid == P25_DUID_TERM || duid == P25_DUID_TERM_LC) {
if (m_rfState == RS_RF_AUDIO) {
writeNetwork(m_rfLDU, m_lastDUID, true);
::memset(data + 2U, 0x00U, P25_TERM_FRAME_LENGTH_BYTES);
// Regenerate Sync
CSync::addP25Sync(data + 2U);
// Regenerate NID
m_nid.encode(data + 2U, P25_DUID_TERM);
// Add busy bits
addBusyBits(data + 2U, P25_TERM_FRAME_LENGTH_BITS, false, true);
m_rfState = RS_RF_LISTENING;
m_rfTimeout.stop();
m_rfData.reset();
m_lastDUID = duid;
if (m_rssi != 0U)
LogMessage("P25, received RF end of voice transmission, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount);
else
LogMessage("P25, received RF end of voice transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits));
m_display->clearP25();
#if defined(DUMP_P25)
closeFile();
#endif
writeNetwork(data + 2U, P25_DUID_TERM, true);
if (m_duplex) {
data[0U] = TAG_EOT;
data[1U] = 0x00U;
writeQueueRF(data, P25_TERM_FRAME_LENGTH_BYTES + 2U);
}
}
} else if (duid == P25_DUID_PDU) {
if (m_rfState != RS_RF_DATA) {
m_rfPDUCount = 0U;
m_rfPDUBits = 0U;
m_rfState = RS_RF_DATA;
m_rfDataFrames = 0U;
}
unsigned int start = m_rfPDUCount * P25_LDU_FRAME_LENGTH_BITS;
unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES];
unsigned int bits = CP25Utils::decode(data + 2U, buffer, start, start + P25_LDU_FRAME_LENGTH_BITS);
for (unsigned int i = 0U; i < bits; i++, m_rfPDUBits++) {
bool b = READ_BIT(buffer, i);
WRITE_BIT(m_rfPDU, m_rfPDUBits, b);
}
if (m_rfPDUCount == 0U) {
CP25Trellis trellis;
unsigned char header[P25_PDU_HEADER_LENGTH_BYTES];
bool valid = trellis.decode12(m_rfPDU + P25_SYNC_LENGTH_BYTES + P25_NID_LENGTH_BYTES, header);
if (valid)
valid = CCRC::checkCCITT162(header, P25_PDU_HEADER_LENGTH_BYTES);
if (valid) {
unsigned int llId = (header[3U] << 16) + (header[4U] << 8) + header[5U];
unsigned int sap = header[1U] & 0x3FU;
m_rfDataFrames = header[6U] & 0x7FU;
LogMessage("P25, received RF data transmission for Local Link Id %u, SAP %u, %u blocks", llId, sap, m_rfDataFrames);
} else {
m_rfPDUCount = 0U;
m_rfPDUBits = 0U;
m_rfState = RS_RF_LISTENING;
m_rfDataFrames = 0U;
}
}
if (m_rfState == RS_RF_DATA) {
m_rfPDUCount++;
unsigned int bitLength = ((m_rfDataFrames + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_SYNC_LENGTH_BITS + P25_NID_LENGTH_BITS;
if (m_rfPDUBits >= bitLength) {
unsigned int offset = P25_SYNC_LENGTH_BYTES + P25_NID_LENGTH_BYTES;
// Regenerate the PDU header
CP25Trellis trellis;
unsigned char header[P25_PDU_HEADER_LENGTH_BYTES];
trellis.decode12(m_rfPDU + offset, header);
trellis.encode12(header, m_rfPDU + offset);
offset += P25_PDU_FEC_LENGTH_BITS;
// Regenerate the PDU data
for (unsigned int i = 0U; i < m_rfDataFrames; i++) {
unsigned char data[P25_PDU_CONFIRMED_LENGTH_BYTES];
bool valid = trellis.decode34(m_rfPDU + offset, data);
if (valid) {
trellis.encode34(data, m_rfPDU + offset);
} else {
valid = trellis.decode12(m_rfPDU + offset, data);
if (valid)
trellis.encode12(data, m_rfPDU + offset);
}
offset += P25_PDU_FEC_LENGTH_BITS;
}
unsigned char pdu[1024U];
// Add the data
unsigned int newBitLength = CP25Utils::encode(m_rfPDU, pdu + 2U, bitLength);
unsigned int newByteLength = newBitLength / 8U;
if ((newBitLength % 8U) > 0U)
newByteLength++;
// Regenerate Sync
CSync::addP25Sync(pdu + 2U);
// Regenerate NID
m_nid.encode(pdu + 2U, P25_DUID_PDU);
// Add busy bits
addBusyBits(pdu + 2U, newBitLength, false, true);
if (m_duplex) {
pdu[0U] = TAG_DATA;
pdu[1U] = 0x00U;
writeQueueRF(pdu, newByteLength + 2U);
}
LogMessage("P25, ended RF data transmission");
m_display->clearP25();
m_rfPDUCount = 0U;
m_rfPDUBits = 0U;
m_rfState = RS_RF_LISTENING;
m_rfDataFrames = 0U;
}
return true;
}
}
return false;
}