diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000000..3754879b9e --- /dev/null +++ b/.gitmodules @@ -0,0 +1,9 @@ +[submodule "3rd-party/lhdcv5-decoder"] + path = 3rd-party/lhdcv5-decoder + url = https://github.com/WillyBilly06/LHDC-V5-Decoder.git +[submodule "3rd-party/lhdcv5-encoder"] + path = 3rd-party/lhdcv5-encoder + url = https://github.com/WillyBilly06/LHDC-V5-Encoder.git +[submodule "3rd-party/portaudio"] + path = 3rd-party/portaudio + url = https://github.com/PortAudio/portaudio.git diff --git a/3rd-party/lhdcv5-decoder b/3rd-party/lhdcv5-decoder new file mode 160000 index 0000000000..63d564b755 --- /dev/null +++ b/3rd-party/lhdcv5-decoder @@ -0,0 +1 @@ +Subproject commit 63d564b755d869e03b6b6c492fe6765dd1151412 diff --git a/3rd-party/lhdcv5-encoder b/3rd-party/lhdcv5-encoder new file mode 160000 index 0000000000..3f9d1980fa --- /dev/null +++ b/3rd-party/lhdcv5-encoder @@ -0,0 +1 @@ +Subproject commit 3f9d1980fa9c57cdfb78792f9455a7dbac50edda diff --git a/3rd-party/portaudio b/3rd-party/portaudio new file mode 160000 index 0000000000..a880212eea --- /dev/null +++ b/3rd-party/portaudio @@ -0,0 +1 @@ +Subproject commit a880212eea40537d4c7906d70dc6adb17c8949e9 diff --git a/chipset/intel/btstack_chipset_intel_firmware.c b/chipset/intel/btstack_chipset_intel_firmware.c index acea0f2ad0..6e4af88b0f 100644 --- a/chipset/intel/btstack_chipset_intel_firmware.c +++ b/chipset/intel/btstack_chipset_intel_firmware.c @@ -100,6 +100,61 @@ typedef struct { uint8_t unlocked_state; } intel_boot_params_t; +#define INTEL_TLV_CNVI_TOP 0x10 +#define INTEL_TLV_CNVR_TOP 0x11 +#define INTEL_TLV_CNVI_BT 0x12 +#define INTEL_TLV_CNVR_BT 0x13 +#define INTEL_TLV_DEV_REV_ID 0x16 +#define INTEL_TLV_IMAGE_TYPE 0x1c +#define INTEL_TLV_TIME_STAMP 0x1d +#define INTEL_TLV_BUILD_TYPE 0x1e +#define INTEL_TLV_BUILD_NUM 0x1f +#define INTEL_TLV_SECURE_BOOT 0x28 +#define INTEL_TLV_OTP_LOCK 0x2a +#define INTEL_TLV_API_LOCK 0x2b +#define INTEL_TLV_DEBUG_LOCK 0x2c +#define INTEL_TLV_MIN_FW 0x2d +#define INTEL_TLV_LIMITED_CCE 0x2e +#define INTEL_TLV_SBE_TYPE 0x2f +#define INTEL_TLV_OTP_BDADDR 0x30 +#define INTEL_TLV_GIT_SHA1 0x32 +#define INTEL_TLV_FW_ID 0x50 + +#define INTEL_IMG_BOOTLOADER 0x01 +#define INTEL_IMG_IML 0x02 +#define INTEL_IMG_OPERATIONAL 0x03 +#define INTEL_RSA_HEADER_LEN 644u +#define INTEL_ECDSA_HEADER_LEN 320u +#define INTEL_ECDSA_OFFSET 644u +#define INTEL_CSS_HEADER_OFFSET 8u +#define INTEL_CSS_RSA_VERSION 0x00010000u +#define INTEL_CSS_ECDSA_VERSION 0x00020000u +#define INTEL_FW_ID_MAXLEN 64u + +typedef struct { + uint32_t cnvi_top; + uint32_t cnvr_top; + uint32_t cnvi_bt; + uint32_t cnvr_bt; + uint16_t dev_rev_id; + uint8_t img_type; + uint16_t timestamp; + uint8_t build_type; + uint32_t build_num; + uint8_t secure_boot; + uint8_t otp_lock; + uint8_t api_lock; + uint8_t debug_lock; + uint8_t min_fw_build_nn; + uint8_t min_fw_build_cw; + uint8_t min_fw_build_yy; + uint8_t limited_cce; + uint8_t sbe_type; + uint32_t git_sha1; + char fw_id[INTEL_FW_ID_MAXLEN]; + bd_addr_t otp_bd_addr; +} intel_version_tlv_t; + typedef enum { INTEL_CONTROLLER_LEGACY, INTEL_CONTROLLER_TLV, @@ -119,14 +174,19 @@ typedef enum { STATE_HANDLE_VENDOR_SPECIFIC_EVENT_02 = 10, STATE_HANDLE_READ_VERSION_2 = 11, STATE_SEND_DDC = 12, + STATE_TLV_SEND_ECDSA_PUBLIC_KEY = 13, + STATE_TLV_SEND_ECDSA_SIGNATURE = 14, STATE_DONE = 15 } state_t; // Vendor specific commands -static const hci_cmd_t hci_intel_read_version = { +static const hci_cmd_t hci_intel_read_version_tlv = { 0xfc05, "1" }; +static const hci_cmd_t hci_intel_read_version_legacy = { + 0xfc05, "" +}; static const hci_cmd_t hci_intel_read_secure_boot_params = { 0xfc0d, "" }; @@ -145,6 +205,12 @@ const char * firmware_folder_path = "."; static intel_version_t intel_version; static intel_boot_params_t intel_boot_params; +static intel_version_tlv_t intel_version_tlv; +static uint32_t intel_boot_param; +static size_t firmware_payload_offset; +static int firmware_payload_started; +static size_t tlv_header_base; +static uint8_t tlv_use_ecdsa; static intel_controller_mode_t controller_mode; @@ -166,6 +232,7 @@ static void (*done)(int result); // protogtypes static void state_machine(uint8_t *packet, uint16_t size); +static int intel_send_fragment(uint8_t fragment_type, uint8_t len); // functions @@ -229,6 +296,106 @@ static int intel_get_firmware_name(intel_version_t *version, intel_boot_params_t return 0; } +static uint8_t intel_tlv_hw_platform(const intel_version_tlv_t *version){ + return (uint8_t)((version->cnvi_bt >> 8) & 0xffu); +} + +static uint8_t intel_tlv_hw_variant(const intel_version_tlv_t *version){ + return (uint8_t)((version->cnvi_bt >> 16) & 0x3fu); +} + +static uint16_t intel_tlv_pack_top(uint32_t top){ + uint16_t value = (uint16_t)(((top & 0x0fffu) << 4) | ((top >> 24) & 0x0fu)); + return (uint16_t)((value >> 8) | (value << 8)); +} + +static int intel_get_firmware_name_tlv(const intel_version_tlv_t *version, const char *folder_path, + const char *suffix, char *firmware_path, size_t firmware_path_len){ + uint16_t cnvi = intel_tlv_pack_top(version->cnvi_top); + uint16_t cnvr = intel_tlv_pack_top(version->cnvr_top); + int len = snprintf(firmware_path, firmware_path_len, "%s/ibt-%04x-%04x.%s", + folder_path, cnvi, cnvr, suffix); + return (len > 0 && (size_t)len < firmware_path_len) ? 0 : -1; +} + +static int intel_parse_version_tlv(const uint8_t *data, uint16_t len, intel_version_tlv_t *version){ + uint16_t pos = 1; + if (data == NULL || version == NULL || len < 1 || data[0] != 0) return -1; + memset(version, 0, sizeof(*version)); + + while ((uint16_t)(pos + 2) <= len){ + uint8_t type = data[pos++]; + uint8_t value_len = data[pos++]; + if ((uint16_t)(pos + value_len) > len) return -1; + const uint8_t *value = &data[pos]; + + switch (type){ + case INTEL_TLV_CNVI_TOP: if (value_len >= 4) version->cnvi_top = little_endian_read_32(value, 0); break; + case INTEL_TLV_CNVR_TOP: if (value_len >= 4) version->cnvr_top = little_endian_read_32(value, 0); break; + case INTEL_TLV_CNVI_BT: if (value_len >= 4) version->cnvi_bt = little_endian_read_32(value, 0); break; + case INTEL_TLV_CNVR_BT: if (value_len >= 4) version->cnvr_bt = little_endian_read_32(value, 0); break; + case INTEL_TLV_DEV_REV_ID: if (value_len >= 2) version->dev_rev_id = little_endian_read_16(value, 0); break; + case INTEL_TLV_IMAGE_TYPE: if (value_len >= 1) version->img_type = value[0]; break; + case INTEL_TLV_TIME_STAMP: if (value_len >= 2) version->timestamp = little_endian_read_16(value, 0); break; + case INTEL_TLV_BUILD_TYPE: if (value_len >= 1) version->build_type = value[0]; break; + case INTEL_TLV_BUILD_NUM: if (value_len >= 4) version->build_num = little_endian_read_32(value, 0); break; + case INTEL_TLV_SECURE_BOOT: if (value_len >= 1) version->secure_boot = value[0]; break; + case INTEL_TLV_OTP_LOCK: if (value_len >= 1) version->otp_lock = value[0]; break; + case INTEL_TLV_API_LOCK: if (value_len >= 1) version->api_lock = value[0]; break; + case INTEL_TLV_DEBUG_LOCK: if (value_len >= 1) version->debug_lock = value[0]; break; + case INTEL_TLV_MIN_FW: + if (value_len >= 3){ + version->min_fw_build_nn = value[0]; + version->min_fw_build_cw = value[1]; + version->min_fw_build_yy = value[2]; + } + break; + case INTEL_TLV_LIMITED_CCE: if (value_len >= 1) version->limited_cce = value[0]; break; + case INTEL_TLV_SBE_TYPE: if (value_len >= 1) version->sbe_type = value[0]; break; + case INTEL_TLV_GIT_SHA1: if (value_len >= 4) version->git_sha1 = little_endian_read_32(value, 0); break; + case INTEL_TLV_FW_ID: { + size_t copy_len = value_len; + if (copy_len >= sizeof(version->fw_id)) copy_len = sizeof(version->fw_id) - 1; + memcpy(version->fw_id, value, copy_len); + version->fw_id[copy_len] = 0; + break; + } + case INTEL_TLV_OTP_BDADDR: + if (value_len >= sizeof(bd_addr_t)) memcpy(version->otp_bd_addr, value, sizeof(bd_addr_t)); + break; + default: + break; + } + pos = (uint16_t)(pos + value_len); + } + return pos == len ? 0 : -1; +} + +static void dump_intel_version_tlv(const intel_version_tlv_t *version){ + log_info("Intel TLV: CNVi top 0x%08" PRIx32 ", CNVr top 0x%08" PRIx32, version->cnvi_top, version->cnvr_top); + log_info("Intel TLV: CNVi BT 0x%08" PRIx32 ", CNVr BT 0x%08" PRIx32, version->cnvi_bt, version->cnvr_bt); + log_info("Intel TLV: platform 0x%02x variant 0x%02x rev 0x%04x image 0x%02x", + intel_tlv_hw_platform(version), intel_tlv_hw_variant(version), version->dev_rev_id, version->img_type); + log_info("Intel TLV: secure boot %u SBE %u limited CCE %u build 0x%08" PRIx32, + version->secure_boot, version->sbe_type, version->limited_cce, version->build_num); + if (version->fw_id[0]) log_info("Intel TLV: FW ID %s", version->fw_id); +} + +static int intel_read_css_version(FILE *file, size_t base, uint32_t *version){ + uint8_t value[4]; + if (file == NULL || version == NULL) return -1; + if (fseek(file, (long)(base + INTEL_CSS_HEADER_OFFSET), SEEK_SET) != 0) return -1; + if (fread(value, 1, sizeof(value), file) != sizeof(value)) return -1; + *version = little_endian_read_32(value, 0); + return 0; +} + +static int intel_seek_and_send_fragment(uint8_t fragment_type, size_t offset, uint8_t len){ + if (fw_file == NULL || fseek(fw_file, (long)offset, SEEK_SET) != 0) return -1; + fw_offset = offset; + return intel_send_fragment(fragment_type, len); +} + static int transport_send_packet(uint8_t packet_type, const uint8_t * packet, uint16_t size){ hci_dump_packet(HCI_COMMAND_DATA_PACKET, 0, (uint8_t*) packet, size); return transport->send_packet(packet_type, (uint8_t *) packet, size); @@ -277,15 +444,16 @@ static int intel_send_fragment(uint8_t fragment_type, uint8_t len){ // @returns -1 on eof static int intel_send_ddc(void){ size_t res; - // read len res = fread(fw_buffer, 1, 1, fw_file); - log_info("offset %6" PRId32 ", read 1 -> res %" PRId32 "", (int32_t)fw_offset, (int32_t)res); if (res == 0) return -1; + if (res != 1) return -2; uint8_t len = fw_buffer[0]; fw_offset += 1; + res = fread(&fw_buffer[1], 1, len, fw_file); - log_info("offset %6" PRId32 ", read %u -> res %" PRId32 "", (int32_t)fw_offset, 1, (int32_t)res); - return transport_send_intel_ddc(fw_buffer, 1 + len); + if (res != len) return -2; + fw_offset += res; + return transport_send_intel_ddc(fw_buffer, (uint8_t)(1 + len)); } static void state_machine(uint8_t *packet, uint16_t size) { @@ -313,30 +481,123 @@ static void state_machine(uint8_t *packet, uint16_t size) { transport_send_cmd(&hci_reset); break; case STATE_HANDLE_HCI_RESET: - // check if HCI Reset was supported - if (packet[0] == 0x0e && packet[1] == 0x04 && packet[3] == 0x03 && packet[4] == 0x0c && packet[5] == 0x00){ - log_info("HCI Reset was successful, no need for firmware upload / or not an Intel chipset"); - (*done)(0); - break; - } - - // Read Intel Version + // This loader is used only by the Intel-specific port. Always query Intel + // version after reset so a bootloader that accepts HCI Reset is not mistaken + // for an already operational controller. + // Parameter 0xff requests the TLV format on modern controllers. state = STATE_HANDLE_READ_VERSION_1; - transport_send_cmd(&hci_intel_read_version, 0xff); + transport_send_cmd(&hci_intel_read_version_tlv, 0xff); break; - case STATE_HANDLE_READ_VERSION_1: - // detect legacy vs. new TLV mode based on Read Version response - if ((size == sizeof(intel_version_t)) || (packet[1] != 0x037)){ + + case STATE_HANDLE_READ_VERSION_1: { + const uint8_t *return_params = hci_event_command_complete_get_return_parameters(packet); + uint16_t return_len = packet[1] >= 3 ? (uint16_t)(packet[1] - 3) : 0; + + if (return_len != sizeof(intel_version_t)){ + uint32_t rsa_version = 0; + uint32_t ecdsa_version = 0; + uint8_t hw_platform; + uint8_t hw_variant; + controller_mode = INTEL_CONTROLLER_TLV; - printf("\nERROR: Intel Controller uses new TLV mode. TLV mode is not supported yet\n"); - printf("Details: https://github.com/torvalds/linux/blob/master/drivers/bluetooth/btintel.c\n\n"); - log_error("TLV mode not supported"); - (*done)(1); + if (intel_parse_version_tlv(return_params, return_len, &intel_version_tlv) != 0){ + log_error("Invalid Intel TLV Read Version response"); + (*done)(1); + break; + } + dump_intel_version_tlv(&intel_version_tlv); + hw_platform = intel_tlv_hw_platform(&intel_version_tlv); + hw_variant = intel_tlv_hw_variant(&intel_version_tlv); + + if (hw_platform != 0x37){ + log_error("Unsupported Intel TLV platform 0x%02x", hw_platform); + (*done)(1); + break; + } + if (intel_version_tlv.img_type == INTEL_IMG_OPERATIONAL){ + printf("Intel TLV firmware is already operational\n"); + (*done)(0); + break; + } + if (intel_version_tlv.img_type != INTEL_IMG_BOOTLOADER){ + log_error("Unsupported Intel image type 0x%02x", intel_version_tlv.img_type); + (*done)(1); + break; + } + if (intel_version_tlv.limited_cce != 0){ + log_error("Intel controller reports limited CCE mode %u", intel_version_tlv.limited_cce); + (*done)(1); + break; + } + if (intel_version_tlv.sbe_type > 1){ + log_error("Unsupported Intel secure boot engine type %u", intel_version_tlv.sbe_type); + (*done)(1); + break; + } + + if (intel_get_firmware_name_tlv(&intel_version_tlv, firmware_folder_path, + "sfi", fw_path, sizeof(fw_path)) != 0){ + log_error("Could not construct Intel TLV firmware filename"); + (*done)(1); + break; + } + printf("Intel TLV platform 0x%02x variant 0x%02x\n", hw_platform, hw_variant); + printf("Firmware %s\n", fw_path); + fw_file = fopen(fw_path, "rb"); + if (fw_file == NULL){ + log_error("can't open file %s", fw_path); + (*done)(1); + break; + } + + if (intel_read_css_version(fw_file, 0, &rsa_version) != 0 || rsa_version != INTEL_CSS_RSA_VERSION){ + log_error("Intel SFI does not contain the expected RSA CSS header"); + fclose(fw_file); + fw_file = NULL; + (*done)(1); + break; + } + + // AX211-generation firmware carries an ECDSA CSS header after the legacy RSA header. + if (hw_variant >= 0x17){ + if (intel_read_css_version(fw_file, INTEL_ECDSA_OFFSET, &ecdsa_version) != 0 || + ecdsa_version != INTEL_CSS_ECDSA_VERSION){ + log_error("Intel SFI does not contain the expected ECDSA CSS header"); + fclose(fw_file); + fw_file = NULL; + (*done)(1); + break; + } + firmware_payload_offset = INTEL_RSA_HEADER_LEN + INTEL_ECDSA_HEADER_LEN; + } else { + firmware_payload_offset = INTEL_RSA_HEADER_LEN; + } + + intel_boot_param = 0; + firmware_payload_started = 0; + vendor_firmware_complete_received = 0; + tlv_use_ecdsa = (uint8_t)(intel_version_tlv.sbe_type == 1 && hw_variant >= 0x17); + tlv_header_base = tlv_use_ecdsa ? INTEL_ECDSA_OFFSET : 0; + + if (tlv_use_ecdsa){ + state = STATE_TLV_SEND_ECDSA_PUBLIC_KEY; + if (intel_seek_and_send_fragment(0x00, tlv_header_base, 128) != 0){ + log_error("Failed to send Intel ECDSA init fragment"); + (*done)(1); + } + } else { + state = STATE_SEND_PUBLIC_KEY_1; + if (intel_seek_and_send_fragment(0x00, 0, 128) != 0){ + log_error("Failed to send Intel RSA init fragment"); + (*done)(1); + } + } break; } // legacy mode - intel_version = *(intel_version_t*) hci_event_command_complete_get_return_parameters(packet); + controller_mode = INTEL_CONTROLLER_LEGACY; + intel_version = *(intel_version_t *)return_params; dump_intel_version(&intel_version); // fw_variant = 0x06 bootloader mode / 0x23 operational mode @@ -347,6 +608,7 @@ static void state_machine(uint8_t *packet, uint16_t size) { if (intel_version.fw_variant != 0x06){ log_error("unknown fw_variant 0x%02x", intel_version.fw_variant); + (*done)(1); break; } @@ -354,6 +616,8 @@ static void state_machine(uint8_t *packet, uint16_t size) { state = STATE_HANDLE_READ_SECURE_BOOT_PARAMS; transport_send_cmd(&hci_intel_read_secure_boot_params); break; + } + case STATE_HANDLE_READ_SECURE_BOOT_PARAMS: intel_boot_params = *(intel_boot_params_t *) hci_event_command_complete_get_return_parameters(packet); dump_intel_boot_params(&intel_boot_params); @@ -385,60 +649,122 @@ static void state_machine(uint8_t *packet, uint16_t size) { intel_send_fragment(0x00, 128); break; case STATE_SEND_PUBLIC_KEY_1: - // send public key / part 1 - offset 128 state = STATE_SEND_PUBLIC_KEY_2; intel_send_fragment(0x03, 128); break; + case STATE_SEND_PUBLIC_KEY_2: - // send public key / part 2 - offset 384 state = STATE_SEND_SIGNATURE_PART_1; intel_send_fragment(0x03, 128); break; + case STATE_SEND_SIGNATURE_PART_1: - // skip 4 bytes + // RSA CSS places a four-byte gap between the public key and signature. res = fread(fw_buffer, 1, 4, fw_file); - log_info("read res %d", (int)res); + if (res != 4){ + log_error("Short read before Intel RSA signature"); + (*done)(1); + break; + } fw_offset += res; - - // send signature / part 1 - offset 388 state = STATE_SEND_SIGNATURE_PART_2; intel_send_fragment(0x02, 128); break; + case STATE_SEND_SIGNATURE_PART_2: - // send signature / part 2 - offset 516 state = STATE_SEND_FIRMWARE_CHUNK; intel_send_fragment(0x02, 128); break; + + case STATE_TLV_SEND_ECDSA_PUBLIC_KEY: + state = STATE_TLV_SEND_ECDSA_SIGNATURE; + intel_send_fragment(0x03, 96); + break; + + case STATE_TLV_SEND_ECDSA_SIGNATURE: + state = STATE_SEND_FIRMWARE_CHUNK; + intel_send_fragment(0x02, 96); + break; + case STATE_SEND_FIRMWARE_CHUNK: - // send firmware chunks - offset 644 - // chunk len must be 4 byte aligned - // multiple commands can be combined + if (controller_mode == INTEL_CONTROLLER_TLV && !firmware_payload_started){ + if (fseek(fw_file, (long)firmware_payload_offset, SEEK_SET) != 0){ + log_error("Failed to seek to Intel TLV firmware payload offset %u", (unsigned)firmware_payload_offset); + (*done)(1); + break; + } + fw_offset = firmware_payload_offset; + firmware_payload_started = 1; + printf("Intel firmware payload offset: %u\n", (unsigned)firmware_payload_offset); + } + + // Secure Send accepts at most 252 firmware bytes per transfer. Batch complete + // HCI commands and stop only on a four-byte aligned boundary. buffer_offset = 0; do { + size_t command_start = buffer_offset; + long file_command_start = ftell(fw_file); res = fread(&fw_buffer[buffer_offset], 1, 3, fw_file); - log_info("fw_offset %6" PRId32 ", buffer_offset %" PRId32 ", read %3u -> res %" PRId32 "", (int32_t)fw_offset, (int32_t)buffer_offset, 3, (int32_t)res); - fw_offset += res; - if (res == 0 ){ - // EOF + if (res == 0){ log_info("End of file"); fclose(fw_file); fw_file = NULL; state = STATE_HANDLE_FIRMWARE_CHUNKS_SENT; break; } - int param_len = fw_buffer[buffer_offset + 2]; + if (res != 3){ + log_error("Truncated Intel firmware HCI command header"); + (*done)(1); + break; + } + fw_offset += 3; + + uint16_t opcode = little_endian_read_16(&fw_buffer[command_start], 0); + uint8_t param_len = fw_buffer[command_start + 2]; + size_t command_len = (size_t)3 + param_len; + + if (command_start + command_len > 252){ + if (command_start != 0 && (command_start & 3u) == 0){ + if (fseek(fw_file, file_command_start, SEEK_SET) != 0){ + log_error("Failed to rewind Intel firmware command"); + (*done)(1); + break; + } + fw_offset -= 3; + break; + } + log_error("Intel firmware command cannot fit secure fragment (%u bytes)", (unsigned)command_len); + (*done)(1); + break; + } + buffer_offset += 3; if (param_len){ res = fread(&fw_buffer[buffer_offset], 1, param_len, fw_file); - fw_offset += res; + if (res != param_len){ + log_error("Truncated Intel firmware HCI command payload"); + (*done)(1); + break; + } + fw_offset += res; buffer_offset += res; } - } while ((buffer_offset & 3) != 0); + + if (opcode == 0xfc0e && param_len >= 4){ + intel_boot_param = little_endian_read_32(&fw_buffer[command_start + 3], 0); + printf("Intel boot address: 0x%08" PRIx32 "\n", intel_boot_param); + } + } while ((buffer_offset & 3u) != 0); if (buffer_offset == 0) break; + if ((buffer_offset & 3u) != 0){ + log_error("Intel secure firmware fragment is not four-byte aligned (%u)", (unsigned)buffer_offset); + (*done)(1); + break; + } waiting_for_command_complete = 1; - transport_send_intel_secure(0x01, fw_buffer, (uint8_t) buffer_offset); + transport_send_intel_secure(0x01, fw_buffer, (uint8_t)buffer_offset); break; case STATE_HANDLE_FIRMWARE_CHUNKS_SENT: @@ -448,9 +774,25 @@ static void state_machine(uint8_t *packet, uint16_t size) { printf("Firmware upload complete\n"); log_info("Vendor Event 0x06 - firmware complete"); - // Reset Params - constants from Windows Intel driver state = STATE_HANDLE_VENDOR_SPECIFIC_EVENT_02; - transport_send_cmd(&hci_intel_reset_param, 0x00, 0x00, 0x00, 0x01, 0x00, 0x08, 0x04, 0x00); + if (controller_mode == INTEL_CONTROLLER_TLV){ + if (intel_boot_param == 0){ + log_error("Intel TLV firmware did not provide a boot address"); + (*done)(1); + break; + } + // Modern Intel reset: reset_type=0, patch_enable=1, ddc_reload=0, + // boot_option=1, followed by the firmware boot address in little endian. + transport_send_cmd(&hci_intel_reset_param, + 0x00, 0x01, 0x00, 0x01, + (uint8_t)(intel_boot_param & 0xffu), + (uint8_t)((intel_boot_param >> 8) & 0xffu), + (uint8_t)((intel_boot_param >> 16) & 0xffu), + (uint8_t)((intel_boot_param >> 24) & 0xffu)); + } else { + // Legacy constants retained for 8260/8265-era controllers. + transport_send_cmd(&hci_intel_reset_param, 0x00, 0x00, 0x00, 0x01, 0x00, 0x08, 0x04, 0x00); + } break; case STATE_HANDLE_VENDOR_SPECIFIC_EVENT_02: @@ -461,45 +803,84 @@ static void state_machine(uint8_t *packet, uint16_t size) { printf("Firmware operational\n"); log_info("Vendor Event 0x02 - firmware operational"); - // Read Intel Version state = STATE_HANDLE_READ_VERSION_2; - transport_send_cmd(&hci_intel_read_version); + if (controller_mode == INTEL_CONTROLLER_TLV){ + transport_send_cmd(&hci_intel_read_version_tlv, 0xff); + } else { + transport_send_cmd(&hci_intel_read_version_legacy); + } break; - case STATE_HANDLE_READ_VERSION_2: - intel_version = *(intel_version_t*) hci_event_command_complete_get_return_parameters(packet); - dump_intel_version(&intel_version); + case STATE_HANDLE_READ_VERSION_2: { + if (controller_mode == INTEL_CONTROLLER_TLV){ + const uint8_t *return_params = hci_event_command_complete_get_return_parameters(packet); + uint16_t return_len = packet[1] >= 3 ? (uint16_t)(packet[1] - 3) : 0; + intel_version_tlv_t operational_version; + + if (intel_parse_version_tlv(return_params, return_len, &operational_version) == 0){ + intel_version_tlv = operational_version; + dump_intel_version_tlv(&intel_version_tlv); + } else { + log_info("Could not parse post-boot Intel TLV version; retaining bootloader identity"); + } - // ddc config - intel_get_firmware_name(&intel_version, &intel_boot_params, firmware_folder_path, - "ddc", fw_path, sizeof(fw_path)); - log_info("Open DDC %s", fw_path); + if (intel_get_firmware_name_tlv(&intel_version_tlv, firmware_folder_path, + "ddc", fw_path, sizeof(fw_path)) != 0){ + log_error("Could not construct Intel TLV DDC filename"); + (*done)(1); + break; + } + } else { + intel_version = *(intel_version_t *)hci_event_command_complete_get_return_parameters(packet); + dump_intel_version(&intel_version); + intel_get_firmware_name(&intel_version, &intel_boot_params, firmware_folder_path, + "ddc", fw_path, sizeof(fw_path)); + } - // open ddc file + log_info("Open DDC %s", fw_path); fw_offset = 0; fw_file = fopen(fw_path, "rb"); if (!fw_file){ + if (controller_mode == INTEL_CONTROLLER_TLV){ + log_info("Intel TLV DDC file not found; continuing without DDC: %s", fw_path); + state = STATE_DONE; + transport_send_cmd(&hci_intel_set_event_mask, 0x87, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00); + break; + } log_error("can't open file %s", fw_path); - (*done)(1); - return; + break; } - // load ddc state = STATE_SEND_DDC; - - /* fall through */ + res = intel_send_ddc(); + if (res == 0) break; + if (res == -2){ + log_error("Malformed Intel DDC file"); + (*done)(1); + break; + } + fclose(fw_file); + fw_file = NULL; + log_info("Load DDC Complete"); + state = STATE_DONE; + transport_send_cmd(&hci_intel_set_event_mask, 0x87, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00); + break; + } case STATE_SEND_DDC: res = intel_send_ddc(); if (res == 0) break; - - // DDC download complete + if (res == -2){ + log_error("Malformed Intel DDC file"); + (*done)(1); + break; + } + if (fw_file){ + fclose(fw_file); + fw_file = NULL; + } log_info("Load DDC Complete"); - - // TODO: check if we need to wait for HCI Command Complete, resp. add another state here - - // Set Intel event mask 0xfc52 state = STATE_DONE; transport_send_cmd(&hci_intel_set_event_mask, 0x87, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00); break; @@ -535,9 +916,13 @@ void btstack_chipset_intel_download_firmware(const hci_transport_t * hci_transpo done = callback; - transport = hci_transport;; + transport = hci_transport; transport->register_packet_handler(&transport_packet_handler); - transport->open(); + if (transport->open() != 0){ + log_error("Could not open Intel HCI transport"); + (*done)(1); + return; + } // get started state = STATE_INITIAL; diff --git a/platform/posix/btstack_audio_portaudio.c b/platform/posix/btstack_audio_portaudio.c index c41bbff8f6..1625601f35 100644 --- a/platform/posix/btstack_audio_portaudio.c +++ b/platform/posix/btstack_audio_portaudio.c @@ -39,6 +39,7 @@ #include +#include #include #include "btstack_debug.h" #include "btstack_audio.h" @@ -47,16 +48,20 @@ #ifdef HAVE_PORTAUDIO #define PA_SAMPLE_TYPE paInt16 -#define NUM_FRAMES_PER_PA_BUFFER 512 -#define NUM_OUTPUT_BUFFERS 5 +#define NUM_FRAMES_PER_PA_BUFFER 128 +#define MAX_OUTPUT_FRAMES_PER_PA_BUFFER 512 +#define NUM_OUTPUT_BUFFERS 4 #define NUM_INPUT_BUFFERS 5 -#define DRIVER_POLL_INTERVAL_MS 5 +#define DRIVER_POLL_INTERVAL_MS 1 #ifndef MAX_NR_AUDIO_CHANNELS #define MAX_NR_AUDIO_CHANNELS 2 #endif #include +#ifdef _WIN32 +#include +#endif // config static int num_channels_sink; @@ -86,10 +91,15 @@ static void (*playback_callback)(int16_t * buffer, uint16_t num_samples, const b static void (*recording_callback)(const int16_t * buffer, uint16_t num_samples, const btstack_audio_context_t * context); // output buffer -static int16_t output_buffer_storage[NUM_OUTPUT_BUFFERS * NUM_FRAMES_PER_PA_BUFFER * MAX_NR_AUDIO_CHANNELS]; +static int16_t output_buffer_storage[NUM_OUTPUT_BUFFERS * MAX_OUTPUT_FRAMES_PER_PA_BUFFER * MAX_NR_AUDIO_CHANNELS]; static int16_t * output_buffers[NUM_OUTPUT_BUFFERS]; +static int16_t output_discard_buffer[MAX_OUTPUT_FRAMES_PER_PA_BUFFER * MAX_NR_AUDIO_CHANNELS]; static int output_buffer_to_play; static int output_buffer_to_fill; +static uint16_t sink_frames_per_buffer = NUM_FRAMES_PER_PA_BUFFER; +/* Monotonic SPSC counters avoid the modulo-index full-lap ambiguity. */ +static volatile uint32_t output_buffers_consumed; +static uint32_t output_buffers_refilled; // input buffer static int16_t input_buffer_storage[NUM_INPUT_BUFFERS * NUM_FRAMES_PER_PA_BUFFER * MAX_NR_AUDIO_CHANNELS]; @@ -120,33 +130,37 @@ static int portaudio_callback_sink( const void * inputBuffer (void) frames_per_buffer; (void) inputBuffer; + uint32_t play_seq = output_buffers_consumed; + uint32_t play_index = play_seq % NUM_OUTPUT_BUFFERS; + // get microsecond timestamp btstack_time_us_t time_us = (uint32_t) (uint64_t) (timeInfo->outputBufferDacTime * 1000000); - sink_playback_audio_contexts[output_buffer_to_play].timestamp = time_us; + sink_playback_audio_contexts[play_index].timestamp = time_us; // simplified volume control uint16_t index; - int16_t * from_buffer = output_buffers[output_buffer_to_play]; + int16_t * from_buffer = output_buffers[play_index]; int16_t * to_buffer = (int16_t *) outputBuffer; - btstack_assert(frames_per_buffer == NUM_FRAMES_PER_PA_BUFFER); + btstack_assert(frames_per_buffer == sink_frames_per_buffer); #if 0 // up to 8 right shifts int right_shift = 8 - btstack_min(8, ((sink_volume + 15) / 16)); - for (index = 0; index < (NUM_FRAMES_PER_PA_BUFFER * num_channels_sink); index++){ + for (index = 0; index < (sink_frames_per_buffer * num_channels_sink); index++){ *to_buffer++ = (*from_buffer++) >> right_shift; } #else // multiply with volume ^ 4 int16_t x2 = sink_volume * sink_volume; int16_t x4 = (x2 * x2) >> 14; - for (index = 0; index < (NUM_FRAMES_PER_PA_BUFFER * num_channels_sink); index++){ + for (index = 0; index < (sink_frames_per_buffer * num_channels_sink); index++){ *to_buffer++ = ((*from_buffer++) * x4) >> 14; } #endif - // next - output_buffer_to_play = (output_buffer_to_play + 1 ) % NUM_OUTPUT_BUFFERS; + // Publish one consumed staging buffer to the BTstack run-loop thread. + output_buffers_consumed = play_seq + 1; + output_buffer_to_play = (int)((play_seq + 1) % NUM_OUTPUT_BUFFERS); return 0; } @@ -179,17 +193,29 @@ static int portaudio_callback_source( const void * inputBuff } static void driver_timer_handler_sink(btstack_timer_source_t * ts){ + uint32_t consumed = output_buffers_consumed; + uint32_t lag = consumed - output_buffers_refilled; + + /* Keep latency bounded. If the run loop missed more playback periods than + * the staging ring can represent, consume/drop the stale PCM now instead + * of leaving it queued and letting latency grow without bound. */ + if (lag > NUM_OUTPUT_BUFFERS) { + uint32_t stale = lag - NUM_OUTPUT_BUFFERS; + while (stale--) { + (*playback_callback)(output_discard_buffer, sink_frames_per_buffer, NULL); + output_buffers_refilled++; + } + } - // playback buffer ready to fill - while (output_buffer_to_play != output_buffer_to_fill){ - (*playback_callback)(output_buffers[output_buffer_to_fill], NUM_FRAMES_PER_PA_BUFFER, - &sink_playback_audio_contexts[output_buffer_to_fill]); - - // next - output_buffer_to_fill = (output_buffer_to_fill + 1 ) % NUM_OUTPUT_BUFFERS; + while (output_buffers_refilled != consumed) { + uint32_t fill_index = output_buffers_refilled % NUM_OUTPUT_BUFFERS; + (*playback_callback)(output_buffers[fill_index], sink_frames_per_buffer, + &sink_playback_audio_contexts[fill_index]); + output_buffers_refilled++; + output_buffer_to_fill = (int)(output_buffers_refilled % NUM_OUTPUT_BUFFERS); + consumed = output_buffers_consumed; } - // re-set timer btstack_run_loop_set_timer(ts, DRIVER_POLL_INTERVAL_MS); btstack_run_loop_add_timer(ts); } @@ -224,8 +250,12 @@ static int btstack_audio_portaudio_sink_init( num_channels_sink = channels; num_bytes_per_sample_sink = 2 * channels; + sink_frames_per_buffer = NUM_FRAMES_PER_PA_BUFFER; + if (samplerate >= 192000) sink_frames_per_buffer = 512; + else if (samplerate >= 88200) sink_frames_per_buffer = 256; + for (int i=0;i= 0) { + const PaHostApiInfo *wasapi_info = Pa_GetHostApiInfo(wasapi_index); + if (wasapi_info != NULL && wasapi_info->defaultOutputDevice != paNoDevice) { + device_index = wasapi_info->defaultOutputDevice; + } + } + if (device_index < 0) device_index = Pa_GetDefaultOutputDevice(); + output_device_info = Pa_GetDeviceInfo(device_index); } + const PaHostApiInfo *selected_host = Pa_GetHostApiInfo(output_device_info->hostApi); + /* -- setup output -- */ PaStreamParameters output_parameters; output_parameters.device = device_index; output_parameters.channelCount = channels; output_parameters.sampleFormat = PA_SAMPLE_TYPE; - output_parameters.suggestedLatency = output_device_info->defaultHighOutputLatency; + output_parameters.suggestedLatency = output_device_info->defaultLowOutputLatency; output_parameters.hostApiSpecificStreamInfo = NULL; log_info("PortAudio: sink device: %s", output_device_info->name); @@ -275,10 +314,25 @@ static int btstack_audio_portaudio_sink_init( NULL, &output_parameters, samplerate, - NUM_FRAMES_PER_PA_BUFFER, - paClipOff, /* we won't output out of range samples so don't bother clipping them */ - portaudio_callback_sink, /* use callback */ - NULL ); + sink_frames_per_buffer, + paClipOff, + portaudio_callback_sink, + NULL ); +#ifdef _WIN32 + if (err == paInvalidSampleRate && selected_host != NULL && selected_host->type == paWASAPI) { + PaWasapiStreamInfo wasapi_info; + memset(&wasapi_info, 0, sizeof(wasapi_info)); + wasapi_info.size = sizeof(wasapi_info); + wasapi_info.hostApiType = paWASAPI; + wasapi_info.version = 1; + wasapi_info.flags = paWinWasapiAutoConvert; + output_parameters.hostApiSpecificStreamInfo = &wasapi_info; + log_info("PortAudio: shared WASAPI rejected %u Hz, retrying with auto-convert", (unsigned)samplerate); + err = Pa_OpenStream(&stream_sink, NULL, &output_parameters, samplerate, + sink_frames_per_buffer, paClipOff, + portaudio_callback_sink, NULL); + } +#endif if (err != paNoError){ log_error("Portudio: error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); @@ -287,11 +341,10 @@ static int btstack_audio_portaudio_sink_init( log_info("PortAudio: sink stream created"); const PaStreamInfo * stream_info = Pa_GetStreamInfo(stream_sink); - // verify latency uint32_t latency_samples = (uint32_t) (stream_info->outputLatency * samplerate); - uint32_t buffer_samples = NUM_FRAMES_PER_PA_BUFFER * NUM_OUTPUT_BUFFERS; - int buffers_needed = (latency_samples + NUM_FRAMES_PER_PA_BUFFER - 1) / NUM_FRAMES_PER_PA_BUFFER; + uint32_t buffer_samples = sink_frames_per_buffer * NUM_OUTPUT_BUFFERS; + int buffers_needed = (latency_samples + sink_frames_per_buffer - 1) / sink_frames_per_buffer; log_info("PortAudio: output latency of %f requires %u buffers, %u buffers available\n", stream_info->outputLatency, buffers_needed, NUM_OUTPUT_BUFFERS); UNUSED(buffers_needed); @@ -418,13 +471,15 @@ static void btstack_audio_portaudio_sink_start_stream(void){ if (!playback_callback) return; - // fill buffers once + // Pre-fill the entire staging ring before PortAudio starts consuming it. uint8_t i; - for (i=0;i/firmware" + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "${CMAKE_CURRENT_SOURCE_DIR}/firmware/ibt-1040-0041.sfi" + "$/firmware/ibt-1040-0041.sfi" + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "${CMAKE_CURRENT_SOURCE_DIR}/firmware/ibt-1040-0041.ddc" + "$/firmware/ibt-1040-0041.ddc") +endif() diff --git a/port/windows-winusb-intel/main.c b/port/windows-winusb-intel/main.c index 811c0f3a6f..eee1691b33 100644 --- a/port/windows-winusb-intel/main.c +++ b/port/windows-winusb-intel/main.c @@ -186,7 +186,12 @@ int main(int argc, const char * argv[]){ // setup USB Transport transport = hci_transport_usb_instance(); - btstack_chipset_intel_download_firmware(hci_transport_usb_instance(), &intel_firmware_done); + // Intel AX211 Bluetooth controller used by this PC. The generic WinUSB + // transport does not include modern Intel IDs in its built-in allowlist. + hci_transport_usb_add_device(0x8087, 0x0033); + btstack_chipset_intel_set_firmware_path("firmware"); + printf("Intel firmware directory: firmware\n"); + btstack_chipset_intel_download_firmware(transport, &intel_firmware_done); // go btstack_run_loop_execute(); diff --git a/port/windows-winusb/CMakeLists.txt b/port/windows-winusb/CMakeLists.txt index 8a3c875f06..22f1f04224 100644 --- a/port/windows-winusb/CMakeLists.txt +++ b/port/windows-winusb/CMakeLists.txt @@ -78,21 +78,41 @@ list(SORT SOURCES) # create static lib add_library(btstack STATIC ${SOURCES}) -# pkgconfig -find_package(PkgConfig QUIET) +# LHDC V5 portable codec libraries +file(GLOB SOURCES_LHDCV5_DECODER "../../3rd-party/lhdcv5-decoder/decoder/*.c") +add_library(lhdcv5_decoder STATIC ${SOURCES_LHDCV5_DECODER}) +target_include_directories(lhdcv5_decoder PUBLIC "${BTSTACK_ROOT}/3rd-party/lhdcv5-decoder/decoder") +target_compile_definitions(lhdcv5_decoder PRIVATE LHDC_HOST_BUILD) +if(MSVC) + target_compile_options(lhdcv5_decoder PRIVATE /FI"${CMAKE_CURRENT_SOURCE_DIR}/lhdcv5_msvc_compat.h") +endif() + +file(GLOB SOURCES_LHDCV5_ENCODER "../../3rd-party/lhdcv5-encoder/src/*.c") +add_library(lhdcv5_encoder STATIC ${SOURCES_LHDCV5_ENCODER}) +target_include_directories(lhdcv5_encoder PUBLIC "${BTSTACK_ROOT}/3rd-party/lhdcv5-encoder/include" PRIVATE "${BTSTACK_ROOT}/3rd-party/lhdcv5-encoder/src") -# portaudio +# PortAudio: use system pkg-config when available, otherwise build the vendored copy. +find_package(PkgConfig QUIET) if (PkgConfig_FOUND) pkg_check_modules(PORTAUDIO portaudio-2.0) - if(PORTAUDIO_FOUND) - message("HAVE_PORTAUDIO") - target_include_directories(btstack PUBLIC ${PORTAUDIO_INCLUDE_DIRS}) - target_link_directories(btstack PUBLIC ${PORTAUDIO_LIBRARY_DIRS}) - target_link_libraries(btstack ${PORTAUDIO_LIBRARIES}) - add_compile_definitions(HAVE_PORTAUDIO) - endif() endif() +if(PORTAUDIO_FOUND) + message("HAVE_PORTAUDIO (pkg-config)") + target_include_directories(btstack PUBLIC ${PORTAUDIO_INCLUDE_DIRS}) + target_link_directories(btstack PUBLIC ${PORTAUDIO_LIBRARY_DIRS}) + target_link_libraries(btstack ${PORTAUDIO_LIBRARIES}) +else() + message("HAVE_PORTAUDIO (vendored)") + set(PA_BUILD_SHARED_LIBS OFF CACHE BOOL "" FORCE) + set(PA_BUILD_EXAMPLES OFF CACHE BOOL "" FORCE) + set(PA_BUILD_TESTS OFF CACHE BOOL "" FORCE) + add_subdirectory("${BTSTACK_ROOT}/3rd-party/portaudio" "${CMAKE_CURRENT_BINARY_DIR}/portaudio") + target_include_directories(btstack PUBLIC "${BTSTACK_ROOT}/3rd-party/portaudio/include") + target_link_libraries(btstack portaudio) +endif() +add_compile_definitions(HAVE_PORTAUDIO) + # get list of examples, skipping mesh_node_demo include(../../example/CMakeLists.txt) set (EXAMPLES ${EXAMPLES_GENERAL} ${EXAMPLES_CLASSIC_ONLY} ${EXAMPLES_LE_ONLY} ${EXAMPLES_DUAL_MODE}) @@ -120,3 +140,11 @@ foreach(EXAMPLE ${EXAMPLES}) add_executable(${EXAMPLE} ${SOURCES_EXAMPLE}) target_link_libraries(${EXAMPLE} btstack setupapi winusb) endforeach(EXAMPLE) + +# Windows A2DP Sink with LHDC V5 software decoder +add_executable(a2dp_sink_lhdcv5_demo a2dp_sink_lhdcv5_demo.c) +target_link_libraries(a2dp_sink_lhdcv5_demo btstack lhdcv5_decoder setupapi winusb) + +# Offline LHDC V5 capture replay / decoder validation +add_executable(lhdcv5_replay lhdcv5_replay.c) +target_link_libraries(lhdcv5_replay lhdcv5_decoder) diff --git a/port/windows-winusb/a2dp_sink_lhdcv5_demo.c b/port/windows-winusb/a2dp_sink_lhdcv5_demo.c new file mode 100644 index 0000000000..da79355452 --- /dev/null +++ b/port/windows-winusb/a2dp_sink_lhdcv5_demo.c @@ -0,0 +1,737 @@ +/* + * BTstack Windows WinUSB A2DP Sink with LHDC V5 software decoding. + * + * LHDC V5 codec implementation: + * https://github.com/WillyBilly06/LHDC-V5-Decoder + * + * This example keeps SBC as the mandatory A2DP fallback and adds an LHDC V5 + * vendor-specific sink endpoint. Decoded audio is converted to stereo S16 PCM + * for BTstack's audio sink (PortAudio on the Windows WinUSB port). + */ + +#define BTSTACK_FILE__ "a2dp_sink_lhdcv5_demo.c" + +#include +#include +#include +#include +#include + +#ifdef _WIN32 +#include +#endif + +#include "btstack.h" +#include "btstack_ring_buffer.h" +#include "lhdc_dec.h" + +/* Host diagnostics required by the decoder test hooks. */ +int g_nzc_formula = 0; +int g_pkframe = 0; + +#define NUM_CHANNELS 2 +#define PCM_BYTES_PER_FRAME 4 + +#define LHDCV5_VENDOR_ID 0x0000053au +#define LHDCV5_CODEC_ID 0x4c35u + +#define LHDCV5_SR_192000 0x01 +#define LHDCV5_SR_96000 0x04 +#define LHDCV5_SR_48000 0x10 +#define LHDCV5_SR_44100 0x20 +#define LHDCV5_SR_MASK 0x35 + +#define LHDCV5_DEPTH_24 0x02 +#define LHDCV5_DEPTH_16 0x04 +#define LHDCV5_DEPTH_MASK 0x07 + +#define LHDCV5_VERSION_1 0x01 +#define LHDCV5_FRAME_5MS 0x10 +#define LHDCV5_FEATURE_LL 0x40 + +#define APTX_VENDOR_ID 0x0000004fu +#define APTX_CODEC_ID 0x0001u +#define APTX_HD_VENDOR_ID 0x000000d7u +#define APTX_HD_CODEC_ID 0x0024u +#define APTX_SR_48000 0x10 +#define APTX_SR_44100 0x20 +#define APTX_CHANNEL_STEREO 0x02 +#define APTX_PCM_BUFFER_SIZE 16384 +#define APTX_PCM_LATENCY_TARGET_MS 40 +#define APTX_PCM_LATENCY_HIGH_MS 48 + +#define LHDCV5_FRAGMENT_BUFFER_SIZE 8192 +#define LHDCV5_PCM_SAMPLES_PER_CH_MAX 1920 +#define PCM_RING_BUFFER_SIZE (256 * 1024) + +static btstack_packet_callback_registration_t hci_event_callback_registration; +static uint8_t sdp_a2dp_sink_service_buffer[150]; +static uint8_t device_id_sdp_service_buffer[100]; + +static uint8_t media_sbc_codec_capabilities[] = { 0xff, 0xff, 2, 53 }; +static uint8_t media_sbc_codec_configuration[4]; + +/* + * BTstack wants only Media Codec Information here, not LOSC/media type/codec type. + * vendor id LE (4), codec id LE (2), rate, depth/bitrate, version/frame, + * features, lossless-raw features. + */ +static const uint8_t media_lhdcv5_codec_capabilities[] = { + 0x3a, 0x05, 0x00, 0x00, + 0x35, 0x4c, + LHDCV5_SR_MASK, + LHDCV5_DEPTH_24 | LHDCV5_DEPTH_16, + LHDCV5_VERSION_1 | LHDCV5_FRAME_5MS, + LHDCV5_FEATURE_LL, + 0x00 +}; +static uint8_t media_lhdcv5_codec_configuration[sizeof(media_lhdcv5_codec_capabilities)]; + +static const uint8_t media_aptx_codec_capabilities[] = { + 0x4f, 0x00, 0x00, 0x00, 0x01, 0x00, + APTX_SR_44100 | APTX_SR_48000 | APTX_CHANNEL_STEREO +}; +static uint8_t media_aptx_codec_configuration[sizeof(media_aptx_codec_capabilities)]; + +static const uint8_t media_aptx_hd_codec_capabilities[] = { + 0xd7, 0x00, 0x00, 0x00, 0x24, 0x00, + APTX_SR_44100 | APTX_SR_48000 | APTX_CHANNEL_STEREO, + 0x00, 0x00, 0x00, 0x00 +}; +static uint8_t media_aptx_hd_codec_configuration[sizeof(media_aptx_hd_codec_capabilities)]; + +static uint8_t sbc_local_seid; +static uint8_t aptx_local_seid; +static uint8_t aptx_hd_local_seid; +static uint8_t lhdcv5_local_seid; +static uint32_t negotiated_sample_rate = 48000; +static uint8_t negotiated_bit_depth = 16; + +static const btstack_sbc_decoder_t *sbc_decoder; +static btstack_sbc_decoder_bluedroid_t sbc_decoder_context; + +static void *lhdcv5_workspace; +static size_t lhdcv5_workspace_size; +static lhdc_decoder_t *lhdcv5_decoder; +static uint8_t lhdcv5_fragment[LHDCV5_FRAGMENT_BUFFER_SIZE]; +static size_t lhdcv5_fragment_size; + +struct aptx_context; +typedef struct aptx_context *(__cdecl *aptx_init_fn)(int hd); +typedef void (__cdecl *aptx_reset_fn)(struct aptx_context *ctx); +typedef void (__cdecl *aptx_finish_fn)(struct aptx_context *ctx); +typedef size_t (__cdecl *aptx_decode_sync_fn)( + struct aptx_context *ctx, const unsigned char *input, size_t input_size, + unsigned char *output, size_t output_size, size_t *written, + int *synced, size_t *dropped); + +static HMODULE aptx_module; +static aptx_init_fn aptx_init_ptr; +static aptx_reset_fn aptx_reset_ptr; +static aptx_finish_fn aptx_finish_ptr; +static aptx_decode_sync_fn aptx_decode_sync_ptr; +static struct aptx_context *aptx_decoder; +static int aptx_decoder_is_hd; + +static btstack_ring_buffer_t pcm_ring_buffer; +static uint8_t pcm_ring_storage[PCM_RING_BUFFER_SIZE]; +static int audio_initialized; +static int audio_started; + +static int is_lhdcv5_codec_info(const uint8_t *codec_info, uint16_t len) { + if (codec_info == NULL || len < 11) return 0; + return little_endian_read_32(codec_info, 0) == LHDCV5_VENDOR_ID && + little_endian_read_16(codec_info, 4) == LHDCV5_CODEC_ID; +} + +static uint32_t lhdcv5_rate_from_codec_info(const uint8_t *codec_info) { + uint8_t rate = codec_info[6] & LHDCV5_SR_MASK; + if (rate & LHDCV5_SR_192000) return 192000; + if (rate & LHDCV5_SR_96000) return 96000; + if (rate & LHDCV5_SR_48000) return 48000; + if (rate & LHDCV5_SR_44100) return 44100; + return 0; +} + +static uint8_t lhdcv5_depth_from_codec_info(const uint8_t *codec_info) { + uint8_t depth = codec_info[7] & LHDCV5_DEPTH_MASK; + if (depth & LHDCV5_DEPTH_24) return 24; + if (depth & LHDCV5_DEPTH_16) return 16; + return 0; +} + +static int is_aptx_codec_info(const uint8_t *codec_info, uint16_t len, int hd) { + if (codec_info == NULL) return 0; + if ((!hd && len < 7) || (hd && len < 11)) return 0; + uint32_t vendor = little_endian_read_32(codec_info, 0); + uint16_t codec = little_endian_read_16(codec_info, 4); + return hd ? (vendor == APTX_HD_VENDOR_ID && codec == APTX_HD_CODEC_ID) + : (vendor == APTX_VENDOR_ID && codec == APTX_CODEC_ID); +} + +static uint32_t aptx_rate_from_codec_info(const uint8_t *codec_info, uint16_t len) { + if (codec_info == NULL || len < 7) return 0; + uint8_t rate = codec_info[6] & 0xf0; + if (rate & APTX_SR_48000) return 48000; + if (rate & APTX_SR_44100) return 44100; + return 0; +} + +static void pcm_queue_reset(void) { + btstack_ring_buffer_init(&pcm_ring_buffer, pcm_ring_storage, sizeof(pcm_ring_storage)); +} + +static void pcm_queue_write(const int16_t *samples, uint32_t frames) { + uint32_t bytes = frames * PCM_BYTES_PER_FRAME; + if (btstack_ring_buffer_bytes_free(&pcm_ring_buffer) < bytes) return; + (void)btstack_ring_buffer_write(&pcm_ring_buffer, (uint8_t *)samples, bytes); +} + +static void playback_handler(int16_t *buffer, uint16_t num_audio_frames, + const btstack_audio_context_t *context) { + UNUSED(context); + uint32_t requested = (uint32_t)num_audio_frames * PCM_BYTES_PER_FRAME; + if (aptx_decoder != NULL && negotiated_sample_rate != 0) { + static uint8_t trim_scratch[2048]; + uint32_t queued = btstack_ring_buffer_bytes_available(&pcm_ring_buffer); + uint32_t target = (negotiated_sample_rate * PCM_BYTES_PER_FRAME * APTX_PCM_LATENCY_TARGET_MS) / 1000; + uint32_t high = (negotiated_sample_rate * PCM_BYTES_PER_FRAME * APTX_PCM_LATENCY_HIGH_MS) / 1000; + target -= target % PCM_BYTES_PER_FRAME; + high -= high % PCM_BYTES_PER_FRAME; + if (queued > high) { + uint32_t trim = queued - target; + trim -= trim % PCM_BYTES_PER_FRAME; + while (trim != 0) { + uint32_t chunk = trim < sizeof(trim_scratch) ? trim : (uint32_t)sizeof(trim_scratch); + chunk -= chunk % PCM_BYTES_PER_FRAME; + uint32_t discarded = 0; + btstack_ring_buffer_read(&pcm_ring_buffer, trim_scratch, chunk, &discarded); + if (discarded == 0) break; + trim -= discarded; + } + } + } + uint32_t read = 0; + btstack_ring_buffer_read(&pcm_ring_buffer, (uint8_t *)buffer, requested, &read); + if (read < requested) { + memset(((uint8_t *)buffer) + read, 0, requested - read); + } +} + +static void audio_close(void) { + const btstack_audio_sink_t *audio = btstack_audio_sink_get_instance(); + if (audio_initialized && audio != NULL) { + if (audio_started) audio->stop_stream(); + audio->close(); + } + audio_initialized = 0; + audio_started = 0; + pcm_queue_reset(); +} + +static void audio_open(uint32_t sample_rate) { + audio_close(); + const btstack_audio_sink_t *audio = btstack_audio_sink_get_instance(); + if (audio == NULL) { + printf("Audio output unavailable. Build/install PortAudio for live playback.\n"); + return; + } + int err = audio->init(NUM_CHANNELS, sample_rate, &playback_handler); + if (err != 0) { + printf("Audio output init FAILED for %" PRIu32 " Hz: %d\n", sample_rate, err); + return; + } + audio_initialized = 1; + uint32_t actual_rate = audio->get_samplerate ? audio->get_samplerate() : sample_rate; + printf("Audio output opened: requested=%" PRIu32 " Hz actual=%" PRIu32 " Hz\n", + sample_rate, actual_rate); + audio->start_stream(); + audio_started = 1; + printf("Audio output started: %" PRIu32 " Hz, stereo S16\n", actual_rate); +} + +static void handle_sbc_pcm(int16_t *data, int num_audio_frames, int num_channels, + int sample_rate, void *context) { + UNUSED(context); + UNUSED(sample_rate); + if (num_channels != NUM_CHANNELS) return; + pcm_queue_write(data, (uint32_t)num_audio_frames); +} + +static void aptx_decoder_close(void) { + if (aptx_decoder != NULL && aptx_finish_ptr != NULL) aptx_finish_ptr(aptx_decoder); + aptx_decoder = NULL; + aptx_decoder_is_hd = 0; +} + +static int aptx_load_library(void) { + if (aptx_module != NULL) return 1; + aptx_module = LoadLibraryA("openaptx0.dll"); + if (aptx_module == NULL) { + printf("aptX: openaptx0.dll not found (error %lu)\n", (unsigned long)GetLastError()); + return 0; + } + aptx_init_ptr = (aptx_init_fn)GetProcAddress(aptx_module, "aptx_init"); + aptx_reset_ptr = (aptx_reset_fn)GetProcAddress(aptx_module, "aptx_reset"); + aptx_finish_ptr = (aptx_finish_fn)GetProcAddress(aptx_module, "aptx_finish"); + aptx_decode_sync_ptr = (aptx_decode_sync_fn)GetProcAddress(aptx_module, "aptx_decode_sync"); + if (!aptx_init_ptr || !aptx_reset_ptr || !aptx_finish_ptr || !aptx_decode_sync_ptr) { + printf("aptX: decoder DLL missing required exports\n"); + FreeLibrary(aptx_module); aptx_module = NULL; return 0; + } + printf("aptX decoder backend loaded: openaptx0.dll\n"); + return 1; +} + +static int aptx_decoder_open(int hd) { + aptx_decoder_close(); + if (!aptx_load_library()) return 0; + aptx_decoder = aptx_init_ptr(hd ? 1 : 0); + if (aptx_decoder == NULL) return 0; + aptx_decoder_is_hd = hd ? 1 : 0; + printf("aptX%s decoder configured: %" PRIu32 " Hz, stereo\n", + hd ? " HD" : "", negotiated_sample_rate); + return 1; +} + +static int32_t aptx_read_s24le(const uint8_t *q) { + int32_t v = (int32_t)q[0] | ((int32_t)q[1] << 8) | ((int32_t)q[2] << 16); + if (v & 0x00800000) v |= (int32_t)0xff000000; + return v; +} + +static void aptx_decode_payload(const uint8_t *payload, size_t payload_size) { + static uint8_t pcm24[48]; + static int16_t s16[APTX_PCM_BUFFER_SIZE / 3]; + if (!aptx_decoder || !payload || payload_size == 0) return; + + const size_t block_size = aptx_decoder_is_hd ? 6 : 4; + size_t offset = 0; + size_t total_samples = 0; + size_t total_dropped = 0; + int final_synced = 0; + + while (payload_size - offset >= block_size) { + size_t written = 0, dropped = 0; + int synced = 0; + size_t processed = aptx_decode_sync_ptr(aptx_decoder, + payload + offset, block_size, pcm24, sizeof(pcm24), + &written, &synced, &dropped); + if (processed != block_size) break; + offset += processed; + total_dropped += dropped; + final_synced = synced; + + size_t samples = written / 3; + if (total_samples + samples > sizeof(s16) / sizeof(s16[0])) break; + for (size_t i = 0; i < samples; i++) + s16[total_samples + i] = (int16_t)(aptx_read_s24le(&pcm24[i * 3]) >> 8); + total_samples += samples; + } + + if (offset != payload_size || total_dropped != 0) { + static uint32_t reports; + if ((reports++ % 100) == 0) + printf("aptX%s sync: processed=%zu/%zu samples=%zu synced=%d dropped=%zu\n", + aptx_decoder_is_hd ? " HD" : "", offset, payload_size, + total_samples, final_synced, total_dropped); + } + + pcm_queue_write(s16, (uint32_t)(total_samples / NUM_CHANNELS)); +} + +static void lhdcv5_decoder_close(void) { + lhdcv5_decoder = NULL; + free(lhdcv5_workspace); + lhdcv5_workspace = NULL; + lhdcv5_workspace_size = 0; + lhdcv5_fragment_size = 0; +} + +static int lhdcv5_decoder_open(uint32_t sample_rate, uint8_t bit_depth) { + lhdcv5_decoder_close(); + + lhdc_dec_config_t config; + memset(&config, 0, sizeof(config)); + config.sample_rate = (lhdc_dec_sample_rate_t)sample_rate; + config.bit_depth = bit_depth == 24 ? LHDC_DEC_BITDEPTH_24 : LHDC_DEC_BITDEPTH_16; + config.frame_duration = LHDC_DEC_FRAME_5MS; + config.channels = NUM_CHANNELS; + config.max_frame_bytes = LHDCV5_FRAGMENT_BUFFER_SIZE; + config.lossless_enable = 0; + + lhdcv5_workspace_size = lhdc_dec_get_workspace_size(sample_rate, 5); + lhdcv5_workspace = calloc(1, lhdcv5_workspace_size); + if (lhdcv5_workspace == NULL) { + printf("LHDC V5: cannot allocate %zu-byte decoder workspace\n", lhdcv5_workspace_size); + return 0; + } + + lhdcv5_decoder = lhdc_dec_init(lhdcv5_workspace, &config); + if (lhdcv5_decoder == NULL) { + printf("LHDC V5: decoder initialization failed\n"); + lhdcv5_decoder_close(); + return 0; + } + + printf("LHDC V5 decoder configured: %" PRIu32 " Hz, %u-bit, 5 ms\n", + sample_rate, bit_depth); + return 1; +} + +static void lhdcv5_emit_pcm(const void *pcm, uint32_t frames, uint8_t bit_depth) { + static int16_t s16[LHDCV5_PCM_SAMPLES_PER_CH_MAX * NUM_CHANNELS]; + uint32_t total_samples = frames * NUM_CHANNELS; + if (total_samples > (uint32_t)(sizeof(s16) / sizeof(s16[0]))) return; + + if (bit_depth == 16) { + memcpy(s16, pcm, total_samples * sizeof(int16_t)); + } else { + const int32_t *s32 = (const int32_t *)pcm; + for (uint32_t i = 0; i < total_samples; i++) { + /* Decoder emits 24 valid bits left-justified in a 32-bit slot. */ + s16[i] = (int16_t)(s32[i] >> 16); + } + } + pcm_queue_write(s16, frames); +} + +static void lhdcv5_decode_payload(const uint8_t *payload, size_t payload_size) { + if (lhdcv5_decoder == NULL || payload == NULL || payload_size < 3) return; + + /* + * Android/onyx prefixes each LHDC transport subpacket with two bytes. + * At 48/96 kHz there is one prefix before the frame stream. At 192 kHz + * the 20 ms A2DP payload contains two 10 ms subpackets: + * [tag, seq] + 2 frames + [tag, seq+1] + 2 frames + * Skip the verified second prefix after frame 2; do not concatenate data + * across A2DP packet boundaries. + */ + const uint8_t prefix_tag = payload[0]; + const uint8_t prefix_seq = payload[1]; + size_t offset = 2; + uint32_t frame_index = 0; + + while (offset + 2 <= payload_size) { + if (negotiated_sample_rate == 192000 && frame_index != 0 && + (frame_index % 2u) == 0 && offset + 2 <= payload_size && + payload[offset] == prefix_tag && + payload[offset + 1] == (uint8_t)(prefix_seq + frame_index / 2u)) { + offset += 2; + if (offset + 2 > payload_size) break; + } + + uint8_t pcm[LHDCV5_PCM_SAMPLES_PER_CH_MAX * NUM_CHANNELS * sizeof(int32_t)]; + size_t consumed = 0; + uint32_t generated = 0; + lhdc_dec_frame_info_t info; + + lhdc_dec_ret_t ret = lhdc_dec_decode_frame( + lhdcv5_decoder, payload + offset, payload_size - offset, + pcm, LHDCV5_PCM_SAMPLES_PER_CH_MAX, + &consumed, &generated, &info); + + if (ret != LHDC_DEC_OK || consumed == 0) { + static uint32_t decode_errors; + if ((decode_errors++ % 100) == 0) + printf("LHDC V5 decode error %d at %zu/%zu\n", (int)ret, offset, payload_size); + break; + } + lhdcv5_emit_pcm(pcm, generated, info.bit_depth); + offset += consumed; + frame_index++; + } +} + +static int read_rtp_header(const uint8_t *packet, uint16_t size, uint16_t *offset) { + if (size < 12) return 0; + uint8_t csrc_count = packet[0] & 0x0f; + uint16_t header_len = (uint16_t)(12 + 4 * csrc_count); + if (size < header_len) return 0; + *offset = header_len; + return 1; +} + +static void handle_lhdcv5_media(uint8_t *packet, uint16_t size) { + uint16_t pos; + if (!read_rtp_header(packet, size, &pos) || size <= pos) return; + + /* bit7 fragmented, bit6 first, bit5 last, low nibble fragment/frame count */ + uint8_t header = packet[pos++]; + int fragmented = (header & 0x80) != 0; + int first = (header & 0x40) != 0; + int last = (header & 0x20) != 0; + + const uint8_t *payload = packet + pos; + size_t payload_size = size - pos; + + if (!fragmented) { + lhdcv5_fragment_size = 0; + lhdcv5_decode_payload(payload, payload_size); + return; + } + + if (first) lhdcv5_fragment_size = 0; + if (payload_size > sizeof(lhdcv5_fragment) - lhdcv5_fragment_size) { + printf("LHDC V5 fragment overflow, dropping packet\n"); + lhdcv5_fragment_size = 0; + return; + } + + memcpy(lhdcv5_fragment + lhdcv5_fragment_size, payload, payload_size); + lhdcv5_fragment_size += payload_size; + + if (last) { + lhdcv5_decode_payload(lhdcv5_fragment, lhdcv5_fragment_size); + lhdcv5_fragment_size = 0; + } +} + +static void handle_sbc_media(uint8_t *packet, uint16_t size) { + uint16_t pos; + if (!read_rtp_header(packet, size, &pos) || size <= pos) return; + pos++; /* SBC payload header */ + if (size <= pos) return; + sbc_decoder->decode_signed_16(&sbc_decoder_context, 0, packet + pos, size - pos); +} + +static void handle_aptx_media(uint8_t *packet, uint16_t size, int hd) { + uint16_t pos = 0; + /* Android aptX Classic omits RTP when SCMS-T is not enabled; aptX HD uses RTP. */ + if (hd && (!read_rtp_header(packet, size, &pos) || size <= pos)) return; + aptx_decode_payload(packet + pos, size - pos); +} + +static void handle_media_data(uint8_t local_seid, uint8_t *packet, uint16_t size) { + if (local_seid == lhdcv5_local_seid) handle_lhdcv5_media(packet, size); + else if (local_seid == aptx_hd_local_seid) handle_aptx_media(packet, size, 1); + else if (local_seid == aptx_local_seid) handle_aptx_media(packet, size, 0); + else if (local_seid == sbc_local_seid && sbc_decoder != NULL) handle_sbc_media(packet, size); +} + +static uint8_t media_config_validator(const avdtp_stream_endpoint_t *stream_endpoint, + const uint8_t *event, uint16_t size) { + UNUSED(size); + uint8_t local_seid = avdtp_local_seid(stream_endpoint); + if (local_seid == sbc_local_seid) return ERROR_CODE_SUCCESS; + + const uint8_t *info = + a2dp_subevent_signaling_media_codec_other_configuration_get_media_codec_information(event); + uint16_t len = + a2dp_subevent_signaling_media_codec_other_configuration_get_media_codec_information_len(event); + + if (local_seid == lhdcv5_local_seid) { + if (!is_lhdcv5_codec_info(info, len)) return AVDTP_ERROR_CODE_UNSUPPORTED_CONFIGURATION; + if (!lhdcv5_rate_from_codec_info(info) || !lhdcv5_depth_from_codec_info(info)) return AVDTP_ERROR_CODE_UNSUPPORTED_CONFIGURATION; + if ((info[8] & LHDCV5_VERSION_1) == 0 || (info[8] & LHDCV5_FRAME_5MS) == 0) return AVDTP_ERROR_CODE_UNSUPPORTED_CONFIGURATION; + return ERROR_CODE_SUCCESS; + } + int hd = local_seid == aptx_hd_local_seid; + if (local_seid != aptx_local_seid && !hd) return ERROR_CODE_SUCCESS; + if (!is_aptx_codec_info(info, len, hd)) return AVDTP_ERROR_CODE_UNSUPPORTED_CONFIGURATION; + if (!aptx_rate_from_codec_info(info, len)) return AVDTP_ERROR_CODE_UNSUPPORTED_CONFIGURATION; + if ((info[6] & APTX_CHANNEL_STEREO) == 0) return AVDTP_ERROR_CODE_UNSUPPORTED_CONFIGURATION; + return ERROR_CODE_SUCCESS; +} + +static void a2dp_packet_handler(uint8_t packet_type, uint16_t channel, + uint8_t *packet, uint16_t size) { + UNUSED(channel); + UNUSED(size); + if (packet_type != HCI_EVENT_PACKET) return; + if (hci_event_packet_get_type(packet) != HCI_EVENT_A2DP_META) return; + + switch (hci_event_a2dp_meta_get_subevent_code(packet)) { + case A2DP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CONFIGURATION: { + const uint8_t *info = + a2dp_subevent_signaling_media_codec_other_configuration_get_media_codec_information(packet); + uint16_t len = + a2dp_subevent_signaling_media_codec_other_configuration_get_media_codec_information_len(packet); + if (is_lhdcv5_codec_info(info, len)) { + negotiated_sample_rate = lhdcv5_rate_from_codec_info(info); + negotiated_bit_depth = lhdcv5_depth_from_codec_info(info); + printf("A2DP: LHDC V5 selected, %" PRIu32 " Hz, %u-bit, LL=%s\n", negotiated_sample_rate, negotiated_bit_depth, + (len > 9 && (info[9] & LHDCV5_FEATURE_LL)) ? "ON" : "OFF"); + } else if (is_aptx_codec_info(info, len, 1)) { + negotiated_sample_rate = aptx_rate_from_codec_info(info, len); negotiated_bit_depth = 24; + printf("A2DP: aptX HD selected, %" PRIu32 " Hz, stereo\n", negotiated_sample_rate); + } else if (is_aptx_codec_info(info, len, 0)) { + negotiated_sample_rate = aptx_rate_from_codec_info(info, len); negotiated_bit_depth = 16; + printf("A2DP: aptX selected, %" PRIu32 " Hz, stereo\n", negotiated_sample_rate); + } else break; + printf("A2DP vendor config (%u):", len); + for (uint16_t i = 0; i < len; i++) printf(" %02x", info[i]); + printf("\n"); + break; + } + + case A2DP_SUBEVENT_SIGNALING_MEDIA_CODEC_SBC_CONFIGURATION: + negotiated_sample_rate = + a2dp_subevent_signaling_media_codec_sbc_configuration_get_sampling_frequency(packet); + negotiated_bit_depth = 16; + printf("A2DP: SBC fallback selected, %" PRIu32 " Hz\n", negotiated_sample_rate); + break; + + case A2DP_SUBEVENT_STREAM_ESTABLISHED: + if (a2dp_subevent_stream_established_get_status(packet) == ERROR_CODE_SUCCESS) { + printf("A2DP stream established, local SEID %u\n", + a2dp_subevent_stream_established_get_local_seid(packet)); + } else { + printf("A2DP stream establishment failed: 0x%02x\n", + a2dp_subevent_stream_established_get_status(packet)); + } + break; + + case A2DP_SUBEVENT_STREAM_STARTED: { + uint8_t local_seid = a2dp_subevent_stream_started_get_local_seid(packet); + pcm_queue_reset(); + if (local_seid == lhdcv5_local_seid) { + if (!lhdcv5_decoder_open(negotiated_sample_rate, negotiated_bit_depth)) break; + } else if (local_seid == aptx_hd_local_seid || local_seid == aptx_local_seid) { + int hd = local_seid == aptx_hd_local_seid; + if (!aptx_decoder_open(hd)) break; + } else if (local_seid == sbc_local_seid) { + sbc_decoder = btstack_sbc_decoder_bluedroid_init_instance(&sbc_decoder_context); + sbc_decoder->configure(&sbc_decoder_context, SBC_MODE_STANDARD, handle_sbc_pcm, NULL); + } + audio_open(negotiated_sample_rate); + printf("A2DP stream started on SEID %u\n", local_seid); + break; + } + + case A2DP_SUBEVENT_STREAM_SUSPENDED: { + const btstack_audio_sink_t *audio = btstack_audio_sink_get_instance(); + if (audio_started && audio != NULL) audio->stop_stream(); + audio_started = 0; + pcm_queue_reset(); + if (lhdcv5_decoder != NULL) lhdc_dec_flush(lhdcv5_decoder); + if (aptx_decoder != NULL && aptx_reset_ptr != NULL) aptx_reset_ptr(aptx_decoder); + printf("A2DP stream suspended\n"); + break; + } + + case A2DP_SUBEVENT_STREAM_STOPPED: + audio_close(); + lhdcv5_decoder_close(); + aptx_decoder_close(); + printf("A2DP stream stopped\n"); + break; + + case A2DP_SUBEVENT_STREAM_RELEASED: + audio_close(); + lhdcv5_decoder_close(); + aptx_decoder_close(); + printf("A2DP stream released\n"); + break; + + default: + break; + } +} + +static void hci_packet_handler(uint8_t packet_type, uint16_t channel, + uint8_t *packet, uint16_t size) { + UNUSED(channel); + UNUSED(size); + if (packet_type != HCI_EVENT_PACKET) return; + + bd_addr_t address; + switch (hci_event_packet_get_type(packet)) { + case HCI_EVENT_PIN_CODE_REQUEST: + hci_event_pin_code_request_get_bd_addr(packet, address); + gap_pin_code_response(address, "0000"); + break; + case HCI_EVENT_USER_CONFIRMATION_REQUEST: + hci_event_user_confirmation_request_get_bd_addr(packet, address); + printf("Pairing confirmation for %s, accepting\n", bd_addr_to_str(address)); + gap_ssp_confirmation_response(address); + break; + default: + break; + } +} + +static void setup_receiver(void) { + l2cap_init(); + sdp_init(); + a2dp_sink_init(); + + a2dp_sink_register_packet_handler(&a2dp_packet_handler); + a2dp_sink_register_media_handler(&handle_media_data); + a2dp_sink_register_media_config_validator(&media_config_validator); + + avdtp_stream_endpoint_t *sbc_endpoint = a2dp_sink_create_stream_endpoint( + AVDTP_AUDIO, AVDTP_CODEC_SBC, + media_sbc_codec_capabilities, sizeof(media_sbc_codec_capabilities), + media_sbc_codec_configuration, sizeof(media_sbc_codec_configuration)); + btstack_assert(sbc_endpoint != NULL); + sbc_local_seid = avdtp_local_seid(sbc_endpoint); + + /* aptX decoding is optional. Only advertise the vendor codecs when an + * external openaptx0.dll backend is available at runtime. */ + if (aptx_load_library()) { + avdtp_stream_endpoint_t *aptx_endpoint = a2dp_sink_create_stream_endpoint( + AVDTP_AUDIO, AVDTP_CODEC_NON_A2DP, + media_aptx_codec_capabilities, sizeof(media_aptx_codec_capabilities), + media_aptx_codec_configuration, sizeof(media_aptx_codec_configuration)); + btstack_assert(aptx_endpoint != NULL); + aptx_local_seid = avdtp_local_seid(aptx_endpoint); + + avdtp_stream_endpoint_t *aptx_hd_endpoint = a2dp_sink_create_stream_endpoint( + AVDTP_AUDIO, AVDTP_CODEC_NON_A2DP, + media_aptx_hd_codec_capabilities, sizeof(media_aptx_hd_codec_capabilities), + media_aptx_hd_codec_configuration, sizeof(media_aptx_hd_codec_configuration)); + btstack_assert(aptx_hd_endpoint != NULL); + aptx_hd_local_seid = avdtp_local_seid(aptx_hd_endpoint); + } + + avdtp_stream_endpoint_t *lhdc_endpoint = a2dp_sink_create_stream_endpoint( + AVDTP_AUDIO, AVDTP_CODEC_NON_A2DP, + media_lhdcv5_codec_capabilities, sizeof(media_lhdcv5_codec_capabilities), + media_lhdcv5_codec_configuration, sizeof(media_lhdcv5_codec_configuration)); + btstack_assert(lhdc_endpoint != NULL); + lhdcv5_local_seid = avdtp_local_seid(lhdc_endpoint); + + memset(sdp_a2dp_sink_service_buffer, 0, sizeof(sdp_a2dp_sink_service_buffer)); + a2dp_sink_create_sdp_record(sdp_a2dp_sink_service_buffer, + sdp_create_service_record_handle(), + AVDTP_SINK_FEATURE_MASK_HEADPHONE, + "BTstack Multi-Codec Receiver", "BTstack"); + sdp_register_service(sdp_a2dp_sink_service_buffer); + + memset(device_id_sdp_service_buffer, 0, sizeof(device_id_sdp_service_buffer)); + device_id_create_sdp_record(device_id_sdp_service_buffer, + sdp_create_service_record_handle(), + DEVICE_ID_VENDOR_ID_SOURCE_BLUETOOTH, + BLUETOOTH_COMPANY_ID_BLUEKITCHEN_GMBH, 1, 1); + sdp_register_service(device_id_sdp_service_buffer); + + gap_set_local_name("BTstack Multi-Codec Receiver 00:00:00:00:00:00"); + gap_set_class_of_device(0x240400); + gap_discoverable_control(1); + gap_set_default_link_policy_settings( + LM_LINK_POLICY_ENABLE_ROLE_SWITCH | LM_LINK_POLICY_ENABLE_SNIFF_MODE); + gap_set_allow_role_switch(true); + + hci_event_callback_registration.callback = &hci_packet_handler; + hci_add_event_handler(&hci_event_callback_registration); + pcm_queue_reset(); + + printf("A2DP endpoints: SBC SEID %u, aptX SEID %u, aptX HD SEID %u, LHDC V5 SEID %u\n", + sbc_local_seid, aptx_local_seid, aptx_hd_local_seid, lhdcv5_local_seid); + if (aptx_local_seid != 0) { + printf("aptX capabilities: Classic + HD, 44.1/48 kHz stereo\n"); + } else { + printf("aptX decoder backend unavailable; aptX endpoints disabled\n"); + } + printf("LHDC V5 capabilities: 44.1/48/96/192 kHz, 16/24-bit, lossy\n"); +} + +int btstack_main(int argc, const char *argv[]) { + UNUSED(argc); + UNUSED(argv); + setup_receiver(); + printf("Starting BTstack multi-codec receiver...\n"); + hci_power_control(HCI_POWER_ON); + return 0; +} diff --git a/port/windows-winusb/lhdcv5_msvc_compat.h b/port/windows-winusb/lhdcv5_msvc_compat.h new file mode 100644 index 0000000000..5a525d7533 --- /dev/null +++ b/port/windows-winusb/lhdcv5_msvc_compat.h @@ -0,0 +1,24 @@ +#ifndef LHDCV5_MSVC_COMPAT_H +#define LHDCV5_MSVC_COMPAT_H + +#ifdef _MSC_VER +#include + +#ifndef __attribute__ +#define __attribute__(x) +#endif + +static __forceinline int lhdcv5_msvc_clz(unsigned int value) { + unsigned long index; + if (_BitScanReverse(&index, value)) { + return 31 - (int)index; + } + return 32; +} + +#ifndef __builtin_clz +#define __builtin_clz(x) lhdcv5_msvc_clz((unsigned int)(x)) +#endif +#endif + +#endif \ No newline at end of file diff --git a/port/windows-winusb/lhdcv5_replay.c b/port/windows-winusb/lhdcv5_replay.c new file mode 100644 index 0000000000..5e2dcd7a99 --- /dev/null +++ b/port/windows-winusb/lhdcv5_replay.c @@ -0,0 +1,99 @@ +#include +#include +#include +#include +#include "lhdc_dec.h" + +int g_nzc_formula = 0; +int g_pkframe = 0; + +static uint8_t *read_all(const char *path, size_t *size) { + FILE *f = NULL; +#ifdef _MSC_VER + fopen_s(&f, path, "rb"); +#else + f = fopen(path, "rb"); +#endif + if (!f) return NULL; + fseek(f, 0, SEEK_END); + long n = ftell(f); + fseek(f, 0, SEEK_SET); + if (n <= 0) { fclose(f); return NULL; } + uint8_t *p = (uint8_t *)malloc((size_t)n); + if (!p || fread(p, 1, (size_t)n, f) != (size_t)n) { + free(p); fclose(f); return NULL; + } + fclose(f); *size = (size_t)n; return p; +} +int main(int argc, char **argv) { + if (argc < 4) { + fprintf(stderr, "usage: %s input.lhdc sample_rate bit_depth [out.pcm]\n", argv[0]); + return 2; + } + uint32_t sr = (uint32_t)strtoul(argv[2], NULL, 10); + uint8_t depth = (uint8_t)strtoul(argv[3], NULL, 10); + size_t input_size = 0; + uint8_t *input = read_all(argv[1], &input_size); + if (!input) { fprintf(stderr, "cannot read input\n"); return 1; } + + lhdc_dec_config_t cfg; + memset(&cfg, 0, sizeof(cfg)); + cfg.sample_rate = (lhdc_dec_sample_rate_t)sr; + cfg.bit_depth = depth == 24 ? LHDC_DEC_BITDEPTH_24 : LHDC_DEC_BITDEPTH_16; + cfg.frame_duration = LHDC_DEC_FRAME_5MS; + cfg.channels = 2; + cfg.max_frame_bytes = 8192; + size_t ws_size = lhdc_dec_get_workspace_size(sr, 5); + void *ws = calloc(1, ws_size); + lhdc_decoder_t *dec = ws ? lhdc_dec_init(ws, &cfg) : NULL; + if (!dec) { fprintf(stderr, "decoder init failed\n"); free(input); free(ws); return 1; } + + FILE *out = NULL; + if (argc > 4) { +#ifdef _MSC_VER + fopen_s(&out, argv[4], "wb"); +#else + out = fopen(argv[4], "wb"); +#endif + } + size_t off = 0; + uint32_t good = 0, bad = 0; + uint64_t pcm_frames = 0; + uint32_t errors[16] = {0}; + while (off + 2 <= input_size) { + uint16_t h = (uint16_t)(input[off] | ((uint16_t)input[off + 1] << 8)); + size_t payload = (size_t)(h & 0x3ff) * 2; + size_t step = 2 + payload; + if (payload == 0 || off + step > input_size) { + fprintf(stderr, "bad framing at %zu hdr=%04x payload=%zu\n", off, h, payload); + break; + } + uint8_t pcm[1920 * 2 * 4]; + size_t consumed = 0; + uint32_t generated = 0; + lhdc_dec_frame_info_t info; + lhdc_dec_ret_t r = lhdc_dec_decode_frame(dec, input + off, step, pcm, 1920, + &consumed, &generated, &info); + if (r == LHDC_DEC_OK && consumed == step) { + good++; pcm_frames += generated; + if (out) fwrite(pcm, 1, (size_t)generated * info.channels * (info.bit_depth / 8), out); + } else { + bad++; + unsigned e = (r < 0 && -r < 16) ? (unsigned)(-r) : 0; + errors[e]++; + if (bad <= 20) fprintf(stderr, "error frame=%u off=%zu ret=%d consumed=%zu step=%zu\n", + good + bad - 1, off, (int)r, consumed, step); + } + off += step; + } + if (out) fclose(out); + printf("input=%zu bytes good=%u bad=%u pcm_frames=%llu\n", + input_size, good, bad, (unsigned long long)pcm_frames); + for (unsigned i = 0; i < 16; i++) { + if (errors[i]) printf("error -%u: %u\n", i, errors[i]); + } + free(input); + free(ws); + lhdc_dec_free_window(); + return bad ? 3 : 0; +}