sample/radio/binding/rtl_fm.c

   1 /*
   2  * rtl-sdr, turns your Realtek RTL2832 based DVB dongle into a SDR receiver
   3  * Copyright (C) 2012 by Steve Markgraf <steve@steve-m.de>
   4  * Copyright (C) 2012 by Hoernchen <la@tfc-server.de>
   5  * Copyright (C) 2012 by Kyle Keen <keenerd@gmail.com>
   6  * Copyright (C) 2013 by Elias Oenal <EliasOenal@gmail.com>
   7  * Copyright (C) 2016, 2017 Konsulko Group
   8  *
   9  * This program is free software: you can redistribute it and/or modify
  10  * it under the terms of the GNU General Public License as published by
  11  * the Free Software Foundation, either version 2 of the License, or
  12  * (at your option) any later version.
  13  *
  14  * This program is distributed in the hope that it will be useful,
  15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17  * GNU General Public License for more details.
  18  *
  19  * You should have received a copy of the GNU General Public License
  20  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  21  */
  22
  23 /*
  24  * Note that this version replaces the standalone main() with separate
  25  * init/start/stop API calls to allow building into another application.
  26  * Other than removing the separate controller thread and adding an output
  27  * function callback, other changes have been kept to a minimum to
  28  * potentially allow using other rtl_fm features by modifying rtl_fm_init.
  29  *
  30  * December 2016, Scott Murray <scott.murray@konsulko.com>
  31  */
  32
  33 /*
  34  * written because people could not do real time
  35  * FM demod on Atom hardware with GNU radio
  36  * based on rtl_sdr.c and rtl_tcp.c
  37  *
  38  * lots of locks, but that is okay
  39  * (no many-to-many locks)
  40  *
  41  * todo:
  42  *       sanity checks
  43  *       scale squelch to other input parameters
  44  *       test all the demodulations
  45  *       pad output on hop
  46  *       frequency ranges could be stored better
  47  *       scaled AM demod amplification
  48  *       auto-hop after time limit
  49  *       peak detector to tune onto stronger signals
  50  *       fifo for active hop frequency
  51  *       clips
  52  *       noise squelch
  53  *       merge stereo patch
  54  *       merge soft agc patch
  55  *       merge udp patch
  56  *       testmode to detect overruns
  57  *       watchdog to reset bad dongle
  58  *       fix oversampling
  59  */
  60
  61 #include <errno.h>
  62 #include <signal.h>
  63 #include <string.h>
  64 #include <stdio.h>
  65 #include <stdlib.h>
  66 #include <unistd.h>
  67 #include <math.h>
  68 #include <pthread.h>
  69
  70 #include "rtl-sdr.h"
  71 #include "rtl_fm.h"
  72 #include "convenience/convenience.h"
  73
  74 #define DEFAULT_SAMPLE_RATE             24000
  75 #define DEFAULT_BUF_LENGTH              RTL_FM_DEFAULT_BUF_LENGTH
  76 #define MAXIMUM_OVERSAMPLE              RTL_FM_MAXIMUM_OVERSAMPLE
  77 #define MAXIMUM_BUF_LENGTH              RTL_FM_MAXIMUM_BUF_LENGTH
  78 #define AUTO_GAIN                       -100
  79 #define BUFFER_DUMP                     4096
  80
  81 #define FREQUENCIES_LIMIT               1000
  82
  83 #define DEFAULT_SQUELCH_LEVEL           140
  84 #define DEFAULT_CONSEQ_SQUELCH          10
  85
  86 static volatile int do_exit = 0;
  87 static int lcm_post[17] = {1,1,1,3,1,5,3,7,1,9,5,11,3,13,7,15,1};
  88 static int ACTUAL_BUF_LENGTH;
  89
  90 static int *atan_lut = NULL;
  91 static int atan_lut_size = 131072; /* 512 KB */
  92 static int atan_lut_coef = 8;
  93
  94 struct dongle_state
  95 {
  96         int      exit_flag;
  97         pthread_t thread;
  98         rtlsdr_dev_t *dev;
  99         int      dev_index;
 100         uint32_t freq;
 101         uint32_t rate;
 102         int      gain;
 103         uint16_t buf16[MAXIMUM_BUF_LENGTH];
 104         uint32_t buf_len;
 105         int      ppm_error;
 106         int      offset_tuning;
 107         int      direct_sampling;
 108         int      mute;
 109         struct demod_state *demod_target;
 110 };
 111
 112 struct demod_state
 113 {
 114         int      exit_flag;
 115         pthread_t thread;
 116         int16_t  lowpassed[MAXIMUM_BUF_LENGTH];
 117         int      lp_len;
 118         int16_t  lp_i_hist[10][6];
 119         int16_t  lp_q_hist[10][6];
 120         int16_t  result[MAXIMUM_BUF_LENGTH];
 121         int16_t  droop_i_hist[9];
 122         int16_t  droop_q_hist[9];
 123         int      result_len;
 124         int      rate_in;
 125         int      rate_out;
 126         int      rate_out2;
 127         int      now_r, now_j;
 128         int      pre_r, pre_j;
 129         int      prev_index;
 130         int      downsample;    /* min 1, max 256 */
 131         int      post_downsample;
 132         int      output_scale;
 133         int      squelch_level, conseq_squelch, squelch_hits, terminate_on_squelch;
 134         int      downsample_passes;
 135         int      comp_fir_size;
 136         int      custom_atan;
 137         int      deemph, deemph_a;
 138         int      now_lpr;
 139         int      prev_lpr_index;
 140         int      dc_block, dc_avg;
 141         void     (*mode_demod)(struct demod_state*);
 142         pthread_rwlock_t rw;
 143         pthread_cond_t ready;
 144         pthread_mutex_t ready_m;
 145         struct output_state *output_target;
 146 };
 147
 148 struct output_state
 149 {
 150         int      exit_flag;
 151         pthread_t thread;
 152         rtl_fm_output_fn_t output_fn;
 153         void     *output_fn_data;
 154         int16_t  result[MAXIMUM_BUF_LENGTH];
 155         int      result_len;
 156         int      rate;
 157         pthread_rwlock_t rw;
 158         pthread_cond_t ready;
 159         pthread_mutex_t ready_m;
 160 };
 161
 162 struct controller_state
 163 {
 164         int      exit_flag;
 165         pthread_t thread;
 166         uint32_t freqs[FREQUENCIES_LIMIT];
 167         int      freq_len;
 168         int      freq_now;
 169         int      edge;
 170         int      wb_mode;
 171         pthread_cond_t hop;
 172         pthread_mutex_t hop_m;
 173
 174         void (*freq_callback)(uint32_t, void*);
 175         void *freq_callback_data;
 176
 177         int scanning;
 178         int scan_direction;
 179         void (*scan_callback)(uint32_t, void*);
 180         void *scan_callback_data;
 181         uint32_t scan_step;
 182         uint32_t scan_min;
 183         uint32_t scan_max;
 184         int scan_squelch_level;
 185         int scan_squelch_count;
 186 };
 187
 188 // multiple of these, eventually
 189 struct dongle_state dongle;
 190 struct demod_state demod;
 191 struct output_state output;
 192 struct controller_state controller;
 193
 194 #if 0
 195 static void sighandler(int signum)
 196 {
 197         fprintf(stderr, "Signal caught, exiting!\n");
 198         do_exit = 1;
 199         rtlsdr_cancel_async(dongle.dev);
 200 }
 201 #endif
 202
 203 /* more cond dumbness */
 204 #define safe_cond_signal(n, m) pthread_mutex_lock(m); pthread_cond_signal(n); pthread_mutex_unlock(m)
 205 #define safe_cond_wait(n, m) pthread_mutex_lock(m); pthread_cond_wait(n, m); pthread_mutex_unlock(m)
 206
 207 /* {length, coef, coef, coef}  and scaled by 2^15
 208    for now, only length 9, optimal way to get +85% bandwidth */
 209 #define CIC_TABLE_MAX 10
 210 int cic_9_tables[][10] = {
 211         {0,},
 212         {9, -156,  -97, 2798, -15489, 61019, -15489, 2798,  -97, -156},
 213         {9, -128, -568, 5593, -24125, 74126, -24125, 5593, -568, -128},
 214         {9, -129, -639, 6187, -26281, 77511, -26281, 6187, -639, -129},
 215         {9, -122, -612, 6082, -26353, 77818, -26353, 6082, -612, -122},
 216         {9, -120, -602, 6015, -26269, 77757, -26269, 6015, -602, -120},
 217         {9, -120, -582, 5951, -26128, 77542, -26128, 5951, -582, -120},
 218         {9, -119, -580, 5931, -26094, 77505, -26094, 5931, -580, -119},
 219         {9, -119, -578, 5921, -26077, 77484, -26077, 5921, -578, -119},
 220         {9, -119, -577, 5917, -26067, 77473, -26067, 5917, -577, -119},
 221         {9, -199, -362, 5303, -25505, 77489, -25505, 5303, -362, -199},
 222 };
 223
 224 void rotate_90(unsigned char *buf, uint32_t len)
 225 /* 90 rotation is 1+0j, 0+1j, -1+0j, 0-1j
 226    or [0, 1, -3, 2, -4, -5, 7, -6] */
 227 {
 228         uint32_t i;
 229         unsigned char tmp;
 230         for (i=0; i<len; i+=8) {
 231                 /* uint8_t negation = 255 - x */
 232                 tmp = 255 - buf[i+3];
 233                 buf[i+3] = buf[i+2];
 234                 buf[i+2] = tmp;
 235
 236                 buf[i+4] = 255 - buf[i+4];
 237                 buf[i+5] = 255 - buf[i+5];
 238
 239                 tmp = 255 - buf[i+6];
 240                 buf[i+6] = buf[i+7];
 241                 buf[i+7] = tmp;
 242         }
 243 }
 244
 245 void low_pass(struct demod_state *d)
 246 /* simple square window FIR */
 247 {
 248         int i=0, i2=0;
 249         while (i < d->lp_len) {
 250                 d->now_r += d->lowpassed[i];
 251                 d->now_j += d->lowpassed[i+1];
 252                 i += 2;
 253                 d->prev_index++;
 254                 if (d->prev_index < d->downsample) {
 255                         continue;
 256                 }
 257                 d->lowpassed[i2]   = d->now_r; // * d->output_scale;
 258                 d->lowpassed[i2+1] = d->now_j; // * d->output_scale;
 259                 d->prev_index = 0;
 260                 d->now_r = 0;
 261                 d->now_j = 0;
 262                 i2 += 2;
 263         }
 264         d->lp_len = i2;
 265 }
 266
 267 int low_pass_simple(int16_t *signal2, int len, int step)
 268 // no wrap around, length must be multiple of step
 269 {
 270         int i, i2, sum;
 271         for(i=0; i < len; i+=step) {
 272                 sum = 0;
 273                 for(i2=0; i2<step; i2++) {
 274                         sum += (int)signal2[i + i2];
 275                 }
 276                 //signal2[i/step] = (int16_t)(sum / step);
 277                 signal2[i/step] = (int16_t)(sum);
 278         }
 279         signal2[i/step + 1] = signal2[i/step];
 280         return len / step;
 281 }
 282
 283 void low_pass_real(struct demod_state *s)
 284 /* simple square window FIR */
 285 // add support for upsampling?
 286 {
 287         int i=0, i2=0;
 288         int fast = (int)s->rate_out;
 289         int slow = s->rate_out2;
 290         while (i < s->result_len) {
 291                 s->now_lpr += s->result[i];
 292                 i++;
 293                 s->prev_lpr_index += slow;
 294                 if (s->prev_lpr_index < fast) {
 295                         continue;
 296                 }
 297                 s->result[i2] = (int16_t)(s->now_lpr / (fast/slow));
 298                 s->prev_lpr_index -= fast;
 299                 s->now_lpr = 0;
 300                 i2 += 1;
 301         }
 302         s->result_len = i2;
 303 }
 304
 305 void fifth_order(int16_t *data, int length, int16_t *hist)
 306 /* for half of interleaved data */
 307 {
 308         int i;
 309         int16_t a, b, c, d, e, f;
 310         a = hist[1];
 311         b = hist[2];
 312         c = hist[3];
 313         d = hist[4];
 314         e = hist[5];
 315         f = data[0];
 316         /* a downsample should improve resolution, so don't fully shift */
 317         data[0] = (a + (b+e)*5 + (c+d)*10 + f) >> 4;
 318         for (i=4; i<length; i+=4) {
 319                 a = c;
 320                 b = d;
 321                 c = e;
 322                 d = f;
 323                 e = data[i-2];
 324                 f = data[i];
 325                 data[i/2] = (a + (b+e)*5 + (c+d)*10 + f) >> 4;
 326         }
 327         /* archive */
 328         hist[0] = a;
 329         hist[1] = b;
 330         hist[2] = c;
 331         hist[3] = d;
 332         hist[4] = e;
 333         hist[5] = f;
 334 }
 335
 336 void generic_fir(int16_t *data, int length, int *fir, int16_t *hist)
 337 /* Okay, not at all generic.  Assumes length 9, fix that eventually. */
 338 {
 339         int d, temp, sum;
 340         for (d=0; d<length; d+=2) {
 341                 temp = data[d];
 342                 sum = 0;
 343                 sum += (hist[0] + hist[8]) * fir[1];
 344                 sum += (hist[1] + hist[7]) * fir[2];
 345                 sum += (hist[2] + hist[6]) * fir[3];
 346                 sum += (hist[3] + hist[5]) * fir[4];
 347                 sum +=            hist[4]  * fir[5];
 348                 data[d] = sum >> 15 ;
 349                 hist[0] = hist[1];
 350                 hist[1] = hist[2];
 351                 hist[2] = hist[3];
 352                 hist[3] = hist[4];
 353                 hist[4] = hist[5];
 354                 hist[5] = hist[6];
 355                 hist[6] = hist[7];
 356                 hist[7] = hist[8];
 357                 hist[8] = temp;
 358         }
 359 }
 360
 361 /* define our own complex math ops
 362    because ARMv5 has no hardware float */
 363
 364 void multiply(int ar, int aj, int br, int bj, int *cr, int *cj)
 365 {
 366         *cr = ar*br - aj*bj;
 367         *cj = aj*br + ar*bj;
 368 }
 369
 370 int polar_discriminant(int ar, int aj, int br, int bj)
 371 {
 372         int cr, cj;
 373         double angle;
 374         multiply(ar, aj, br, -bj, &cr, &cj);
 375         angle = atan2((double)cj, (double)cr);
 376         return (int)(angle / 3.14159 * (1<<14));
 377 }
 378
 379 int fast_atan2(int y, int x)
 380 /* pre scaled for int16 */
 381 {
 382         int yabs, angle;
 383         int pi4=(1<<12), pi34=3*(1<<12);  // note pi = 1<<14
 384         if (x==0 && y==0) {
 385                 return 0;
 386         }
 387         yabs = y;
 388         if (yabs < 0) {
 389                 yabs = -yabs;
 390         }
 391         if (x >= 0) {
 392                 angle = pi4  - pi4 * (x-yabs) / (x+yabs);
 393         } else {
 394                 angle = pi34 - pi4 * (x+yabs) / (yabs-x);
 395         }
 396         if (y < 0) {
 397                 return -angle;
 398         }
 399         return angle;
 400 }
 401
 402 int polar_disc_fast(int ar, int aj, int br, int bj)
 403 {
 404         int cr, cj;
 405         multiply(ar, aj, br, -bj, &cr, &cj);
 406         return fast_atan2(cj, cr);
 407 }
 408
 409 int atan_lut_init(void)
 410 {
 411         int i = 0;
 412
 413         atan_lut = malloc(atan_lut_size * sizeof(int));
 414
 415         for (i = 0; i < atan_lut_size; i++) {
 416                 atan_lut[i] = (int) (atan((double) i / (1<<atan_lut_coef)) / 3.14159 * (1<<14));
 417         }
 418
 419         return 0;
 420 }
 421
 422 int polar_disc_lut(int ar, int aj, int br, int bj)
 423 {
 424         int cr, cj, x, x_abs;
 425
 426         multiply(ar, aj, br, -bj, &cr, &cj);
 427
 428         /* special cases */
 429         if (cr == 0 || cj == 0) {
 430                 if (cr == 0 && cj == 0)
 431                         {return 0;}
 432                 if (cr == 0 && cj > 0)
 433                         {return 1 << 13;}
 434                 if (cr == 0 && cj < 0)
 435                         {return -(1 << 13);}
 436                 if (cj == 0 && cr > 0)
 437                         {return 0;}
 438                 if (cj == 0 && cr < 0)
 439                         {return 1 << 14;}
 440         }
 441
 442         /* real range -32768 - 32768 use 64x range -> absolute maximum: 2097152 */
 443         x = (cj << atan_lut_coef) / cr;
 444         x_abs = abs(x);
 445
 446         if (x_abs >= atan_lut_size) {
 447                 /* we can use linear range, but it is not necessary */
 448                 return (cj > 0) ? 1<<13 : -1<<13;
 449         }
 450
 451         if (x > 0) {
 452                 return (cj > 0) ? atan_lut[x] : atan_lut[x] - (1<<14);
 453         } else {
 454                 return (cj > 0) ? (1<<14) - atan_lut[-x] : -atan_lut[-x];
 455         }
 456
 457         return 0;
 458 }
 459
 460 void fm_demod(struct demod_state *fm)
 461 {
 462         int i, pcm;
 463         int16_t *lp = fm->lowpassed;
 464         pcm = polar_discriminant(lp[0], lp[1],
 465                 fm->pre_r, fm->pre_j);
 466         fm->result[0] = (int16_t)pcm;
 467         for (i = 2; i < (fm->lp_len-1); i += 2) {
 468                 switch (fm->custom_atan) {
 469                 case 0:
 470                         pcm = polar_discriminant(lp[i], lp[i+1],
 471                                 lp[i-2], lp[i-1]);
 472                         break;
 473                 case 1:
 474                         pcm = polar_disc_fast(lp[i], lp[i+1],
 475                                 lp[i-2], lp[i-1]);
 476                         break;
 477                 case 2:
 478                         pcm = polar_disc_lut(lp[i], lp[i+1],
 479                                 lp[i-2], lp[i-1]);
 480                         break;
 481                 }
 482                 fm->result[i/2] = (int16_t)pcm;
 483         }
 484         fm->pre_r = lp[fm->lp_len - 2];
 485         fm->pre_j = lp[fm->lp_len - 1];
 486         fm->result_len = fm->lp_len/2;
 487 }
 488
 489 void am_demod(struct demod_state *fm)
 490 // todo, fix this extreme laziness
 491 {
 492         int i, pcm;
 493         int16_t *lp = fm->lowpassed;
 494         int16_t *r  = fm->result;
 495         for (i = 0; i < fm->lp_len; i += 2) {
 496                 // hypot uses floats but won't overflow
 497                 //r[i/2] = (int16_t)hypot(lp[i], lp[i+1]);
 498                 pcm = lp[i] * lp[i];
 499                 pcm += lp[i+1] * lp[i+1];
 500                 r[i/2] = (int16_t)sqrt(pcm) * fm->output_scale;
 501         }
 502         fm->result_len = fm->lp_len/2;
 503         // lowpass? (3khz)  highpass?  (dc)
 504 }
 505
 506 void usb_demod(struct demod_state *fm)
 507 {
 508         int i, pcm;
 509         int16_t *lp = fm->lowpassed;
 510         int16_t *r  = fm->result;
 511         for (i = 0; i < fm->lp_len; i += 2) {
 512                 pcm = lp[i] + lp[i+1];
 513                 r[i/2] = (int16_t)pcm * fm->output_scale;
 514         }
 515         fm->result_len = fm->lp_len/2;
 516 }
 517
 518 void lsb_demod(struct demod_state *fm)
 519 {
 520         int i, pcm;
 521         int16_t *lp = fm->lowpassed;
 522         int16_t *r  = fm->result;
 523         for (i = 0; i < fm->lp_len; i += 2) {
 524                 pcm = lp[i] - lp[i+1];
 525                 r[i/2] = (int16_t)pcm * fm->output_scale;
 526         }
 527         fm->result_len = fm->lp_len/2;
 528 }
 529
 530 void raw_demod(struct demod_state *fm)
 531 {
 532         int i;
 533         for (i = 0; i < fm->lp_len; i++) {
 534                 fm->result[i] = (int16_t)fm->lowpassed[i];
 535         }
 536         fm->result_len = fm->lp_len;
 537 }
 538
 539 void deemph_filter(struct demod_state *fm)
 540 {
 541         static int avg;  // cheating...
 542         int i, d;
 543         // de-emph IIR
 544         // avg = avg * (1 - alpha) + sample * alpha;
 545         for (i = 0; i < fm->result_len; i++) {
 546                 d = fm->result[i] - avg;
 547                 if (d > 0) {
 548                         avg += (d + fm->deemph_a/2) / fm->deemph_a;
 549                 } else {
 550                         avg += (d - fm->deemph_a/2) / fm->deemph_a;
 551                 }
 552                 fm->result[i] = (int16_t)avg;
 553         }
 554 }
 555
 556 void dc_block_filter(struct demod_state *fm)
 557 {
 558         int i, avg;
 559         int64_t sum = 0;
 560         for (i=0; i < fm->result_len; i++) {
 561                 sum += fm->result[i];
 562         }
 563         avg = sum / fm->result_len;
 564         avg = (avg + fm->dc_avg * 9) / 10;
 565         for (i=0; i < fm->result_len; i++) {
 566                 fm->result[i] -= avg;
 567         }
 568         fm->dc_avg = avg;
 569 }
 570
 571 int mad(int16_t *samples, int len, int step)
 572 /* mean average deviation */
 573 {
 574         int i=0, sum=0, ave=0;
 575         if (len == 0)
 576                 {return 0;}
 577         for (i=0; i<len; i+=step) {
 578                 sum += samples[i];
 579         }
 580         ave = sum / (len * step);
 581         sum = 0;
 582         for (i=0; i<len; i+=step) {
 583                 sum += abs(samples[i] - ave);
 584         }
 585         return sum / (len / step);
 586 }
 587
 588 int rms(int16_t *samples, int len, int step)
 589 /* largely lifted from rtl_power */
 590 {
 591         int i;
 592         long p, t, s;
 593         double dc, err;
 594
 595         p = t = 0L;
 596         for (i=0; i<len; i+=step) {
 597                 s = (long)samples[i];
 598                 t += s;
 599                 p += s * s;
 600         }
 601         /* correct for dc offset in squares */
 602         dc = (double)(t*step) / (double)len;
 603         err = t * 2 * dc - dc * dc * len;
 604
 605         return (int)sqrt((p-err) / len);
 606 }
 607
 608 void arbitrary_upsample(int16_t *buf1, int16_t *buf2, int len1, int len2)
 609 /* linear interpolation, len1 < len2 */
 610 {
 611         int i = 1;
 612         int j = 0;
 613         int tick = 0;
 614         double frac;  // use integers...
 615         while (j < len2) {
 616                 frac = (double)tick / (double)len2;
 617                 buf2[j] = (int16_t)(buf1[i-1]*(1-frac) + buf1[i]*frac);
 618                 j++;
 619                 tick += len1;
 620                 if (tick > len2) {
 621                         tick -= len2;
 622                         i++;
 623                 }
 624                 if (i >= len1) {
 625                         i = len1 - 1;
 626                         tick = len2;
 627                 }
 628         }
 629 }
 630
 631 void arbitrary_downsample(int16_t *buf1, int16_t *buf2, int len1, int len2)
 632 /* fractional boxcar lowpass, len1 > len2 */
 633 {
 634         int i = 1;
 635         int j = 0;
 636         int tick = 0;
 637         double remainder = 0;
 638         double frac;  // use integers...
 639         buf2[0] = 0;
 640         while (j < len2) {
 641                 frac = 1.0;
 642                 if ((tick + len2) > len1) {
 643                         frac = (double)(len1 - tick) / (double)len2;}
 644                 buf2[j] += (int16_t)((double)buf1[i] * frac + remainder);
 645                 remainder = (double)buf1[i] * (1.0-frac);
 646                 tick += len2;
 647                 i++;
 648                 if (tick > len1) {
 649                         j++;
 650                         buf2[j] = 0;
 651                         tick -= len1;
 652                 }
 653                 if (i >= len1) {
 654                         i = len1 - 1;
 655                         tick = len1;
 656                 }
 657         }
 658         for (j=0; j<len2; j++) {
 659                 buf2[j] = buf2[j] * len2 / len1;}
 660 }
 661
 662 void arbitrary_resample(int16_t *buf1, int16_t *buf2, int len1, int len2)
 663 /* up to you to calculate lengths and make sure it does not go OOB
 664  * okay for buffers to overlap, if you are downsampling */
 665 {
 666         if (len1 < len2) {
 667                 arbitrary_upsample(buf1, buf2, len1, len2);
 668         } else {
 669                 arbitrary_downsample(buf1, buf2, len1, len2);
 670         }
 671 }
 672
 673 void full_demod(struct demod_state *d)
 674 {
 675         int i, ds_p;
 676         int sr = 0;
 677         ds_p = d->downsample_passes;
 678         if (ds_p) {
 679                 for (i=0; i < ds_p; i++) {
 680                         fifth_order(d->lowpassed,   (d->lp_len >> i), d->lp_i_hist[i]);
 681                         fifth_order(d->lowpassed+1, (d->lp_len >> i) - 1, d->lp_q_hist[i]);
 682                 }
 683                 d->lp_len = d->lp_len >> ds_p;
 684                 /* droop compensation */
 685                 if (d->comp_fir_size == 9 && ds_p <= CIC_TABLE_MAX) {
 686                         generic_fir(d->lowpassed, d->lp_len,
 687                                 cic_9_tables[ds_p], d->droop_i_hist);
 688                         generic_fir(d->lowpassed+1, d->lp_len-1,
 689                                 cic_9_tables[ds_p], d->droop_q_hist);
 690                 }
 691         } else {
 692                 low_pass(d);
 693         }
 694         /* power squelch */
 695         if (d->squelch_level) {
 696                 sr = rms(d->lowpassed, d->lp_len, 1);
 697                 if (sr < d->squelch_level) {
 698                         d->squelch_hits++;
 699                         for (i=0; i< d->lp_len; i++) {
 700                                 d->lowpassed[i] = 0;
 701                         }
 702                 } else {
 703                         d->squelch_hits = 0;
 704                 }
 705         }
 706         d->mode_demod(d);  /* lowpassed -> result */
 707         if (d->mode_demod == &raw_demod) {
 708                 return;
 709         }
 710         /* todo, fm noise squelch */
 711         // use nicer filter here too?
 712         if (d->post_downsample > 1) {
 713                 d->result_len = low_pass_simple(d->result, d->result_len, d->post_downsample);}
 714         if (d->deemph) {
 715                 deemph_filter(d);}
 716         if (d->dc_block) {
 717                 dc_block_filter(d);}
 718         if (d->rate_out2 > 0) {
 719                 low_pass_real(d);
 720                 //arbitrary_resample(d->result, d->result, d->result_len, d->result_len * d->rate_out2 / d->rate_out);
 721         }
 722 }
 723
 724 static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx)
 725 {
 726         int i;
 727         struct dongle_state *s = ctx;
 728         struct demod_state *d = s->demod_target;
 729
 730         if (do_exit) {
 731                 return;}
 732         if (!ctx) {
 733                 return;}
 734         if (s->mute) {
 735                 for (i=0; i<s->mute; i++) {
 736                         buf[i] = 127;}
 737                 s->mute = 0;
 738         }
 739         if (!s->offset_tuning) {
 740                 rotate_90(buf, len);}
 741         for (i=0; i<(int)len; i++) {
 742                 s->buf16[i] = (int16_t)buf[i] - 127;}
 743         pthread_rwlock_wrlock(&d->rw);
 744         memcpy(d->lowpassed, s->buf16, 2*len);
 745         d->lp_len = len;
 746         pthread_rwlock_unlock(&d->rw);
 747         safe_cond_signal(&d->ready, &d->ready_m);
 748 }
 749
 750 static void *dongle_thread_fn(void *arg)
 751 {
 752         struct dongle_state *s = arg;
 753         fprintf(stderr, "dongle_thread_fn running\n");
 754         rtlsdr_read_async(s->dev, rtlsdr_callback, s, 0, s->buf_len);
 755         fprintf(stderr, "dongle_thread_fn exited!\n");
 756         return 0;
 757 }
 758
 759 static void rtl_fm_scan_callback(void)
 760 {
 761         struct controller_state *s = &controller;
 762         uint32_t frequency = rtl_fm_get_freq();
 763
 764         if(!s->scanning)
 765                 return;
 766
 767         if(!s->scan_direction) {
 768                 frequency += s->scan_step;
 769                 if(frequency > s->scan_max)
 770                         frequency = s->scan_min;
 771         } else {
 772                 frequency -= s->scan_step;
 773                 if(frequency < s->scan_min)
 774                         frequency = s->scan_max;
 775         }
 776
 777         rtl_fm_set_freq(frequency);
 778 }
 779
 780 static void rtl_fm_scan_end_callback(void)
 781 {
 782         struct controller_state *s = &controller;
 783
 784         if(!s->scanning)
 785                 return;
 786
 787         rtl_fm_scan_stop();
 788
 789         if(s->scan_callback)
 790                 s->scan_callback(rtl_fm_get_freq(), s->scan_callback_data);
 791 }
 792
 793 static void *demod_thread_fn(void *arg)
 794 {
 795         struct demod_state *d = arg;
 796         struct output_state *o = d->output_target;
 797         fprintf(stderr, "demod_thread_fn running\n");
 798         while (!do_exit) {
 799                 safe_cond_wait(&d->ready, &d->ready_m);
 800                 pthread_rwlock_wrlock(&d->rw);
 801                 full_demod(d);
 802                 pthread_rwlock_unlock(&d->rw);
 803                 if (d->exit_flag) {
 804                         do_exit = 1;
 805                 }
 806                 if (d->squelch_level) {
 807                         if(d->squelch_hits > d->conseq_squelch) {
 808                                 d->squelch_hits = d->conseq_squelch + 1;  /* hair trigger */
 809                                 //safe_cond_signal(&controller.hop, &controller.hop_m);
 810                                 rtl_fm_scan_callback();
 811                                 continue;
 812                         } else if(!d->squelch_hits) {
 813                                 rtl_fm_scan_end_callback();
 814                         }
 815                 }
 816                 pthread_rwlock_wrlock(&o->rw);
 817                 memcpy(o->result, d->result, 2*d->result_len);
 818                 o->result_len = d->result_len;
 819                 pthread_rwlock_unlock(&o->rw);
 820                 safe_cond_signal(&o->ready, &o->ready_m);
 821         }
 822         fprintf(stderr, "demod_thread_fn exited!\n");
 823         return 0;
 824 }
 825
 826 static void *output_thread_fn(void *arg)
 827 {
 828         struct output_state *s = arg;
 829         fprintf(stderr, "output_thread_fn running\n");
 830         while (!do_exit) {
 831                 // use timedwait and pad out under runs
 832                 safe_cond_wait(&s->ready, &s->ready_m);
 833                 pthread_rwlock_rdlock(&s->rw);
 834                 if(s->output_fn) {
 835                         s->output_fn(s->result, s->result_len, s->output_fn_data);
 836                 }
 837                 pthread_rwlock_unlock(&s->rw);
 838         }
 839         fprintf(stderr, "output_thread_fn exited!\n");
 840         return 0;
 841 }
 842
 843 static void optimal_settings(int freq, int rate)
 844 {
 845         // giant ball of hacks
 846         // seems unable to do a single pass, 2:1
 847         int capture_freq, capture_rate;
 848         struct dongle_state *d = &dongle;
 849         struct demod_state *dm = &demod;
 850         struct controller_state *cs = &controller;
 851         dm->downsample = (1000000 / dm->rate_in) + 1;
 852         if (dm->downsample_passes) {
 853                 dm->downsample_passes = (int)log2(dm->downsample) + 1;
 854                 dm->downsample = 1 << dm->downsample_passes;
 855         }
 856         capture_freq = freq;
 857         capture_rate = dm->downsample * dm->rate_in;
 858         if (!d->offset_tuning) {
 859                 capture_freq = freq + capture_rate/4;}
 860         capture_freq += cs->edge * dm->rate_in / 2;
 861         dm->output_scale = (1<<15) / (128 * dm->downsample);
 862         if (dm->output_scale < 1) {
 863                 dm->output_scale = 1;}
 864         if (dm->mode_demod == &fm_demod) {
 865                 dm->output_scale = 1;}
 866         d->freq = (uint32_t)capture_freq;
 867         d->rate = (uint32_t)capture_rate;
 868 }
 869
 870
 871 void frequency_range(struct controller_state *s, char *arg)
 872 {
 873         char *start, *stop, *step;
 874         int i;
 875         start = arg;
 876         stop = strchr(start, ':') + 1;
 877         stop[-1] = '\0';
 878         step = strchr(stop, ':') + 1;
 879         step[-1] = '\0';
 880         for(i=(int)atofs(start); i<=(int)atofs(stop); i+=(int)atofs(step))
 881         {
 882                 s->freqs[s->freq_len] = (uint32_t)i;
 883                 s->freq_len++;
 884                 if (s->freq_len >= FREQUENCIES_LIMIT) {
 885                         break;}
 886         }
 887         stop[-1] = ':';
 888         step[-1] = ':';
 889 }
 890
 891 void dongle_init(struct dongle_state *s)
 892 {
 893         s->rate = DEFAULT_SAMPLE_RATE;
 894         s->gain = AUTO_GAIN; // tenths of a dB
 895         s->mute = 0;
 896         s->direct_sampling = 0;
 897         s->offset_tuning = 0;
 898         s->demod_target = &demod;
 899 }
 900
 901 void demod_init(struct demod_state *s)
 902 {
 903         s->rate_in = DEFAULT_SAMPLE_RATE;
 904         s->rate_out = DEFAULT_SAMPLE_RATE;
 905         s->squelch_level = 0;
 906         s->conseq_squelch = DEFAULT_CONSEQ_SQUELCH;
 907         s->terminate_on_squelch = 0;
 908         s->squelch_hits = DEFAULT_CONSEQ_SQUELCH + 1;
 909         s->downsample_passes = 0;
 910         s->comp_fir_size = 0;
 911         s->prev_index = 0;
 912         s->post_downsample = 1;  // once this works, default = 4
 913         s->custom_atan = 0;
 914         s->deemph = 0;
 915         s->rate_out2 = -1;  // flag for disabled
 916         s->mode_demod = &fm_demod;
 917         s->pre_j = s->pre_r = s->now_r = s->now_j = 0;
 918         s->prev_lpr_index = 0;
 919         s->deemph_a = 0;
 920         s->now_lpr = 0;
 921         s->dc_block = 0;
 922         s->dc_avg = 0;
 923         pthread_rwlock_init(&s->rw, NULL);
 924         pthread_cond_init(&s->ready, NULL);
 925         pthread_mutex_init(&s->ready_m, NULL);
 926         s->output_target = &output;
 927 }
 928
 929 void demod_cleanup(struct demod_state *s)
 930 {
 931         pthread_rwlock_destroy(&s->rw);
 932         pthread_cond_destroy(&s->ready);
 933         pthread_mutex_destroy(&s->ready_m);
 934 }
 935
 936 void output_init(struct output_state *s)
 937 {
 938         s->rate = DEFAULT_SAMPLE_RATE;
 939         s->output_fn = NULL;
 940         s->output_fn_data = NULL;
 941         pthread_rwlock_init(&s->rw, NULL);
 942         pthread_cond_init(&s->ready, NULL);
 943         pthread_mutex_init(&s->ready_m, NULL);
 944 }
 945
 946 void output_cleanup(struct output_state *s)
 947 {
 948         pthread_rwlock_destroy(&s->rw);
 949         pthread_cond_destroy(&s->ready);
 950         pthread_mutex_destroy(&s->ready_m);
 951 }
 952
 953 void controller_init(struct controller_state *s)
 954 {
 955         s->freqs[0] = 100000000;
 956         s->freq_len = 0;
 957         s->edge = 0;
 958         s->wb_mode = 0;
 959         pthread_cond_init(&s->hop, NULL);
 960         pthread_mutex_init(&s->hop_m, NULL);
 961 }
 962
 963 void controller_cleanup(struct controller_state *s)
 964 {
 965         pthread_cond_destroy(&s->hop);
 966         pthread_mutex_destroy(&s->hop_m);
 967 }
 968
 969 void sanity_checks(void)
 970 {
 971         if (controller.freq_len == 0) {
 972                 fprintf(stderr, "Please specify a frequency.\n");
 973                 exit(1);
 974         }
 975
 976         if (controller.freq_len >= FREQUENCIES_LIMIT) {
 977                 fprintf(stderr, "Too many channels, maximum %i.\n", FREQUENCIES_LIMIT);
 978                 exit(1);
 979         }
 980
 981         if (controller.freq_len > 1 && demod.squelch_level == 0) {
 982                 fprintf(stderr, "Please specify a squelch level.  Required for scanning multiple frequencies.\n");
 983                 exit(1);
 984         }
 985
 986 }
 987
 988 int rtl_fm_init(uint32_t freq,
 989                 uint32_t sample_rate,
 990                 uint32_t resample_rate,
 991                 rtl_fm_output_fn_t output_fn,
 992                 void *output_fn_data)
 993 {
 994         int r = 0;
 995
 996         dongle_init(&dongle);
 997         demod_init(&demod);
 998         output_init(&output);
 999         controller_init(&controller);
1000
1001         /*
1002          * Simulate the effects of command line arguments:
1003          *
1004          * -W wbfm -s <sample rate> -r <resample rate>
1005          */
1006
1007         /* Set initial frequency */
1008         controller.freqs[0] = freq;
1009         controller.freq_len++;
1010
1011         /* Set mode to wbfm */
1012         controller.wb_mode = 1;
1013         demod.mode_demod = &fm_demod;
1014         demod.rate_in = 170000;
1015         demod.rate_out = 170000;
1016         demod.rate_out2 = 32000;
1017         demod.custom_atan = 1;
1018         //demod.post_downsample = 4;
1019         demod.deemph = 1;
1020         controller.scan_squelch_count = DEFAULT_CONSEQ_SQUELCH;
1021         controller.scan_squelch_level = DEFAULT_SQUELCH_LEVEL;
1022         demod.squelch_level = 0;
1023
1024         /* Adjust frequency for wb mode */
1025         controller.freqs[0] += 16000;
1026
1027         /* Set sample rate */
1028         demod.rate_in = sample_rate;
1029         demod.rate_out = sample_rate;
1030
1031         /* Set resample rate */
1032         output.rate = (int) resample_rate;
1033         demod.rate_out2 = (int) resample_rate;
1034
1035         /* Set output function pointer */
1036         if(output_fn) {
1037                 output.output_fn = output_fn;
1038                 output.output_fn_data = output_fn_data;
1039         }
1040
1041         /* quadruple sample_rate to limit to Δθ to ±π/2 */
1042         demod.rate_in *= demod.post_downsample;
1043
1044         if (!output.rate) {
1045                 output.rate = demod.rate_out;
1046         }
1047
1048         sanity_checks();
1049
1050         if (controller.freq_len > 1) {
1051                 demod.terminate_on_squelch = 0;
1052         }
1053
1054         ACTUAL_BUF_LENGTH = lcm_post[demod.post_downsample] * DEFAULT_BUF_LENGTH;
1055
1056         dongle.dev_index = verbose_device_search("0");
1057         if (dongle.dev_index < 0) {
1058                 return -1;
1059         }
1060
1061         r = rtlsdr_open(&dongle.dev, (uint32_t)dongle.dev_index);
1062         if (r < 0) {
1063                 fprintf(stderr, "Failed to open rtlsdr device #%d.\n", dongle.dev_index);
1064                 return r;
1065         }
1066
1067         if (demod.deemph) {
1068                 demod.deemph_a = (int)round(1.0/((1.0-exp(-1.0/(demod.rate_out * 75e-6)))));
1069         }
1070
1071         /* Set the tuner gain */
1072         if (dongle.gain == AUTO_GAIN) {
1073                 verbose_auto_gain(dongle.dev);
1074         } else {
1075                 dongle.gain = nearest_gain(dongle.dev, dongle.gain);
1076                 verbose_gain_set(dongle.dev, dongle.gain);
1077         }
1078
1079         verbose_ppm_set(dongle.dev, dongle.ppm_error);
1080
1081         //r = rtlsdr_set_testmode(dongle.dev, 1);
1082
1083         return r;
1084 }
1085
1086 void rtl_fm_start(void)
1087 {
1088         struct controller_state *s = &controller;
1089
1090         /*
1091          * A bunch of the following is pulled from the controller_thread_fn,
1092          * which has been removed.
1093          */
1094
1095         /* Reset endpoint before we start reading from it (mandatory) */
1096         verbose_reset_buffer(dongle.dev);
1097
1098         /* set up primary channel */
1099         optimal_settings(s->freqs[0], demod.rate_in);
1100         if (dongle.direct_sampling) {
1101                 verbose_direct_sampling(dongle.dev, 1);}
1102         if (dongle.offset_tuning) {
1103                 verbose_offset_tuning(dongle.dev);}
1104
1105         /* Set the frequency */
1106         verbose_set_frequency(dongle.dev, dongle.freq);
1107         fprintf(stderr, "Oversampling input by: %ix.\n", demod.downsample);
1108         fprintf(stderr, "Oversampling output by: %ix.\n", demod.post_downsample);
1109         fprintf(stderr, "Buffer size: %0.2fms\n",
1110                 1000 * 0.5 * (float)ACTUAL_BUF_LENGTH / (float)dongle.rate);
1111
1112         /* Set the sample rate */
1113         verbose_set_sample_rate(dongle.dev, dongle.rate);
1114         fprintf(stderr, "Output at %u Hz.\n", demod.rate_in/demod.post_downsample);
1115         usleep(100000);
1116
1117         rtl_fm_scan_stop();
1118
1119         do_exit = 0;
1120         pthread_create(&output.thread, NULL, output_thread_fn, (void *)(&output));
1121         pthread_create(&demod.thread, NULL, demod_thread_fn, (void *)(&demod));
1122         pthread_create(&dongle.thread, NULL, dongle_thread_fn, (void *)(&dongle));
1123 }
1124
1125 void rtl_fm_set_freq(uint32_t freq)
1126 {
1127         struct controller_state *s = &controller;
1128
1129         if(s->freqs[0] == freq)
1130                 return;
1131
1132         s->freqs[0] = freq;
1133         s->freq_len = 1;
1134
1135         if (s->wb_mode) {
1136                 s->freqs[0] += 16000;
1137         }
1138
1139         optimal_settings(s->freqs[0], demod.rate_in);
1140         if (dongle.offset_tuning) {
1141                 verbose_offset_tuning(dongle.dev);
1142         }
1143         rtlsdr_set_center_freq(dongle.dev, dongle.freq);
1144
1145         // It does not look like refreshing the sample rate is desirable
1146         // (e.g. the scanning code in the removed controller thread function
1147         // did not do it), and behavior seemed a bit less robust with it
1148         // present.  However, I am leaving this here as a reminder to revisit
1149         // via some more testing.
1150         //rtlsdr_set_sample_rate(dongle.dev, dongle.rate);
1151
1152         // This triggers a mute during the frequency change
1153         dongle.mute = BUFFER_DUMP;
1154
1155         if(s->freq_callback)
1156                 s->freq_callback(freq, s->freq_callback_data);
1157 }
1158
1159 void rtl_fm_set_freq_callback(void (*callback)(uint32_t, void *),
1160                               void *data)
1161 {
1162         struct controller_state *s = &controller;
1163
1164         s->freq_callback = callback;
1165         s->freq_callback_data = data;
1166 }
1167
1168 uint32_t rtl_fm_get_freq(void)
1169 {
1170         struct controller_state *s = &controller;
1171         uint32_t frequency = s->freqs[0];
1172
1173         if (s->wb_mode)
1174                 frequency -= 16000;
1175
1176         return frequency;
1177 }
1178
1179 void rtl_fm_stop(void)
1180 {
1181         rtl_fm_scan_stop();
1182
1183         rtlsdr_cancel_async(dongle.dev);
1184         do_exit = 1;
1185         pthread_join(dongle.thread, NULL);
1186         safe_cond_signal(&demod.ready, &demod.ready_m);
1187         pthread_join(demod.thread, NULL);
1188         safe_cond_signal(&output.ready, &output.ready_m);
1189         pthread_join(output.thread, NULL);
1190 }
1191
1192 void rtl_fm_scan_start(int direction,
1193                        void (*callback)(uint32_t, void *),
1194                        void *data,
1195                        uint32_t step,
1196                        uint32_t min,
1197                        uint32_t max)
1198 {
1199         struct controller_state *s = &controller;
1200         struct demod_state *dm = &demod;
1201         uint32_t frequency = rtl_fm_get_freq();
1202
1203         if(s->scanning && s->scan_direction == direction)
1204                 return;
1205
1206         s->scanning = 1;
1207         s->scan_direction = direction;
1208         s->scan_callback = callback;
1209         s->scan_callback_data = data;
1210         s->scan_step = step;
1211         s->scan_min = min;
1212         s->scan_max = max;
1213
1214         /* Start scan by stepping in the desired direction */
1215         if(!direction) {
1216                 frequency += s->scan_step;
1217                 if(frequency > s->scan_max)
1218                         frequency = s->scan_min;
1219         } else {
1220                 frequency -= s->scan_step;
1221                 if(frequency < s->scan_min)
1222                         frequency = s->scan_max;
1223         }
1224
1225         rtl_fm_set_freq(frequency);
1226
1227         dm->conseq_squelch = s->scan_squelch_count;
1228         dm->squelch_hits = s->scan_squelch_count + 1;
1229         dm->squelch_level = s->scan_squelch_level;
1230 }
1231
1232 void rtl_fm_scan_stop(void)
1233 {
1234         struct controller_state *s = &controller;
1235         struct demod_state *dm = &demod;
1236
1237         s->scanning = 0;
1238
1239         dm->squelch_hits = s->scan_squelch_count + 1;
1240         dm->squelch_level = 0;
1241 }
1242
1243 void rtl_fm_scan_set_squelch_level(int level)
1244 {
1245         struct controller_state *s = &controller;
1246
1247         s->scan_squelch_level = level;
1248 }
1249
1250 void rtl_fm_scan_set_squelch_limit(int count)
1251 {
1252         struct controller_state *s = &controller;
1253
1254         s->scan_squelch_count = count;
1255 }
1256
1257 void rtl_fm_cleanup(void)
1258 {
1259         //dongle_cleanup(&dongle);
1260         demod_cleanup(&demod);
1261         output_cleanup(&output);
1262         controller_cleanup(&controller);
1263
1264         rtlsdr_close(dongle.dev);
1265 }
1266
1267 // vim: tabstop=8:softtabstop=8:shiftwidth=8:noexpandtab