diff --git a/chipset-bcm/bt_control_bcm.c b/chipset-bcm/bt_control_bcm.c
new file mode 100644
index 000000000..8f21f53ec
--- /dev/null
+++ b/chipset-bcm/bt_control_bcm.c
@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2009-2012 by Matthias Ringwald
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holders nor the names of
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ * 4. Any redistribution, use, or modification is done solely for
+ *    personal benefit and not for any commercial purpose or for
+ *    monetary gain.
+ *
+ * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
+ * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
+ * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * Please inquire about commercial licensing options at btstack@ringwald.ch
+ *
+ */
+
+/*
+ *  bt_control_bcm.c
+ *
+ *  Adapter to use Broadcom-based chipsets with BTstack
+ */
+
+
+#include "btstack-config.h"
+#include "bt_control_bcm.h"
+
+#include <stddef.h>   /* NULL */
+#include <stdio.h> 
+#include <string.h>   /* memcpy */
+
+#include "bt_control.h"
+#include "debug.h"
+
+// actual init script provided by separate bt_firmware_image.c from WICED SDK
+extern const uint8_t brcm_patchram_buf[];
+extern const int     brcm_patch_ram_length;
+extern const char    brcm_patch_version[];
+
+//
+static uint32_t init_script_offset;
+static int send_download_command;
+
+static int bt_control_bcm_on(void *config){
+    log_info("Broadcom init script %s, len %u", brcm_patch_version, brcm_patch_ram_length);
+    init_script_offset = 0;
+    send_download_command = 1;
+    return 0;
+}
+
+// @note: Broadcom chips require higher UART clock for baud rate > 3000000 -> limit baud rate in hci.c
+static int bcm_baudrate_cmd(void * config, uint32_t baudrate, uint8_t *hci_cmd_buffer){
+    hci_cmd_buffer[0] = 0x18;
+    hci_cmd_buffer[1] = 0xfc;
+    hci_cmd_buffer[2] = 0x06;
+    hci_cmd_buffer[3] = 0x00;
+    hci_cmd_buffer[4] = 0x00;
+    bt_store_32(hci_cmd_buffer, 5, baudrate);
+    return 0;
+}
+
+// @note: bd addr has to be set after sending init script (it might just get re-set)
+static int bt_control_bcm_set_bd_addr_cmd(void * config, bd_addr_t addr, uint8_t *hci_cmd_buffer){
+    hci_cmd_buffer[0] = 0x01;
+    hci_cmd_buffer[1] = 0xfc;
+    hci_cmd_buffer[2] = 0x06;
+    bt_flip_addr(&hci_cmd_buffer[3], addr);
+    return 0;
+}
+
+static int bt_control_bcm_next_cmd(void *config, uint8_t *hci_cmd_buffer){
+
+    // send download firmware command
+    if (send_download_command){
+        send_download_command = 0;
+        hci_cmd_buffer[0] = 0x2e;
+        hci_cmd_buffer[1] = 0xfc;
+        hci_cmd_buffer[2] = 0x00;
+        return 1;
+    }
+
+    if (init_script_offset >= brcm_patch_ram_length) {
+        return 0;
+    }
+
+    // use memcpy with pointer
+    int cmd_len = 3 + brcm_patchram_buf[init_script_offset+2];
+    memcpy(&hci_cmd_buffer[0], &brcm_patchram_buf[init_script_offset], cmd_len); 
+    init_script_offset += cmd_len;
+    return 1; 
+}
+
+// MARK: const structs 
+
+static const bt_control_t bt_control_bcm = {
+    bt_control_bcm_on,                     // on
+    NULL,                                  // off
+    NULL,                                  // sleep
+    NULL,                                  // wake
+    NULL,                                  // valid
+    NULL,                                  // name
+    bcm_baudrate_cmd,                      // baudrate_cmd
+    bt_control_bcm_next_cmd,               // next_cmd
+    NULL,                                  // register_for_power_notifications
+    NULL,                                  // hw_error
+    bt_control_bcm_set_bd_addr_cmd,        // set_bd_addr_cmd
+};
+
+// MARK: public API
+bt_control_t * bt_control_bcm_instance(void){
+    return (bt_control_t*) &bt_control_bcm;
+}
diff --git a/chipset-bcm/bt_control_bcm.h b/chipset-bcm/bt_control_bcm.h
new file mode 100644
index 000000000..6de9f3370
--- /dev/null
+++ b/chipset-bcm/bt_control_bcm.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) 2009-2012 by Matthias Ringwald
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holders nor the names of
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ * 4. Any redistribution, use, or modification is done solely for
+ *    personal benefit and not for any commercial purpose or for
+ *    monetary gain.
+ *
+ * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
+ * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
+ * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * Please inquire about commercial licensing options at btstack@ringwald.ch
+ *
+ */
+
+/*
+ *  bt_control_bcm.h
+ *
+ *  Adapter to use Broadcom-based chipsets with BTstack
+ */
+ 
+#ifndef __BT_CONTROL_BCM_H
+#define __BT_CONTROL_BCM_H
+
+#if defined __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include "bt_control.h"
+
+bt_control_t * bt_control_bcm_instance(void);
+
+#if defined __cplusplus
+}
+#endif
+
+#endif // __BT_CONTROL_BCM_H
diff --git a/chipset/cc256x/bt_control_cc256x.c b/chipset/cc256x/bt_control_cc256x.c
index 1c059dc64..88b693fdd 100644
--- a/chipset/cc256x/bt_control_cc256x.c
+++ b/chipset/cc256x/bt_control_cc256x.c
@@ -86,12 +86,21 @@ static uint32_t init_script_offset  = 0;
 static int16_t  init_power_in_dB    = 13; // 13 dBm
 static int      init_ehcill_enabled = 0;
 
+static int      init_send_route_sco_over_hci = 0;
 
 static int bt_control_cc256x_on(void *config){
 	init_script_offset = 0;
+#ifdef HAVE_SCO_OVER_HCI
+    init_send_route_sco_over_hci = 1;
+#endif
 	return 0;
 }
 
