LDAC.cpp

// Copyright RF Creations Ltd 2026
// Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE)

/**
 * @file LDAC.cpp
 * @brief LDAC codec plugin for blueSPY
 *
 * Implements LDAC decoding for AVDTP/A2DP Classic audio streams
 * using the libldacdec decoder library. LDAC is Sony's high-resolution audio
 * codec supporting up to 96kHz/24bit audio.
 */

#include "bluespy_codec_interface.h"
#include "bluespy_codec_utils.h"
#include "codec_structures.h"
#include "ldacdec.h"

#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

/*------------------------------------------------------------------------------
 * Constants & Offsets
 *----------------------------------------------------------------------------*/

#define PCM_BUFFER_SAMPLES 8192 /* Max 16-bit samples per decode cycle */
#define MIN_PAYLOAD_SIZE 20     /* Minimum valid LDAC packet size */

/** Sony Vendor ID (little-endian) */
#define VENDOR_ID_SONY 0x0000012D

/** LDAC Codec ID */
#define CODEC_ID_LDAC 0xAA

/** LDAC sync byte */
#define LDAC_SYNC_BYTE 0xAA

/** LDAC Configuration Offsets (Media_Codec_Specific_Information) */
#define LDAC_CONFIG_LEN_MIN 8
#define LDAC_OFFSET_CODEC_ID 4
#define LDAC_OFFSET_FREQ 6
#define LDAC_OFFSET_CH_MODE 7

/** LDAC sample rate bits (in config byte 6) */
#define LDAC_FREQ_192000 0x01
#define LDAC_FREQ_176400 0x02
#define LDAC_FREQ_96000 0x04
#define LDAC_FREQ_88200 0x08
#define LDAC_FREQ_48000 0x10
#define LDAC_FREQ_44100 0x20

/** LDAC channel mode bits (in config byte 7) */
#define LDAC_CH_MODE_STEREO 0x01
#define LDAC_CH_MODE_DUAL 0x02
#define LDAC_CH_MODE_MONO 0x04

/*------------------------------------------------------------------------------
 * Types
 *----------------------------------------------------------------------------*/

/**
 * @brief LDAC decoder state
 */
typedef struct {
    bluespy_audiostream_id parent_stream_id;
    bool initialized;

    /* RTP sequence tracking */
    bool has_last_seq;
    uint16_t last_rtp_seq;

    /* Gap estimation heuristic */
    uint32_t samples_per_packet;

    /* ldacBT decoder instance */
    ldacdec_t decoder;

    /* Stream configuration (may be updated during decode) */
    uint32_t sample_rate;
    uint8_t channels;

    /* Output buffer */
    int16_t pcm_buffer[PCM_BUFFER_SAMPLES];
} LDAC_stream;

/*------------------------------------------------------------------------------
 * Configuration Parsing
 *----------------------------------------------------------------------------*/

/**
 * @brief Check if configuration is for LDAC codec
 *
 * @param cap  AVDTP Media Codec capability structure
 * @return true if this is an LDAC configuration
 */
static bool is_ldac_config(const AVDTP_Service_Capabilities_Media_Codec_t* cap) {
    if (cap->Media_Codec_Type != AVDTP_Codec_Vendor_Specific) {
        return false;
    }

    const uint8_t* info = cap->Media_Codec_Specific_Information;
    uint32_t vendor_id = read_le32(info);
    uint8_t codec_id = info[LDAC_OFFSET_CODEC_ID];

    return (vendor_id == VENDOR_ID_SONY && codec_id == CODEC_ID_LDAC);
}

/**
 * @brief Parse sample rate from LDAC configuration
 *
 * @param config  Pointer to Media_Codec_Specific_Information
 * @return Sample rate in Hz
 */
static uint32_t parse_sample_rate(const uint8_t* config) {
    uint8_t freq_bits = config[LDAC_OFFSET_FREQ];

    if (freq_bits & LDAC_FREQ_96000)
        return 96000;
    if (freq_bits & LDAC_FREQ_88200)
        return 88200;
    if (freq_bits & LDAC_FREQ_48000)
        return 48000;
    if (freq_bits & LDAC_FREQ_44100)
        return 44100;
    if (freq_bits & LDAC_FREQ_192000)
        return 192000;
    if (freq_bits & LDAC_FREQ_176400)
        return 176400;

    /* Default to 48kHz if no bits are explicitly matched */
    return 48000;
}

