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