From b3f76298ca1ccca314e568b77c5d0fcf231f5b47 Mon Sep 17 00:00:00 2001
From: Milanka Ringwald <mila@ringwald.ch>
Date: Wed, 31 Aug 2016 15:43:29 +0200
Subject: [PATCH] hfp: fix comments, init code

---
 example/hfp_ag_demo.c         | 27 +++++++++++++++++++++++----
 example/sco_demo_util.c       | 12 ++++++++++--
 src/classic/btstack_sbc_plc.c |  7 ++++---
 3 files changed, 37 insertions(+), 9 deletions(-)

diff --git a/example/hfp_ag_demo.c b/example/hfp_ag_demo.c
index 48e606c93..dc3692ee8 100644
--- a/example/hfp_ag_demo.c
+++ b/example/hfp_ag_demo.c
@@ -72,9 +72,10 @@ const char hfp_ag_service_name[] = "BTstack HFP AG Test";
 // CC256x
 // bd_addr_t device_addr = { 0xD0, 0x39, 0x72, 0xCD, 0x83, 0x45};
 // Minijamox
-bd_addr_t device_addr = { 0x00, 0x21, 0x3c, 0xac, 0xf7, 0x38};
+bd_addr_t device_addr = { 0x00, 0x15, 0x83, 0x5F, 0x9D, 0x46};
 
-static uint8_t codecs[] = {HFP_CODEC_CVSD, HFP_CODEC_MSBC};
+// static uint8_t codecs[] = {HFP_CODEC_CVSD, HFP_CODEC_MSBC};
+static uint8_t codecs[] = {HFP_CODEC_CVSD};
 static uint8_t negotiated_codec = HFP_CODEC_CVSD;
 
 static hci_con_handle_t acl_handle = -1;
@@ -125,6 +126,21 @@ int deviceCount = 0;
 enum STATE {INIT, W4_INQUIRY_MODE_COMPLETE, ACTIVE} ;
 enum STATE state = INIT;
 
+static void dump_supported_codecs(){
+    int i;
+    printf("Supported codecs: ");
+    for (i = 0; i < sizeof(codecs); i++){
+        switch(codecs[i]){
+            case HFP_CODEC_CVSD:
+                printf(" CVSD");
+                break;
+            case HFP_CODEC_MSBC:
+                printf(" mSBC");
+                break;
+        }
+    }
+    printf("\n");
+}
 
 static int getDeviceIndexForAddress( bd_addr_t addr){
     int j;
@@ -601,11 +617,14 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t * even
                     acl_handle = hfp_subevent_service_level_connection_established_get_con_handle(event);
                     hfp_subevent_service_level_connection_established_get_bd_addr(event, device_addr);
                     printf("Service level connection established from %s.\n", bd_addr_to_str(device_addr));
+                    
+                    dump_supported_codecs();
                     if (hci_extended_sco_link_supported()){
-                        printf("Supported Codecs: CVSD, mSBC.\n");
+                        printf("eSCO supported by controller.\n");
                     } else {
-                        printf("Supported Codecs: CVSD. mSBC disabled, eSCO not supported by controller).\n");
+                        printf("eSCO not supported by controller.\n");
                     }
+
                    break;
                 case HFP_SUBEVENT_SERVICE_LEVEL_CONNECTION_RELEASED:
                     printf("Service level connection released.\n");
diff --git a/example/sco_demo_util.c b/example/sco_demo_util.c
index fa2b0b532..0b87c3787 100644
--- a/example/sco_demo_util.c
+++ b/example/sco_demo_util.c
@@ -286,6 +286,12 @@ static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){
             sco_demo_close();
         }
         dump_data = 0;
+
+        // convert back
+        int i;
+        for (i=0;i<samples_to_write;i++){
+            packet[3+i] += 128;            
+        }
     }
 }
 
@@ -462,9 +468,11 @@ void sco_demo_receive(uint8_t * packet, uint16_t size){
 #endif
 #endif
 
-    if (packet[1] & 0xf0){
-        printf("SCO CRC Error: %x - data: ", packet[1] >> 4);
+    if (packet[1] & 0x30){
+        printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4);
+        log_info("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4);
         printf_hexdump(&packet[3], size-3);
+
         return;
     }
 
diff --git a/src/classic/btstack_sbc_plc.c b/src/classic/btstack_sbc_plc.c
index 0724965ab..d337e35fa 100644
--- a/src/classic/btstack_sbc_plc.c
+++ b/src/classic/btstack_sbc_plc.c
@@ -75,9 +75,10 @@ void btstack_sbc_plc_init(btstack_sbc_plc_state_t *plc_state){
     int i;
     plc_state->nbf=0;
     plc_state->bestlag=0;
-    for (i=0;i<LHIST+SBCRT;i++){
-        plc_state->hist[i] = 0; 
-    }
+    // for (i=0;i<LHIST+FS+CVSDRT+OLAL;i++){
+    //     plc_state->hist[i] = 0; 
+    // }
+    memset(plc_state->hist,0,sizeof(plc_state->hist));
        
 }