153 lines
3.6 KiB
C++
153 lines
3.6 KiB
C++
// P25 TDMA Decoder (C) Copyright 2013, 2014 Max H. Parke KA1RBI
|
|
//
|
|
// This file is part of OP25
|
|
//
|
|
// OP25 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 3, or (at your option)
|
|
// any later version.
|
|
//
|
|
// OP25 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 OP25; see the file COPYING. If not, write to the Free
|
|
// Software Foundation, Inc., 51 Franklin Street, Boston, MA
|
|
// 02110-1301, USA.
|
|
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include "p25p2_vf.h"
|
|
#include "rs.h"
|
|
|
|
static void load_i(const uint8_t src[], int& dest, int count) {
|
|
dest = 0;
|
|
for (int i=0; i<count; i++)
|
|
dest = (dest << 1) + src[count-1-i];
|
|
}
|
|
|
|
void p25p2_vf::process_vcw(const uint8_t vf[], int* b) {
|
|
int c0,c1,c2,c3;
|
|
int u0,u1,u2,u3;
|
|
extract_vcw(vf, c0, c1, c2, c3 );
|
|
|
|
u0 = gly24128Dec(c0);
|
|
int pr[24];
|
|
int _m1[24];
|
|
pr[0] = 16 * u0;
|
|
for (int n=1; n < 24; n++) {
|
|
pr[n] = (173*pr[n-1] + 13849) - 65536 * int((173*pr[n-1]+13849)/65536);
|
|
_m1[n-1] = (pr[n] / 32768) & 1;
|
|
}
|
|
_m1[23] = 0;
|
|
int m1 = 0;
|
|
for (int i=0; i<23; i++)
|
|
m1 = (m1 << 1) + _m1[i];
|
|
|
|
u1 = gly23127Dec(c1 ^ m1);
|
|
u2 = c2;
|
|
u3 = c3;
|
|
// int b[9];
|
|
b[0] = ((u0 >> 5) & 0x78) + ((u3 >> 9) & 0x7);
|
|
b[1] = ((u0 >> 3) & 0x1e) + ((u3 >> 13) & 0x1);
|
|
b[2] = ((u0 << 1) & 0x1e) + ((u3 >> 12) & 0x1);
|
|
b[3] = ((u1 >> 3) & 0x1fe) + ((u3 >> 8) & 0x1);
|
|
b[4] = ((u1 << 3) & 0x78) + ((u3 >> 5) & 0x7);
|
|
b[5] = ((u2 >> 6) & 0x1e) + ((u3 >> 4) & 0x1);
|
|
b[6] = ((u2 >> 3) & 0x0e) + ((u3 >> 3) & 0x1);
|
|
b[7] = ( u2 & 0x0e) + ((u3 >> 2) & 0x1);
|
|
b[8] = ((u2 << 2) & 0x04) + ( u3 & 0x3);
|
|
}
|
|
|
|
void p25p2_vf::extract_vcw(const uint8_t _vf[], int& _c0, int& _c1, int& _c2, int& _c3){
|
|
uint8_t vf[72];
|
|
uint8_t c0[24];
|
|
uint8_t c1[23];
|
|
uint8_t c2[11];
|
|
uint8_t c3[14];
|
|
|
|
for (int i=0; i<sizeof(vf)/2; i++) {
|
|
vf[i*2] = (_vf[i] >> 1) & 1;
|
|
vf[i*2+1] = _vf[i] & 1;
|
|
}
|
|
|
|
c0[23] = vf[0];
|
|
c0[5] = vf[1];
|
|
c1[10] = vf[2];
|
|
c2[3] = vf[3];
|
|
c0[22] = vf[4];
|
|
c0[4] = vf[5];
|
|
c1[9] = vf[6];
|
|
c2[2] = vf[7];
|
|
c0[21] = vf[8];
|
|
c0[3] = vf[9];
|
|
c1[8] = vf[10];
|
|
c2[1] = vf[11];
|
|
c0[20] = vf[12];
|
|
c0[2] = vf[13];
|
|
c1[7] = vf[14];
|
|
c2[0] = vf[15];
|
|
c0[19] = vf[16];
|
|
c0[1] = vf[17];
|
|
c1[6] = vf[18];
|
|
c3[13] = vf[19];
|
|
c0[18] = vf[20];
|
|
c0[0] = vf[21];
|
|
c1[5] = vf[22];
|
|
c3[12] = vf[23];
|
|
c0[17] = vf[24];
|
|
c1[22] = vf[25];
|
|
c1[4] = vf[26];
|
|
c3[11] = vf[27];
|
|
c0[16] = vf[28];
|
|
c1[21] = vf[29];
|
|
c1[3] = vf[30];
|
|
c3[10] = vf[31];
|
|
c0[15] = vf[32];
|
|
c1[20] = vf[33];
|
|
c1[2] = vf[34];
|
|
c3[9] = vf[35];
|
|
c0[14] = vf[36];
|
|
c1[19] = vf[37];
|
|
c1[1] = vf[38];
|
|
c3[8] = vf[39];
|
|
c0[13] = vf[40];
|
|
c1[18] = vf[41];
|
|
c1[0] = vf[42];
|
|
c3[7] = vf[43];
|
|
c0[12] = vf[44];
|
|
c1[17] = vf[45];
|
|
c2[10] = vf[46];
|
|
c3[6] = vf[47];
|
|
c0[11] = vf[48];
|
|
c1[16] = vf[49];
|
|
c2[9] = vf[50];
|
|
c3[5] = vf[51];
|
|
c0[10] = vf[52];
|
|
c1[15] = vf[53];
|
|
c2[8] = vf[54];
|
|
c3[4] = vf[55];
|
|
c0[9] = vf[56];
|
|
c1[14] = vf[57];
|
|
c2[7] = vf[58];
|
|
c3[3] = vf[59];
|
|
c0[8] = vf[60];
|
|
c1[13] = vf[61];
|
|
c2[6] = vf[62];
|
|
c3[2] = vf[63];
|
|
c0[7] = vf[64];
|
|
c1[12] = vf[65];
|
|
c2[5] = vf[66];
|
|
c3[1] = vf[67];
|
|
c0[6] = vf[68];
|
|
c1[11] = vf[69];
|
|
c2[4] = vf[70];
|
|
c3[0] = vf[71];
|
|
load_i(c0, _c0, 24);
|
|
load_i(c1, _c1, 23);
|
|
load_i(c2, _c2, 11);
|
|
load_i(c3, _c3, 14);
|
|
}
|