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