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 #include <stdint.h> 41 #include <stdio.h> 42 #include <stdlib.h> 43 #include <string.h> 44 45 #include "btstack.h" 46 #include "classic/avdtp.h" 47 #include "classic/avdtp_util.h" 48 #include "classic/avdtp_initiator.h" 49 50 static int avdtp_initiator_send_signaling_cmd(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label){ 51 uint8_t command[2]; 52 command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); 53 command[1] = (uint8_t)identifier; 54 return l2cap_send(cid, command, sizeof(command)); 55 } 56 57 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){ 58 uint8_t command[3]; 59 command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); 60 command[1] = (uint8_t)identifier; 61 command[2] = sep_id << 2; 62 return l2cap_send(cid, command, sizeof(command)); 63 } 64 65 void avdtp_initiator_stream_config_subsm(avdtp_connection_t * connection, uint8_t *packet, uint16_t size, int offset, avdtp_context_t * context){ 66 // int status = 0; 67 avdtp_stream_endpoint_t * stream_endpoint = NULL; 68 69 uint8_t remote_sep_index; 70 avdtp_sep_t sep; 71 if (connection->initiator_connection_state == AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ANSWER) { 72 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; 73 } else { 74 stream_endpoint = avdtp_stream_endpoint_associated_with_acp_seid(connection->remote_seid, context); 75 if (!stream_endpoint){ 76 stream_endpoint = avdtp_stream_endpoint_with_seid(connection->local_seid, context); 77 } 78 if (!stream_endpoint) return; 79 sep.seid = connection->remote_seid; 80 81 if (stream_endpoint->initiator_config_state != AVDTP_INITIATOR_W4_ANSWER) return; 82 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_STREAM_CONFIG_IDLE; 83 } 84 85 switch (connection->signaling_packet.message_type){ 86 case AVDTP_RESPONSE_ACCEPT_MSG: 87 switch (connection->signaling_packet.signal_identifier){ 88 case AVDTP_SI_DISCOVER:{ 89 if (connection->signaling_packet.transaction_label != connection->initiator_transaction_label){ 90 log_info(" unexpected transaction label, got %d, expected %d", connection->signaling_packet.transaction_label, connection->initiator_transaction_label); 91 // status = BAD_HEADER_FORMAT; 92 break; 93 } 94 95 if (size == 3){ 96 log_info(" ERROR code %02x", packet[offset]); 97 break; 98 } 99 100 int i; 101 for (i = offset; i < size; i += 2){ 102 sep.seid = packet[i] >> 2; 103 offset++; 104 if (sep.seid < 0x01 || sep.seid > 0x3E){ 105 log_info(" invalid sep id"); 106 // status = BAD_ACP_SEID; 107 break; 108 } 109 sep.in_use = (packet[i] >> 1) & 0x01; 110 sep.media_type = (avdtp_media_type_t)(packet[i+1] >> 4); 111 sep.type = (avdtp_sep_type_t)((packet[i+1] >> 3) & 0x01); 112 113 if (avdtp_find_remote_sep(connection, sep.seid) == 0xFF){ 114 connection->remote_seps[connection->remote_seps_num++] = sep; 115 } 116 avdtp_signaling_emit_sep(context->avdtp_callback, connection->avdtp_cid, sep); 117 } 118 break; 119 } 120 121 case AVDTP_SI_GET_CAPABILITIES: 122 case AVDTP_SI_GET_ALL_CAPABILITIES: 123 sep.registered_service_categories = avdtp_unpack_service_capabilities(connection, &sep.capabilities, packet+offset, size-offset); 124 avdtp_emit_capabilities(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->remote_seid, &sep.capabilities, sep.registered_service_categories); 125 break; 126 127 case AVDTP_SI_GET_CONFIGURATION: 128 // sep.configured_service_categories = avdtp_unpack_service_capabilities(connection, &sep.configuration, packet+offset, size-offset); 129 // if (get_bit16(sep.configured_service_categories, AVDTP_MEDIA_CODEC)){ 130 // switch (sep.configuration.media_codec.media_codec_type){ 131 // case AVDTP_CODEC_SBC: 132 // avdtp_signaling_emit_media_codec_sbc_configuration(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->remote_seid, 133 // sep.configuration.media_codec.media_type, sep.configuration.media_codec.media_codec_information); 134 // break; 135 // default: 136 // avdtp_signaling_emit_media_codec_other_configuration(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->remote_seid, sep.configuration.media_codec); 137 // break; 138 // } 139 // } 140 break; 141 142 case AVDTP_SI_RECONFIGURE: 143 if (!stream_endpoint){ 144 log_error("AVDTP_SI_RECONFIGURE: stream endpoint is null"); 145 break; 146 } 147 sep.configured_service_categories = avdtp_unpack_service_capabilities(connection, &sep.configuration, connection->signaling_packet.command+4, connection->signaling_packet.size-4); 148 // TODO check if configuration is supported 149 150 remote_sep_index = avdtp_find_remote_sep(connection, sep.seid); 151 if (remote_sep_index != 0xFF){ 152 stream_endpoint->remote_sep_index = remote_sep_index; 153 connection->remote_seps[stream_endpoint->remote_sep_index] = sep; 154 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CONFIGURED; 155 log_info("INT: update seid %d, to %p", connection->remote_seps[stream_endpoint->remote_sep_index].seid, stream_endpoint); 156 } 157 break; 158 159 case AVDTP_SI_SET_CONFIGURATION:{ 160 avdtp_configuration_timer_stop(connection); 161 if (!stream_endpoint){ 162 log_error("AVDTP_SI_SET_CONFIGURATION: stream endpoint is null"); 163 break; 164 } 165 sep.configured_service_categories = stream_endpoint->remote_configuration_bitmap; 166 sep.configuration = stream_endpoint->remote_configuration; 167 sep.in_use = 1; 168 // TODO check if configuration is supported 169 170 // find or add sep 171 remote_sep_index = avdtp_find_remote_sep(connection, sep.seid); 172 if (remote_sep_index != 0xFF){ 173 stream_endpoint->remote_sep_index = remote_sep_index; 174 } else { 175 stream_endpoint->remote_sep_index = connection->remote_seps_num; 176 connection->remote_seps_num++; 177 } 178 connection->remote_seps[stream_endpoint->remote_sep_index] = sep; 179 log_info("INT: configured remote seid %d, to %p", connection->remote_seps[stream_endpoint->remote_sep_index].seid, stream_endpoint); 180 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CONFIGURED; 181 182 switch (stream_endpoint->media_codec_type){ 183 case AVDTP_CODEC_SBC: 184 avdtp_signaling_emit_media_codec_sbc_configuration(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->remote_seid, 185 stream_endpoint->media_type, stream_endpoint->media_codec_sbc_info); 186 break; 187 default: 188 // TODO: we don\t have codec info to emit config 189 // avdtp_signaling_emit_media_codec_other_configuration(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->remote_seid, sep.configuration.media_codec); 190 break; 191 } 192 193 break; 194 } 195 196 case AVDTP_SI_OPEN: 197 if (!stream_endpoint){ 198 log_error("AVDTP_SI_OPEN: stream endpoint is null"); 199 break; 200 } 201 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W2_REQUEST_OPEN_STREAM) { 202 log_error("AVDTP_SI_OPEN in wrong stream endpoint state"); 203 return; 204 } 205 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED; 206 connection->local_seid = stream_endpoint->sep.seid; 207 l2cap_create_channel(context->packet_handler, connection->remote_addr, BLUETOOTH_PROTOCOL_AVDTP, 0xffff, NULL); 208 return; 209 case AVDTP_SI_START: 210 if (!stream_endpoint){ 211 log_error("AVDTP_SI_START: stream endpoint is null"); 212 break; 213 } 214 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_OPENED) { 215 log_error("AVDTP_SI_START in wrong stream endpoint state"); 216 return; 217 } 218 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_STREAMING; 219 break; 220 case AVDTP_SI_SUSPEND: 221 if (!stream_endpoint){ 222 log_error("AVDTP_SI_SUSPEND: stream endpoint is null"); 223 break; 224 } 225 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_STREAMING) { 226 log_error("AVDTP_SI_SUSPEND in wrong stream endpoint state"); 227 return; 228 } 229 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_OPENED; 230 break; 231 case AVDTP_SI_CLOSE: 232 if (!stream_endpoint){ 233 log_error("AVDTP_SI_CLOSE: stream endpoint is null"); 234 break; 235 } 236 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CLOSING; 237 break; 238 case AVDTP_SI_ABORT: 239 if (!stream_endpoint){ 240 log_error("AVDTP_SI_ABORT: stream endpoint is null"); 241 break; 242 } 243 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_ABORTING; 244 break; 245 default: 246 log_info(" AVDTP_RESPONSE_ACCEPT_MSG, signal %d not implemented", connection->signaling_packet.signal_identifier); 247 break; 248 } 249 avdtp_signaling_emit_accept(context->avdtp_callback, connection->avdtp_cid, 0, connection->signaling_packet.signal_identifier); 250 connection->initiator_transaction_label++; 251 break; 252 case AVDTP_RESPONSE_REJECT_MSG: 253 switch (connection->signaling_packet.signal_identifier){ 254 case AVDTP_SI_SET_CONFIGURATION: 255 connection->is_initiator = 0; 256 log_info("Received reject for set configuration, role changed from initiator to acceptor. Start timer."); 257 avdtp_configuration_timer_start(connection); 258 break; 259 default: 260 break; 261 } 262 log_info(" AVDTP_RESPONSE_REJECT_MSG signal %d", connection->signaling_packet.signal_identifier); 263 avdtp_signaling_emit_reject(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->signaling_packet.signal_identifier); 264 return; 265 case AVDTP_GENERAL_REJECT_MSG: 266 log_info(" AVDTP_GENERAL_REJECT_MSG signal %d", connection->signaling_packet.signal_identifier); 267 avdtp_signaling_emit_general_reject(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->signaling_packet.signal_identifier); 268 return; 269 default: 270 break; 271 } 272 } 273 274 void avdtp_initiator_stream_config_subsm_run(avdtp_connection_t * connection, avdtp_context_t * context){ 275 int sent = 1; 276 switch (connection->initiator_connection_state){ 277 case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS: 278 log_info("INT: AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS"); 279 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ANSWER; 280 avdtp_initiator_send_signaling_cmd(connection->l2cap_signaling_cid, AVDTP_SI_DISCOVER, connection->initiator_transaction_label); 281 break; 282 case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CAPABILITIES: 283 log_info("INT: AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CAPABILITIES"); 284 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ANSWER; 285 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_GET_CAPABILITIES, connection->initiator_transaction_label, connection->remote_seid); 286 break; 287 case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_ALL_CAPABILITIES: 288 log_info("INT: AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_ALL_CAPABILITIES"); 289 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ANSWER; 290 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_GET_ALL_CAPABILITIES, connection->initiator_transaction_label, connection->remote_seid); 291 break; 292 case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CONFIGURATION: 293 log_info("INT: AVDTP_INITIATOR_W4_GET_CONFIGURATION"); 294 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ANSWER; 295 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_GET_CONFIGURATION, connection->initiator_transaction_label, connection->remote_seid); 296 break; 297 default: 298 sent = 0; 299 break; 300 } 301 302 if (sent) return; 303 sent = 1; 304 305 avdtp_stream_endpoint_t * stream_endpoint = NULL; 306 307 stream_endpoint = avdtp_stream_endpoint_associated_with_acp_seid(connection->remote_seid, context); 308 if (!stream_endpoint){ 309 stream_endpoint = avdtp_stream_endpoint_with_seid(connection->local_seid, context); 310 } 311 if (!stream_endpoint) return; 312 313 avdtp_initiator_stream_endpoint_state_t stream_endpoint_state = stream_endpoint->initiator_config_state; 314 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W4_ANSWER; 315 316 if (stream_endpoint->start_stream){ 317 stream_endpoint->start_stream = 0; 318 if (stream_endpoint->state == AVDTP_STREAM_ENDPOINT_OPENED){ 319 connection->local_seid = stream_endpoint->sep.seid; 320 connection->remote_seid = connection->remote_seps[stream_endpoint->remote_sep_index].seid; 321 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_START, connection->initiator_transaction_label++, connection->remote_seid); 322 return; 323 } 324 return; 325 } 326 327 if (stream_endpoint->stop_stream){ 328 stream_endpoint->stop_stream = 0; 329 if (stream_endpoint->state >= AVDTP_STREAM_ENDPOINT_OPENED){ 330 connection->local_seid = stream_endpoint->sep.seid; 331 connection->remote_seid = connection->remote_seps[stream_endpoint->remote_sep_index].seid; 332 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_CLOSE, connection->initiator_transaction_label++, connection->remote_seid); 333 return; 334 } 335 } 336 337 if (stream_endpoint->abort_stream){ 338 stream_endpoint->abort_stream = 0; 339 switch (stream_endpoint->state){ 340 case AVDTP_STREAM_ENDPOINT_CONFIGURED: 341 case AVDTP_STREAM_ENDPOINT_CLOSING: 342 case AVDTP_STREAM_ENDPOINT_OPENED: 343 case AVDTP_STREAM_ENDPOINT_STREAMING: 344 connection->local_seid = stream_endpoint->sep.seid; 345 connection->remote_seid = connection->remote_seps[stream_endpoint->remote_sep_index].seid; 346 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_ABORTING; 347 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_ABORT, connection->initiator_transaction_label++, connection->remote_seid); 348 return; 349 default: 350 break; 351 } 352 } 353 354 if (stream_endpoint->suspend_stream){ 355 stream_endpoint->suspend_stream = 0; 356 if (stream_endpoint->state == AVDTP_STREAM_ENDPOINT_STREAMING){ 357 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_STREAMING; 358 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_SUSPEND, connection->initiator_transaction_label, connection->remote_seid); 359 return; 360 } 361 } 362 363 if (stream_endpoint->send_stream){ 364 stream_endpoint->send_stream = 0; 365 if (stream_endpoint->state == AVDTP_STREAM_ENDPOINT_STREAMING){ 366 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_STREAMING; 367 avdtp_streaming_emit_can_send_media_packet_now(context->avdtp_callback, stream_endpoint->l2cap_media_cid, stream_endpoint->sep.seid, stream_endpoint->sequence_number); 368 return; 369 } 370 } 371 372 switch (stream_endpoint_state){ 373 case AVDTP_INITIATOR_W2_SET_CONFIGURATION: 374 case AVDTP_INITIATOR_W2_RECONFIGURE_STREAM_WITH_SEID:{ 375 if (stream_endpoint_state == AVDTP_INITIATOR_W2_SET_CONFIGURATION && !connection->is_initiator){ 376 log_info("initiator SM stop sending SET_CONFIGURATION cmd: current role is acceptor"); 377 connection->is_configuration_initiated_locally = 0; 378 break; 379 } 380 log_info("initiator SM prepare SET_CONFIGURATION cmd"); 381 connection->is_configuration_initiated_locally = 1; 382 log_info("INT: AVDTP_INITIATOR_W2_(RE)CONFIGURATION bitmap, int seid %d, acp seid %d", connection->local_seid, connection->remote_seid); 383 // log_info_hexdump( connection->remote_capabilities.media_codec.media_codec_information, connection->remote_capabilities.media_codec.media_codec_information_len); 384 connection->signaling_packet.acp_seid = connection->remote_seid; 385 connection->signaling_packet.int_seid = connection->local_seid; 386 387 connection->signaling_packet.signal_identifier = AVDTP_SI_SET_CONFIGURATION; 388 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CONFIGURATION_SUBSTATEMACHINE; 389 if (stream_endpoint_state == AVDTP_INITIATOR_W2_RECONFIGURE_STREAM_WITH_SEID){ 390 connection->signaling_packet.signal_identifier = AVDTP_SI_RECONFIGURE; 391 } 392 393 avdtp_prepare_capabilities(&connection->signaling_packet, connection->initiator_transaction_label, stream_endpoint->remote_configuration_bitmap, stream_endpoint->remote_configuration, connection->signaling_packet.signal_identifier); 394 l2cap_reserve_packet_buffer(); 395 uint8_t * out_buffer = l2cap_get_outgoing_buffer(); 396 uint16_t pos = avdtp_signaling_create_fragment(connection->l2cap_signaling_cid, &connection->signaling_packet, out_buffer); 397 if (connection->signaling_packet.packet_type != AVDTP_SINGLE_PACKET && connection->signaling_packet.packet_type != AVDTP_END_PACKET){ 398 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_FRAGMENTATED_COMMAND; 399 log_info("INT: fragmented"); 400 } 401 l2cap_send_prepared(connection->l2cap_signaling_cid, pos); 402 break; 403 } 404 case AVDTP_INITIATOR_FRAGMENTATED_COMMAND:{ 405 l2cap_reserve_packet_buffer(); 406 uint8_t * out_buffer = l2cap_get_outgoing_buffer(); 407 uint16_t pos = avdtp_signaling_create_fragment(connection->l2cap_signaling_cid, &connection->signaling_packet, out_buffer); 408 if (connection->signaling_packet.packet_type != AVDTP_SINGLE_PACKET && connection->signaling_packet.packet_type != AVDTP_END_PACKET){ 409 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_FRAGMENTATED_COMMAND; 410 log_info("INT: fragmented"); 411 } 412 l2cap_send_prepared(connection->l2cap_signaling_cid, pos); 413 break; 414 } 415 case AVDTP_INITIATOR_W2_OPEN_STREAM: 416 switch (stream_endpoint->state){ 417 case AVDTP_STREAM_ENDPOINT_W2_REQUEST_OPEN_STREAM: 418 log_info("INT: AVDTP_STREAM_ENDPOINT_W2_REQUEST_OPEN_STREAM"); 419 avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_OPEN, connection->initiator_transaction_label, connection->remote_seid); 420 break; 421 default: 422 sent = 0; 423 break; 424 } 425 break; 426 default: 427 sent = 0; 428 break; 429 } 430 431 // check fragmentation 432 if (connection->signaling_packet.packet_type != AVDTP_SINGLE_PACKET && connection->signaling_packet.packet_type != AVDTP_END_PACKET){ 433 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 434 } 435 } 436