xref: /btstack/example/sco_demo_util.c (revision bf958e42ac0dfb7d6865b6638e1b8ba57df9bcf0)
1 /*
2  * Copyright (C) 2016 BlueKitchen GmbH
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  *    notice, this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  *    notice, this list of conditions and the following disclaimer in the
12  *    documentation and/or other materials provided with the distribution.
13  * 3. Neither the name of the copyright holders nor the names of
14  *    contributors may be used to endorse or promote products derived
15  *    from this software without specific prior written permission.
16  * 4. Any redistribution, use, or modification is done solely for
17  *    personal benefit and not for any commercial purpose or for
18  *    monetary gain.
19  *
20  * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
21  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
24  * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
30  * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
31  * SUCH DAMAGE.
32  *
33  * Please inquire about commercial licensing options at
34  * [email protected]
35  *
36  */
37 
38 #define BTSTACK_FILE__ "sco_demo_util.c"
39 
40 /*
41  * sco_demo_util.c - send/receive test data via SCO, used by hfp_*_demo and hsp_*_demo
42  */
43 
44 #include <stdio.h>
45 
46 #include "sco_demo_util.h"
47 
48 #include "btstack_audio.h"
49 #include "btstack_debug.h"
50 #include "btstack_ring_buffer.h"
51 #include "classic/btstack_cvsd_plc.h"
52 #include "classic/btstack_sbc.h"
53 #include "classic/hfp.h"
54 #include "classic/hfp_msbc.h"
55 
56 #ifdef _MSC_VER
57 // ignore deprecated warning for fopen
58 #pragma warning(disable : 4996)
59 #endif
60 
61 #ifdef HAVE_POSIX_FILE_IO
62 #include "wav_util.h"
63 #endif
64 
65 // test modes
66 #define SCO_DEMO_MODE_SINE		 0
67 #define SCO_DEMO_MODE_MICROPHONE 1
68 
69 // SCO demo configuration
70 #define SCO_DEMO_MODE               SCO_DEMO_MODE_MICROPHONE
71 
72 // number of sco packets until 'report' on console
73 #define SCO_REPORT_PERIOD           100
74 
75 
76 #ifdef HAVE_POSIX_FILE_IO
77 // length and name of wav file on disk
78 #define SCO_WAV_DURATION_IN_SECONDS 15
79 #define SCO_WAV_FILENAME            "sco_input.wav"
80 #endif
81 
82 // constants
83 #define NUM_CHANNELS            1
84 #define SAMPLE_RATE_8KHZ        8000
85 #define SAMPLE_RATE_16KHZ       16000
86 #define BYTES_PER_FRAME         2
87 
88 // pre-buffer for CVSD and mSBC - also defines latency
89 #define SCO_PREBUFFER_MS      50
90 #define PREBUFFER_BYTES_8KHZ  (SCO_PREBUFFER_MS *  SAMPLE_RATE_8KHZ/1000 * BYTES_PER_FRAME)
91 #define PREBUFFER_BYTES_16KHZ (SCO_PREBUFFER_MS * SAMPLE_RATE_16KHZ/1000 * BYTES_PER_FRAME)
92 
93 static uint16_t              audio_prebuffer_bytes;
94 
95 // output
96 static int                   audio_output_paused  = 0;
97 static uint8_t               audio_output_ring_buffer_storage[2 * PREBUFFER_BYTES_16KHZ];
98 static btstack_ring_buffer_t audio_output_ring_buffer;
99 
100 // input
101 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE
102 #define USE_AUDIO_INPUT
103 #else
104 #define USE_ADUIO_GENERATOR
105 static void (*sco_demo_audio_generator)(uint16_t num_samples, int16_t * data);
106 #endif
107 static int                   audio_input_paused  = 0;
108 static uint8_t               audio_input_ring_buffer_storage[2 * PREBUFFER_BYTES_16KHZ];
109 static btstack_ring_buffer_t audio_input_ring_buffer;
110 
111 static int count_sent = 0;
112 static int count_received = 0;
113 
114 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
115 static btstack_sbc_decoder_state_t decoder_state;
116 #endif
117 
118 static btstack_cvsd_plc_state_t cvsd_plc_state;
119 
120 #define MAX_NUM_MSBC_SAMPLES (16*8)
121 
122 int num_samples_to_write;
123 int num_audio_frames;
124 
125 // generic codec support
126 typedef struct {
127     void (*init)(void);
128     void(*receive)(const uint8_t * packet, uint16_t size);
129     void (*fill_payload)(uint8_t * payload_buffer, uint16_t sco_payload_length);
130     void (*close)(void);
131 } codec_support_t;
132 static const codec_support_t * codec_current = NULL;
133 
134 // sine generator
135 
136 #ifdef USE_ADUIO_GENERATOR
137 static unsigned int phase;
138 
139 // input signal: pre-computed sine wave, 266 Hz at 16000 kHz
140 static const int16_t sine_int16_at_16000hz[] = {
141      0,   3135,   6237,   9270,  12202,  14999,  17633,  20073,  22294,  24270,
142  25980,  27406,  28531,  29344,  29835,  30000,  29835,  29344,  28531,  27406,
143  25980,  24270,  22294,  20073,  17633,  14999,  12202,   9270,   6237,   3135,
144      0,  -3135,  -6237,  -9270, -12202, -14999, -17633, -20073, -22294, -24270,
145 -25980, -27406, -28531, -29344, -29835, -30000, -29835, -29344, -28531, -27406,
146 -25980, -24270, -22294, -20073, -17633, -14999, -12202,  -9270,  -6237,  -3135,
147 };
148 
149 // 8 kHz samples for CVSD/SCO packets in little endian
150 static void sco_demo_sine_wave_int16_at_8000_hz_host_endian(uint16_t num_samples, int16_t * data){
151     unsigned int i;
152     for (i=0; i < num_samples; i++){
153         data[i] = sine_int16_at_16000hz[phase];
154         // ony use every second sample from 16khz table to get 8khz
155         phase += 2;
156         if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){
157             phase = 0;
158         }
159     }
160 }
161 
162 // 16 kHz samples for mSBC encoder in host endianess
163 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
164 static void sco_demo_sine_wave_int16_at_16000_hz_host_endian(uint16_t num_samples, int16_t * data){
165     unsigned int i;
166     for (i=0; i < num_samples; i++){
167         data[i] = sine_int16_at_16000hz[phase++];
168         if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){
169             phase = 0;
170         }
171     }
172 }
173 #endif
174 #endif
175 
176 // Audio Playback / Recording
177 
178 static void audio_playback_callback(int16_t * buffer, uint16_t num_samples){
179 
180     // fill with silence while paused
181     if (audio_output_paused){
182         if (btstack_ring_buffer_bytes_available(&audio_output_ring_buffer) < audio_prebuffer_bytes){
183             memset(buffer, 0, num_samples * BYTES_PER_FRAME);
184            return;
185         } else {
186             // resume playback
187             audio_output_paused = 0;
188         }
189     }
190 
191     // get data from ringbuffer
192     uint32_t bytes_read = 0;
193     btstack_ring_buffer_read(&audio_output_ring_buffer, (uint8_t *) buffer, num_samples * BYTES_PER_FRAME, &bytes_read);
194     num_samples -= bytes_read / BYTES_PER_FRAME;
195     buffer      += bytes_read / BYTES_PER_FRAME;
196 
197     // fill with 0 if not enough
198     if (num_samples){
199         memset(buffer, 0, num_samples * BYTES_PER_FRAME);
200         audio_output_paused = 1;
201     }
202 }
203 
204 #ifdef USE_AUDIO_INPUT
205 static void audio_recording_callback(const int16_t * buffer, uint16_t num_samples){
206     btstack_ring_buffer_write(&audio_input_ring_buffer, (uint8_t *)buffer, num_samples * 2);
207 }
208 #endif
209 
210 // return 1 if ok
211 static int audio_initialize(int sample_rate){
212 
213     // -- output -- //
214 
215     // init buffers
216     memset(audio_output_ring_buffer_storage, 0, sizeof(audio_output_ring_buffer_storage));
217     btstack_ring_buffer_init(&audio_output_ring_buffer, audio_output_ring_buffer_storage, sizeof(audio_output_ring_buffer_storage));
218 
219     // config and setup audio playback
220     const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance();
221     if (!audio_sink) return 0;
222 
223     audio_sink->init(1, sample_rate, &audio_playback_callback);
224     audio_sink->start_stream();
225 
226     audio_output_paused  = 1;
227 
228     // -- input -- //
229 
230     // init buffers
231     memset(audio_input_ring_buffer_storage, 0, sizeof(audio_input_ring_buffer_storage));
232     btstack_ring_buffer_init(&audio_input_ring_buffer, audio_input_ring_buffer_storage, sizeof(audio_input_ring_buffer_storage));
233     audio_input_paused  = 1;
234 
235 #ifdef USE_AUDIO_INPUT
236     // config and setup audio recording
237     const btstack_audio_source_t * audio_source = btstack_audio_source_get_instance();
238     if (!audio_source) return 0;
239 
240     audio_source->init(1, sample_rate, &audio_recording_callback);
241     audio_source->start_stream();
242 #endif
243 
244     return 1;
245 }
246 
247 static void audio_terminate(void){
248     const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance();
249     if (!audio_sink) return;
250     audio_sink->close();
251 
252 #ifdef USE_AUDIO_INPUT
253     const btstack_audio_source_t * audio_source= btstack_audio_source_get_instance();
254     if (!audio_source) return;
255     audio_source->close();
256 #endif
257 }
258 
259 
260 // CVSD - 8 kHz
261 
262 static void sco_demo_cvsd_init(void){
263     printf("SCO Demo: Init CVSD\n");
264 
265     btstack_cvsd_plc_init(&cvsd_plc_state);
266 
267     audio_prebuffer_bytes = PREBUFFER_BYTES_8KHZ;
268 
269 #ifdef USE_ADUIO_GENERATOR
270     sco_demo_audio_generator = &sco_demo_sine_wave_int16_at_8000_hz_host_endian;
271 #endif
272 
273 #ifdef SCO_WAV_FILENAME
274     num_samples_to_write = SAMPLE_RATE_8KHZ * SCO_WAV_DURATION_IN_SECONDS;
275     wav_writer_open(SCO_WAV_FILENAME, 1, SAMPLE_RATE_8KHZ);
276 #endif
277 
278     audio_initialize(SAMPLE_RATE_8KHZ);
279 }
280 
281 static void sco_demo_cvsd_receive(const uint8_t * packet, uint16_t size){
282 
283     int16_t audio_frame_out[128];    //
284 
285     if (size > sizeof(audio_frame_out)){
286         printf("sco_demo_cvsd_receive: SCO packet larger than local output buffer - dropping data.\n");
287         return;
288     }
289 
290     const int audio_bytes_read = size - 3;
291     const int num_samples = audio_bytes_read / BYTES_PER_FRAME;
292 
293     // convert into host endian
294     int16_t audio_frame_in[128];
295     int i;
296     for (i=0;i<num_samples;i++){
297         audio_frame_in[i] = little_endian_read_16(packet, 3 + i * 2);
298     }
299 
300     // treat packet as bad frame if controller does not report 'all good'
301     bool bad_frame = (packet[1] & 0x30) != 0;
302 
303     btstack_cvsd_plc_process_data(&cvsd_plc_state, bad_frame, audio_frame_in, num_samples, audio_frame_out);
304 
305 #ifdef SCO_WAV_FILENAME
306     // Samples in CVSD SCO packet are in little endian, ready for wav files (take shortcut)
307     const int samples_to_write = btstack_min(num_samples, num_samples_to_write);
308     wav_writer_write_le_int16(samples_to_write, audio_frame_out);
309     num_samples_to_write -= samples_to_write;
310     if (num_samples_to_write == 0){
311         wav_writer_close();
312     }
313 #endif
314 
315     btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read);
316 }
317 
318 static void sco_demo_cvsd_fill_payload(uint8_t * payload_buffer, uint16_t sco_payload_length){
319     uint16_t bytes_to_copy = sco_payload_length;
320 
321     // get data from ringbuffer
322     uint16_t pos = 0;
323     if (!audio_input_paused){
324         uint16_t samples_to_copy = sco_payload_length / 2;
325         uint32_t bytes_read = 0;
326         btstack_ring_buffer_read(&audio_input_ring_buffer, payload_buffer, bytes_to_copy, &bytes_read);
327         // flip 16 on big endian systems
328         // @note We don't use (uint16_t *) casts since all sample addresses are odd which causes crahses on some systems
329         if (btstack_is_big_endian()){
330             uint16_t i;
331             for (i=0;i<samples_to_copy/2;i+=2){
332                 uint8_t tmp           = payload_buffer[i*2];
333                 payload_buffer[i*2]   = payload_buffer[i*2+1];
334                 payload_buffer[i*2+1] = tmp;
335             }
336         }
337         bytes_to_copy -= bytes_read;
338         pos           += bytes_read;
339     }
340 
341     // fill with 0 if not enough
342     if (bytes_to_copy){
343         memset(payload_buffer + pos, 0, bytes_to_copy);
344         audio_input_paused = 1;
345     }
346 }
347 
348 static void sco_demo_cvsd_close(void){
349     printf("Used CVSD with PLC, number of proccesed frames: \n - %d good frames, \n - %d bad frames.\n", cvsd_plc_state.good_frames_nr, cvsd_plc_state.bad_frames_nr);
350 }
351 
352 static const codec_support_t codec_cvsd = {
353         .init         = &sco_demo_cvsd_init,
354         .receive      = &sco_demo_cvsd_receive,
355         .fill_payload = &sco_demo_cvsd_fill_payload,
356         .close        = &sco_demo_cvsd_close
357 };
358 
359 // mSBC - 16 kHz
360 
361 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
362 
363 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){
364     UNUSED(context);
365     UNUSED(sample_rate);
366     UNUSED(data);
367     UNUSED(num_samples);
368     UNUSED(num_channels);
369 
370     // samples in callback in host endianess, ready for playback
371     btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2);
372 
373 #ifdef SCO_WAV_FILENAME
374     if (!num_samples_to_write) return;
375     num_samples = btstack_min(num_samples, num_samples_to_write);
376     num_samples_to_write -= num_samples;
377     wav_writer_write_int16(num_samples, data);
378     if (num_samples_to_write == 0){
379         wav_writer_close();
380     }
381 #endif /* SCO_WAV_FILENAME */
382 }
383 
384 static void sco_demo_msbc_init(void){
385     printf("SCO Demo: Init mSBC\n");
386 
387     btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL);
388     hfp_msbc_init();
389 
390     audio_prebuffer_bytes = PREBUFFER_BYTES_16KHZ;
391 
392 #ifdef USE_ADUIO_GENERATOR
393     sco_demo_audio_generator = &sco_demo_sine_wave_int16_at_16000_hz_host_endian;
394 #endif
395 
396 #ifdef SCO_WAV_FILENAME
397     num_samples_to_write = SAMPLE_RATE_16KHZ * SCO_WAV_DURATION_IN_SECONDS;
398     wav_writer_open(SCO_WAV_FILENAME, 1, SAMPLE_RATE_16KHZ);
399 #endif
400 
401     audio_initialize(SAMPLE_RATE_16KHZ);
402 }
403 
404 static void sco_demo_msbc_receive(const uint8_t * packet, uint16_t size){
405     btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3);
406 }
407 
408 void sco_demo_msbc_fill_payload(uint8_t * payload_buffer, uint16_t sco_payload_length){
409     if (!audio_input_paused){
410         int num_samples = hfp_msbc_num_audio_samples_per_frame();
411         btstack_assert(num_samples <= MAX_NUM_MSBC_SAMPLES);
412         if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= (unsigned int)(num_samples * BYTES_PER_FRAME)){
413             int16_t sample_buffer[MAX_NUM_MSBC_SAMPLES];
414             uint32_t bytes_read;
415             btstack_ring_buffer_read(&audio_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * BYTES_PER_FRAME, &bytes_read);
416             hfp_msbc_encode_audio_frame(sample_buffer);
417             num_audio_frames++;
418         }
419         btstack_assert(hfp_msbc_num_bytes_in_stream() >= sco_payload_length);
420     }
421 
422     // get data from encoder, fill with 0 if not enough
423     if (audio_input_paused || hfp_msbc_num_bytes_in_stream() < sco_payload_length){
424         // just send '0's
425         memset(payload_buffer, 0, sco_payload_length);
426         audio_input_paused = 1;
427     } else {
428         hfp_msbc_read_from_stream(payload_buffer, sco_payload_length);
429     }
430 }
431 
432 static void sco_demo_msbc_close(void){
433     printf("Used mSBC with PLC, number of processed frames: \n - %d good frames, \n - %d zero frames, \n - %d bad frames.\n", decoder_state.good_frames_nr, decoder_state.zero_frames_nr, decoder_state.bad_frames_nr);
434 }
435 
436 static const codec_support_t codec_msbc = {
437         .init         = &sco_demo_msbc_init,
438         .receive      = &sco_demo_msbc_receive,
439         .fill_payload = &sco_demo_msbc_fill_payload,
440         .close        = &sco_demo_msbc_close
441 };
442 
443 #endif /* ENABLE_HFP_WIDE_BAND_SPEECH */
444 
445 void sco_demo_init(void){
446 
447 #ifdef ENABLE_CLASSIC_LEGACY_CONNECTIONS_FOR_SCO_DEMOS
448     printf("Disable BR/EDR Secure Connctions due to incompatibilities with SCO connections\n");
449     gap_secure_connections_enable(false);
450 #endif
451 
452 	// status
453 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE
454     printf("SCO Demo: Sending and receiving audio via btstack_audio.\n");
455 #endif
456 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
457     if (btstack_audio_sink_get_instance()){
458         printf("SCO Demo: Sending sine wave, audio output via btstack_audio.\n");
459     } else {
460         printf("SCO Demo: Sending sine wave, hexdump received data.\n");
461     }
462 #endif
463 
464     // Set SCO for CVSD (mSBC or other codecs automatically use 8-bit transparent mode)
465     hci_set_sco_voice_setting(0x60);    // linear, unsigned, 16-bit, CVSD
466 }
467 
468 void sco_demo_set_codec(uint8_t negotiated_codec){
469     switch (negotiated_codec){
470         case HFP_CODEC_CVSD:
471             codec_current = &codec_cvsd;
472             break;
473 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
474         case HFP_CODEC_MSBC:
475             codec_current = &codec_msbc;
476             break;
477 #endif
478         default:
479             btstack_assert(false);
480             break;
481     }
482 
483     codec_current->init();
484 }
485 
486 void sco_demo_receive(uint8_t * packet, uint16_t size){
487     static uint32_t packets = 0;
488     static uint32_t crc_errors = 0;
489     static uint32_t data_received = 0;
490     static uint32_t byte_errors = 0;
491 
492     count_received++;
493 
494     data_received += size - 3;
495     packets++;
496     if (data_received > 100000){
497         printf("Summary: data %07u, packets %04u, packet with crc errors %0u, byte errors %04u\n",  (unsigned int) data_received,  (unsigned int) packets, (unsigned int) crc_errors, (unsigned int) byte_errors);
498         crc_errors = 0;
499         byte_errors = 0;
500         data_received = 0;
501         packets = 0;
502     }
503 
504     codec_current->receive(packet, size);
505 }
506 
507 void sco_demo_send(hci_con_handle_t sco_handle){
508 
509     if (sco_handle == HCI_CON_HANDLE_INVALID) return;
510 
511     int sco_packet_length = hci_get_sco_packet_length();
512     int sco_payload_length = sco_packet_length - 3;
513 
514     hci_reserve_packet_buffer();
515     uint8_t * sco_packet = hci_get_outgoing_packet_buffer();
516 
517 #ifdef USE_ADUIO_GENERATOR
518     #define REFILL_SAMPLES 16
519     // re-fill audio buffer
520     uint16_t samples_free = btstack_ring_buffer_bytes_free(&audio_input_ring_buffer) / 2;
521     while (samples_free > 0){
522         int16_t samples_buffer[REFILL_SAMPLES];
523         uint16_t samples_to_add = btstack_min(samples_free, REFILL_SAMPLES);
524         (*sco_demo_audio_generator)(samples_to_add, samples_buffer);
525         btstack_ring_buffer_write(&audio_input_ring_buffer, (uint8_t *)samples_buffer, samples_to_add * 2);
526         samples_free -= samples_to_add;
527     }
528 #endif
529 
530     // resume if pre-buffer is filled
531     if (audio_input_paused){
532         if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= audio_prebuffer_bytes){
533             // resume sending
534             audio_input_paused = 0;
535         }
536     }
537 
538     // fill payload by codec
539     codec_current->fill_payload(&sco_packet[3], sco_payload_length);
540 
541     // set handle + flags
542     little_endian_store_16(sco_packet, 0, sco_handle);
543     // set len
544     sco_packet[2] = sco_payload_length;
545     // finally send packet
546     hci_send_sco_packet_buffer(sco_packet_length);
547 
548     // request another send event
549     hci_request_sco_can_send_now_event();
550 
551     count_sent++;
552     if ((count_sent % SCO_REPORT_PERIOD) == 0) {
553         printf("SCO: sent %u, received %u\n", count_sent, count_received);
554     }
555 }
556 
557 void sco_demo_close(void){
558     printf("SCO demo close\n");
559 
560     printf("SCO demo statistics: ");
561     codec_current->close();
562     codec_current = NULL;
563 
564 #if defined(SCO_WAV_FILENAME)
565     wav_writer_close();
566 #endif
567 
568     audio_terminate();
569 }
570