diff --git a/src/hfp.c b/src/hfp.c
index 83f2f4c4f..89e11c84d 100644
--- a/src/hfp.c
+++ b/src/hfp.c
@@ -134,9 +134,9 @@ int send_str_over_rfcomm(uint16_t cid, char * command){
     if (!rfcomm_can_send_packet_now(cid)) return 1;
     int err = rfcomm_send_internal(cid, (uint8_t*) command, strlen(command));
     if (err){
-        printf("rfcomm_send_internal -> error 0x%02x \n", err);
+        log_error("rfcomm_send_internal -> error 0x%02x \n", err);
     } 
-    return err;
+    return 1;
 }
 
 void hfp_set_codec(hfp_connection_t * context, uint8_t *packet, uint16_t size){
@@ -274,11 +274,13 @@ void hfp_reset_context_flags(hfp_connection_t * context){
     context->notify_ag_on_new_codecs = 0;
     
     // establish codecs connection
-    context->trigger_codec_connection_setup = 0;
+    context->ag_trigger_codec_connection_setup = 0;
+    context->hf_trigger_codec_connection_setup = 0;
     context->suggested_codec = 0;
-    
+    context->negotiated_codec = 0;
+    context->codec_confirmed = 0;
+
     context->establish_audio_connection = 0; 
-    
 }
 
 static hfp_connection_t * create_hfp_connection_context(){
@@ -684,20 +686,25 @@ void process_command(hfp_connection_t * context){
     } 
 
     if (isHandsFree && strncmp((char *)context->line_buffer+offset, HFP_EXTENDED_AUDIO_GATEWAY_ERROR, strlen(HFP_EXTENDED_AUDIO_GATEWAY_ERROR)) == 0){
-        printf(" process command 1 %s \n", context->line_buffer);
         context->command = HFP_CMD_EXTENDED_AUDIO_GATEWAY_ERROR;
         return;
     }
 
     if (!isHandsFree && strncmp((char *)context->line_buffer+offset, HFP_ENABLE_EXTENDED_AUDIO_GATEWAY_ERROR, strlen(HFP_ENABLE_EXTENDED_AUDIO_GATEWAY_ERROR)) == 0){
-        printf(" process command 2 %s \n", context->line_buffer);
         context->command = HFP_CMD_ENABLE_EXTENDED_AUDIO_GATEWAY_ERROR;
         return;
     }
 
     if (strncmp((char *)context->line_buffer+offset, HFP_TRIGGER_CODEC_CONNECTION_SETUP, strlen(HFP_TRIGGER_CODEC_CONNECTION_SETUP)) == 0){
         context->command = HFP_CMD_TRIGGER_CODEC_CONNECTION_SETUP;
-        context->trigger_codec_connection_setup = 1;
+        // printf("HFP_CMD_TRIGGER_CODEC_CONNECTION_SETUP update command\n");
+        if (isHandsFree){
+            context->hf_trigger_codec_connection_setup = 1;
+            printf("TRIGGER_CODEC_CONNECTION_SETUP hf_trigger_codec_connection_setup = 1\n");
+        } else {
+            context->hf_trigger_codec_connection_setup = 1;
+            printf("TRIGGER_CODEC_CONNECTION_SETUP hf_trigger_codec_connection_setup = 1\n");
+        }
         return;
     } 
 
@@ -835,9 +842,11 @@ void hfp_parse(hfp_connection_t * context, uint8_t byte){
             switch (context->command){
                 case HFP_CMD_HF_CONFIRMED_CODEC:
                     context->codec_confirmed = atoi((char*)context->line_buffer);
+                    printf("hfp parse HFP_CMD_HF_CONFIRMED_CODEC %d\n", context->codec_confirmed);
                     break;
                 case HFP_CMD_AG_SUGGESTED_CODEC:
                     context->suggested_codec = atoi((char*)context->line_buffer);
+                    printf("hfp parse HFP_CMD_AG_SUGGESTED_CODEC %d\n", context->suggested_codec);
                     break;
                 case HFP_CMD_SUPPORTED_FEATURES:
                     context->remote_supported_features = atoi((char*)context->line_buffer);
@@ -1056,32 +1065,6 @@ void hfp_release_service_level_connection(hfp_connection_t * context){
     return;
 }
 
-void hfp_negotiate_codecs(hfp_connection_t * connection){
-    if (!connection){
-        log_error("HFP HF: connection doesn't exist.");
-        return;
-    }
-    
-    if (connection->state >= HFP_W2_DISCONNECT_SCO) return;
-    
-    if (connection->state != HFP_SLE_W4_EXCHANGE_COMMON_CODEC){
-        connection->trigger_codec_connection_setup = 1;
-    }
-}
-
-
-void hfp_establish_audio_connection(hfp_connection_t * connection){
-    connection->establish_audio_connection = 0;
-    if (connection->state == HFP_AUDIO_CONNECTION_ESTABLISHED) return;
-    if (connection->state >= HFP_W2_DISCONNECT_SCO) return;
-    
-    connection->establish_audio_connection = 1;
-    if (connection->state < HFP_SLE_W4_EXCHANGE_COMMON_CODEC){
-        connection->trigger_codec_connection_setup = 1;
-    }
-}
-
-
 void hfp_release_audio_connection(hfp_connection_t * connection){
     if (!connection) return;
     if (connection->state >= HFP_W2_DISCONNECT_SCO) return;
diff --git a/src/hfp.h b/src/hfp.h
index 2f7f54256..50f09b787 100644
--- a/src/hfp.h
+++ b/src/hfp.h
@@ -361,7 +361,8 @@ typedef struct hfp_connection {
     uint8_t notify_ag_on_new_codecs;
     
     // establish codecs connection
-    uint8_t trigger_codec_connection_setup;
+    uint8_t hf_trigger_codec_connection_setup;
+    uint8_t ag_trigger_codec_connection_setup;
     uint8_t ag_ready_for_codecs_connection_setup;
     uint8_t suggested_codec;
     uint8_t codec_confirmed;
@@ -393,8 +394,6 @@ void hfp_establish_service_level_connection(bd_addr_t bd_addr, uint16_t service_
 void hfp_release_service_level_connection(hfp_connection_t * connection);
 void hfp_reset_context_flags(hfp_connection_t * context);
 
-void hfp_negotiate_codecs(hfp_connection_t * context);
-void hfp_establish_audio_connection(hfp_connection_t * context);
 void hfp_release_audio_connection(hfp_connection_t * context);
 
 const char * hfp_hf_feature(int index);
diff --git a/src/hfp_ag.c b/src/hfp_ag.c
index 5cdf1b71d..29321773a 100644
--- a/src/hfp_ag.c
+++ b/src/hfp_ag.c
@@ -163,7 +163,6 @@ void hfp_ag_create_sdp_record(uint8_t * service, int rfcomm_channel_nr, const ch
 int hfp_ag_exchange_supported_features_cmd(uint16_t cid){
     char buffer[40];
     sprintf(buffer, "\r\n%s:%d\r\n\r\nOK\r\n", HFP_SUPPORTED_FEATURES, hfp_supported_features);
-    // printf("exchange_supported_features %s\n", buffer);
     return send_str_over_rfcomm(cid, buffer);
 }
 
@@ -263,7 +262,6 @@ int hfp_ag_retrieve_indicators_cmd(uint16_t cid, hfp_connection_t * context){
     offset += hfp_ag_indicators_join(buffer+offset, sizeof(buffer)-offset, context);
     
     buffer[offset] = 0;
-    printf("hfp_ag_retrieve_indicators_cmd send %s\n", buffer+2);
     
     offset += snprintf(buffer+offset, sizeof(buffer)-offset, "\r\n\r\nOK\r\n");
     buffer[offset] = 0;
@@ -276,7 +274,6 @@ int hfp_ag_retrieve_indicators_status_cmd(uint16_t cid){
     offset += hfp_ag_indicators_status_join(buffer+offset, sizeof(buffer)-offset);
     
     buffer[offset] = 0;
-    printf("send %s\n", buffer+2);
     
     offset += snprintf(buffer+offset, sizeof(buffer)-offset, "\r\n\r\nOK\r\n");
     buffer[offset] = 0;
@@ -295,7 +292,6 @@ int hfp_ag_retrieve_can_hold_call_cmd(uint16_t cid){
     offset += hfp_ag_call_services_join(buffer+offset, sizeof(buffer)-offset);
     
     buffer[offset] = 0;
-    printf("send %s\n", buffer+2);
     
     offset += snprintf(buffer+offset, sizeof(buffer)-offset, "\r\n\r\nOK\r\n");
     buffer[offset] = 0;
@@ -313,7 +309,6 @@ int hfp_ag_retrieve_supported_generic_status_indicators_cmd(uint16_t cid){
     offset += hfp_hf_indicators_join(buffer+offset, sizeof(buffer)-offset);
     
     buffer[offset] = 0;
-    printf("send %s\n", buffer+2);
     
     offset += snprintf(buffer+offset, sizeof(buffer)-offset, "\r\n\r\nOK\r\n");
     buffer[offset] = 0;
@@ -325,8 +320,6 @@ int hfp_ag_retrieve_initital_supported_generic_status_indicators_cmd(uint16_t ci
     int offset = hfp_hf_indicators_initial_status_join(buffer, sizeof(buffer));
     
     buffer[offset] = 0;
-    printf("send %s\n", buffer+2);
-    
     offset += snprintf(buffer+offset, sizeof(buffer)-offset, "\r\nOK\r\n");
     buffer[offset] = 0;
     return send_str_over_rfcomm(cid, buffer);
@@ -351,7 +344,7 @@ int hfp_ag_report_network_operator_name_cmd(uint16_t cid, hfp_network_opearator_
 
 int hfp_ag_cmd_suggest_codec(uint16_t cid, uint8_t codec){
     char buffer[30];
-    sprintf(buffer, "\r\nOK\r\n%s=%d\r\n", HFP_CONFIRM_COMMON_CODEC, codec);
+    sprintf(buffer, "\r\n%s:%d\r\n", HFP_CONFIRM_COMMON_CODEC, codec);
     return send_str_over_rfcomm(cid, buffer);
 }
 
@@ -369,7 +362,11 @@ static uint8_t hfp_ag_suggest_codec(hfp_connection_t *context){
     return codec;
 }
 
+
 static void hfp_ag_run_for_context_service_level_connection(hfp_connection_t * context){
+    if (context->state > HFP_CODECS_CONNECTION_ESTABLISHED) return;
+    printf(" AG run for context_service_level_connection \n");
+
     switch(context->command){
         case HFP_CMD_SUPPORTED_FEATURES:
             switch(context->state){
@@ -490,27 +487,15 @@ static void hfp_ag_run_for_context_service_level_connection(hfp_connection_t * c
                 break;
             }
             break;
-        case HFP_CMD_NONE:
-            switch(context->state){
-                case HFP_W2_DISCONNECT_RFCOMM:
-                    // printf("rfcomm_disconnect_internal cid 0x%02x\n", context->rfcomm_cid);
-                    context->state = HFP_W4_RFCOMM_DISCONNECTED;
-                    rfcomm_disconnect_internal(context->rfcomm_cid);
-                    break;
-                default:
-                    printf("Unhandled command, send default ERROR\n");
-                    hfp_ag_error(context->rfcomm_cid);
-                    break;
-            }
-            break;
         default:
-            printf("default: hfp_ag_run_for_context_service_level_connection \n");
             break;
     }
 }
 
 static void hfp_ag_run_for_context_service_level_connection_queries(hfp_connection_t * context){
-    if (context->state < HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED) return;
+    if (context->state != HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED) return;
+    
+    printf(" AG run for service_level_connection_queries \n");
     if (context->enable_status_update_for_ag_indicators){
         int i;
         for (i = 0; i < context->ag_indicators_nr; i++){
@@ -529,56 +514,83 @@ static void hfp_ag_run_for_context_service_level_connection_queries(hfp_connecti
             return;
         }
     }
+
+
 }
 
 static void hfp_ag_run_for_context_codecs_connection(hfp_connection_t * context){
     if (context->state < HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED) return;
+    if (context->state > HFP_CODECS_CONNECTION_ESTABLISHED) return;
+    printf(" AG run for context_codecs_connection \n");
     switch (context->state){
         case HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED:
-            if (context->notify_ag_on_new_codecs){
+            if (context->notify_ag_on_new_codecs){ // received BAC
+                printf(" received BAC \n");
                 context->notify_ag_on_new_codecs = 0;
-                hfp_ag_ok(context->rfcomm_cid);
-                if (!context->negotiated_codec) return;
-                if (hfp_ag_suggest_codec(context) == context->negotiated_codec) return;
                 context->suggested_codec = hfp_ag_suggest_codec(context);
-                context->trigger_codec_connection_setup = 1;
-                context->state = HFP_SLE_W2_EXCHANGE_COMMON_CODEC;
-                return;
-            }
-        case HFP_SLE_W2_EXCHANGE_COMMON_CODEC:
-            if (context->notify_ag_on_new_codecs){
-                context->notify_ag_on_new_codecs = 0;
                 hfp_ag_ok(context->rfcomm_cid);
-                if (!context->negotiated_codec) return;
-                if (hfp_ag_suggest_codec(context) == context->negotiated_codec) return;
-                context->suggested_codec = hfp_ag_suggest_codec(context);
-                context->trigger_codec_connection_setup = 1;
-                context->state = HFP_SLE_W2_EXCHANGE_COMMON_CODEC;
                 return;
             }
 
-            if (context->trigger_codec_connection_setup){
-                context->trigger_codec_connection_setup = 0;
-                hfp_ag_cmd_suggest_codec(context->rfcomm_cid, context->suggested_codec);
-                context->state = HFP_SLE_W4_EXCHANGE_COMMON_CODEC;
+            if (context->hf_trigger_codec_connection_setup){ // received BCC
+                printf(" received BCC \n");
+                context->hf_trigger_codec_connection_setup = 0;
+                context->ag_trigger_codec_connection_setup = 1;
+                hfp_ag_ok(context->rfcomm_cid);
+                return;
             }
+              
+            if (context->ag_trigger_codec_connection_setup){ // received BCS
+                printf(" send BCS \n");
+                context->ag_trigger_codec_connection_setup = 0;
+                context->state = HFP_SLE_W4_EXCHANGE_COMMON_CODEC;
+                context->suggested_codec = hfp_ag_suggest_codec(context);
+                hfp_ag_cmd_suggest_codec(context->rfcomm_cid, context->suggested_codec);
+                return;
+            }
+          
             break;
         case HFP_SLE_W4_EXCHANGE_COMMON_CODEC:
-            if (context->codec_confirmed){
-                // TODO check if they are equal?
-                if (context->codec_confirmed == context->suggested_codec){
-                    context->negotiated_codec = context->codec_confirmed;
-                    hfp_ag_ok(context->rfcomm_cid);
-                    context->state = HFP_CODECS_CONNECTION_ESTABLISHED;
-                    return;
-                } else {
-                    hfp_ag_error(context->rfcomm_cid);
-                    context->state = HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED;
+            if (context->notify_ag_on_new_codecs){ // received BAC
+                context->notify_ag_on_new_codecs = 0;
+                if (context->suggested_codec != hfp_ag_suggest_codec(context)){
+                    context->suggested_codec = hfp_ag_suggest_codec(context);
+                    context->state = HFP_SLE_W4_EXCHANGE_COMMON_CODEC;
                 }
-                context->codec_confirmed = 0;
-                context->suggested_codec = 0;
-                break;
+                hfp_ag_ok(context->rfcomm_cid);
+                return;
             }
+            
+            if (context->codec_confirmed != context->suggested_codec){
+                context->state = HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED;
+                hfp_ag_error(context->rfcomm_cid);
+                return;
+            } 
+            context->negotiated_codec = context->codec_confirmed;
+            context->state = HFP_CODECS_CONNECTION_ESTABLISHED;
+            hfp_ag_ok(context->rfcomm_cid);
+            hfp_emit_event(hfp_callback, HFP_SUBEVENT_CODECS_CONNECTION_COMPLETE, 0);
+            return;
+        
+        case HFP_CODECS_CONNECTION_ESTABLISHED:
+            if (context->notify_ag_on_new_codecs){ // received BAC
+                context->notify_ag_on_new_codecs = 0;
+                if (context->suggested_codec != hfp_ag_suggest_codec(context)){
+                    context->suggested_codec = hfp_ag_suggest_codec(context);
+                    context->state = HFP_SLE_W4_EXCHANGE_COMMON_CODEC;
+                }
+                hfp_ag_ok(context->rfcomm_cid);
+                return;
+            }
+            if (context->ag_trigger_codec_connection_setup){ 
+                context->ag_trigger_codec_connection_setup = 0;
+                if (context->negotiated_codec != hfp_ag_suggest_codec(context)){
+                    context->state = HFP_SLE_W4_EXCHANGE_COMMON_CODEC;
+                    context->suggested_codec = hfp_ag_suggest_codec(context);
+                    hfp_ag_cmd_suggest_codec(context->rfcomm_cid, context->suggested_codec);
+                }
+            }
+            break;
         default:
             break;
     }
@@ -588,10 +600,7 @@ static void hfp_ag_run_for_context_codecs_connection(hfp_connection_t * context)
         case HFP_CMD_TRIGGER_CODEC_CONNECTION_SETUP:
             switch (context->state){
                 case HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED:
-                    // hfp_ag_ok(context->rfcomm_cid);
-                    context->trigger_codec_connection_setup = 1;
-                    context->suggested_codec = hfp_ag_suggest_codec(context);
-                    hfp_ag_cmd_suggest_codec(context->rfcomm_cid, context->negotiated_codec);
+                    hfp_ag_cmd_suggest_codec(context->rfcomm_cid, context->suggested_codec);
                     context->state = HFP_SLE_W4_EXCHANGE_COMMON_CODEC;
                     break;
                 default:
@@ -618,7 +627,9 @@ static void hfp_ag_run_for_context_codecs_connection(hfp_connection_t * context)
 
 void hfp_run_for_context(hfp_connection_t *context){
     if (!context) return;
+
     if (!rfcomm_can_send_packet_now(context->rfcomm_cid)) return;
+
     // printf("AG hfp_run_for_context 1 state %d, command %d\n", context->state, context->command);
     if (context->send_ok){
         hfp_ag_ok(context->rfcomm_cid);
@@ -632,13 +643,27 @@ void hfp_run_for_context(hfp_connection_t *context){
         return;
     }
 
+    if (!rfcomm_can_send_packet_now(context->rfcomm_cid)) return;
     hfp_ag_run_for_context_service_level_connection(context);
+
+    if (!rfcomm_can_send_packet_now(context->rfcomm_cid)) return;
     hfp_ag_run_for_context_service_level_connection_queries(context);
+    if (!rfcomm_can_send_packet_now(context->rfcomm_cid)) return;
     hfp_ag_run_for_context_codecs_connection(context);
 
-    
-    // done
+    if (context->command == HFP_CMD_NONE){
+        switch(context->state){
+            case HFP_W2_DISCONNECT_RFCOMM:
+                // printf("rfcomm_disconnect_internal cid 0x%02x\n", context->rfcomm_cid);
+                context->state = HFP_W4_RFCOMM_DISCONNECTED;
+                rfcomm_disconnect_internal(context->rfcomm_cid);
+                break;
+            default:
+                break;
+        }
+    }
     context->command = HFP_CMD_NONE;
+
 }
 
 static void hfp_handle_rfcomm_event(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
@@ -672,7 +697,7 @@ static void hfp_run(){
 static void packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
     switch (packet_type){
         case RFCOMM_DATA_PACKET:
-            printf("\nAG received: %s\n", packet);
+            printf("\nAG received: %s", packet);
             hfp_handle_rfcomm_event(packet_type, channel, packet, size);
             break;
         case HCI_EVENT_PACKET:
@@ -799,7 +824,15 @@ void hfp_ag_negotiate_codecs(bd_addr_t bd_addr){
     hfp_ag_establish_service_level_connection(bd_addr);
     hfp_connection_t * connection = get_hfp_connection_context_for_bd_addr(bd_addr);
     if (!has_codec_negotiation_feature(connection)) return;
-    hfp_negotiate_codecs(connection);
+    if (connection->remote_codecs_nr == 0) return;
+    
+    if (connection->state >= HFP_W2_DISCONNECT_SCO) return;
+    
+    if (connection->state != HFP_SLE_W2_EXCHANGE_COMMON_CODEC &&
+        connection->state != HFP_SLE_W4_EXCHANGE_COMMON_CODEC){
+        connection->ag_trigger_codec_connection_setup = 1;
+    }
+
     hfp_run_for_context(connection);
 }
 
@@ -808,7 +841,14 @@ void hfp_ag_establish_audio_connection(bd_addr_t bd_addr){
     hfp_ag_establish_service_level_connection(bd_addr);
     hfp_connection_t * connection = get_hfp_connection_context_for_bd_addr(bd_addr);
     if (!has_codec_negotiation_feature(connection)) return;
-    hfp_establish_audio_connection(connection);
+    connection->establish_audio_connection = 0;
+    if (connection->state == HFP_AUDIO_CONNECTION_ESTABLISHED) return;
+    if (connection->state >= HFP_W2_DISCONNECT_SCO) return;
+    
+    connection->establish_audio_connection = 1;
+    if (connection->state < HFP_SLE_W4_EXCHANGE_COMMON_CODEC){
+        connection->ag_trigger_codec_connection_setup = 1;
+    }
     hfp_run_for_context(connection);
 }
 
diff --git a/src/hfp_ag.h b/src/hfp_ag.h
index d45faad69..10f4c1cea 100644
--- a/src/hfp_ag.h
+++ b/src/hfp_ag.h
@@ -152,6 +152,8 @@ void hfp_ag_transfer_callheld_status(bd_addr_t bd_addr, hfp_callheld_status_t st
 /**
  * @brief 
  */
+void hfp_ag_negotiate_codecs(bd_addr_t bd_addr);
+
 void hfp_ag_establish_audio_connection(bd_addr_t bd_addr);
 
 /**
diff --git a/src/hfp_hf.c b/src/hfp_hf.c
index c6aec3bfe..d330adebb 100644
--- a/src/hfp_hf.c
+++ b/src/hfp_hf.c
@@ -456,7 +456,7 @@ static void hfp_hf_run_for_context_codecs_connection(hfp_connection_t * context)
                 break;
             }
 
-            if (context->trigger_codec_connection_setup){
+            if (context->hf_trigger_codec_connection_setup){
                 context->state = HFP_SLE_W2_EXCHANGE_COMMON_CODEC;
                 context->wait_ok = 1;
                 hfp_hf_cmd_trigger_codec_connection_setup(context->rfcomm_cid);
@@ -509,7 +509,7 @@ static void hfp_hf_run_for_context_codecs_connection(hfp_connection_t * context)
                 break;
             }
 
-            if (context->trigger_codec_connection_setup){
+            if (context->hf_trigger_codec_connection_setup){
                 context->state = HFP_SLE_W2_EXCHANGE_COMMON_CODEC;
                 context->wait_ok = 1;
                 hfp_hf_cmd_trigger_codec_connection_setup(context->rfcomm_cid);
@@ -534,8 +534,8 @@ static void hfp_hf_handle_ok_codecs_connection(hfp_connection_t * context){
                 break;
             }
         case HFP_SLE_W2_EXCHANGE_COMMON_CODEC:
-            if (context->trigger_codec_connection_setup){
-                context->trigger_codec_connection_setup = 0;
+            if (context->hf_trigger_codec_connection_setup){
+                context->hf_trigger_codec_connection_setup = 0;
                 context->state = HFP_SLE_W4_EXCHANGE_COMMON_CODEC;
                 break;
             }
@@ -761,7 +761,14 @@ void hfp_hf_negotiate_codecs(bd_addr_t bd_addr){
     hfp_hf_establish_service_level_connection(bd_addr);
     hfp_connection_t * connection = get_hfp_connection_context_for_bd_addr(bd_addr);
     if (!has_codec_negotiation_feature(connection)) return;
-    hfp_negotiate_codecs(connection);
+    if (connection->remote_codecs_nr == 0) return;
+    
+    if (connection->state >= HFP_W2_DISCONNECT_SCO) return;
+    
+    if (connection->state != HFP_SLE_W2_EXCHANGE_COMMON_CODEC &&
+        connection->state != HFP_SLE_W4_EXCHANGE_COMMON_CODEC){
+        connection->hf_trigger_codec_connection_setup = 1;
+    }
     hfp_run_for_context(connection);
 }
 
@@ -770,7 +777,14 @@ void hfp_hf_establish_audio_connection(bd_addr_t bd_addr){
     hfp_hf_establish_service_level_connection(bd_addr);
     hfp_connection_t * connection = get_hfp_connection_context_for_bd_addr(bd_addr);
     if (!has_codec_negotiation_feature(connection)) return;
-    hfp_establish_audio_connection(connection);
+        connection->establish_audio_connection = 0;
+    if (connection->state == HFP_AUDIO_CONNECTION_ESTABLISHED) return;
+    if (connection->state >= HFP_W2_DISCONNECT_SCO) return;
+    
+    connection->establish_audio_connection = 1;
+    if (connection->state < HFP_SLE_W4_EXCHANGE_COMMON_CODEC){
+        connection->hf_trigger_codec_connection_setup = 1;
+    }
     hfp_run_for_context(connection);
 }
 
diff --git a/test/hfp/hfp_ag_client_test.c b/test/hfp/hfp_ag_client_test.c
index 9dc6c8aa3..3c3da1ab4 100644
--- a/test/hfp/hfp_ag_client_test.c
+++ b/test/hfp/hfp_ag_client_test.c
@@ -99,20 +99,38 @@ static uint8_t codecs_connection_established = 0;
 static uint8_t audio_connection_established = 0;
 static uint8_t service_level_connection_released = 0;
 
-void hfp_ag_run_test_sequence(char ** test_steps, int nr_test_steps){
+
+int expected_rfcomm_command(const char * expected_cmd){
+    char * ag_cmd = (char *)get_rfcomm_payload();
+    int ag_len = get_rfcomm_payload_len();
+    int expected_len = strlen(expected_cmd);
+    for (int i = 0; i < ag_len; i++){
+        if ( (ag_cmd+i)[0] == '\r' || (ag_cmd+i)[0] == '\n' ) {
+            continue;
+        }
+        if (memcmp(ag_cmd + i, expected_cmd, expected_len) == 0) return 1;
+    }
+    return 0;
+}
+
+#define HFP_NOP_CMD ""
+void simulate_test_sequence(char ** test_steps, int nr_test_steps){
     int i = 0;
     for (i=0; i < nr_test_steps; i++){
         char * cmd = test_steps[i];
-        if (memcmp(cmd, "OK", 2) == 0) continue;
+        printf(" test step %d %s\n", i, cmd);
         if (memcmp(cmd, "AT", 2) == 0){
-            printf("---> next step %s\n", cmd);
-            inject_rfcomm_command((uint8_t*)cmd, strlen(cmd));
+            inject_rfcomm_command_to_ag((uint8_t*)cmd, strlen(cmd));
+        } else if (memcmp(cmd, "NOP", 3) == 0){
+            printf("Trigger AG to run state machine\n");
+            inject_rfcomm_command_to_ag((uint8_t*)"NOP",3);
         } else {
             int expected_cmd = expected_rfcomm_command(cmd);
             if (!expected_cmd){
                 printf("\nError: Expected:'%s', but got:'%s'", cmd, (char *)get_rfcomm_payload());
                 return;
-            }
+            } 
+            printf("AG response verified %s\n", cmd);
         }
     }
 }
@@ -126,11 +144,13 @@ void packet_handler(uint8_t * event, uint16_t event_size){
     
     switch (event[2]) {   
         case HFP_SUBEVENT_SERVICE_LEVEL_CONNECTION_ESTABLISHED:
+            printf("\n\n\n\n ** SLC established **\n\n\n\n\n");
             service_level_connection_established = 1;
             codecs_connection_established = 0;
             audio_connection_established = 0;
             break;
         case HFP_SUBEVENT_CODECS_CONNECTION_COMPLETE:
+            printf("\n\n\n\n ** CC established **\n\n\n\n\n");
             codecs_connection_established = 1;
             audio_connection_established = 0;
             break;
@@ -147,7 +167,7 @@ void packet_handler(uint8_t * event, uint16_t event_size){
 }
 
 
-TEST_GROUP(AudiogatewayClient){
+TEST_GROUP(HFPClient){
     void setup(void){
         service_level_connection_established = 0;
         codecs_connection_established = 0;
@@ -165,18 +185,42 @@ TEST_GROUP(AudiogatewayClient){
     void setup_hfp_service_level_connection(char ** test_steps, int nr_test_steps){
         service_level_connection_established = 0;
         hfp_ag_establish_service_level_connection(device_addr);
-        hfp_ag_run_test_sequence((char **) test_steps, nr_test_steps);
-        CHECK_EQUAL(service_level_connection_established, 1);
+        simulate_test_sequence((char **) test_steps, nr_test_steps);
     }
+
+    void setup_hfp_codecs_connection(char ** test_steps, int nr_test_steps){
+        codecs_connection_established = 0;
+        //hfp_ag_negotiate_codecs(device_addr);
+        simulate_test_sequence((char **) test_steps, nr_test_steps);
+    }
+
 };
 
-
-TEST(AudiogatewayClient, HFServiceLevelConnectionEstablished){
-    for (int i = 0; i < slc_tests_size(); i++){
-        setup_hfp_service_level_connection(hfp_slc_tests()[i].test, hfp_slc_tests()[i].len);
+TEST(HFPClient, HFCodecsConnectionEstablished){
+    setup_hfp_service_level_connection(default_slc_setup(), default_slc_setup_size());
+    CHECK_EQUAL(service_level_connection_established, 1);
+    
+    for (int i = 0; i < cc_tests_size(); i++){
+        setup_hfp_codecs_connection(hfp_cc_tests()[i].test, hfp_cc_tests()[i].len);
+        CHECK_EQUAL(codecs_connection_established, 1);
     }
 }
 
+// TEST(HFPClient, HFServiceLevelConnectionCommands){
+//     setup_hfp_service_level_connection(default_slc_setup(), default_slc_setup_size());
+//     for (int i = 0; i < slc_cmds_tests_size(); i++){
+//         simulate_test_sequence(hfp_slc_cmds_tests()[i].test, hfp_slc_cmds_tests()[i].len);
+//     }
+//     CHECK_EQUAL(service_level_connection_established, 1);
+// }
+
+// TEST(HFPClient, HFServiceLevelConnectionEstablished){
+//     for (int i = 0; i < slc_tests_size(); i++){
+//         setup_hfp_service_level_connection(hfp_slc_tests()[i].test, hfp_slc_tests()[i].len);
+//         CHECK_EQUAL(service_level_connection_established, 1);
+//     }
+// }
+
 
 int main (int argc, const char * argv[]){
     hfp_ag_init(rfcomm_channel_nr, 1007, codecs, sizeof(codecs), 
diff --git a/test/hfp/hfp_ag_parser_test.c b/test/hfp/hfp_ag_parser_test.c
index 609f10503..cc69fd197 100644
--- a/test/hfp/hfp_ag_parser_test.c
+++ b/test/hfp/hfp_ag_parser_test.c
@@ -230,7 +230,7 @@ TEST(HFPParser, HFP_AG_TRIGGER_CODEC_CONNECTION_SETUP){
     }
 
     CHECK_EQUAL(context.command, HFP_CMD_TRIGGER_CODEC_CONNECTION_SETUP);
-    CHECK_EQUAL(context.trigger_codec_connection_setup, 1);
+    CHECK_EQUAL(context.ag_trigger_codec_connection_setup, 1);
 }
 
 TEST(HFPParser, HFP_AG_CONFIRM_COMMON_CODEC){
diff --git a/test/hfp/hfp_hf_client_test.c b/test/hfp/hfp_hf_client_test.c
index 36ca0966f..5eab8c6ab 100644
--- a/test/hfp/hfp_hf_client_test.c
+++ b/test/hfp/hfp_hf_client_test.c
@@ -78,6 +78,35 @@ static uint8_t codecs_connection_established = 0;
 static uint8_t audio_connection_established = 0;
 static uint8_t service_level_connection_released = 0;
 
+int expected_rfcomm_command(const char * cmd){
+    char * ag_cmd = (char *)get_rfcomm_payload();
+    int offset = 2;
+    int cmd_size = strlen(cmd);
+
+    int cmd_found = memcmp(ag_cmd+offset, cmd, cmd_size) == 0;
+    while (!cmd_found && get_rfcomm_payload_len() - cmd_size >= offset){
+        offset++;
+        cmd_found = strncmp(ag_cmd+offset, cmd, cmd_size) == 0;
+    }
+    if (!cmd_found) return 0;
+    
+    // AG cmds that are not followed by OK
+    if (memcmp(ag_cmd+offset, "+BCS", 4) == 0){
+        return cmd_found;
+    }
+
+    offset += strlen(cmd)+4;
+    // printf("cmd found, offset %d, cmd %s\n", offset, ag_cmd+offset);
+    int ok_found = memcmp(ag_cmd+offset, "OK", 2) == 0;
+    while (!ok_found && get_rfcomm_payload_len() - 2 >= offset){
+        offset++;
+        // printf("cmd found, offset %d, cmd %s\n", offset, ag_cmd+offset);
+        ok_found = memcmp(ag_cmd+offset, "OK", 2) == 0;
+    }
+    // printf("cmd found, ok found %d\n", ok_found);
+    return cmd_found && ok_found;
+}
+
 void hfp_hf_run_test_sequence(char ** test_steps, int nr_test_steps){
     int i = 0;
     for (i=0; i < nr_test_steps; i++){
@@ -148,7 +177,7 @@ void packet_handler(uint8_t * event, uint16_t event_size){
 
 
 
-TEST_GROUP(HandsfreeClient){
+TEST_GROUP(HFPClient){
 
     void setup(void){
         service_level_connection_established = 0;
@@ -164,7 +193,7 @@ TEST_GROUP(HandsfreeClient){
         }
     }
 
-    void setup_hfp_service_level_connection(char ** test_steps, int nr_test_steps){
+    void verify_hfp_service_level_connection_established(char ** test_steps, int nr_test_steps){
         service_level_connection_established = 0;
         hfp_hf_establish_service_level_connection(device_addr);
         hfp_hf_run_test_sequence((char **) test_steps, nr_test_steps);
@@ -173,7 +202,7 @@ TEST_GROUP(HandsfreeClient){
         inject_rfcomm_command((uint8_t*)HFP_OK, strlen(HFP_OK));
     }
 
-    void setup_hfp_codecs_connection_state_machine(char ** test_steps, int nr_test_steps){
+    void verify_hfp_codecs_connection_established(char ** test_steps, int nr_test_steps){
         codecs_connection_established = 0;
         hfp_hf_negotiate_codecs(device_addr);
         hfp_hf_run_test_sequence((char **) test_steps, nr_test_steps);
@@ -183,22 +212,24 @@ TEST_GROUP(HandsfreeClient){
 };
 
 
-TEST(HandsfreeClient, HFCodecsConnectionEstablished){
-    setup_hfp_service_level_connection(default_slc_setup(), default_slc_setup_size());
+TEST(HFPClient, HFCodecsConnectionEstablished){
+    verify_hfp_service_level_connection_established(default_slc_setup(), default_slc_setup_size());
     for (int i = 0; i < cc_tests_size(); i++){
-        setup_hfp_codecs_connection_state_machine(hfp_cc_tests()[i].test, hfp_cc_tests()[i].len);
+        verify_hfp_codecs_connection_established(hfp_cc_tests()[i].test, hfp_cc_tests()[i].len);
     }
 }
 
-TEST(HandsfreeClient, HFCodecChange){
-    setup_hfp_service_level_connection(default_slc_setup(), default_slc_setup_size());
-    hfp_hf_run_test_sequence(default_cc_setup(), default_cc_setup_size());
+TEST(HFPClient, HFServiceLevelConnectionCommands){
+    verify_hfp_service_level_connection_established(default_slc_setup(), default_slc_setup_size());
+    for (int i = 0; i < slc_cmds_tests_size(); i++){
+        hfp_hf_run_test_sequence(hfp_slc_cmds_tests()[i].test, hfp_slc_cmds_tests()[i].len);
+    }
     CHECK_EQUAL(service_level_connection_established, 1);
 }
 
-TEST(HandsfreeClient, HFServiceLevelConnectionEstablished){
+TEST(HFPClient, HFServiceLevelConnectionEstablished){
     for (int i = 0; i < slc_tests_size(); i++){
-        setup_hfp_service_level_connection(hfp_slc_tests()[i].test, hfp_slc_tests()[i].len);
+        verify_hfp_service_level_connection_established(hfp_slc_tests()[i].test, hfp_slc_tests()[i].len);
     }
 }
 
diff --git a/test/hfp/mock.c b/test/hfp/mock.c
index af0525433..e8db31c7d 100644
--- a/test/hfp/mock.c
+++ b/test/hfp/mock.c
@@ -75,33 +75,43 @@ uint16_t get_rfcomm_payload_len(){
 }
 
 static void prepare_rfcomm_buffer(uint8_t * data, int len){
-	memset(&rfcomm_payload, 0, 200);
+    if (len <= 0) return;
+    memset(&rfcomm_payload, 0, 200);
 	int pos = 0;
-	if (memcmp((char*)data, "AT", 2) != 0){
-		rfcomm_payload[pos++] = '\r';
+
+    if (strncmp((char*)data, "AT", 2) == 0){
+        strncpy((char*)&rfcomm_payload[pos], (char*)data, len);
+        pos += len;
+    } else {
+    	rfcomm_payload[pos++] = '\r';
 		rfcomm_payload[pos++] = '\n';
-	}
-	memcpy((char*)&rfcomm_payload[pos], data, len);
-	pos += len;
-	if (memcmp((char*)data, "AT", 2) != 0){
-		rfcomm_payload[pos++] = '\r';
-		rfcomm_payload[pos++] = '\n';
-        rfcomm_payload[pos++] = 'O';
-        rfcomm_payload[pos++] = 'K';
-    }   
-    rfcomm_payload[pos++] = '\r';
-    rfcomm_payload[pos] = '\n';
+        strncpy((char*)&rfcomm_payload[pos], (char*)data, len);
+        pos += len;
+    
+        if (memcmp((char*)data, "+BAC", 4) != 0 &&
+            memcmp((char*)data, "+BCS", 4) != 0){
+            rfcomm_payload[pos++] = '\r';
+            rfcomm_payload[pos++] = '\n';
+            rfcomm_payload[pos++] = 'O';
+            rfcomm_payload[pos++] = 'K';
+        }   
+    
+    }
+	rfcomm_payload[pos++] = '\r';
+    rfcomm_payload[pos++] = '\n';
+    rfcomm_payload[pos] = 0;
 	rfcomm_payload_len = pos;
 }	
 
 
 int  rfcomm_send_internal(uint16_t rfcomm_cid, uint8_t *data, uint16_t len){
-	if (memcmp((char*)data, "AT", 2) == 0){
-		printf("HF send: %s", data);
+	if (strncmp((char*)data, "AT", 2) == 0){
+		printf("Verify HF state machine response: %s", data);
 	} else {
-		printf("AG send: %s", data);
+        printf("Verify AG state machine response: %s", data+2);
 	}
-	prepare_rfcomm_buffer(data, len);
+	strncpy((char*)&rfcomm_payload[0], (char*)data, len);
+    rfcomm_payload_len = len;
 	return 0;
 }
 
@@ -188,47 +198,43 @@ void rfcomm_accept_connection_internal(uint16_t rfcomm_cid){
 	printf("rfcomm_accept_connection_internal \n");
 }
 
-// HFP Mock API
-
-int expected_rfcomm_command(const char * cmd){
-    char * ag_cmd = (char *)get_rfcomm_payload();
-    int offset = 0;
-    int cmd_size = strlen(cmd);
-
-    if (memcmp(ag_cmd, "OK", 2) == 0){
-        int ok_found = memcmp(ag_cmd+offset, "OK", 2) == 0;
-        while (!ok_found && get_rfcomm_payload_len() - 2 >= offset){
-            offset++;
-            ok_found = memcmp(ag_cmd+offset, "OK", 2) == 0;
-        }
-        return ok_found;
-    }
-
-    int cmd_found = memcmp(ag_cmd, cmd, cmd_size) == 0;
-    while (!cmd_found && get_rfcomm_payload_len() - cmd_size >= offset){
-        offset++;
-        cmd_found = strncmp(ag_cmd+offset, cmd, cmd_size) == 0;
-    }
-    
-    if (!cmd_found) return 0;
-    offset += strlen(cmd);
-
-    int ok_found = memcmp(ag_cmd+offset, "OK", 2) == 0;
-    while (!ok_found && get_rfcomm_payload_len() - 2 >= offset){
-        offset++;
-        ok_found = memcmp(ag_cmd+offset, "OK", 2) == 0;
-    }
-  
-    return cmd_found && ok_found;
-}
-
 
 void inject_rfcomm_command(uint8_t * data, int len){
     prepare_rfcomm_buffer(data, len);
+    if (memcmp((char*)data, "AT", 2) == 0){
+        printf("\n\n ---> Send cmd to AG state machine: %s", data);
+    } else if (memcmp((char*)data, "+", 1) == 0){
+
+    } else {
+        printf("\n\n ---> Send cmd to HF state machine: %s", data);
+    }
+    (*registered_rfcomm_packet_handler)(active_connection, RFCOMM_DATA_PACKET, rfcomm_cid, (uint8_t *) &rfcomm_payload[0], rfcomm_payload_len);
+}
+
+void inject_rfcomm_command_to_hf(uint8_t * data, int len){
+    if (memcmp((char*)data, "AT", 2) == 0) return;
+    
+    prepare_rfcomm_buffer(data, len);
+    if (data[0] == '+'){
+        printf("\n\n ---> Send cmd to HF state machine: %s", data);
+    } else {
+        printf("\n\n ---> trigger HF state machine: %s", data);
+    }
+    (*registered_rfcomm_packet_handler)(active_connection, RFCOMM_DATA_PACKET, rfcomm_cid, (uint8_t *) &rfcomm_payload[0], rfcomm_payload_len);
+}
+
+void inject_rfcomm_command_to_ag(uint8_t * data, int len){
+    if (data[0] == '+') return;
+    
+    prepare_rfcomm_buffer(data, len);
+    if (memcmp((char*)data, "AT", 2) == 0){
+        printf("\n\n ---> Send cmd to AG state machine: %s", data);
+    } else {
+        printf("\n\n ---> trigger AG state machine: %s", data);
+    }
     (*registered_rfcomm_packet_handler)(active_connection, RFCOMM_DATA_PACKET, rfcomm_cid, (uint8_t *) &rfcomm_payload[0], rfcomm_payload_len);
 }
 
 
 
 
-
diff --git a/test/hfp/mock.h b/test/hfp/mock.h
index b193ad6df..de3b32531 100644
--- a/test/hfp/mock.h
+++ b/test/hfp/mock.h
@@ -50,4 +50,6 @@
 uint8_t * get_rfcomm_payload();
 uint16_t get_rfcomm_payload_len();
 void inject_rfcomm_command(uint8_t * data, int len);
-int expected_rfcomm_command(const char * cmd);
+
+void inject_rfcomm_command_to_ag(uint8_t * data, int len);
+void inject_rfcomm_command_to_hf(uint8_t * data, int len);
diff --git a/test/hfp/test_sequences.c b/test/hfp/test_sequences.c
index c73c676aa..a7f5a5b1d 100644
--- a/test/hfp/test_sequences.c
+++ b/test/hfp/test_sequences.c
@@ -86,7 +86,6 @@ hfp_test_item_t slc_cmds_tests[] = {
     TEST_SEQUENCE(slc_cmds_test1)
 };
 /* Codecs Connection (cc) test sequences */
-
 const char * cc_test1[] = {
     "AT+BCC", 
     "OK",
@@ -96,6 +95,39 @@ const char * cc_test1[] = {
 };
 
 const char * cc_test2[] = {
+    "AT+BCC", 
+    "OK",
+    "NOP",
+    "NOP+BCS:1",
+    "AT+BCS=1",
+    "OK"
+};
+
+const char * cc_test3[] = {
+    "AT+BAC=1,2", 
+    "OK",
+    "AT+BCC",
+    "OK",
+    "+BCS:1",
+    "AT+BCS=1",
+    "OK"
+};
+
+
+const char * cc_test4[] = {
+    "AT+BAC=1,2", 
+    "OK",
+    "AT+BCC",
+    "OK",
+    "+BCS:1",
+    "AT+BAC=2,3", 
+    "OK",
+    "+BCS:2",
+    "AT+BCS=2",
+    "OK"
+};
+
+const char * cc_test5[] = {
     "AT+BCC", 
     "OK",
     "+BCS:1",
@@ -107,44 +139,37 @@ const char * cc_test2[] = {
 };
 
 hfp_test_item_t cc_tests[] = {
-    TEST_SEQUENCE(cc_test1),
-    TEST_SEQUENCE(cc_test2)
+    TEST_SEQUENCE(cc_test1)//,
+    // TEST_SEQUENCE(cc_test2),
+    // TEST_SEQUENCE(cc_test3)
 };
 
 
 
 //////////////
 
-int test_item_size = sizeof(hfp_test_item_t);
+static int test_item_size = sizeof(hfp_test_item_t);
 
 // SLC
-hfp_test_item_t * hfp_slc_tests(){
-    return slc_tests;
-}
+hfp_test_item_t * hfp_slc_tests(){ return slc_tests;}
+int slc_tests_size(){ return sizeof(slc_tests)/test_item_size;}
 
-int slc_tests_size(){
-    return sizeof(slc_tests)/test_item_size;
-}
+char ** default_slc_setup() { return (char **)slc_test1;}
+int default_slc_setup_size(){ return sizeof(slc_test1)/sizeof(char*);}
 
-char ** default_slc_setup(){
-    return (char **)slc_test1;
-}
+// SLC commands
+hfp_test_item_t * hfp_slc_cmds_tests(){ return slc_cmds_tests;}
+int slc_cmds_tests_size(){ return sizeof(slc_cmds_tests)/test_item_size;}
 
-int default_slc_setup_size(){
-    return sizeof(slc_test1)/sizeof(char*);
-}
+char ** default_slc_cmds_setup() { return (char **)slc_cmds_test1;}
+int default_slc_cmds_setup_size(){ return sizeof(slc_cmds_test1)/sizeof(char*);}
 
 // CC
-hfp_test_item_t * hfp_cc_tests(){
-    return cc_tests;
-}
-int cc_tests_size(){
-    return sizeof(cc_tests) /test_item_size;
-}
-char ** default_cc_setup(){
-    return (char **)cc_test1;
-}
-int default_cc_setup_size(){
-    return sizeof(cc_test1)/sizeof(char*);  
+hfp_test_item_t * hfp_cc_tests(){ return cc_tests;}
+int cc_tests_size(){ return sizeof(cc_tests) /test_item_size;
 }
+char ** default_cc_setup() { return (char **)cc_test1;}
+int default_cc_setup_size(){ return sizeof(cc_test1)/sizeof(char*);}
+
+
  
\ No newline at end of file
diff --git a/test/hfp/test_sequences.h b/test/hfp/test_sequences.h
index f5d7eb6f5..d05e40440 100644
--- a/test/hfp/test_sequences.h
+++ b/test/hfp/test_sequences.h
@@ -60,6 +60,9 @@ int default_slc_setup_size();
 
 /* Service Level Connection (slc) common commands */
 hfp_test_item_t * hfp_slc_cmds_tests();
+int slc_cmds_tests_size();
+char ** default_slc_cmds_setup();
+int default_slc_cmds_setup_size();
 
 /* Codecs Connection (cc) test sequences */
 hfp_test_item_t * hfp_cc_tests();