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_initiator.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 #include "avdtp_initiator.h" 51 52 53 static int avdtp_initiator_send_signaling_cmd(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label){ 54 uint8_t command[2]; 55 command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); 56 command[1] = (uint8_t)identifier; 57 return l2cap_send(cid, command, sizeof(command)); 58 } 59 60 static int avdtp_initiator_send_signaling_cmd_with_seid(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label, uint8_t sep_id){ 61 uint8_t command[3]; 62 command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); 63 command[1] = (uint8_t)identifier; 64 command[2] = sep_id << 2; 65 return l2cap_send(cid, command, sizeof(command)); 66 } 67 68 static void avdtp_signaling_emit_media_codec_capability(btstack_packet_handler_t callback, uint16_t con_handle, avdtp_sep_t sep){ 69 if (get_bit16(sep.registered_service_categories, AVDTP_MEDIA_CODEC)){ 70 switch (sep.capabilities.media_codec.media_codec_type){ 71 case AVDTP_CODEC_SBC: 72 avdtp_signaling_emit_media_codec_sbc_capability(callback, con_handle, sep.capabilities.media_codec); 73 break; 74 default: 75 avdtp_signaling_emit_media_codec_other_capability(callback, con_handle, sep.capabilities.media_codec); 76 break; 77 } 78 } 79 } 80 81 static void avdtp_signaling_emit_media_codec_configuration(btstack_packet_handler_t callback, uint16_t con_handle, avdtp_sep_t sep){ 82 if (get_bit16(sep.registered_service_categories, AVDTP_MEDIA_CODEC)){ 83 switch (sep.capabilities.media_codec.media_codec_type){ 84 case AVDTP_CODEC_SBC: 85 avdtp_signaling_emit_media_codec_sbc_configuration(callback, con_handle, sep.capabilities.media_codec); 86 break; 87 default: 88 avdtp_signaling_emit_media_codec_other_configuration(callback, con_handle, sep.capabilities.media_codec); 89 break; 90 } 91 } 92 } 93 94 void avdtp_initiator_stream_config_subsm(avdtp_connection_t * connection, uint8_t *packet, uint16_t size, int offset, avdtp_context_t * context){ 95 int status = 0; 96 avdtp_stream_endpoint_t * stream_endpoint = NULL; 97 98 uint8_t remote_sep_index; 99 avdtp_sep_t sep; 100 if (connection->initiator_connection_state == AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ANSWER) { 101 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; 102 } else { 103 stream_endpoint = avdtp_stream_endpoint_associated_with_acp_seid(connection->acp_seid, context); 104 if (!stream_endpoint){ 105 stream_endpoint = avdtp_stream_endpoint_with_seid(connection->int_seid, context); 106 } 107 if (!stream_endpoint) return; 108 sep.seid = connection->acp_seid; 109 110 printf(" INT: local seid %d, remote seid %d, ident %d \n", connection->int_seid, connection->acp_seid, connection->signaling_packet.signal_identifier); 111 if (stream_endpoint->initiator_config_state != AVDTP_INITIATOR_W4_ANSWER) return; 112 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_STREAM_CONFIG_IDLE; 113 } 114 115 switch (connection->signaling_packet.message_type){ 116 case AVDTP_RESPONSE_ACCEPT_MSG: 117 printf(" INT: AVDTP_RESPONSE_ACCEPT_MSG: "); 118 switch (connection->signaling_packet.signal_identifier){ 119 case AVDTP_SI_DISCOVER:{ 120 printf("AVDTP_SI_DISCOVER\n"); 121 if (connection->signaling_packet.transaction_label != connection->initiator_transaction_label){ 122 printf(" unexpected transaction label, got %d, expected %d\n", connection->signaling_packet.transaction_label, connection->initiator_transaction_label); 123 status = BAD_HEADER_FORMAT; 124 break; 125 } 126 127 if (size == 3){ 128 printf(" ERROR code %02x\n", packet[offset]); 129 status = packet[offset]; 130 break; 131 } 132 133 int i; 134 for (i = offset; i < size; i += 2){ 135 sep.seid = packet[i] >> 2; 136 offset++; 137 if (sep.seid < 0x01 || sep.seid > 0x3E){ 138 printf(" invalid sep id\n"); 139 status = BAD_ACP_SEID; 140 break; 141 } 142 sep.in_use = (packet[i] >> 1) & 0x01; 143 sep.media_type = (avdtp_media_type_t)(packet[i+1] >> 4); 144 sep.type = (avdtp_sep_type_t)((packet[i+1] >> 3) & 0x01); 145 avdtp_signaling_emit_sep(context->avdtp_callback, connection->con_handle, sep); 146 } 147 break; 148 } 149 150 case AVDTP_SI_GET_CAPABILITIES: 151 case AVDTP_SI_GET_ALL_CAPABILITIES: 152 printf("AVDTP_SI_GET(_ALL)_CAPABILITIES\n"); 153 sep.registered_service_categories = avdtp_unpack_service_capabilities(connection, &sep.capabilities, packet+offset, size-offset); 154 avdtp_signaling_emit_media_codec_capability(context->avdtp_callback, connection->con_handle, sep); 155 break; 156 157 case AVDTP_SI_GET_CONFIGURATION: 158 printf("AVDTP_SI_GET_CONFIGURATION\n"); 159 sep.configured_service_categories = avdtp_unpack_service_capabilities(connection, &sep.configuration, packet+offset, size-offset); 160 161 avdtp_signaling_emit_media_codec_configuration(context->avdtp_callback, connection->con_handle, sep); 162 break; 163 164 case AVDTP_SI_RECONFIGURE: 165 printf("AVDTP_SI_RECONFIGURE\n"); 166 sep.configured_service_categories = avdtp_unpack_service_capabilities(connection, &sep.configuration, connection->signaling_packet.command+4, connection->signaling_packet.size-4); 167 // TODO check if configuration is supported 168 169 remote_sep_index = avdtp_get_index_of_remote_stream_endpoint_with_seid(stream_endpoint, sep.seid); 170 if (remote_sep_index != 0xFF){ 171 stream_endpoint->remote_sep_index = remote_sep_index; 172 stream_endpoint->remote_seps[stream_endpoint->remote_sep_index] = sep; 173 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CONFIGURED; 174 printf(" INT: update seid %d, to %p\n", stream_endpoint->remote_seps[stream_endpoint->remote_sep_index].seid, stream_endpoint); 175 } 176 break; 177 178 case AVDTP_SI_SET_CONFIGURATION:{ 179 printf("AVDTP_SI_SET_CONFIGURATION\n"); 180 sep.configured_service_categories = connection->remote_capabilities_bitmap; 181 sep.configuration = connection->remote_capabilities; 182 sep.in_use = 1; 183 // TODO check if configuration is supported 184 185 // find or add sep 186 remote_sep_index = avdtp_get_index_of_remote_stream_endpoint_with_seid(stream_endpoint, sep.seid); 187 if (remote_sep_index != 0xFF){ 188 stream_endpoint->remote_sep_index = remote_sep_index; 189 } else { 190 stream_endpoint->remote_sep_index = stream_endpoint->remote_seps_num; 191 stream_endpoint->remote_seps_num++; 192 } 193 stream_endpoint->remote_seps[stream_endpoint->remote_sep_index] = sep; 194 printf(" INT: configured remote seid %d, to %p\n", stream_endpoint->remote_seps[stream_endpoint->remote_sep_index].seid, stream_endpoint); 195 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CONFIGURED; 196 197 // TODO: use actual config 198 // TODO: consider reconfiguration 199 btstack_sbc_encoder_init(&stream_endpoint->sbc_encoder_state, SBC_MODE_STANDARD, 16, 8, 2, 44100, 53); 200 break; 201 } 202 203 case AVDTP_SI_OPEN: 204 printf("AVDTP_SI_OPEN\n"); 205 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W2_REQUEST_OPEN_STREAM) { 206 log_error("AVDTP_SI_OPEN in wrong stream endpoint state"); 207 return; 208 } 209 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED; 210 connection->query_seid = stream_endpoint->sep.seid; 211 l2cap_create_channel(context->packet_handler, connection->remote_addr, BLUETOOTH_PROTOCOL_AVDTP, 0xffff, NULL); 212 return; 213 case AVDTP_SI_START: 214 printf("AVDTP_SI_START\n"); 215 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_OPENED) { 216 log_error("AVDTP_SI_START in wrong stream endpoint state"); 217 return; 218 } 219 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_STREAMING; 220 break; 221 case AVDTP_SI_SUSPEND: 222 printf("AVDTP_SI_SUSPEND\n"); 223 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_STREAMING) { 224 log_error("AVDTP_SI_SUSPEND in wrong stream endpoint state"); 225 return; 226 } 227 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_OPENED; 228 break; 229 case AVDTP_SI_CLOSE: 230 printf("AVDTP_SI_CLOSE\n"); 231 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CLOSING; 232 break; 233 case AVDTP_SI_ABORT: 234 printf("AVDTP_SI_ABORT\n"); 235 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_ABORTING; 236 break; 237 default: 238 status = 1; 239 printf(" AVDTP_RESPONSE_ACCEPT_MSG, signal %d not implemented\n", connection->signaling_packet.signal_identifier); 240 break; 241 } 242 break; 243 case AVDTP_RESPONSE_REJECT_MSG: 244 printf(" AVDTP_RESPONSE_REJECT_MSG signal %d\n", connection->signaling_packet.signal_identifier); 245 avdtp_signaling_emit_reject(context->avdtp_callback, connection->con_handle, connection->signaling_packet.signal_identifier); 246 return; 247 case AVDTP_GENERAL_REJECT_MSG: 248 printf(" AVDTP_GENERAL_REJECT_MSG signal %d\n", connection->signaling_packet.signal_identifier); 249 avdtp_signaling_emit_general_reject(context->avdtp_callback, connection->con_handle, connection->signaling_packet.signal_identifier); 250 return; 251 default: 252 break; 253 } 254 255 connection->initiator_transaction_label++; 256 connection->int_seid = 0; 257 connection->acp_seid = 0; 258 avdtp_signaling_emit_accept(context->avdtp_callback, connection->con_handle, connection->signaling_packet.signal_identifier, status); 259 } 260 261 void avdtp_initiator_stream_config_subsm_run(avdtp_connection_t * connection, avdtp_context_t * context){ 262 int sent = 1; 263 264 switch (connection->initiator_connection_state){ 265 case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS: 266 printf(" INT: AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS\n"); 267 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ANSWER; 268 avdtp_initiator_send_signaling_cmd(connection->l2cap_signaling_cid, AVDTP_SI_DISCOVER, connection->initiator_transaction_label); 269 break; 270 case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CAPABILITIES: 271 printf(" INT: AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CAPABILITIES\n"); 272 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ANSWER; 273 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_GET_CAPABILITIES, connection->initiator_transaction_label, connection->acp_seid); 274 break; 275 case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_ALL_CAPABILITIES: 276 printf(" INT: AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_ALL_CAPABILITIES\n"); 277 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ANSWER; 278 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_GET_ALL_CAPABILITIES, connection->initiator_transaction_label, connection->acp_seid); 279 break; 280 case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CONFIGURATION: 281 printf(" INT: AVDTP_INITIATOR_W4_GET_CONFIGURATION\n"); 282 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ANSWER; 283 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_GET_CONFIGURATION, connection->initiator_transaction_label, connection->acp_seid); 284 break; 285 default: 286 sent = 0; 287 break; 288 } 289 290 if (sent) return; 291 sent = 1; 292 avdtp_stream_endpoint_t * stream_endpoint = NULL; 293 294 // printf(" run int seid %d, acp seid %d\n", connection->int_seid, connection->acp_seid); 295 296 stream_endpoint = avdtp_stream_endpoint_associated_with_acp_seid(connection->acp_seid, context); 297 if (!stream_endpoint){ 298 stream_endpoint = avdtp_stream_endpoint_with_seid(connection->int_seid, context); 299 } 300 if (!stream_endpoint) return; 301 302 avdtp_initiator_stream_endpoint_state_t stream_endpoint_state = stream_endpoint->initiator_config_state; 303 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W4_ANSWER; 304 305 switch (stream_endpoint_state){ 306 case AVDTP_INITIATOR_W2_SET_CONFIGURATION: 307 case AVDTP_INITIATOR_W2_RECONFIGURE_STREAM_WITH_SEID:{ 308 printf(" INT: AVDTP_INITIATOR_W2_(RE)CONFIGURATION bitmap, int seid %d, acp seid %d\n", connection->int_seid, connection->acp_seid); 309 // printf_hexdump( connection->remote_capabilities.media_codec.media_codec_information, connection->remote_capabilities.media_codec.media_codec_information_len); 310 connection->signaling_packet.acp_seid = connection->acp_seid; 311 connection->signaling_packet.int_seid = connection->int_seid; 312 313 connection->signaling_packet.signal_identifier = AVDTP_SI_SET_CONFIGURATION; 314 315 if (stream_endpoint_state == AVDTP_INITIATOR_W2_RECONFIGURE_STREAM_WITH_SEID){ 316 connection->signaling_packet.signal_identifier = AVDTP_SI_RECONFIGURE; 317 } 318 319 avdtp_prepare_capabilities(&connection->signaling_packet, connection->initiator_transaction_label, connection->remote_capabilities_bitmap, connection->remote_capabilities, connection->signaling_packet.signal_identifier); 320 l2cap_reserve_packet_buffer(); 321 uint8_t * out_buffer = l2cap_get_outgoing_buffer(); 322 uint16_t pos = avdtp_signaling_create_fragment(connection->l2cap_signaling_cid, &connection->signaling_packet, out_buffer); 323 if (connection->signaling_packet.packet_type != AVDTP_SINGLE_PACKET && connection->signaling_packet.packet_type != AVDTP_END_PACKET){ 324 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_FRAGMENTATED_COMMAND; 325 printf(" INT: fragmented\n"); 326 } 327 l2cap_send_prepared(connection->l2cap_signaling_cid, pos); 328 break; 329 } 330 case AVDTP_INITIATOR_FRAGMENTATED_COMMAND:{ 331 l2cap_reserve_packet_buffer(); 332 uint8_t * out_buffer = l2cap_get_outgoing_buffer(); 333 uint16_t pos = avdtp_signaling_create_fragment(connection->l2cap_signaling_cid, &connection->signaling_packet, out_buffer); 334 if (connection->signaling_packet.packet_type != AVDTP_SINGLE_PACKET && connection->signaling_packet.packet_type != AVDTP_END_PACKET){ 335 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_FRAGMENTATED_COMMAND; 336 printf(" INT: fragmented\n"); 337 } 338 l2cap_send_prepared(connection->l2cap_signaling_cid, pos); 339 break; 340 } 341 case AVDTP_INITIATOR_W2_OPEN_STREAM: 342 switch (stream_endpoint->state){ 343 case AVDTP_STREAM_ENDPOINT_W2_REQUEST_OPEN_STREAM: 344 printf(" INT: AVDTP_STREAM_ENDPOINT_W2_REQUEST_OPEN_STREAM\n"); 345 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_OPEN, connection->initiator_transaction_label, connection->acp_seid); 346 break; 347 default: 348 break; 349 } 350 break; 351 case AVDTP_INITIATOR_W2_SUSPEND_STREAM_WITH_SEID: 352 printf(" INT: AVDTP_INITIATOR_W4_SUSPEND_STREAM_WITH_SEID\n"); 353 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_SUSPEND, connection->initiator_transaction_label, connection->acp_seid); 354 break; 355 case AVDTP_INITIATOR_W2_STREAMING_START: 356 printf(" INT: AVDTP_INITIATOR_W4_STREAMING_START\n"); 357 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_START, connection->initiator_transaction_label, connection->acp_seid); 358 break; 359 case AVDTP_INITIATOR_W2_STREAMING_STOP: 360 printf(" INT: AVDTP_INITIATOR_W4_STREAMING_STOP\n"); 361 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_CLOSE, connection->initiator_transaction_label, connection->acp_seid); 362 break; 363 case AVDTP_INITIATOR_W2_STREAMING_ABORT: 364 printf(" INT: AVDTP_INITIATOR_W4_STREAMING_ABORT\n"); 365 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_ABORTING; 366 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_ABORT, connection->initiator_transaction_label, connection->acp_seid); 367 break; 368 default: 369 break; 370 } 371 372 373 // check fragmentation 374 if (connection->signaling_packet.packet_type != AVDTP_SINGLE_PACKET && connection->signaling_packet.packet_type != AVDTP_END_PACKET){ 375 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 376 } 377 } 378