bindings/radio/radio-rtlsdr.c

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