extend HFP AG for using mSBC

example/Makefile.inc

@@ -199,13 +199,13 @@ led_counter: ${CORE_OBJ} ${COMMON_OBJ} led_counter.c
 gap_le_advertisements: ${CORE_OBJ} ${COMMON_OBJ} ad_parser.c gap_le_advertisements.c
 	${CC} $^ ${CFLAGS} ${LDFLAGS} -o $@
 
-hsp_hs_demo: ${CORE_OBJ} ${COMMON_OBJ} ${SDP_CLIENT} ${SBC_DECODER_OBJ} sco_demo_util.o hsp_hs.o hsp_hs_demo.c
+hsp_hs_demo: ${CORE_OBJ} ${COMMON_OBJ} ${SDP_CLIENT} ${SBC_DECODER_OBJ} ${SBC_ENCODER_OBJ} sco_demo_util.o hsp_hs.o hsp_hs_demo.c
 	${CC} $^ ${CFLAGS} ${LDFLAGS} -o $@
 
-hsp_ag_demo: ${CORE_OBJ} ${COMMON_OBJ} ${SDP_CLIENT} ${SBC_DECODER_OBJ} sco_demo_util.o hsp_ag.o hsp_ag_demo.c
+hsp_ag_demo: ${CORE_OBJ} ${COMMON_OBJ} ${SDP_CLIENT} ${SBC_DECODER_OBJ} ${SBC_ENCODER_OBJ} sco_demo_util.o hsp_ag.o hsp_ag_demo.c
 	${CC} $^ ${CFLAGS} ${LDFLAGS} -o $@
 
-hfp_ag_demo: ${CORE_OBJ} ${COMMON_OBJ} ${SDP_CLIENT} ${SBC_DECODER_OBJ} sco_demo_util.o hfp.o hfp_gsm_model.o hfp_ag.o hfp_ag_demo.c
+hfp_ag_demo: ${CORE_OBJ} ${COMMON_OBJ} ${SDP_CLIENT} ${SBC_DECODER_OBJ} ${SBC_ENCODER_OBJ} sco_demo_util.o hfp.o hfp_gsm_model.o hfp_ag.o hfp_ag_demo.c
 	${CC} $^ ${CFLAGS} ${LDFLAGS} -o $@
 
 hfp_hf_demo: ${CORE_OBJ} ${COMMON_OBJ} ${SDP_CLIENT} ${SBC_DECODER_OBJ} ${SBC_ENCODER_OBJ} sco_demo_util.o hfp.o hfp_hf.o hfp_hf_demo.c

example/hfp_ag_demo.c

@@ -72,7 +72,9 @@ const char hfp_ag_service_name[] = "BTstack HFP AG Test";
 // CC256x
 bd_addr_t device_addr = { 0xD0, 0x39, 0x72, 0xCD, 0x83, 0x45};
 
-static uint8_t codecs[1] = {HFP_CODEC_CVSD};
+static uint8_t codecs[] = {HFP_CODEC_CVSD, HFP_CODEC_MSBC};
+static uint8_t negotiated_codec = HFP_CODEC_CVSD;
+
 static uint16_t handle = -1;
 static hci_con_handle_t sco_handle;
 static int memory_1_enabled = 1;
@@ -584,6 +586,11 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t * even
             }
 
             switch (event[2]) {   
+                case HFP_SUBEVENT_CODECS_CONNECTION_COMPLETE:
+                    negotiated_codec = hfp_subevent_codecs_connection_complete_get_negotiated_codec(event);
+                    printf("Codec connection established with codec 0x%02x.\n", negotiated_codec);
+                    sco_demo_set_codec(negotiated_codec);
+                    break;
                 case HFP_SUBEVENT_SERVICE_LEVEL_CONNECTION_ESTABLISHED:
                     handle = hfp_subevent_service_level_connection_established_get_con_handle(event);
                     printf("Service level connection established.\n");

example/sco_demo_util.c

@@ -45,6 +45,9 @@
 #include "sco_demo_util.h"
 #include "btstack_debug.h"
 #include "btstack_sbc_decoder.h"
+#include "btstack_sbc_encoder.h"
+#include "hfp_msbc.h"
+#include "hfp.h"
 
 // configure test mode
 #define SCO_DEMO_MODE_SINE		0
@@ -58,6 +61,8 @@
 
 #ifdef HAVE_POSIX_FILE_IO
 #define SCO_WAV_FILENAME "sco_input.wav"
+#define SCO_MSBC_FILENAME "sco_output.msbc"
+
 #define SCO_WAV_DURATION_IN_SECONDS 30
 #endif
 
@@ -85,10 +90,11 @@ typedef struct wav_writer_state {
 
 static int dump_data = 1;
 
-static int phase;
+static int phase = 0;
 static int count_sent = 0;
 static int count_received = 0;
-static uint8_t negotiated_codec = 0; // CVSD
+static uint8_t negotiated_codec = HFP_CODEC_CVSD; 
+static int num_audio_frames = 0;
 
 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
 // input signal: pre-computed sine wave, 160 Hz
@@ -180,6 +186,25 @@ static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, i
     }
 }
 
