src/radio-api.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
  20 #include "local-def.h"
  21
  22 /* -------------- RADIO DEFINITIONS ------------------ */
  23
  24 #include <math.h>
  25 #include <pthread.h>
  26 #include <rtl-sdr.h>
  27
  28 #define pthread_signal(n, m) pthread_mutex_lock(m); pthread_cond_signal(n); pthread_mutex_unlock(m)
  29 #define pthread_wait(n, m) pthread_mutex_lock(m); pthread_cond_wait(n, m); pthread_mutex_unlock(m)
  30 #define BUF_LEN 16*16384
  31
  32 typedef enum { FM, AM } Mode;
  33 typedef struct dongle_ctx dongle_ctx;
  34 typedef struct demod_ctx demod_ctx;
  35 typedef struct output_ctx output_ctx;
  36
  37 struct dongle_ctx {
  38     pthread_t thr;
  39     unsigned char thr_finished;
  40     uint16_t buf[BUF_LEN];
  41     uint32_t buf_len;
  42 };
  43
  44 struct demod_ctx {
  45     pthread_t thr;
  46     unsigned char thr_finished;
  47     pthread_rwlock_t lck;
  48     pthread_cond_t ok;
  49     pthread_mutex_t ok_m;
  50     int pre_r, pre_j, now_r, now_j, index;
  51     int pre_index, now_index;
  52     int16_t buf[BUF_LEN];
  53     int buf_len;
  54     int16_t res[BUF_LEN];
  55     int res_len;
  56 };
  57
  58 struct output_ctx {
  59     pthread_t thr;
  60     unsigned char thr_finished;
  61     pthread_rwlock_t lck;
  62     pthread_cond_t ok;
  63     pthread_mutex_t ok_m;
  64     int16_t buf[BUF_LEN];
  65     int buf_len;
  66 };
  67
  68 struct dev_ctx {
  69     rtlsdr_dev_t* dev;
  70     Mode mode;
  71     float freq;
  72     unsigned char mute;
  73     unsigned char should_run;
  74      /* thread contexts */
  75     dongle_ctx *dongle;
  76     demod_ctx *demod;
  77     output_ctx *output;
  78 };
  79
  80
  81 void* _dongle_thread_fn (void *);
  82 void* _demod_thread_fn (void *);
  83 void* _output_thread_fn (void *);
  84 unsigned int _radio_dev_count (void);
  85 const char* _radio_dev_name (unsigned int);
  86 unsigned char _radio_dev_init (struct dev_ctx *, unsigned int);
  87 unsigned char _radio_dev_free (struct dev_ctx *);
  88 void _radio_apply_params (struct dev_ctx *);
  89 void _radio_start_threads (struct dev_ctx *);
  90 void _radio_stop_threads (struct dev_ctx *);
  91
  92 static unsigned int init_dev_count;
  93 static struct dev_ctx **dev_ctx;
  94
  95 /* ------------- RADIO IMPLEMENTATION ----------------- */
  96
  97  /* ---- PUBLIC FUNCTIONS --- */
  98
  99 void radio_on () {
 100     init_dev_count = _radio_dev_count();
 101     int i;
 102
 103     dev_ctx = (struct dev_ctx**) malloc(init_dev_count * sizeof(struct dev_ctx));
 104
 105     for (i = 0; i < init_dev_count; i++) {
 106         dev_ctx[i] = (struct dev_ctx*) malloc(sizeof(struct dev_ctx));
 107         dev_ctx[i]->dev = NULL;
 108         dev_ctx[i]->mode = FM;
 109         dev_ctx[i]->freq = 100.0;
 110         dev_ctx[i]->mute = 0;
 111         dev_ctx[i]->should_run = 0;
 112         dev_ctx[i]->dongle = NULL;
 113         dev_ctx[i]->demod = NULL;
 114         dev_ctx[i]->output = NULL;
 115         _radio_dev_init(dev_ctx[i], i);
 116     }
 117 }
 118
 119 void radio_off () {
 120     int i;
 121
 122     for (i = 0; i < init_dev_count; i++) {
 123         _radio_dev_free(dev_ctx[i]);
 124         free(dev_ctx[i]);
 125     }
 126     free(dev_ctx);
 127 }
 128
 129 void radio_set_mode (struct dev_ctx *dev_ctx, Mode mode) {
 130     dev_ctx->mode = mode;
 131     _radio_apply_params(dev_ctx);
 132 }
 133
 134 void radio_set_freq (struct dev_ctx *dev_ctx, float freq) {
 135     dev_ctx->freq = freq;
 136     _radio_apply_params(dev_ctx);
 137 }
 138
 139 void radio_set_mute (struct dev_ctx *dev_ctx, unsigned char mute) {
 140     dev_ctx->mute = mute;
 141     _radio_apply_params(dev_ctx);
 142 }
 143
 144 void radio_play (struct dev_ctx *dev_ctx) {
 145     _radio_start_threads(dev_ctx);
 146 }
 147
 148 void radio_stop (struct dev_ctx *dev_ctx) {
 149     _radio_stop_threads(dev_ctx);
 150 }
 151
 152  /* --- HELPER FUNCTIONS --- */
 153
 154 unsigned int _radio_dev_count () {
 155     return rtlsdr_get_device_count();
 156 }
 157
 158 const char* _radio_dev_name (unsigned int num) {
 159     return rtlsdr_get_device_name(num);
 160 }
 161
 162 unsigned char _radio_dev_init (struct dev_ctx *dev_ctx, unsigned int num) {
 163     rtlsdr_dev_t *dev = dev_ctx->dev;
 164
 165     if (rtlsdr_open(&dev, num) < 0)
 166         return 0;
 167
 168     rtlsdr_set_tuner_gain_mode(dev, 0);
 169
 170     if (rtlsdr_reset_buffer(dev) < 0)
 171         return 0;
 172
 173     // dev_ctx->dev = dev; REQUIRED IN C TOO ? TEST !
 174
 175     _radio_apply_params(dev_ctx);
 176
 177     return 1;
 178 }
 179
 180 unsigned char _radio_dev_free (struct dev_ctx *dev_ctx) {
 181     rtlsdr_dev_t *dev = dev_ctx->dev;
 182
 183     if (rtlsdr_close(dev) < 0)
 184         return 0;
 185     dev = NULL;
 186
 187     return 1;
 188 }
 189
 190 void _radio_apply_params (struct dev_ctx *dev_ctx) {
 191     rtlsdr_dev_t *dev = dev_ctx->dev;
 192     Mode mode = dev_ctx->mode;
 193     float freq = dev_ctx->freq;
 194     int rate;
 195
 196     freq *= 1000000;
 197     rate = ((1000000 / 200000) + 1) * 200000;
 198
 199     if (mode == FM)
 200         freq += 16000;
 201     freq += rate / 4;
 202
 203     rtlsdr_set_center_freq(dev, freq);
 204     rtlsdr_set_sample_rate(dev, rate);
 205
 206     // dev_ctx->dev = dev; REQUIRED IN C TOO ? TEST !
 207 }
 208
 209 void _radio_start_threads (struct dev_ctx *dev_ctx) {
 210     rtlsdr_dev_t *dev = dev_ctx->dev;
 211     dev_ctx->dongle = (dongle_ctx*) malloc(sizeof(dongle_ctx));
 212     dev_ctx->demod = (demod_ctx*) malloc(sizeof(demod_ctx));
 213     dev_ctx->output = (output_ctx*) malloc(sizeof(output_ctx));
 214
 215     dongle_ctx *dongle = dev_ctx->dongle;
 216     demod_ctx *demod = dev_ctx->demod;
 217     output_ctx *output = dev_ctx->output;
 218
 219     pthread_rwlock_init(&demod->lck, NULL);
 220     pthread_cond_init(&demod->ok, NULL);
 221     pthread_mutex_init(&demod->ok_m, NULL);
 222     pthread_rwlock_init(&output->lck, NULL);
 223     pthread_cond_init(&output->ok, NULL);
 224     pthread_mutex_init(&output->ok_m, NULL);
 225
 226     dev_ctx->should_run = 1;
 227
 228      /* dongle thread */
 229     dongle->thr_finished = 0;
 230     pthread_create(&dongle->thr, NULL, _dongle_thread_fn, (void*)dev_ctx);
 231
 232      /* demod thread */
 233     demod->pre_r = demod->pre_j = 0;
 234     demod->now_r = demod->now_j = 0;
 235     demod->index = demod->pre_index = demod->now_index = 0;
 236     demod->thr_finished = 0;
 237     pthread_create(&demod->thr, NULL, _demod_thread_fn, (void*)dev_ctx);
 238
 239      /* output thread */
 240     output->thr_finished = 0;
 241     pthread_create(&output->thr, NULL, _output_thread_fn, (void*)dev_ctx);
 242 }
 243
 244 void _radio_stop_threads (struct dev_ctx *dev_ctx) {
 245     rtlsdr_dev_t *dev = dev_ctx->dev;
 246     dongle_ctx *dongle = dev_ctx->dongle;
 247     demod_ctx *demod = dev_ctx->demod;
 248     output_ctx *output = dev_ctx->output;
 249
 250     if (!dongle || !demod || !output)
 251         return;
 252
 253      /* stop each "while" loop in threads */
 254     dev_ctx->should_run = 0;
 255
 256     rtlsdr_cancel_async(dev);
 257     pthread_signal(&demod->ok, &demod->ok_m);
 258     pthread_signal(&output->ok, &output->ok_m);
 259
 260     while (!dongle->thr_finished ||
 261            !demod->thr_finished ||
 262            !output->thr_finished)
 263         usleep(100000);
 264
 265     pthread_join(dongle->thr, NULL);
 266     pthread_join(demod->thr, NULL);
 267     pthread_join(output->thr, NULL);
 268     pthread_rwlock_destroy(&demod->lck);
 269     pthread_cond_destroy(&demod->ok);
 270     pthread_mutex_destroy(&demod->ok_m);
 271     pthread_rwlock_destroy(&output->lck);
 272     pthread_cond_destroy(&output->ok);
 273     pthread_mutex_destroy(&output->ok_m);
 274
 275     free(dongle); dev_ctx->dongle = NULL;
 276     free(demod); dev_ctx->demod = NULL;
 277     free(output); dev_ctx->output = NULL;
 278 }
 279
 280  /* ---- LOCAL THREADED FUNCTIONS ---- */
 281
 282 static void _rtlsdr_callback (unsigned char *buf, uint32_t len, void *ctx) {
 283     struct dev_ctx *dev_ctx = (struct dev_ctx *)ctx;
 284     dongle_ctx *dongle = dev_ctx->dongle;
 285     demod_ctx *demod = dev_ctx->demod;
 286     unsigned char tmp;
 287     int i;
 288
 289     if (!dev_ctx->should_run)
 290         return;
 291
 292      /* rotate 90° */
 293     for (i = 0; i < (int)len; i += 8) {
 294         tmp = 255 - buf[i+3];
 295         buf[i+3] = buf[i+2];
 296         buf[i+2] = tmp;
 297
 298         buf[i+4] = 255 - buf[i+4];
 299         buf[i+5] = 255 - buf[i+5];
 300
 301         tmp = 255 - buf[i+6];
 302         buf[i+6] = buf[i+7];
 303         buf[i+7] = tmp;
 304     }
 305
 306      /* write data */
 307     for (i = 0; i < (int)len; i++)
 308         dongle->buf[i] = (int16_t)buf[i] - 127;
 309
 310      /* lock demod thread, write to it, unlock */
 311        pthread_rwlock_wrlock(&demod->lck);
 312     memcpy(demod->buf, dongle->buf, 2 * len);
 313     demod->buf_len = len;
 314        pthread_rwlock_unlock(&demod->lck);
 315        pthread_signal(&demod->ok, &demod->ok_m);
 316 }
 317  /**/
 318 void* _dongle_thread_fn (void *ctx) {
 319     struct dev_ctx *dev_ctx = (struct dev_ctx *)ctx;
 320     struct dongle_ctx *dongle = dev_ctx->dongle;
 321
 322     rtlsdr_read_async(dev_ctx->dev, _rtlsdr_callback, dev_ctx, 0, 0);
 323
 324     dongle->thr_finished = 1;
 325     return 0;
 326 }
 327
 328 void _lowpass_demod (void *ctx) {
 329     demod_ctx *demod = (demod_ctx *)ctx;
 330     int i=0, i2=0;
 331
 332     while (i < demod->buf_len) {
 333         demod->now_r += demod->buf[i];
 334         demod->now_j += demod->buf[i+1];
 335         i += 2;
 336         demod->index++;
 337         if (demod->index < ((1000000 / 200000) + 1))
 338             continue;
 339         demod->buf[i2] = demod->now_r;
 340         demod->buf[i2+1] = demod->now_j;
 341         demod->index = 0;
 342         demod->now_r = demod->now_j = 0;
 343         i2 += 2;
 344     }
 345     demod->buf_len = i2;
 346 }
 347  /**/
 348 void _lowpassreal_demod (void *ctx) {
 349     demod_ctx *demod = (demod_ctx *)ctx;
 350     int i=0, i2=0;
 351     int fast = 200000;
 352     int slow = 48000;
 353
 354     while (i < demod->res_len) {
 355         demod->now_index += demod->res[i];
 356         i++;
 357         demod->pre_index += slow;
 358         if (demod->pre_index < fast)
 359             continue;
 360         demod->res[i2] = (int16_t)(demod->now_index / (fast/slow));
 361         demod->pre_index -= fast;
 362         demod->now_index = 0;
 363         i2 += 1;
 364     }
 365     demod->res_len = i2;
 366 }
 367  /**/
 368 void _multiply (int ar, int aj, int br, int bj, int *cr, int *cj) {
 369     *cr = ar*br - aj*bj;
 370     *cj = aj*br + ar*bj;
 371 }
 372  /**/
 373 int _polar_discriminant (int ar, int aj, int br, int bj) {
 374     int cr, cj;
 375     double angle;
 376     _multiply(ar, aj, br, -bj, &cr, &cj);
 377     angle = atan2((double)cj, (double)cr);
 378     return (int)(angle / 3.14159 * (1<<14));
 379 }
 380  /**/
 381 void _fm_demod (void *ctx) {
 382     demod_ctx *demod = (demod_ctx *)ctx;
 383     int16_t *buf = demod->buf;
 384     int buf_len = demod->buf_len;
 385     int pcm, i;
 386
 387     pcm = _polar_discriminant(buf[0], buf[1], demod->pre_r, demod->pre_j);
 388     demod->res[0] = (int16_t)pcm;
 389
 390     for (i = 2; i < (buf_len-1); i += 2) {
 391         pcm = _polar_discriminant(buf[i], buf[i+1], buf[i-2], buf[i-1]);
 392         demod->res[i/2] = (int16_t)pcm;
 393     }
 394     demod->pre_r = buf[buf_len - 2];
 395     demod->pre_j = buf[buf_len - 1];
 396     demod->res_len = buf_len/2;
 397 }
 398  /**/
 399 void _am_demod (void *ctx) {
 400     demod_ctx *demod = (demod_ctx *)ctx;
 401     int16_t *buf = demod->buf;
 402     int buf_len = demod->buf_len;
 403     int pcm, i;
 404
 405     for (i = 0; i < buf_len; i += 2) {
 406         pcm = buf[i] * buf[i];
 407         pcm += buf[i+1] * buf[i+1];
 408         demod->res[i/2] = (int16_t)sqrt(pcm);
 409     }
 410     demod->res_len = buf_len/2;
 411 }
 412  /**/
 413 void* _demod_thread_fn (void *ctx) {
 414     struct dev_ctx *dev_ctx = (struct dev_ctx *)ctx;
 415     demod_ctx *demod = dev_ctx->demod;
 416     output_ctx *output = dev_ctx->output;
 417
 418     while(dev_ctx->should_run) {
 419             pthread_wait(&demod->ok, &demod->ok_m);
 420             pthread_rwlock_wrlock(&demod->lck);
 421         _lowpass_demod(demod);
 422         if (dev_ctx->mode == FM)
 423             _fm_demod(demod);
 424         else
 425             _am_demod(demod);
 426         _lowpassreal_demod(demod);
 427            pthread_rwlock_unlock(&demod->lck);
 428
 429          /* lock demod thread, write to it, unlock */
 430            pthread_rwlock_wrlock(&output->lck);
 431         memcpy(output->buf, demod->res, 2 * demod->res_len);
 432         output->buf_len = demod->res_len;
 433            pthread_rwlock_unlock(&output->lck);
 434            pthread_signal(&output->ok, &output->ok_m);
 435     }
 436
 437     demod->thr_finished = 1;
 438     return 0;
 439 }
 440
 441 void* _output_thread_fn (void *ctx) {
 442     struct dev_ctx *dev_ctx = (struct dev_ctx *)ctx;
 443     output_ctx *output = dev_ctx->output;
 444
 445     while (dev_ctx->should_run) {
 446            pthread_wait(&output->ok, &output->ok_m);
 447            pthread_rwlock_rdlock(&output->lck);
 448         //if (!dev_ctx->mute)
 449         //    mRadio->PlayAlsa((void*)&output->buf, output->buf_len);
 450            pthread_rwlock_unlock(&output->lck);
 451     }
 452
 453     output->thr_finished = 1;
 454     return 0;
 455 }
 456
 457 /* -------------- PLUGIN BINDING ------------------- */
 458
 459 STATIC json_object* start (AFB_session *session, AFB_request *request, void* handle) {
 460     json_object *response;
 461     char query [512];
 462
 463     // request all query key/value
 464     getQueryAll (request, query, sizeof(query));
 465
 466     // check if we have some post data
 467     if (request->post == NULL)  request->post="NoData";
 468
 469     // return response to caller
 470     response = jsonNewMessage(AFB_SUCCESS, "Start Radio plugin query={%s} PostData: \'%s\' ", query, request->post);
 471
 472     //if (verbose) fprintf(stderr, "%d: \n", pingcount);
 473     return (response);
 474 }
 475
 476 STATIC json_object* stop (AFB_session *session, AFB_request *request, void* handle) {
 477     json_object *response;
 478     char query [512];
 479
 480     getQueryAll (request, query, sizeof(query));
 481
 482     if (request->post == NULL)  request->post="NoData";
 483
 484     response = jsonNewMessage(AFB_SUCCESS, "Stop Radio plugin query={%s} PostData: \'%s\' ", query, request->post);
 485
 486     return (response);
 487 }
 488
 489
 490 STATIC struct {
 491     void * somedata;
 492 } handle;
 493
 494
 495 STATIC  AFB_restapi pluginApis[]= {
 496   {"start"    , (AFB_apiCB)start      , "Ping Application Framework", NULL},
 497   {"stop"     , (AFB_apiCB)stop       , "Ping Application Framework", NULL},
 498   {0,0,0}
 499 };
 500
 501 PUBLIC AFB_plugin *radioRegister (AFB_session *session) {
 502     AFB_plugin *plugin = malloc (sizeof (AFB_plugin));
 503     plugin->type  = AFB_PLUGIN;
 504     plugin->info  = "Application Framework Binder - Radio plugin";
 505     plugin->prefix  = "radio";
 506     plugin->apis  = pluginApis;
 507
 508     return (plugin);
 509 };