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