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 #include <stdio.h>
00028
00029 #include "asterisk.h"
00030
00031 ASTERISK_FILE_VERSION(__FILE__, "$Revision: 7221 $")
00032
00033 #include "asterisk/fskmodem.h"
00034
00035 #define NBW 2
00036 #define BWLIST {75,800}
00037 #define NF 6
00038 #define FLIST {1400,1800,1200,2200,1300,2100}
00039
00040 #define STATE_SEARCH_STARTBIT 0
00041 #define STATE_SEARCH_STARTBIT2 1
00042 #define STATE_SEARCH_STARTBIT3 2
00043 #define STATE_GET_BYTE 3
00044
00045 static inline float get_sample(short **buffer, int *len)
00046 {
00047 float retval;
00048 retval = (float) **buffer / 256;
00049 (*buffer)++;
00050 (*len)--;
00051 return retval;
00052 }
00053
00054 #define GET_SAMPLE get_sample(&buffer, len)
00055
00056
00057
00058
00059
00060
00061
00062 static double coef_in[NF][NBW][8]={
00063 #include "coef_in.h"
00064 };
00065
00066
00067
00068
00069
00070
00071
00072 static double coef_out[NBW][8]={
00073 #include "coef_out.h"
00074 };
00075
00076
00077
00078 static inline float filtroM(fsk_data *fskd,float in)
00079 {
00080 int i,j;
00081 double s;
00082 double *pc;
00083
00084 pc=&coef_in[fskd->f_mark_idx][fskd->bw][0];
00085 fskd->fmxv[(fskd->fmp+6)&7]=in*(*pc++);
00086
00087 s=(fskd->fmxv[(fskd->fmp+6)&7] - fskd->fmxv[fskd->fmp]) + 3 * (fskd->fmxv[(fskd->fmp+2)&7] - fskd->fmxv[(fskd->fmp+4)&7]);
00088 for (i=0,j=fskd->fmp;i<6;i++,j++) s+=fskd->fmyv[j&7]*(*pc++);
00089 fskd->fmyv[j&7]=s;
00090 fskd->fmp++; fskd->fmp&=7;
00091 return s;
00092 }
00093
00094
00095 static inline float filtroS(fsk_data *fskd,float in)
00096 {
00097 int i,j;
00098 double s;
00099 double *pc;
00100
00101 pc=&coef_in[fskd->f_space_idx][fskd->bw][0];
00102 fskd->fsxv[(fskd->fsp+6)&7]=in*(*pc++);
00103
00104 s=(fskd->fsxv[(fskd->fsp+6)&7] - fskd->fsxv[fskd->fsp]) + 3 * (fskd->fsxv[(fskd->fsp+2)&7] - fskd->fsxv[(fskd->fsp+4)&7]);
00105 for (i=0,j=fskd->fsp;i<6;i++,j++) s+=fskd->fsyv[j&7]*(*pc++);
00106 fskd->fsyv[j&7]=s;
00107 fskd->fsp++; fskd->fsp&=7;
00108 return s;
00109 }
00110
00111
00112 static inline float filtroL(fsk_data *fskd,float in)
00113 {
00114 int i,j;
00115 double s;
00116 double *pc;
00117
00118 pc=&coef_out[fskd->bw][0];
00119 fskd->flxv[(fskd->flp + 6) & 7]=in * (*pc++);
00120
00121 s= (fskd->flxv[fskd->flp] + fskd->flxv[(fskd->flp+6)&7]) +
00122 6 * (fskd->flxv[(fskd->flp+1)&7] + fskd->flxv[(fskd->flp+5)&7]) +
00123 15 * (fskd->flxv[(fskd->flp+2)&7] + fskd->flxv[(fskd->flp+4)&7]) +
00124 20 * fskd->flxv[(fskd->flp+3)&7];
00125
00126 for (i=0,j=fskd->flp;i<6;i++,j++) s+=fskd->flyv[j&7]*(*pc++);
00127 fskd->flyv[j&7]=s;
00128 fskd->flp++; fskd->flp&=7;
00129 return s;
00130 }
00131
00132 static inline int demodulador(fsk_data *fskd, float *retval, float x)
00133 {
00134 float xS,xM;
00135
00136 fskd->cola_in[fskd->pcola]=x;
00137
00138 xS=filtroS(fskd,x);
00139 xM=filtroM(fskd,x);
00140
00141 fskd->cola_filtro[fskd->pcola]=xM-xS;
00142
00143 x=filtroL(fskd,xM*xM - xS*xS);
00144
00145 fskd->cola_demod[fskd->pcola++]=x;
00146 fskd->pcola &= (NCOLA-1);
00147
00148 *retval = x;
00149 return(0);
00150 }
00151
00152 static int get_bit_raw(fsk_data *fskd, short *buffer, int *len)
00153 {
00154
00155 float x,spb,spb2,ds;
00156 int f;
00157
00158 spb=fskd->spb;
00159 if (fskd->spb == 7) spb = 8000.0 / 1200.0;
00160 ds=spb/32.;
00161 spb2=spb/2.;
00162
00163 for (f=0;;){
00164 if (demodulador(fskd,&x, GET_SAMPLE)) return(-1);
00165 if ((x*fskd->x0)<0) {
00166 if (!f) {
00167 if (fskd->cont<(spb2)) fskd->cont+=ds; else fskd->cont-=ds;
00168 f=1;
00169 }
00170 }
00171 fskd->x0=x;
00172 fskd->cont+=1.;
00173 if (fskd->cont>spb) {
00174 fskd->cont-=spb;
00175 break;
00176 }
00177 }
00178 f=(x>0)?0x80:0;
00179 return(f);
00180 }
00181
00182 int fsk_serie(fsk_data *fskd, short *buffer, int *len, int *outbyte)
00183 {
00184 int a;
00185 int i,j,n1,r;
00186 int samples=0;
00187 int olen;
00188 switch(fskd->state) {
00189
00190 case STATE_SEARCH_STARTBIT2:
00191 goto search_startbit2;
00192 case STATE_SEARCH_STARTBIT3:
00193 goto search_startbit3;
00194 case STATE_GET_BYTE:
00195 goto getbyte;
00196 }
00197
00198 do {
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215 if (demodulador(fskd,&fskd->x1,GET_SAMPLE)) return(-1);
00216 samples++;
00217 for(;;)
00218 {
00219 search_startbit2:
00220 if (!*len) {
00221 fskd->state = STATE_SEARCH_STARTBIT2;
00222 return 0;
00223 }
00224 samples++;
00225 if (demodulador(fskd,&fskd->x2,GET_SAMPLE)) return(-1);
00226 #if 0
00227 printf("x2 = %5.5f ", fskd->x2);
00228 #endif
00229 if (fskd->x2 < -0.5) break;
00230 }
00231 search_startbit3:
00232
00233 i=fskd->spb/2;
00234 if (*len < i) {
00235 fskd->state = STATE_SEARCH_STARTBIT3;
00236 return 0;
00237 }
00238 for(;i;i--) { if (demodulador(fskd,&fskd->x1,GET_SAMPLE)) return(-1);
00239 #if 0
00240 printf("x1 = %5.5f ", fskd->x1);
00241 #endif
00242 samples++; }
00243
00244
00245
00246 } while (fskd->x1>0);
00247 fskd->state = STATE_GET_BYTE;
00248
00249 getbyte:
00250
00251
00252
00253 if (fskd->nbit < 8) {
00254 if (*len < 1320)
00255 return 0;
00256 } else {
00257 if (*len < 80)
00258 return 0;
00259 }
00260
00261 j=fskd->nbit;
00262 for (a=n1=0;j;j--) {
00263 olen = *len;
00264 i=get_bit_raw(fskd, buffer, len);
00265 buffer += (olen - *len);
00266 if (i == -1) return(-1);
00267 if (i) n1++;
00268 a>>=1; a|=i;
00269 }
00270 j=8-fskd->nbit;
00271 a>>=j;
00272
00273
00274 if (fskd->paridad) {
00275 olen = *len;
00276 i=get_bit_raw(fskd, buffer, len);
00277 buffer += (olen - *len);
00278 if (i == -1) return(-1);
00279 if (i) n1++;
00280 if (fskd->paridad==1) {
00281 if (n1&1) a|=0x100;
00282 } else {
00283 if (!(n1&1)) a|=0x100;
00284 }
00285 }
00286
00287
00288
00289 for (j=fskd->nstop;j;j--) {
00290 r = get_bit_raw(fskd, buffer, len);
00291 if (r == -1) return(-1);
00292 if (!r) a|=0x200;
00293 }
00294
00295
00296
00297
00298
00299 *outbyte = a;
00300 fskd->state = STATE_SEARCH_STARTBIT;
00301 return 1;
00302 }