static int read_sbc_header(uint8_t * packet, int size, int * offset, avdtp_sbc_codec_header_t * sbc_header){
int sbc_header_len = 12; // without crc
int pos = *offset;
if (size - pos < sbc_header_len){
printf("Not enough data to read SBC header, expected %d, received %d\n", sbc_header_len, size-pos);
return 0;
}
sbc_header->fragmentation = get_bit16(packet[pos], 7);
sbc_header->starting_packet = get_bit16(packet[pos], 6);
sbc_header->last_packet = get_bit16(packet[pos], 5);
sbc_header->num_frames = packet[pos] & 0x0f;
pos++;
// printf("SBC HEADER: num_frames %u, fragmented %u, start %u, stop %u\n", sbc_header.num_frames, sbc_header.fragmentation, sbc_header.starting_packet, sbc_header.last_packet);
*offset = pos;
return 1;
}
static int read_media_data_header(uint8_t *packet, int size, int *offset, avdtp_media_packet_header_t *media_header){
int media_header_len = 12; // without crc
int pos = *offset;
if (size - pos < media_header_len){
printf("Not enough data to read media packet header, expected %d, received %d\n", media_header_len, size-pos);
return 0;
}
media_header->version = packet[pos] & 0x03;
media_header->padding = get_bit16(packet[pos],2);
media_header->extension = get_bit16(packet[pos],3);
media_header->csrc_count = (packet[pos] >> 4) & 0x0F;
pos++;
media_header->marker = get_bit16(packet[pos],0);
media_header->payload_type = (packet[pos] >> 1) & 0x7F;
pos++;
media_header->sequence_number = big_endian_read_16(packet, pos);
pos+=2;
media_header->timestamp = big_endian_read_32(packet, pos);
pos+=4;
media_header->synchronization_source = big_endian_read_32(packet, pos);
pos+=4;
*offset = pos;
// TODO: read csrc list
// printf_hexdump( packet, pos );
if (!is_media_header_reported_once){
is_media_header_reported_once = 1;
printf("MEDIA HEADER: %u timestamp, version %u, padding %u, extension %u, csrc_count %u\n",
media_header->timestamp, media_header->version, media_header->padding, media_header->extension, media_header->csrc_count);
printf("MEDIA HEADER: marker %02x, payload_type %02x, sequence_number %u, synchronization_source %u\n",
media_header->marker, media_header->payload_type, media_header->sequence_number, media_header->synchronization_source);
}
return 1;
}
static void handle_l2cap_media_data_packet(uint8_t seid, uint8_t *packet, uint16_t size){
UNUSED(seid);
int pos = 0;
avdtp_media_packet_header_t media_header;
media_header.version = packet[pos] & 0x03;
media_header.padding = get_bit16(packet[pos],2);
media_header.extension = get_bit16(packet[pos],3);
media_header.csrc_count = (packet[pos] >> 4) & 0x0F;
pos++;
media_header.marker = get_bit16(packet[pos],0);
media_header.payload_type = (packet[pos] >> 1) & 0x7F;
pos++;
media_header.sequence_number = big_endian_read_16(packet, pos);
pos+=2;
media_header.timestamp = big_endian_read_32(packet, pos);
pos+=4;
media_header.synchronization_source = big_endian_read_32(packet, pos);
pos+=4;
UNUSED(media_header);
// TODO: read csrc list
// printf_hexdump( packet, pos );
// printf("MEDIA HEADER: %u timestamp, version %u, padding %u, extension %u, csrc_count %u\n",
// media_header.timestamp, media_header.version, media_header.padding, media_header.extension, media_header.csrc_count);
// printf("MEDIA HEADER: marker %02x, payload_type %02x, sequence_number %u, synchronization_source %u\n",
// media_header.marker, media_header.payload_type, media_header.sequence_number, media_header.synchronization_source);
avdtp_sbc_codec_header_t sbc_header;
sbc_header.fragmentation = get_bit16(packet[pos], 7);
sbc_header.starting_packet = get_bit16(packet[pos], 6);
sbc_header.last_packet = get_bit16(packet[pos], 5);
sbc_header.num_frames = packet[pos] & 0x0f;
pos++;
UNUSED(sbc_header);
// printf("SBC HEADER: num_frames %u, fragmented %u, start %u, stop %u\n", sbc_header.num_frames, sbc_header.fragmentation, sbc_header.starting_packet, sbc_header.last_packet);
// printf_hexdump( packet+pos, size-pos );
#ifdef DECODE_SBC
btstack_sbc_decoder_process_data(&state, 0, packet+pos, size-pos);
#endif
#ifdef STORE_SBC_TO_SBC_FILE
fwrite(packet+pos, size-pos, 1, sbc_file);
#endif
#ifdef HAVE_PORTAUDIO
log_info("PA: bytes avail after recv: %d", btstack_ring_buffer_bytes_available(&ring_buffer));
#endif
}
void avdtp_initiator_stream_config_subsm(avdtp_connection_t * connection, uint8_t *packet, uint16_t size, int offset, avdtp_context_t * context){
// int status = 0;
avdtp_stream_endpoint_t * stream_endpoint = NULL;
uint8_t remote_sep_index;
avdtp_sep_t sep;
if (connection->initiator_connection_state == AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ANSWER) {
connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE;
} else {
stream_endpoint = avdtp_stream_endpoint_associated_with_acp_seid(connection->remote_seid, context);
if (!stream_endpoint){
stream_endpoint = avdtp_stream_endpoint_with_seid(connection->local_seid, context);
}
if (!stream_endpoint) return;
sep.seid = connection->remote_seid;
if (stream_endpoint->initiator_config_state != AVDTP_INITIATOR_W4_ANSWER) return;
stream_endpoint->initiator_config_state = AVDTP_INITIATOR_STREAM_CONFIG_IDLE;
}
switch (connection->signaling_packet.message_type){
case AVDTP_RESPONSE_ACCEPT_MSG:
switch (connection->signaling_packet.signal_identifier){
case AVDTP_SI_DISCOVER:{
if (connection->signaling_packet.transaction_label != connection->initiator_transaction_label){
log_info(" unexpected transaction label, got %d, expected %d", connection->signaling_packet.transaction_label, connection->initiator_transaction_label);
// status = BAD_HEADER_FORMAT;
break;
}
if (size == 3){
log_info(" ERROR code %02x", packet[offset]);
break;
}
int i;
for (i = offset; i < size; i += 2){
sep.seid = packet[i] >> 2;
offset++;
if (sep.seid < 0x01 || sep.seid > 0x3E){
log_info(" invalid sep id");
// status = BAD_ACP_SEID;
break;
}
sep.in_use = (packet[i] >> 1) & 0x01;
sep.media_type = (avdtp_media_type_t)(packet[i+1] >> 4);
sep.type = (avdtp_sep_type_t)((packet[i+1] >> 3) & 0x01);
if (avdtp_find_remote_sep(connection, sep.seid) == 0xFF){
connection->remote_seps[connection->remote_seps_num++] = sep;
}
avdtp_signaling_emit_sep(context->avdtp_callback, connection->avdtp_cid, sep);
}
break;
}
case AVDTP_SI_GET_CAPABILITIES:
case AVDTP_SI_GET_ALL_CAPABILITIES:
sep.registered_service_categories = avdtp_unpack_service_capabilities(connection, &sep.capabilities, packet+offset, size-offset);
if (get_bit16(sep.registered_service_categories, AVDTP_MEDIA_CODEC)){
switch (sep.capabilities.media_codec.media_codec_type){
case AVDTP_CODEC_SBC:
avdtp_signaling_emit_media_codec_sbc_capability(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->remote_seid, sep.capabilities.media_codec);
break;
default:
avdtp_signaling_emit_media_codec_other_capability(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->remote_seid, sep.capabilities.media_codec);
break;
}
}
break;
case AVDTP_SI_GET_CONFIGURATION:
sep.configured_service_categories = avdtp_unpack_service_capabilities(connection, &sep.configuration, packet+offset, size-offset);
if (get_bit16(sep.configured_service_categories, AVDTP_MEDIA_CODEC)){
switch (sep.configuration.media_codec.media_codec_type){
case AVDTP_CODEC_SBC:
avdtp_signaling_emit_media_codec_sbc_configuration(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->remote_seid, sep.configuration.media_codec);
break;
default:
avdtp_signaling_emit_media_codec_other_configuration(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->remote_seid, sep.configuration.media_codec);
break;
}
}
break;
case AVDTP_SI_RECONFIGURE:
if (!stream_endpoint){
log_error("AVDTP_SI_RECONFIGURE: stream endpoint is null");
break;
}
sep.configured_service_categories = avdtp_unpack_service_capabilities(connection, &sep.configuration, connection->signaling_packet.command+4, connection->signaling_packet.size-4);
// TODO check if configuration is supported
remote_sep_index = avdtp_find_remote_sep(connection, sep.seid);
if (remote_sep_index != 0xFF){
stream_endpoint->remote_sep_index = remote_sep_index;
connection->remote_seps[stream_endpoint->remote_sep_index] = sep;
stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CONFIGURED;
log_info("INT: update seid %d, to %p", connection->remote_seps[stream_endpoint->remote_sep_index].seid, stream_endpoint);
}
break;
case AVDTP_SI_SET_CONFIGURATION:{
if (!stream_endpoint){
log_error("AVDTP_SI_SET_CONFIGURATION: stream endpoint is null");
break;
}
sep.configured_service_categories = stream_endpoint->remote_capabilities_bitmap;
sep.configuration = stream_endpoint->remote_capabilities;
sep.in_use = 1;
// TODO check if configuration is supported
// find or add sep
remote_sep_index = avdtp_find_remote_sep(connection, sep.seid);
if (remote_sep_index != 0xFF){
stream_endpoint->remote_sep_index = remote_sep_index;
} else {
stream_endpoint->remote_sep_index = connection->remote_seps_num;
connection->remote_seps_num++;
}
connection->remote_seps[stream_endpoint->remote_sep_index] = sep;
log_info("INT: configured remote seid %d, to %p", connection->remote_seps[stream_endpoint->remote_sep_index].seid, stream_endpoint);
stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CONFIGURED;
break;
}
case AVDTP_SI_OPEN:
if (!stream_endpoint){
log_error("AVDTP_SI_OPEN: stream endpoint is null");
break;
}
if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W2_REQUEST_OPEN_STREAM) {
log_error("AVDTP_SI_OPEN in wrong stream endpoint state");
return;
}
stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED;
connection->local_seid = stream_endpoint->sep.seid;
l2cap_create_channel(context->packet_handler, connection->remote_addr, BLUETOOTH_PROTOCOL_AVDTP, 0xffff, NULL);
return;
case AVDTP_SI_START:
if (!stream_endpoint){
log_error("AVDTP_SI_START: stream endpoint is null");
break;
}
if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_OPENED) {
log_error("AVDTP_SI_START in wrong stream endpoint state");
return;
}
stream_endpoint->state = AVDTP_STREAM_ENDPOINT_STREAMING;
break;
case AVDTP_SI_SUSPEND:
if (!stream_endpoint){
log_error("AVDTP_SI_SUSPEND: stream endpoint is null");
break;
}
if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_STREAMING) {
log_error("AVDTP_SI_SUSPEND in wrong stream endpoint state");
return;
}
stream_endpoint->state = AVDTP_STREAM_ENDPOINT_OPENED;
break;
case AVDTP_SI_CLOSE:
if (!stream_endpoint){
log_error("AVDTP_SI_CLOSE: stream endpoint is null");
break;
}
stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CLOSING;
break;
case AVDTP_SI_ABORT:
if (!stream_endpoint){
log_error("AVDTP_SI_ABORT: stream endpoint is null");
break;
}
stream_endpoint->state = AVDTP_STREAM_ENDPOINT_ABORTING;
break;
default:
log_info(" AVDTP_RESPONSE_ACCEPT_MSG, signal %d not implemented", connection->signaling_packet.signal_identifier);
break;
}
avdtp_signaling_emit_accept(context->avdtp_callback, connection->avdtp_cid, 0, connection->signaling_packet.signal_identifier);
connection->initiator_transaction_label++;
break;
case AVDTP_RESPONSE_REJECT_MSG:
log_info(" AVDTP_RESPONSE_REJECT_MSG signal %d", connection->signaling_packet.signal_identifier);
avdtp_signaling_emit_reject(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->signaling_packet.signal_identifier);
return;
case AVDTP_GENERAL_REJECT_MSG:
log_info(" AVDTP_GENERAL_REJECT_MSG signal %d", connection->signaling_packet.signal_identifier);
avdtp_signaling_emit_general_reject(context->avdtp_callback, connection->avdtp_cid, connection->local_seid, connection->signaling_packet.signal_identifier);
return;
default:
break;
}
}
