plugins/radio/radio-rtlsdr.c

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