00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #include "asterisk.h"
00032
00033 ASTERISK_FILE_VERSION(__FILE__, "$Revision: 132512 $")
00034
00035 #include <stdio.h>
00036
00037 #include "asterisk/fskmodem.h"
00038
00039 #define NBW 2
00040 #define BWLIST {75,800}
00041 #define NF 6
00042 #define FLIST {1400,1800,1200,2200,1300,2100}
00043
00044 #define STATE_SEARCH_STARTBIT 0
00045 #define STATE_SEARCH_STARTBIT2 1
00046 #define STATE_SEARCH_STARTBIT3 2
00047 #define STATE_GET_BYTE 3
00048
00049 static inline float get_sample(short **buffer, int *len)
00050 {
00051 float retval;
00052 retval = (float) **buffer / 256;
00053 (*buffer)++;
00054 (*len)--;
00055 return retval;
00056 };
00057
00058 #define GET_SAMPLE get_sample(&buffer, len)
00059
00060
00061
00062
00063
00064
00065
00066
00067 static double coef_in[NF][NBW][8] = {
00068 {
00069 { 1.8229206611e-04,-7.8997325866e-01,2.2401819940e+00,-4.6751353581e+00,5.5080745712e+00,-5.0571565772e+00,2.6215820004e+00,0.0000000000e+00, },
00070 { 9.8532175289e-02,-5.6297236492e-02,3.3146713415e-01,-9.2239200436e-01,1.4844365184e+00,-2.0183258642e+00,2.0074154497e+00,0.0000000000e+00, },
00071 },
00072 {
00073 { 1.8229206610e-04,-7.8997325866e-01,7.7191410839e-01,-2.8075643964e+00,1.6948618347e+00,-3.0367273700e+00,9.0333559408e-01,0.0000000000e+00, } ,
00074 { 9.8531161839e-02,-5.6297236492e-02,1.1421579050e-01,-4.8122536483e-01,4.0121072432e-01,-7.4834487567e-01,6.9170822332e-01,0.0000000000e+00, },
00075 },
00076 {
00077 { 1.8229206611e-04,-7.8997325866e-01,2.9003821430e+00,-6.1082779024e+00,7.7169345751e+00,-6.6075999680e+00,3.3941838836e+00,0.0000000000e+00, },
00078 { 9.8539686961e-02,-5.6297236492e-02,4.2915323820e-01,-1.2609358633e+00,2.2399213250e+00,-2.9928879142e+00,2.5990173742e+00,0.0000000000e+00, },
00079 },
00080 {
00081 { 1.8229206610e-04,-7.8997325866e-01,-7.7191410839e-01,-2.8075643964e+00,-1.6948618347e+00,-3.0367273700e+00,-9.0333559408e-01,0.0000000000e+00, },
00082 { 9.8531161839e-02,-5.6297236492e-02,-1.1421579050e-01,-4.8122536483e-01,-4.0121072432e-01,-7.4834487567e-01,-6.9170822332e-01,0.0000000000e+00, },
00083 },
00084 {
00085 { 1.8229206611e-04,-7.8997325866e-01,2.5782298908e+00,-5.3629717478e+00,6.5890882172e+00,-5.8012914776e+00,3.0171839130e+00,0.0000000000e+00, },
00086 { 9.8534230718e-02,-5.6297236492e-02,3.8148618075e-01,-1.0848760410e+00,1.8441165168e+00,-2.4860666655e+00,2.3103384142e+00,0.0000000000e+00, },
00087 },
00088 {
00089 { 1.8229206610e-04,-7.8997325866e-01,-3.8715051001e-01,-2.6192408538e+00,-8.3977994034e-01,-2.8329897913e+00,-4.5306444352e-01,0.0000000000e+00, },
00090 { 9.8531160936e-02,-5.6297236492e-02,-5.7284484199e-02,-4.3673866734e-01,-1.9564766257e-01,-6.2028156584e-01,-3.4692356122e-01,0.0000000000e+00, },
00091 },
00092 };
00093
00094
00095
00096
00097
00098
00099
00100 static double coef_out[NBW][8] = {
00101 { 1.3868644653e-08,-6.3283665042e-01,4.0895057217e+00,-1.1020074592e+01,1.5850766191e+01,-1.2835109292e+01,5.5477477340e+00,0.0000000000e+00, },
00102 { 3.1262119724e-03,-7.8390522307e-03,8.5209627801e-02,-4.0804129163e-01,1.1157139955e+00,-1.8767603680e+00,1.8916395224e+00,0.0000000000e+00, },
00103 };
00104
00105
00106
00107 static inline float filterM(fsk_data *fskd,float in)
00108 {
00109 int i, j;
00110 double s;
00111 double *pc;
00112
00113 pc = &coef_in[fskd->f_mark_idx][fskd->bw][0];
00114 fskd->fmxv[(fskd->fmp+6)&7] = in*(*pc++);
00115
00116 s = (fskd->fmxv[(fskd->fmp + 6) & 7] - fskd->fmxv[fskd->fmp]) + 3 * (fskd->fmxv[(fskd->fmp + 2) & 7] - fskd->fmxv[(fskd->fmp + 4) & 7]);
00117 for (i = 0, j = fskd->fmp; i < 6; i++, j++)
00118 s += fskd->fmyv[j&7]*(*pc++);
00119 fskd->fmyv[j&7] = s;
00120 fskd->fmp++;
00121 fskd->fmp &= 7;
00122 return s;
00123 }
00124
00125
00126 static inline float filterS(fsk_data *fskd,float in)
00127 {
00128 int i, j;
00129 double s;
00130 double *pc;
00131
00132 pc = &coef_in[fskd->f_space_idx][fskd->bw][0];
00133 fskd->fsxv[(fskd->fsp+6)&7] = in*(*pc++);
00134
00135 s = (fskd->fsxv[(fskd->fsp + 6) & 7] - fskd->fsxv[fskd->fsp]) + 3 * (fskd->fsxv[(fskd->fsp + 2) & 7] - fskd->fsxv[(fskd->fsp + 4) & 7]);
00136 for (i = 0, j = fskd->fsp; i < 6; i++, j++)
00137 s += fskd->fsyv[j&7]*(*pc++);
00138 fskd->fsyv[j&7] = s;
00139 fskd->fsp++;
00140 fskd->fsp &= 7;
00141 return s;
00142 }
00143
00144
00145 static inline float filterL(fsk_data *fskd,float in)
00146 {
00147 int i, j;
00148 double s;
00149 double *pc;
00150
00151 pc = &coef_out[fskd->bw][0];
00152 fskd->flxv[(fskd->flp + 6) & 7] = in * (*pc++);
00153
00154 s = (fskd->flxv[fskd->flp] + fskd->flxv[(fskd->flp+6)&7]) +
00155 6 * (fskd->flxv[(fskd->flp+1)&7] + fskd->flxv[(fskd->flp+5)&7]) +
00156 15 * (fskd->flxv[(fskd->flp+2)&7] + fskd->flxv[(fskd->flp+4)&7]) +
00157 20 * fskd->flxv[(fskd->flp+3)&7];
00158
00159 for (i = 0,j = fskd->flp;i<6;i++,j++)
00160 s += fskd->flyv[j&7]*(*pc++);
00161 fskd->flyv[j&7] = s;
00162 fskd->flp++;
00163 fskd->flp &= 7;
00164 return s;
00165 }
00166
00167 static inline int demodulator(fsk_data *fskd, float *retval, float x)
00168 {
00169 float xS,xM;
00170
00171 fskd->cola_in[fskd->pcola] = x;
00172
00173 xS = filterS(fskd,x);
00174 xM = filterM(fskd,x);
00175
00176 fskd->cola_filter[fskd->pcola] = xM-xS;
00177
00178 x = filterL(fskd,xM*xM - xS*xS);
00179
00180 fskd->cola_demod[fskd->pcola++] = x;
00181 fskd->pcola &= (NCOLA-1);
00182
00183 *retval = x;
00184 return 0;
00185 }
00186
00187 static int get_bit_raw(fsk_data *fskd, short *buffer, int *len)
00188 {
00189
00190 float x,spb,spb2,ds;
00191 int f;
00192
00193 spb = fskd->spb;
00194 if (fskd->spb == 7)
00195 spb = 8000.0 / 1200.0;
00196 ds = spb/32.;
00197 spb2 = spb/2.;
00198
00199 for (f = 0;;) {
00200 if (demodulator(fskd, &x, GET_SAMPLE))
00201 return -1;
00202 if ((x * fskd->x0) < 0) {
00203 if (!f) {
00204 if (fskd->cont<(spb2))
00205 fskd->cont += ds;
00206 else
00207 fskd->cont -= ds;
00208 f = 1;
00209 }
00210 }
00211 fskd->x0 = x;
00212 fskd->cont += 1.;
00213 if (fskd->cont > spb) {
00214 fskd->cont -= spb;
00215 break;
00216 }
00217 }
00218 f = (x > 0) ? 0x80 : 0;
00219 return f;
00220 }
00221
00222 int fsk_serial(fsk_data *fskd, short *buffer, int *len, int *outbyte)
00223 {
00224 int a;
00225 int i,j,n1,r;
00226 int samples = 0;
00227 int olen;
00228 int beginlen=*len;
00229 int beginlenx;
00230
00231 switch (fskd->state) {
00232
00233 case STATE_SEARCH_STARTBIT2:
00234 goto search_startbit2;
00235 case STATE_SEARCH_STARTBIT3:
00236 goto search_startbit3;
00237 case STATE_GET_BYTE:
00238 goto getbyte;
00239 }
00240
00241 do {
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258 beginlenx=beginlen;
00259 if (demodulator(fskd, &fskd->x1, GET_SAMPLE))
00260 return -1;
00261 samples++;
00262 for (;;) {
00263 search_startbit2:
00264 if (*len <= 0) {
00265 fskd->state = STATE_SEARCH_STARTBIT2;
00266 return 0;
00267 }
00268 samples++;
00269 if (demodulator(fskd, &fskd->x2, GET_SAMPLE))
00270 return(-1);
00271 #if 0
00272 printf("x2 = %5.5f ", fskd->x2);
00273 #endif
00274 if (fskd->x2 < -0.5)
00275 break;
00276 }
00277 search_startbit3:
00278
00279 i = fskd->spb/2;
00280 if (*len < i) {
00281 fskd->state = STATE_SEARCH_STARTBIT3;
00282 return 0;
00283 }
00284 for (; i>0; i--) {
00285 if (demodulator(fskd, &fskd->x1, GET_SAMPLE))
00286 return(-1);
00287 #if 0
00288 printf("x1 = %5.5f ", fskd->x1);
00289 #endif
00290 samples++;
00291 }
00292
00293
00294
00295 } while (fskd->x1 > 0);
00296 fskd->state = STATE_GET_BYTE;
00297
00298 getbyte:
00299
00300
00301
00302 if (fskd->nbit < 8) {
00303 if (*len < 1320)
00304 return 0;
00305 } else {
00306 if (*len < 80)
00307 return 0;
00308 }
00309
00310 j = fskd->nbit;
00311 for (a = n1 = 0; j; j--) {
00312 olen = *len;
00313 i = get_bit_raw(fskd, buffer, len);
00314 buffer += (olen - *len);
00315 if (i == -1)
00316 return(-1);
00317 if (i)
00318 n1++;
00319 a >>= 1;
00320 a |= i;
00321 }
00322 j = 8-fskd->nbit;
00323 a >>= j;
00324
00325
00326 if (fskd->parity) {
00327 olen = *len;
00328 i = get_bit_raw(fskd, buffer, len);
00329 buffer += (olen - *len);
00330 if (i == -1)
00331 return(-1);
00332 if (i)
00333 n1++;
00334 if (fskd->parity == 1) {
00335 if (n1&1)
00336 a |= 0x100;
00337 } else {
00338 if (!(n1&1))
00339 a |= 0x100;
00340 }
00341 }
00342
00343
00344
00345 for (j = fskd->nstop;j;j--) {
00346 r = get_bit_raw(fskd, buffer, len);
00347 if (r == -1)
00348 return(-1);
00349 if (!r)
00350 a |= 0x200;
00351 }
00352
00353
00354
00355
00356
00357 *outbyte = a;
00358 fskd->state = STATE_SEARCH_STARTBIT;
00359 return 1;
00360 }