static void a2dp_source_setup_media_header(uint8_t * media_packet, int size, int *offset, uint8_t marker, uint16_t sequence_number){
if (size < AVDTP_MEDIA_PAYLOAD_HEADER_SIZE){
log_error("small outgoing buffer");
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 hal_audio_dma_process(btstack_data_source_t * ds, btstack_data_source_callback_type_t callback_type){
UNUSED(ds);
UNUSED(callback_type);
if (!media_initialized) return;
int trigger_resume = 0;
if (audio_stream_paused) {
if (sbc_frame_size && btstack_ring_buffer_bytes_available(&ring_buffer) >= OPTIMAL_FRAMES_MIN * sbc_frame_size){
trigger_resume = 1;
// reset buffers
playback_buffer = NUM_AUDIO_BUFFERS - 1;
write_buffer = 0;
} else {
return;
}
}
while (playback_buffer != write_buffer && btstack_ring_buffer_bytes_available(&ring_buffer) >= sbc_frame_size ){
uint8_t frame[MAX_SBC_FRAME_SIZE];
uint32_t bytes_read = 0;
btstack_ring_buffer_read(&ring_buffer, frame, sbc_frame_size, &bytes_read);
btstack_sbc_decoder_process_data(&state, 0, frame, sbc_frame_size);
}
if (trigger_resume){
printf("%6u - resume\n", (int) btstack_run_loop_get_time_ms());
audio_stream_paused = 0;
}
}
static void test_track_sent(int bytes_sent){
test_data_sent += test_data_len;
// evaluate
uint32_t now = btstack_run_loop_get_time_ms();
uint32_t time_passed = now - test_data_start;
if (time_passed < REPORT_INTERVAL_MS) return;
// print speed
int bytes_per_second = test_data_sent * 1000 / time_passed;
printf("%u bytes sent-> %u.%03u kB/s\n", (int) test_data_sent, (int) bytes_per_second / 1000, bytes_per_second % 1000);
// restart
test_data_start = now;
test_data_sent = 0;
}
static void test_track_data(le_streamer_connection_t * context, int bytes_sent){
context->test_data_sent += bytes_sent;
// evaluate
uint32_t now = btstack_run_loop_get_time_ms();
uint32_t time_passed = now - context->test_data_start;
if (time_passed < REPORT_INTERVAL_MS) return;
// print speed
int bytes_per_second = context->test_data_sent * 1000 / time_passed;
printf("%c: %"PRIu32" bytes -> %u.%03u kB/s\n", context->name, context->test_data_sent, bytes_per_second / 1000, bytes_per_second % 1000);
// restart
context->test_data_start = now;
context->test_data_sent = 0;
}
static void avdtp_audio_timeout_handler(btstack_timer_source_t * timer){
a2dp_media_sending_context_t * context = (a2dp_media_sending_context_t *) btstack_run_loop_get_timer_context(timer);
btstack_run_loop_set_timer(&context->audio_timer, AUDIO_TIMEOUT_MS);
btstack_run_loop_add_timer(&context->audio_timer);
uint32_t now = btstack_run_loop_get_time_ms();
uint32_t update_period_ms = AUDIO_TIMEOUT_MS;
if (context->time_audio_data_sent > 0){
update_period_ms = now - context->time_audio_data_sent;
}
uint32_t num_samples = (update_period_ms * a2dp_sample_rate()) / 1000;
context->acc_num_missed_samples += (update_period_ms * a2dp_sample_rate()) % 1000;
while (context->acc_num_missed_samples >= 1000){
num_samples++;
context->acc_num_missed_samples -= 1000;
}
context->time_audio_data_sent = now;
context->samples_ready += num_samples;
if (context->sbc_ready_to_send) return;
fill_sbc_audio_buffer(context);
if ((context->sbc_storage_count + btstack_sbc_encoder_sbc_buffer_length()) > context->max_media_payload_size){
// schedule sending
context->sbc_ready_to_send = 1;
// a2dp_source_stream_endpoint_request_can_send_now(context->local_seid);
avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_for_seid(context->local_seid, &a2dp_source_context);
if (!stream_endpoint) {
printf("no stream_endpoint for seid %d\n", context->local_seid);
return;
}
stream_endpoint->send_stream = 1;
if (stream_endpoint->connection){
avdtp_request_can_send_now_initiator(stream_endpoint->connection, stream_endpoint->l2cap_media_cid);
}
}
}
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;
if (!read_media_data_header(packet, size, &pos, &media_header)) return;
avdtp_sbc_codec_header_t sbc_header;
if (!read_sbc_header(packet, size, &pos, &sbc_header)) return;
#ifdef HAVE_AUDIO_DMA
// store sbc frame size for buffer management
sbc_frame_size = (size-pos)/ sbc_header.num_frames;
#endif
#if defined(HAVE_PORTAUDIO) || defined(STORE_SBC_TO_WAV_FILE)
btstack_sbc_decoder_process_data(&state, 0, packet+pos, size-pos);
#endif
#ifdef HAVE_AUDIO_DMA
btstack_ring_buffer_write(&ring_buffer, packet+pos, size-pos);
// decide on audio sync drift based on number of sbc frames in queue
int sbc_frames_in_buffer = btstack_ring_buffer_bytes_available(&ring_buffer) / sbc_frame_size;
if (sbc_frames_in_buffer < OPTIMAL_FRAMES_MIN){
sbc_samples_fix = 1; // duplicate last sample
} else if (sbc_frames_in_buffer <= OPTIMAL_FRAMES_MAX){
sbc_samples_fix = 0; // nothing to do
} else {
sbc_samples_fix = -1; // drop last sample
}
// dump
printf("%6u %03u %d\n", (int) btstack_run_loop_get_time_ms(), sbc_frames_in_buffer, sbc_samples_fix);
#endif
#ifdef STORE_SBC_TO_SBC_FILE
fwrite(packet+pos, size-pos, 1, sbc_file);
#endif
}
void hal_audio_dma_done(void){
if (audio_stream_paused){
hal_audio_dma_play((const uint8_t *) silent_buffer, DMA_AUDIO_FRAMES*4);
return;
}
// next buffer
int next_playback_buffer = next_buffer(playback_buffer);
uint8_t * playback_data;
if (next_playback_buffer == write_buffer){
// TODO: stop codec while playing silence when getting 'stream paused'
// start playing silence
audio_stream_paused = 1;
hal_audio_dma_play((const uint8_t *) silent_buffer, DMA_AUDIO_FRAMES*4);
printf("%6u - paused - bytes in buffer %u\n", (int) btstack_run_loop_get_time_ms(), btstack_ring_buffer_bytes_available(&ring_buffer));
return;
}
playback_buffer = next_playback_buffer;
playback_data = start_of_buffer(playback_buffer);
hal_audio_dma_play(playback_data, audio_samples_len[playback_buffer]);
// btstack_run_loop_embedded_trigger();
}
fclose(wav_file);
}
dump_data = 0;
}
#endif
#endif
if (packet[1] & 0xf0){
printf("SCO CRC Error: %x - data: ", packet[1] >> 4);
printf_hexdump(&packet[3], size-3);
return;
}
#if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
#ifdef USE_PORTAUDIO
uint32_t start = btstack_run_loop_get_time_ms();
Pa_WriteStream( stream, &packet[3], size -3);
uint32_t end = btstack_run_loop_get_time_ms();
if (end - start > 5){
printf("Portaudio: write stream took %u ms\n", end - start);
}
dump_data = 0;
#endif
#endif
if (dump_data){
printf("data: ");
#if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII
int i;
for (i=3;i<size;i++){
printf("%c", packet[i]);
static void test_reset(void){
test_data_start = btstack_run_loop_get_time_ms();
test_data_sent = 0;
}
static void test_reset(le_streamer_connection_t * context){
context->test_data_start = btstack_run_loop_get_time_ms();
context->test_data_sent = 0;
}
