From afac2a041bab5147bbcba64b04501e36db33a8b6 Mon Sep 17 00:00:00 2001
From: Milanka Ringwald <mila@ringwald.ch>
Date: Tue, 2 Oct 2018 10:04:32 +0200
Subject: [PATCH] hfp_ag: refactor hfp_init_link_settings

---
 src/classic/hfp.c    | 22 ++++++++++++++++++++++
 src/classic/hfp.h    |  1 +
 src/classic/hfp_ag.c | 28 +++++-----------------------
 3 files changed, 28 insertions(+), 23 deletions(-)

diff --git a/src/classic/hfp.c b/src/classic/hfp.c
index 6df55986a..67e632837 100644
--- a/src/classic/hfp.c
+++ b/src/classic/hfp.c
@@ -1524,6 +1524,28 @@ void hfp_set_hf_run_for_context(void (*callback)(hfp_connection_t * hfp_connecti
 void hfp_init(void){
 }
 
+void hfp_init_link_settings(hfp_connection_t * hfp_connection, uint8_t esco_s4_supported){
+    // determine highest possible link setting
+    hfp_connection->link_setting = HFP_LINK_SETTINGS_D1;
+    // anything else requires eSCO support on both sides
+    if (hci_extended_sco_link_supported() && hci_remote_esco_supported(hfp_connection->acl_handle)){
+        switch (hfp_connection->negotiated_codec){
+            case HFP_CODEC_CVSD:
+                hfp_connection->link_setting = HFP_LINK_SETTINGS_S3;
+                if (esco_s4_supported){
+                    hfp_connection->link_setting = HFP_LINK_SETTINGS_S4;
+                }
+                break;
+            case HFP_CODEC_MSBC:
+                hfp_connection->link_setting = HFP_LINK_SETTINGS_T2;
+                break;
+            default:
+                break;
+        }
+    }
+    log_info("hfp_init_link_settings: %u", hfp_connection->link_setting);
+}
+
 #define HFP_HF_RX_DEBUG_PRINT_LINE 80
 
 void hfp_log_rfcomm_message(const char * tag, uint8_t * packet, uint16_t size){
diff --git a/src/classic/hfp.h b/src/classic/hfp.h
index 656bad0b1..e5764b4c5 100644
--- a/src/classic/hfp.h
+++ b/src/classic/hfp.h
@@ -676,6 +676,7 @@ void hfp_release_audio_connection(hfp_connection_t * connection);
 void hfp_setup_synchronous_connection(hfp_connection_t * connection);
 int hfp_supports_codec(uint8_t codec, int codecs_nr, uint8_t * codecs);
 void hfp_hf_drop_mSBC_if_eSCO_not_supported(uint8_t * codecs, uint8_t * codecs_nr);
+void hfp_init_link_settings(hfp_connection_t * hfp_connection, uint8_t esco_s4_supported);
 
 const char * hfp_hf_feature(int index);
 const char * hfp_ag_feature(int index);
diff --git a/src/classic/hfp_ag.c b/src/classic/hfp_ag.c
index 999545148..9d9b8ebb2 100644
--- a/src/classic/hfp_ag.c
+++ b/src/classic/hfp_ag.c
@@ -516,27 +516,8 @@ static uint8_t hfp_ag_suggest_codec(hfp_connection_t *hfp_connection){
     return HFP_CODEC_CVSD;
 }
 
-static void hfp_init_link_settings(hfp_connection_t * hfp_connection){
-    // determine highest possible link setting
-    hfp_connection->link_setting = HFP_LINK_SETTINGS_D1;
-    // anything else requires eSCO support on both sides
-    if (hci_extended_sco_link_supported() && hci_remote_esco_supported(hfp_connection->acl_handle)){
-        switch (hfp_connection->negotiated_codec){
-            case HFP_CODEC_CVSD:
-                hfp_connection->link_setting = HFP_LINK_SETTINGS_S3;
-                if ((hfp_connection->remote_supported_features & (1<<HFP_HFSF_ESCO_S4))
-                &&  (hfp_supported_features                    & (1<<HFP_AGSF_ESCO_S4))){
-                    hfp_connection->link_setting = HFP_LINK_SETTINGS_S4;
-                }
-                break;
-            case HFP_CODEC_MSBC:
-                hfp_connection->link_setting = HFP_LINK_SETTINGS_T2;
-                break;
-            default:
-                break;
-        }
-    }
-    log_info("hfp_init_link_settings: %u", hfp_connection->link_setting);
+static uint8_t hfp_ag_esco_s4_supported(hfp_connection_t * hfp_connection){
+    return (hfp_connection->remote_supported_features & (1<<HFP_HFSF_ESCO_S4)) &&  (hfp_supported_features  & (1<<HFP_AGSF_ESCO_S4));
 }
 
 static int codecs_exchange_state_machine(hfp_connection_t * hfp_connection){
@@ -602,7 +583,8 @@ static int codecs_exchange_state_machine(hfp_connection_t * hfp_connection){
             log_info("hfp: codec confirmed: %s", hfp_connection->negotiated_codec == HFP_CODEC_MSBC ? "mSBC" : "CVSD");
             hfp_ag_send_ok(hfp_connection->rfcomm_cid);           
             // now, pick link settings
-            hfp_init_link_settings(hfp_connection);
+
+            hfp_init_link_settings(hfp_connection, hfp_ag_esco_s4_supported(hfp_connection));
             return 1; 
         default:
             break;
@@ -2170,7 +2152,7 @@ static void hfp_ag_setup_audio_connection(hfp_connection_t * hfp_connection){
         hfp_connection->negotiated_codec = HFP_CODEC_CVSD;
         hfp_connection->codecs_state = HFP_CODECS_EXCHANGED;
         // now, pick link settings
-        hfp_init_link_settings(hfp_connection);
+        hfp_init_link_settings(hfp_connection, hfp_ag_esco_s4_supported(hfp_connection));
         return;
     }