diff --git a/src/hfp.c b/src/hfp.c
index 1fdc03cb5..2df34c775 100644
--- a/src/hfp.c
+++ b/src/hfp.c
@@ -279,12 +279,7 @@ void hfp_reset_context_flags(hfp_connection_t * context){
     context->enable_extended_audio_gateway_error_report = 0;
     context->extended_audio_gateway_error = 0;
 
-    // can come any time (here taken into account only after SLE),
-    // if codec negotiation feature is set
-    context->notify_ag_on_new_codecs = 0;
-    
     // establish codecs connection
-    context->hf_trigger_codec_connection_setup = 0;
     context->suggested_codec = 0;
     context->negotiated_codec = 0;
     context->codec_confirmed = 0;
@@ -669,7 +664,6 @@ static void process_command(hfp_connection_t * context){
 
     if (strncmp((char *)context->line_buffer+offset, HFP_AVAILABLE_CODECS, strlen(HFP_AVAILABLE_CODECS)) == 0){
         context->command = HFP_CMD_AVAILABLE_CODECS;
-        context->notify_ag_on_new_codecs = 1;
         return;
     }
 
@@ -739,8 +733,6 @@ static void process_command(hfp_connection_t * context){
 
     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;
-        // printf("HFP_CMD_TRIGGER_CODEC_CONNECTION_SETUP update command\n");
-        context->hf_trigger_codec_connection_setup = 1;
         return;
     } 
 
diff --git a/src/hfp.h b/src/hfp.h
index 0d099883e..800d423a6 100644
--- a/src/hfp.h
+++ b/src/hfp.h
@@ -242,27 +242,22 @@ typedef enum {
     HFP_RETRIEVE_GENERIC_STATUS_INDICATORS,
     HFP_W4_RETRIEVE_GENERIC_STATUS_INDICATORS,
     
-    HFP_RETRIEVE_INITITAL_STATE_GENERIC_STATUS_INDICATORS, // 20
+    HFP_RETRIEVE_INITITAL_STATE_GENERIC_STATUS_INDICATORS, 
     HFP_W4_RETRIEVE_INITITAL_STATE_GENERIC_STATUS_INDICATORS,
     
-    HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED, // 22
-    
-    HFP_SLE_W2_EXCHANGE_COMMON_CODEC,
-    HFP_SLE_W4_EXCHANGE_COMMON_CODEC,
-    
-    HFP_CODECS_CONNECTION_ESTABLISHED, // 25
+    HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED, 
     
     HFP_W2_CONNECT_SCO,
     HFP_W4_SCO_CONNECTED,
     
-    HFP_AUDIO_CONNECTION_ESTABLISHED,
+    HFP_AUDIO_CONNECTION_ESTABLISHED, 
     
     HFP_W2_DISCONNECT_SCO,
-    HFP_W4_SCO_DISCONNECTED, // 30
+    HFP_W4_SCO_DISCONNECTED, 
 
     HFP_W2_DISCONNECT_RFCOMM,
     HFP_W4_RFCOMM_DISCONNECTED, 
-    HFP_W4_RFCOMM_DISCONNECTED_AND_RESTART,
+    HFP_W4_RFCOMM_DISCONNECTED_AND_RESTART, 
     HFP_W4_CONNECTION_ESTABLISHED_TO_SHUTDOWN
 } hfp_state_t;
 
@@ -270,6 +265,7 @@ typedef enum {
     HFP_CODECS_IDLE,
     HFP_CODECS_RECEIVED_LIST,
     HFP_CODECS_RECEIVED_TRIGGER_CODEC_EXCHANGE,
+    HFP_CODECS_W4_AG_COMMON_CODEC,
     HFP_CODECS_AG_SENT_COMMON_CODEC,
     HFP_CODECS_AG_RESEND_COMMON_CODEC,
     HFP_CODECS_EXCHANGED,
@@ -395,12 +391,7 @@ typedef struct hfp_connection {
     uint8_t enable_extended_audio_gateway_error_report;
     uint8_t extended_audio_gateway_error;
 
-    // can come any time (here taken into account only after SLE),
-    // if codec negotiation feature is set
-    uint8_t notify_ag_on_new_codecs;
-    
     // establish codecs connection
-    uint8_t hf_trigger_codec_connection_setup;
     uint8_t suggested_codec;
     uint8_t codec_confirmed;
 
diff --git a/src/hfp_ag.c b/src/hfp_ag.c
index 4ca432c8b..5378c4ea2 100644
--- a/src/hfp_ag.c
+++ b/src/hfp_ag.c
@@ -184,10 +184,6 @@ static int hfp_ag_report_extended_audio_gateway_error(uint16_t cid, uint8_t erro
     return send_str_over_rfcomm(cid, buffer);
 }
 
-static int hfp_ag_retrieve_codec_cmd(uint16_t cid){
-    return hfp_ag_ok(cid);
-}
-
 static int hfp_ag_indicators_join(char * buffer, int buffer_size, hfp_connection_t * context){
     if (buffer_size < get_hfp_ag_indicators_nr(context) * (1 + sizeof(hfp_ag_indicator_t))) return 0;
     int i;
diff --git a/src/hfp_hf.c b/src/hfp_hf.c
index 701ec6573..df539593e 100644
--- a/src/hfp_hf.c
+++ b/src/hfp_hf.c
@@ -461,154 +461,81 @@ static void hfp_hf_handle_ok_service_level_connection_queries(hfp_connection_t *
     }
 }
 
-
-static int hfp_hf_run_for_context_codecs_connection(hfp_connection_t * context){
-    if (context->state <= HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED ||
-        context->state > HFP_CODECS_CONNECTION_ESTABLISHED) return 0;
+static int codecs_exchange_state_machine(hfp_connection_t * context){
+    if (context->wait_ok) return 0;
     int done = 0;
-    // handle audio connection setup
-    // printf("hfp_run_for_context state %d \n", context->state);
-    if (context->wait_ok) return done;
-
-    switch (context->state){
-        case HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED:
-            if (context->notify_ag_on_new_codecs){
-                context->wait_ok = 1;
-                hfp_hf_cmd_notify_on_codecs(context->rfcomm_cid);
-                done = 1;
-                break;
+    
+    switch(context->command){
+        case HFP_CMD_AVAILABLE_CODECS:
+            switch (context->codecs_state){
+                case HFP_CODECS_W4_AG_COMMON_CODEC:
+                    context->codec_confirmed = 0;
+                    context->suggested_codec = 0;
+                    context->negotiated_codec = 0;
+                    break;
+                case HFP_CODECS_EXCHANGED:
+                    context->negotiated_codec = 0;
+                    context->codecs_state = HFP_CODECS_W4_AG_COMMON_CODEC;
+                    break;
+                default:
+                    break;
             }
 
-            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);
-                done = 1;
-                break;
-            }
+            hfp_hf_cmd_notify_on_codecs(context->rfcomm_cid);
+            done = 1;
 
-            if (context->suggested_codec){
-                context->state = HFP_SLE_W4_EXCHANGE_COMMON_CODEC;
-                context->codec_confirmed = 1;
-                context->wait_ok = 1;
+            break;
+        case HFP_CMD_TRIGGER_CODEC_CONNECTION_SETUP:
+            context->codecs_state = HFP_CODECS_RECEIVED_TRIGGER_CODEC_EXCHANGE;
+            hfp_hf_cmd_trigger_codec_connection_setup(context->rfcomm_cid);
+            done = 1;
+            break;
+
+        case HFP_CMD_AG_SUGGESTED_CODEC:
+            if (hfp_hf_supports_codec(context->suggested_codec)){
+                context->codec_confirmed = context->suggested_codec;
                 hfp_hf_cmd_confirm_codec(context->rfcomm_cid, context->suggested_codec);
                 done = 1;
-                break;
-            }
-            break;
-        
-        case HFP_SLE_W4_EXCHANGE_COMMON_CODEC:
-            if (context->notify_ag_on_new_codecs){
-                context->wait_ok = 1;
+            } else {
                 context->codec_confirmed = 0;
                 context->suggested_codec = 0;
                 context->negotiated_codec = 0;
                 hfp_hf_cmd_notify_on_codecs(context->rfcomm_cid);
                 done = 1;
-                break;
             }
-            if (context->suggested_codec){
-                if (hfp_hf_supports_codec(context->suggested_codec)){
-                    context->codec_confirmed = context->suggested_codec;
-                    context->wait_ok = 1;
-                    hfp_hf_cmd_confirm_codec(context->rfcomm_cid, context->suggested_codec);
-                    done = 1;
-                } else {
-                    context->notify_ag_on_new_codecs = 1;
-                    context->wait_ok = 1;
-                    context->codec_confirmed = 0;
-                    context->suggested_codec = 0;
-                    context->negotiated_codec = 0;
-                    hfp_hf_cmd_notify_on_codecs(context->rfcomm_cid);
-                    done = 1;
-                }
-                
-                break;
-            }
-            
             break;
-            
-        case HFP_CODECS_CONNECTION_ESTABLISHED:
-            if (context->notify_ag_on_new_codecs){
-                context->negotiated_codec = 0;
-                context->wait_ok = 1;
-                context->state = HFP_SLE_W4_EXCHANGE_COMMON_CODEC;
-                hfp_hf_cmd_notify_on_codecs(context->rfcomm_cid);
-                done = 1;
-                break;
-            }
+        
+        default:
+            break;
+    }
 
-            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);
-                done = 1;
-                break;
-            }
-
-            if (context->establish_audio_connection){
-                // TODO AUDIO CONNECTION
-            }
-            break;
-       default:
-            break;
+    if (done){
+         context->wait_ok = 1;
     }
     return done;
 }
 
 static void hfp_hf_handle_ok_codecs_connection(hfp_connection_t * context){
-        // handle audio connection setup
-    switch (context->state){
-        case HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED:
-            if (context->notify_ag_on_new_codecs){
-                context->notify_ag_on_new_codecs = 0;
-                break;
-            }
-        case HFP_SLE_W2_EXCHANGE_COMMON_CODEC:
-            if (context->hf_trigger_codec_connection_setup){
-                context->hf_trigger_codec_connection_setup = 0;
-                context->state = HFP_SLE_W4_EXCHANGE_COMMON_CODEC;
-                break;
-            }
+    // handle audio connection setup
+    switch (context->codecs_state){
+        case HFP_CODECS_RECEIVED_TRIGGER_CODEC_EXCHANGE:
+            context->codecs_state = HFP_CODECS_W4_AG_COMMON_CODEC;
             break;
-        case HFP_SLE_W4_EXCHANGE_COMMON_CODEC:
-            if (context->notify_ag_on_new_codecs){
-                context->codec_confirmed = 0;
-                context->suggested_codec = 0;
-                context->notify_ag_on_new_codecs = 0;
-                break;
-            }
-            if (context->codec_confirmed && context->suggested_codec){
-                context->negotiated_codec = context->suggested_codec; 
-                context->codec_confirmed = 0;
-                context->suggested_codec = 0;
-                context->state = HFP_CODECS_CONNECTION_ESTABLISHED;
-                hfp_emit_event(hfp_callback, HFP_SUBEVENT_CODECS_CONNECTION_COMPLETE, 0);
-                break;
-            }
-            break;
-
-        case HFP_AUDIO_CONNECTION_ESTABLISHED:
-            printf("HFP_AUDIO_CONNECTION_ESTABLISHED \n");
-            break;
-
         default:
             break;
     }
 }
 
 static void hfp_run_for_context(hfp_connection_t * context){
-
     if (!context) return;
     if (!rfcomm_can_send_packet_now(context->rfcomm_cid)) return;
     
     int done = hfp_hf_run_for_context_service_level_connection(context);
-    
-    if (rfcomm_can_send_packet_now(context->rfcomm_cid) && !done){
+    if (!done){
         done = hfp_hf_run_for_context_service_level_connection_queries(context);
-        if (rfcomm_can_send_packet_now(context->rfcomm_cid) && !done){
-            done = hfp_hf_run_for_context_codecs_connection(context);
-        }
+    }
+    if (!done){
+        done = codecs_exchange_state_machine(context);
     }
 
     if (done) return;
@@ -631,7 +558,6 @@ static void hfp_hf_switch_on_ok(hfp_connection_t *context){
     hfp_hf_handle_ok_service_level_connection_establishment(context);
     hfp_hf_handle_ok_service_level_connection_queries(context);
     hfp_hf_handle_ok_codecs_connection(context);
-
     // done
     context->command = HFP_CMD_NONE;
 }
@@ -718,7 +644,7 @@ void hfp_hf_set_codecs(uint8_t * codecs, int codecs_nr){
     while (linked_list_iterator_has_next(&it)){
         hfp_connection_t * connection = (hfp_connection_t *)linked_list_iterator_next(&it);
         if (!connection) continue;
-        connection->notify_ag_on_new_codecs = 1;
+        connection->command = HFP_CMD_AVAILABLE_CODECS;
         hfp_run_for_context(connection);
     }
 }
@@ -795,22 +721,6 @@ void hfp_hf_enable_report_extended_audio_gateway_error_result_code(bd_addr_t bd_
     hfp_run_for_context(connection);
 }
 
-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;
-    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);
-}
-
-
 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);
@@ -820,9 +730,13 @@ void hfp_hf_establish_audio_connection(bd_addr_t bd_addr){
     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;
-    }
+    switch (connection->codecs_state){
+        case HFP_CODECS_W4_AG_COMMON_CODEC:
+            break;
+        default:
+            connection->command = HFP_CMD_TRIGGER_CODEC_CONNECTION_SETUP;
+            break;
+    } 
     hfp_run_for_context(connection);
 }
 
diff --git a/src/hfp_hf.h b/src/hfp_hf.h
index f0e7d8feb..9529c3ab5 100644
--- a/src/hfp_hf.h
+++ b/src/hfp_hf.h
@@ -141,8 +141,6 @@ void hfp_hf_enable_report_extended_audio_gateway_error_result_code(bd_addr_t bd_
 /**
  * @brief 
  */
-void hfp_hf_negotiate_codecs(bd_addr_t bd_addr);
-
 void hfp_hf_establish_audio_connection(bd_addr_t bd_addr);
 
 /**