Add the IMBE packing/unpacking code.
This commit is contained in:
Jonathan Naylor
parent
4c5d1492ce
commit
3011ff4a11
260
P25Audio.cpp
260
P25Audio.cpp
|
|
@ -18,10 +18,25 @@
|
|||
|
||||
#include "P25Audio.h"
|
||||
#include "P25Utils.h"
|
||||
#include "Golay24128.h"
|
||||
#include "Hamming.h"
|
||||
|
||||
#include <cstdio>
|
||||
#include <cassert>
|
||||
|
||||
const unsigned int IMBE_INTERLEAVE[] = {
|
||||
0, 7, 12, 19, 24, 31, 36, 43, 48, 55, 60, 67, 72, 79, 84, 91, 96, 103, 108, 115, 120, 127, 132, 139,
|
||||
1, 6, 13, 18, 25, 30, 37, 42, 49, 54, 61, 66, 73, 78, 85, 90, 97, 102, 109, 114, 121, 126, 133, 138,
|
||||
2, 9, 14, 21, 26, 33, 38, 45, 50, 57, 62, 69, 74, 81, 86, 93, 98, 105, 110, 117, 122, 129, 134, 141,
|
||||
3, 8, 15, 20, 27, 32, 39, 44, 51, 56, 63, 68, 75, 80, 87, 92, 99, 104, 111, 116, 123, 128, 135, 140,
|
||||
4, 11, 16, 23, 28, 35, 40, 47, 52, 59, 64, 71, 76, 83, 88, 95, 100, 107, 112, 119, 124, 131, 136, 143,
|
||||
5, 10, 17, 22, 29, 34, 41, 46, 53, 58, 65, 70, 77, 82, 89, 94, 101, 106, 113, 118, 125, 130, 137, 142};
|
||||
|
||||
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])
|
||||
|
||||
CP25Audio::CP25Audio() :
|
||||
m_fec()
|
||||
{
|
||||
|
|
@ -77,3 +92,248 @@ unsigned int CP25Audio::process(unsigned char* data)
|
|||
|
||||
return errs;
|
||||
}
|
||||
|
||||
void CP25Audio::decode(const unsigned char* data, unsigned char* imbe, unsigned int n)
|
||||
{
|
||||
assert(data != NULL);
|
||||
assert(imbe != NULL);
|
||||
|
||||
unsigned char temp[18U];
|
||||
|
||||
switch (n) {
|
||||
case 0U:
|
||||
CP25Utils::decode(data, temp, 114U, 262U);
|
||||
break;
|
||||
case 1U:
|
||||
CP25Utils::decode(data, temp, 262U, 410U);
|
||||
break;
|
||||
case 2U:
|
||||
CP25Utils::decode(data, temp, 452U, 600U);
|
||||
break;
|
||||
case 3U:
|
||||
CP25Utils::decode(data, temp, 640U, 788U);
|
||||
break;
|
||||
case 4U:
|
||||
CP25Utils::decode(data, temp, 830U, 978U);
|
||||
break;
|
||||
case 5U:
|
||||
CP25Utils::decode(data, temp, 1020U, 1168U);
|
||||
break;
|
||||
case 6U:
|
||||
CP25Utils::decode(data, temp, 1208U, 1356U);
|
||||
break;
|
||||
case 7U:
|
||||
CP25Utils::decode(data, temp, 1398U, 1546U);
|
||||
break;
|
||||
case 8U:
|
||||
CP25Utils::decode(data, temp, 1578U, 1726U);
|
||||
break;
|
||||
default:
|
||||
return;
|
||||
}
|
||||
|
||||
bool bit[144U];
|
||||
|
||||
// De-interleave
|
||||
for (unsigned int i = 0U; i < 144U; i++) {
|
||||
unsigned int n = IMBE_INTERLEAVE[i];
|
||||
bit[i] = READ_BIT(temp, n);
|
||||
}
|
||||
|
||||
// now ..
|
||||
|
||||
// 12 voice bits 0
|
||||
// 11 golay bits 12
|
||||
//
|
||||
// 12 voice bits 23
|
||||
// 11 golay bits 35
|
||||
//
|
||||
// 12 voice bits 46
|
||||
// 11 golay bits 58
|
||||
//
|
||||
// 12 voice bits 69
|
||||
// 11 golay bits 81
|
||||
//
|
||||
// 11 voice bits 92
|
||||
// 4 hamming bits 103
|
||||
//
|
||||
// 11 voice bits 107
|
||||
// 4 hamming bits 118
|
||||
//
|
||||
// 11 voice bits 122
|
||||
// 4 hamming bits 133
|
||||
//
|
||||
// 7 voice bits 137
|
||||
|
||||
// c0
|
||||
unsigned int c0data = 0U;
|
||||
for (unsigned int i = 0U; i < 12U; i++)
|
||||
c0data = (c0data << 1) | (bit[i] ? 0x01U : 0x00U);
|
||||
|
||||
bool prn[114U];
|
||||
|
||||
// Create the whitening vector and save it for future use
|
||||
unsigned int p = 16U * c0data;
|
||||
for (unsigned int i = 0U; i < 114U; i++) {
|
||||
p = (173U * p + 13849U) % 65536U;
|
||||
prn[i] = p >= 32768U;
|
||||
}
|
||||
|
||||
// De-whiten some bits
|
||||
for (unsigned int i = 0U; i < 114U; i++)
|
||||
bit[i + 23U] ^= prn[i];
|
||||
|
||||
unsigned int offset = 0U;
|
||||
for (unsigned int i = 0U; i < 12U; i++, offset++)
|
||||
WRITE_BIT(imbe, offset, bit[i + 0U]);
|
||||
for (unsigned int i = 0U; i < 12U; i++, offset++)
|
||||
WRITE_BIT(imbe, offset, bit[i + 23U]);
|
||||
for (unsigned int i = 0U; i < 12U; i++, offset++)
|
||||
WRITE_BIT(imbe, offset, bit[i + 46U]);
|
||||
for (unsigned int i = 0U; i < 12U; i++, offset++)
|
||||
WRITE_BIT(imbe, offset, bit[i + 69U]);
|
||||
for (unsigned int i = 0U; i < 11U; i++, offset++)
|
||||
WRITE_BIT(imbe, offset, bit[i + 92U]);
|
||||
for (unsigned int i = 0U; i < 11U; i++, offset++)
|
||||
WRITE_BIT(imbe, offset, bit[i + 107U]);
|
||||
for (unsigned int i = 0U; i < 11U; i++, offset++)
|
||||
WRITE_BIT(imbe, offset, bit[i + 122U]);
|
||||
for (unsigned int i = 0U; i < 7U; i++, offset++)
|
||||
WRITE_BIT(imbe, offset, bit[i + 137U]);
|
||||
}
|
||||
|
||||
void CP25Audio::encode(unsigned char* data, const unsigned char* imbe, unsigned int n)
|
||||
{
|
||||
assert(data != NULL);
|
||||
assert(imbe != NULL);
|
||||
|
||||
bool bTemp[144U];
|
||||
bool* bit = bTemp;
|
||||
|
||||
// c0
|
||||
unsigned int c0 = 0U;
|
||||
for (unsigned int i = 0U; i < 12U; i++) {
|
||||
bool b = READ_BIT(imbe, i);
|
||||
c0 = (c0 << 1) | (b ? 0x01U : 0x00U);
|
||||
}
|
||||
unsigned int g2 = CGolay24128::encode23127(c0);
|
||||
for (int i = 23; i >= 0; i--) {
|
||||
bit[i] = (g2 & 0x01U) == 0x01U;
|
||||
g2 >>= 1;
|
||||
}
|
||||
bit += 23U;
|
||||
|
||||
// c1
|
||||
unsigned int c1 = 0U;
|
||||
for (unsigned int i = 12U; i < 24U; i++) {
|
||||
bool b = READ_BIT(imbe, i);
|
||||
c1 = (c1 << 1) | (b ? 0x01U : 0x00U);
|
||||
}
|
||||
g2 = CGolay24128::encode23127(c1);
|
||||
for (int i = 23; i >= 0; i--) {
|
||||
bit[i] = (g2 & 0x01U) == 0x01U;
|
||||
g2 >>= 1;
|
||||
}
|
||||
bit += 23U;
|
||||
|
||||
// c2
|
||||
unsigned int c2 = 0;
|
||||
for (unsigned int i = 24U; i < 36U; i++) {
|
||||
bool b = READ_BIT(imbe, i);
|
||||
c2 = (c2 << 1) | (b ? 0x01U : 0x00U);
|
||||
}
|
||||
g2 = CGolay24128::encode23127(c2);
|
||||
for (int i = 23; i >= 0; i--) {
|
||||
bit[i] = (g2 & 0x01U) == 0x01U;
|
||||
g2 >>= 1;
|
||||
}
|
||||
bit += 23U;
|
||||
|
||||
// c3
|
||||
unsigned int c3 = 0U;
|
||||
for (unsigned int i = 36U; i < 48U; i++) {
|
||||
bool b = READ_BIT(imbe, i);
|
||||
c3 = (c3 << 1) | (b ? 0x01U : 0x00U);
|
||||
}
|
||||
g2 = CGolay24128::encode23127(c3);
|
||||
for (int i = 23; i >= 0; i--) {
|
||||
bit[i] = (g2 & 0x01U) == 0x01U;
|
||||
g2 >>= 1;
|
||||
}
|
||||
bit += 23U;
|
||||
|
||||
// c4
|
||||
for (unsigned int i = 0U; i < 11U; i++)
|
||||
bit[i] = READ_BIT(imbe, i + 48U);
|
||||
CHamming::encode15113_1(bit);
|
||||
bit += 15U;
|
||||
|
||||
// c5
|
||||
for (unsigned int i = 0U; i < 11U; i++)
|
||||
bit[i] = READ_BIT(imbe, i + 59U);
|
||||
CHamming::encode15113_1(bit);
|
||||
bit += 15U;
|
||||
|
||||
// c6
|
||||
for (unsigned int i = 0U; i < 11U; i++)
|
||||
bit[i] = READ_BIT(imbe, i + 70U);
|
||||
CHamming::encode15113_1(bit);
|
||||
bit += 15U;
|
||||
|
||||
// c7
|
||||
for (unsigned int i = 0U; i < 7U; i++)
|
||||
bit[i] = READ_BIT(imbe, i + 81U);
|
||||
|
||||
bool prn[114U];
|
||||
|
||||
// Create the whitening vector and save it for future use
|
||||
unsigned int p = 16U * c0;
|
||||
for (unsigned int i = 0U; i < 114U; i++) {
|
||||
p = (173U * p + 13849U) % 65536U;
|
||||
prn[i] = p >= 32768U;
|
||||
}
|
||||
|
||||
// Whiten some bits
|
||||
for (unsigned int i = 0U; i < 114U; i++)
|
||||
bTemp[i + 23U] ^= prn[i];
|
||||
|
||||
unsigned char temp[18U];
|
||||
|
||||
// Interleave
|
||||
for (unsigned int i = 0U; i < 144U; i++) {
|
||||
unsigned int n = IMBE_INTERLEAVE[i];
|
||||
WRITE_BIT(temp, n, bTemp[i]);
|
||||
}
|
||||
|
||||
switch (n) {
|
||||
case 0U:
|
||||
CP25Utils::encode(temp, data, 114U, 262U);
|
||||
break;
|
||||
case 1U:
|
||||
CP25Utils::encode(temp, data, 262U, 410U);
|
||||
break;
|
||||
case 2U:
|
||||
CP25Utils::encode(temp, data, 452U, 600U);
|
||||
break;
|
||||
case 3U:
|
||||
CP25Utils::encode(temp, data, 640U, 788U);
|
||||
break;
|
||||
case 4U:
|
||||
CP25Utils::encode(temp, data, 830U, 978U);
|
||||
break;
|
||||
case 5U:
|
||||
CP25Utils::encode(temp, data, 1020U, 1168U);
|
||||
break;
|
||||
case 6U:
|
||||
CP25Utils::encode(temp, data, 1208U, 1356U);
|
||||
break;
|
||||
case 7U:
|
||||
CP25Utils::encode(temp, data, 1398U, 1546U);
|
||||
break;
|
||||
case 8U:
|
||||
CP25Utils::encode(data, temp, 1578U, 1726U);
|
||||
break;
|
||||
default:
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -28,6 +28,10 @@ public:
|
|||
|
||||
unsigned int process(unsigned char* data);
|
||||
|
||||
void encode(unsigned char* data, const unsigned char* imbe, unsigned int n);
|
||||
|
||||
void decode(const unsigned char* data, unsigned char* imbe, unsigned int n);
|
||||
|
||||
private:
|
||||
CAMBEFEC m_fec;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -133,8 +133,8 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len)
|
|||
// Regenerate NID
|
||||
m_nid.encode(data + 2U, P25_DUID_HEADER);
|
||||
|
||||
// Regenerate Enc Data
|
||||
m_rfData.processHeader(data + 2U);
|
||||
// Add the dummy header
|
||||
m_netData.createHeader(data + 2U);
|
||||
|
||||
// Add busy bits
|
||||
addBusyBits(data + 2U, P25_HDR_FRAME_LENGTH_BITS, false, true);
|
||||
|
|
@ -223,8 +223,8 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len)
|
|||
// Regenerate NID
|
||||
m_nid.encode(data + 2U, P25_DUID_LDU2);
|
||||
|
||||
// Regenerate LDU2 Data
|
||||
m_rfData.processLDU2(data + 2U);
|
||||
// Add the dummy LDU2 data
|
||||
m_netData.createLDU2(data + 2U);
|
||||
|
||||
// Regenerate the Low Speed Data
|
||||
m_lsd.process(data + 2U);
|
||||
|
|
@ -578,6 +578,15 @@ void CP25Control::createLDU1()
|
|||
m_netData.createLDU1(buffer + 2U);
|
||||
|
||||
// Add the Audio
|
||||
m_audio.encode(buffer + 2U, m_netLDU1 + 10U, 0U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU1 + 26U, 1U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU1 + 55U, 2U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU1 + 80U, 3U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU1 + 105U, 4U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU1 + 130U, 5U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU1 + 155U, 6U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU1 + 180U, 7U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU1 + 204U, 8U);
|
||||
|
||||
// Add the Low Speed Data
|
||||
m_lsd.encode(buffer + 2U, m_netLDU1[201U], m_netLDU1[202U]);
|
||||
|
|
@ -608,6 +617,15 @@ void CP25Control::createLDU2()
|
|||
m_netData.createLDU2(buffer + 2U);
|
||||
|
||||
// Add the Audio
|
||||
m_audio.encode(buffer + 2U, m_netLDU2 + 10U, 0U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU2 + 26U, 1U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU2 + 55U, 2U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU2 + 80U, 3U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU2 + 105U, 4U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU2 + 130U, 5U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU2 + 155U, 6U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU2 + 180U, 7U);
|
||||
m_audio.encode(buffer + 2U, m_netLDU2 + 204U, 8U);
|
||||
|
||||
// Add the Low Speed Data
|
||||
m_lsd.encode(buffer + 2U, m_netLDU2[201U], m_netLDU2[202U]);
|
||||
|
|
|
|||
Loading…
Reference in New Issue