Skip to content

Commit

Permalink
WIP
Browse files Browse the repository at this point in the history
  • Loading branch information
@arkq
arkq committed Dec 15, 2024
1 parent ab7ad97 commit d2a0f87
Show file tree
Hide file tree
Showing 2 changed files with 102 additions and 8 deletions.
100 changes: 94 additions & 6 deletions src/a2dp-lhdc.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -428,11 +428,14 @@ void *a2dp_lhdc_dec_thread(struct ba_transport_pcm *t_pcm) {
pthread_cleanup_push(PTHREAD_CLEANUP(lhdcBT_dec_deinit_decoder), NULL);

ffb_t bt = { 0 };
ffb_t bt_payload = { 0 };
ffb_t pcm = { 0 };
pthread_cleanup_push(PTHREAD_CLEANUP(ffb_free), &bt);
pthread_cleanup_push(PTHREAD_CLEANUP(ffb_free), &bt_payload);
pthread_cleanup_push(PTHREAD_CLEANUP(ffb_free), &pcm);

if (ffb_init_int32_t(&pcm, 16 * 256 * channels) == -1 ||
ffb_init_uint8_t(&bt_payload, t->mtu_read) == -1 ||
ffb_init_uint8_t(&bt, t->mtu_read) == -1) {
error("Couldn't create data buffers: %s", strerror(errno));
goto fail_ffb;
Expand Down Expand Up @@ -466,17 +469,95 @@ void *a2dp_lhdc_dec_thread(struct ba_transport_pcm *t_pcm) {
continue;
}

const uint8_t *rtp_payload = (uint8_t *)rtp_lhdc_media_header;
size_t rtp_payload_len = len - (rtp_payload - (uint8_t *)bt.data);
const uint8_t *payload = (uint8_t *)(rtp_lhdc_media_header + 1);
size_t payload_len = len - (payload - (uint8_t *)bt.data);

int rv;
uint32_t decoded = ffb_blen_in(&pcm);
if ((rv = lhdcBT_dec_decode(rtp_payload, rtp_payload_len, pcm.data, &decoded, 24)) != 0) {
error("LHDC decoding error: %s", lhdcBT_dec_strerror(rv));
if (ffb_len_in(&bt_payload) < (size_t)len) {
debug("Resizing LHDC payload buffer: %zd -> %zd", bt_payload.nmemb, bt_payload.nmemb + t->mtu_read);
if (ffb_init_uint8_t(&bt_payload, bt_payload.nmemb + t->mtu_read) == -1)
error("Couldn't resize LHDC payload buffer: %s", strerror(errno));
}

if (ffb_blen_out(&bt_payload) == 0) {
/* Decoder API requires the LHDC media header to be present. */
payload = (uint8_t *)rtp_lhdc_media_header;
payload_len = len;
}

if (ffb_len_in(&bt_payload) >= payload_len) {
memcpy(bt_payload.tail, payload, payload_len);
ffb_seek(&bt_payload, payload_len);
}

if (rtp_lhdc_media_header->fragmented &&
!rtp_lhdc_media_header->last_fragment) {
debug("Fragmented LHDC frame [%u]: payload len: %zd",
rtp.seq_number, payload_len);
continue;
}

size_t bt_payload_len = ffb_len_out(&bt_payload);

rtp_lhdc_media_header_t * xx = bt_payload.data;

uint8_t * xx_payload = (uint8_t *)(xx + 1);
size_t xx_payload_len = bt_payload_len - sizeof(*xx);

int frame_len = (0xF & xx_payload[4]) << 8 | xx_payload[5];
debug("Decoding LHDC frames:%u seq number:%u, payload len: %zd lhdc-frame-len=%u",
xx->frame_count, xx->seq_number, bt_payload_len, frame_len);
hexdump("P", bt_payload.data, bt_payload_len);

if (xx->frame_count == 0) {
debug("LHDC frame count is 0, whyyyyyy?");
ffb_rewind(&bt_payload);
continue;
}

static int o = 0;
if (o++ > 6) break;

int frame_count = xx->frame_count;
int seq_number = xx->seq_number;


for (int i = 0; i < frame_count; i++) {

xx->fragmented = 0;
xx->first_fragment = 0;
xx->last_fragment = 0;
xx->frame_count = 1;
xx->seq_number = seq_number++;
xx_payload_len = ffb_len_out(&bt_payload) - sizeof(*xx);

// find sync word
int offset = 0;
while (offset < xx_payload_len - 1) {
if (xx_payload[offset] == 0x4C)
break;
offset++;
}

int frame_len = (0xF & xx_payload[offset - 1]) << 8 | xx_payload[offset - 2];
debug("iteration %d, offset %d, frame_len %d (payload %zu)", i, offset, frame_len, xx_payload_len);
hexdump("F", xx_payload, frame_len);
debug("rest of payload: %d", xx_payload_len - frame_len);
hexdump("R", xx_payload + frame_len, xx_payload_len - frame_len);

if (frame_len > xx_payload_len) {
debug("LHDC frame len is too big: %d > %zd", frame_len, xx_payload_len);
break;
}

int rv;
uint32_t decoded = ffb_blen_in(&pcm);
if ((rv = lhdcBT_dec_decode(bt_payload.data, 2+frame_len, pcm.data, &decoded, 24)) != 0) {
error("LHDC decoding error: %s", lhdcBT_dec_strerror(rv));
break;
}

const size_t samples = decoded / sample_size;
debug("Decoded %d bytes, %d samples", decoded, samples);

/* Upscale decoded 24-bit PCM samples to 32-bit. */
for (size_t i = 0; i < samples; i++)
Expand All @@ -489,6 +570,12 @@ void *a2dp_lhdc_dec_thread(struct ba_transport_pcm *t_pcm) {
/* update local state with decoded PCM frames */
rtp_state_update(&rtp, samples / channels);

ffb_shift(&bt_payload, frame_len);

}

ffb_rewind(&bt_payload);

}

fail:
Expand All @@ -497,6 +584,7 @@ void *a2dp_lhdc_dec_thread(struct ba_transport_pcm *t_pcm) {
pthread_cleanup_pop(1);
pthread_cleanup_pop(1);
pthread_cleanup_pop(1);
pthread_cleanup_pop(1);
fail_open:
pthread_cleanup_pop(1);
return NULL;
Expand Down
10 changes: 8 additions & 2 deletions src/rtp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,11 +77,17 @@ typedef struct rtp_mpeg_audio_header {
* LHDC media payload header. */
typedef struct rtp_lhdc_media_header {
#if __BYTE_ORDER == __BIG_ENDIAN
uint8_t frame_count:6;
uint8_t fragmented:1;
uint8_t first_fragment:1;
uint8_t last_fragment:1;
uint8_t frame_count:3;
uint8_t latency:2;
#else
uint8_t latency:2;
uint8_t frame_count:6;
uint8_t frame_count:3;
uint8_t last_fragment:1;
uint8_t first_fragment:1;
uint8_t fragmented:1;
#endif
uint8_t seq_number;
} __attribute__ ((packed)) rtp_lhdc_media_header_t;
Expand Down

0 comments on commit d2a0f87

Please sign in to comment.