diff --git a/src/classic/avdtp.c b/src/classic/avdtp.c index 8a46e005b..c13cbd248 100644 --- a/src/classic/avdtp.c +++ b/src/classic/avdtp.c @@ -425,7 +425,6 @@ void avdtp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet if (!stream_endpoint->connection) break; if (stream_endpoint->state == AVDTP_STREAM_ENDPOINT_STREAMING_W2_SEND){ stream_endpoint->state = AVDTP_STREAM_ENDPOINT_STREAMING; - stream_endpoint->sequence_number++; avdtp_streaming_emit_can_send_media_packet_now(context->avdtp_callback, stream_endpoint->l2cap_media_cid, stream_endpoint->sep.seid, stream_endpoint->sequence_number); } connection = stream_endpoint->connection; diff --git a/src/classic/avdtp_source.c b/src/classic/avdtp_source.c index 8d2c10fe9..1c87a4953 100644 --- a/src/classic/avdtp_source.c +++ b/src/classic/avdtp_source.c @@ -281,3 +281,87 @@ void avdtp_source_request_can_send_now(uint16_t con_handle){ stream_endpoint->state = AVDTP_STREAM_ENDPOINT_STREAMING_W2_SEND; avdtp_request_can_send_now_self(stream_endpoint->connection, stream_endpoint->l2cap_media_cid); } + + +static void avdtp_source_setup_media_header(uint8_t * media_packet, int size, int *offset, uint8_t marker, uint16_t sequence_number){ + if (size < 12){ + printf("small outgoing buffer\n"); + return; + } + + uint8_t rtp_version = 2; + uint8_t padding = 0; + uint8_t extension = 0; + uint8_t csrc_count = 0; + uint8_t payload_type = 0x60; + // uint16_t sequence_number = stream_endpoint->sequence_number; + uint32_t timestamp = btstack_run_loop_get_time_ms(); + uint32_t ssrc = 0x11223344; + + // rtp header (min size 12B) + int pos = 0; + // int mtu = l2cap_get_remote_mtu_for_local_cid(stream_endpoint->l2cap_media_cid); + + media_packet[pos++] = (rtp_version << 6) | (padding << 5) | (extension << 4) | csrc_count; + media_packet[pos++] = (marker << 1) | payload_type; + big_endian_store_16(media_packet, pos, sequence_number); + pos += 2; + big_endian_store_32(media_packet, pos, timestamp); + pos += 4; + big_endian_store_32(media_packet, pos, ssrc); // only used for multicast + pos += 4; + *offset = pos; +} + +static void avdtp_source_copy_media_payload(uint8_t * media_packet, int size, int * offset, btstack_ring_buffer_t * sbc_ring_buffer){ + if (size < 18){ + printf("small outgoing buffer\n"); + return; + } + int pos = *offset; + // media payload + // sbc_header (size 1B) + uint8_t sbc_header_index = pos; + pos++; + uint8_t fragmentation = 0; + uint8_t starting_packet = 0; // set to 1 for the first packet of a fragmented SBC frame + uint8_t last_packet = 0; // set to 1 for the last packet of a fragmented SBC frame + uint8_t num_frames = 0; + + uint32_t total_sbc_bytes_read = 0; + uint8_t sbc_frame_size = 0; + // payload + uint16_t sbc_frame_bytes = btstack_sbc_encoder_sbc_buffer_length(); + + while (size - 13 - total_sbc_bytes_read >= sbc_frame_bytes && btstack_ring_buffer_bytes_available(sbc_ring_buffer)){ + uint32_t number_of_bytes_read = 0; + btstack_ring_buffer_read(sbc_ring_buffer, &sbc_frame_size, 1, &number_of_bytes_read); + btstack_ring_buffer_read(sbc_ring_buffer, media_packet + pos, sbc_frame_size, &number_of_bytes_read); + pos += sbc_frame_size; + total_sbc_bytes_read += sbc_frame_size; + num_frames++; + // printf("send sbc frame: timestamp %d, seq. nr %d\n", timestamp, stream_endpoint->sequence_number); + } + media_packet[sbc_header_index] = (fragmentation << 7) | (starting_packet << 6) | (last_packet << 5) | num_frames; + *offset = pos; +} + +void avdtp_source_stream_send_media_payload(uint16_t l2cap_media_cid, btstack_ring_buffer_t * sbc_ring_buffer, uint8_t marker){ + avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_for_l2cap_cid(l2cap_media_cid, &avdtp_source_context); + if (!stream_endpoint) { + printf("no stream_endpoint found for 0x%02x", l2cap_media_cid); + return; + } + + int size = l2cap_get_remote_mtu_for_local_cid(l2cap_media_cid); + int offset = 0; + + l2cap_reserve_packet_buffer(); + uint8_t * media_packet = l2cap_get_outgoing_buffer(); + //int size = l2cap_get_remote_mtu_for_local_cid(l2cap_media_cid); + avdtp_source_setup_media_header(media_packet, size, &offset, marker, stream_endpoint->sequence_number); + avdtp_source_copy_media_payload(media_packet, size, &offset, sbc_ring_buffer); + stream_endpoint->sequence_number++; + l2cap_send_prepared(l2cap_media_cid, offset); +} + diff --git a/src/classic/avdtp_source.h b/src/classic/avdtp_source.h index 5ce2aa405..cc463e690 100644 --- a/src/classic/avdtp_source.h +++ b/src/classic/avdtp_source.h @@ -167,6 +167,7 @@ avdtp_stream_endpoint_t * avdtp_source_create_stream_endpoint(avdtp_sep_type_t s int avdtp_source_streaming_endpoint_ready(uint16_t con_handle); void avdtp_source_request_can_send_now(uint16_t con_handle); +void avdtp_source_stream_send_media_payload(uint16_t l2cap_media_cid, btstack_ring_buffer_t * sbc_ring_buffer, uint8_t marker); /* API_END */ #if defined __cplusplus diff --git a/test/avdtp/avdtp_source_demo.c b/test/avdtp/avdtp_source_demo.c index c61bbe5a8..afb0cdc36 100644 --- a/test/avdtp/avdtp_source_demo.c +++ b/test/avdtp/avdtp_source_demo.c @@ -142,7 +142,6 @@ static uint8_t media_sbc_codec_configuration[4]; static uint8_t audio_samples_storage[44100*4]; // 1s buffer static uint8_t sbc_samples_storage[44100*4]; -static uint8_t media_packet[1024]; static avdtp_stream_endpoint_context_t streaming_context; static paTestData sin_data; @@ -188,8 +187,6 @@ avdtp_application_state_t app_state = AVDTP_APPLICATION_IDLE; static btstack_packet_callback_registration_t hci_event_callback_registration; -static void send_media_packet_now(uint16_t l2cap_media_cid, int mtu, uint16_t sequence_number); - static const char * avdtp_si2str(uint16_t index){ if (index <= 0 || index > sizeof(avdtp_si_name)) return avdtp_si_name[0]; return avdtp_si_name[index]; @@ -444,8 +441,10 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe printf(" --- avdtp source --- Rejected %s\n", avdtp_si2str(signal_identifier)); break; case AVDTP_SUBEVENT_STREAMING_CAN_SEND_MEDIA_PACKET_NOW: { - uint16_t sequence_number = avdtp_subevent_streaming_can_send_media_packet_now_get_sequence_number(packet); - send_media_packet_now(local_stream_endpoint->l2cap_media_cid, l2cap_get_remote_mtu_for_local_cid(local_stream_endpoint->l2cap_media_cid), sequence_number); + avdtp_source_stream_send_media_payload(local_stream_endpoint->l2cap_media_cid, &streaming_context.sbc_ring_buffer, 0); + if (btstack_ring_buffer_bytes_available(&streaming_context.sbc_ring_buffer)){ + avdtp_source_request_can_send_now(local_stream_endpoint->l2cap_media_cid); + } break; } default: @@ -494,40 +493,35 @@ static uint8_t media_sbc_codec_capabilities[] = { 2, 53 }; -static void fill_sbc_ring_buffer(uint8_t * sbc_frame, int sbc_frame_size, avdtp_stream_endpoint_context_t * context){ - if (btstack_ring_buffer_bytes_free(&context->sbc_ring_buffer) >= sbc_frame_size ){ - // printf(" fill_sbc_ring_buffer\n"); - uint8_t size_buffer = sbc_frame_size; - btstack_ring_buffer_write(&context->sbc_ring_buffer, &size_buffer, 1); - btstack_ring_buffer_write(&context->sbc_ring_buffer, sbc_frame, sbc_frame_size); - } else { - printf("No space in sbc buffer\n"); - } -} -static void avdtp_source_stream_endpoint_run(void){ +static int fill_sbc_ring_buffer(void){ // performe sbc encoding int total_num_bytes_read = 0; int num_audio_samples_to_read = btstack_sbc_encoder_num_audio_frames(); int audio_bytes_to_read = num_audio_samples_to_read * BYTES_PER_AUDIO_SAMPLE; while (btstack_ring_buffer_bytes_available(&streaming_context.audio_ring_buffer) >= audio_bytes_to_read - && btstack_ring_buffer_bytes_free(&streaming_context.sbc_ring_buffer) >= 120){ // TODO use real value + && btstack_ring_buffer_bytes_free(&streaming_context.sbc_ring_buffer) >= btstack_sbc_encoder_sbc_buffer_length()){ // TODO use real value uint32_t number_of_bytes_read = 0; uint8_t pcm_frame[256*BYTES_PER_AUDIO_SAMPLE]; btstack_ring_buffer_read(&streaming_context.audio_ring_buffer, pcm_frame, audio_bytes_to_read, &number_of_bytes_read); btstack_sbc_encoder_process_data((int16_t *) pcm_frame); - uint16_t sbc_frame_bytes = btstack_sbc_encoder_sbc_buffer_length(); - + uint16_t sbc_frame_size = btstack_sbc_encoder_sbc_buffer_length(); + uint8_t * sbc_frame = btstack_sbc_encoder_sbc_buffer(); + total_num_bytes_read += number_of_bytes_read; - fill_sbc_ring_buffer(btstack_sbc_encoder_sbc_buffer(), sbc_frame_bytes, &streaming_context); - } - // schedule sending - if (total_num_bytes_read != 0){ - avdtp_source_request_can_send_now(media_con_handle); + + if (btstack_ring_buffer_bytes_free(&streaming_context.sbc_ring_buffer) >= sbc_frame_size ){ + uint8_t size_buffer = sbc_frame_size; + btstack_ring_buffer_write(&streaming_context.sbc_ring_buffer, &size_buffer, 1); + btstack_ring_buffer_write(&streaming_context.sbc_ring_buffer, sbc_frame, sbc_frame_size); + } else { + printf("No space in sbc buffer\n"); + } } + return total_num_bytes_read; } static void fill_audio_ring_buffer(void *userData, int num_samples_to_write){ @@ -573,8 +567,11 @@ static void avdtp_fill_audio_ring_buffer_timeout_handler(btstack_timer_source_t fill_audio_ring_buffer(&sin_data, num_samples); streaming_context.time_audio_data_sent = now; - avdtp_source_stream_endpoint_run(); - // + int total_num_bytes_read = fill_sbc_ring_buffer(); + // schedule sending + if (total_num_bytes_read != 0){ + avdtp_source_request_can_send_now(media_con_handle); + } } static void avdtp_fill_audio_ring_buffer_timer_start(void){ @@ -599,61 +596,6 @@ static void avdtp_source_stream_data_stop(){ avdtp_fill_audio_ring_buffer_timer_stop(); } -static void send_media_packet_now(uint16_t l2cap_media_cid, int mtu, uint16_t sequence_number){ - //send sbc - uint8_t rtp_version = 2; - uint8_t padding = 0; - uint8_t extension = 0; - uint8_t csrc_count = 0; - uint8_t marker = 0; - uint8_t payload_type = 0x60; - // uint16_t sequence_number = stream_endpoint->sequence_number; - uint32_t timestamp = btstack_run_loop_get_time_ms(); - uint32_t ssrc = 0x11223344; - - // rtp header (min size 12B) - int pos = 0; - // int mtu = l2cap_get_remote_mtu_for_local_cid(stream_endpoint->l2cap_media_cid); - - media_packet[pos++] = (rtp_version << 6) | (padding << 5) | (extension << 4) | csrc_count; - media_packet[pos++] = (marker << 1) | payload_type; - big_endian_store_16(media_packet, pos, sequence_number); - pos += 2; - big_endian_store_32(media_packet, pos, timestamp); - pos += 4; - big_endian_store_32(media_packet, pos, ssrc); // only used for multicast - pos += 4; - - // media payload - // sbc_header (size 1B) - uint8_t sbc_header_index = pos; - pos++; - uint8_t fragmentation = 0; - uint8_t starting_packet = 0; // set to 1 for the first packet of a fragmented SBC frame - uint8_t last_packet = 0; // set to 1 for the last packet of a fragmented SBC frame - uint8_t num_frames = 0; - - uint32_t total_sbc_bytes_read = 0; - uint8_t sbc_frame_size = 0; - // payload - uint16_t sbc_frame_bytes = btstack_sbc_encoder_sbc_buffer_length(); - - while (mtu - 13 - total_sbc_bytes_read >= sbc_frame_bytes && btstack_ring_buffer_bytes_available(&streaming_context.sbc_ring_buffer)){ - uint32_t number_of_bytes_read = 0; - btstack_ring_buffer_read(&streaming_context.sbc_ring_buffer, &sbc_frame_size, 1, &number_of_bytes_read); - btstack_ring_buffer_read(&streaming_context.sbc_ring_buffer, media_packet + pos, sbc_frame_size, &number_of_bytes_read); - pos += sbc_frame_size; - total_sbc_bytes_read += sbc_frame_size; - num_frames++; - // printf("send sbc frame: timestamp %d, seq. nr %d\n", timestamp, stream_endpoint->sequence_number); - } - media_packet[sbc_header_index] = (fragmentation << 7) | (starting_packet << 6) | (last_packet << 5) | num_frames; - l2cap_send(l2cap_media_cid, media_packet, pos); - - if (btstack_ring_buffer_bytes_available(&streaming_context.sbc_ring_buffer)){ - avdtp_source_request_can_send_now(media_con_handle); - } -} static void stdin_process(btstack_data_source_t *ds, btstack_data_source_callback_type_t callback_type){ UNUSED(ds); @@ -722,7 +664,6 @@ static void stdin_process(btstack_data_source_t *ds, btstack_data_source_callbac } } - int btstack_main(int argc, const char * argv[]); int btstack_main(int argc, const char * argv[]){ UNUSED(argc);