+static void sco_demo_generate_sine(int16_t * pcm_samples){
+    int i;
+    for (i=0; i < hfp_msbc_num_audio_samples_per_frame(); i++){
+        uint8_t buf[2];
+        buf[0] = sine[phase++];
+        if (phase >= sizeof(sine)) phase = 0;
+        buf[1] = sine[phase++];
+        if (phase >= sizeof(sine)) phase = 0;
+        pcm_samples[i] = little_endian_read_16(buf, 0);  
+    }
+}
+
+static void sco_demo_fill_audio_frame(void){
+    if (!hfp_msbc_can_encode_audio_frame_now()) return;
+    int16_t read_buffer[8*16*2];
+    sco_demo_generate_sine(read_buffer);
+    hfp_msbc_encode_audio_frame(read_buffer);
+    num_audio_frames++;
+}
 
 static void sco_demo_init_mSBC(void){
     wav_writer_state.wav_file = wav_init(SCO_WAV_FILENAME);
@@ -195,6 +220,10 @@ static void sco_demo_init_mSBC(void){
     num_samples_to_write = num_samples;
     
     write_wav_header(wav_writer_state.wav_file, sample_rate, num_channels, num_samples, bytes_per_sample);
+
+    wav_init(SCO_MSBC_FILENAME);
+    hfp_msbc_init();
+    sco_demo_fill_audio_frame();
 }
 
 static void sco_demo_init_CVSD(void){
@@ -247,7 +276,7 @@ void sco_demo_close(void){
     
 #if 0
     printf("SCO Demo: closing wav file\n");
-    if (negotiated_codec == 0x02){
+    if (negotiated_codec == HFP_CODEC_MSBC){
         wav_writer_state_t * writer_state = (wav_writer_state_t*) decoder_state.context;
         if (!writer_state->wav_file) return;
         rewind(writer_state->wav_file);
@@ -265,12 +294,20 @@ void sco_demo_set_codec(uint8_t codec){
     negotiated_codec = codec;
 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
 #ifdef SCO_WAV_FILENAME
-    if (negotiated_codec == 0x02){
+    if (negotiated_codec == HFP_CODEC_MSBC){
         sco_demo_init_mSBC();
     } else {
         sco_demo_init_CVSD();
     }
 #endif
+#ifdef SCO_SBC_FILENAME
+    if (negotiated_codec == HFP_CODEC_MSBC){
+        sco_demo_init_mSBC();
+    } else {
+        sco_demo_init_CVSD();
+    }
+#endif
+
 #endif
 }
 
@@ -346,26 +383,36 @@ void sco_demo_send(hci_con_handle_t sco_handle){
     little_endian_store_16(sco_packet, 0, sco_handle);
     // set len
     sco_packet[2] = sco_payload_length;
-    const int frames_per_packet = sco_payload_length;    // for 8-bit data. for 16-bit data it's /2
+    const int audio_samples_per_packet = sco_payload_length;    // for 8-bit data. for 16-bit data it's /2
 
 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
-    int i;
+    if (negotiated_codec == HFP_CODEC_MSBC){
-    for (i=0;i<frames_per_packet;i++){
+
-        sco_packet[3+i] = sine[phase];
+        if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){
-        phase++;
+            log_error("mSBC stream is empty.");
-        if (phase >= sizeof(sine)) phase = 0;
+        }
+        hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length);
+        sco_demo_fill_audio_frame();
+    } else {
+        int i;
+        for (i=0;i<audio_samples_per_packet;i++){
+            sco_packet[3+i] = sine[phase];
+            phase++;
+            if (phase >= sizeof(sine)) phase = 0;
+        }
     }
 #else
 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII
-    memset(&sco_packet[3], phase++, frames_per_packet);
+    memset(&sco_packet[3], phase++, audio_samples_per_packet);
     if (phase > 'z') phase = 'a';
 #else
     int j;
-    for (j=0;j<frames_per_packet;j++){
+    for (j=0;j<audio_samples_per_packet;j++){
         sco_packet[3+j] = phase++;
     }
 #endif
 #endif
+
     hci_send_sco_packet_buffer(sco_packet_length);
 
     // request another send event
@@ -390,7 +437,7 @@ void sco_demo_receive(uint8_t * packet, uint16_t size){
 
 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
 #ifdef SCO_WAV_FILENAME
-    if (negotiated_codec == 0x02){
+    if (negotiated_codec == HFP_CODEC_MSBC){
         sco_demo_receive_mSBC(packet, size);
     } else {
         sco_demo_receive_CVSD(packet, size);

src/classic/btstack_sbc_decoder.h

@@ -44,17 +44,13 @@
 #define __SBC_DECODER_H
 
 #include <stdint.h>
+#include "btstack_sbc.h"
 #include "btstack_sbc_plc.h"
 
 #if defined __cplusplus
 extern "C" {
 #endif
 
-typedef enum{
-    SBC_MODE_STANDARD,
-    SBC_MODE_mSBC
-} sbc_mode_t;
-
 typedef struct {
     void * context;
     void (*handle_pcm_data)(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context);

test/sbc/sbc_encoder_test.c

@@ -77,7 +77,7 @@ static ssize_t __read(int fd, void *buf, size_t count){
 static int read_audio_frame(int wav_fd){
     int i;
     int bytes_read = 0;  
-    for (i=0; i < hfp_msbc_num_audio_samples_per_frame() * 2/2; i++){
+    for (i=0; i < hfp_msbc_num_audio_samples_per_frame(); i++){
         uint8_t buf[2];
         bytes_read +=__read(wav_fd, &buf, 2);
         read_buffer[i] = little_endian_read_16(buf, 0);