diff --git a/test/avdtp/avdtp.h b/test/avdtp/avdtp.h index d8f68e08b..0e0d307db 100644 --- a/test/avdtp/avdtp.h +++ b/test/avdtp/avdtp.h @@ -350,6 +350,16 @@ typedef enum { AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_SEPS_DISCOVERED } avdtp_initiator_connection_state_t; +typedef struct { + uint8_t command[200]; + uint16_t size; + uint16_t offset; + avdtp_signal_identifier_t signal_identifier; + avdtp_message_type_t message_type; + avdtp_packet_type_t packet_type; + uint16_t transaction_label; +} avdtp_signaling_packet_state_t; + typedef struct { btstack_linked_item_t item; @@ -362,6 +372,9 @@ typedef struct { avdtp_acceptor_connection_state_t acceptor_connection_state; avdtp_initiator_connection_state_t initiator_connection_state; + // used for fragmentation + avdtp_signaling_packet_state_t sig_packet; + uint8_t disconnect; uint8_t initiator_transaction_label; uint8_t acceptor_transaction_label; diff --git a/test/avdtp/avdtp_acceptor.c b/test/avdtp/avdtp_acceptor.c index c89d4811e..e3bb4fc37 100644 --- a/test/avdtp/avdtp_acceptor.c +++ b/test/avdtp/avdtp_acceptor.c @@ -47,6 +47,49 @@ #include "avdtp_util.h" #include "avdtp_acceptor.h" +static int avdtp_acceptor_signaling_response_send_fragmented(uint16_t cid, avdtp_signaling_packet_state_t * sig_packet) { + int mtu = l2cap_get_remote_mtu_for_local_cid(cid); + // hack for test + mtu = 10; + int data_len = 0; + + uint8_t *out_buffer = NULL; + uint16_t offset = sig_packet->offset; + uint16_t pos = 1; + + l2cap_reserve_packet_buffer(); + out_buffer = l2cap_get_outgoing_buffer(); + + if (offset == 0){ + if (sig_packet->size <= mtu - 2){ + sig_packet->packet_type = AVDTP_SINGLE_PACKET; + out_buffer[pos++] = sig_packet->signal_identifier; + data_len = sig_packet->size; + } else { + sig_packet->packet_type = AVDTP_START_PACKET; + out_buffer[pos++] = 1; + out_buffer[pos++] = sig_packet->signal_identifier; + data_len = mtu - 3; + sig_packet->offset = data_len; + } + } else { + int remaining_bytes = sig_packet->size - offset; + + if (remaining_bytes <= mtu - 1){ + sig_packet->packet_type = AVDTP_END_PACKET; + data_len = remaining_bytes; + sig_packet->offset = 0; + } else{ + sig_packet->packet_type = AVDTP_CONTINUE_PACKET; + data_len = mtu - 1; + sig_packet->offset += data_len; + } + } + out_buffer[0] = avdtp_header(sig_packet->transaction_label, sig_packet->packet_type, sig_packet->message_type); + memcpy(out_buffer+pos, sig_packet->command + offset, data_len); + return l2cap_send_prepared(cid, pos); +} + static int avdtp_pack_service_capabilities(uint8_t * buffer, int size, avdtp_capabilities_t caps, avdtp_service_category_t category, uint8_t pack_all_capabilities){ int i; // pos = 0 reserved for length @@ -151,44 +194,46 @@ static uint16_t avdtp_unpack_service_capabilities(avdtp_capabilities_t * caps, u return registered_service_categories; } -static inline int avdtp_acceptor_send_capabilities(uint16_t cid, uint8_t transaction_label, avdtp_sep_t sep, uint8_t identifier){ - uint8_t command[100]; +static inline int avdtp_acceptor_send_capabilities(uint16_t cid, avdtp_signaling_packet_state_t * sig_packet, uint8_t transaction_label, avdtp_sep_t sep, uint8_t identifier){ uint8_t pack_all_capabilities = 1; if (identifier == AVDTP_SI_GET_CAPABILITIES){ pack_all_capabilities = 0; } - int pos = 0; - command[pos++] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_RESPONSE_ACCEPT_MSG); - command[pos++] = identifier; + sig_packet->size = 0; int i = 0; for (i = 1; i < 9; i++){ if (get_bit16(sep.registered_service_categories, i)){ // service category - command[pos++] = i; - pos += avdtp_pack_service_capabilities(command+pos, sizeof(command)-pos, sep.capabilities, (avdtp_service_category_t)i, pack_all_capabilities); + sig_packet->command[sig_packet->size++] = i; + sig_packet->size += avdtp_pack_service_capabilities(sig_packet->command+sig_packet->size, sizeof(sig_packet->command)-sig_packet->size, sep.capabilities, (avdtp_service_category_t)i, pack_all_capabilities); } } - command[pos++] = 0x04; - command[pos++] = 0x02; - command[pos++] = 0x02; - command[pos++] = 0x00; + sig_packet->command[sig_packet->size++] = 0x04; + sig_packet->command[sig_packet->size++] = 0x02; + sig_packet->command[sig_packet->size++] = 0x02; + sig_packet->command[sig_packet->size++] = 0x00; // printf(" avdtp_acceptor_send_capabilities_response: \n"); - // printf_hexdump(command, pos); - return l2cap_send(cid, command, pos); + // printf_hexdump(sig_packet->command, sig_packet->size); + sig_packet->signal_identifier = identifier; + sig_packet->transaction_label = transaction_label; + sig_packet->message_type = AVDTP_RESPONSE_ACCEPT_MSG; + sig_packet->offset = 0; + + return avdtp_acceptor_signaling_response_send_fragmented(cid, sig_packet); } -static int avdtp_acceptor_send_capabilities_response(uint16_t cid, uint8_t transaction_label, avdtp_sep_t sep){ - return avdtp_acceptor_send_capabilities(cid, transaction_label, sep, AVDTP_SI_GET_CAPABILITIES); +static int avdtp_acceptor_send_capabilities_response(uint16_t cid, avdtp_signaling_packet_state_t * sig_packet, uint8_t transaction_label, avdtp_sep_t sep){ + return avdtp_acceptor_send_capabilities(cid, sig_packet, transaction_label, sep, AVDTP_SI_GET_CAPABILITIES); } -static int avdtp_acceptor_send_all_capabilities_response(uint16_t cid, uint8_t transaction_label, avdtp_sep_t sep){ - return avdtp_acceptor_send_capabilities(cid, transaction_label, sep, AVDTP_SI_GET_ALL_CAPABILITIES); +static int avdtp_acceptor_send_all_capabilities_response(uint16_t cid, avdtp_signaling_packet_state_t * sig_packet, uint8_t transaction_label, avdtp_sep_t sep){ + return avdtp_acceptor_send_capabilities(cid, sig_packet, transaction_label, sep, AVDTP_SI_GET_ALL_CAPABILITIES); } -static int avdtp_acceptor_send_stream_configuration_response(uint16_t cid, uint8_t transaction_label, avdtp_sep_t sep){ - return avdtp_acceptor_send_capabilities(cid, transaction_label, sep, AVDTP_SI_GET_CONFIGURATION); +static int avdtp_acceptor_send_stream_configuration_response(uint16_t cid, avdtp_signaling_packet_state_t * sig_packet, uint8_t transaction_label, avdtp_sep_t sep){ + return avdtp_acceptor_send_capabilities(cid, sig_packet, transaction_label, sep, AVDTP_SI_GET_CONFIGURATION); } static int avdtp_acceptor_send_accept_response(uint16_t cid, uint8_t transaction_label, avdtp_signal_identifier_t identifier){ @@ -198,26 +243,18 @@ static int avdtp_acceptor_send_accept_response(uint16_t cid, uint8_t transactio return l2cap_send(cid, command, sizeof(command)); } -typedef struct { - uint8_t command[200]; - uint16_t size; - uint16_t offset; -} avdtp_signaling_packet_state_t; - -static avdtp_signaling_packet_state_t sig_packet; - -static int avdtp_acceptor_process_chunk(avdtp_signaling_packet_header_t * signaling_header, uint8_t * packet, uint16_t size){ - memcpy(sig_packet.command + sig_packet.size, packet, size); - sig_packet.size += size; +static int avdtp_acceptor_process_chunk(avdtp_signaling_packet_header_t * signaling_header, avdtp_signaling_packet_state_t * sig_packet, uint8_t * packet, uint16_t size){ + memcpy(sig_packet->command + sig_packet->size, packet, size); + sig_packet->size += size; return signaling_header->packet_type == AVDTP_SINGLE_PACKET || signaling_header->packet_type == AVDTP_END_PACKET; } -int avdtp_acceptor_stream_config_subsm(avdtp_stream_endpoint_t * stream_endpoint, avdtp_signaling_packet_header_t * signaling_header, uint8_t * packet, uint16_t size){ +int avdtp_acceptor_stream_config_subsm(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint, avdtp_signaling_packet_header_t * signaling_header, uint8_t * packet, uint16_t size){ if (!stream_endpoint) return 0; - if (!avdtp_acceptor_process_chunk(signaling_header, packet, size)) return 0; - uint16_t packet_size = sig_packet.size; - sig_packet.size = 0; + if (!avdtp_acceptor_process_chunk(signaling_header, &connection->sig_packet, packet, size)) return 0; + uint16_t packet_size = connection->sig_packet.size; + connection->sig_packet.size = 0; int request_to_send = 1; switch (stream_endpoint->acceptor_config_state){ @@ -235,8 +272,8 @@ int avdtp_acceptor_stream_config_subsm(avdtp_stream_endpoint_t * stream_endpoint printf(" ACP: AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION \n"); stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION; avdtp_sep_t sep; - sep.seid = sig_packet.command[3] >> 2; - sep.registered_service_categories = avdtp_unpack_service_capabilities(&sep.capabilities, sig_packet.command+4, packet_size-4); + sep.seid = connection->sig_packet.command[3] >> 2; + sep.registered_service_categories = avdtp_unpack_service_capabilities(&sep.capabilities, connection->sig_packet.command+4, packet_size-4); // find or add sep stream_endpoint->remote_sep_index = 0xFF; @@ -369,14 +406,14 @@ static int avdtp_acceptor_send_response_reject_service_category(uint16_t cid, a return l2cap_send(cid, command, sizeof(command)); } -static int avdtp_acceptor_send_response_reject(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label){ +static int avdtp_acceptor_send_response_reject(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label){ uint8_t command[2]; command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_GENERAL_REJECT_MSG); command[1] = (uint8_t)identifier; return l2cap_send(cid, command, sizeof(command)); } -static int avdtp_acceptor_send_response_reject_with_error_code(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t error_code, uint8_t transaction_label){ +static int avdtp_acceptor_send_response_reject_with_error_code(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t error_code, uint8_t transaction_label){ uint8_t command[3]; command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_GENERAL_REJECT_MSG); command[1] = (uint8_t)identifier; @@ -400,11 +437,15 @@ int avdtp_acceptor_stream_config_subsm_run(avdtp_connection_t * connection, avdt break; case AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES: printf(" ACP: DONE\n"); - avdtp_acceptor_send_capabilities_response(cid, trid, stream_endpoint->sep); + avdtp_acceptor_send_capabilities_response(cid, &connection->sig_packet, trid, stream_endpoint->sep); + if (connection->sig_packet.packet_type != AVDTP_SINGLE_PACKET && connection->sig_packet.packet_type != AVDTP_END_PACKET){ + stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES; + + } break; case AVDTP_ACCEPTOR_W2_ANSWER_GET_ALL_CAPABILITIES: printf(" ACP: DONE\n"); - avdtp_acceptor_send_all_capabilities_response(cid, trid, stream_endpoint->sep); + avdtp_acceptor_send_all_capabilities_response(cid, &connection->sig_packet, trid, stream_endpoint->sep); break; case AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION: printf(" ACP: DONE\n"); @@ -415,7 +456,7 @@ int avdtp_acceptor_stream_config_subsm_run(avdtp_connection_t * connection, avdt break; case AVDTP_ACCEPTOR_W2_ANSWER_GET_CONFIGURATION: printf(" ACP: DONE\n"); - avdtp_acceptor_send_stream_configuration_response(cid, trid, stream_endpoint->remote_seps[stream_endpoint->remote_sep_index]); + avdtp_acceptor_send_stream_configuration_response(cid, &connection->sig_packet, trid, stream_endpoint->remote_seps[stream_endpoint->remote_sep_index]); break; case AVDTP_ACCEPTOR_W4_L2CAP_FOR_MEDIA_CONNECTED: stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_W4_L2CAP_FOR_MEDIA_CONNECTED; @@ -462,6 +503,10 @@ int avdtp_acceptor_stream_config_subsm_run(avdtp_connection_t * connection, avdt default: printf(" ACP: NOT IMPLEMENTED\n"); return 0; - } + } + if (connection->sig_packet.packet_type != AVDTP_SINGLE_PACKET && connection->sig_packet.packet_type != AVDTP_END_PACKET){ + stream_endpoint->acceptor_config_state = acceptor_config_state; + connection->wait_to_send_acceptor = 1; + } return sent; } diff --git a/test/avdtp/avdtp_acceptor.h b/test/avdtp/avdtp_acceptor.h index bf00393ff..64a34ccfc 100644 --- a/test/avdtp/avdtp_acceptor.h +++ b/test/avdtp/avdtp_acceptor.h @@ -51,7 +51,7 @@ extern "C" { #endif -int avdtp_acceptor_stream_config_subsm(avdtp_stream_endpoint_t * stream_endpoint, avdtp_signaling_packet_header_t * signaling_header, uint8_t *packet, uint16_t size); +int avdtp_acceptor_stream_config_subsm(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint, avdtp_signaling_packet_header_t * signaling_header, uint8_t *packet, uint16_t size); int avdtp_acceptor_stream_config_subsm_run(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint); #if defined __cplusplus diff --git a/test/avdtp/avdtp_sink.c b/test/avdtp/avdtp_sink.c index d9e986d56..973f694e5 100644 --- a/test/avdtp/avdtp_sink.c +++ b/test/avdtp/avdtp_sink.c @@ -455,7 +455,7 @@ static int handle_l2cap_data_packet_for_stream_endpoint(avdtp_connection_t * con avdtp_read_signaling_header(&signaling_header, packet, size); if (signaling_header.message_type == AVDTP_CMD_MSG){ - if (avdtp_acceptor_stream_config_subsm(stream_endpoint, &signaling_header, packet, size)){ + if (avdtp_acceptor_stream_config_subsm(connection, stream_endpoint, &signaling_header, packet, size)){ avdtp_sink_request_can_send_now_acceptor(connection, connection->l2cap_signaling_cid); return 1; }