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 "asterisk/fskmodem.h"
00040
00041 #define NBW 2
00042 #define BWLIST {75,800}
00043 #define NF 6
00044 #define FLIST {1400,1800,1200,2200,1300,2100}
00045
00046 #define STATE_SEARCH_STARTBIT 0
00047 #define STATE_SEARCH_STARTBIT2 1
00048 #define STATE_SEARCH_STARTBIT3 2
00049 #define STATE_GET_BYTE 3
00050
00051 static inline int iget_sample(short **buffer, int *len)
00052 {
00053 int retval;
00054 retval = (int) **buffer;
00055 (*buffer)++;
00056 (*len)--;
00057 return retval;
00058 }
00059
00060 #define IGET_SAMPLE iget_sample(&buffer, len)
00061
00062
00063
00064
00065
00066
00067
00068 static double coef_in[NF][NBW][8]={
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 }, }, { { 1.8229206610e-04,-7.8997325866e-01,7.7191410839e-01,-2.8075643964e+00,1.6948618347e+00,-3.0367273700e+00,9.0333559408e-01,0.0000000000e+00,
00072 }, { 9.8531161839e-02,-5.6297236492e-02,1.1421579050e-01,-4.8122536483e-01,4.0121072432e-01,-7.4834487567e-01,6.9170822332e-01,0.0000000000e+00,
00073 }, }, { { 1.8229206611e-04,-7.8997325866e-01,2.9003821430e+00,-6.1082779024e+00,7.7169345751e+00,-6.6075999680e+00,3.3941838836e+00,0.0000000000e+00,
00074 }, { 9.8539686961e-02,-5.6297236492e-02,4.2915323820e-01,-1.2609358633e+00,2.2399213250e+00,-2.9928879142e+00,2.5990173742e+00,0.0000000000e+00,
00075 }, }, { { 1.8229206610e-04,-7.8997325866e-01,-7.7191410839e-01,-2.8075643964e+00,-1.6948618347e+00,-3.0367273700e+00,-9.0333559408e-01,0.0000000000e+00,
00076 }, { 9.8531161839e-02,-5.6297236492e-02,-1.1421579050e-01,-4.8122536483e-01,-4.0121072432e-01,-7.4834487567e-01,-6.9170822332e-01,0.0000000000e+00,
00077 }, }, { { 1.8229206611e-04,-7.8997325866e-01,2.5782298908e+00,-5.3629717478e+00,6.5890882172e+00,-5.8012914776e+00,3.0171839130e+00,0.0000000000e+00,
00078 }, { 9.8534230718e-02,-5.6297236492e-02,3.8148618075e-01,-1.0848760410e+00,1.8441165168e+00,-2.4860666655e+00,2.3103384142e+00,0.0000000000e+00,
00079 }, }, { { 1.8229206610e-04,-7.8997325866e-01,-3.8715051001e-01,-2.6192408538e+00,-8.3977994034e-01,-2.8329897913e+00,-4.5306444352e-01,0.0000000000e+00,
00080 }, { 9.8531160936e-02,-5.6297236492e-02,-5.7284484199e-02,-4.3673866734e-01,-1.9564766257e-01,-6.2028156584e-01,-3.4692356122e-01,0.0000000000e+00,
00081 }, },
00082 };
00083
00084
00085
00086
00087
00088
00089
00090 static double coef_out[NBW][8]={
00091 { 1.3868644653e-08,-6.3283665042e-01,4.0895057217e+00,-1.1020074592e+01,1.5850766191e+01,-1.2835109292e+01,5.5477477340e+00,0.0000000000e+00,
00092 }, { 3.1262119724e-03,-7.8390522307e-03,8.5209627801e-02,-4.0804129163e-01,1.1157139955e+00,-1.8767603680e+00,1.8916395224e+00,0.0000000000e+00
00093 },
00094 };
00095
00096
00097 static inline int ibpdfilter(struct filter_struct * fs, int in)
00098 {
00099 int i,j;
00100 int s;
00101 int64_t s_interim;
00102
00103
00104 s = in * fs->icoefs[0];
00105 fs->ixv[(fs->ip + 6) & 7] = s;
00106
00107 s = (fs->ixv[fs->ip] + fs->ixv[(fs->ip + 6) & 7]) +
00108 6 * (fs->ixv[(fs->ip + 1) & 7] + fs->ixv[(fs->ip + 5) & 7]) +
00109 15 * (fs->ixv[(fs->ip + 2) & 7] + fs->ixv[(fs->ip + 4) & 7]) +
00110 20 * fs->ixv[(fs->ip + 3) & 7];
00111
00112 for (i = 1, j = fs->ip; i < 7; i++, j++) {
00113
00114 s_interim = (int64_t)(fs->iyv[j & 7]) *
00115 (int64_t)(fs->icoefs[i]) /
00116 (int64_t)(1024);
00117 s += (int) s_interim;
00118 }
00119 fs->iyv[j & 7] = s;
00120 fs->ip++;
00121 fs->ip &= 7;
00122 return s;
00123 }
00124
00125
00126 static inline int ibpfilter(struct filter_struct * fs, int in)
00127 {
00128 int i, j;
00129 int s;
00130 int64_t s_interim;
00131
00132
00133 s = in * fs->icoefs[0] / 256;
00134 fs->ixv[(fs->ip + 6) & 7] = s;
00135
00136 s = (fs->ixv[(fs->ip + 6) & 7] - fs->ixv[fs->ip])
00137 + 3 * (fs->ixv[(fs->ip + 2) & 7] - fs->ixv[(fs->ip + 4) & 7]);
00138
00139 for (i = 1, j = fs->ip; i < 7; i++, j++) {
00140 s_interim = (int64_t)(fs->iyv[j & 7]) *
00141 (int64_t)(fs->icoefs[i]) /
00142 (int64_t)(256);
00143 s += (int) s_interim;
00144 }
00145 fs->iyv[j & 7] = s;
00146 fs->ip++;
00147 fs->ip &= 7;
00148 return s;
00149 }
00150
00151 static inline int idemodulator(fsk_data *fskd, int *retval, int x)
00152 {
00153 int is, im, id;
00154 int ilin2;
00155
00156 is = ibpfilter(&fskd->space_filter, x);
00157 im = ibpfilter(&fskd->mark_filter, x);
00158
00159 ilin2 = ((im * im) - (is * is)) / (256 * 256);
00160
00161 id = ibpdfilter(&fskd->demod_filter, ilin2);
00162
00163 *retval = id;
00164 return 0;
00165 }
00166
00167 static int get_bit_raw(fsk_data *fskd, short *buffer, int *len)
00168 {
00169
00170 int f;
00171
00172 int ix;
00173
00174 for (f = 0;;) {
00175 if (idemodulator(fskd, &ix, IGET_SAMPLE)) return(-1);
00176 if ((ix * fskd->xi0) < 0) {
00177 if (!f) {
00178 if (fskd->icont < (fskd->pllispb2)) {
00179 fskd->icont += fskd->pllids;
00180 } else {
00181 fskd->icont -= fskd->pllids;
00182 }
00183 f = 1;
00184 }
00185 }
00186 fskd->xi0 = ix;
00187 fskd->icont += 32;
00188 if (fskd->icont > fskd->pllispb) {
00189 fskd->icont -= fskd->pllispb;
00190 break;
00191 }
00192 }
00193 f = (ix > 0) ? 0x80 : 0;
00194 return f;
00195 }
00196
00197 int fskmodem_init(fsk_data *fskd)
00198 {
00199 int i;
00200
00201 fskd->space_filter.ip = 0;
00202 fskd->mark_filter.ip = 0;
00203 fskd->demod_filter.ip = 0;
00204
00205 for ( i = 0 ; i < 7 ; i++ ) {
00206 fskd->space_filter.icoefs[i] =
00207 coef_in[fskd->f_space_idx][fskd->bw][i] * 256;
00208 fskd->space_filter.ixv[i] = 0;;
00209 fskd->space_filter.iyv[i] = 0;;
00210
00211 fskd->mark_filter.icoefs[i] =
00212 coef_in[fskd->f_mark_idx][fskd->bw][i] * 256;
00213 fskd->mark_filter.ixv[i] = 0;;
00214 fskd->mark_filter.iyv[i] = 0;;
00215
00216 fskd->demod_filter.icoefs[i] =
00217 coef_out[fskd->bw][i] * 1024;
00218 fskd->demod_filter.ixv[i] = 0;;
00219 fskd->demod_filter.iyv[i] = 0;;
00220 }
00221 return 0;
00222 }
00223
00224 int fsk_serial(fsk_data *fskd, short *buffer, int *len, int *outbyte)
00225 {
00226 int a;
00227 int i, j, n1, r;
00228 int samples = 0;
00229 int olen;
00230 int beginlen = *len;
00231 int beginlenx;
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
00261
00262 beginlenx = beginlen;
00263 if (idemodulator(fskd, &fskd->xi1, IGET_SAMPLE))
00264 return -1;
00265 samples++;
00266 for(;;) {
00267 search_startbit2:
00268 if (*len <= 0) {
00269 fskd->state = STATE_SEARCH_STARTBIT2;
00270 return 0;
00271 }
00272 samples++;
00273 if (idemodulator(fskd, &fskd->xi2, IGET_SAMPLE))
00274 return -1;
00275 #if 0
00276 printf("xi2 = %d ", fskd->xi2);
00277 #endif
00278 if (fskd->xi2 < 512) {
00279 break;
00280 }
00281 }
00282 search_startbit3:
00283
00284 i = fskd->ispb / 2;
00285 if (*len < i) {
00286 fskd->state = STATE_SEARCH_STARTBIT3;
00287 return 0;
00288 }
00289 for (; i > 0; i--) {
00290 if (idemodulator(fskd, &fskd->xi1, IGET_SAMPLE))
00291 return(-1);
00292 #if 0
00293 printf("xi1 = %d ", fskd->xi1);
00294 #endif
00295 samples++;
00296 }
00297
00298
00299
00300 } while (fskd->xi1 > 0);
00301 fskd->state = STATE_GET_BYTE;
00302
00303 getbyte:
00304
00305
00306
00307 if (fskd->nbit < 8) {
00308 if (*len < 1320)
00309 return 0;
00310 } else {
00311 if (*len < 80)
00312 return 0;
00313 }
00314
00315
00316 j = fskd->nbit;
00317 for (a = n1 = 0; j; j--) {
00318 olen = *len;
00319 i = get_bit_raw(fskd, buffer, len);
00320 buffer += (olen - *len);
00321 if (i == -1)
00322 return -1;
00323 if (i)
00324 n1++;
00325 a >>= 1;
00326 a |= i;
00327 }
00328 j = 8 - fskd->nbit;
00329 a >>= j;
00330
00331
00332 if (fskd->parity) {
00333 olen = *len;
00334 i = get_bit_raw(fskd, buffer, len);
00335 buffer += (olen - *len);
00336 if (i == -1)
00337 return -1;
00338 if (i)
00339 n1++;
00340 if (fskd->parity == 1) {
00341 if (n1 & 1)
00342 a |= 0x100;
00343 } else {
00344 if (!(n1 & 1))
00345 a |= 0x100;
00346 }
00347 }
00348
00349
00350
00351 for (j = fskd->instop; j; j--) {
00352 r = get_bit_raw(fskd, buffer, len);
00353 if (r == -1)
00354 return -1;
00355 if (!r)
00356 a |= 0x200;
00357 }
00358
00359
00360
00361
00362
00363
00364 *outbyte = a;
00365 fskd->state = STATE_SEARCH_STARTBIT;
00366 return 1;
00367 }