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: 316265 $")
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
00229 switch (fskd->state) {
00230
00231 case STATE_SEARCH_STARTBIT2:
00232 goto search_startbit2;
00233 case STATE_SEARCH_STARTBIT3:
00234 goto search_startbit3;
00235 case STATE_GET_BYTE:
00236 goto getbyte;
00237 }
00238
00239 do {
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256 if (demodulator(fskd, &fskd->x1, GET_SAMPLE))
00257 return -1;
00258 samples++;
00259 for (;;) {
00260 search_startbit2:
00261 if (*len <= 0) {
00262 fskd->state = STATE_SEARCH_STARTBIT2;
00263 return 0;
00264 }
00265 samples++;
00266 if (demodulator(fskd, &fskd->x2, GET_SAMPLE))
00267 return(-1);
00268 #if 0
00269 printf("x2 = %5.5f ", fskd->x2);
00270 #endif
00271 if (fskd->x2 < -0.5)
00272 break;
00273 }
00274 search_startbit3:
00275
00276 i = fskd->spb/2;
00277 if (*len < i) {
00278 fskd->state = STATE_SEARCH_STARTBIT3;
00279 return 0;
00280 }
00281 for (; i>0; i--) {
00282 if (demodulator(fskd, &fskd->x1, GET_SAMPLE))
00283 return(-1);
00284 #if 0
00285 printf("x1 = %5.5f ", fskd->x1);
00286 #endif
00287 samples++;
00288 }
00289
00290
00291
00292 } while (fskd->x1 > 0);
00293 fskd->state = STATE_GET_BYTE;
00294
00295 getbyte:
00296
00297
00298
00299 if (fskd->nbit < 8) {
00300 if (*len < 1320)
00301 return 0;
00302 } else {
00303 if (*len < 80)
00304 return 0;
00305 }
00306
00307 j = fskd->nbit;
00308 for (a = n1 = 0; j; j--) {
00309 olen = *len;
00310 i = get_bit_raw(fskd, buffer, len);
00311 buffer += (olen - *len);
00312 if (i == -1)
00313 return(-1);
00314 if (i)
00315 n1++;
00316 a >>= 1;
00317 a |= i;
00318 }
00319 j = 8-fskd->nbit;
00320 a >>= j;
00321
00322
00323 if (fskd->parity) {
00324 olen = *len;
00325 i = get_bit_raw(fskd, buffer, len);
00326 buffer += (olen - *len);
00327 if (i == -1)
00328 return(-1);
00329 if (i)
00330 n1++;
00331 if (fskd->parity == 1) {
00332 if (n1&1)
00333 a |= 0x100;
00334 } else {
00335 if (!(n1&1))
00336 a |= 0x100;
00337 }
00338 }
00339
00340
00341
00342 for (j = fskd->nstop;j;j--) {
00343 r = get_bit_raw(fskd, buffer, len);
00344 if (r == -1)
00345 return(-1);
00346 if (!r)
00347 a |= 0x200;
00348 }
00349
00350
00351
00352
00353
00354 *outbyte = a;
00355 fskd->state = STATE_SEARCH_STARTBIT;
00356 return 1;
00357 }