/**
 * @brief Parse channel count from LDAC configuration
 *
 * @param config  Pointer to Media_Codec_Specific_Information
 * @return Number of channels (1 or 2)
 */
static uint8_t parse_channels(const uint8_t* config) {
    uint8_t ch_bits = config[LDAC_OFFSET_CH_MODE];

    // Note: Both STEREO and DUAL mean 2 channels of output for the decoder
    if (ch_bits & LDAC_CH_MODE_STEREO)
        return 2;
    if (ch_bits & LDAC_CH_MODE_DUAL)
        return 2;
    if (ch_bits & LDAC_CH_MODE_MONO)
        return 1;

    /* Default to Stereo if no bits are explicitly matched */
    return 2;
}

/*------------------------------------------------------------------------------
 * RTP / Frame Processing
 *----------------------------------------------------------------------------*/

/**
 * @brief Find LDAC sync byte in buffer
 *
 * @param data      Buffer to search
 * @param length    Length of buffer
 * @return Offset to sync byte, or length if not found
 */
static uint32_t find_sync_byte(const uint8_t* data, uint32_t length) {
    for (uint32_t i = 0; i < length; ++i) {
        if (data[i] == LDAC_SYNC_BYTE) {
            return i;
        }
    }
    return length;
}

/*------------------------------------------------------------------------------
 * API Implementation
 *----------------------------------------------------------------------------*/

