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