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