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_ASCII 1
68 #define SCO_DEMO_MODE_COUNTER 2
69 #define SCO_DEMO_MODE_55 3
70 #define SCO_DEMO_MODE_00 4
71 #define SCO_DEMO_MODE_MICROPHONE 5
72
73 // SCO demo configuration
74 #define SCO_DEMO_MODE SCO_DEMO_MODE_MICROPHONE
75
76 // number of sco packets until 'report' on console
77 #define SCO_REPORT_PERIOD 100
78
79
80 #ifdef HAVE_POSIX_FILE_IO
81 // length and name of wav file on disk
82 #define SCO_WAV_DURATION_IN_SECONDS 15
83 #define SCO_WAV_FILENAME "sco_input.wav"
84
85 // name of sbc test files
86 #define SCO_MSBC_OUT_FILENAME "sco_output.msbc"
87 #define SCO_MSBC_IN_FILENAME "sco_input.msbc"
88 #endif
89
90
91 // pre-buffer for CVSD and mSBC - also defines latency
92 #define SCO_CVSD_PA_PREBUFFER_MS 50
93 #define SCO_MSBC_PA_PREBUFFER_MS 50
94
95 // constants
96 #define NUM_CHANNELS 1
97 #define CVSD_SAMPLE_RATE 8000
98 #define MSBC_SAMPLE_RATE 16000
99 #define BYTES_PER_FRAME 2
100
101 #define CVSD_PA_PREBUFFER_BYTES (SCO_CVSD_PA_PREBUFFER_MS * CVSD_SAMPLE_RATE/1000 * BYTES_PER_FRAME)
102 #define MSBC_PA_PREBUFFER_BYTES (SCO_MSBC_PA_PREBUFFER_MS * MSBC_SAMPLE_RATE/1000 * BYTES_PER_FRAME)
103
104 // output
105
106 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE)
107 static int audio_output_paused = 0;
108 static uint8_t audio_output_ring_buffer_storage[2*MSBC_PA_PREBUFFER_BYTES];
109 static btstack_ring_buffer_t audio_output_ring_buffer;
110 #endif
111
112
113 // input
114 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE
115 #define USE_AUDIO_INPUT
116 static int audio_input_paused = 0;
117 static uint8_t audio_input_ring_buffer_storage[2*8000]; // full second input buffer
118 static btstack_ring_buffer_t audio_input_ring_buffer;
119 #endif
120
121 static int dump_data = 1;
122 static int count_sent = 0;
123 static int count_received = 0;
124 static int negotiated_codec = -1;
125
126 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
127 static btstack_sbc_decoder_state_t decoder_state;
128
129 #ifdef HAVE_POSIX_FILE_IO
130 FILE * msbc_file_in;
131 FILE * msbc_file_out;
132 #endif
133
134 #endif
135
136 static btstack_cvsd_plc_state_t cvsd_plc_state;
137
138 #define MAX_NUM_MSBC_SAMPLES (16*8)
139
140 int num_samples_to_write;
141 int num_audio_frames;
142 unsigned int phase;
143
144 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
145
146 // input signal: pre-computed sine wave, 266 Hz at 16000 kHz
147 static const int16_t sine_int16_at_16000hz[] = {
148 0, 3135, 6237, 9270, 12202, 14999, 17633, 20073, 22294, 24270,
149 25980, 27406, 28531, 29344, 29835, 30000, 29835, 29344, 28531, 27406,
150 25980, 24270, 22294, 20073, 17633, 14999, 12202, 9270, 6237, 3135,
151 0, -3135, -6237, -9270, -12202, -14999, -17633, -20073, -22294, -24270,
152 -25980, -27406, -28531, -29344, -29835, -30000, -29835, -29344, -28531, -27406,
153 -25980, -24270, -22294, -20073, -17633, -14999, -12202, -9270, -6237, -3135,
154 };
155
156 // 8 kHz samples for CVSD/SCO packets in little endian
sco_demo_sine_wave_int16_at_8000_hz_little_endian(unsigned int num_samples,uint8_t * data)157 static void sco_demo_sine_wave_int16_at_8000_hz_little_endian(unsigned int num_samples, uint8_t * data){
158 unsigned int i;
159 for (i=0; i < num_samples; i++){
160 int16_t sample = sine_int16_at_16000hz[phase];
161 little_endian_store_16(data, i * 2, sample);
162 // ony use every second sample from 16khz table to get 8khz
163 phase += 2;
164 if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){
165 phase = 0;
166 }
167 }
168 }
169
170 // 16 kHz samples for mSBC encoder in host endianess
171 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
sco_demo_sine_wave_int16_at_16000_hz_host_endian(unsigned int num_samples,int16_t * data)172 static void sco_demo_sine_wave_int16_at_16000_hz_host_endian(unsigned int num_samples, int16_t * data){
173 unsigned int i;
174 for (i=0; i < num_samples; i++){
175 data[i] = sine_int16_at_16000hz[phase++];
176 if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){
177 phase = 0;
178 }
179 }
180 }
181
sco_demo_msbc_fill_sine_audio_frame(void)182 static void sco_demo_msbc_fill_sine_audio_frame(void){
183 if (!hfp_msbc_can_encode_audio_frame_now()) return;
184 int num_samples = hfp_msbc_num_audio_samples_per_frame();
185 if (num_samples > MAX_NUM_MSBC_SAMPLES) return;
186 int16_t sample_buffer[MAX_NUM_MSBC_SAMPLES];
187 sco_demo_sine_wave_int16_at_16000_hz_host_endian(num_samples, sample_buffer);
188 hfp_msbc_encode_audio_frame(sample_buffer);
189 num_audio_frames++;
190 }
191 #endif
192 #endif
193
194 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE)
195
playback_callback(int16_t * buffer,uint16_t num_samples)196 static void playback_callback(int16_t * buffer, uint16_t num_samples){
197
198 uint32_t prebuffer_bytes;
199 switch (negotiated_codec){
200 case HFP_CODEC_MSBC:
201 prebuffer_bytes = MSBC_PA_PREBUFFER_BYTES;
202 break;
203 case HFP_CODEC_CVSD:
204 default:
205 prebuffer_bytes = CVSD_PA_PREBUFFER_BYTES;
206 break;
207 }
208
209 // fill with silence while paused
210 if (audio_output_paused){
211 if (btstack_ring_buffer_bytes_available(&audio_output_ring_buffer) < prebuffer_bytes){
212 memset(buffer, 0, num_samples * BYTES_PER_FRAME);
213 return;
214 } else {
215 // resume playback
216 audio_output_paused = 0;
217 }
218 }
219
220 // get data from ringbuffer
221 uint32_t bytes_read = 0;
222 btstack_ring_buffer_read(&audio_output_ring_buffer, (uint8_t *) buffer, num_samples * BYTES_PER_FRAME, &bytes_read);
223 num_samples -= bytes_read / BYTES_PER_FRAME;
224 buffer += bytes_read / BYTES_PER_FRAME;
225
226 // fill with 0 if not enough
227 if (num_samples){
228 memset(buffer, 0, num_samples * BYTES_PER_FRAME);
229 audio_output_paused = 1;
230 }
231 }
232
233 #ifdef USE_AUDIO_INPUT
recording_callback(const int16_t * buffer,uint16_t num_samples)234 static void recording_callback(const int16_t * buffer, uint16_t num_samples){
235 btstack_ring_buffer_write(&audio_input_ring_buffer, (uint8_t *)buffer, num_samples * 2);
236 }
237 #endif
238
239 // return 1 if ok
audio_initialize(int sample_rate)240 static int audio_initialize(int sample_rate){
241
242 // -- output -- //
243
244 // init buffers
245 memset(audio_output_ring_buffer_storage, 0, sizeof(audio_output_ring_buffer_storage));
246 btstack_ring_buffer_init(&audio_output_ring_buffer, audio_output_ring_buffer_storage, sizeof(audio_output_ring_buffer_storage));
247
248 // config and setup audio playback
249 const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance();
250 if (!audio_sink) return 0;
251
252 audio_sink->init(1, sample_rate, &playback_callback);
253 audio_sink->start_stream();
254
255 audio_output_paused = 1;
256
257 // -- input -- //
258
259 #ifdef USE_AUDIO_INPUT
260 // init buffers
261 memset(audio_input_ring_buffer_storage, 0, sizeof(audio_input_ring_buffer_storage));
262 btstack_ring_buffer_init(&audio_input_ring_buffer, audio_input_ring_buffer_storage, sizeof(audio_input_ring_buffer_storage));
263
264 // config and setup audio recording
265 const btstack_audio_source_t * audio_source = btstack_audio_source_get_instance();
266 if (!audio_source) return 0;
267
268 audio_source->init(1, sample_rate, &recording_callback);
269 audio_source->start_stream();
270
271 audio_input_paused = 1;
272 #endif
273
274 return 1;
275 }
276
audio_terminate(void)277 static void audio_terminate(void){
278 const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance();
279 if (!audio_sink) return;
280 audio_sink->close();
281
282 #ifdef USE_AUDIO_INPUT
283 const btstack_audio_source_t * audio_source= btstack_audio_source_get_instance();
284 if (!audio_source) return;
285 audio_source->close();
286 #endif
287 }
288
289 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
290
handle_pcm_data(int16_t * data,int num_samples,int num_channels,int sample_rate,void * context)291 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){
292 UNUSED(context);
293 UNUSED(sample_rate);
294 UNUSED(data);
295 UNUSED(num_samples);
296 UNUSED(num_channels);
297
298 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE)
299
300 // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels);
301
302 // samples in callback in host endianess, ready for playback
303 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2);
304
305 #ifdef SCO_WAV_FILENAME
306 if (!num_samples_to_write) return;
307 num_samples = btstack_min(num_samples, num_samples_to_write);
308 num_samples_to_write -= num_samples;
309 wav_writer_write_int16(num_samples, data);
310 if (num_samples_to_write == 0){
311 wav_writer_close();
312 }
313 #endif /* SCO_WAV_FILENAME */
314
315 #endif /* Demo mode sine or microphone */
316 }
317 #endif /* ENABLE_HFP_WIDE_BAND_SPEECH */
318
319
320 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
321
sco_demo_init_mSBC(void)322 static void sco_demo_init_mSBC(void){
323 printf("SCO Demo: Init mSBC\n");
324
325 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL);
326 hfp_msbc_init();
327
328 #ifdef SCO_WAV_FILENAME
329 num_samples_to_write = MSBC_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS;
330 wav_writer_open(SCO_WAV_FILENAME, 1, MSBC_SAMPLE_RATE);
331 #endif
332
333 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
334 sco_demo_msbc_fill_sine_audio_frame();
335 #endif
336
337 #ifdef SCO_MSBC_IN_FILENAME
338 msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb");
339 printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in);
340 #endif
341
342 #ifdef SCO_MSBC_OUT_FILENAME
343 msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb");
344 printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out);
345 #endif
346
347 audio_initialize(MSBC_SAMPLE_RATE);
348 }
349
sco_demo_receive_mSBC(uint8_t * packet,uint16_t size)350 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){
351 #ifdef HAVE_POSIX_FILE_IO
352 if (num_samples_to_write){
353 if (msbc_file_in){
354 // log incoming mSBC data for testing
355 fwrite(packet+3, size-3, 1, msbc_file_in);
356 }
357 }
358 #endif
359 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3);
360 }
361 #endif
362
sco_demo_init_CVSD(void)363 static void sco_demo_init_CVSD(void){
364 printf("SCO Demo: Init CVSD\n");
365
366 btstack_cvsd_plc_init(&cvsd_plc_state);
367
368 #ifdef SCO_WAV_FILENAME
369 num_samples_to_write = CVSD_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS;
370 wav_writer_open(SCO_WAV_FILENAME, 1, CVSD_SAMPLE_RATE);
371 #endif
372
373 audio_initialize(CVSD_SAMPLE_RATE);
374 }
375
sco_demo_receive_CVSD(uint8_t * packet,uint16_t size)376 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){
377
378 int16_t audio_frame_out[128]; //
379
380 if (size > sizeof(audio_frame_out)){
381 printf("sco_demo_receive_CVSD: SCO packet larger than local output buffer - dropping data.\n");
382 return;
383 }
384
385 const int audio_bytes_read = size - 3;
386 const int num_samples = audio_bytes_read / BYTES_PER_FRAME;
387
388 // convert into host endian
389 int16_t audio_frame_in[128];
390 int i;
391 for (i=0;i<num_samples;i++){
392 audio_frame_in[i] = little_endian_read_16(packet, 3 + i * 2);
393 }
394
395 // treat packet as bad frame if controller does not report 'all good'
396 bool bad_frame = (packet[1] & 0x30) != 0;
397
398 btstack_cvsd_plc_process_data(&cvsd_plc_state, bad_frame, audio_frame_in, num_samples, audio_frame_out);
399
400 #ifdef SCO_WAV_FILENAME
401 // Samples in CVSD SCO packet are in little endian, ready for wav files (take shortcut)
402 const int samples_to_write = btstack_min(num_samples, num_samples_to_write);
403 wav_writer_write_le_int16(samples_to_write, audio_frame_out);
404 num_samples_to_write -= samples_to_write;
405 if (num_samples_to_write == 0){
406 wav_writer_close();
407 }
408 #endif
409
410 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read);
411 }
412
413 #endif
414
415
sco_demo_close(void)416 void sco_demo_close(void){
417 printf("SCO demo close\n");
418
419 printf("SCO demo statistics: ");
420 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
421 if (negotiated_codec == HFP_CODEC_MSBC){
422 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);
423 } else
424 #endif
425 {
426 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);
427 }
428
429 negotiated_codec = -1;
430
431 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE)
432
433 #if defined(SCO_WAV_FILENAME)
434 wav_writer_close();
435 #endif
436
437 audio_terminate();
438
439 #endif
440 }
441
sco_demo_set_codec(uint8_t codec)442 void sco_demo_set_codec(uint8_t codec){
443 if (negotiated_codec == codec) return;
444 negotiated_codec = codec;
445
446 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE)
447 if (negotiated_codec == HFP_CODEC_MSBC){
448 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
449 sco_demo_init_mSBC();
450 #endif
451 } else {
452 sco_demo_init_CVSD();
453 }
454 #endif
455 }
456
sco_demo_init(void)457 void sco_demo_init(void){
458
459 #ifdef ENABLE_CLASSIC_LEGACY_CONNECTIONS_FOR_SCO_DEMOS
460 printf("Disable BR/EDR Secure Connctions due to incompatibilities with SCO connections\n");
461 gap_secure_connections_enable(false);
462 #endif
463
464 // status
465 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE
466 printf("SCO Demo: Sending and receiving audio via btstack_audio.\n");
467 #endif
468 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
469 if (btstack_audio_sink_get_instance()){
470 printf("SCO Demo: Sending sine wave, audio output via btstack_audio.\n");
471 } else {
472 printf("SCO Demo: Sending sine wave, hexdump received data.\n");
473 }
474 #endif
475 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII
476 printf("SCO Demo: Sending ASCII blocks, print received data.\n");
477 #endif
478 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER
479 printf("SCO Demo: Sending counter value, hexdump received data.\n");
480 #endif
481
482 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE)
483 hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD
484 #else
485 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent
486 #endif
487 }
488
sco_demo_send(hci_con_handle_t sco_handle)489 void sco_demo_send(hci_con_handle_t sco_handle){
490
491 if (sco_handle == HCI_CON_HANDLE_INVALID) return;
492
493 int sco_packet_length = hci_get_sco_packet_length();
494 int sco_payload_length = sco_packet_length - 3;
495
496 hci_reserve_packet_buffer();
497 uint8_t * sco_packet = hci_get_outgoing_packet_buffer();
498 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
499 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
500 if (negotiated_codec == HFP_CODEC_MSBC){
501
502 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){
503 log_error("mSBC stream is empty.");
504 }
505 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length);
506 #ifdef HAVE_POSIX_FILE_IO
507 if (msbc_file_out){
508 // log outgoing mSBC data for testing
509 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out);
510 }
511 #endif
512 sco_demo_msbc_fill_sine_audio_frame();
513 } else
514 #endif
515 {
516 const int audio_samples_per_packet = sco_payload_length / BYTES_PER_FRAME;
517 sco_demo_sine_wave_int16_at_8000_hz_little_endian(audio_samples_per_packet, &sco_packet[3]);
518 }
519 #endif
520
521 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE
522
523 if (btstack_audio_source_get_instance()){
524
525 if (negotiated_codec == HFP_CODEC_MSBC){
526 // MSBC
527
528 if (audio_input_paused){
529 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= MSBC_PA_PREBUFFER_BYTES){
530 // resume sending
531 audio_input_paused = 0;
532 }
533 }
534
535 if (!audio_input_paused){
536 int num_samples = hfp_msbc_num_audio_samples_per_frame();
537 if (num_samples > MAX_NUM_MSBC_SAMPLES) return; // assert
538 if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= (unsigned int)(num_samples * BYTES_PER_FRAME)){
539 int16_t sample_buffer[MAX_NUM_MSBC_SAMPLES];
540 uint32_t bytes_read;
541 btstack_ring_buffer_read(&audio_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * BYTES_PER_FRAME, &bytes_read);
542 hfp_msbc_encode_audio_frame(sample_buffer);
543 num_audio_frames++;
544 }
545 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){
546 log_error("mSBC stream should not be empty.");
547 }
548 }
549
550 if (audio_input_paused || hfp_msbc_num_bytes_in_stream() < sco_payload_length){
551 memset(sco_packet + 3, 0, sco_payload_length);
552 audio_input_paused = 1;
553 } else {
554 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length);
555 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
556 #ifdef HAVE_POSIX_FILE_IO
557 if (msbc_file_out){
558 // log outgoing mSBC data for testing
559 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out);
560 }
561 #endif
562 #endif
563 }
564
565 } else {
566 // CVSD
567
568 log_debug("send: bytes avail %u, free %u", btstack_ring_buffer_bytes_available(&audio_input_ring_buffer), btstack_ring_buffer_bytes_free(&audio_input_ring_buffer));
569 // fill with silence while paused
570 int bytes_to_copy = sco_payload_length;
571 if (audio_input_paused){
572 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= CVSD_PA_PREBUFFER_BYTES){
573 // resume sending
574 audio_input_paused = 0;
575 }
576 }
577
578 // get data from ringbuffer
579 uint16_t pos = 0;
580 uint8_t * sample_data = &sco_packet[3];
581 if (!audio_input_paused){
582 uint32_t bytes_read = 0;
583 btstack_ring_buffer_read(&audio_input_ring_buffer, sample_data, bytes_to_copy, &bytes_read);
584 // flip 16 on big endian systems
585 // @note We don't use (uint16_t *) casts since all sample addresses are odd which causes crahses on some systems
586 if (btstack_is_big_endian()){
587 unsigned int i;
588 for (i=0;i<bytes_read;i+=2){
589 uint8_t tmp = sample_data[i*2];
590 sample_data[i*2] = sample_data[i*2+1];
591 sample_data[i*2+1] = tmp;
592 }
593 }
594 bytes_to_copy -= bytes_read;
595 pos += bytes_read;
596 }
597
598 // fill with 0 if not enough
599 if (bytes_to_copy){
600 memset(sample_data + pos, 0, bytes_to_copy);
601 audio_input_paused = 1;
602 }
603 }
604 }
605 else {
606 // just send '0's
607 memset(sco_packet + 3, 0, sco_payload_length);
608 }
609 #endif
610
611 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII
612 // store packet counter-xxxx
613 snprintf((char *)&sco_packet[3], 5, "%04u", phase++);
614 uint8_t ascii = (phase & 0x0f) + 'a';
615 sco_packet[3+4] = '-';
616 memset(&sco_packet[3+5], ascii, sco_payload_length-5);
617 #endif
618 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER
619 int j;
620 for (j=0;j<sco_payload_length;j++){
621 sco_packet[3+j] = phase++;
622 }
623 #endif
624 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55
625 int j;
626 for (j=0;j<sco_payload_length;j++){
627 // sco_packet[3+j] = j & 1 ? 0x35 : 0x53;
628 sco_packet[3+j] = 0x55;
629 }
630 #endif
631 #if SCO_DEMO_MODE == SCO_DEMO_MODE_00
632 int j;
633 for (j=0;j<sco_payload_length;j++){
634 sco_packet[3+j] = 0x00;
635 }
636 // additional hack
637 // big_endian_store_16(sco_packet, 5, phase++);
638 (void) phase;
639 #endif
640
641 // test silence
642 // memset(sco_packet+3, 0, sco_payload_length);
643
644 // set handle + flags
645 little_endian_store_16(sco_packet, 0, sco_handle);
646 // set len
647 sco_packet[2] = sco_payload_length;
648 // finally send packet
649 hci_send_sco_packet_buffer(sco_packet_length);
650
651 // request another send event
652 hci_request_sco_can_send_now_event();
653
654 count_sent++;
655 #if SCO_DEMO_MODE != SCO_DEMO_MODE_55
656 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report();
657 #endif
658 }
659
660 /**
661 * @brief Process received data
662 */
663 #define ANSI_COLOR_RED "\x1b[31m"
664 #define ANSI_COLOR_GREEN "\x1b[32m"
665 #define ANSI_COLOR_YELLOW "\x1b[33m"
666 #define ANSI_COLOR_BLUE "\x1b[34m"
667 #define ANSI_COLOR_MAGENTA "\x1b[35m"
668 #define ANSI_COLOR_CYAN "\x1b[36m"
669 #define ANSI_COLOR_RESET "\x1b[0m"
670
sco_demo_receive(uint8_t * packet,uint16_t size)671 void sco_demo_receive(uint8_t * packet, uint16_t size){
672
673 dump_data = 1;
674
675 count_received++;
676 static uint32_t packets = 0;
677 static uint32_t crc_errors = 0;
678 static uint32_t data_received = 0;
679 static uint32_t byte_errors = 0;
680
681 data_received += size - 3;
682 packets++;
683 if (data_received > 100000){
684 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);
685 crc_errors = 0;
686 byte_errors = 0;
687 data_received = 0;
688 packets = 0;
689 }
690
691 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE)
692 switch (negotiated_codec){
693 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
694 case HFP_CODEC_MSBC:
695 sco_demo_receive_mSBC(packet, size);
696 break;
697 #endif
698 case HFP_CODEC_CVSD:
699 sco_demo_receive_CVSD(packet, size);
700 break;
701 default:
702 break;
703 }
704 dump_data = 0;
705 #endif
706
707 #if 0
708 if (packet[1] & 0x30){
709 crc_errors++;
710 printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4);
711 printf_hexdump(&packet[3], size-3);
712 return;
713 }
714 #endif
715
716 if (dump_data){
717 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII
718 printf("data: ");
719 int i;
720 for (i=3;i<size;i++){
721 printf("%c", packet[i]);
722 }
723 printf("\n");
724 dump_data = 0;
725 #endif
726 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER
727 // colored hexdump with expected
728 static uint8_t expected_byte = 0;
729 int i;
730 printf("data: ");
731 for (i=3;i<size;i++){
732 if (packet[i] != expected_byte){
733 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]);
734 } else {
735 printf("%02x ", packet[i]);
736 }
737 expected_byte = packet[i]+1;
738 }
739 printf("\n");
740 #endif
741 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00
742 int i;
743 int contains_error = 0;
744 for (i=3;i<size;i++){
745 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){
746 contains_error = 1;
747 byte_errors++;
748 }
749 }
750 if (contains_error){
751 printf("data: ");
752 for (i=0;i<3;i++){
753 printf("%02x ", packet[i]);
754 }
755 for (i=3;i<size;i++){
756 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){
757 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]);
758 } else {
759 printf("%02x ", packet[i]);
760 }
761 }
762 printf("\n");
763 }
764 #endif
765 }
766 }
767