+// route SCO over HCI (connection type=1, tx buffer size = 0x00 (don't change), tx buffer max latency=0x0000(don't chnage)), accept packets - 0)
+static const uint8_t hci_route_sco_over_hci[] = {
+    0x10, 0xfe, 0x05, 0x01, 0x00, 0x00, 0x00, 0x00
+};
+
 // UART Baud Rate control from: http://e2e.ti.com/support/low_power_rf/f/660/p/134850/484763.aspx
 static int cc256x_baudrate_cmd(void * config, uint32_t baudrate, uint8_t *hci_cmd_buffer){
     hci_cmd_buffer[0] = 0x36;
@@ -213,6 +222,12 @@ static int bt_control_cc256x_update_command(uint8_t *hci_cmd_buffer){
 static int bt_control_cc256x_next_cmd(void *config, uint8_t *hci_cmd_buffer){
 
     if (init_script_offset >= cc256x_init_script_size) {
+        // append send route SCO over HCI if requested
+        if (init_send_route_sco_over_hci){
+            init_send_route_sco_over_hci = 0;
+            memcpy(hci_cmd_buffer, hci_route_sco_over_hci, sizeof(hci_route_sco_over_hci));
+            return 1;
+        }
         return 0;
     }
     
diff --git a/example/embedded/Makefile.inc b/example/embedded/Makefile.inc
index 607ec7443..d695b84a9 100644
--- a/example/embedded/Makefile.inc
+++ b/example/embedded/Makefile.inc
@@ -72,6 +72,7 @@ EXAMPLES = 					\
 	spp_counter 			\
 	spp_streamer			\
     gap_le_advertisements   \
+    hsp_hs_test             \
 
 EXAMPLES_USING_LE =			\
 	ancs_client 			\
@@ -164,10 +165,9 @@ 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_test: ${CORE_OBJ} ${COMMON_OBJ} sdp_query_rfcomm.o sdp_parser.o sdp_client.o hsp_hs.o hsp_hs_test.c
+hsp_hs_test: ${CORE_OBJ} ${COMMON_OBJ} ${SDP_CLIENT} hsp_hs.o hsp_hs_test.c
 	${CC} $^ ${CFLAGS} ${LDFLAGS} -o $@
 
-
 clean:
 	rm -f ${EXAMPLES}
 	rm -f  *.o *.out *.hex 
diff --git a/example/embedded/hsp_hs_test.c b/example/embedded/hsp_hs_test.c
index 633da2ff5..0071a9e7d 100644
--- a/example/embedded/hsp_hs_test.c
+++ b/example/embedded/hsp_hs_test.c
@@ -39,6 +39,8 @@
 //
 // Minimal test for HSP Headset (!! UNDER DEVELOPMENT !!)
 //
+// Requires HAVE_SCO and HAVE_SCO_OVER_HCI to be defined
+//
 // Tested working setups: 
 // - Ubuntu 14 64-bit, CC2564B connected via FTDI USB-2-UART adapter, 921600 baud
 //
@@ -57,12 +59,11 @@
 #include <string.h>
 #include <math.h>
 
-#include "hci_cmds.h"
 #include "run_loop.h"
+#include "sdp_util.h"
 
-#include "classic/sdp_util.h"
-#include "classic/sdp.h"
-#include "classic/hsp_hs.h"
+#include "sdp.h"
+#include "hsp_hs.h"
 
 #include "hci.h"
 #include "l2cap.h"
@@ -140,7 +141,7 @@ static void sco_packet_handler(uint8_t packet_type, uint8_t * packet, uint16_t s
     // hexdumpf(packet, size);
 }
 
-void packet_handler(uint8_t * event, uint16_t event_size){
+static void packet_handler(uint8_t * event, uint16_t event_size){
 
     // printf("Packet handler event 0x%02x\n", event[0]);
     
@@ -149,8 +150,7 @@ void packet_handler(uint8_t * event, uint16_t event_size){
     switch (event[0]) {
         case BTSTACK_EVENT_STATE:
             if (event[2] != HCI_STATE_WORKING) break;
-            // request num completed events for SCO packets
-            hci_send_cmd(&hci_write_synchronous_flow_control_enable, 1);
+            printf("Working!\n");
             break;
         case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS:
             // printf("HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS\n");
@@ -208,6 +208,7 @@ void packet_handler(uint8_t * event, uint16_t event_size){
     }
 }
 
+int btstack_main(int argc, const char * argv[]);
 int btstack_main(int argc, const char * argv[]){
 
 #ifdef TABLE_SIZE
diff --git a/port/libusb/btstack-config.h b/port/libusb/btstack-config.h
index 8c5f79fa7..d31eb0430 100644
--- a/port/libusb/btstack-config.h
+++ b/port/libusb/btstack-config.h
@@ -3,23 +3,24 @@
 #ifndef __BTSTACK_CONFIG
 #define __BTSTACK_CONFIG
 
-#define HAVE_TRANSPORT_USB
+#define ENABLE_LOG_ERROR
+#define ENABLE_LOG_INFO 
 #define HAVE_BLE
-#define USE_POSIX_RUN_LOOP
-#define HAVE_SDP
+#define HAVE_BZERO
+#define HAVE_HCI_DUMP
+#define HAVE_MALLOC
 #define HAVE_RFCOMM
-#define REMOTE_DEVICE_DB remote_device_db_iphone
+#define HAVE_SDP
 #define HAVE_SO_NOSIGPIPE
 #define HAVE_TIME
-#define HAVE_MALLOC
-#define HAVE_BZERO
-#define SDP_DES_DUMP
-#define ENABLE_LOG_INFO 
-#define ENABLE_LOG_ERROR
-#define HCI_INCOMING_PRE_BUFFER_SIZE 14 // sizeof benep heade, avoid memcpy
+#define HAVE_SCO
+#define HAVE_SCO_OVER_HCI
 #define HCI_ACL_PAYLOAD_SIZE (1691 + 4)
-#define HAVE_HCI_DUMP
+#define HCI_INCOMING_PRE_BUFFER_SIZE 14 // sizeof BNEP header, avoid memcpy
+#define REMOTE_DEVICE_DB remote_device_db_iphone
 #define SDP_DES_DUMP
+#define SDP_DES_DUMP
+#define USE_POSIX_RUN_LOOP
 
 #define HAVE_SCO
 
diff --git a/port/posix-cc2564b/Makefile b/port/posix-cc2564b/Makefile
index 40c422b1a..c44ba471f 100644
--- a/port/posix-cc2564b/Makefile
+++ b/port/posix-cc2564b/Makefile
@@ -14,9 +14,10 @@ include ${BTSTACK_ROOT}/chipset/cc256x/Makefile.inc
 CFLAGS += -I$(BTSTACK_ROOT)/chipset/cc256x \
 		  -I$(BTSTACK_ROOT)/platform/embedded
 
-# CC = gcc-fsf-4.9
 CFLAGS  += -g -Wall 
-# CFLAGS += -Werror
+
+# 
+CFLAGS += -Wmissing-prototypes -Wstrict-prototypes -Wshadow -Werror
 
 VPATH += ${BTSTACK_ROOT}/platform/posix/src
 VPATH += ${BTSTACK_ROOT}/platform/embedded
diff --git a/port/posix-cc2564b/main.c b/port/posix-cc2564b/main.c
index ed4893a1d..73ef6169e 100644
--- a/port/posix-cc2564b/main.c
+++ b/port/posix-cc2564b/main.c
@@ -63,10 +63,10 @@
 int btstack_main(int argc, const char * argv[]);
 
 
-static hci_transport_config_uart_t config = {
+static hci_transport_config_uart_t hci_uart_config_cc256x = {
     HCI_TRANSPORT_CONFIG_UART,
     115200,
-    0,  // main baudrate
+    921600,  // main baudrate
     1,  // flow control
     NULL,
 };
@@ -97,18 +97,30 @@ int main(int argc, const char * argv[]){
 	btstack_memory_init();
     run_loop_init(run_loop_posix_get_instance());
 	    
+#if 0
+    // Ubuntu
+
+    // use logger: format HCI_DUMP_PACKETLOGGER, HCI_DUMP_BLUEZ or HCI_DUMP_STDOUT
+    hci_dump_open("hci_dump.pklg", HCI_DUMP_PACKETLOGGER);
+
+    // pick serial port
+    hci_uart_config_cc256x.device_name = "/dev/ttyUSB0";
+#else
+    // OS X
+
     // use logger: format HCI_DUMP_PACKETLOGGER, HCI_DUMP_BLUEZ or HCI_DUMP_STDOUT
     hci_dump_open("/tmp/hci_dump.pklg", HCI_DUMP_PACKETLOGGER);
 
     // pick serial port
-    config.device_name = "/dev/tty.usbserial-A900K0VK";
+    hci_uart_config_cc256x.device_name = "/dev/tty.usbserial-A900K0VK";
+#endif
 
     // init HCI
 	hci_transport_t    * transport = hci_transport_h4_posix_instance();
 	bt_control_t       * control   = bt_control_cc256x_instance();
     remote_device_db_t * remote_db = (remote_device_db_t *) &remote_device_db_fs;
         
-	hci_init(transport, &config, control, remote_db);
+	hci_init(transport, &hci_uart_config_cc256x, control, remote_db);
     
     // handle CTRL-c
     signal(SIGINT, sigint_handler);
diff --git a/src/classic/hfp.h b/src/classic/hfp.h
index 801163c1b..57327d2d1 100644
--- a/src/classic/hfp.h
+++ b/src/classic/hfp.h
@@ -255,6 +255,7 @@ typedef enum {
     HFP_CALLHELD_STATUS_CALL_ON_HOLD_AND_NO_ACTIVE_CALLS 
 } hfp_callheld_status_t;
 
+
 typedef enum {
     HFP_AG_INCOMING_CALL,
     HFP_AG_INCOMING_CALL_ACCEPTED_BY_AG,
@@ -275,7 +276,12 @@ typedef enum {
     HFP_AG_RESPONSE_AND_HOLD_REJECT_HELD_CALL_BY_AG,
     HFP_AG_RESPONSE_AND_HOLD_ACCEPT_INCOMING_CALL_BY_HF,
     HFP_AG_RESPONSE_AND_HOLD_ACCEPT_HELD_CALL_BY_HF,
-    HFP_AG_RESPONSE_AND_HOLD_REJECT_HELD_CALL_BY_HF
+    HFP_AG_RESPONSE_AND_HOLD_REJECT_HELD_CALL_BY_HF,
+    HFP_AG_CALL_HOLD_USER_BUSY,
+    HFP_AG_CALL_HOLD_RELEASE_ACTIVE_ACCEPT_HELD_OR_WAITING_CALL,
+    HFP_AG_CALL_HOLD_PARK_ACTIVE_ACCEPT_HELD_OR_WAITING_CALL,
+    HFP_AG_CALL_HOLD_ADD_HELD_CALL,
+    HFP_AG_CALL_HOLD_EXIT_AND_JOIN_CALLS
 } hfp_ag_call_event_t;
 
 
diff --git a/src/classic/hfp_ag.c b/src/classic/hfp_ag.c
index 6bd7254d6..180a5192f 100644
--- a/src/classic/hfp_ag.c
+++ b/src/classic/hfp_ag.c
@@ -76,8 +76,6 @@ static int  hfp_ag_call_hold_services_nr = 0;
 static char *hfp_ag_call_hold_services[6];
 static hfp_callback_t hfp_callback;
 
-
-static hfp_call_status_t hfp_ag_call_state;
 static hfp_callsetup_status_t hfp_ag_callsetup_state;
 static hfp_callheld_status_t hfp_ag_callheld_state;
 static hfp_response_and_hold_state_t hfp_ag_response_and_hold_state;
@@ -96,6 +94,20 @@ static void hfp_run_for_context(hfp_connection_t *context);
 static void hfp_ag_setup_audio_connection(hfp_connection_t * connection);
 static void hfp_ag_hf_start_ringing(hfp_connection_t * context);
 
+static hfp_call_status_t get_hfp_ag_call_state(void){
+    return hfp_gsm_call_status();
+}
+
+static hfp_callsetup_status_t get_hfp_ag_callsetup_state(void){
+    return hfp_ag_callsetup_state;
+    //return hfp_gsm_callsetup_status();
+}
+
+static hfp_callheld_status_t get_hfp_ag_callheld_state(void){
+    return hfp_ag_callheld_state;
+    // return hfp_gsm_callheld_status();
+}
+
 hfp_generic_status_indicator_t * get_hfp_generic_status_indicators();
 int get_hfp_generic_status_indicators_nr();
 void set_hfp_generic_status_indicators(hfp_generic_status_indicator_t * indicators, int indicator_nr);
@@ -626,12 +638,12 @@ static void hfp_ag_slc_established(hfp_connection_t * context){
     hfp_init_link_settings(context);
 
     // if active call exist, set per-connection state active, too (when audio is on)
-    if (hfp_ag_call_state == HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT){
+    if (get_hfp_ag_call_state() == HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT){
         context->call_state = HFP_CALL_W4_AUDIO_CONNECTION_FOR_ACTIVE;
     }
     // if AG is ringing, also start ringing on the HF
-    if (hfp_ag_call_state == HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS &&
-        hfp_ag_callsetup_state == HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS){
+    if (get_hfp_ag_call_state() == HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS &&
+        get_hfp_ag_callsetup_state() == HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS){
         hfp_ag_hf_start_ringing(context);
     }
 }
@@ -1044,13 +1056,20 @@ static void hfp_ag_set_callheld_state(hfp_callheld_status_t state){
     indicator->status = state;
 }
 
-static void hfp_ag_set_call_state(hfp_call_status_t state){
-    hfp_ag_call_state = state;
+static void hfp_ag_set_callheld_indicator(){
+    hfp_ag_indicator_t * indicator = get_ag_indicator_for_name("callheld");
+    if (!indicator){
+        log_error("hfp_ag_set_callheld_state: callheld indicator is missing");
+    };
+    indicator->status = hfp_gsm_callheld_status();
+}
+
+static void hfp_ag_set_call_indicator(){
     hfp_ag_indicator_t * indicator = get_ag_indicator_for_name("call");
     if (!indicator){
         log_error("hfp_ag_set_call_state: call indicator is missing");
     };
-    indicator->status = state;
+    indicator->status = hfp_gsm_call_status();
 }
 
 static void hfp_ag_stop_ringing(void){
@@ -1115,13 +1134,18 @@ static int call_setup_state_machine(hfp_connection_t * connection){
 // connection is used to identify originating HF
 static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connection){
     int indicator_index;
-
+    int callsetup_indicator_index = get_ag_indicator_index_for_name("callsetup");
+    int callheld_indicator_index = get_ag_indicator_index_for_name("callheld");
+    int call_indicator_index = get_ag_indicator_index_for_name("call");
+    
+    //printf("hfp_ag_call_sm event %d \n", event);
     switch (event){
         case HFP_AG_INCOMING_CALL:
-            switch (hfp_ag_call_state){
+            switch (get_hfp_ag_call_state()){
                 case HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS:
-                    switch (hfp_ag_callsetup_state){
+                    switch (get_hfp_ag_callsetup_state()){
                         case HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS:
+                            hfp_gsm_handle_event(HFP_AG_INCOMING_CALL);
                             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS);
                             hfp_ag_trigger_incoming_call();
                             printf("AG rings\n");
@@ -1131,8 +1155,9 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
                     }
                     break;
                 case HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT:
-                    switch (hfp_ag_callsetup_state){
+                    switch (get_hfp_ag_callsetup_state()){
                         case HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS:
+                            hfp_gsm_handle_event(HFP_AG_INCOMING_CALL);
                             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS);
                             hfp_ag_trigger_incoming_call();
                             printf("AG call waiting\n");
@@ -1146,11 +1171,12 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
         case HFP_AG_INCOMING_CALL_ACCEPTED_BY_AG:
             // clear CLIP
             clip_type = 0;
-            switch (hfp_ag_call_state){
+            switch (get_hfp_ag_call_state()){
                 case HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS:
-                    switch (hfp_ag_callsetup_state){
+                    switch (get_hfp_ag_callsetup_state()){
                         case HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS:
-                            hfp_ag_set_call_state(HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT);
+                            hfp_gsm_handle_event(HFP_AG_INCOMING_CALL_ACCEPTED_BY_AG);
+                            hfp_ag_set_call_indicator();
                             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
                             hfp_ag_ag_accept_call();
                             printf("AG answers call, accept call by GSM\n");
@@ -1160,9 +1186,10 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
                     }
                     break;
                 case HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT:
-                    switch (hfp_ag_callsetup_state){
+                    switch (get_hfp_ag_callsetup_state()){
                         case HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS:
                             printf("AG: current call is placed on hold, incoming call gets active\n");
+                            hfp_gsm_handle_event(HFP_AG_INCOMING_CALL_ACCEPTED_BY_AG);
                             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
                             hfp_ag_set_callheld_state(HFP_CALLHELD_STATUS_CALL_ON_HOLD_OR_SWAPPED);
                             hfp_ag_transfer_callsetup_state();
@@ -1176,11 +1203,12 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
             break;
         
         case HFP_AG_HELD_CALL_JOINED_BY_AG:
-            switch (hfp_ag_call_state){
+            switch (get_hfp_ag_call_state()){
                 case HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT:
-                    switch (hfp_ag_callheld_state){
+                    switch (get_hfp_ag_callheld_state()){
                         case HFP_CALLHELD_STATUS_CALL_ON_HOLD_OR_SWAPPED:
                             printf("AG: joining held call with active call\n");
+                            hfp_gsm_handle_event(HFP_AG_HELD_CALL_JOINED_BY_AG);
                             hfp_ag_set_callheld_state(HFP_CALLHELD_STATUS_NO_CALLS_HELD);
                             hfp_ag_transfer_callheld_state();
                             hfp_emit_event(hfp_callback, HFP_SUBEVENT_CONFERENCE_CALL, 0);
@@ -1197,12 +1225,13 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
         case HFP_AG_INCOMING_CALL_ACCEPTED_BY_HF:
             // clear CLIP
             clip_type = 0;
-            switch (hfp_ag_call_state){
+            switch (get_hfp_ag_call_state()){
                 case HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS:
-                    switch (hfp_ag_callsetup_state){
+                    switch (get_hfp_ag_callsetup_state()){
                         case HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS:
+                            hfp_gsm_handle_event(HFP_AG_INCOMING_CALL_ACCEPTED_BY_HF);
                             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
-                            hfp_ag_set_call_state(HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT);
+                            hfp_ag_set_call_indicator();
                             hfp_ag_hf_accept_call(connection);
                             printf("HF answers call, accept call by GSM\n");
                             hfp_emit_event(hfp_callback, HFP_CMD_CALL_ANSWERED, 0);
@@ -1217,17 +1246,18 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
             break;
 
         case HFP_AG_RESPONSE_AND_HOLD_ACCEPT_INCOMING_CALL_BY_AG:
-             // clear CLIP
+            // clear CLIP
             clip_type = 0;
-            switch (hfp_ag_call_state){
+            switch (get_hfp_ag_call_state()){
                 case HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS:
-                    switch (hfp_ag_callsetup_state){
+                    switch (get_hfp_ag_callsetup_state()){
                         case HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS:
+                            hfp_gsm_handle_event(HFP_AG_RESPONSE_AND_HOLD_ACCEPT_INCOMING_CALL_BY_AG);
                             hfp_ag_response_and_hold_active = 1;
                             hfp_ag_response_and_hold_state = HFP_RESPONSE_AND_HOLD_INCOMING_ON_HOLD;
                             hfp_ag_send_response_and_hold_state(hfp_ag_response_and_hold_state);
                             // as with regualr call
-                            hfp_ag_set_call_state(HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT);
+                            hfp_ag_set_call_indicator();
                             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
                             hfp_ag_ag_accept_call();
                             printf("AG response and hold - hold by AG\n");
@@ -1242,17 +1272,18 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
             break;
 
         case HFP_AG_RESPONSE_AND_HOLD_ACCEPT_INCOMING_CALL_BY_HF:
-             // clear CLIP
+            // clear CLIP
             clip_type = 0;
-            switch (hfp_ag_call_state){
+            switch (get_hfp_ag_call_state()){
                 case HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS:
-                    switch (hfp_ag_callsetup_state){
+                    switch (get_hfp_ag_callsetup_state()){
                         case HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS:
+                            hfp_gsm_handle_event(HFP_AG_RESPONSE_AND_HOLD_ACCEPT_INCOMING_CALL_BY_HF);
                             hfp_ag_response_and_hold_active = 1;
                             hfp_ag_response_and_hold_state = HFP_RESPONSE_AND_HOLD_INCOMING_ON_HOLD;
                             hfp_ag_send_response_and_hold_state(hfp_ag_response_and_hold_state);
                             // as with regualr call
-                            hfp_ag_set_call_state(HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT);
+                            hfp_ag_set_call_indicator();
                             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
                             hfp_ag_hf_accept_call(connection);
                             printf("AG response and hold - hold by HF\n");
@@ -1270,6 +1301,7 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
         case HFP_AG_RESPONSE_AND_HOLD_ACCEPT_HELD_CALL_BY_HF:
             if (!hfp_ag_response_and_hold_active) break;
             if (hfp_ag_response_and_hold_state != HFP_RESPONSE_AND_HOLD_INCOMING_ON_HOLD) break;
+            hfp_gsm_handle_event(HFP_AG_RESPONSE_AND_HOLD_ACCEPT_HELD_CALL_BY_AG);
             hfp_ag_response_and_hold_active = 0;
             hfp_ag_response_and_hold_state = HFP_RESPONSE_AND_HOLD_HELD_INCOMING_ACCEPTED;
             hfp_ag_send_response_and_hold_state(hfp_ag_response_and_hold_state);
@@ -1280,21 +1312,23 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
         case HFP_AG_RESPONSE_AND_HOLD_REJECT_HELD_CALL_BY_HF:
             if (!hfp_ag_response_and_hold_active) break;
             if (hfp_ag_response_and_hold_state != HFP_RESPONSE_AND_HOLD_INCOMING_ON_HOLD) break;
+            hfp_gsm_handle_event(HFP_AG_RESPONSE_AND_HOLD_REJECT_HELD_CALL_BY_AG);
             hfp_ag_response_and_hold_active = 0;
             hfp_ag_response_and_hold_state = HFP_RESPONSE_AND_HOLD_HELD_INCOMING_REJECTED;
             hfp_ag_send_response_and_hold_state(hfp_ag_response_and_hold_state);
             // from terminate by ag
-            hfp_ag_set_call_state(HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS);
+            hfp_ag_set_call_indicator();
             hfp_ag_trigger_terminate_call();
             break;
 
         case HFP_AG_TERMINATE_CALL_BY_HF:
             // clear CLIP
             clip_type = 0;
-            switch (hfp_ag_call_state){
+            switch (get_hfp_ag_call_state()){
                 case HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS:
-                    switch (hfp_ag_callsetup_state){
+                    switch (get_hfp_ag_callsetup_state()){
                         case HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS:
+                            hfp_gsm_handle_event(HFP_AG_TERMINATE_CALL_BY_HF);
                             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
                             hfp_ag_transfer_callsetup_state();
                             hfp_ag_trigger_reject_call();
@@ -1302,6 +1336,7 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
                             break;
                         case HFP_CALLSETUP_STATUS_OUTGOING_CALL_SETUP_IN_DIALING_STATE:
                         case HFP_CALLSETUP_STATUS_OUTGOING_CALL_SETUP_IN_ALERTING_STATE:
+                            hfp_gsm_handle_event(HFP_AG_TERMINATE_CALL_BY_HF);
                             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
                             hfp_ag_transfer_callsetup_state();
                             printf("AG terminate outgoing call process\n");                            
@@ -1310,7 +1345,8 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
                     }
                     break;
                 case HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT:
-                    hfp_ag_set_call_state(HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS);
+                    hfp_gsm_handle_event(HFP_AG_TERMINATE_CALL_BY_HF);
+                    hfp_ag_set_call_indicator();
                     hfp_ag_transfer_call_state();
                     connection->call_state = HFP_CALL_IDLE;
                     printf("AG terminate call\n");
@@ -1321,10 +1357,11 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
         case HFP_AG_TERMINATE_CALL_BY_AG:
             // clear CLIP
             clip_type = 0;
-            switch (hfp_ag_call_state){
+            switch (get_hfp_ag_call_state()){
                 case HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS:
-                    switch (hfp_ag_callsetup_state){
+                    switch (get_hfp_ag_callsetup_state()){
                         case HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS:
+                            hfp_gsm_handle_event(HFP_AG_TERMINATE_CALL_BY_AG);
                             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
                             hfp_ag_trigger_reject_call();
                             printf("AG Rejected Incoming call, AG terminate call\n");
@@ -1333,8 +1370,9 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
                             break;
                     }
                 case HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT:
+                    hfp_gsm_handle_event(HFP_AG_TERMINATE_CALL_BY_AG);
                     hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
-                    hfp_ag_set_call_state(HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS);
+                    hfp_ag_set_call_indicator();
                     hfp_ag_trigger_terminate_call();
                     printf("AG terminate call\n");
                     break;
@@ -1345,9 +1383,9 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
         case HFP_AG_CALL_DROPPED:
             // clear CLIP
             clip_type = 0;
-            switch (hfp_ag_call_state){
+            switch (get_hfp_ag_call_state()){
                 case HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS:
-                    switch (hfp_ag_callsetup_state){
+                    switch (get_hfp_ag_callsetup_state()){
                         case HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS:
                             hfp_ag_stop_ringing();
                             printf("Incoming call interrupted\n");
@@ -1360,16 +1398,19 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
                         default:
                             break;
                     }
+                    hfp_gsm_handle_event(HFP_AG_CALL_DROPPED);
                     hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
                     hfp_ag_transfer_callsetup_state();
                     break;
                 case HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT:
                     if (hfp_ag_response_and_hold_active) {
+                        hfp_gsm_handle_event(HFP_AG_CALL_DROPPED);
                         hfp_ag_response_and_hold_state = HFP_RESPONSE_AND_HOLD_HELD_INCOMING_REJECTED;
                         hfp_ag_send_response_and_hold_state(hfp_ag_response_and_hold_state);
                     }
+                    hfp_gsm_handle_event(HFP_AG_CALL_DROPPED);
                     hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
-                    hfp_ag_set_call_state(HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS);
+                    hfp_ag_set_call_indicator();
                     hfp_ag_trigger_terminate_call();
                     printf("AG notify call dropped\n");
                     break;
@@ -1379,6 +1420,12 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
             break;
 
         case HFP_AG_OUTGOING_CALL_INITIATED:
+            // directly reject call if number of free slots is exceeded
+            if (!hfp_gsm_call_possible()){
+                connection->send_error = 1;
+                hfp_run_for_context(connection);  
+                break;
+            }
             hfp_gsm_handle_event(HFP_AG_OUTGOING_CALL_INITIATED);
             connection->call_state = HFP_CALL_OUTGOING_INITIATED;
 
@@ -1386,6 +1433,13 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
             break;
 
         case HFP_AG_OUTGOING_REDIAL_INITIATED:
+            // directly reject call if number of free slots is exceeded
+            if (!hfp_gsm_call_possible()){
+                connection->send_error = 1;
+                hfp_run_for_context(connection);  
+                break;
+            }
+
             hfp_gsm_handle_event(HFP_AG_OUTGOING_REDIAL_INITIATED);
             connection->call_state = HFP_CALL_OUTGOING_INITIATED;
 
@@ -1393,35 +1447,38 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
             break;
 
         case HFP_AG_OUTGOING_CALL_REJECTED:
-            hfp_gsm_handle_event(HFP_AG_OUTGOING_CALL_REJECTED);
             connection = hfp_ag_connection_for_call_state(HFP_CALL_OUTGOING_INITIATED);
             if (!connection){
                 log_info("hfp_ag_call_sm: did not find outgoing connection in initiated state");
                 break;
             }
+            
+            hfp_gsm_handle_event(HFP_AG_OUTGOING_CALL_REJECTED);
             connection->call_state = HFP_CALL_IDLE;
             connection->send_error = 1;
             hfp_run_for_context(connection);
             break;
 
-        case HFP_AG_OUTGOING_CALL_ACCEPTED:
-            // hfp_gsm_handle_event();
+        case HFP_AG_OUTGOING_CALL_ACCEPTED:{
             connection = hfp_ag_connection_for_call_state(HFP_CALL_OUTGOING_INITIATED);
             if (!connection){
                 log_info("hfp_ag_call_sm: did not find outgoing connection in initiated state");
                 break;
             }
-
+            
             connection->ok_pending = 1;
             connection->call_state = HFP_CALL_OUTGOING_DIALING;
 
             // trigger callsetup to be
+            int put_call_on_hold = get_hfp_ag_call_state() == HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT;
+            hfp_gsm_handle_event(HFP_AG_OUTGOING_CALL_ACCEPTED);
+
             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_OUTGOING_CALL_SETUP_IN_DIALING_STATE);
             indicator_index = get_ag_indicator_index_for_name("callsetup");
             connection->ag_indicators_status_update_bitmap = store_bit(connection->ag_indicators_status_update_bitmap, indicator_index, 1);
 
             // put current call on hold if active
-            if (hfp_ag_call_state == HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT){
+            if (put_call_on_hold){
                 printf("AG putting current call on hold for new outgoing call\n");
                 hfp_ag_set_callheld_state(HFP_CALLHELD_STATUS_CALL_ON_HOLD_AND_NO_ACTIVE_CALLS);
                 indicator_index = get_ag_indicator_index_for_name("callheld");
@@ -1431,7 +1488,7 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
             // start audio if needed
             hfp_ag_establish_audio_connection(connection->remote_addr);
             break;
-
+        }
         case HFP_AG_OUTGOING_CALL_RINGING:
             // hfp_gsm_handle_event();
             connection = hfp_ag_connection_for_call_state(HFP_CALL_OUTGOING_DIALING);
@@ -1439,13 +1496,14 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
                 log_info("hfp_ag_call_sm: did not find outgoing connection in dialing state");
                 break;
             }
+            
+            hfp_gsm_handle_event(HFP_AG_OUTGOING_CALL_RINGING);
             connection->call_state = HFP_CALL_OUTGOING_RINGING;
             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_OUTGOING_CALL_SETUP_IN_ALERTING_STATE);
             hfp_ag_transfer_callsetup_state();
             break;
 
-        case HFP_AG_OUTGOING_CALL_ESTABLISHED:
-            // hfp_gsm_handle_event();
+        case HFP_AG_OUTGOING_CALL_ESTABLISHED:{
             // get outgoing call
             connection = hfp_ag_connection_for_call_state(HFP_CALL_OUTGOING_RINGING);
             if (!connection){
@@ -1455,20 +1513,94 @@ static void hfp_ag_call_sm(hfp_ag_call_event_t event, hfp_connection_t * connect
                 log_info("hfp_ag_call_sm: did not find outgoing connection");
                 break;
             }
+
+            int CALLHELD_STATUS_CALL_ON_HOLD_AND_NO_ACTIVE_CALLS = get_hfp_ag_callheld_state() == HFP_CALLHELD_STATUS_CALL_ON_HOLD_AND_NO_ACTIVE_CALLS;
+            hfp_gsm_handle_event(HFP_AG_OUTGOING_CALL_ESTABLISHED);
             connection->call_state = HFP_CALL_ACTIVE;
             hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
-            hfp_ag_set_call_state(HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT);
+            hfp_ag_set_call_indicator();
             hfp_ag_transfer_call_state();
             hfp_ag_transfer_callsetup_state();
-            if (hfp_ag_callheld_state == HFP_CALLHELD_STATUS_CALL_ON_HOLD_AND_NO_ACTIVE_CALLS){
+            if (CALLHELD_STATUS_CALL_ON_HOLD_AND_NO_ACTIVE_CALLS){
                 hfp_ag_set_callheld_state(HFP_CALLHELD_STATUS_CALL_ON_HOLD_OR_SWAPPED);
                 hfp_ag_transfer_callheld_state();
             }
             break;
+        }
 
+        case HFP_AG_CALL_HOLD_USER_BUSY:
+            hfp_gsm_handle_event(HFP_AG_CALL_HOLD_USER_BUSY);
+            hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
+            connection->ag_indicators_status_update_bitmap = store_bit(connection->ag_indicators_status_update_bitmap, callsetup_indicator_index, 1);
+            connection->call_state = HFP_CALL_ACTIVE;
+            printf("AG: Call Waiting, User Busy\n");
+            break;
+        
+        case HFP_AG_CALL_HOLD_RELEASE_ACTIVE_ACCEPT_HELD_OR_WAITING_CALL:{
+            int call_setup_in_progress = get_hfp_ag_callsetup_state() != HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+            int call_held = get_hfp_ag_callheld_state() != HFP_CALLHELD_STATUS_NO_CALLS_HELD;
+
+            // Releases all active calls (if any exist) and accepts the other (held or waiting) call.
+            if (call_setup_in_progress){
+                printf("AG: Call Dropped, Accept new call\n");
+                hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
+                connection->ag_indicators_status_update_bitmap = store_bit(connection->ag_indicators_status_update_bitmap, callsetup_indicator_index, 1);
+            } else {
+                printf("AG: Call Dropped, Resume held call\n");
+            }
+            if (call_held){
+                hfp_ag_set_callheld_state(HFP_CALLHELD_STATUS_NO_CALLS_HELD);
+                connection->ag_indicators_status_update_bitmap = store_bit(connection->ag_indicators_status_update_bitmap, callheld_indicator_index, 1);
+            }
+            if (call_held || call_setup_in_progress){
+                hfp_gsm_handle_event(HFP_AG_CALL_HOLD_RELEASE_ACTIVE_ACCEPT_HELD_OR_WAITING_CALL);
+            }
+            connection->call_state = HFP_CALL_ACTIVE;
+            break;
+        }
+        case HFP_AG_CALL_HOLD_PARK_ACTIVE_ACCEPT_HELD_OR_WAITING_CALL:{
+            int call_setup_in_progress = get_hfp_ag_callsetup_state() != HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+            // Places all active calls (if any exist) on hold and accepts the other (held or waiting) call.
+            // only update if callsetup changed
+            if (call_setup_in_progress){
+                printf("AG: Call on Hold, Accept new call\n");
+                hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
+                connection->ag_indicators_status_update_bitmap = store_bit(connection->ag_indicators_status_update_bitmap, callsetup_indicator_index, 1);
+            } else {
+                printf("AG: Swap calls\n");
+            }
+            hfp_gsm_handle_event(HFP_AG_CALL_HOLD_PARK_ACTIVE_ACCEPT_HELD_OR_WAITING_CALL);
+            hfp_ag_set_callheld_state(HFP_CALLHELD_STATUS_CALL_ON_HOLD_OR_SWAPPED);
+            connection->ag_indicators_status_update_bitmap = store_bit(connection->ag_indicators_status_update_bitmap, callheld_indicator_index, 1);
+            connection->call_state = HFP_CALL_ACTIVE;
+            break;
+        }
+        case HFP_AG_CALL_HOLD_ADD_HELD_CALL:
+            // Adds a held call to the conversation.
+            if (get_hfp_ag_callheld_state() != HFP_CALLHELD_STATUS_NO_CALLS_HELD){
+                printf("AG: Join 3-way-call\n");
+                hfp_gsm_handle_event(HFP_AG_CALL_HOLD_ADD_HELD_CALL);
+                hfp_ag_set_callheld_state(HFP_CALLHELD_STATUS_NO_CALLS_HELD);
+                connection->ag_indicators_status_update_bitmap = store_bit(connection->ag_indicators_status_update_bitmap, callheld_indicator_index, 1);
+                hfp_emit_event(hfp_callback, HFP_SUBEVENT_CONFERENCE_CALL, 0);
+            }
+            connection->call_state = HFP_CALL_ACTIVE;
+            break;
+        case HFP_AG_CALL_HOLD_EXIT_AND_JOIN_CALLS:
+            // Connects the two calls and disconnects the subscriber from both calls (Explicit Call Transfer)
+            hfp_gsm_handle_event(HFP_AG_CALL_HOLD_EXIT_AND_JOIN_CALLS);
+            printf("AG: Transfer call -> Connect two calls and disconnect\n");
+            hfp_ag_set_call_indicator();
+            hfp_ag_set_callheld_state(HFP_CALLHELD_STATUS_NO_CALLS_HELD);
+            connection->ag_indicators_status_update_bitmap = store_bit(connection->ag_indicators_status_update_bitmap, call_indicator_index, 1);
+            connection->ag_indicators_status_update_bitmap = store_bit(connection->ag_indicators_status_update_bitmap, callheld_indicator_index, 1);
+            connection->call_state = HFP_CALL_IDLE;
+            break;
+        
         default:
             break;
     }
+   
 }
 
 static void hfp_run_for_context(hfp_connection_t *context){
@@ -1739,73 +1871,24 @@ static void hfp_handle_rfcomm_data(uint8_t packet_type, uint16_t channel, uint8_
         case HFP_CMD_CALL_HOLD: {
             // TODO: fully implement this
             log_error("HFP: unhandled call hold type %c", context->line_buffer[0]);
-            int callsetup_indicator_index = get_ag_indicator_index_for_name("callsetup");
-            int callheld_indicator_index = get_ag_indicator_index_for_name("callheld");
-            int call_indicator_index = get_ag_indicator_index_for_name("call");
+            context->command = HFP_CMD_NONE;
+            context->ok_pending = 1;
+                    
             switch (context->line_buffer[0]){
                 case '0':
-                    context->command = HFP_CMD_NONE;
-                    context->ok_pending = 1;
-                    hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
-                    context->ag_indicators_status_update_bitmap = store_bit(context->ag_indicators_status_update_bitmap, callsetup_indicator_index, 1);
-                    context->call_state = HFP_CALL_ACTIVE;
-                    printf("AG: Call Waiting, User Busy\n");
+                    hfp_ag_call_sm(HFP_AG_CALL_HOLD_USER_BUSY, context);
                     break;
                 case '1':
-                    // Releases all active calls (if any exist) and accepts the other (held or waiting) call.
-                    context->command = HFP_CMD_NONE;
-                    context->ok_pending = 1;
-                    if (hfp_ag_callsetup_state != HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS){
-                        printf("AG: Call Dropped, Accept new call\n");
-                        hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
-                        context->ag_indicators_status_update_bitmap = store_bit(context->ag_indicators_status_update_bitmap, callsetup_indicator_index, 1);
-                    } else {
-                        printf("AG: Call Dropped, Resume held call\n");
-                    }
-                    if (hfp_ag_callheld_state != HFP_CALLHELD_STATUS_NO_CALLS_HELD){
-                        hfp_ag_set_callheld_state(HFP_CALLHELD_STATUS_NO_CALLS_HELD);
-                        context->ag_indicators_status_update_bitmap = store_bit(context->ag_indicators_status_update_bitmap, callheld_indicator_index, 1);
-                    }
-                    context->call_state = HFP_CALL_ACTIVE;
+                    hfp_ag_call_sm(HFP_AG_CALL_HOLD_RELEASE_ACTIVE_ACCEPT_HELD_OR_WAITING_CALL, context);
                     break;
                 case '2':
-                    // Places all active calls (if any exist) on hold and accepts the other (held or waiting) call.
-                    context->command = HFP_CMD_NONE;
-                    context->ok_pending = 1;
-                    // only update if callsetup changed
-                    if (hfp_ag_callsetup_state != HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS){
-                        printf("AG: Call on Hold, Accept new call\n");
-                        hfp_ag_set_callsetup_state(HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS);
-                        context->ag_indicators_status_update_bitmap = store_bit(context->ag_indicators_status_update_bitmap, callsetup_indicator_index, 1);
-                    } else {
-                        printf("AG: Swap calls\n");
-                    }
-                    hfp_ag_set_callheld_state(HFP_CALLHELD_STATUS_CALL_ON_HOLD_OR_SWAPPED);
-                    context->ag_indicators_status_update_bitmap = store_bit(context->ag_indicators_status_update_bitmap, callheld_indicator_index, 1);
-                    context->call_state = HFP_CALL_ACTIVE;
+                    hfp_ag_call_sm(HFP_AG_CALL_HOLD_PARK_ACTIVE_ACCEPT_HELD_OR_WAITING_CALL, context);
                     break;
                 case '3':
-                    // Adds a held call to the conversation.
-                    context->command = HFP_CMD_NONE;
-                    context->ok_pending = 1;
-                    if (hfp_ag_callheld_state != HFP_CALLHELD_STATUS_NO_CALLS_HELD){
-                        printf("AG: Join 3-way-call\n");
-                        hfp_ag_set_callheld_state(HFP_CALLHELD_STATUS_NO_CALLS_HELD);
-                        context->ag_indicators_status_update_bitmap = store_bit(context->ag_indicators_status_update_bitmap, callheld_indicator_index, 1);
-                        hfp_emit_event(hfp_callback, HFP_SUBEVENT_CONFERENCE_CALL, 0);
-                    }
-                    context->call_state = HFP_CALL_ACTIVE;
+                    hfp_ag_call_sm(HFP_AG_CALL_HOLD_ADD_HELD_CALL, context);
                     break;
                 case '4':
-                    // Connects the two calls and disconnects the subscriber from both calls (Explicit Call Transfer)
-                    context->command = HFP_CMD_NONE;
-                    context->ok_pending = 1;
-                    printf("AG: Transfer call -> Connect two calls and disconnect\n");
-                    hfp_ag_set_call_state(HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS);
-                    hfp_ag_set_callheld_state(HFP_CALLHELD_STATUS_NO_CALLS_HELD);
-                    context->ag_indicators_status_update_bitmap = store_bit(context->ag_indicators_status_update_bitmap, call_indicator_index, 1);
-                    context->ag_indicators_status_update_bitmap = store_bit(context->ag_indicators_status_update_bitmap, callheld_indicator_index, 1);
-                    context->call_state = HFP_CALL_IDLE;
+                    hfp_ag_call_sm(HFP_AG_CALL_HOLD_EXIT_AND_JOIN_CALLS, context);
                     break;
                 default:
                     break;
@@ -1907,9 +1990,17 @@ void hfp_ag_init(uint16_t rfcomm_channel_nr, uint32_t supported_features,
     hfp_ag_call_hold_services_nr = call_hold_services_nr;
     memcpy(hfp_ag_call_hold_services, call_hold_services, call_hold_services_nr * sizeof(char *));
 
-    hfp_ag_call_state = HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS;
     hfp_ag_callsetup_state = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
     hfp_ag_callheld_state = HFP_CALLHELD_STATUS_NO_CALLS_HELD;
+
+
+    hfp_ag_response_and_hold_active = 0;
+    clip_type = 0;       // 0 == not set
+    memset(clip_number,0,sizeof(clip_number));
+    subscriber_numbers = NULL;
+    subscriber_numbers_count = 0;
+
+    hfp_gsm_init();
 }
 
 void hfp_ag_establish_service_level_connection(bd_addr_t bd_addr){
diff --git a/src/classic/hfp_gsm_model.c b/src/classic/hfp_gsm_model.c
index 110358ae7..fd241e389 100644
--- a/src/classic/hfp_gsm_model.c
+++ b/src/classic/hfp_gsm_model.c
@@ -64,6 +64,8 @@
 
 typedef enum{
     CALL_NONE,
+    CALL_INITIATED,
+    CALL_RESPONSE_HOLD,
     CALL_ACTIVE,
     CALL_HELD
 } hfp_gsm_call_status_t;
@@ -71,15 +73,21 @@ typedef enum{
 
 typedef struct {
     hfp_gsm_call_status_t status;
+    uint8_t clip_type;
+    char    clip_number[25];
 } hfp_gsm_call_t;
-
-
-// 
+ 
 static hfp_gsm_call_t gsm_calls[HFP_GSM_MAX_NR_CALLS]; 
-
-//
 static hfp_callsetup_status_t callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
 
+void hfp_gsm_init(void){
+    memset(gsm_calls, 0, sizeof(gsm_calls));
+    int i;
+    for (i = 0; i < HFP_GSM_MAX_NR_CALLS; i++){
+        gsm_calls[i].status = CALL_NONE;
+    }
+    callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+} 
 
 static int get_number_calls_with_status(hfp_gsm_call_status_t status){
     int i, count = 0;
@@ -105,9 +113,21 @@ static inline int get_active_call_index(){
     return get_call_index_with_status(CALL_ACTIVE);
 }
 
-// static inline int get_number_none_calls(){
-//     return get_number_calls_with_status(CALL_NONE);
-// }
+static inline int get_initiated_call_index(){
+    return get_call_index_with_status(CALL_INITIATED);
+}
+
+static inline int get_held_call_index(){
+    return get_call_index_with_status(CALL_HELD);
+}
+
+static inline int get_response_held_call_index(){
+    return get_call_index_with_status(CALL_RESPONSE_HOLD);
+}
+
+static inline int get_number_none_calls(){
+    return get_number_calls_with_status(CALL_NONE);
+}
 
 static inline int get_number_active_calls(){
     return get_number_calls_with_status(CALL_ACTIVE);
@@ -117,8 +137,12 @@ static inline int get_number_held_calls(){
     return get_number_calls_with_status(CALL_HELD);
 }
 
+static inline int get_number_response_held_calls(){
+    return get_number_calls_with_status(CALL_RESPONSE_HOLD);
+}
+
 hfp_call_status_t hfp_gsm_call_status(){
-    if (get_number_active_calls() + get_number_held_calls()){
+    if (get_number_active_calls() + get_number_held_calls() + get_number_response_held_calls()){
         return HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT;
     }
     return HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS;
@@ -139,33 +163,224 @@ hfp_callsetup_status_t hfp_gsm_callsetup_status(){
     return callsetup_status;
 }
 
+int hfp_gsm_response_held_active(){
+    return get_response_held_call_index() != -1 ;
+}
+
+int hfp_gsm_call_possible(void){
+    return get_number_none_calls() > 0;
+}
+
 void hfp_gsm_handle_event(hfp_ag_call_event_t event){
     int next_free_slot = get_none_call_index();
     int current_call_index = get_active_call_index();
-                
+    int initiated_call_index = get_initiated_call_index();
+    int held_call_index = get_held_call_index();
+    printf("hfp_gsm_handle_event %d \n", event);
     switch (event){
         case HFP_AG_OUTGOING_CALL_INITIATED:
         case HFP_AG_OUTGOING_REDIAL_INITIATED:
-
             if (next_free_slot == -1){
-                log_error("max nr gsm call exceeded");
+                log_error("gsm: max call nr exceeded");
                 return;
             }
+            break;
 
+        case HFP_AG_OUTGOING_CALL_REJECTED:
+            if (current_call_index != -1){
+                gsm_calls[current_call_index].status = CALL_NONE;
+            }
+            callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+            break;
+
+        case HFP_AG_OUTGOING_CALL_ACCEPTED:
             if (current_call_index != -1){
                 gsm_calls[current_call_index].status = CALL_HELD;
             }
-            gsm_calls[next_free_slot].status = CALL_ACTIVE;
+            gsm_calls[next_free_slot].status = CALL_INITIATED;
             callsetup_status = HFP_CALLSETUP_STATUS_OUTGOING_CALL_SETUP_IN_DIALING_STATE;
             break;
-        case HFP_AG_OUTGOING_CALL_REJECTED:
-            break;
-        case HFP_AG_OUTGOING_CALL_ACCEPTED:
-            break;
+        
         case HFP_AG_OUTGOING_CALL_RINGING:
+            if (current_call_index == -1){
+                log_error("gsm: no active call");
+                return;
+            }
+            callsetup_status = HFP_CALLSETUP_STATUS_OUTGOING_CALL_SETUP_IN_ALERTING_STATE;
             break;
         case HFP_AG_OUTGOING_CALL_ESTABLISHED:
+            callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+            gsm_calls[initiated_call_index].status = CALL_ACTIVE;
             break;
+
+        case HFP_AG_INCOMING_CALL:
+            if (hfp_gsm_callsetup_status() != HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS) break;
+            callsetup_status = HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS;
+            gsm_calls[next_free_slot].status = CALL_INITIATED;
+            gsm_calls[next_free_slot].clip_type = 0;
+            break;
+        
+        case HFP_AG_INCOMING_CALL_ACCEPTED_BY_AG:
+            if (hfp_gsm_callsetup_status() != HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS) break;
+            callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+            
+            if (hfp_gsm_call_status() == HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT){
+                gsm_calls[current_call_index].status = CALL_HELD;
+            }
+            gsm_calls[initiated_call_index].status = CALL_ACTIVE;
+            break;
+
+        case HFP_AG_HELD_CALL_JOINED_BY_AG:
+            if (hfp_gsm_call_status() != HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT) break;
+            
+            // TODO: mark joined calls with "multiparty flag" (if we cannot calculate it otherwise)
+            // TODO: is following condition correct? Can we join incoming call before it is answered?
+            if (callsetup_status == HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS){
+                gsm_calls[initiated_call_index].status = CALL_ACTIVE;
+                callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+                break;
+            }
+
+            if (hfp_gsm_callheld_status() == HFP_CALLHELD_STATUS_CALL_ON_HOLD_OR_SWAPPED) {
+                gsm_calls[held_call_index].status = CALL_ACTIVE;
+                break;
+            } 
+            break;
+
+        case HFP_AG_INCOMING_CALL_ACCEPTED_BY_HF:
+            if (hfp_gsm_callsetup_status() != HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS) break;
+            if (hfp_gsm_call_status() != HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS) break;
+            callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+            gsm_calls[initiated_call_index].status = CALL_ACTIVE;
+            break;
+
+        case HFP_AG_RESPONSE_AND_HOLD_ACCEPT_INCOMING_CALL_BY_AG:
+        case HFP_AG_RESPONSE_AND_HOLD_ACCEPT_INCOMING_CALL_BY_HF:
+            if (hfp_gsm_callsetup_status() != HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS) break;
+            if (hfp_gsm_call_status() != HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS) break;
+            callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+            gsm_calls[initiated_call_index].status = CALL_RESPONSE_HOLD; 
+            break;
+
+        case HFP_AG_RESPONSE_AND_HOLD_ACCEPT_HELD_CALL_BY_AG:
+        case HFP_AG_RESPONSE_AND_HOLD_ACCEPT_HELD_CALL_BY_HF:
+            if (!hfp_gsm_response_held_active()) break;
+            gsm_calls[get_response_held_call_index()].status = CALL_ACTIVE;
+            break;
+
+        case HFP_AG_RESPONSE_AND_HOLD_REJECT_HELD_CALL_BY_AG:
+        case HFP_AG_RESPONSE_AND_HOLD_REJECT_HELD_CALL_BY_HF:
+            if (!hfp_gsm_response_held_active()) break;
+            gsm_calls[get_response_held_call_index()].status = CALL_NONE;
+            break;
+
+            
+        case HFP_AG_TERMINATE_CALL_BY_HF:
+            switch (hfp_gsm_call_status()){
+                case HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS:
+                    callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+                    break;
+                case HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT:
+                    gsm_calls[current_call_index].status = CALL_NONE;
+                    break;
+            }
+            break;
+
+        case HFP_AG_TERMINATE_CALL_BY_AG:
+            switch (hfp_gsm_call_status()){
+                case HFP_CALL_STATUS_NO_HELD_OR_ACTIVE_CALLS:
+                    if (hfp_gsm_callsetup_status() != HFP_CALLSETUP_STATUS_INCOMING_CALL_SETUP_IN_PROGRESS) break;
+                    callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+                    break;
+                case HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT:
+                    callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+                    gsm_calls[current_call_index].status = CALL_NONE;
+                    break;
+                default:
+                    break;
+            }
+            break;
+
+        case HFP_AG_CALL_DROPPED:{
+                callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+                if (hfp_gsm_call_status() != HFP_CALL_STATUS_ACTIVE_OR_HELD_CALL_IS_PRESENT) break;
+                
+                int i ;
+                for (i = 0; i < HFP_GSM_MAX_NR_CALLS; i++){
+                    gsm_calls[i].status = CALL_NONE;
+                    gsm_calls[i].clip_type = 0;
+                    gsm_calls[i].clip_number[0] = '\0';
+                }
+            }
+            break;
+        case HFP_AG_CALL_HOLD_USER_BUSY:
+            // Held or waiting call gets active, 
+            callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+            gsm_calls[initiated_call_index].status = CALL_NONE;
+            gsm_calls[held_call_index].status = CALL_ACTIVE;
+            break;
+        
+        case HFP_AG_CALL_HOLD_RELEASE_ACTIVE_ACCEPT_HELD_OR_WAITING_CALL:{
+            int i ;
+            for (i = 0; i < HFP_GSM_MAX_NR_CALLS; i++){
+                if (gsm_calls[i].status == CALL_ACTIVE){
+                    gsm_calls[i].clip_type = 0;
+                    gsm_calls[i].clip_number[0] = '\0';
+                    gsm_calls[i].status = CALL_NONE;
+                }
+            }
+
+            if (callsetup_status != HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS){
+                gsm_calls[initiated_call_index].status = CALL_ACTIVE;
+            } else {
+                gsm_calls[held_call_index].status = CALL_ACTIVE;
+            }
+            callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+            break;
+        }
+        case HFP_AG_CALL_HOLD_PARK_ACTIVE_ACCEPT_HELD_OR_WAITING_CALL:{
+            int i ;
+            for (i = 0; i < HFP_GSM_MAX_NR_CALLS; i++){
+                if (gsm_calls[i].status == CALL_ACTIVE){
+                    gsm_calls[i].clip_type = 0;
+                    gsm_calls[i].clip_number[0] = '\0';
+                    gsm_calls[i].status = CALL_HELD;
+                }
+            }
+
+            if (callsetup_status != HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS){
+                gsm_calls[initiated_call_index].status = CALL_ACTIVE;
+            } else {
+                gsm_calls[held_call_index].status = CALL_ACTIVE;
+            }
+            callsetup_status = HFP_CALLSETUP_STATUS_NO_CALL_SETUP_IN_PROGRESS;
+            break;
+        }            
+        case HFP_AG_CALL_HOLD_ADD_HELD_CALL:{
+            if (hfp_gsm_callheld_status() != HFP_CALLHELD_STATUS_NO_CALLS_HELD){
+                int i ;
+                for (i = 0; i < HFP_GSM_MAX_NR_CALLS; i++){
+                    if (gsm_calls[i].status == CALL_HELD){
+                        gsm_calls[i].clip_type = 0;
+                        gsm_calls[i].clip_number[0] = '\0';
+                        gsm_calls[i].status = CALL_ACTIVE;
+                    }
+                }
+            }
+            gsm_calls[initiated_call_index].status = CALL_ACTIVE;
+
+            break;
+        }
+        case HFP_AG_CALL_HOLD_EXIT_AND_JOIN_CALLS:{
+            int i ;
+            for (i = 0; i < HFP_GSM_MAX_NR_CALLS; i++){
+                gsm_calls[i].clip_type = 0;
+                gsm_calls[i].clip_number[0] = '\0';
+                gsm_calls[i].status = CALL_NONE;
+            }
+        
+            break;
+        }
         default:
             break;
     }
diff --git a/src/classic/hfp_gsm_model.h b/src/classic/hfp_gsm_model.h
index dd72c3d7b..f19ba026e 100644
--- a/src/classic/hfp_gsm_model.h
+++ b/src/classic/hfp_gsm_model.h
@@ -63,6 +63,10 @@ hfp_callheld_status_t hfp_gsm_callheld_status();
 hfp_call_status_t hfp_gsm_call_status();
 hfp_callsetup_status_t hfp_gsm_callsetup_status();
 
+int hfp_gsm_call_possible(void);
+
+void hfp_gsm_init(void);
+
 void hfp_gsm_handle_event(hfp_ag_call_event_t event);
 
 // /**
diff --git a/src/classic/hsp_hs.c b/src/classic/hsp_hs.c
index c27fe7cf5..2d8a1c828 100644
--- a/src/classic/hsp_hs.c
+++ b/src/classic/hsp_hs.c
@@ -168,7 +168,7 @@ void hsp_hs_create_service(uint8_t * service, uint32_t service_record_handle, in
     de_add_number(service,  DE_UINT, DE_SIZE_16, SDP_ServiceClassIDList);
     attribute = de_push_sequence(service);
     {
-        //  "UUID for PAN Service" / see Bluetooth Erratum #3507
+        //  see Bluetooth Erratum #3507
         de_add_number(attribute, DE_UUID, DE_SIZE_16, SDP_HSP);          // 0x1108
         de_add_number(attribute, DE_UUID, DE_SIZE_16, SDP_Headset_HS);   // 0x1131
         de_add_number(attribute, DE_UUID, DE_SIZE_16, SDP_GenericAudio); // 0x1203
@@ -206,12 +206,12 @@ void hsp_hs_create_service(uint8_t * service, uint32_t service_record_handle, in
     de_add_number(service,  DE_UINT, DE_SIZE_16, SDP_BluetoothProfileDescriptorList);
     attribute = de_push_sequence(service);
     {
-        uint8_t *sppProfile = de_push_sequence(attribute);
+        uint8_t *hsp_profile = de_push_sequence(attribute);
         {
-            de_add_number(sppProfile,  DE_UUID, DE_SIZE_16, SDP_HSP); 
-            de_add_number(sppProfile,  DE_UINT, DE_SIZE_16, 0x0102); // Verision 1.2
+            de_add_number(hsp_profile,  DE_UUID, DE_SIZE_16, SDP_HSP); 
+            de_add_number(hsp_profile,  DE_UINT, DE_SIZE_16, 0x0102); // Verision 1.2
         }
-        de_pop_sequence(attribute, sppProfile);
+        de_pop_sequence(attribute, hsp_profile);
     }
     de_pop_sequence(service, attribute);
 
diff --git a/src/hci.c b/src/hci.c
index dea876592..b8855ede0 100644
--- a/src/hci.c
+++ b/src/hci.c
@@ -1050,6 +1050,10 @@ static void hci_initializing_run(void){
             hci_send_cmd(&hci_write_scan_enable, (hci_stack->connectable << 1) | hci_stack->discoverable); // page scan
             hci_stack->substate = HCI_INIT_W4_WRITE_SCAN_ENABLE;
             break;
+        case HCI_INIT_WRITE_SYNCHRONOUS_FLOW_CONTROL_ENABLE:
+            hci_stack->substate = HCI_INIT_W4_WRITE_SYNCHRONOUS_FLOW_CONTROL_ENABLE;
+            hci_send_cmd(&hci_write_synchronous_flow_control_enable, 1); // SCO tracking enabled
+            break;
 #ifdef HAVE_BLE
         // LE INIT
         case HCI_INIT_LE_READ_BUFFER_SIZE:
@@ -1249,17 +1253,34 @@ static void hci_initializing_event_handler(uint8_t * packet, uint16_t size){
                 return;
             }
             break;
+        case HCI_INIT_W4_WRITE_PAGE_TIMEOUT:
+            break;
         case HCI_INIT_W4_LE_READ_BUFFER_SIZE:
             // skip write le host if not supported (e.g. on LE only EM9301)
             if (hci_stack->local_supported_commands[0] & 0x02) break;
             hci_stack->substate = HCI_INIT_LE_SET_SCAN_PARAMETERS;
             return;
+
+#ifdef HAVE_SCO_OVER_HCI
+        case HCI_INIT_W4_WRITE_SCAN_ENABLE:
+            // just go to next state
+            break;
+        case HCI_INIT_W4_WRITE_SYNCHRONOUS_FLOW_CONTROL_ENABLE:
+            if (!hci_le_supported()){
+                // SKIP LE init for Classic only configuration
+                hci_stack->substate = HCI_INIT_DONE;
+                return;
+            }
+            break;
+#else
         case HCI_INIT_W4_WRITE_SCAN_ENABLE:
             if (!hci_le_supported()){
                 // SKIP LE init for Classic only configuration
                 hci_stack->substate = HCI_INIT_DONE;
                 return;
             }
+#endif
+            break;
         default:
             break;
     }
@@ -1366,10 +1387,9 @@ static void event_handler(uint8_t *packet, int size){
             }
             if (COMMAND_COMPLETE_EVENT(packet, hci_write_synchronous_flow_control_enable)){
                 if (packet[5] == 0){
-                    // @TODO cache value from hci_write_synchronous_flow_control_enable instead of assuming == 1
                     hci_stack->synchronous_flow_control_enabled = 1;
                 }
-            }
+            } 
             break;
             
         case HCI_EVENT_COMMAND_STATUS:
diff --git a/src/hci.h b/src/hci.h
index ade45ae6b..c18b38005 100644
--- a/src/hci.h
+++ b/src/hci.h
@@ -458,15 +458,16 @@ typedef enum hci_init_state{
     HCI_INIT_W4_WRITE_SIMPLE_PAIRING_MODE,
     HCI_INIT_WRITE_PAGE_TIMEOUT,
     HCI_INIT_W4_WRITE_PAGE_TIMEOUT,
-    // HCI_INIT_WRITE_SYNCHRONOUS_FLOW_CONTROL_ENABLE,
-    // HCI_INIT_W4_SYNCHRONOUS_FLOW_CONTROL_ENABLE,
-
     HCI_INIT_WRITE_CLASS_OF_DEVICE,
     HCI_INIT_W4_WRITE_CLASS_OF_DEVICE,
     HCI_INIT_WRITE_LOCAL_NAME,
     HCI_INIT_W4_WRITE_LOCAL_NAME,
     HCI_INIT_WRITE_SCAN_ENABLE,
     HCI_INIT_W4_WRITE_SCAN_ENABLE,
+    
+    HCI_INIT_WRITE_SYNCHRONOUS_FLOW_CONTROL_ENABLE,
+    HCI_INIT_W4_WRITE_SYNCHRONOUS_FLOW_CONTROL_ENABLE,
+
     HCI_INIT_LE_READ_BUFFER_SIZE,
     HCI_INIT_W4_LE_READ_BUFFER_SIZE,
     HCI_INIT_WRITE_LE_HOST_SUPPORTED,
diff --git a/test/hfp/hfp_ag_client_test.c b/test/hfp/hfp_ag_client_test.c
index 53ed83da8..41891aa31 100644
--- a/test/hfp/hfp_ag_client_test.c
+++ b/test/hfp/hfp_ag_client_test.c
@@ -337,7 +337,8 @@ static void simulate_test_sequence(hfp_test_item_t * test_item){
     int previous_step = -1;
     while ( i < test_item->len){
         previous_step++;
-        if (i < previous_step) exit(0);
+        CHECK_EQUAL(i >= previous_step, 1);
+        
         char * expected_cmd = test_steps[i];
         int expected_cmd_len = strlen(expected_cmd);
         printf("\nStep %d, %s \n", i, expected_cmd);
@@ -527,6 +528,7 @@ TEST_GROUP(HFPClient){
 
 TEST(HFPClient, PTSRHHTests){
     for (int i = 0; i < hfp_pts_ag_rhh_tests_size(); i++){
+        setup();
         simulate_test_sequence(&hfp_pts_ag_rhh_tests()[i]);
         teardown();
     }
@@ -534,6 +536,7 @@ TEST(HFPClient, PTSRHHTests){
 
 TEST(HFPClient, PTSECCTests){
     for (int i = 0; i < hfp_pts_ag_ecc_tests_size(); i++){
+        setup();
         simulate_test_sequence(&hfp_pts_ag_ecc_tests()[i]);
         teardown();
     }
@@ -541,6 +544,7 @@ TEST(HFPClient, PTSECCTests){
 
 TEST(HFPClient, PTSECSTests){
     for (int i = 0; i < hfp_pts_ag_ecs_tests_size(); i++){
+        setup();
         simulate_test_sequence(&hfp_pts_ag_ecs_tests()[i]);
         teardown();
     }
@@ -548,6 +552,7 @@ TEST(HFPClient, PTSECSTests){
 
 TEST(HFPClient, PTSTWCTests){
     for (int i = 0; i < hfp_pts_ag_twc_tests_size(); i++){
+        setup();
         simulate_test_sequence(&hfp_pts_ag_twc_tests()[i]);
         teardown();
     }
@@ -555,6 +560,7 @@ TEST(HFPClient, PTSTWCTests){
 
 TEST(HFPClient, PTSATATests){
     for (int i = 0; i < hfp_pts_ag_ata_tests_size(); i++){
+        setup();
         simulate_test_sequence(&hfp_pts_ag_ata_tests()[i]);
         teardown();
     }
@@ -562,6 +568,7 @@ TEST(HFPClient, PTSATATests){
 
 TEST(HFPClient, PTSSLCTests){
     for (int i = 0; i < hfp_pts_ag_slc_tests_size(); i++){
+        setup();
         simulate_test_sequence(&hfp_pts_ag_slc_tests()[i]);
         teardown();
     }
diff --git a/test/hfp/hfp_hf_client_test.c b/test/hfp/hfp_hf_client_test.c
index 345a53078..987dd7032 100644
--- a/test/hfp/hfp_hf_client_test.c
+++ b/test/hfp/hfp_hf_client_test.c
@@ -318,7 +318,8 @@ void simulate_test_sequence(hfp_test_item_t * test_item){
     int previous_step = -1;
     while ( i < test_item->len){
         previous_step++;
-        if (i < previous_step) exit(0);
+        CHECK_EQUAL(i >= previous_step, 1);
+        
         char * expected_cmd = test_steps[i];
         int expected_cmd_len = strlen(expected_cmd);
         printf("\nStep %d, %s \n", i, expected_cmd);
@@ -495,6 +496,7 @@ TEST_GROUP(HFPClient){
 
 TEST(HFPClient, PTSRHHTests){
     for (int i = 0; i < hfp_pts_hf_rhh_tests_size(); i++){
+        setup();
         simulate_test_sequence(&hfp_pts_hf_rhh_tests()[i]);
         teardown();
     }
@@ -502,6 +504,7 @@ TEST(HFPClient, PTSRHHTests){
 
 TEST(HFPClient, PTSECCTests){
     for (int i = 0; i < hfp_pts_hf_ecc_tests_size(); i++){
+        setup();
         simulate_test_sequence(&hfp_pts_hf_ecc_tests()[i]);
         teardown();
     }
@@ -509,6 +512,7 @@ TEST(HFPClient, PTSECCTests){
 
 TEST(HFPClient, PTSECSTests){
     for (int i = 0; i < hfp_pts_hf_ecs_tests_size(); i++){
+        setup();
         simulate_test_sequence(&hfp_pts_hf_ecs_tests()[i]);
         teardown();
     }
@@ -516,6 +520,7 @@ TEST(HFPClient, PTSECSTests){
 
 TEST(HFPClient, PTSTWCTests){
     for (int i = 0; i < hfp_pts_hf_twc_tests_size(); i++){
+        setup();
         simulate_test_sequence(&hfp_pts_hf_twc_tests()[i]);
         teardown();
     }
@@ -523,6 +528,7 @@ TEST(HFPClient, PTSTWCTests){
 
 TEST(HFPClient, PTSATATests){
     for (int i = 0; i < hfp_pts_hf_ata_tests_size(); i++){
+        setup();
         simulate_test_sequence(&hfp_pts_hf_ata_tests()[i]);
         teardown();
     }
@@ -530,6 +536,7 @@ TEST(HFPClient, PTSATATests){
 
 TEST(HFPClient, PTSSLCTests){
     for (int i = 0; i < hfp_pts_hf_slc_tests_size(); i++){
+        setup();
         simulate_test_sequence(&hfp_pts_hf_slc_tests()[i]);
         teardown();
     }
diff --git a/test/hfp/test_sequences.c b/test/hfp/test_sequences.c
index f7dca7fc3..498821f71 100644
--- a/test/hfp/test_sequences.c
+++ b/test/hfp/test_sequences.c
@@ -1883,7 +1883,8 @@ const char * TC_AG_ECS_BV_03_I[] = {
     "OK" ,
     "AT+CMEE=1" ,
     "OK" ,
-    "USER:c" , // "+CIEV:3,1" ,
+    "USER:c" , 
+    "+CIEV:3,1" ,
     "RING" ,
     "+CLIP: \"1234567\",129" ,
     "ATA" ,
@@ -1902,7 +1903,7 @@ const char * TC_AG_ECS_BV_03_I[] = {
 hfp_test_item_t pts_ag_ecs_tests[] = {
     TEST_SEQUENCE(TC_AG_ECS_BV_01_I),
     TEST_SEQUENCE(TC_AG_ECS_BV_02_I),
-    // TEST_SEQUENCE(TC_AG_ECS_BV_03_I)
+    TEST_SEQUENCE(TC_AG_ECS_BV_03_I)
 };