00001
00019
00020
00021
00022
00023
00024
00025
00026 #include <sent/stddefs.h>
00027 #include <sent/adin.h>
00028
00030 #define USE_HEADER_COEF
00031
00033
00034 #ifdef USE_HEADER_COEF
00036 #include "lpfcoef_3to4.h"
00037 #include "lpfcoef_2to1.h"
00038 #endif
00039
00040
00041 #define mod(x) ((x) & (DS_RBSIZE -1))
00042
00043
00044 #ifdef USE_HEADER_COEF
00045
00050 static void
00051 load_filter_from_header_2to1(DS_FILTER *f)
00052 {
00053 int i;
00054
00055
00056 for(i=0;i<DS_RBSIZE + 1; i++) {
00057 if (i >= lpfcoef_2to1_num) break;
00058 f->hdn[i] = lpfcoef_2to1[i];
00059 }
00060 f->hdn_len = i - 1;
00061 }
00062
00068 static void
00069 load_filter_from_header_3to4(DS_FILTER *f)
00070 {
00071 int i;
00072
00073
00074 for(i=0;i<DS_RBSIZE + 1; i++) {
00075 if (i >= lpfcoef_3to4_num) break;
00076 f->hdn[i] = lpfcoef_3to4[i];
00077 }
00078 f->hdn_len = i - 1;
00079 }
00080
00081 #else
00082
00089 static boolean
00090 load_filter(DS_FILTER *f, char *coeffile)
00091 {
00092 FILE *fp;
00093 static char buf[512];
00094 int i;
00095
00096
00097 if ((fp = fopen(coeffile, "r")) == NULL) {
00098 jlog("Error: ds48to16: failed to open filter coefficient file \"%s\"\n", coeffile);
00099 return FALSE;
00100 }
00101 for(i=0;i<DS_RBSIZE + 1; i++) {
00102 if (fgets(buf, 512, fp) == NULL) break;
00103 f->hdn[i] = atof(buf);
00104 }
00105 fclose(fp);
00106 if (i <= 0) {
00107 jlog("Error: ds48to16: failed to read filter coefficient from \"%s\"\n", coeffile);
00108 return FALSE;
00109 }
00110 f->hdn_len = i - 1;
00111
00112 return TRUE;
00113 }
00114
00115 #endif
00116
00124 static void
00125 init_filter(DS_FILTER *f, int d, int u)
00126 {
00127
00128 f->decrate = d;
00129 f->intrate = u;
00130
00131
00132 f->delay = f->hdn_len / (2 * f->decrate);
00133
00134
00135 f->indx = 0;
00136
00137
00138 f->bp = 0;
00139
00140
00141 f->count = 1;
00142 }
00143
00151 static void
00152 firin(DS_FILTER *f, double in)
00153 {
00154 f->indx = mod(f->indx - 1);
00155 f->rb[f->indx] = in;
00156 }
00157
00166 static double
00167 firout(DS_FILTER *f, int os)
00168 {
00169 double out;
00170 int k, l;
00171
00172 out = 0.0;
00173 for(k = os, l = f->indx ; k <= f->hdn_len; k += f->intrate, l = mod(l + 1)) {
00174 out += f->rb[l] * f->hdn[k];
00175 }
00176 return(out);
00177 }
00178
00191 static int
00192 do_filter(DS_FILTER *f, double *dst, double *src, int len, int maxlen)
00193 {
00194 int dstlen;
00195 int s;
00196 int d;
00197 int i, k;
00198
00199 s = 0;
00200 dstlen = 0;
00201
00202 while(1) {
00203
00204
00205 while (f->bp < DS_BUFSIZE) {
00206 if (s >= len) break;
00207 f->x[f->bp++] = src[s++];
00208 }
00209 if (f->bp < DS_BUFSIZE) {
00210
00211 break;
00212 }
00213
00214 d = 0;
00215 for(k=0;k<f->bp;k++) {
00216 firin(f, f->x[k]);
00217 for(i=0;i<f->intrate;i++) {
00218 f->count--;
00219 if(f->count == 0) {
00220 f->y[d++] = firout(f, i);
00221 f->count = f->decrate;
00222 }
00223 }
00224 }
00225
00226 if(f->delay) {
00227 if(d > f->delay) {
00228
00229 d -= f->delay;
00230 for(i=0;i<d;i++) {
00231 if (dstlen >= maxlen) break;
00232 dst[dstlen++] = f->y[f->delay + i];
00233 }
00234 f->delay = 0;
00235 if (dstlen >= maxlen) {
00236 jlog("Error: ds48to16: buffer overflow in down sampling, inputs may be lost!\n");
00237 return -1;
00238 }
00239 } else {
00240
00241 f->delay -= d;
00242 }
00243 } else {
00244
00245 for(i=0;i<d;i++) {
00246 if (dstlen >= maxlen) break;
00247 dst[dstlen++] = f->y[i];
00248 }
00249 if (dstlen >= maxlen) {
00250 jlog("Error: ds48to16: buffer overflow in down sampling, inputs may be lost!\n");
00251 return -1;
00252 }
00253 }
00254
00255
00256 f->bp -= DS_BUFSIZE;
00257 }
00258
00259 return dstlen;
00260 }
00261
00262
00268 DS_BUFFER *
00269 ds48to16_new()
00270 {
00271 DS_BUFFER *ds;
00272 int i;
00273
00274
00275
00276 ds = (DS_BUFFER *)mymalloc(sizeof(DS_BUFFER));
00277 for(i=0;i<3;i++) ds->fir[i] = (DS_FILTER *)mymalloc(sizeof(DS_FILTER));
00278 #ifdef USE_HEADER_COEF
00279
00280 load_filter_from_header_3to4(ds->fir[0]);
00281 load_filter_from_header_2to1(ds->fir[1]);
00282 load_filter_from_header_2to1(ds->fir[2]);
00283 jlog("Stat: ds48to16: loaded FIR filters for down sampling\n");
00284 #else
00285
00286 if (load_filter(ds->fir[0], "lpfcoef.3to4") == FALSE) return FALSE;
00287 if (load_filter(ds->fir[1], "lpfcoef.2to1") == FALSE) return FALSE;
00288 if (load_filter(ds->fir[2], "lpfcoef.2to1") == FALSE) return FALSE;
00289 jlog("Stat: ds48to16: initialize FIR filters for down sampling\n");
00290 #endif
00291 init_filter(ds->fir[0], 3, 4);
00292 init_filter(ds->fir[1], 2, 1);
00293 init_filter(ds->fir[2], 2, 1);
00294
00295 ds->buflen = 0;
00296
00297 return(ds);
00298 }
00299
00306 void
00307 ds48to16_free(DS_BUFFER *ds)
00308 {
00309 int i;
00310
00311 if (ds->buflen != 0) {
00312 for(i=0;i<4;i++) free(ds->buf[i]);
00313 }
00314 for(i=0;i<3;i++) free(ds->fir[i]);
00315
00316 free(ds);
00317 }
00318
00331 int
00332 ds48to16(SP16 *dst, SP16 *src, int srclen, int maxdstlen, DS_BUFFER *ds)
00333 {
00334 int i, n, tmplen;
00335
00336 if (ds->buflen == 0) {
00337 ds->buflen = srclen * 2;
00338 for(n=0;n<4;n++) {
00339 ds->buf[n] = (double *)mymalloc(sizeof(double) * ds->buflen);
00340 }
00341 } else if (ds->buflen < srclen * 2) {
00342 ds->buflen = srclen * 2;
00343 for(n=0;n<4;n++) {
00344 ds->buf[n] = (double *)myrealloc(ds->buf[n], sizeof(double) * ds->buflen);
00345 }
00346 }
00347
00348 for(i=0;i<srclen;i++) ds->buf[0][i] = src[i];
00349
00350 tmplen = srclen;
00351 for(n=0;n<3;n++) {
00352 tmplen = do_filter(ds->fir[n], ds->buf[n+1], ds->buf[n], tmplen, ds->buflen);
00353 }
00354
00355 if (maxdstlen < tmplen) {
00356 jlog("Error: ds48to16: down sampled num > required!\n");
00357 return -1;
00358 }
00359 for(i=0;i<tmplen;i++) dst[i] = ds->buf[3][i];
00360
00361 return(tmplen);
00362 }