From f55ac442b001ffcde8d0aaa53f5b3c3a90f2319e Mon Sep 17 00:00:00 2001
From: Matthias Ringwald <matthias@ringwald.ch>
Date: Tue, 26 Feb 2019 18:35:47 +0100
Subject: [PATCH] example/sco_demo_util: ENABLE_SCO_STEREO_PLAYBACK uses stereo
 audio sink

---
 example/sco_demo_util.c | 44 +++++++++++++++++++++++++++++++++--------
 1 file changed, 36 insertions(+), 8 deletions(-)

diff --git a/example/sco_demo_util.c b/example/sco_demo_util.c
index 44ec41fe7..c5e440574 100644
--- a/example/sco_demo_util.c
+++ b/example/sco_demo_util.c
@@ -71,6 +71,8 @@
 // number of sco packets until 'report' on console
 #define SCO_REPORT_PERIOD           100
 
+// #define ENABLE_SCO_STEREO_PLAYBACK
+
 #ifdef HAVE_POSIX_FILE_IO
 // length and name of wav file on disk
 #define SCO_WAV_DURATION_IN_SECONDS 15
@@ -187,8 +189,6 @@ static void sco_demo_msbc_fill_sine_audio_frame(void){
 
 static void playback_callback(int16_t * buffer, uint16_t num_samples){
 
-    // config based on codec
-    int bytes_to_copy   = num_samples * BYTES_PER_FRAME;
     uint32_t prebuffer_bytes;
     switch (negotiated_codec){
         case HFP_CODEC_MSBC:
@@ -203,8 +203,12 @@ static void playback_callback(int16_t * buffer, uint16_t num_samples){
     // fill with silence while paused
     if (audio_output_paused){
         if (btstack_ring_buffer_bytes_available(&audio_output_ring_buffer) < prebuffer_bytes){
-            memset(buffer, 0, bytes_to_copy);
-            return;
+#ifdef ENABLE_SCO_STEREO_PLAYBACK
+            memset(buffer, 0, num_samples * BYTES_PER_FRAME * 2);
+#else
+            memset(buffer, 0, num_samples * BYTES_PER_FRAME);
+#endif 
+           return;
         } else {
             // resume playback
             audio_output_paused = 0;
@@ -213,12 +217,32 @@ static void playback_callback(int16_t * buffer, uint16_t num_samples){
 
     // get data from ringbuffer
     uint32_t bytes_read = 0;
-    btstack_ring_buffer_read(&audio_output_ring_buffer, (uint8_t *) buffer, bytes_to_copy, &bytes_read);
-    bytes_to_copy -= bytes_read;
+#ifdef ENABLE_SCO_STEREO_PLAYBACK
+    while (num_samples){
+        int16_t temp[16];
+        unsigned int bytes_to_read = btstack_min(num_samples * BYTES_PER_FRAME, sizeof(temp));
+        btstack_ring_buffer_read(&audio_output_ring_buffer, (uint8_t *) &temp[0], bytes_to_read, &bytes_read);
+        if (bytes_read == 0) break;
+        unsigned int i;
+        for (i=0;i<bytes_read / BYTES_PER_FRAME;i++){
+            *buffer++ = temp[i];
+            *buffer++ = temp[i];
+            num_samples--;
+        }
+    }
+#else
+    btstack_ring_buffer_read(&audio_output_ring_buffer, (uint8_t *) buffer, num_samples * BYTES_PER_FRAME, &bytes_read);
+    num_samples -= bytes_read / BYTES_PER_FRAME;
+    buffer      += bytes_read / BYTES_PER_FRAME;
+#endif
 
     // fill with 0 if not enough
-    if (bytes_to_copy){
-        memset(buffer + bytes_read, 0, bytes_to_copy);
+    if (num_samples){
+#ifdef ENABLE_SCO_STEREO_PLAYBACK
+        memset(buffer, 0, num_samples * BYTES_PER_FRAME * 2);
+#else
+        memset(buffer, 0, num_samples * BYTES_PER_FRAME);
+#endif
         audio_output_paused = 1;
     }
 }
@@ -242,7 +266,11 @@ static int audio_initialize(int sample_rate){
     const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance();
     if (!audio_sink) return 0;
 
+#ifdef ENABLE_SCO_STEREO_PLAYBACK
+    audio_sink->init(2, sample_rate, &playback_callback);
+#else
     audio_sink->init(1, sample_rate, &playback_callback);
+#endif
     audio_sink->start_stream();
 
     audio_output_paused  = 1;