From 63331bf43df1e8e5a8790ce11a37cfe3d7dc67af Mon Sep 17 00:00:00 2001
From: Milanka Ringwald <mila@ringwald.ch>
Date: Wed, 29 Nov 2017 14:19:32 +0100
Subject: [PATCH] avdtp source: send event on signaling connection established
 for both incoming and outgoing connections

---
 example/a2dp_source_demo.c    | 121 +++++++++++++++++++---------------
 src/btstack_defines.h         |   7 +-
 src/btstack_event.h           |  17 +++--
 src/classic/a2dp_source.c     |  15 +++--
 src/classic/avdtp.c           |   9 ++-
 src/classic/avdtp.h           |  10 ++-
 src/classic/avdtp_acceptor.c  |   9 +++
 src/classic/avdtp_initiator.c |   5 ++
 8 files changed, 124 insertions(+), 69 deletions(-)

diff --git a/example/a2dp_source_demo.c b/example/a2dp_source_demo.c
index af6879f6a..40fdf35b0 100644
--- a/example/a2dp_source_demo.c
+++ b/example/a2dp_source_demo.c
@@ -44,7 +44,7 @@
 // *****************************************************************************
 /* EXAMPLE_START(a2dp_source_demo): Serve audio stream and handle remote playback control and queries.
  *
- * @text This A2DP Source example demonstrates how to send an audio data stream 
+ * @text This  A2DP Source example demonstrates how to send an audio data stream 
  * to a remote A2DP Sink device and how to switch between two audio data sources.  
  * In addition, the AVRCP Target is used to answer queries on currently played media,
  * as well as to handle remote playback control, i.e. play, stop, repeat, etc.
@@ -90,6 +90,7 @@ typedef struct {
     uint16_t a2dp_cid;
     uint8_t  local_seid;
     uint8_t  connected;
+    uint8_t  stream_opened;
 
     uint32_t time_audio_data_sent; // ms
     uint32_t acc_num_missed_samples;
@@ -142,10 +143,12 @@ static btstack_packet_callback_registration_t hci_event_callback_registration;
 // mac 2013:        static const char * device_addr_string = "84:38:35:65:d1:15";
 // phone 2013:      static const char * device_addr_string = "D8:BB:2C:DF:F0:F2";
 // Minijambox:      
-static const char * device_addr_string = "00:21:3C:AC:F7:38";
+// static const char * device_addr_string = "00:21:3C:AC:F7:38";
 // Philips SHB9100: static const char * device_addr_string = "00:22:37:05:FD:E8";
 // RT-B6:           static const char * device_addr_string = "00:75:58:FF:C9:7D";
 // BT dongle:       static const char * device_addr_string = "00:1A:7D:DA:71:0A";
+// Sony MDR-ZX330BT
+static const char * device_addr_string = "00:18:09:28:50:18";
 #endif
 
 static bd_addr_t device_addr;
@@ -242,14 +245,14 @@ static int a2dp_source_and_avrcp_services_init(void){
     hci_add_event_handler(&hci_event_callback_registration);
 
     l2cap_init();
-    // Initialize A2DP Source.
+    // Initialize  A2DP Source.
     a2dp_source_init();
     a2dp_source_register_packet_handler(&a2dp_source_packet_handler);
 
     // Create stream endpoint.
     avdtp_stream_endpoint_t * local_stream_endpoint = a2dp_source_create_stream_endpoint(AVDTP_AUDIO, AVDTP_CODEC_SBC, media_sbc_codec_capabilities, sizeof(media_sbc_codec_capabilities), media_sbc_codec_configuration, sizeof(media_sbc_codec_configuration));
     if (!local_stream_endpoint){
-        printf("A2DP source demo: not enough memory to create local stream endpoint\n");
+        printf(" A2DP Source demo: not enough memory to create local stream endpoint\n");
         return 1;
     }
     media_tracker.local_seid = avdtp_local_seid(local_stream_endpoint);
@@ -261,7 +264,7 @@ static int a2dp_source_and_avrcp_services_init(void){
     // Initialize SDP, 
     sdp_init();
     
-    // Create A2DP source service record and register it with SDP.
+    // Create  A2DP Source service record and register it with SDP.
     memset(sdp_a2dp_source_service_buffer, 0, sizeof(sdp_a2dp_source_service_buffer));
     a2dp_source_create_sdp_record(sdp_a2dp_source_service_buffer, 0x10002, 1, NULL, NULL);
     sdp_register_service(sdp_a2dp_source_service_buffer);
@@ -274,7 +277,7 @@ static int a2dp_source_and_avrcp_services_init(void){
     // Set local name with a template Bluetooth address, that will be automatically
     // replaced with a actual address once it is available, i.e. when BTstack boots
     // up and starts talking to a Bluetooth module.
-    gap_set_local_name("A2DP Source Demo 00:00:00:00:00:00");
+    gap_set_local_name(" A2DP Source Demo 00:00:00:00:00:00");
     gap_discoverable_control(1);
     gap_set_class_of_device(0x200408);
     
@@ -433,28 +436,21 @@ static void a2dp_source_packet_handler(uint8_t packet_type, uint16_t channel, ui
 
     if (hci_event_packet_get_type(packet) != HCI_EVENT_A2DP_META) return;
     switch (packet[2]){
-        case A2DP_SUBEVENT_INCOMING_CONNECTION_ESTABLISHED:
-            a2dp_subevent_incoming_connection_established_get_bd_addr(packet, address);
-            cid = a2dp_subevent_incoming_connection_established_get_a2dp_cid(packet);
+        case A2DP_SUBEVENT_SIGNALING_CONNECTION_ESTABLISHED:
+            a2dp_subevent_signaling_connection_established_get_bd_addr(packet, address);
+            cid = a2dp_subevent_signaling_connection_established_get_a2dp_cid(packet);
             
             if (!media_tracker.a2dp_cid){
                 media_tracker.a2dp_cid = cid;
             } else if (cid != media_tracker.a2dp_cid){
-                printf("A2DP: Incoming connection failure, received cid 0x%02x, expected cid 0x%02x\n", cid, media_tracker.a2dp_cid);
+                printf(" A2DP Source demo: Connection failed, received cid 0x%02x, expected cid 0x%02x\n", cid, media_tracker.a2dp_cid);
                 break;
             }
-            
-            media_tracker.connected = 1;
-            printf("A2DP: Incoming connection established: address %s, a2dp cid 0x%02x. Create stream on local seid %d.\n", 
-                bd_addr_to_str(address), media_tracker.a2dp_cid, media_tracker.local_seid);
-            status = a2dp_source_establish_stream(device_addr, media_tracker.local_seid, &media_tracker.a2dp_cid);
-            if (status != ERROR_CODE_SUCCESS){
-                printf("Could not perform command, status 0x%2x\n", status);
-            }
+            printf(" A2DP Source demo: Connected to address %s, a2dp cid 0x%02x.\n", bd_addr_to_str(address), media_tracker.a2dp_cid);
             break;
 
          case A2DP_SUBEVENT_SIGNALING_MEDIA_CODEC_SBC_CONFIGURATION:{
-            printf("A2DP Sink demo: received SBC codec configuration.\n");
+            printf(" A2DP Source demo: Received SBC codec configuration.\n");
             sbc_configuration.reconfigure = a2dp_subevent_signaling_media_codec_sbc_configuration_get_reconfigure(packet);
             sbc_configuration.num_channels = a2dp_subevent_signaling_media_codec_sbc_configuration_get_num_channels(packet);
             sbc_configuration.sampling_frequency = a2dp_subevent_signaling_media_codec_sbc_configuration_get_sampling_frequency(packet);
@@ -471,36 +467,42 @@ static void a2dp_source_packet_handler(uint8_t packet_type, uint16_t channel, ui
                 sbc_configuration.allocation_method, sbc_configuration.sampling_frequency, 
                 sbc_configuration.max_bitpool_value,
                 sbc_configuration.channel_mode);
+            
+            status = a2dp_source_establish_stream(device_addr, media_tracker.local_seid, &media_tracker.a2dp_cid);
+            if (status != ERROR_CODE_SUCCESS){
+                printf("Could not perform command, status 0x%2x\n", status);
+            }
             break;
         }  
 
         case A2DP_SUBEVENT_STREAM_ESTABLISHED:
-            media_tracker.connected = 1;
             a2dp_subevent_stream_established_get_bd_addr(packet, address);
             status = a2dp_subevent_stream_established_get_status(packet);
             if (status){
-                printf("A2DP: Stream establishment failed: status 0x%02x.\n", status);
+                printf(" A2DP Source demo: Stream failed, status 0x%02x.\n", status);
                 break;
             }
             local_seid = a2dp_subevent_stream_established_get_local_seid(packet);
             if (local_seid != media_tracker.local_seid){
-                printf("A2DP: Stream establishment failed: wrong local seid %d, expected %d.\n", local_seid, media_tracker.local_seid);
+                printf(" A2DP Source demo: Stream failed, wrong local seid %d, expected %d.\n", local_seid, media_tracker.local_seid);
                 break;    
             }
-            media_tracker.a2dp_cid = a2dp_subevent_stream_established_get_a2dp_cid(packet);
-            printf("A2DP: Stream established: address %s, a2dp cid 0x%02x, local seid %d, remote seid %d.\n", bd_addr_to_str(address),
+            printf(" A2DP Source demo: Stream established, address %s, a2dp cid 0x%02x, local seid %d, remote seid %d.\n", bd_addr_to_str(address),
                 media_tracker.a2dp_cid, media_tracker.local_seid, a2dp_subevent_stream_established_get_remote_seid(packet));
-            printf("Start playing mod, cid 0x%02x.\n", media_tracker.a2dp_cid);
+            printf(" A2DP Source demo: Start playing mod, a2dp cid 0x%02x.\n", media_tracker.a2dp_cid);
+            media_tracker.stream_opened = 1;
             data_source = STREAM_MOD;
             status = a2dp_source_start_stream(media_tracker.a2dp_cid, media_tracker.local_seid);
             break;
 
         case A2DP_SUBEVENT_STREAM_STARTED:
             play_info.status = AVRCP_PLAYBACK_STATUS_PLAYING;
-            avrcp_target_set_now_playing_info(avrcp_cid, &tracks[data_source], sizeof(tracks)/sizeof(avrcp_track_t));
-            avrcp_target_set_playback_status(avrcp_cid, AVRCP_PLAYBACK_STATUS_PLAYING);
+            if (avrcp_connected){
+                avrcp_target_set_now_playing_info(avrcp_cid, &tracks[data_source], sizeof(tracks)/sizeof(avrcp_track_t));
+                avrcp_target_set_playback_status(avrcp_cid, AVRCP_PLAYBACK_STATUS_PLAYING);
+            }
             a2dp_demo_timer_start(&media_tracker);
-            printf("A2DP: Stream started.\n");
+            printf(" A2DP Source demo: Stream started.\n");
             break;
 
         case A2DP_SUBEVENT_STREAMING_CAN_SEND_MEDIA_PACKET_NOW:
@@ -509,28 +511,35 @@ static void a2dp_source_packet_handler(uint8_t packet_type, uint16_t channel, ui
 
         case A2DP_SUBEVENT_STREAM_SUSPENDED:
             play_info.status = AVRCP_PLAYBACK_STATUS_PAUSED;
-            avrcp_target_set_playback_status(avrcp_cid, AVRCP_PLAYBACK_STATUS_PAUSED);
-            printf("A2DP: Stream paused.\n");
+            if (avrcp_connected){
+                avrcp_target_set_playback_status(avrcp_cid, AVRCP_PLAYBACK_STATUS_PAUSED);
+            }
+            printf(" A2DP Source demo: Stream paused.\n");
             a2dp_demo_timer_stop(&media_tracker);
             break;
 
         case A2DP_SUBEVENT_STREAM_RELEASED:
-            avrcp_target_set_now_playing_info(avrcp_cid, NULL, sizeof(tracks)/sizeof(avrcp_track_t));
             play_info.status = AVRCP_PLAYBACK_STATUS_STOPPED;
-            avrcp_target_set_playback_status(avrcp_cid, AVRCP_PLAYBACK_STATUS_STOPPED);
-            printf("A2DP: Stream released.\n");
+            cid = a2dp_subevent_stream_released_get_a2dp_cid(packet);
+            if (cid == media_tracker.a2dp_cid) {
+                media_tracker.stream_opened = 0;
+                printf(" A2DP Source demo: Stream released.\n");
+            }
+            if (avrcp_connected){
+                avrcp_target_set_now_playing_info(avrcp_cid, NULL, sizeof(tracks)/sizeof(avrcp_track_t));
+                avrcp_target_set_playback_status(avrcp_cid, AVRCP_PLAYBACK_STATUS_STOPPED);
+            }
             a2dp_demo_timer_stop(&media_tracker);
             break;
         case A2DP_SUBEVENT_SIGNALING_CONNECTION_RELEASED:
-            printf("A2DP: Signaling released.\n");
             cid = a2dp_subevent_signaling_connection_released_get_a2dp_cid(packet);
             if (cid == media_tracker.a2dp_cid) {
                 media_tracker.connected = 0;
                 media_tracker.a2dp_cid = 0;
+                printf(" A2DP Source demo: Signaling released.\n\n");
             }
             break;
         default:
-            printf("A2DP: event 0x%02x is not parsed\n", packet[2]);
             break; 
     }
 }
@@ -549,20 +558,20 @@ static void avrcp_target_packet_handler(uint8_t packet_type, uint16_t channel, u
         case AVRCP_SUBEVENT_CONNECTION_ESTABLISHED: {
             local_cid = avrcp_subevent_connection_established_get_avrcp_cid(packet);
             // if (avrcp_cid != 0 && avrcp_cid != local_cid) {
-            //     printf("AVRCP: Connection failed, expected 0x%02X l2cap cid, received 0x%02X\n", avrcp_cid, local_cid);
+            //     printf("AVRCP Source demo: Connection failed, expected 0x%02X l2cap cid, received 0x%02X\n", avrcp_cid, local_cid);
             //     return;
             // }
             // if (avrcp_cid != local_cid) break;
             
             status = avrcp_subevent_connection_established_get_status(packet);
             if (status != ERROR_CODE_SUCCESS){
-                printf("AVRCP: Connection failed: status 0x%02x\n", status);
+                printf("AVRCP Source demo: Connection failed, status 0x%02x\n", status);
                 return;
             }
             avrcp_connected = 1;
             avrcp_cid = local_cid;
             avrcp_subevent_connection_established_get_bd_addr(packet, event_addr);
-            printf("AVRCP: connected to %s, avrcp_cid 0x%02x\n", bd_addr_to_str(event_addr), local_cid);
+            printf("AVRCP Source demo: Connected to %s, avrcp_cid 0x%02x\n", bd_addr_to_str(event_addr), local_cid);
             
             avrcp_target_set_now_playing_info(avrcp_cid, NULL, sizeof(tracks)/sizeof(avrcp_track_t));
             avrcp_target_set_unit_info(avrcp_cid, AVRCP_SUBUNIT_TYPE_AUDIO, company_id);
@@ -587,15 +596,15 @@ static void avrcp_target_packet_handler(uint8_t packet_type, uint16_t channel, u
             if (!media_tracker.connected) break;
             switch (operation_id){
                 case AVRCP_OPERATION_ID_PLAY:
-                    printf("AVRCP: PLAY\n");
+                    printf("AVRCP Source demo: PLAY\n");
                     status = a2dp_source_start_stream(media_tracker.a2dp_cid, media_tracker.local_seid);
                     break;
                 case AVRCP_OPERATION_ID_PAUSE:
-                    printf("AVRCP: PAUSE\n");
+                    printf("AVRCP Source demo: PAUSE\n");
                     status = a2dp_source_pause_stream(media_tracker.a2dp_cid, media_tracker.local_seid);
                     break;
                 case AVRCP_OPERATION_ID_STOP:
-                    printf("AVRCP: STOP\n");
+                    printf("AVRCP Source demo: STOP\n");
                     status = a2dp_source_disconnect(media_tracker.a2dp_cid);
                     break;
                 default:
@@ -604,11 +613,11 @@ static void avrcp_target_packet_handler(uint8_t packet_type, uint16_t channel, u
             break;
         }
         case AVRCP_SUBEVENT_CONNECTION_RELEASED:
-            printf("AVRCP: Channel released: avrcp_cid 0x%02x\n", avrcp_subevent_connection_released_get_avrcp_cid(packet));
+            printf("AVRCP Source demo: Disconnected, avrcp_cid 0x%02x\n", avrcp_subevent_connection_released_get_avrcp_cid(packet));
             avrcp_cid = 0;
+            avrcp_connected = 0;
             return;
         default:
-            printf("AVRCP: event not parsed %02x\n", packet[2]);
             break;
     }
 
@@ -621,7 +630,7 @@ static void avrcp_target_packet_handler(uint8_t packet_type, uint16_t channel, u
 static void show_usage(void){
     bd_addr_t      iut_address;
     gap_local_bd_addr(iut_address);
-    printf("\n--- Bluetooth A2DP Source/AVRCP Target Demo %s ---\n", bd_addr_to_str(iut_address));
+    printf("\n--- Bluetooth  A2DP Source/AVRCP Target Demo %s ---\n", bd_addr_to_str(iut_address));
     printf("b      - AVDTP Source create  connection to addr %s\n", device_addr_string);
     printf("B      - AVDTP Source disconnect\n");
     printf("c      - AVRCP Target create connection to addr %s\n", device_addr_string);
@@ -665,23 +674,29 @@ static void stdin_process(char cmd){
         case 't':
             printf("STREAM_PTS_TEST.\n");
             data_source = STREAM_PTS_TEST;
-            avrcp_target_set_now_playing_info(avrcp_cid, &tracks[data_source], sizeof(tracks)/sizeof(avrcp_track_t));
-            if (!media_tracker.connected) break;
+            if (avrcp_connected){
+                avrcp_target_set_now_playing_info(avrcp_cid, &tracks[data_source], sizeof(tracks)/sizeof(avrcp_track_t));
+            }
+            if (!media_tracker.stream_opened) break;
             status = a2dp_source_start_stream(media_tracker.a2dp_cid, media_tracker.local_seid);
             break;
 
         case 'x':
-            avrcp_target_set_now_playing_info(avrcp_cid, &tracks[data_source], sizeof(tracks)/sizeof(avrcp_track_t));
+            if (avrcp_connected){
+                avrcp_target_set_now_playing_info(avrcp_cid, &tracks[data_source], sizeof(tracks)/sizeof(avrcp_track_t));
+            }
             printf("Playing sine.\n");
             data_source = STREAM_SINE;
-            if (!media_tracker.connected) break;
+            if (!media_tracker.stream_opened) break;
             status = a2dp_source_start_stream(media_tracker.a2dp_cid, media_tracker.local_seid);
             break;
         case 'z':
-            avrcp_target_set_now_playing_info(avrcp_cid, &tracks[data_source], sizeof(tracks)/sizeof(avrcp_track_t));
+            if (avrcp_connected){
+                avrcp_target_set_now_playing_info(avrcp_cid, &tracks[data_source], sizeof(tracks)/sizeof(avrcp_track_t));
+            }
             printf("Playing mod.\n");
             data_source = STREAM_MOD;
-            if (!media_tracker.connected) break;
+            if (!media_tracker.stream_opened) break;
             status = a2dp_source_start_stream(media_tracker.a2dp_cid, media_tracker.local_seid);
             break;
         case 'p':
@@ -690,8 +705,10 @@ static void stdin_process(char cmd){
             status = a2dp_source_pause_stream(media_tracker.a2dp_cid, media_tracker.local_seid);
             break;
         case '0':
-            avrcp_target_set_now_playing_info(avrcp_cid, NULL, sizeof(tracks)/sizeof(avrcp_track_t));
-            printf("Reset now playing info\n");
+            if (avrcp_connected){
+                avrcp_target_set_now_playing_info(avrcp_cid, NULL, sizeof(tracks)/sizeof(avrcp_track_t));
+                printf("Reset now playing info\n");
+            }
             break;
         default:
             show_usage();
diff --git a/src/btstack_defines.h b/src/btstack_defines.h
index b0b0a3a82..59f142501 100644
--- a/src/btstack_defines.h
+++ b/src/btstack_defines.h
@@ -1543,12 +1543,13 @@ typedef uint8_t sm_key_t[16];
 #define A2DP_SUBEVENT_COMMAND_REJECTED                              0x0A
 
 /**
- * @format 12B          Signaling channel is opened.
- * @param subevent_code 
+ * @format 12B1
+ * @param subevent_code
  * @param a2dp_cid
  * @param bd_addr
+ * @param status 0 == OK
  */
