diff --git a/src/classic/avdtp_util.c b/src/classic/avdtp_util.c index 6f38ca698..a4ef86522 100644 --- a/src/classic/avdtp_util.c +++ b/src/classic/avdtp_util.c @@ -335,7 +335,7 @@ static int avdtp_unpack_service_capabilities_has_errors(avdtp_connection_t * con } break; case AVDTP_RECOVERY: - if (cap_len < 3){ + if (cap_len != 3){ log_info(" ERROR: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n"); connection->reject_service_category = category; connection->error_code = BAD_RECOVERY_FORMAT; @@ -351,6 +351,13 @@ static int avdtp_unpack_service_capabilities_has_errors(avdtp_connection_t * con } break; case AVDTP_HEADER_COMPRESSION: + // TODO: find error code for bad header compression + if (cap_len != 1){ + log_info(" ERROR: REJECT CATEGORY, BAD_HEADER_COMPRESSION\n"); + connection->reject_service_category = category; + connection->error_code = BAD_RECOVERY_FORMAT; + return 1; + } break; case AVDTP_MULTIPLEXING: break; @@ -363,97 +370,82 @@ static int avdtp_unpack_service_capabilities_has_errors(avdtp_connection_t * con } uint16_t avdtp_unpack_service_capabilities(avdtp_connection_t * connection, avdtp_capabilities_t * caps, uint8_t * packet, uint16_t size){ - if (size == 0) return 0; - uint16_t registered_service_categories = 0; - int pos = 0; int i; - avdtp_service_category_t category = (avdtp_service_category_t)packet[pos++]; - uint8_t cap_len = packet[pos++]; - if (cap_len > size - pos){ - connection->reject_service_category = category; - connection->error_code = BAD_LENGTH; - return 0; - } - - if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)) return 0; - - int processed_cap_len = 0; - int rfa = 0; - - while (pos < size){ - rfa = 0; - processed_cap_len = pos; + + uint16_t registered_service_categories = 0; + uint16_t to_process = size; + + while (to_process >= 2){ + + avdtp_service_category_t category = (avdtp_service_category_t) packet[0]; + uint8_t cap_len = packet[1]; + packet += 2; + to_process -= 2; + + if (cap_len > to_process){ + connection->reject_service_category = category; + connection->error_code = BAD_LENGTH; + return 0; + } + + if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)) return 0; + + int category_valid = 1; + + uint8_t * data = packet; + uint16_t pos = 0; + switch(category){ case AVDTP_RECOVERY: - caps->recovery.recovery_type = packet[pos++]; - caps->recovery.maximum_recovery_window_size = packet[pos++]; - caps->recovery.maximum_number_media_packets = packet[pos++]; + caps->recovery.recovery_type = data[pos++]; + caps->recovery.maximum_recovery_window_size = data[pos++]; + caps->recovery.maximum_number_media_packets = data[pos++]; break; case AVDTP_CONTENT_PROTECTION: - caps->content_protection.cp_type = big_endian_read_16(packet, pos); - pos+=2; - + caps->content_protection.cp_type = big_endian_read_16(data, 0); caps->content_protection.cp_type_value_len = cap_len - 2; - pos += caps->content_protection.cp_type_value_len; - // connection->reject_service_category = category; // connection->error_code = UNSUPPORTED_CONFIGURATION; // support for content protection goes here break; - case AVDTP_HEADER_COMPRESSION: - caps->header_compression.back_ch = packet[pos] >> 7; - caps->header_compression.media = packet[pos] >> 6; - caps->header_compression.recovery = packet[pos] >> 5; - pos++; + caps->header_compression.back_ch = (data[0] >> 7) & 1; + caps->header_compression.media = (data[0] >> 6) & 1; + caps->header_compression.recovery = (data[0] >> 5) & 1; break; case AVDTP_MULTIPLEXING: - caps->multiplexing_mode.fragmentation = packet[pos++] >> 7; + caps->multiplexing_mode.fragmentation = (data[pos++] >> 7) & 1; // read [tsid, tcid] for media, reporting. recovery respectively caps->multiplexing_mode.transport_identifiers_num = 3; for (i=0; imultiplexing_mode.transport_identifiers_num; i++){ - caps->multiplexing_mode.transport_session_identifiers[i] = packet[pos++] >> 7; - caps->multiplexing_mode.tcid[i] = packet[pos++] >> 7; + caps->multiplexing_mode.transport_session_identifiers[i] = (data[pos++] >> 7) & 1; + caps->multiplexing_mode.tcid[i] = (data[pos++] >> 7) & 1; } break; case AVDTP_MEDIA_CODEC: - caps->media_codec.media_type = (avdtp_media_type_t)(packet[pos++] >> 4); - caps->media_codec.media_codec_type = (avdtp_media_codec_type_t)(packet[pos++]); + caps->media_codec.media_type = (avdtp_media_type_t)(data[pos++] >> 4); + caps->media_codec.media_codec_type = (avdtp_media_codec_type_t)(data[pos++]); caps->media_codec.media_codec_information_len = cap_len - 2; - caps->media_codec.media_codec_information = &packet[pos]; - pos += caps->media_codec.media_codec_information_len; + caps->media_codec.media_codec_information = &data[pos++]; break; case AVDTP_MEDIA_TRANSPORT: case AVDTP_REPORTING: - case AVDTP_DELAY_REPORTING: - pos += cap_len; + case AVDTP_DELAY_REPORTING: break; default: - pos += cap_len; - rfa = 1; + category_valid = 0; break; } - processed_cap_len = pos - processed_cap_len; - // printf("processed category %d, cap_len %d, processed_cap_len %d, rfa %d\n", category, cap_len, processed_cap_len, rfa); - if (cap_len == processed_cap_len){ - // printf("pos %d, size %d \n", rfa, size - 2); + if (category_valid) { + registered_service_categories = store_bit16(registered_service_categories, category, 1); + } - if (!rfa) { - registered_service_categories = store_bit16(registered_service_categories, category, 1); - } - if (pos < size-2){ - //int old_pos = pos; - category = (avdtp_service_category_t)packet[pos++]; - cap_len = packet[pos++]; - if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)){ - log_error("avdtp_unpack_service_capabilities_has_errors"); - return 0; - } - } - } + packet += cap_len; + to_process -= cap_len; } + return registered_service_categories; } @@ -644,7 +636,6 @@ void avdtp_signaling_emit_sep_done(btstack_packet_handler_t callback, uint16_t a (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); } - void avdtp_signaling_emit_accept(btstack_packet_handler_t callback, uint16_t avdtp_cid, uint8_t local_seid, avdtp_signal_identifier_t identifier){ if (!callback) return; uint8_t event[7];