35cab388b7cc17c85e4e7f12e35e874190728b85
[src/app-framework-binder.git] / 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 typedef struct  {
38     pthread_t thr;
39     unsigned char thr_finished;
40     uint16_t buf[BUF_LEN];
41     uint32_t buf_len;
42 } dongle_ctx;
43
44 typedef struct  {
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 } demod_ctx;
57
58 typedef struct  {
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 } output_ctx;
67
68 typedef struct  {
69     int used;  // radio is free ???
70     rtlsdr_dev_t* dev;
71     Mode mode;
72     float freq;
73     unsigned char mute;
74     unsigned char should_run;
75      /* thread contexts */
76     dongle_ctx *dongle;
77     demod_ctx *demod;
78     output_ctx *output;
79 } dev_ctx;
80
81
82 void* _dongle_thread_fn (void *);
83 void* _demod_thread_fn (void *);
84 void* _output_thread_fn (void *);
85 unsigned int _radio_dev_count (void);
86 const char* _radio_dev_name (unsigned int);
87 unsigned char _radio_dev_init (struct dev_ctx *, unsigned int);
88 unsigned char _radio_dev_free (struct dev_ctx *);
89 void _radio_apply_params (struct dev_ctx *);
90 void _radio_start_threads (struct dev_ctx *);
91 void _radio_stop_threads (struct dev_ctx *);
92
93 static unsigned int init_dev_count;
94 static dev_ctx **dev_ctx;
95
96 /* ------------- RADIO IMPLEMENTATION ----------------- */
97
98
99 // Radio initialisation should be done only when user start the radio and not at plugin intialisation
100 // Making this call too early would impose to restart the binder to detect a radio.
101 STATIC initRadio() {
102  
103     init_dev_count = _radio_dev_count();
104     int i;
105
106     dev_ctx = (dev_ctx**) malloc(init_dev_count * sizeof(dev_ctx));
107
108     for (i = 0; i < init_dev_count; i++) {
109         dev_ctx[i] = (struct dev_ctx*) malloc(sizeof(struct dev_ctx));
110         dev_ctx[i]->dev = NULL;
111         dev_ctx[i]->mode = FM;
112         dev_ctx[i]->freq = 100.0;
113         dev_ctx[i]->mute = 0;
114         dev_ctx[i]->should_run = 0;
115         dev_ctx[i]->dongle = NULL;
116         dev_ctx[i]->demod = NULL;
117         dev_ctx[i]->output = NULL;
118         _radio_dev_init(dev_ctx[i], i);
119     }
120 }
121
122 STATIC void radio_off () {
123     int i;
124
125     for (i = 0; i < init_dev_count; i++) {
126         _radio_dev_free(dev_ctx[i]);
127         free(dev_ctx[i]);
128     }
129     free(dev_ctx);
130 }
131
132 STATIC void radio_set_mode (dev_ctx *dev_ctx, Mode mode) {
133     dev_ctx->mode = mode;
134     _radio_apply_params(dev_ctx);
135 }
136
137 STATIC void radio_set_freq (dev_ctx *dev_ctx, float freq) {
138     dev_ctx->freq = freq;
139     _radio_apply_params(dev_ctx);
140 }
141
142 STATIC void radio_set_mute (dev_ctx *dev_ctx, unsigned char mute) {
143     dev_ctx->mute = mute;
144     _radio_apply_params(dev_ctx);
145 }
146
147 STATIC void radio_play (dev_ctx *dev_ctx) {
148     _radio_start_threads(dev_ctx);
149 }
150
151 STATIC void radio_stop (dev_ctx *dev_ctx) {
152     _radio_stop_threads(dev_ctx);
153 }
154
155  /* --- HELPER FUNCTIONS --- */
156
157 STATIC unsigned int _radio_dev_count () {
158     return rtlsdr_get_device_count();
159 }
160
161 STATIC const char* _radio_dev_name (unsigned int num) {
162     return rtlsdr_get_device_name(num);
163 }
164
165 STATIC unsigned char _radio_dev_init (dev_ctx *dev_ctx, unsigned int num) {
166     rtlsdr_dev_t *dev = dev_ctx->dev;
167
168     if (rtlsdr_open(&dev, num) < 0)
169         return 0;
170
171     rtlsdr_set_tuner_gain_mode(dev, 0);
172
173     if (rtlsdr_reset_buffer(dev) < 0)
174         return 0;
175
176     // dev_ctx->dev = dev; REQUIRED IN C TOO ? TEST !
177
178     _radio_apply_params(dev_ctx);
179
180     return 1;
181 }
182
183 STATIC unsigned char _radio_dev_free (dev_ctx *dev_ctx) {
184     rtlsdr_dev_t *dev = dev_ctx->dev;
185
186     if (rtlsdr_close(dev) < 0)
187         return 0;
188     dev = NULL;
189
190     return 1;
191 }
192
193 STATIC void _radio_apply_params (dev_ctx *dev_ctx) {
194     rtlsdr_dev_t *dev = dev_ctx->dev;
195     Mode mode = dev_ctx->mode;
196     float freq = dev_ctx->freq;
197     int rate;
198
199     freq *= 1000000;
200     rate = ((1000000 / 200000) + 1) * 200000;
201
202     if (mode == FM)
203         freq += 16000;
204     freq += rate / 4;
205
206     rtlsdr_set_center_freq(dev, freq);
207     rtlsdr_set_sample_rate(dev, rate);
208
209     // dev_ctx->dev = dev; REQUIRED IN C TOO ? TEST !
210 }
211
212 STATIC void _radio_start_threads (dev_ctx *dev_ctx) {
213     rtlsdr_dev_t *dev = dev_ctx->dev;
214     dev_ctx->dongle = (dongle_ctx*) malloc(sizeof(dongle_ctx));
215     dev_ctx->demod = (demod_ctx*) malloc(sizeof(demod_ctx));
216     dev_ctx->output = (output_ctx*) malloc(sizeof(output_ctx));
217
218     dongle_ctx *dongle = dev_ctx->dongle;
219     demod_ctx *demod = dev_ctx->demod;
220     output_ctx *output = dev_ctx->output;
221
222     pthread_rwlock_init(&demod->lck, NULL);
223     pthread_cond_init(&demod->ok, NULL);
224     pthread_mutex_init(&demod->ok_m, NULL);
225     pthread_rwlock_init(&output->lck, NULL);
226     pthread_cond_init(&output->ok, NULL);
227     pthread_mutex_init(&output->ok_m, NULL);
228
229     dev_ctx->should_run = 1;
230
231      /* dongle thread */
232     dongle->thr_finished = 0;
233     pthread_create(&dongle->thr, NULL, _dongle_thread_fn, (void*)dev_ctx);
234
235      /* demod thread */
236     demod->pre_r = demod->pre_j = 0;
237     demod->now_r = demod->now_j = 0;
238     demod->index = demod->pre_index = demod->now_index = 0;
239     demod->thr_finished = 0;
240     pthread_create(&demod->thr, NULL, _demod_thread_fn, (void*)dev_ctx);
241
242      /* output thread */
243     output->thr_finished = 0;
244     pthread_create(&output->thr, NULL, _output_thread_fn, (void*)dev_ctx);
245 }
246
247 STATIC void _radio_stop_threads (dev_ctx *dev_ctx) {
248     rtlsdr_dev_t *dev = dev_ctx->dev;
249     dongle_ctx *dongle = dev_ctx->dongle;
250     demod_ctx *demod = dev_ctx->demod;
251     output_ctx *output = dev_ctx->output;
252
253     if (!dongle || !demod || !output)
254         return;
255
256      /* stop each "while" loop in threads */
257     dev_ctx->should_run = 0;
258
259     rtlsdr_cancel_async(dev);
260     pthread_signal(&demod->ok, &demod->ok_m);
261     pthread_signal(&output->ok, &output->ok_m);
262
263     while (!dongle->thr_finished ||
264            !demod->thr_finished ||
265            !output->thr_finished)
266         usleep(100000);
267
268     pthread_join(dongle->thr, NULL);
269     pthread_join(demod->thr, NULL);
270     pthread_join(output->thr, NULL);
271     pthread_rwlock_destroy(&demod->lck);
272     pthread_cond_destroy(&demod->ok);
273     pthread_mutex_destroy(&demod->ok_m);
274     pthread_rwlock_destroy(&output->lck);
275     pthread_cond_destroy(&output->ok);
276     pthread_mutex_destroy(&output->ok_m);
277
278     free(dongle); dev_ctx->dongle = NULL;
279     free(demod); dev_ctx->demod = NULL;
280     free(output); dev_ctx->output = NULL;
281 }
282
283  /* ---- LOCAL THREADED FUNCTIONS ---- */
284
285 STATIC void _rtlsdr_callback (unsigned char *buf, uint32_t len, void *ctx) {
286     dev_ctx *dev_ctx = (dev_ctx *)ctx;
287     dongle_ctx *dongle = dev_ctx->dongle;
288     demod_ctx *demod = dev_ctx->demod;
289     unsigned char tmp;
290     int i;
291
292     if (!dev_ctx->should_run)
293         return;
294
295      /* rotate 90° */
296     for (i = 0; i < (int)len; i += 8) {
297         tmp = 255 - buf[i+3];
298         buf[i+3] = buf[i+2];
299         buf[i+2] = tmp;
300
301         buf[i+4] = 255 - buf[i+4];
302         buf[i+5] = 255 - buf[i+5];
303
304         tmp = 255 - buf[i+6];
305         buf[i+6] = buf[i+7];
306         buf[i+7] = tmp;
307     }
308
309      /* write data */
310     for (i = 0; i < (int)len; i++)
311         dongle->buf[i] = (int16_t)buf[i] - 127;
312
313      /* lock demod thread, write to it, unlock */
314        pthread_rwlock_wrlock(&demod->lck);
315     memcpy(demod->buf, dongle->buf, 2 * len);
316     demod->buf_len = len;
317        pthread_rwlock_unlock(&demod->lck);
318        pthread_signal(&demod->ok, &demod->ok_m);
319 }
320  /**/
321 STATIC void* _dongle_thread_fn (void *ctx) {
322     dev_ctx *dev_ctx = (dev_ctx *)ctx;
323     dongle_ctx *dongle = dev_ctx->dongle;
324
325     rtlsdr_read_async(dev_ctx->dev, _rtlsdr_callback, dev_ctx, 0, 0);
326
327     dongle->thr_finished = 1;
328     return 0;
329 }
330
331 STATIC void _lowpass_demod (void *ctx) {
332     demod_ctx *demod = (demod_ctx *)ctx;
333     int i=0, i2=0;
334
335     while (i < demod->buf_len) {
336         demod->now_r += demod->buf[i];
337         demod->now_j += demod->buf[i+1];
338         i += 2;
339         demod->index++;
340         if (demod->index < ((1000000 / 200000) + 1))
341             continue;
342         demod->buf[i2] = demod->now_r;
343         demod->buf[i2+1] = demod->now_j;
344         demod->index = 0;
345         demod->now_r = demod->now_j = 0;
346         i2 += 2;
347     }
348     demod->buf_len = i2;
349 }
350  /**/
351 STATIC void _lowpassreal_demod (void *ctx) {
352     demod_ctx *demod = (demod_ctx *)ctx;
353     int i=0, i2=0;
354     int fast = 200000;
355     int slow = 48000;
356
357     while (i < demod->res_len) {
358         demod->now_index += demod->res[i];
359         i++;
360         demod->pre_index += slow;
361         if (demod->pre_index < fast)
362             continue;
363         demod->res[i2] = (int16_t)(demod->now_index / (fast/slow));
364         demod->pre_index -= fast;
365         demod->now_index = 0;
366         i2 += 1;
367     }
368     demod->res_len = i2;
369 }
370  /**/
371 STATIC void _multiply (int ar, int aj, int br, int bj, int *cr, int *cj) {
372     *cr = ar*br - aj*bj;
373     *cj = aj*br + ar*bj;
374 }
375  /**/
376 STATIC int _polar_discriminant (int ar, int aj, int br, int bj) {
377     int cr, cj;
378     double angle;
379     _multiply(ar, aj, br, -bj, &cr, &cj);
380     angle = atan2((double)cj, (double)cr);
381     return (int)(angle / 3.14159 * (1<<14));
382 }
383  /**/
384 STATIC void _fm_demod (void *ctx) {
385     demod_ctx *demod = (demod_ctx *)ctx;
386     int16_t *buf = demod->buf;
387     int buf_len = demod->buf_len;
388     int pcm, i;
389
390     pcm = _polar_discriminant(buf[0], buf[1], demod->pre_r, demod->pre_j);
391     demod->res[0] = (int16_t)pcm;
392
393     for (i = 2; i < (buf_len-1); i += 2) {
394         pcm = _polar_discriminant(buf[i], buf[i+1], buf[i-2], buf[i-1]);
395         demod->res[i/2] = (int16_t)pcm;
396     }
397     demod->pre_r = buf[buf_len - 2];
398     demod->pre_j = buf[buf_len - 1];
399     demod->res_len = buf_len/2;
400 }
401  /**/
402 STATIC void _am_demod (void *ctx) {
403     demod_ctx *demod = (demod_ctx *)ctx;
404     int16_t *buf = demod->buf;
405     int buf_len = demod->buf_len;
406     int pcm, i;
407
408     for (i = 0; i < buf_len; i += 2) {
409         pcm = buf[i] * buf[i];
410         pcm += buf[i+1] * buf[i+1];
411         demod->res[i/2] = (int16_t)sqrt(pcm);
412     }
413     demod->res_len = buf_len/2;
414 }
415  /**/
416 STATIC void* _demod_thread_fn (void *ctx) {
417     dev_ctx *dev_ctx = (dev_ctx *)ctx;
418     demod_ctx *demod = dev_ctx->demod;
419     output_ctx *output = dev_ctx->output;
420
421     while(dev_ctx->should_run) {
422             pthread_wait(&demod->ok, &demod->ok_m);
423             pthread_rwlock_wrlock(&demod->lck);
424         _lowpass_demod(demod);
425         if (dev_ctx->mode == FM)
426             _fm_demod(demod);
427         else
428             _am_demod(demod);
429         _lowpassreal_demod(demod);
430            pthread_rwlock_unlock(&demod->lck);
431
432          /* lock demod thread, write to it, unlock */
433            pthread_rwlock_wrlock(&output->lck);
434         memcpy(output->buf, demod->res, 2 * demod->res_len);
435         output->buf_len = demod->res_len;
436            pthread_rwlock_unlock(&output->lck);
437            pthread_signal(&output->ok, &output->ok_m);
438     }
439
440     demod->thr_finished = 1;
441     return 0;
442 }
443
444 STATIC void* _output_thread_fn (void *ctx) {
445     dev_ctx *dev_ctx = (dev_ctx *)ctx;
446     output_ctx *output = dev_ctx->output;
447
448     while (dev_ctx->should_run) {
449            pthread_wait(&output->ok, &output->ok_m);
450            pthread_rwlock_rdlock(&output->lck);
451         //if (!dev_ctx->mute)
452         //    mRadio->PlayAlsa((void*)&output->buf, output->buf_len);
453            pthread_rwlock_unlock(&output->lck);
454     }
455
456     output->thr_finished = 1;
457     return 0;
458 }
459
460
461
462 STATIC json_object* start (AFB_session *session, AFB_request *request, void* handle) {
463     json_object *response;
464     char query [512];
465
466     // request all query key/value
467     getQueryAll (request, query, sizeof(query));
468
469     // check if we have some post data
470     if (request->post == NULL)  request->post="NoData";
471
472     // return response to caller
473     response = jsonNewMessage(AFB_SUCCESS, "Start Radio plugin query={%s} PostData: \'%s\' ", query, request->post);
474
475     //if (verbose) fprintf(stderr, "%d: \n", pingcount);
476     return (response);
477 }
478
479 STATIC json_object* stop (AFB_session *session, AFB_request *request, void* handle) {
480     json_object *response;
481     char query [512];
482
483     getQueryAll (request, query, sizeof(query));
484
485     if (request->post == NULL)  request->post="NoData";
486
487     response = jsonNewMessage(AFB_SUCCESS, "Stop Radio plugin query={%s} PostData: \'%s\' ", query, request->post);
488
489     return (response);
490 }
491
492
493 // ********************************************************
494
495 // FULUP integration proposal with client session context
496
497 // ********************************************************
498
499
500 #define MAX_RADIO 10
501
502 // Structure holding existing radio with current usage status
503 typdef struct {
504     int   idx;
505     char *name;
506     int  used;
507 }radioDevT;
508
509 // Radio plugin handle should store everething API may need
510 typdef struc {
511   radioT *radios[MAX_RADIO];  // pointer to existing radio
512   int devCount;
513     
514 } pluginHandleT;
515
516 // Client Context Structure Hold any specific to client [will be destroyed when client leave]
517 typdef struc {
518     dev_ctx radio; // pointer to client radio
519     int     idx;   // index of radio within global array
520     
521 } clientHandleT;
522
523
524 // It his was not a demo only, it should be smarter to enable hot plug/unplug
525 STATIC updateRadioDevList(pluginHandleT *handle) {
526   int idx;  
527     
528   // loop on existing radio if any
529   for (idx = 0; idx < _radio_dev_count; idx++) {
530       if (idx == MAX_RADIO) break;
531       handle->radios[idx] = calloc(1, sizeof(radioDevT)) // use calloc to set used to FALSE
532       handle->radios[idx]->name = _radio_dev_name(num); 
533   }
534   handle->devCount = _radio_dev_count;
535 }
536
537
538 // This is call at plugin load time [radio devices might still not be visible]
539 STATIC  pluginHandleT* initRadioPlugin() {
540     
541   // Allocate Plugin handle  
542   pluginHandleT  *handle = calloc (1,sizeof (pluginHandleT)); // init handle with zero
543   
544   // Some initialisation steps
545   updateRadioDevList(handle);
546   
547   return (handle);
548 }
549
550 // Stop a radio free related ressource and make it avaliable for other clients
551 STATIC AFB_error releaseRadio (pluginHandleT* handle, AFB_clientCtx *client) {
552     
553    // change radio status
554    handle->radios[client->idx].used = FALSE;
555    
556    // stop related threads and free attached resources
557    radio_stop (&client->radio);
558    
559    // May be some further cleanup ????
560    
561    return (AFB_SUCCESS); // Could it fails ????
562 }
563
564
565 // Start a radio and reserve exclusive usage to requesting client
566 STATIC clientHandleT  *reserveRadio (pluginHandleT* handle) {
567     clientHandleT *client;
568     
569    // loop on existing radio if any
570     for (idx = 0; idx < _radio_dev_count; idx++) {
571         if (handle->radios[client->idx].used = FALSE) break;
572     }
573     
574     // No avaliable radio return now
575     if (idx == MAX_RADIO) return (NULL);
576     
577    // Book radio
578    handle->radios[client->idx].used = TRUE;
579    
580    // create client handle 
581    client = calloc (1, sizeof (clientHandleT));
582    
583    // stop related threads and free attached resources
584    radio_start (&client->radio);
585    
586    // May be some things to do ????
587    
588    
589    return (client);
590 }
591
592 // This is called when client session died [ex; client quit for more than 15mn]
593 STATIC freeRadio (clientHandleT *client) {
594     
595     releaseRadio (handle, client);
596     free (client);
597 }
598
599
600 STATIC json_object* powerOnOff (AFB_session *session, AFB_request *request) {
601     json_object *jresp;
602     AFB_clientCtx *client=request->client; // get client context from request
603     dev_ctx *dev_ctx = (dev_ctx *)client->ctx;
604    
605     // Make sure binder was started with client session
606     if ((client != NULL) {
607         request->errcode=MHD_HTTP_FORBIDDEN;
608         return (jsonNewMessage(AFB_FAIL, "Radio binder need session [--token=xxxx]"));        
609     }
610      
611     // If we have a handle radio was on let power it down
612     if (client->handle != NULL) {
613         
614         releaseRadio (handle, client);  // poweroff client related radio
615         
616         jresp = json_object_new_object();
617         json_object_object_add(jresp, "power", json_object_new_string ("off"));        
618         return (jresp);
619     }
620         
621     // request a new client context token and check result 
622     if (AFB_UNAUTH == ctxTokenCreate (request)) {
623         request->errcode=MHD_HTTP_UNAUTHORIZED;
624         jresp= jsonNewMessage(AFB_FAIL, "You're not authorized to request a radio [make sure you have the right authentication token");
625         return (jresp);
626     }
627     
628     // Client is clean let's look it we have an avaliable radio to propose
629     
630     // make sure we have last hot plug dongle visible
631     updateRadioDevList (handle); 
632     
633     // get try to get an unused radio
634     client->handle = reserveRadio (handle);  
635     if (client->handle == NULL) {
636        return (jsonNewMessage(AFB_FAIL, "Sory No More Radio Avaliable")); 
637     }  
638     
639     // At this point we should have something to retreive radio status before last poweroff [but this is only a demonstrator]
640 }
641
642
643 STATIC  AFB_restapi pluginApis[]= {
644   {"power"  , (AFB_apiCB)powerOnOff , "Ping Application Framework"},
645   {"start"  , (AFB_apiCB)start      , "Ping Application Framework"},
646   {"stop"   , (AFB_apiCB)stop       , "Ping Application Framework"},
647   {NULL}
648 };
649
650 PUBLIC AFB_plugin *radioRegister (AFB_session *session) {
651     AFB_plugin *plugin = malloc (sizeof (AFB_plugin));
652     plugin->type  = AFB_PLUGIN_JSON;
653     plugin->info  = "Application Framework Binder - Radio plugin";
654     plugin->prefix  = "radio";
655     plugin->apis  = pluginApis;
656     
657     plugin->handle = initRadioPlugin();
658     plugin->freeCtxCB = freeRadio();
659
660     return (plugin);
661 };