extern "C" {

BLUESPY_CODEC_API bluespy_audio_codec_lib_info init(void) {
    return bluespy_audio_codec_lib_info{.api_version = BLUESPY_AUDIO_API_VERSION,
                                        .codec_name = "LDAC"};
}

BLUESPY_CODEC_API bluespy_audio_codec_init_ret
new_codec_stream(bluespy_audiostream_id stream_id, const bluespy_audio_codec_info* info) {
    bluespy_audio_codec_init_ret ret = {
        .error = -1, .format = {0}, .fns = {0}, .context_handle = 0};

    /* Only handle AVDTP container */
    if (!info || info->container != BLUESPY_CODEC_AVDTP) {
        return ret;
    }

    /* Validate configuration */
    const AVDTP_Service_Capabilities_Media_Codec_t* cap =
        (const AVDTP_Service_Capabilities_Media_Codec_t*)info->config;
    if (!cap || !is_ldac_config(cap)) {
        return ret;
    }
    if (info->config_len < LDAC_CONFIG_LEN_MIN) {
        ret.error = -2;
        return ret;
    }

    /* Parse configuration */
    const uint8_t* codec_info = cap->Media_Codec_Specific_Information;
    uint32_t sample_rate = parse_sample_rate(codec_info);
    if (sample_rate == 0) {
        ret.error = -4;
        return ret;
    }
    uint8_t channels = parse_channels(codec_info);

    /* Dry run to allow the host to check if this codec format is supported */
    if (stream_id == BLUESPY_ID_INVALID) {
        ret.error = 0;
        ret.format.sample_rate = sample_rate;
        ret.format.n_channels = channels;
        return ret;
    }

    /* Allocate State */
    LDAC_stream* stream = (LDAC_stream*)calloc(1, sizeof(LDAC_stream));
    if (!stream) {
        ret.error = -3;
        return ret;
    }
    stream->sample_rate = sample_rate;
    stream->channels = channels;
    stream->parent_stream_id = stream_id;

    /* Initialise LDAC decoder */
    memset(&stream->decoder, 0, sizeof(stream->decoder));
    if (ldacdecInit(&stream->decoder) < 0) {
        free(stream);
        ret.error = -4;
        return ret;
    }

    stream->initialized = true;
    stream->has_last_seq = false;
    stream->last_rtp_seq = 0;
    stream->samples_per_packet = 128 * stream->channels; // conservative initial guess

    /* Success */
    ret.error = 0;
    ret.context_handle = (uintptr_t)stream;

    ret.format.sample_rate = stream->sample_rate;
    ret.format.n_channels = stream->channels;
    ret.format.sample_format = BLUESPY_AUDIO_FORMAT_S16_LE;
    ret.fns.decode = codec_decode;
    ret.fns.deinit = codec_deinit;

    return ret;
}

BLUESPY_CODEC_API void codec_decode(uintptr_t context, const uint8_t* payload, uint32_t payload_len,
                                    bluespy_event_id event_id, uint64_t sequence_number) {
    (void)sequence_number;

    LDAC_stream* stream = (LDAC_stream*)context;
    if (!stream || !stream->initialized) {
        return;
    }

    if (!payload || payload_len < MIN_PAYLOAD_SIZE) {
        return;
    }

    uint32_t rtp_len = get_rtp_header_length(payload, payload_len);
    if (rtp_len == 0) {
        return;
    }

    /* Extract RTP sequence number and calculate gap */
    uint16_t rtp_seq = read_be16(payload + RTP_SEQ_OFFSET);
    uint32_t missing_samples = 0;

    if (stream->has_last_seq) {
        int32_t diff = calculate_rtp_seq_diff(rtp_seq, stream->last_rtp_seq);

        if (diff > 1) {
            // Gap detected
            uint32_t missing_packets = (uint32_t)(diff - 1);
            missing_samples = missing_packets * stream->samples_per_packet;
        }
    }

    stream->last_rtp_seq = rtp_seq;
    stream->has_last_seq = true;

    /* Strip RTP header */
    const uint8_t* frame = payload + rtp_len;
    uint32_t remaining = payload_len - rtp_len;

    /* Find first LDAC sync byte (0xAA) */
    uint32_t sync_offset = find_sync_byte(frame, remaining);
    if (sync_offset >= remaining) {
        return;
    }

    frame += sync_offset;
    remaining -= sync_offset;

    /* Decode LDAC frames */
    int16_t* pcm_out = stream->pcm_buffer;
    size_t total_samples = 0;
    const size_t max_samples = PCM_BUFFER_SAMPLES;

    while (remaining > 0 && total_samples < max_samples) {
        int bytes_consumed = 0;
        int result =
            ldacDecode(&stream->decoder, (uint8_t*)frame, pcm_out + total_samples, &bytes_consumed);

        if (result < 0) {
            /* Decode error - attempt to resync */
            uint32_t resync_offset = find_sync_byte(frame + 1, remaining - 1);
            if (resync_offset >= remaining - 1) {
                break; /* No more sync bytes found */
            }
            frame += 1 + resync_offset;
            remaining -= 1 + resync_offset;
            continue;
        }

        if (bytes_consumed <= 0) {
            break;
        }

        if ((uint32_t)bytes_consumed > remaining) {
            break;
        }

        frame += bytes_consumed;
        remaining -= bytes_consumed;

        /* Get frame info from decoder */
        int frame_samples = stream->decoder.frame.frameSamples;
        int frame_channels = stream->decoder.frame.channelCount;
        size_t samples_decoded = (size_t)(frame_samples * frame_channels);

        total_samples += samples_decoded;
    }

    if (total_samples > 0) {
        stream->samples_per_packet = (uint32_t)total_samples / stream->channels;

        /* Update stream parameters from decoder (may change during stream) */
        stream->sample_rate = ldacdecGetSampleRate(&stream->decoder);
        stream->channels = (uint8_t)ldacdecGetChannelCount(&stream->decoder);

        /* Deliver decoded audio */
        uint32_t pcm_bytes = (uint32_t)(total_samples * sizeof(int16_t));
        bluespy_add_audio((const uint8_t*)stream->pcm_buffer, pcm_bytes, event_id, missing_samples);
    } else if (missing_samples > 0) {
        bluespy_add_audio(NULL, 0, event_id, missing_samples);
    }
}

BLUESPY_CODEC_API void codec_deinit(uintptr_t context) {
    LDAC_stream* stream = (LDAC_stream*)context;
    if (stream) {
        free(stream);
    }
}

} // end extern "C"