xref: /btstack/src/classic/avdtp_util.c (revision 3e3fbf3df68dce1ab27af5efcdf0ea980eb93f6c)
1 /*
2  * Copyright (C) 2016 BlueKitchen GmbH
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  *    notice, this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  *    notice, this list of conditions and the following disclaimer in the
12  *    documentation and/or other materials provided with the distribution.
13  * 3. Neither the name of the copyright holders nor the names of
14  *    contributors may be used to endorse or promote products derived
15  *    from this software without specific prior written permission.
16  * 4. Any redistribution, use, or modification is done solely for
17  *    personal benefit and not for any commercial purpose or for
18  *    monetary gain.
19  *
20  * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
21  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
24  * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
30  * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
31  * SUCH DAMAGE.
32  *
33  * Please inquire about commercial licensing options at
34  * [email protected]
35  *
36  */
37 
38 #define __BTSTACK_FILE__ "avdtp_util.c"
39 
40 
41 #include <stdint.h>
42 #include <stdio.h>
43 #include <stdlib.h>
44 #include <string.h>
45 #include <unistd.h>
46 
47 #include "btstack.h"
48 #include "avdtp.h"
49 #include "avdtp_util.h"
50 
51 inline uint8_t avdtp_header(uint8_t tr_label, avdtp_packet_type_t packet_type, avdtp_message_type_t msg_type){
52     return (tr_label<<4) | ((uint8_t)packet_type<<2) | (uint8_t)msg_type;
53 }
54 
55 void avdtp_initialize_stream_endpoint(avdtp_stream_endpoint_t * stream_endpoint){
56     stream_endpoint->connection = NULL;
57     stream_endpoint->state = AVDTP_STREAM_ENDPOINT_IDLE;
58     stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE;
59     stream_endpoint->initiator_config_state = AVDTP_INITIATOR_STREAM_CONFIG_IDLE;
60     stream_endpoint->remote_sep_index = AVDTP_INVALID_SEP_INDEX;
61     stream_endpoint->media_disconnect = 0;
62     stream_endpoint->sep.in_use = 0;
63     stream_endpoint->remote_sep_index = 0;
64 }
65 
66 avdtp_stream_endpoint_t * avdtp_stream_endpoint_for_seid(uint16_t seid, avdtp_context_t * context){
67     btstack_linked_list_iterator_t it;
68     btstack_linked_list_iterator_init(&it, &context->stream_endpoints);
69     while (btstack_linked_list_iterator_has_next(&it)){
70         avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it);
71         if (stream_endpoint->sep.seid == seid){
72             return stream_endpoint;
73         }
74     }
75     return NULL;
76 }
77 
78 avdtp_connection_t * avdtp_connection_for_bd_addr(bd_addr_t addr, avdtp_context_t * context){
79     btstack_linked_list_iterator_t it;
80     btstack_linked_list_iterator_init(&it, &context->connections);
81     while (btstack_linked_list_iterator_has_next(&it)){
82         avdtp_connection_t * connection = (avdtp_connection_t *)btstack_linked_list_iterator_next(&it);
83         if (memcmp(addr, connection->remote_addr, 6) != 0) continue;
84         return connection;
85     }
86     return NULL;
87 }
88 
89 avdtp_connection_t * avdtp_connection_for_l2cap_signaling_cid(uint16_t l2cap_cid, avdtp_context_t * context){
90     btstack_linked_list_iterator_t it;
91     btstack_linked_list_iterator_init(&it, &context->connections);
92     while (btstack_linked_list_iterator_has_next(&it)){
93         avdtp_connection_t * connection = (avdtp_connection_t *)btstack_linked_list_iterator_next(&it);
94         if (connection->l2cap_signaling_cid != l2cap_cid) continue;
95         return connection;
96     }
97     return NULL;
98 }
99 
100 avdtp_stream_endpoint_t * avdtp_stream_endpoint_for_l2cap_cid(uint16_t l2cap_cid, avdtp_context_t * context){
101     btstack_linked_list_iterator_t it;
102     btstack_linked_list_iterator_init(&it, &context->stream_endpoints);
103     while (btstack_linked_list_iterator_has_next(&it)){
104         avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it);
105         if (stream_endpoint->l2cap_media_cid == l2cap_cid){
106             return stream_endpoint;
107         }
108         if (stream_endpoint->l2cap_reporting_cid == l2cap_cid){
109             return stream_endpoint;
110         }
111         if (stream_endpoint->l2cap_recovery_cid == l2cap_cid){
112             return stream_endpoint;
113         }
114     }
115     return NULL;
116 }
117 
118 avdtp_stream_endpoint_t * avdtp_stream_endpoint_with_seid(uint8_t seid, avdtp_context_t * context){
119     btstack_linked_list_iterator_t it;
120     btstack_linked_list_iterator_init(&it, &context->stream_endpoints);
121     while (btstack_linked_list_iterator_has_next(&it)){
122         avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it);
123         if (stream_endpoint->sep.seid == seid){
124             return stream_endpoint;
125         }
126     }
127     return NULL;
128 }
129 
130 avdtp_stream_endpoint_t * avdtp_stream_endpoint_associated_with_acp_seid(uint16_t acp_seid, avdtp_context_t * context){
131     btstack_linked_list_iterator_t it;
132     btstack_linked_list_iterator_init(&it, &context->stream_endpoints);
133     while (btstack_linked_list_iterator_has_next(&it)){
134         avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it);
135 
136         if (stream_endpoint->remote_sep_index != AVDTP_INVALID_SEP_INDEX){
137             if (!stream_endpoint->connection) continue;
138             if (stream_endpoint->connection->remote_seps[stream_endpoint->remote_sep_index].seid == acp_seid){
139                 return stream_endpoint;
140             }
141         }
142     }
143     return NULL;
144 }
145 
146 int get_bit16(uint16_t bitmap, int position){
147     return (bitmap >> position) & 1;
148 }
149 
150 uint8_t store_bit16(uint16_t bitmap, int position, uint8_t value){
151     if (value){
152         bitmap |= 1 << position;
153     } else {
154         bitmap &= ~ (1 << position);
155     }
156     return bitmap;
157 }
158 
159 int avdtp_read_signaling_header(avdtp_signaling_packet_t * signaling_header, uint8_t * packet, uint16_t size){
160     int pos = 0;
161     if (size < 2) return pos;
162     signaling_header->transaction_label = packet[pos] >> 4;
163     signaling_header->packet_type = (avdtp_packet_type_t)((packet[pos] >> 2) & 0x03);
164     signaling_header->message_type = (avdtp_message_type_t) (packet[pos] & 0x03);
165     pos++;
166     memset(signaling_header->command, 0, sizeof(signaling_header->command));
167     switch (signaling_header->packet_type){
168         case AVDTP_SINGLE_PACKET:
169             signaling_header->num_packets = 0;
170             signaling_header->offset = 0;
171             signaling_header->size = 0;
172             break;
173         case AVDTP_END_PACKET:
174             signaling_header->num_packets = 0;
175             break;
176         case AVDTP_START_PACKET:
177             signaling_header->num_packets = packet[pos++];
178             signaling_header->size = 0;
179             signaling_header->offset = 0;
180             break;
181         case AVDTP_CONTINUE_PACKET:
182             if (signaling_header->num_packets <= 0) {
183                 log_info("    ERROR: wrong num fragmented packets\n");
184                 break;
185             }
186             signaling_header->num_packets--;
187             break;
188     }
189     signaling_header->signal_identifier = packet[pos++] & 0x3f;
190     return pos;
191 }
192 
193 int avdtp_pack_service_capabilities(uint8_t * buffer, int size, avdtp_capabilities_t caps, avdtp_service_category_t category, uint8_t pack_all_capabilities){
194     UNUSED(size);
195 
196     int i;
197     // pos = 0 reserved for length
198     int pos = 1;
199     switch(category){
200         case AVDTP_MEDIA_TRANSPORT:
201         case AVDTP_REPORTING:
202             break;
203         case AVDTP_DELAY_REPORTING:
204             if (!pack_all_capabilities) break;
205             break;
206         case AVDTP_RECOVERY:
207             buffer[pos++] = caps.recovery.recovery_type; // 0x01=RFC2733
208             buffer[pos++] = caps.recovery.maximum_recovery_window_size;
209             buffer[pos++] = caps.recovery.maximum_number_media_packets;
210             break;
211         case AVDTP_CONTENT_PROTECTION:
212             buffer[pos++] = caps.content_protection.cp_type_value_len + 2;
213             big_endian_store_16(buffer, pos, caps.content_protection.cp_type);
214             pos += 2;
215             memcpy(buffer+pos, caps.content_protection.cp_type_value, caps.content_protection.cp_type_value_len);
216             break;
217         case AVDTP_HEADER_COMPRESSION:
218             buffer[pos++] = (caps.header_compression.back_ch << 7) | (caps.header_compression.media << 6) | (caps.header_compression.recovery << 5);
219             break;
220         case AVDTP_MULTIPLEXING:
221             buffer[pos++] = caps.multiplexing_mode.fragmentation << 7;
222             for (i=0; i<caps.multiplexing_mode.transport_identifiers_num; i++){
223                 buffer[pos++] = caps.multiplexing_mode.transport_session_identifiers[i] << 7;
224                 buffer[pos++] = caps.multiplexing_mode.tcid[i] << 7;
225                 // media, reporting. recovery
226             }
227             break;
228         case AVDTP_MEDIA_CODEC:
229             buffer[pos++] = ((uint8_t)caps.media_codec.media_type) << 4;
230             buffer[pos++] = (uint8_t)caps.media_codec.media_codec_type;
231             for (i = 0; i<caps.media_codec.media_codec_information_len; i++){
232                 buffer[pos++] = caps.media_codec.media_codec_information[i];
233             }
234             break;
235         default:
236             break;
237     }
238     buffer[0] = pos - 1; // length
239     return pos;
240 }
241 
242 static int avdtp_unpack_service_capabilities_has_errors(avdtp_connection_t * connection, avdtp_service_category_t category, uint8_t cap_len){
243     connection->error_code = 0;
244 
245     if (category == AVDTP_SERVICE_CATEGORY_INVALID_0 ||
246         (category == AVDTP_SERVICE_CATEGORY_INVALID_FF && connection->signaling_packet.signal_identifier == AVDTP_SI_RECONFIGURE)){
247         log_info("    ERROR: BAD SERVICE CATEGORY %d\n", category);
248         connection->reject_service_category = category;
249         connection->error_code = BAD_SERV_CATEGORY;
250         return 1;
251     }
252 
253     if (connection->signaling_packet.signal_identifier == AVDTP_SI_RECONFIGURE){
254         if (category != AVDTP_CONTENT_PROTECTION && category != AVDTP_MEDIA_CODEC){
255             log_info("    ERROR: REJECT CATEGORY, INVALID_CAPABILITIES\n");
256             connection->reject_service_category = category;
257             connection->error_code = INVALID_CAPABILITIES;
258             return 1;
259         }
260     }
261 
262     switch(category){
263         case AVDTP_MEDIA_TRANSPORT:
264             if (cap_len != 0){
265                 log_info("    ERROR: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n");
266                 connection->reject_service_category = category;
267                 connection->error_code = BAD_MEDIA_TRANSPORT_FORMAT;
268                 return 1;
269             }
270             break;
271         case AVDTP_REPORTING:
272         case AVDTP_DELAY_REPORTING:
273             if (cap_len != 0){
274                 log_info("    ERROR: REJECT CATEGORY, BAD_LENGTH\n");
275                 connection->reject_service_category = category;
276                 connection->error_code = BAD_LENGTH;
277                 return 1;
278             }
279             break;
280         case AVDTP_RECOVERY:
281             if (cap_len < 3){
282                 log_info("    ERROR: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n");
283                 connection->reject_service_category = category;
284                 connection->error_code = BAD_RECOVERY_FORMAT;
285                 return 1;
286             }
287             break;
288         case AVDTP_CONTENT_PROTECTION:
289             if (cap_len < 2){
290                 log_info("    ERROR: REJECT CATEGORY, BAD_CP_FORMAT\n");
291                 connection->reject_service_category = category;
292                 connection->error_code = BAD_CP_FORMAT;
293                 return 1;
294             }
295             break;
296         case AVDTP_HEADER_COMPRESSION:
297             break;
298         case AVDTP_MULTIPLEXING:
299             break;
300         case AVDTP_MEDIA_CODEC:
301             break;
302         default:
303             break;
304     }
305     return 0;
306 }
307 
308 uint16_t avdtp_unpack_service_capabilities(avdtp_connection_t * connection, avdtp_capabilities_t * caps, uint8_t * packet, uint16_t size){
309     if (size == 0) return 0;
310 
311     uint16_t registered_service_categories = 0;
312     int pos = 0;
313     int i;
314     avdtp_service_category_t category = (avdtp_service_category_t)packet[pos++];
315     uint8_t cap_len = packet[pos++];
316 
317     if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)) return 0;
318     int processed_cap_len = 0;
319     int rfa = 0;
320 
321     while (pos < size){
322         if (cap_len > size - pos){
323             connection->reject_service_category = category;
324             connection->error_code = BAD_LENGTH;
325             return 0;
326         }
327         rfa = 0;
328         processed_cap_len = pos;
329         switch(category){
330             case AVDTP_RECOVERY:
331                 caps->recovery.recovery_type = packet[pos++];
332                 caps->recovery.maximum_recovery_window_size = packet[pos++];
333                 caps->recovery.maximum_number_media_packets = packet[pos++];
334                 break;
335             case AVDTP_CONTENT_PROTECTION:
336                 caps->content_protection.cp_type = big_endian_read_16(packet, pos);
337                 pos+=2;
338 
339                 caps->content_protection.cp_type_value_len = cap_len - 2;
340                 pos += caps->content_protection.cp_type_value_len;
341 
342                 // connection->reject_service_category = category;
343                 // connection->error_code = UNSUPPORTED_CONFIGURATION;
344                 // support for content protection goes here
345                 break;
346 
347             case AVDTP_HEADER_COMPRESSION:
348                 caps->header_compression.back_ch  = packet[pos] >> 7;
349                 caps->header_compression.media    = packet[pos] >> 6;
350                 caps->header_compression.recovery = packet[pos] >> 5;
351                 pos++;
352                 break;
353             case AVDTP_MULTIPLEXING:
354                 caps->multiplexing_mode.fragmentation = packet[pos++] >> 7;
355                 // read [tsid, tcid] for media, reporting. recovery respectively
356                 caps->multiplexing_mode.transport_identifiers_num = 3;
357                 for (i=0; i<caps->multiplexing_mode.transport_identifiers_num; i++){
358                     caps->multiplexing_mode.transport_session_identifiers[i] = packet[pos++] >> 7;
359                     caps->multiplexing_mode.tcid[i] = packet[pos++] >> 7;
360                 }
361                 break;
362             case AVDTP_MEDIA_CODEC:
363                 caps->media_codec.media_type = packet[pos++] >> 4;
364                 caps->media_codec.media_codec_type = packet[pos++];
365                 caps->media_codec.media_codec_information_len = cap_len - 2;
366                 caps->media_codec.media_codec_information = &packet[pos];
367                 pos += caps->media_codec.media_codec_information_len;
368                 break;
369             case AVDTP_MEDIA_TRANSPORT:
370             case AVDTP_REPORTING:
371             case AVDTP_DELAY_REPORTING:
372                 pos += cap_len;
373                 break;
374             default:
375                 pos += cap_len;
376                 rfa = 1;
377                 break;
378         }
379         processed_cap_len = pos - processed_cap_len;
380 
381         if (cap_len == processed_cap_len){
382             if (!rfa) {
383                 registered_service_categories = store_bit16(registered_service_categories, category, 1);
384             }
385             if (pos < size-2){
386                 //int old_pos = pos;
387                 category = (avdtp_service_category_t)packet[pos++];
388                 cap_len = packet[pos++];
389                 if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)) return 0;
390             }
391         }
392     }
393     return registered_service_categories;
394 }
395 
396 void avdtp_prepare_capabilities(avdtp_signaling_packet_t * signaling_packet, uint8_t transaction_label, uint16_t registered_service_categories, avdtp_capabilities_t capabilities, uint8_t identifier){
397     if (signaling_packet->offset) return;
398     uint8_t pack_all_capabilities = 1;
399     signaling_packet->message_type = AVDTP_RESPONSE_ACCEPT_MSG;
400     int i;
401 
402     signaling_packet->size = 0;
403     memset(signaling_packet->command, 0 , sizeof(signaling_packet->command));
404 
405 
406     switch (identifier) {
407         case AVDTP_SI_GET_CAPABILITIES:
408             pack_all_capabilities = 0;
409             break;
410         case AVDTP_SI_GET_ALL_CAPABILITIES:
411             pack_all_capabilities = 1;
412             break;
413         case AVDTP_SI_SET_CONFIGURATION:
414             signaling_packet->command[signaling_packet->size++] = signaling_packet->acp_seid << 2;
415             signaling_packet->command[signaling_packet->size++] = signaling_packet->int_seid << 2;
416             signaling_packet->message_type = AVDTP_CMD_MSG;
417             break;
418         case AVDTP_SI_RECONFIGURE:
419             signaling_packet->command[signaling_packet->size++] = signaling_packet->acp_seid << 2;
420             signaling_packet->message_type = AVDTP_CMD_MSG;
421             break;
422         default:
423             log_error("avdtp_prepare_capabilities wrong identifier %d", identifier);
424             break;
425     }
426 
427     for (i = 1; i < 9; i++){
428         int registered_category = get_bit16(registered_service_categories, i);
429         if (!registered_category && (identifier == AVDTP_SI_SET_CONFIGURATION || identifier == AVDTP_SI_RECONFIGURE)){
430             // TODO: introduce bitmap of mandatory categories
431             if (i == 1){
432                 registered_category = 1;
433             }
434         }
435         if (registered_category){
436             // service category
437             signaling_packet->command[signaling_packet->size++] = i;
438             signaling_packet->size += avdtp_pack_service_capabilities(signaling_packet->command+signaling_packet->size, sizeof(signaling_packet->command)-signaling_packet->size, capabilities, (avdtp_service_category_t)i, pack_all_capabilities);
439         }
440     }
441 
442     signaling_packet->signal_identifier = identifier;
443     signaling_packet->transaction_label = transaction_label;
444 }
445 
446 int avdtp_signaling_create_fragment(uint16_t cid, avdtp_signaling_packet_t * signaling_packet, uint8_t * out_buffer) {
447     int mtu = l2cap_get_remote_mtu_for_local_cid(cid);
448     int data_len = 0;
449 
450     uint16_t offset = signaling_packet->offset;
451     uint16_t pos = 1;
452 
453     if (offset == 0){
454         if (signaling_packet->size <= mtu - 2){
455             signaling_packet->packet_type = AVDTP_SINGLE_PACKET;
456             out_buffer[pos++] = signaling_packet->signal_identifier;
457             data_len = signaling_packet->size;
458         } else {
459             signaling_packet->packet_type = AVDTP_START_PACKET;
460             out_buffer[pos++] = (mtu + signaling_packet->size)/ (mtu-1);
461             out_buffer[pos++] = signaling_packet->signal_identifier;
462             data_len = mtu - 3;
463             signaling_packet->offset = data_len;
464         }
465     } else {
466         int remaining_bytes = signaling_packet->size - offset;
467         if (remaining_bytes <= mtu - 1){
468             signaling_packet->packet_type = AVDTP_END_PACKET;
469             data_len = remaining_bytes;
470             signaling_packet->offset = 0;
471         } else{
472             signaling_packet->packet_type = AVDTP_CONTINUE_PACKET;
473             data_len = mtu - 1;
474             signaling_packet->offset += data_len;
475         }
476     }
477     out_buffer[0] = avdtp_header(signaling_packet->transaction_label, signaling_packet->packet_type, signaling_packet->message_type);
478     memcpy(out_buffer+pos, signaling_packet->command + offset, data_len);
479     pos += data_len;
480     return pos;
481 }
482 
483 
484 void avdtp_signaling_emit_connection_established(btstack_packet_handler_t callback, uint16_t avdtp_cid, bd_addr_t addr, uint8_t status){
485     if (!callback) return;
486     uint8_t event[12];
487     int pos = 0;
488     event[pos++] = HCI_EVENT_AVDTP_META;
489     event[pos++] = sizeof(event) - 2;
490     event[pos++] = AVDTP_SUBEVENT_SIGNALING_CONNECTION_ESTABLISHED;
491     little_endian_store_16(event, pos, avdtp_cid);
492     pos += 2;
493     reverse_bd_addr(addr,&event[pos]);
494     pos += 6;
495     event[pos++] = status;
496     (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
497 }
498 
499 void avdtp_streaming_emit_can_send_media_packet_now(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t seid, uint16_t sequence_number){
500     if (!callback) return;
501     uint8_t event[8];
502     int pos = 0;
503     event[pos++] = HCI_EVENT_AVDTP_META;
504     event[pos++] = sizeof(event) - 2;
505     event[pos++] = AVDTP_SUBEVENT_STREAMING_CAN_SEND_MEDIA_PACKET_NOW;
506     little_endian_store_16(event, pos, avdtp_cid);
507     pos += 2;
508     event[pos++] = seid;
509     little_endian_store_16(event, pos, sequence_number);
510     pos += 2;
511     (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
512 }
513 
514 void avdtp_streaming_emit_connection_established(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t int_seid, uint8_t acp_seid, uint8_t status){
515     if (!callback) return;
516     uint8_t event[8];
517     int pos = 0;
518     event[pos++] = HCI_EVENT_AVDTP_META;
519     event[pos++] = sizeof(event) - 2;
520     event[pos++] = AVDTP_SUBEVENT_STREAMING_CONNECTION_ESTABLISHED;
521     little_endian_store_16(event, pos, avdtp_cid);
522     pos += 2;
523     event[pos++] = int_seid;
524     event[pos++] = acp_seid;
525     event[pos++] = status;
526     (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
527 }
528 
529 void avdtp_signaling_emit_sep(btstack_packet_handler_t callback, uint16_t avdtp_cid, avdtp_sep_t sep){
530     if (!callback) return;
531     uint8_t event[9];
532     int pos = 0;
533     event[pos++] = HCI_EVENT_AVDTP_META;
534     event[pos++] = sizeof(event) - 2;
535     event[pos++] = AVDTP_SUBEVENT_SIGNALING_SEP_FOUND;
536     little_endian_store_16(event, pos, avdtp_cid);
537     pos += 2;
538     event[pos++] = sep.seid;
539     event[pos++] = sep.in_use;
540     event[pos++] = sep.media_type;
541     event[pos++] = sep.type;
542     (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
543 }
544 
545 void avdtp_signaling_emit_accept(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t int_seid, avdtp_signal_identifier_t identifier, uint8_t status){
546     if (!callback) return;
547     uint8_t event[8];
548     int pos = 0;
549     event[pos++] = HCI_EVENT_AVDTP_META;
550     event[pos++] = sizeof(event) - 2;
551     event[pos++] = AVDTP_SUBEVENT_SIGNALING_ACCEPT;
552     little_endian_store_16(event, pos, avdtp_cid);
553     pos += 2;
554     event[pos++] = int_seid;
555     event[pos++] = identifier;
556     event[pos++] = status;
557     (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
558 }
559 
560 void avdtp_signaling_emit_reject(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t int_seid, avdtp_signal_identifier_t identifier){
561     if (!callback) return;
562     uint8_t event[7];
563     int pos = 0;
564     event[pos++] = HCI_EVENT_AVDTP_META;
565     event[pos++] = sizeof(event) - 2;
566     event[pos++] = AVDTP_SUBEVENT_SIGNALING_REJECT;
567     little_endian_store_16(event, pos, avdtp_cid);
568     pos += 2;
569     event[pos++] = int_seid;
570     event[pos++] = identifier;
571     (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
572 }
573 
574 void avdtp_signaling_emit_general_reject(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t int_seid, avdtp_signal_identifier_t identifier){
575     if (!callback) return;
576     uint8_t event[7];
577     int pos = 0;
578     event[pos++] = HCI_EVENT_AVDTP_META;
579     event[pos++] = sizeof(event) - 2;
580     event[pos++] = AVDTP_SUBEVENT_SIGNALING_GENERAL_REJECT;
581     little_endian_store_16(event, pos, avdtp_cid);
582     pos += 2;
583     event[pos++] = int_seid;
584     event[pos++] = identifier;
585     (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
586 }
587 
588 void avdtp_signaling_emit_media_codec_sbc_capability(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t int_seid, uint8_t acp_seid, adtvp_media_codec_capabilities_t media_codec){
589     if (!callback) return;
590     uint8_t event[15];
591     int pos = 0;
592     event[pos++] = HCI_EVENT_AVDTP_META;
593     event[pos++] = sizeof(event) - 2;
594     event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_SBC_CAPABILITY;
595     little_endian_store_16(event, pos, avdtp_cid);
596     pos += 2;
597     event[pos++] = int_seid;
598     event[pos++] = acp_seid;
599     event[pos++] = media_codec.media_type;
600     event[pos++] = media_codec.media_codec_information[0] >> 4;
601     event[pos++] = media_codec.media_codec_information[0] & 0x0F;
602     event[pos++] = media_codec.media_codec_information[1] >> 4;
603     event[pos++] = (media_codec.media_codec_information[1] & 0x0F) >> 2;
604     event[pos++] = media_codec.media_codec_information[1] & 0x03;
605     event[pos++] = media_codec.media_codec_information[2];
606     event[pos++] = media_codec.media_codec_information[3];
607     (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
608 }
609 
610 void avdtp_signaling_emit_media_codec_other_capability(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t int_seid, uint8_t acp_seid, adtvp_media_codec_capabilities_t media_codec){
611     if (!callback) return;
612         uint8_t event[111];
613     int pos = 0;
614     event[pos++] = HCI_EVENT_AVDTP_META;
615     event[pos++] = sizeof(event) - 2;
616     event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CAPABILITY;
617     little_endian_store_16(event, pos, avdtp_cid);
618     pos += 2;
619     event[pos++] = int_seid;
620     event[pos++] = acp_seid;
621     event[pos++] = media_codec.media_type;
622     little_endian_store_16(event, pos, media_codec.media_codec_type);
623     pos += 2;
624     little_endian_store_16(event, pos, media_codec.media_codec_information_len);
625     pos += 2;
626     if (media_codec.media_codec_information_len < 100){
627         memcpy(event+pos, media_codec.media_codec_information, media_codec.media_codec_information_len);
628     } else {
629         memcpy(event+pos, media_codec.media_codec_information, 100);
630     }
631     (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
632 }
633 
634 static inline void avdtp_signaling_emit_media_codec_sbc(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t int_seid, uint8_t acp_seid, adtvp_media_codec_capabilities_t media_codec, uint8_t reconfigure){
635     if (!callback) return;
636     uint8_t event[16+2];
637     int pos = 0;
638     event[pos++] = HCI_EVENT_AVDTP_META;
639     event[pos++] = sizeof(event) - 2;
640 
641     event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_SBC_CONFIGURATION;
642     little_endian_store_16(event, pos, avdtp_cid);
643     pos += 2;
644     event[pos++] = int_seid;
645     event[pos++] = acp_seid;
646     event[pos++] = reconfigure;
647 
648     uint8_t num_channels = 0;
649     uint16_t sampling_frequency = 0;
650     uint8_t subbands = 0;
651     uint8_t block_length = 0;
652 
653     uint8_t sampling_frequency_bitmap = media_codec.media_codec_information[0] >> 4;
654     uint8_t channel_mode_bitmap = media_codec.media_codec_information[0] & 0x0F;
655     uint8_t block_length_bitmap = media_codec.media_codec_information[1] >> 4;
656     uint8_t subbands_bitmap = (media_codec.media_codec_information[1] & 0x0F) >> 2;
657 
658     if (channel_mode_bitmap & AVDTP_SBC_MONO){
659         num_channels = 1;
660     }
661     if ( (channel_mode_bitmap & AVDTP_SBC_JOINT_STEREO) ||
662          (channel_mode_bitmap & AVDTP_SBC_STEREO) ||
663          (channel_mode_bitmap & AVDTP_SBC_DUAL_CHANNEL) ){
664         num_channels = 2;
665     }
666 
667     if (sampling_frequency_bitmap & AVDTP_SBC_16000){
668         sampling_frequency = 16000;
669     }
670     if (sampling_frequency_bitmap & AVDTP_SBC_32000){
671         sampling_frequency = 32000;
672     }
673     if (sampling_frequency_bitmap & AVDTP_SBC_44100){
674         sampling_frequency = 44100;
675     }
676     if (sampling_frequency_bitmap & AVDTP_SBC_48000){
677         sampling_frequency = 48000;
678     }
679 
680     if (subbands_bitmap & AVDTP_SBC_SUBBANDS_4){
681         subbands = 4;
682     }
683     if (subbands_bitmap & AVDTP_SBC_SUBBANDS_8){
684         subbands = 8;
685     }
686 
687     if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_4){
688         block_length = 4;
689     }
690     if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_8){
691         block_length = 8;
692     }
693     if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_12){
694         block_length = 12;
695     }
696     if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_16){
697         block_length = 16;
698     }
699 
700     event[pos++] = media_codec.media_type;
701     little_endian_store_16(event, pos, sampling_frequency);
702     pos += 2;
703 
704     event[pos++] = channel_mode_bitmap;
705     event[pos++] = num_channels;
706     event[pos++] = block_length;
707     event[pos++] = subbands;
708     event[pos++] = media_codec.media_codec_information[1] & 0x03;
709     event[pos++] = media_codec.media_codec_information[2];
710     event[pos++] = media_codec.media_codec_information[3];
711     (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
712 }
713 
714 void avdtp_signaling_emit_media_codec_sbc_configuration(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t int_seid, uint8_t acp_seid, adtvp_media_codec_capabilities_t media_codec){
715     if (!callback) return;
716     avdtp_signaling_emit_media_codec_sbc(callback, avdtp_cid, int_seid, acp_seid, media_codec, 0);
717 }
718 
719 void avdtp_signaling_emit_media_codec_sbc_reconfiguration(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t int_seid, uint8_t acp_seid, adtvp_media_codec_capabilities_t media_codec){
720     if (!callback) return;
721     avdtp_signaling_emit_media_codec_sbc(callback, avdtp_cid, int_seid, acp_seid, media_codec, 1);
722 }
723 
724 static inline void avdtp_signaling_emit_media_codec_other(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t int_seid, uint8_t acp_seid, adtvp_media_codec_capabilities_t media_codec, uint8_t reconfigure){
725     uint8_t event[112];
726     int pos = 0;
727     event[pos++] = HCI_EVENT_AVDTP_META;
728     event[pos++] = sizeof(event) - 2;
729     event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CONFIGURATION;
730     little_endian_store_16(event, pos, avdtp_cid);
731     pos += 2;
732     event[pos++] = int_seid;
733     event[pos++] = acp_seid;
734     event[pos++] = reconfigure;
735 
736     event[pos++] = media_codec.media_type;
737     little_endian_store_16(event, pos, media_codec.media_codec_type);
738     pos += 2;
739     little_endian_store_16(event, pos, media_codec.media_codec_information_len);
740     pos += 2;
741 
742     if (media_codec.media_codec_information_len < 100){
743         memcpy(event+pos, media_codec.media_codec_information, media_codec.media_codec_information_len);
744     } else {
745         memcpy(event+pos, media_codec.media_codec_information, 100);
746     }
747     (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
748 }
749 
750 void avdtp_signaling_emit_media_codec_other_configuration(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t int_seid, uint8_t acp_seid, adtvp_media_codec_capabilities_t media_codec){
751     if (!callback) return;
752     avdtp_signaling_emit_media_codec_other(callback, avdtp_cid, int_seid, acp_seid, media_codec, 0);
753 }
754 
755 void avdtp_signaling_emit_media_codec_other_reconfiguration(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t int_seid, uint8_t acp_seid, adtvp_media_codec_capabilities_t media_codec){
756     if (!callback) return;
757     avdtp_signaling_emit_media_codec_other(callback, avdtp_cid, int_seid, acp_seid, media_codec, 1);
758 }
759 
760 
761 void avdtp_request_can_send_now_acceptor(avdtp_connection_t * connection, uint16_t l2cap_cid){
762     connection->wait_to_send_acceptor = 1;
763     l2cap_request_can_send_now_event(l2cap_cid);
764 }
765 void avdtp_request_can_send_now_initiator(avdtp_connection_t * connection, uint16_t l2cap_cid){
766     connection->wait_to_send_initiator = 1;
767     l2cap_request_can_send_now_event(l2cap_cid);
768 }
769 void avdtp_request_can_send_now_self(avdtp_connection_t * connection, uint16_t l2cap_cid){
770     connection->wait_to_send_self = 1;
771     l2cap_request_can_send_now_event(l2cap_cid);
772 }
773 
774 uint8_t avdtp_get_index_of_remote_stream_endpoint_with_seid(avdtp_stream_endpoint_t * stream_endpoint, uint16_t seid){
775     if (!stream_endpoint->connection) return 0xFF;
776     if (stream_endpoint->connection->remote_seps[stream_endpoint->remote_sep_index].seid == seid){
777         return stream_endpoint->remote_sep_index;
778     }
779     int i;
780     for (i=0; i < stream_endpoint->connection->remote_seps_num; i++){
781         if (stream_endpoint->connection->remote_seps[i].seid == seid){
782             return i;
783         }
784     }
785     return 0xFF;
786 }
787 
788 uint8_t avdtp_find_remote_sep(avdtp_connection_t * connection, uint8_t remote_seid){
789     if (!connection) return 0xFF;
790     int i;
791     for (i = 0; i < connection->remote_seps_num; i++){
792         if (connection->remote_seps[i].seid == remote_seid){
793             return i;
794         }
795     }
796     return 0xFF;
797 }
798