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