From c1c40ea12957f6aceeda5d0173f9a072324e3713 Mon Sep 17 00:00:00 2001
From: Matthias Ringwald <matthias@ringwald.ch>
Date: Mon, 16 Dec 2019 12:44:07 +0100
Subject: [PATCH] avdtp: add avdtp_get_signaling_packet_type

---
 src/classic/avdtp.c      | 10 +++++++---
 src/classic/avdtp_util.c |  4 ++++
 src/classic/avdtp_util.h |  2 ++
 3 files changed, 13 insertions(+), 3 deletions(-)

diff --git a/src/classic/avdtp.c b/src/classic/avdtp.c
index 20e8b9030..1d8748d02 100644
--- a/src/classic/avdtp.c
+++ b/src/classic/avdtp.c
@@ -362,14 +362,18 @@ avdtp_stream_endpoint_t * avdtp_create_stream_endpoint(avdtp_sep_type_t sep_type
     return stream_endpoint;
 }
 
-
 static void handle_l2cap_data_packet_for_signaling_connection(avdtp_connection_t * connection, uint8_t *packet, uint16_t size, avdtp_context_t * context){
-    int offset = avdtp_read_signaling_header(&connection->signaling_packet, packet, size);
-    switch (connection->signaling_packet.message_type){
+    if (size < 2) return;
+
+    uint16_t offset;
+    avdtp_message_type_t message_type = avdtp_get_signaling_packet_type(packet);
+    switch (message_type){
         case AVDTP_CMD_MSG:
+            offset = avdtp_read_signaling_header(&connection->signaling_packet, packet, size);
             avdtp_acceptor_stream_config_subsm(connection, packet, size, offset, context);
             break;
         default:
+            offset = avdtp_read_signaling_header(&connection->signaling_packet, packet, size);
             avdtp_initiator_stream_config_subsm(connection, packet, size, offset, context);
             break;
     }
diff --git a/src/classic/avdtp_util.c b/src/classic/avdtp_util.c
index 786520196..f94dacefb 100644
--- a/src/classic/avdtp_util.c
+++ b/src/classic/avdtp_util.c
@@ -213,6 +213,10 @@ uint16_t store_bit16(uint16_t bitmap, int position, uint8_t value){
     return bitmap;
 }
 
+avdtp_message_type_t avdtp_get_signaling_packet_type(uint8_t * packet){
+    return (avdtp_message_type_t) (packet[0] & 0x03);
+}
+
 int avdtp_read_signaling_header(avdtp_signaling_packet_t * signaling_header, uint8_t * packet, uint16_t size){
     int pos = 0;
     if (size < 2) return pos;   
diff --git a/src/classic/avdtp_util.h b/src/classic/avdtp_util.h
index 3bc663d4e..1df7f1735 100644
--- a/src/classic/avdtp_util.h
+++ b/src/classic/avdtp_util.h
@@ -65,6 +65,8 @@ static inline uint8_t avdtp_header(uint8_t tr_label, avdtp_packet_type_t packet_
     return (tr_label<<4) | ((uint8_t)packet_type<<2) | (uint8_t)msg_type;
 }
 
+avdtp_message_type_t avdtp_get_signaling_packet_type(uint8_t * packet);
+
 int     avdtp_read_signaling_header(avdtp_signaling_packet_t * signaling_header, uint8_t * packet, uint16_t size);
 
 uint16_t store_bit16(uint16_t bitmap, int position, uint8_t value);