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