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