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