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