plugins/radio/radio-rtlsdr.c

   1 /*
   2  * Copyright (C) 2015 "IoT.bzh"
   3  * Author "Manuel Bachmann"
   4  *
   5  * This program is free software: you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation, either version 3 of the License, or
   8  * (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17  */
  18
  19 #include "radio-api.h"
  20 #include "radio-rtlsdr.h"
  21
  22 static audioCtxHandleT *actx = NULL;
  23
  24 /* ------------- RADIO RTLSDR IMPLEMENTATION ---------------- */
  25
  26 /* --- PUBLIC FUNCTIONS --- */
  27
  28 /* Radio initialization should be done only when user start the radio and not at plugin initialization
  29    Making this call too early would impose to restart the binder to detect a radio */
  30 PUBLIC unsigned char _radio_on (unsigned int num, radioCtxHandleT *ctx) {
  31
  32     if (num >= _radio_dev_count())
  33         return 0;
  34
  35     if (init_dev_count < _radio_dev_count()) {
  36         init_dev_count = _radio_dev_count();
  37         dev_ctx = (dev_ctx_T**) realloc (dev_ctx, init_dev_count * sizeof(dev_ctx_T));
  38     }
  39
  40     dev_ctx[num] = (dev_ctx_T*) malloc (sizeof(dev_ctx_T));
  41     dev_ctx[num]->dev = NULL;
  42     dev_ctx[num]->mode = ctx->mode;
  43     dev_ctx[num]->freq = ctx->freq;
  44     dev_ctx[num]->mute = ctx->mute;
  45     dev_ctx[num]->should_run = 0;
  46     dev_ctx[num]->dongle = NULL;
  47     dev_ctx[num]->demod = NULL;
  48     dev_ctx[num]->output = NULL;
  49     _radio_dev_init(dev_ctx[num], num);
  50
  51     actx = malloc (sizeof(audioCtxHandleT));
  52     actx->idx = -1;
  53     actx->volume = 25;
  54     actx->channels = 2;
  55     actx->mute = 0;
  56     _alsa_init ("default", actx);
  57
  58     return 1;
  59 }
  60
  61 PUBLIC void _radio_off (unsigned int num) {
  62
  63     if (num >= _radio_dev_count())
  64         return;
  65
  66     if (dev_ctx[num]) {
  67         _radio_dev_free(dev_ctx[num]);
  68         free(dev_ctx[num]);
  69     }
  70
  71     /* free(dev_ctx); */
  72
  73     _alsa_free ("default");
  74     free (actx);
  75 }
  76
  77 PUBLIC void _radio_set_mode (unsigned int num, Mode mode) {
  78     if (!dev_ctx || !dev_ctx[num])
  79         return;
  80
  81     dev_ctx[num]->mode = mode;
  82     _radio_apply_params(dev_ctx[num]);
  83 }
  84
  85 PUBLIC void _radio_set_freq (unsigned int num, float freq) {
  86     if (!dev_ctx || !dev_ctx[num])
  87         return;
  88
  89     dev_ctx[num]->freq = freq;
  90     _radio_apply_params(dev_ctx[num]);
  91 }
  92
  93 PUBLIC void _radio_set_mute (unsigned int num, unsigned char mute) {
  94     if (!dev_ctx || !dev_ctx[num])
  95         return;
  96
  97     dev_ctx[num]->mute = mute;
  98     _radio_apply_params(dev_ctx[num]);
  99 }
 100
 101 PUBLIC void _radio_play (unsigned int num) {
 102     if (!dev_ctx || !dev_ctx[num])
 103         return;
 104
 105     _radio_start_threads(dev_ctx[num]);
 106 }
 107
 108 PUBLIC void _radio_stop (unsigned int num) {
 109     if (!dev_ctx || !dev_ctx[num])
 110         return;
 111
 112     _radio_stop_threads(dev_ctx[num]);
 113 }
 114
 115 PUBLIC unsigned int _radio_dev_count () {
 116     return rtlsdr_get_device_count();
 117 }
 118
 119 PUBLIC const char* _radio_dev_name (unsigned int num) {
 120     return rtlsdr_get_device_name(num);
 121 }
 122
 123
 124 /* --- LOCAL HELPER FUNCTIONS --- */
 125
 126 STATIC unsigned char _radio_dev_init (dev_ctx_T *dev_ctx, unsigned int num) {
 127     rtlsdr_dev_t *dev = dev_ctx->dev;
 128
 129     if (rtlsdr_open(&dev, num) < 0)
 130         return 0;
 131
 132     rtlsdr_set_tuner_gain_mode(dev, 0);
 133
 134     if (rtlsdr_reset_buffer(dev) < 0)
 135         return 0;
 136
 137     dev_ctx->dev = dev;
 138
 139     _radio_apply_params(dev_ctx);
 140
 141     return 1;
 142 }
 143
 144 STATIC unsigned char _radio_dev_free (dev_ctx_T *dev_ctx) {
 145     rtlsdr_dev_t *dev = dev_ctx->dev;
 146
 147     if (rtlsdr_close(dev) < 0)
 148         return 0;
 149     dev = NULL;
 150
 151     dev_ctx->dev = dev;
 152
 153     return 1;
 154 }
 155
 156 STATIC void _radio_apply_params (dev_ctx_T *dev_ctx) {
 157     rtlsdr_dev_t *dev = dev_ctx->dev;
 158     Mode mode = dev_ctx->mode;
 159     float freq = dev_ctx->freq;
 160     int rate;
 161
 162     freq *= 1000000;
 163     rate = ((1000000 / 200000) + 1) * 200000;
 164
 165     if (mode == FM)
 166         freq += 16000;
 167     freq += rate / 4;
 168
 169     rtlsdr_set_center_freq(dev, freq);
 170     rtlsdr_set_sample_rate(dev, rate);
 171
 172     dev_ctx->dev = dev;
 173 }
 174
 175 STATIC void _radio_start_threads (dev_ctx_T *dev_ctx) {
 176     rtlsdr_dev_t *dev = dev_ctx->dev;
 177     dev_ctx->dongle = (dongle_ctx*) malloc(sizeof(dongle_ctx));
 178     dev_ctx->demod = (demod_ctx*) malloc(sizeof(demod_ctx));
 179     dev_ctx->output = (output_ctx*) malloc(sizeof(output_ctx));
 180
 181     dongle_ctx *dongle = dev_ctx->dongle;
 182     demod_ctx *demod = dev_ctx->demod;
 183     output_ctx *output = dev_ctx->output;
 184
 185     pthread_rwlock_init(&demod->lck, NULL);
 186     pthread_cond_init(&demod->ok, NULL);
 187     pthread_mutex_init(&demod->ok_m, NULL);
 188     pthread_rwlock_init(&output->lck, NULL);
 189     pthread_cond_init(&output->ok, NULL);
 190     pthread_mutex_init(&output->ok_m, NULL);
 191
 192     dev_ctx->should_run = 1;
 193
 194      /* dongle thread */
 195     dongle->thr_finished = 0;
 196     pthread_create(&dongle->thr, NULL, _dongle_thread_fn, (void*)dev_ctx);
 197
 198      /* demod thread */
 199     demod->pre_r = demod->pre_j = 0;
 200     demod->now_r = demod->now_j = 0;
 201     demod->index = demod->pre_index = demod->now_index = 0;
 202     demod->thr_finished = 0;
 203     pthread_create(&demod->thr, NULL, _demod_thread_fn, (void*)dev_ctx);
 204
 205      /* output thread */
 206     output->thr_finished = 0;
 207     pthread_create(&output->thr, NULL, _output_thread_fn, (void*)dev_ctx);
 208 }
 209
 210 STATIC void _radio_stop_threads (dev_ctx_T *dev_ctx) {
 211     rtlsdr_dev_t *dev = dev_ctx->dev;
 212     dongle_ctx *dongle = dev_ctx->dongle;
 213     demod_ctx *demod = dev_ctx->demod;
 214     output_ctx *output = dev_ctx->output;
 215
 216     if (!dongle || !demod || !output)
 217         return;
 218
 219      /* stop each "while" loop in threads */
 220     dev_ctx->should_run = 0;
 221
 222     rtlsdr_cancel_async(dev);
 223     pthread_signal(&demod->ok, &demod->ok_m);
 224     pthread_signal(&output->ok, &output->ok_m);
 225
 226     while (!dongle->thr_finished ||
 227            !demod->thr_finished ||
 228            !output->thr_finished)
 229         usleep(100000);
 230
 231     pthread_join(dongle->thr, NULL);
 232     pthread_join(demod->thr, NULL);
 233     pthread_join(output->thr, NULL);
 234     pthread_rwlock_destroy(&demod->lck);
 235     pthread_cond_destroy(&demod->ok);
 236     pthread_mutex_destroy(&demod->ok_m);
 237     pthread_rwlock_destroy(&output->lck);
 238     pthread_cond_destroy(&output->ok);
 239     pthread_mutex_destroy(&output->ok_m);
 240
 241     free(dongle); dev_ctx->dongle = NULL;
 242     free(demod); dev_ctx->demod = NULL;
 243     free(output); dev_ctx->output = NULL;
 244 }
 245
 246  /* ---- LOCAL THREADED FUNCTIONS ---- */
 247
 248 STATIC void _rtlsdr_callback (unsigned char *buf, uint32_t len, void *ctx) {
 249     dev_ctx_T *dev_ctx = (dev_ctx_T *)ctx;
 250     dongle_ctx *dongle = dev_ctx->dongle;
 251     demod_ctx *demod = dev_ctx->demod;
 252     unsigned char tmp;
 253     int i;
 254
 255     if (!dev_ctx->should_run)
 256         return;
 257
 258      /* rotate 90° */
 259     for (i = 0; i < (int)len; i += 8) {
 260         tmp = 255 - buf[i+3];
 261         buf[i+3] = buf[i+2];
 262         buf[i+2] = tmp;
 263
 264         buf[i+4] = 255 - buf[i+4];
 265         buf[i+5] = 255 - buf[i+5];
 266
 267         tmp = 255 - buf[i+6];
 268         buf[i+6] = buf[i+7];
 269         buf[i+7] = tmp;
 270     }
 271
 272      /* write data */
 273     for (i = 0; i < (int)len; i++)
 274         dongle->buf[i] = (int16_t)buf[i] - 127;
 275
 276      /* lock demod thread, write to it, unlock */
 277        pthread_rwlock_wrlock(&demod->lck);
 278     memcpy(demod->buf, dongle->buf, 2 * len);
 279     demod->buf_len = len;
 280        pthread_rwlock_unlock(&demod->lck);
 281        pthread_signal(&demod->ok, &demod->ok_m);
 282 }
 283  /**/
 284 STATIC void* _dongle_thread_fn (void *ctx) {
 285     dev_ctx_T *dev_ctx = (dev_ctx_T *)ctx;
 286     dongle_ctx *dongle = dev_ctx->dongle;
 287
 288     rtlsdr_read_async(dev_ctx->dev, _rtlsdr_callback, dev_ctx, 0, 0);
 289
 290     dongle->thr_finished = 1;
 291     return 0;
 292 }
 293
 294 STATIC void _lowpass_demod (void *ctx) {
 295     demod_ctx *demod = (demod_ctx *)ctx;
 296     int i=0, i2=0;
 297
 298     while (i < demod->buf_len) {
 299         demod->now_r += demod->buf[i];
 300         demod->now_j += demod->buf[i+1];
 301         i += 2;
 302         demod->index++;
 303         if (demod->index < ((1000000 / 200000) + 1))
 304             continue;
 305         demod->buf[i2] = demod->now_r;
 306         demod->buf[i2+1] = demod->now_j;
 307         demod->index = 0;
 308         demod->now_r = demod->now_j = 0;
 309         i2 += 2;
 310     }
 311     demod->buf_len = i2;
 312 }
 313  /**/
 314 STATIC void _lowpassreal_demod (void *ctx) {
 315     demod_ctx *demod = (demod_ctx *)ctx;
 316     int i=0, i2=0;
 317     int fast = 200000;
 318     int slow = 48000;
 319
 320     while (i < demod->res_len) {
 321         demod->now_index += demod->res[i];
 322         i++;
 323         demod->pre_index += slow;
 324         if (demod->pre_index < fast)
 325             continue;
 326         demod->res[i2] = (int16_t)(demod->now_index / (fast/slow));
 327         demod->pre_index -= fast;
 328         demod->now_index = 0;
 329         i2 += 1;
 330     }
 331     demod->res_len = i2;
 332 }
 333  /**/
 334 STATIC void _multiply (int ar, int aj, int br, int bj, int *cr, int *cj) {
 335     *cr = ar*br - aj*bj;
 336     *cj = aj*br + ar*bj;
 337 }
 338  /**/
 339 STATIC int _polar_discriminant (int ar, int aj, int br, int bj) {
 340     int cr, cj;
 341     double angle;
 342     _multiply(ar, aj, br, -bj, &cr, &cj);
 343     angle = atan2((double)cj, (double)cr);
 344     return (int)(angle / 3.14159 * (1<<14));
 345 }
 346  /**/
 347 STATIC void _fm_demod (void *ctx) {
 348     demod_ctx *demod = (demod_ctx *)ctx;
 349     int16_t *buf = demod->buf;
 350     int buf_len = demod->buf_len;
 351     int pcm, i;
 352
 353     pcm = _polar_discriminant(buf[0], buf[1], demod->pre_r, demod->pre_j);
 354     demod->res[0] = (int16_t)pcm;
 355
 356     for (i = 2; i < (buf_len-1); i += 2) {
 357         pcm = _polar_discriminant(buf[i], buf[i+1], buf[i-2], buf[i-1]);
 358         demod->res[i/2] = (int16_t)pcm;
 359     }
 360     demod->pre_r = buf[buf_len - 2];
 361     demod->pre_j = buf[buf_len - 1];
 362     demod->res_len = buf_len/2;
 363 }
 364  /**/
 365 STATIC void _am_demod (void *ctx) {
 366     demod_ctx *demod = (demod_ctx *)ctx;
 367     int16_t *buf = demod->buf;
 368     int buf_len = demod->buf_len;
 369     int pcm, i;
 370
 371     for (i = 0; i < buf_len; i += 2) {
 372         pcm = buf[i] * buf[i];
 373         pcm += buf[i+1] * buf[i+1];
 374         demod->res[i/2] = (int16_t)sqrt(pcm);
 375     }
 376     demod->res_len = buf_len/2;
 377 }
 378  /**/
 379 STATIC void* _demod_thread_fn (void *ctx) {
 380     dev_ctx_T *dev_ctx = (dev_ctx_T *)ctx;
 381     demod_ctx *demod = dev_ctx->demod;
 382     output_ctx *output = dev_ctx->output;
 383
 384     while(dev_ctx->should_run) {
 385             pthread_wait(&demod->ok, &demod->ok_m);
 386             pthread_rwlock_wrlock(&demod->lck);
 387         _lowpass_demod(demod);
 388         if (dev_ctx->mode == FM)
 389             _fm_demod(demod);
 390         else
 391             _am_demod(demod);
 392         _lowpassreal_demod(demod);
 393            pthread_rwlock_unlock(&demod->lck);
 394
 395          /* lock demod thread, write to it, unlock */
 396            pthread_rwlock_wrlock(&output->lck);
 397         memcpy(output->buf, demod->res, 2 * demod->res_len);
 398         output->buf_len = demod->res_len;
 399            pthread_rwlock_unlock(&output->lck);
 400            pthread_signal(&output->ok, &output->ok_m);
 401     }
 402
 403     demod->thr_finished = 1;
 404     return 0;
 405 }
 406
 407 STATIC void* _output_thread_fn (void *ctx) {
 408     dev_ctx_T *dev_ctx = (dev_ctx_T *)ctx;
 409     output_ctx *output = dev_ctx->output;
 410
 411     while (dev_ctx->should_run) {
 412            pthread_wait(&output->ok, &output->ok_m);
 413            pthread_rwlock_rdlock(&output->lck);
 414            if (!dev_ctx->mute)
 415             _alsa_play(actx->idx, (void*)&output->buf, output->buf_len);
 416            pthread_rwlock_unlock(&output->lck);
 417     }
 418
 419     output->thr_finished = 1;
 420     return 0;
 421 }