-#define A2DP_SUBEVENT_INCOMING_CONNECTION_ESTABLISHED               0x0B
+#define A2DP_SUBEVENT_SIGNALING_CONNECTION_ESTABLISHED              0x0B
 
 /**
  * @format 12            Signaling channel is released.
diff --git a/src/btstack_event.h b/src/btstack_event.h
index eadf5abf1..4cb8ad957 100644
--- a/src/btstack_event.h
+++ b/src/btstack_event.h
@@ -4992,23 +4992,32 @@ static inline uint8_t a2dp_subevent_command_rejected_get_signal_identifier(const
 }
 
 /**
- * @brief Get field a2dp_cid from event A2DP_SUBEVENT_INCOMING_CONNECTION_ESTABLISHED
+ * @brief Get field a2dp_cid from event A2DP_SUBEVENT_SIGNALING_CONNECTION_ESTABLISHED
  * @param event packet
  * @return a2dp_cid
  * @note: btstack_type 2
  */
-static inline uint16_t a2dp_subevent_incoming_connection_established_get_a2dp_cid(const uint8_t * event){
+static inline uint16_t a2dp_subevent_signaling_connection_established_get_a2dp_cid(const uint8_t * event){
     return little_endian_read_16(event, 3);
 }
 /**
- * @brief Get field bd_addr from event A2DP_SUBEVENT_INCOMING_CONNECTION_ESTABLISHED
+ * @brief Get field bd_addr from event A2DP_SUBEVENT_SIGNALING_CONNECTION_ESTABLISHED
  * @param event packet
  * @param Pointer to storage for bd_addr
  * @note: btstack_type B
  */
-static inline void a2dp_subevent_incoming_connection_established_get_bd_addr(const uint8_t * event, bd_addr_t bd_addr){
+static inline void a2dp_subevent_signaling_connection_established_get_bd_addr(const uint8_t * event, bd_addr_t bd_addr){
     reverse_bd_addr(&event[5], bd_addr);    
 }
+/**
+ * @brief Get field status from event A2DP_SUBEVENT_SIGNALING_CONNECTION_ESTABLISHED
+ * @param event packet
+ * @return status
+ * @note: btstack_type 1
+ */
+static inline uint8_t a2dp_subevent_signaling_connection_established_get_status(const uint8_t * event){
+    return event[11];
+}
 
 /**
  * @brief Get field a2dp_cid from event A2DP_SUBEVENT_SIGNALING_CONNECTION_RELEASED
diff --git a/src/classic/a2dp_source.c b/src/classic/a2dp_source.c
index daffcae64..333073321 100644
--- a/src/classic/a2dp_source.c
+++ b/src/classic/a2dp_source.c
@@ -189,13 +189,13 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
             
             sc.active_remote_sep = NULL;
             next_remote_sep_index_to_query = 0;
-            if (!sc.local_stream_endpoint) {
+            if (sc.local_stream_endpoint) {
                 app_state = A2DP_CONNECTED;
                 uint8_t event[11];
                 int pos = 0;
                 event[pos++] = HCI_EVENT_A2DP_META;
                 event[pos++] = sizeof(event) - 2;
-                event[pos++] = A2DP_SUBEVENT_INCOMING_CONNECTION_ESTABLISHED;
+                event[pos++] = A2DP_SUBEVENT_SIGNALING_CONNECTION_ESTABLISHED;
                 little_endian_store_16(event, pos, cid);
                 pos += 2;
                 reverse_bd_addr(event+pos, sc.remote_addr);
@@ -204,6 +204,8 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
                 return;
             }
 
+            log_info("A2DP_SUBEVENT_SIGNALING_CONNECTION established avdtp_cid 0x%02x ---", a2dp_source_context.avdtp_cid);
+                
             app_state = A2DP_W2_DISCOVER_SEPS;
             avdtp_source_discover_stream_endpoints(cid);
             break;
@@ -251,11 +253,10 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
             sc.channel_mode = avdtp_subevent_signaling_media_codec_sbc_configuration_get_channel_mode(packet);
             // TODO: deal with reconfigure: avdtp_subevent_signaling_media_codec_sbc_configuration_get_reconfigure(packet);
             log_info("SBC Config: sample rate %u, max bitpool %u", sc.sampling_frequency, sc.max_bitpool_value);
+            app_state = A2DP_W2_OPEN_STREAM_WITH_SEID;
             a2dp_signaling_emit_media_codec_sbc(a2dp_source_context.a2dp_callback, packet, size);
-       
             break;
         }  
-
        
         case AVDTP_SUBEVENT_STREAMING_CAN_SEND_MEDIA_PACKET_NOW: 
             cid = avdtp_subevent_streaming_can_send_media_packet_now_get_avdtp_cid(packet);
@@ -283,7 +284,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
             // TODO check cid
             signal_identifier = avdtp_subevent_signaling_accept_get_signal_identifier(packet);
             cid = avdtp_subevent_signaling_accept_get_avdtp_cid(packet);
-            log_info("Accepted %d", signal_identifier);
+            log_info("Accepted %d, state %d", signal_identifier, app_state);
             
             switch (app_state){
                 case A2DP_W2_DISCOVER_SEPS:
@@ -312,11 +313,13 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
                     break;
                 case A2DP_W2_SET_CONFIGURATION:{
                     if (!sc.local_stream_endpoint) return;
-                    app_state = A2DP_W2_OPEN_STREAM_WITH_SEID;
+                    app_state = A2DP_W4_SET_CONFIGURATION;
+                    log_info("A2DP configuration is set, wait for A2DP_W2_OPEN_STREAM_WITH_SEID ... ");
                     avdtp_source_set_configuration(cid, avdtp_stream_endpoint_seid(sc.local_stream_endpoint), sc.active_remote_sep->seid, sc.local_stream_endpoint->remote_configuration_bitmap, sc.local_stream_endpoint->remote_configuration);
                     break;
                 }
                 case A2DP_W2_OPEN_STREAM_WITH_SEID:{
+                    log_info("A2DP open stream ");
                     app_state = A2DP_W4_OPEN_STREAM_WITH_SEID;
                     avdtp_source_open_stream(cid, avdtp_stream_endpoint_seid(sc.local_stream_endpoint), sc.active_remote_sep->seid);
                     break;
diff --git a/src/classic/avdtp.c b/src/classic/avdtp.c
index aa405a08b..b093fb8df 100644
--- a/src/classic/avdtp.c
+++ b/src/classic/avdtp.c
@@ -65,7 +65,7 @@ static const unsigned int attribute_value_buffer_size = sizeof(attribute_value);
 // } avdtp_sdp_query_context_t;
 
 static avdtp_context_t * sdp_query_context;
-static uint16_t avdtp_cid_counter = 0;
+static uint16_t avdtp_cid_counter = 0x55;
 
 static void (*handle_media_data)(uint8_t local_seid, uint8_t *packet, uint16_t size);
 static void avdtp_handle_sdp_client_query_result(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
@@ -115,6 +115,7 @@ uint8_t avdtp_connect(bd_addr_t remote, avdtp_sep_type_t query_role, avdtp_conte
     switch (connection->state){
         case AVDTP_SIGNALING_CONNECTION_IDLE:
             connection->state = AVDTP_SIGNALING_W4_SDP_QUERY_COMPLETE;
+            connection->is_initiator = 1;
             sdp_query_context = avdtp_context;
             avdtp_context->avdtp_l2cap_psm = 0;
             avdtp_context->avdtp_version = 0;
@@ -279,6 +280,7 @@ avdtp_connection_t * avdtp_create_connection(bd_addr_t remote_addr, avdtp_contex
     connection->state = AVDTP_SIGNALING_CONNECTION_IDLE;
     connection->initiator_transaction_label = avdtp_get_next_initiator_transaction_label(context);
     connection->avdtp_cid = avdtp_get_next_avdtp_cid();
+    context->avdtp_cid = connection->avdtp_cid;
     memcpy(connection->remote_addr, remote_addr, 6);
     btstack_linked_list_add(&context->connections, (btstack_linked_item_t *) connection);
     return connection;
@@ -507,6 +509,7 @@ void avdtp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet
                     
                     if (!connection || connection->state == AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED){
                         connection = avdtp_create_connection(event_addr, context);
+                        connection->is_initiator = 0;
                         connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED;
                         log_info("L2CAP_EVENT_INCOMING_CONNECTION, connection %p, state connection %d, avdtp cid 0x%02x", connection, connection->state, connection->avdtp_cid);
                         l2cap_accept_connection(local_cid);
@@ -904,7 +907,9 @@ void avdtp_set_configuration(uint16_t avdtp_cid, uint8_t local_seid, uint8_t rem
         log_error("avdtp_set_configuration: no initiator stream endpoint for seid %d", local_seid);
         return;
     }        
-    
+    connection->is_configuration_initiated_localy = 1;
+    connection->is_initiator = 1;
+
     connection->initiator_transaction_label++;
     connection->remote_seid = remote_seid;
     connection->local_seid = local_seid;
diff --git a/src/classic/avdtp.h b/src/classic/avdtp.h
index c54e0975a..50d0b0467 100644
--- a/src/classic/avdtp.h
+++ b/src/classic/avdtp.h
@@ -373,6 +373,7 @@ typedef struct {
     avdtp_packet_type_t         packet_type;
 } avdtp_signaling_packet_t;
 
+
 typedef struct {
     btstack_linked_item_t    item;
     bd_addr_t remote_addr;
@@ -410,6 +411,10 @@ typedef struct {
     // store configurations with remote seps
     avdtp_sep_t remote_seps[MAX_NUM_SEPS];
     uint8_t remote_seps_num;
+
+    // store current role
+    uint8_t is_initiator;
+    uint8_t is_configuration_initiated_localy;
 } avdtp_connection_t;
 
 typedef enum {
@@ -418,12 +423,12 @@ typedef enum {
     A2DP_W2_DISCOVER_SEPS,
     A2DP_W2_GET_CAPABILITIES,
     A2DP_W2_GET_ALL_CAPABILITIES,
-    A2DP_W2_SET_CONFIGURATION,
+    A2DP_W2_SET_CONFIGURATION,      //5
     A2DP_W4_GET_CONFIGURATION,
     A2DP_W4_SET_CONFIGURATION,
     A2DP_W2_SUSPEND_STREAM_WITH_SEID,
     A2DP_W2_RECONFIGURE_WITH_SEID,
-    A2DP_W2_OPEN_STREAM_WITH_SEID,
+    A2DP_W2_OPEN_STREAM_WITH_SEID,   //10
     A2DP_W4_OPEN_STREAM_WITH_SEID,
     A2DP_W2_START_STREAM_WITH_SEID,
     A2DP_W2_ABORT_STREAM_WITH_SEID,
@@ -470,6 +475,7 @@ typedef struct avdtp_stream_endpoint {
     uint8_t suspend_stream;
     
     uint16_t sequence_number;
+
 } avdtp_stream_endpoint_t;
 
 typedef struct {
diff --git a/src/classic/avdtp_acceptor.c b/src/classic/avdtp_acceptor.c
index 961889abf..006eeee9b 100644
--- a/src/classic/avdtp_acceptor.c
+++ b/src/classic/avdtp_acceptor.c
@@ -194,6 +194,15 @@ void avdtp_acceptor_stream_config_subsm(avdtp_connection_t * connection, uint8_t
                     stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES;
                     break;
                 case AVDTP_SI_SET_CONFIGURATION:{
+                    if (connection->is_configuration_initiated_localy){
+                        log_info("ACP: Set configuration already initiated localy, reject cmd ");
+                        // fire configuration parsing errors 
+                        connection->reject_signal_identifier = connection->signaling_packet.signal_identifier;
+                        stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_W2_REJECT_UNKNOWN_CMD;
+                        connection->is_initiator = 1;
+                        break;
+                    }
+
                     log_info("ACP: AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION ");
                     stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION;
                     connection->reject_service_category = 0;
diff --git a/src/classic/avdtp_initiator.c b/src/classic/avdtp_initiator.c
index 5b605a981..9b7b38b35 100644
--- a/src/classic/avdtp_initiator.c
+++ b/src/classic/avdtp_initiator.c
@@ -374,6 +374,11 @@ int sent = 1;
     switch (stream_endpoint_state){
         case AVDTP_INITIATOR_W2_SET_CONFIGURATION:
         case AVDTP_INITIATOR_W2_RECONFIGURE_STREAM_WITH_SEID:{
+            if (!connection->is_initiator){
+                connection->is_configuration_initiated_localy = 0;
+                break;
+            }
+
             log_info("INT: AVDTP_INITIATOR_W2_(RE)CONFIGURATION bitmap, int seid %d, acp seid %d", connection->local_seid, connection->remote_seid);
             // log_info_hexdump(  connection->remote_capabilities.media_codec.media_codec_information,  connection->remote_capabilities.media_codec.media_codec_information_len);
             connection->signaling_packet.acp_seid = connection->remote_seid;