gsm-voice-routing.c

   1 /*
   2  * GTA04 gsm voice routing utility
   3  * Copyright (c) 2012 Radek Polak
   4  *
   5  * gta04-gsm-voice-routing is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU Lesser General Public
   7  * License as published by the Free Software Foundation; either
   8  * version 2.1 of the License, or (at your option) any later version.
   9  *
  10  * gta04-gsm-voice-routing 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 GNU
  13  * Lesser General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU Lesser General Public
  16  * License along with gta04-gsm-voice-routing; if not, write to the Free Software
  17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  18  */
  19
  20 /*
  21
  22 This program routes sound between GTA04 internal sound card ("default") and
  23 and UMTS modem sound card ("hw:1,0")
  24
  25 The function can be written with following shell script:
  26
  27 arecord -fS16_LE | aplay -Dhw:1,0 &
  28 arecord -Dhw:1,0 -fS16_LE | aplay
  29
  30 However the program is written in C for better control over the process. E.g.
  31 we want to wait with routing until sound is available from UMTS.
  32
  33 We have 4 streams called r0, p1, r1, p0
  34
  35 r0 - record from hw:0,0 (default) internal sound card
  36 r1 - record from hw:1,0 umts sound card
  37 p0 - play on hw:0,0 (default) internal sound card
  38 p1 - play on hw:1,0 umts sound card
  39
  40 All streams have rate 8000 (rate of umts sound card), 1 channel and 16bit per
  41 sample (SND_PCM_FORMAT_S16_LE).
  42
  43 We set buffer_size of sound card to 1024.
  44 The sound card buffer consists of 4 periods.
  45 Period has 256 frames
  46 Frame has just one sample (one channel) and both sample and frame are 2 bytes
  47
  48 We always play/record one period (256 samples, 512 bytes). At rate 8000Hz if
  49 we had 8000 samples it would be 1s, with 256 samples it makes 31.25ms long
  50 period - this is our latency.
  51
  52 */
  53
  54 /* Use the newer ALSA API */
  55 #define ALSA_PCM_NEW_HW_PARAMS_API
  56
  57 /* Define this to enable speex acoustic echo cancellation */
  58 #define USE_SPEEX_AEC
  59
  60 /* Define this to use walkie talkie echo reduction */
  61 //#define USE_WALKIE_TALKIE_AEC
  62
  63 #include <time.h>
  64 #include <fcntl.h>
  65 #include <signal.h>
  66 #include <unistd.h>
  67 #include <string.h>
  68 #include <sys/stat.h>
  69 #include <alsa/asoundlib.h>
  70
  71 #ifdef USE_SPEEX_AEC
  72 #include <speex/speex_echo.h>
  73 #endif
  74
  75 #define ERR_PCM_OPEN_FAILED -1
  76 #define ERR_HW_PARAMS_ANY -2
  77 #define ERR_HW_PARAMS_SET_ACCESS -3
  78 #define ERR_HW_PARAMS_SET_FORMAT -4
  79 #define ERR_HW_PARAMS_SET_CHANNELS -5
  80 #define ERR_HW_PARAMS_SET_RATE -6
  81 #define ERR_SW_PARAMS_CURRENT -7
  82 #define ERR_HW_PARAMS_SET_PERIOD_SIZE -8
  83 #define ERR_HW_PARAMS_SET_BUFFER_SIZE -9
  84 #define ERR_HW_PARAMS -10
  85 #define ERR_BUFFER_ALLOC_FAILED -11
  86 #define ERR_SW_PARAMS_SET_START_THRESHOLD -12
  87 #define ERR_SW_PARAMS_SET_STOP_THRESHOLD -13
  88 #define ERR_SW_PARAMS -14
  89 #define ERR_READ_OVERRUN -15
  90 #define ERR_READ -16
  91 #define ERR_SHORT_READ -17
  92 #define ERR_WRITE_UNDERRUN -18
  93 #define ERR_WRITE -19
  94 #define ERR_SHORT_WRITE -20
  95 #define ERR_TERMINATING -21
  96
  97 #define s16 short
  98 #define u16 unsigned short
  99
 100 FILE *logfile;
 101 int terminating = 0;
 102
 103 struct route_stream
 104 {
 105     const char *id;             // in: one of r0, r1, p0, p1
 106     char *pcm_name;             // in: "default" or "hw:1,0"
 107     snd_pcm_stream_t stream;    // in: SND_PCM_STREAM_PLAYBACK / SND_PCM_STREAM_CAPTURE
 108     snd_pcm_uframes_t start_threshold;  // in: start treshold or 0 to keep default
 109     snd_pcm_uframes_t stop_threshold;   // in: stop treshold or 0 to keep default
 110     snd_pcm_uframes_t buffer_size;  // in/out: hw buffer size, e.g. 1024 frames
 111     snd_pcm_uframes_t period_size;  // in/out: period size, e.g. 256 frames
 112
 113     snd_pcm_t *handle;          // out: pcm handle
 114     snd_pcm_hw_params_t *hwparams;  // out:
 115     snd_pcm_sw_params_t *swparams;  // out:
 116     int period_buffer_size;     // out: size 2000 (256 frames=256 samples, one sample=2bytes)
 117     char *period_buffer;        // out: allocated buffer for playing/recording
 118 };
 119
 120 /* Dump error on stderr with stream and error description, and return given
 121    return_code */
 122 static int err(const char *msg, int snd_err, struct route_stream *s,
 123                int return_code)
 124 {
 125     if(terminating) {
 126         return ERR_TERMINATING;
 127     }
 128
 129     fprintf(logfile, "%s (%s): %s", s->id, s->pcm_name, msg);
 130     if (snd_err < 0) {
 131         fprintf(logfile, ": %s", snd_strerror(snd_err));
 132     }
 133     fprintf(logfile, "\n");
 134     return return_code;
 135 }
 136
 137 static int open_route_stream(struct route_stream *s)
 138 {
 139     int rc;
 140
 141     /* Open PCM device for playback. */
 142     rc = snd_pcm_open(&(s->handle), s->pcm_name, s->stream, 0);
 143     if (rc < 0) {
 144         return err("unable to open pcm device", rc, s, ERR_PCM_OPEN_FAILED);
 145     }
 146
 147     /* Allocate a hardware parameters object. */
 148     snd_pcm_hw_params_alloca(&(s->hwparams));
 149
 150     /* Fill it in with default values. */
 151     rc = snd_pcm_hw_params_any(s->handle, s->hwparams);
 152     if (rc < 0) {
 153         return err("snd_pcm_hw_params_any failed", rc, s, ERR_HW_PARAMS_ANY);
 154     }
 155
 156     /* Interleaved mode */
 157     rc = snd_pcm_hw_params_set_access(s->handle, s->hwparams,
 158                                       SND_PCM_ACCESS_RW_INTERLEAVED);
 159     if (rc < 0) {
 160         return err("snd_pcm_hw_params_set_access failed", rc, s,
 161                    ERR_HW_PARAMS_SET_ACCESS);
 162     }
 163
 164     /* Signed 16-bit little-endian format */
 165     rc = snd_pcm_hw_params_set_format(s->handle, s->hwparams,
 166                                       SND_PCM_FORMAT_S16_LE);
 167     if (rc < 0) {
 168         return err("snd_pcm_hw_params_set_format failed", rc, s,
 169                    ERR_HW_PARAMS_SET_FORMAT);
 170     }
 171
 172     /* One channel (mono) */
 173     rc = snd_pcm_hw_params_set_channels(s->handle, s->hwparams, 1);
 174     if (rc < 0) {
 175         return err("snd_pcm_hw_params_set_channels failed", rc, s,
 176                    ERR_HW_PARAMS_SET_CHANNELS);
 177     }
 178
 179     /* 8000 bits/second sampling rate (umts modem quality) */
 180     rc = snd_pcm_hw_params_set_rate(s->handle, s->hwparams, 8000, 0);
 181     if (rc < 0) {
 182         return err("snd_pcm_hw_params_set_rate_near failed", rc, s,
 183                    ERR_HW_PARAMS_SET_RATE);
 184     }
 185
 186     /* Period size in frames (e.g. 256) */
 187     rc = snd_pcm_hw_params_set_period_size(s->handle, s->hwparams,
 188                                            s->period_size, 0);
 189     if (rc < 0) {
 190         return err("snd_pcm_hw_params_set_period_size failed", rc, s,
 191                    ERR_HW_PARAMS_SET_PERIOD_SIZE);
 192     }
 193
 194     /* Buffer size in frames (e.g. 1024) */
 195     rc = snd_pcm_hw_params_set_buffer_size(s->handle, s->hwparams,
 196                                            s->buffer_size);
 197     if (rc < 0) {
 198         return err("snd_pcm_hw_params_set_buffer_size failed", rc, s,
 199                    ERR_HW_PARAMS_SET_BUFFER_SIZE);
 200     }
 201
 202     /* Write the parameters to the driver */
 203     rc = snd_pcm_hw_params(s->handle, s->hwparams);
 204     if (rc < 0) {
 205         return err("snd_pcm_hw_params failed", rc, s, ERR_HW_PARAMS);
 206     }
 207
 208     /* Allocate buffer for one period twice as big as period_size because:
 209        1 frame = 1 sample = 2 bytes because of S16_LE and 1 channel */
 210     s->period_buffer_size = 2 * s->period_size;
 211     s->period_buffer = (char *)malloc(s->period_buffer_size);
 212     if (s->period_buffer == 0) {
 213         return err("period_buffer alloc failed", 0, s, ERR_BUFFER_ALLOC_FAILED);
 214     }
 215
 216     /* Setup software params */
 217     if (s->start_threshold > 0 || s->stop_threshold > 0) {
 218         snd_pcm_sw_params_alloca(&(s->swparams));
 219
 220         rc = snd_pcm_sw_params_current(s->handle, s->swparams);
 221         if (rc < 0) {
 222             return err("snd_pcm_sw_params_current failed", rc, s,
 223                        ERR_SW_PARAMS_CURRENT);
 224         }
 225
 226         /* start_threshold */
 227         if (s->start_threshold > 0) {
 228             rc = snd_pcm_sw_params_set_start_threshold(s->handle,
 229                                                        s->swparams,
 230                                                        s->start_threshold);
 231             if (rc < 0) {
 232                 return err("snd_pcm_sw_params_set_start_threshold failed", rc,
 233                            s, ERR_SW_PARAMS_SET_START_THRESHOLD);
 234             }
 235         }
 236
 237         /* stop_threshold */
 238         if (s->stop_threshold > 0) {
 239             rc = snd_pcm_sw_params_set_stop_threshold(s->handle,
 240                                                       s->swparams,
 241                                                       s->stop_threshold);
 242             if (rc < 0) {
 243                 return err("snd_pcm_sw_params_set_start_threshold failed", rc,
 244                            s, ERR_SW_PARAMS_SET_STOP_THRESHOLD);
 245             }
 246         }
 247
 248         rc = snd_pcm_sw_params(s->handle, s->swparams);
 249         if (rc < 0) {
 250             return err("snd_pcm_sw_params failed", rc, s, ERR_SW_PARAMS);
 251         }
 252     }
 253
 254     /* Uncomment to dump hw setup */
 255     /* static snd_output_t *log;
 256        snd_output_stdio_attach(&log, logfile, 0);
 257        snd_pcm_dump(s->handle, log);
 258        snd_output_close(log); */
 259
 260     return 0;
 261 }
 262
 263 static int close_route_stream(struct route_stream *s)
 264 {
 265     if (s->handle == 0) {
 266         return 0;
 267     }
 268     snd_pcm_close(s->handle);
 269     s->handle = 0;
 270     if (s->period_buffer == 0) {
 271         return 0;
 272     }
 273     free(s->period_buffer);
 274     s->period_buffer = 0;
 275     return 0;
 276 }
 277
 278 static void open_route_stream_repeated(struct route_stream *s)
 279 {
 280     int rc;
 281     for (;;) {
 282         rc = open_route_stream(s);
 283         if (rc == 0) {
 284             return;
 285         }
 286         close_route_stream(s);
 287         fprintf(logfile, "retrying in 100 ms\n");
 288         usleep(1000 * 100);
 289     }
 290 }
 291
 292 static int route_stream_read(struct route_stream *s)
 293 {
 294     int rc;
 295
 296     if (terminating) {
 297         return ERR_TERMINATING;
 298     }
 299
 300     rc = snd_pcm_readi(s->handle, s->period_buffer, s->period_size);
 301     if (rc == s->period_size) {
 302         return 0;
 303     }
 304
 305     /* EPIPE means overrun */
 306     if (rc == -EPIPE) {
 307         err("overrun occured", rc, s, ERR_READ_OVERRUN);
 308         snd_pcm_prepare(s->handle);
 309         return ERR_READ_OVERRUN;
 310     }
 311
 312     if (rc < 0) {
 313         return err("snd_pcm_readi failed", rc, s, ERR_READ);
 314     }
 315
 316     return err("short read", rc, s, ERR_SHORT_READ);
 317 }
 318
 319 static int route_stream_write(struct route_stream *s)
 320 {
 321     int rc;
 322
 323     if (terminating) {
 324         return ERR_TERMINATING;
 325     }
 326
 327     rc = snd_pcm_writei(s->handle, s->period_buffer, s->period_size);
 328     if (rc == s->period_size) {
 329         return 0;
 330     }
 331
 332     /* EPIPE means underrun */
 333     if (rc == -EPIPE) {
 334         err("underrun occured", rc, s, ERR_WRITE_UNDERRUN);
 335         snd_pcm_prepare(s->handle);
 336         return ERR_WRITE_UNDERRUN;
 337     }
 338
 339     if (rc < 0) {
 340         return err("snd_pcm_writei failed", rc, s, ERR_WRITE);
 341     }
 342
 343     return err("short write", rc, s, ERR_SHORT_WRITE);
 344 }
 345
 346 /*static void log_with_timestamp(const char *msg)
 347 {
 348     struct timespec tp;
 349     clock_gettime(CLOCK_MONOTONIC, &tp);
 350     fprintf(logfile, "%ld %ld: %s\n", tp.tv_sec, tp.tv_nsec, msg);
 351 }*/
 352
 353 // Write string count bytes long to file
 354 static void write_file(const char *path, const char *value, size_t count)
 355 {
 356     int fd = open(path, O_WRONLY);
 357     if (fd < 0) {
 358         return;
 359     }
 360     write(fd, value, count);
 361     close(fd);
 362 }
 363
 364 int aux_red_state = 0;
 365 int aux_green_state = 0;
 366
 367 static void set_aux_leds(int red, int green)
 368 {
 369     if (aux_red_state == red && aux_green_state == green) {
 370         return;
 371     }
 372     aux_red_state = red;
 373     aux_green_state = green;
 374
 375     if (red) {
 376         write_file("/sys/class/leds/gta04:red:aux/brightness", "255", 3);
 377     } else {
 378         write_file("/sys/class/leds/gta04:red:aux/brightness", "0", 1);
 379     }
 380     if (green) {
 381         write_file("/sys/class/leds/gta04:green:aux/brightness", "255", 3);
 382     } else {
 383         write_file("/sys/class/leds/gta04:green:aux/brightness", "0", 1);
 384     }
 385 }
 386
 387 static void blink_aux()
 388 {
 389     static int last_blink_secs;
 390
 391     struct timespec tp;
 392     clock_gettime(CLOCK_MONOTONIC, &tp);
 393
 394     if (tp.tv_sec == last_blink_secs) {
 395         return;
 396     }
 397     last_blink_secs = tp.tv_sec;
 398     set_aux_leds(!aux_red_state, aux_red_state);
 399 }
 400
 401 static void show_progress()
 402 {
 403 /*   static int counter = 0;
 404     char ch = "|\\-/"[(counter++) % 4];
 405     fputc(ch, logfile);
 406     fputc('\b', logfile);
 407     fflush(logfile);*/
 408 }
 409
 410 #ifdef USE_WALKIE_TALKIE_AEC
 411
 412 static void vol_up(char *buf, snd_pcm_uframes_t period_size)
 413 {
 414     int i;
 415     s16 *val = (s16 *) (buf);
 416     for (i = 0; i < period_size; i++) {
 417         *val = *val * 2;
 418         val++;
 419     }
 420 }
 421
 422 static void vol_down(char *buf, snd_pcm_uframes_t period_size)
 423 {
 424     int i;
 425     s16 *val = (s16 *) (buf);
 426     for (i = 0; i < period_size; i++) {
 427         *val = 0;               // *val / 2;
 428         val++;
 429     }
 430 }
 431
 432 /* Reduce echo by adjusting volumes in record and playback buffer with simple
 433    walkie-talkie like algorithm.
 434
 435    First we decide which buffer has higher volume (i.e. if you are speaking or
 436    listening). This buffer is kept as is and we made the other buffer silent or
 437    very low volume.
 438
 439    We use integer for computing volumes, this should be fine if the buffer size
 440    is not too big (65535 is max).
 441 */
 442 static void reduce_echo(char *p0, char *p1, snd_pcm_uframes_t period_size)
 443 {
 444     int i;
 445     int sum_p0 = 0;
 446     int sum_p1 = 0;
 447     s16 *v0;
 448     s16 *v1;
 449
 450     v0 = (s16 *) (p0);
 451     v1 = (s16 *) (p1);
 452     for (i = 0; i < period_size; i++) {
 453         sum_p0 += abs(*v0);
 454         sum_p1 += abs(*v1);
 455         v0++;
 456         v1++;
 457     }
 458
 459     int diff = sum_p0 - sum_p1;
 460
 461     /* 10000 seems to be good limit value. Silence is ~2000 and speech is
 462        ~80000. It would be nice to somehow compute this value on the fly, but
 463        i have no idea how to do it now */
 464     if (diff > 10000) {
 465         /* fprintf(stderr, "listening p0=%*d    p1=%*d    diff=%*d\n", -6, sum_p0,
 466            -6, sum_p1, -6, diff); */
 467         vol_up(p0, period_size);
 468         vol_down(p1, period_size);
 469         set_aux_leds(0, 1);
 470         return;
 471
 472     } else if (diff < -10000) {
 473         /* fprintf(stderr, "talking   p0=%*d    p1=%*d    diff=%*d\n", -6, sum_p0,
 474            -6, sum_p1, -6, diff); */
 475         vol_up(p1, period_size);
 476         vol_down(p0, period_size);
 477         set_aux_leds(1, 0);
 478     } else {
 479         set_aux_leds(0, 0);
 480     }
 481     //printf("%d, %d, %d\n", round++, sum_a, sum_b);
 482 }
 483 #endif
 484
 485 struct route_stream p0 = {
 486     .id = "p0",
 487     .pcm_name = "default",
 488     .stream = SND_PCM_STREAM_PLAYBACK,
 489     .start_threshold = 1024,
 490     .stop_threshold = 1024,
 491     .buffer_size = 1024,
 492     .period_size = 256,
 493     .handle = 0,
 494     .period_buffer = 0
 495 };
 496
 497 struct route_stream r0 = {
 498     .id = "r0",
 499     .pcm_name = "default",
 500     .stream = SND_PCM_STREAM_CAPTURE,
 501     .start_threshold = 0,
 502     .stop_threshold = 0,
 503     .buffer_size = 1024,
 504     .period_size = 256,
 505     .handle = 0,
 506     .period_buffer = 0
 507 };
 508
 509 struct route_stream p1 = {
 510     .id = "p1",
 511     .pcm_name = "hw:1,0",
 512     .stream = SND_PCM_STREAM_PLAYBACK,
 513     .start_threshold = 1024,
 514     .stop_threshold = 1024,
 515     .buffer_size = 1024,
 516     .period_size = 256,
 517     .handle = 0,
 518     .period_buffer = 0
 519 };
 520
 521 struct route_stream r1 = {
 522     .id = "r1",
 523     .pcm_name = "hw:1,0",
 524     .stream = SND_PCM_STREAM_CAPTURE,
 525     .start_threshold = 0,
 526     .stop_threshold = 0,
 527     .buffer_size = 1024,
 528     .period_size = 256,
 529     .handle = 0,
 530     .period_buffer = 0
 531 };
 532
 533 static void cleanup()
 534 {
 535     close_route_stream(&p0);
 536     close_route_stream(&p1);
 537     close_route_stream(&r0);
 538     close_route_stream(&r1);
 539
 540     set_aux_leds(0, 0);
 541     fclose(logfile);
 542 }
 543
 544 static void sighandler(int signum)
 545 {
 546     if (terminating) {
 547         return;
 548     }
 549     terminating = 1;
 550     fprintf(logfile, "gsm-voice-routing ending - signal %d\n", signum);
 551     cleanup();
 552     exit(0);
 553 }
 554
 555 int main()
 556 {
 557     int rc;
 558     int started = 0;
 559     char *logfilename;
 560 #ifdef USE_SPEEX_AEC
 561     SpeexEchoState *echo_state;
 562 #endif
 563
 564     // Register for TERM and interrupt signals
 565     signal(SIGINT, sighandler);
 566     signal(SIGTERM, sighandler);
 567
 568     blink_aux();                // turn red led on so that we know we started
 569
 570     logfile = stderr;
 571     logfilename = getenv("GSM_VOICE_ROUTING_LOGFILE");
 572     if (logfilename) {
 573         FILE *f = fopen(logfilename, "w");
 574         if (f) {
 575             logfile = f;
 576         } else {
 577             fprintf(stderr, "failed to open logfile %s\n", logfilename);
 578         }
 579     }
 580     fprintf(logfile, "gsm-voice-routing started\n");
 581
 582     /* We want realtime process priority */
 583     rc = nice(-20);
 584     if (rc != -20) {
 585         fprintf(logfile, "nice() failed\n");
 586     }
 587 #ifdef USE_SPEEX_AEC
 588     /* 256=frame (period size), 4096 is filter length (recommended is 1/3 of
 589        reverbation time - for 1s it's 8000 / 3 */
 590     echo_state = speex_echo_state_init(256, 8192);
 591 #endif
 592
 593     /* Open streams - umts first */
 594     open_route_stream_repeated(&p1);
 595     open_route_stream_repeated(&r1);
 596     open_route_stream_repeated(&p0);
 597     open_route_stream_repeated(&r0);
 598
 599     while (route_stream_read(&r1))
 600             ;
 601     snd_pcm_start(r0.handle);
 602     snd_pcm_start(r1.handle);
 603
 604     /* Route sound */
 605     while (!terminating) {
 606
 607         /* Recording  - first from internal card (so that we always clean the
 608            recording buffer), then UMTS, which can fail */
 609         if (route_stream_read(&r0)) {
 610             blink_aux();
 611             continue;
 612         }
 613
 614         rc = route_stream_read(&r1);
 615         if (rc == ERR_READ && started) {
 616             fprintf(logfile,
 617                     "read error after some succesful routing (hangup)\n");
 618             break;
 619         }
 620         if (rc != 0) {
 621             continue;
 622         }
 623
 624         if (started) {
 625             show_progress();
 626         } else {
 627             fprintf(logfile, "voice routing started\n");
 628             started = 1;
 629         }
 630
 631 #ifdef USE_SPEEX_AEC
 632         speex_echo_cancellation(echo_state, (spx_int16_t *) r0.period_buffer,
 633                                 (spx_int16_t *) p0.period_buffer,
 634                                 (spx_int16_t *) p1.period_buffer);
 635
 636         memmove(p0.period_buffer, r1.period_buffer, r1.period_buffer_size);
 637 #endif
 638
 639 #ifdef USE_WALKIE_TALKIE_AEC
 640         memmove(p0.period_buffer, r1.period_buffer, r1.period_buffer_size);
 641         memmove(p1.period_buffer, r0.period_buffer, r0.period_buffer_size);
 642         reduce_echo(p0.period_buffer, p1.period_buffer, p0.period_size);
 643 #endif
 644
 645         route_stream_write(&p0);
 646         route_stream_write(&p1);
 647     }
 648
 649 #ifdef USE_SPEEX_AEC
 650     speex_echo_state_destroy(echo_state);
 651 #endif
 652
 653     fprintf(logfile, "gsm-voice-routing ending\n");
 654     cleanup();
 655     return 0;
 656 }