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