diff --git a/app/boards/intel_adsp_ace15_mtpm.conf b/app/boards/intel_adsp_ace15_mtpm.conf index 65cf5a620db1..a967959f531c 100644 --- a/app/boards/intel_adsp_ace15_mtpm.conf +++ b/app/boards/intel_adsp_ace15_mtpm.conf @@ -72,6 +72,7 @@ CONFIG_DAI_DMIC_HAS_MULTIPLE_LINE_SYNC=y CONFIG_DAI_DMIC_HW_IOCLK=38400000 CONFIG_DAI_INIT_PRIORITY=70 CONFIG_DMA_DW_LLI_POOL_SIZE=50 +CONFIG_DMA_DW_FIFO_PARTITION=y CONFIG_DMA_INTEL_ADSP_GPDMA=y CONFIG_MEMORY_WIN_2_SIZE=12288 CONFIG_MM_DRV_INTEL_ADSP_TLB_REMAP_UNUSED_RAM=y diff --git a/app/boards/intel_adsp_ace30_ptl.conf b/app/boards/intel_adsp_ace30_ptl.conf index 43f718981453..1baa6a87dcdd 100644 --- a/app/boards/intel_adsp_ace30_ptl.conf +++ b/app/boards/intel_adsp_ace30_ptl.conf @@ -15,6 +15,7 @@ CONFIG_FORMAT_CONVERT_HIFI3=n CONFIG_COMP_GOOGLE_RTC_AUDIO_PROCESSING=m CONFIG_GOOGLE_RTC_AUDIO_PROCESSING_MOCK=y CONFIG_COMP_STFT_PROCESS=y +CONFIG_COMP_BT_CODEC=y # SOF / infrastructure CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL=n diff --git a/app/boards/intel_adsp_ace30_wcl.conf b/app/boards/intel_adsp_ace30_wcl.conf index 98f5ba341fc3..87737ef61562 100644 --- a/app/boards/intel_adsp_ace30_wcl.conf +++ b/app/boards/intel_adsp_ace30_wcl.conf @@ -15,6 +15,7 @@ CONFIG_FORMAT_CONVERT_HIFI3=n CONFIG_COMP_GOOGLE_RTC_AUDIO_PROCESSING=m CONFIG_GOOGLE_RTC_AUDIO_PROCESSING_MOCK=y CONFIG_COMP_STFT_PROCESS=y +CONFIG_COMP_BT_CODEC=y # SOF / infrastructure CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL=n diff --git a/src/audio/CMakeLists.txt b/src/audio/CMakeLists.txt index 0ca532950f5c..acaa0c2f708d 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -178,3 +178,7 @@ else() ### library, e.g. testbench ### ) endif() endif() + +if(CONFIG_COMP_BT_CODEC) + add_subdirectory(bt_codec) +endif() diff --git a/src/audio/Kconfig b/src/audio/Kconfig index dbd12f1626b2..e5720ceb7ada 100644 --- a/src/audio/Kconfig +++ b/src/audio/Kconfig @@ -451,4 +451,6 @@ config DAI_VERBOSE_GLITCH_WARNINGS always emit a warning. This option useful to debug issues with DAI DMA operation and issues with low-latency scheduling. +rsource "bt_codec/Kconfig" + endmenu diff --git a/src/audio/bt_codec/CMakeLists.txt b/src/audio/bt_codec/CMakeLists.txt new file mode 100644 index 000000000000..ac38a5cfa6b4 --- /dev/null +++ b/src/audio/bt_codec/CMakeLists.txt @@ -0,0 +1,74 @@ +# SPDX-License-Identifier: BSD-3-Clause + +set(LIBLC3_DIR "") +if(DEFINED ZEPHYR_LIBLC3_MODULE_DIR) + if(EXISTS "${ZEPHYR_LIBLC3_MODULE_DIR}/include/lc3.h") + set(LIBLC3_DIR "${ZEPHYR_LIBLC3_MODULE_DIR}") + endif() +endif() + +if("${LIBLC3_DIR}" STREQUAL "") + cmake_path(SET LIBLC3_DIR NORMALIZE "${CMAKE_CURRENT_LIST_DIR}/../../../../modules/lib/liblc3") +endif() + +zephyr_include_directories( + ${LIBLC3_DIR}/include + ${LIBLC3_DIR}/src +) + +set(LIBLC3_SOURCES + ${LIBLC3_DIR}/src/attdet.c + ${LIBLC3_DIR}/src/bits.c + ${LIBLC3_DIR}/src/bwdet.c + ${LIBLC3_DIR}/src/energy.c + ${LIBLC3_DIR}/src/lc3.c + ${LIBLC3_DIR}/src/ltpf.c + ${LIBLC3_DIR}/src/mdct.c + ${LIBLC3_DIR}/src/plc.c + ${LIBLC3_DIR}/src/sns.c + ${LIBLC3_DIR}/src/spec.c + ${LIBLC3_DIR}/src/tables.c + ${LIBLC3_DIR}/src/tns.c +) + +zephyr_compile_definitions( + LC3_PLUS=0 + LC3_PLUS_HR=0 +) + +add_local_sources(sof + bt_codec.c + bt_codec_algos.c + ${LIBLC3_SOURCES} +) + +set(BT_CODEC_COMPILE_OPTIONS + -O3 + -ffast-math + -fno-finite-math-only + -Wno-array-bounds + -Wno-double-promotion + -include + ${CMAKE_CURRENT_LIST_DIR}/lc3_math_shim.h +) + +# When building with Clang on platforms where hardware FPU is disabled (e.g. +# ACE 3.0 PTL / WCL with CONFIG_FPU=n), strip the Xtensa FP0 feature so liblc3 +# and bt_codec emit software floating-point calls instead of hardware FP0 +# instructions that fault when CPENABLE bit 0 is cleared. +if(CMAKE_C_COMPILER_ID MATCHES "Clang" AND NOT CONFIG_FPU) + list(APPEND BT_CODEC_COMPILE_OPTIONS + -Xclang + -target-feature + -Xclang + -fp + ) +endif() + +set_source_files_properties( + ${CMAKE_CURRENT_LIST_DIR}/bt_codec.c + ${CMAKE_CURRENT_LIST_DIR}/bt_codec_algos.c + ${LIBLC3_SOURCES} + TARGET_DIRECTORY modules_sof + PROPERTIES COMPILE_OPTIONS "${BT_CODEC_COMPILE_OPTIONS}" +) diff --git a/src/audio/bt_codec/Kconfig b/src/audio/bt_codec/Kconfig new file mode 100644 index 000000000000..ced1c166b9f4 --- /dev/null +++ b/src/audio/bt_codec/Kconfig @@ -0,0 +1,11 @@ +# SPDX-License-Identifier: BSD-3-Clause + +config COMP_BT_CODEC + bool "Bluetooth Transparent Mode DSP Codec components (LC3, mSBC, G.722, SB4)" + depends on COMP_MODULE_ADAPTER && IPC_MAJOR_4 + help + Select to include Bluetooth Transparent Mode (0x03) DSP audio codec + offload components for SSP2-BT playback and capture pipelines. + Provides bt_lc3 (Zephyr liblc3 7.5 ms codec), bt_msbc (Bluetooth HFP + H2-framed mSBC), bt_g722 (ITU-T Rec. G.722 SB-ADPCM), and bt_sb4 + (experimental 4-band tree-QMF subband ADPCM). diff --git a/src/audio/bt_codec/README.md b/src/audio/bt_codec/README.md new file mode 100644 index 000000000000..0ed9c2a3a8c0 --- /dev/null +++ b/src/audio/bt_codec/README.md @@ -0,0 +1,407 @@ +# Bluetooth Audio Offload & Transparent DSP Codec Modules (`bt_codec`) + +> **Runs in:** Application — under userspace configs (e.g. PTL/WCL with `CONFIG_SOF_USERSPACE_LL`), +> these IPC4 processing modules run in MMU-isolated **user** threads. See +> [User/kernel split](../../include/sof/userspace/README.md). + +This directory implements Sound Open Firmware (SOF) DSP audio encoder/decoder modules for +Bluetooth **Transparent Mode (`Coding_Format = 0x03`)** over the dedicated Bluetooth SSP audio link +(`SSP2-BT`, HCI `Data_Path_ID = 0x01`, ALSA `Bluetooth` PCM `hw:0,20`). + +Together with SOF's `SSP2-BT` passthrough topologies, SOF supports **12 Bluetooth audio offload +protocols** across two architectural modes: + +1. **BT Controller Hardware Offload (`BT_TRANSP_CODEC="none"`)**: + The SOF topology connects `host-copier` directly to `dai-copier.SSP` (`SSP2-BT`), and the + Bluetooth controller performs over-the-air audio encoding/decoding in hardware (`A2DP SBC`, + `A2DP AAC`, `eSCO mSBC`, `eSCO CVSD`, `eSCO G.711 u-law`, `eSCO G.711 A-law`, `eSCO Transparent` + raw PCM, and `LE Audio CIS LC3`). +2. **SOF DSP Transparent Codec Offload (`BT_TRANSP_CODEC="lc3" | "msbc" | "g722" | "sb4"`)**: + The Bluetooth controller runs an unencoded eSCO **Transparent (`0x03`)** air link over `SSP2-BT` + (`8 kHz` `S16_LE`), while the SOF DSP runs wideband (`16 kHz`) or narrowband (`8 kHz`) mono audio + compression (`S16_LE` / `S32_LE`) on playback and decompression (`S16_LE`) on capture inside + `bt_codec`. + +--- + +## Architecture & Data Flow + +```mermaid +flowchart LR + subgraph Host["Linux Host (ALSA + Bluetooth)"] + ALSA["aplay / arecord\n(hw:0,20 / hw:0,36, 8/16 kHz)"] + BTSock["BT Socket (SCO / ISO / L2CAP)\nsetsockopt(BT_CODEC, path=0x01)"] + end + + subgraph DSP["SOF DSP (SSP2-BT Pipelines)"] + HC_PB["host-copier.20/36.playback"] + ENC["bt_lc3 / bt_msbc /\nbt_g722 / bt_sb4\n(Encode 8/16 kHz -> 60B pkt)"] + DC_PB["dai-copier.SSP.BT.playback"] + + DC_CP["dai-copier.SSP.BT.capture"] + DEC["bt_lc3 / bt_msbc /\nbt_g722 / bt_sb4\n(Decode 60B pkt -> 8/16 kHz)"] + HC_CP["host-copier.20.capture"] + end + + subgraph BTC["Bluetooth Controller"] + SSP["SSP2 TDM Interface\n(Data_Path_ID = 0x01)"] + RF["BR/EDR eSCO / ACL / LE CIS\nOver-the-Air Radio"] + end + + ALSA -->|Playback PCM| HC_PB --> ENC --> DC_PB --> SSP --> RF + RF --> SSP --> DC_CP --> DEC --> HC_CP -->|Capture PCM| ALSA + BTSock -.->|HCI 0x0c83 + 0x043d/0x206e/0xfc1e| BTC +``` + +### Self-Synchronizing 60-Byte Transparent Packet Framing + +Over an `8 kHz` `S16_LE` eSCO `Transparent (0x03)` link, `7.5 ms` equals `60` samples (`60` bytes +when carrying 1 air byte in the upper 8 bits of each 16-bit TDM sample). Each SOF DSP codec encodes +`120` wideband (`16 kHz`) or `60` narrowband (`8 kHz`) PCM samples per `7.5 ms` frame into a +self-synchronizing `60`-byte packet so DSP-to-DSP links achieve deterministic frame lock even across +arbitrary mid-stream air byte offsets: + +| Module | UUID | Algorithm | Host PCM Format | Compressed Payload (per 7.5 ms) | SSP Link Format | +|---|---|---|---|---|---| +| `bt_lc3` | `4c433301-b700-4a10-8000010203040501` | Zephyr `liblc3` (60.8 kbps, 7.5 ms) | `8 / 16 kHz` 1ch `S16_LE` / `S32_LE` | `57` bytes (`456` bits) + sync + CRC-8 | `8 kHz` 1ch `S16_LE` | +| `bt_msbc` | `4d534243-b700-4a10-8000010203040502` | Bluetooth SIG HFP v1.8 / A2DP v1.3 `mSBC` (`bitpool=26`, H2 header) | `8 / 16 kHz` 1ch `S16_LE` / `S32_LE` | `2B` H2 + `57B` mSBC + `0x00` pad | `8 kHz` 1ch `S16_LE` | +| `bt_g722` | `47373232-b700-4a10-8000010203040503` | ITU-T Rec. G.722 Mode 2 (56 kbps 2-band 24-tap QMF SB-ADPCM) | `8 / 16 kHz` 1ch `S16_LE` / `S32_LE` | `420` bits (`53B`) + sync + CRC-8 | `8 kHz` 1ch `S16_LE` | +| `bt_sb4` | `53423401-b700-4a10-8000010203040504` | Experimental 4-band tree-QMF subband ADPCM (`6/4/3/2`-bit) | `8 / 16 kHz` 1ch `S16_LE` / `S32_LE` | `450` bits (`57B`) + sync + CRC-8 | `8 kHz` 1ch `S16_LE` | + +--- + +## Build & Topology Configuration + +### 1. Firmware Build (`Kconfig`) + +Enable `CONFIG_COMP_BT_CODEC=y` (enabled by default on ACE 3.0 `intel_adsp_ace30_ptl.conf` and +`intel_adsp_ace30_wcl.conf`) and ensure `west update` has fetched `modules/lib/liblc3`: + +```bash +west update liblc3 +./sof/scripts/xtensa-build-zephyr.py -p ptl +./sof/scripts/xtensa-build-zephyr.py -p wcl +``` + +### 2. Feature Topologies (`tools/topology/topology2`) + +Build the production topologies: + +```bash +cmake -S sof/tools -B sof/build-tools +cmake --build sof/build-tools --target topologies2 -j$(nproc) +``` + +Generated Bluetooth topologies (`PCM 20: Bluetooth` duplex low-latency at `hw:0,20`, plus +`PCM 36: Deepbuffer Bluetooth` playback at `hw:0,36` with a `100 ms` L2 SRAM DMA buffer and `D0i3` +streaming compatibility): +- **BT Controller Hardware Offload (`BT_TRANSP_CODEC="none"`, `BT_DEEP_BUFFER="true"`)**: + - SoundWire RT722 (PTL / WCL): `sof-{ptl,wcl}-rt722-4ch-bt.tplg`, `sof-{ptl,wcl}-ssp2-bt-id10.tplg` + - HDA Generic (MTL / ARL / PTL): `sof-hda-generic-ace1-bt.tplg`, `sof-hda-generic-ace3-bt.tplg`, `sof-hda-generic-ace3-{2ch,4ch}-bt.tplg` +- **SOF DSP Transparent Codec Feature Topologies (`c` in `lc3`, `msbc`, `g722`, `sb4`, `BT_DEEP_BUFFER="true"`)**: + - SoundWire RT722 (PTL / WCL): `sof-{ptl,wcl}-rt722-4ch-bt-${c}.tplg` + - Standalone SSP2-BT (PTL / WCL): `sof-{ptl,wcl}-ssp2-bt-id10-${c}.tplg` + - HDA Generic (PTL): `sof-hda-generic-ace3-bt-${c}.tplg` + +### 3. Linux Kernel Driver Prerequisites + +On HDA-based platforms (`sof-hda-dsp`), enable the `SSP2-BT` machine driver DAI link via +`bt_link_mask=0x4` (bit `2` = `SSP2`) in `/etc/modprobe.d/sof-bt.conf`: + +```ini +options snd_sof_intel_hda_generic bt_link_mask=0x4 +``` + +Verify that ALSA exposes `card 0, device 20: Bluetooth (*)` after loading SOF: + +```bash +aplay -l | grep -i bluetooth +``` + +--- + +## Hardware Testing Guide + +> **Important (`codec_provider` SSP Clocking):** +> On Intel Bluetooth controllers, `SSP2-BT` (`BCLK`/`FSYNC`) is driven by the Bluetooth controller +> (`codec_provider`) **only while an offloaded SCO, ISO, or A2DP connection is active**. Always +> open the Bluetooth socket *before* (or concurrently with) `aplay`/`arecord` and keep the socket +> open until `aplay`/`arecord` finishes. + +### 1. Initialize the Bluetooth Controller (Both DUTs) + +Stop `bluetoothd` so raw test sockets own `PSM 0x0019` / SCO / ISO directly, and enable BR/EDR, LE, +Secure Simple Pairing (SSP), and Wideband Speech (WBS): + +```bash +systemctl stop bluetooth +btmgmt --index 0 power off +btmgmt --index 0 bredr on +btmgmt --index 0 le on +btmgmt --index 0 ssp on +btmgmt --index 0 bondable on +btmgmt --index 0 wbs on +btmgmt --index 0 connectable on +btmgmt --index 0 discov on +btmgmt --index 0 power on +hciconfig hci0 piscan +``` + +### 2. Minimal `BT_CODEC` Offload Socket Helpers + +Linux configures Bluetooth SSP offload via `setsockopt(fd, SOL_BLUETOOTH, BT_CODEC, ...)` +(`SOL_BLUETOOTH = 274`, `BT_CODEC = 19`) using `struct bt_codecs`: + +```c +/* Minimal eSCO SSP offload helper: gcc -O2 sco_offload.c -o sco_offload */ +#include +#include +#include +#include +#include +#include + +#define BTPROTO_SCO 2 +#define SOL_BLUETOOTH 274 +#define BT_DEFER_SETUP 7 +#define BT_CODEC 19 + +typedef struct { uint8_t b[6]; } __attribute__((packed)) bdaddr_t; +struct sockaddr_sco { sa_family_t sco_family; bdaddr_t sco_bdaddr; }; +struct bt_codec { + uint8_t id; uint16_t cid; uint16_t vid; uint8_t data_path; uint8_t num_caps; +} __attribute__((packed)); +struct bt_codecs { + uint8_t num_codecs; struct bt_codec codecs[1]; +} __attribute__((packed)); + +static void parse_bdaddr(const char *s, bdaddr_t *ba) { + unsigned int b[6]; + sscanf(s, "%x:%x:%x:%x:%x:%x", &b[5], &b[4], &b[3], &b[2], &b[1], &b[0]); + for (int i = 0; i < 6; i++) ba->b[i] = (uint8_t)b[i]; +} + +/* Usage: + * Server: ./sco_offload listen + * Client: ./sco_offload connect + */ +int main(int argc, char **argv) { + int is_srv = (argc > 1 && argv[1][0] == 'l'); + int argi = is_srv ? 2 : 3; + uint8_t codec_id = strtoul(argv[argi], NULL, 0); + uint8_t path = strtoul(argv[argi + 1], NULL, 0); + int hold = atoi(argv[argi + 2]); + + int sk = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO); + struct sockaddr_sco loc = { .sco_family = AF_BLUETOOTH }; + bind(sk, (struct sockaddr *)&loc, sizeof(loc)); + + struct bt_codecs cs = { + .num_codecs = 1, + .codecs = {{ .id = codec_id, .data_path = path }}, + }; + if (setsockopt(sk, SOL_BLUETOOTH, BT_CODEC, &cs, sizeof(cs)) < 0) { + perror("setsockopt(BT_CODEC)"); + return 1; + } + + if (is_srv) { + int defer = 1; + setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP, &defer, sizeof(defer)); + listen(sk, 1); + int csk = accept(sk, NULL, NULL); + if (csk < 0) { perror("accept"); return 1; } + char ch; + recv(csk, &ch, 1, 0); + struct pollfd pfd = { .fd = csk, .events = POLLOUT }; + poll(&pfd, 1, 5000); + puts("SCO SRV connected!"); + fflush(stdout); + sleep(hold); + close(csk); + } else { + struct sockaddr_sco rem = { .sco_family = AF_BLUETOOTH }; + parse_bdaddr(argv[2], &rem.sco_bdaddr); + if (connect(sk, (struct sockaddr *)&rem, sizeof(rem)) < 0) { + perror("connect"); + return 1; + } + puts("SCO CLI connected!"); + fflush(stdout); + sleep(hold); + } + close(sk); + return 0; +} +``` + +### 3. Testing BT Controller Hardware Offload Protocols + +Ensure a passthrough `-bt.tplg` topology is active (e.g. `sof-wcl-rt722-4ch-bt.tplg` or +`sof-hda-generic-ace3-bt.tplg`). + +#### A. HFP / eSCO Controller Offload (`mSBC`, `CVSD`, `u-law`, `A-law`, `Transparent`) + +| Protocol | `` | `` | ALSA PCM Format (`hw:0,20`) | +|---|---|---|---| +| `mSBC` (Wideband Speech) | `0x05` | `0x01` | `-r 16000 -c 1 -f S16_LE` | +| `CVSD` (Narrowband) | `0x02` | `0x01` | `-r 8000 -c 1 -f S16_LE` | +| `G.711 u-law` | `0x00` | `0x01` | `-r 8000 -c 1 -f S16_LE` | +| `G.711 A-law` | `0x01` | `0x01` | `-r 8000 -c 1 -f S16_LE` | +| `Transparent` (Raw PCM) | `0x03` | `0x01` | `-r 8000 -c 1 -f S16_LE` | + +```bash +# DUT A (Responder, BD_ADDR AA:BB:CC:DD:EE:FF): +./sco_offload listen 0x05 0x01 10 & + +# DUT B (Initiator): connect eSCO mSBC (0x05) over SSP2-BT (0x01) +./sco_offload connect AA:BB:CC:DD:EE:FF 0x05 0x01 10 & + +# Once connected, stream & capture full-duplex audio simultaneously on both DUTs: +# On DUT B: +aplay -D hw:0,20 -r 16000 -c 1 -f S16_LE -d 4 tone_1000hz_16k.wav & +arecord -D hw:0,20 -r 16000 -c 1 -f S16_LE -d 4 rx_dut_b.wav & + +# On DUT A: +aplay -D hw:0,20 -r 16000 -c 1 -f S16_LE -d 4 tone_500hz_16k.wav & +arecord -D hw:0,20 -r 16000 -c 1 -f S16_LE -d 4 rx_dut_a.wav & +wait +``` + +#### B. A2DP Controller Offload (`SBC 0x00` & `AAC 0x02`, 48 kHz Stereo) + +On `BTPROTO_L2CAP` media sockets (`PSM 0x0019`), `setsockopt(SOL_BLUETOOTH, BT_CODEC)` with +`data_path = 0x01` and AVDTP Media Codec Capabilities (`id = 0x00` for SBC or `id = 0x02` for AAC) +triggers Microsoft vendor extension (`0xfc1e`) subcommands `0x08` (`Avdtp_Open`) and `0x09` +(`Avdtp_Start`), encoding `48 kHz` stereo PCM from `hw:0,20`: + +```bash +# On Transmitter DUT (Low-Latency FE hw:0,20 or Deepbuffer FE hw:0,36): +aplay -D hw:0,20 -r 48000 -c 2 -f S16_LE -d 5 tone_1000hz_48k_2ch.wav & +sleep 0.3 +./a2dp_tx AA:BB:CC:DD:EE:FF sbc 4 # or 'aac' +wait +``` + +#### D. Deepbuffer Playback & Concurrent Mixing (`hw:0,36`) + +When `BT_DEEP_BUFFER=true` is enabled in topology, ALSA exposes `device 36: Deepbuffer Bluetooth` +(`hw:0,36`) alongside `device 20: Bluetooth` (`hw:0,20`). `hw:0,36` allocates a `100 ms` L2 SRAM +host-copier buffer (`lp_mode = 1`, `playback_compatible_d0i3 = true`), enforces ALSA period times +`>= 110 ms`, allows PCI `D0i3` streaming when no low-latency stream is active, and mixes in `S32_LE` +with `hw:0,20`: + +```bash +# Deepbuffer A2DP playback (>= 110 ms period, D0i3 streaming eligible after 5 s): +aplay -D hw:0,36 --period-time=250000 --buffer-time=1000000 \ + -r 48000 -c 2 -f S16_LE -d 10 tone_1000hz_48k_2ch.wav & +sleep 0.3 +./a2dp_tx AA:BB:CC:DD:EE:FF sbc 9 +wait + +# Concurrent stream mixing (Deepbuffer media on hw:0,36 + Low-Latency alert on hw:0,20): +aplay -D hw:0,36 --period-time=125000 --buffer-time=500000 \ + -r 48000 -c 2 -f S16_LE -d 6 tone_440hz_48k_2ch.wav & +aplay -D hw:0,20 -r 48000 -c 2 -f S16_LE -d 6 tone_1000hz_48k_2ch.wav & +sleep 0.3 +./a2dp_tx AA:BB:CC:DD:EE:FF sbc 5 +wait +``` + +#### C. LE Audio CIS Controller Offload (`LC3 0x06`, 16 kHz Mono) + +On the CIS Peripheral, enable connectable LE Extended Advertising via HCI (`0x08 0x0036` and +`0x08 0x0039`), listen on a `BTPROTO_ISO` socket configured with `BT_CODEC` (`id = 0x06`, +`data_path = 0x01`), and connect from the CIS Central (`BT_ISO_QOS`: `interval = 10000` us, +`sdu = 40` octets): + +```bash +# On CIS Peripheral (DUT A): +hcitool -i hci0 cmd 0x08 0x0036 \ + 01 13 00 20 00 00 40 00 00 07 00 00 00 00 00 00 00 00 00 7f 01 00 01 00 00 +hcitool -i hci0 cmd 0x08 0x0039 01 01 01 00 00 00 +./iso_srv2 0x06 0x01 12 & + +# On CIS Central (DUT B): +./iso_cli2 AA:BB:CC:DD:EE:FF 0x06 0x01 12 & + +# Once CIS is established, run full-duplex 16 kHz PCM on hw:0,20 on both DUTs: +aplay -D hw:0,20 -r 16000 -c 1 -f S16_LE -d 4 tone_1000hz_16k.wav & +arecord -D hw:0,20 -r 16000 -c 1 -f S16_LE -d 4 rx_iso_lc3.wav & +wait +``` + +--- + +### 4. Testing SOF DSP Transparent Codec Offload (`bt_lc3`, `bt_msbc`, `bt_g722`, `bt_sb4`) + +To test DSP-to-DSP encoding and decoding on SOF, load the matching `*-bt-${c}.tplg` feature +topology (`c` in `lc3`, `msbc`, `g722`, `sb4`) on both DUTs and establish an eSCO **Transparent +(`0x03`)** connection with `data_path = 0x01`: + +```bash +CODEC=lc3 # lc3 | msbc | g722 | sb4 + +# 1. Point active topology symlink to the feature topology and reload SOF +# Example on Wildcat Lake RT722 (DUT A): +ln -sf sof-wcl-rt722-4ch-bt-${CODEC}.tplg \ + /lib/firmware/intel/sof-ipc4-tplg/sof-wcl-rt722-4ch.tplg +rmmod snd_sof_pci_intel_ptl && modprobe snd_sof_pci_intel_ptl + +# Example on Panther Lake HDA (DUT B): +ln -sf sof-hda-generic-ace3-bt-${CODEC}.tplg \ + /lib/firmware/intel/sof-ipc4-tplg/sof-hda-generic.tplg +rmmod snd_sof_pci_intel_ptl && modprobe snd_sof_pci_intel_ptl + +# 2. Open eSCO Transparent (0x03) link over SSP2-BT (0x01) +# DUT A: +./sco_offload listen 0x03 0x01 10 & +# DUT B: +./sco_offload connect AA:BB:CC:DD:EE:FF 0x03 0x01 10 & + +# 3. Stream & record 16 kHz wideband (or 8 kHz narrowband) PCM on hw:0,20 / hw:0,36 +# DUT B (transmits 1000 Hz, captures 500 Hz): +aplay -D hw:0,20 -r 16000 -c 1 -f S16_LE -d 4 tone_1000hz_16k.wav & +arecord -D hw:0,20 -r 16000 -c 1 -f S16_LE -d 4 rx_dut_b_${CODEC}.wav & + +# DUT A (transmits 500 Hz, captures 1000 Hz): +aplay -D hw:0,20 -r 16000 -c 1 -f S16_LE -d 4 tone_500hz_16k.wav & +arecord -D hw:0,20 -r 16000 -c 1 -f S16_LE -d 4 rx_dut_a_${CODEC}.wav & +wait +``` + +--- + +### 5. Automated Sine Tone Generation & FFT Verification + +Generate clean test tones (`500 Hz` and `1000 Hz`) and verify captured recordings using a +Hanning-windowed FFT: + +```bash +# Generate 500 Hz and 1000 Hz 16 kHz mono S16_LE test WAVs +python3 -c ' +import wave, numpy as np +sr, dur = 16000, 6 +t = np.arange(sr * dur) / sr +for hz, fn in [(500.0, "tone_500hz_16k.wav"), (1000.0, "tone_1000hz_16k.wav")]: + pcm = (20000.0 * np.sin(2.0 * np.pi * hz * t)).astype(np.int16) + with wave.open(fn, "wb") as w: + w.setnchannels(1); w.setsampwidth(2); w.setframerate(sr) + w.writeframes(pcm.tobytes()) +' + +# Verify RMS amplitude and dominant FFT frequency of captured WAV files +python3 -c ' +import sys, wave, numpy as np +for fn in sys.argv[1:]: + with wave.open(fn, "rb") as w: + sr, ch = w.getframerate(), w.getnchannels() + s = np.frombuffer(w.readframes(w.getnframes()), dtype=np.int16)[::ch].astype(np.float64) + nz = np.where(np.abs(s) > 500)[0] + seg = s[nz[0] + sr // 8 : nz[-1] - sr // 8] if len(nz) > sr // 2 else s + rms = float(np.sqrt(np.mean(seg ** 2))) if len(seg) else 0.0 + fft = np.abs(np.fft.rfft(seg * np.hanning(len(seg)))) + freqs = np.fft.rfftfreq(len(seg), 1.0 / sr) + print(f"{fn}: sr={sr} Hz, dominant={freqs[np.argmax(fft[1:]) + 1]:.2f} Hz, rms={rms:.1f}") +' rx_dut_*.wav +``` diff --git a/src/audio/bt_codec/bt_codec.c b/src/audio/bt_codec/bt_codec.c new file mode 100644 index 000000000000..bba6e79a78fa --- /dev/null +++ b/src/audio/bt_codec/bt_codec.c @@ -0,0 +1,412 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Intel Corporation. All rights reserved. +// +// SOF Bluetooth Transparent Mode (0x03) DSP Codec Offload Components: +// - bt_lc3 (Zephyr liblc3, 16 kHz WB / 8 kHz NB) +// - bt_msbc (Bluetooth HFP v1.8 / A2DP v1.3 H2-framed mSBC) +// - bt_g722 (ITU-T Rec. G.722 SB-ADPCM) +// - bt_sb4 (Experimental 4-band 24-tap tree-QMF subband ADPCM) + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "lc3_math_shim.h" +#include "bt_codec_algos.h" + +#undef fabsf +#undef fminf +#undef fmaxf +#undef truncf +#undef floorf +#undef ceilf +#undef roundf +#undef ldexpf +#undef frexpf +#undef sqrtf +#undef log10f + +__weak float fabsf(float x) { return sof_lc3_fabsf(x); } +__weak float fminf(float a, float b) { return sof_lc3_fminf(a, b); } +__weak float fmaxf(float a, float b) { return sof_lc3_fmaxf(a, b); } +__weak float truncf(float x) { return sof_lc3_truncf(x); } +__weak float floorf(float x) { return sof_lc3_floorf(x); } +__weak float ceilf(float x) { return sof_lc3_ceilf(x); } +__weak float roundf(float x) { return sof_lc3_roundf(x); } +__weak float ldexpf(float x, int exp) { return sof_lc3_ldexpf(x, exp); } +__weak float frexpf(float x, int *exp) { return sof_lc3_frexpf(x, exp); } +__weak float sqrtf(float x) { return sof_lc3_sqrtf(x); } +__weak float log10f(float x) { return sof_lc3_log10f(x); } + +LOG_MODULE_REGISTER(bt_codec, CONFIG_SOF_LOG_LEVEL); + +SOF_DEFINE_REG_UUID(bt_lc3); +SOF_DEFINE_REG_UUID(bt_msbc); +SOF_DEFINE_REG_UUID(bt_g722); +SOF_DEFINE_REG_UUID(bt_sb4); + +struct bt_codec_comp_data { + enum bt_codec_type codec_type; + enum sof_ipc_frame frame_format; + uint32_t pcm_rate; + uint32_t wire_rate; + uint32_t pcm_step; + uint32_t frame_bytes; + struct bt_codec_state state; +}; + +static int bt_codec_init_common(struct processing_module *mod, enum bt_codec_type type) +{ + struct module_data *md = &mod->priv; + struct comp_dev *dev = mod->dev; + struct bt_codec_comp_data *cd; + + cd = mod_zalloc(mod, sizeof(*cd)); + if (!cd) + return -ENOMEM; + + cd->codec_type = type; + cd->pcm_rate = 8000; + cd->wire_rate = 8000; + cd->pcm_step = 1; + md->private = cd; + comp_info(dev, "bt_codec init type=%d", (int)type); + return 0; +} + +static int bt_lc3_init(struct processing_module *mod) +{ + return bt_codec_init_common(mod, BT_CODEC_LC3); +} + +static int bt_msbc_init(struct processing_module *mod) +{ + return bt_codec_init_common(mod, BT_CODEC_MSBC); +} + +static int bt_g722_init(struct processing_module *mod) +{ + return bt_codec_init_common(mod, BT_CODEC_G722); +} + +static int bt_sb4_init(struct processing_module *mod) +{ + return bt_codec_init_common(mod, BT_CODEC_SB4); +} + +static int bt_codec_free(struct processing_module *mod) +{ + struct bt_codec_comp_data *cd = module_get_private_data(mod); + + mod_free(mod, cd); + return 0; +} + +static void bt_codec_params(struct processing_module *mod, + struct sof_source *source, struct sof_sink *sink) +{ + struct sof_ipc_stream_params *params = mod->stream_params; + struct comp_dev *dev = mod->dev; + + ipc4_base_module_cfg_to_stream_params(&mod->priv.cfg.base_cfg, params); + + if (mod->priv.cfg.nb_input_pins == 1 && mod->priv.cfg.nb_output_pins == 1) { + ipc4_update_source_format(source, &mod->priv.cfg.input_pins[0].audio_fmt); + ipc4_update_sink_format(sink, &mod->priv.cfg.output_pins[0].audio_fmt); + } else { + ipc4_update_source_format(source, &mod->priv.cfg.base_cfg.audio_fmt); + ipc4_update_sink_format(sink, &mod->priv.cfg.base_cfg.audio_fmt); + } + + component_set_nearest_period_frames(dev, sink_get_rate(sink)); +} + +static int bt_codec_prepare(struct processing_module *mod, + struct sof_source **sources, int num_of_sources, + struct sof_sink **sinks, int num_of_sinks) +{ + struct bt_codec_comp_data *cd = module_get_private_data(mod); + struct comp_dev *dev = mod->dev; + enum sof_ipc_frame in_fmt, out_fmt; + uint32_t in_ch, out_ch, in_rate, out_rate; + bool is_enc; + + if (num_of_sources != 1 || num_of_sinks != 1) + return -EINVAL; + + bt_codec_params(mod, sources[0], sinks[0]); + + in_fmt = source_get_frm_fmt(sources[0]); + out_fmt = sink_get_frm_fmt(sinks[0]); + in_ch = source_get_channels(sources[0]); + out_ch = sink_get_channels(sinks[0]); + in_rate = source_get_rate(sources[0]); + out_rate = sink_get_rate(sinks[0]); + + if (in_ch != 1 || out_ch != 1) { + comp_err(dev, "bt_codec invalid channels in=%u out=%u (expected mono)", + in_ch, out_ch); + return -EINVAL; + } + + if (in_fmt != out_fmt || + (in_fmt != SOF_IPC_FRAME_S16_LE && in_fmt != SOF_IPC_FRAME_S32_LE)) { + comp_err(dev, "bt_codec unsupported frame fmt in=%d out=%d", + (int)in_fmt, (int)out_fmt); + return -EINVAL; + } + + is_enc = (dev->direction == SOF_IPC_STREAM_PLAYBACK); + cd->pcm_rate = is_enc ? in_rate : out_rate; + cd->wire_rate = is_enc ? out_rate : in_rate; + + if (cd->wire_rate != 8000 || + (cd->pcm_rate != 8000 && cd->pcm_rate != 16000)) { + comp_err(dev, "bt_codec invalid rates in=%u out=%u (pcm=%u wire=%u)", + in_rate, out_rate, cd->pcm_rate, cd->wire_rate); + return -EINVAL; + } + + cd->frame_format = in_fmt; + cd->pcm_step = (cd->pcm_rate == 16000) ? 2u : 1u; + cd->frame_bytes = source_get_frame_bytes(sources[0]); + + bt_codec_state_init(&cd->state, cd->codec_type, is_enc, cd->pcm_rate); + + comp_info(dev, "bt_codec prepare: type=%d enc=%d pcm_rate=%u wire_rate=%u fmt=%d", + (int)cd->codec_type, (int)is_enc, cd->pcm_rate, cd->wire_rate, + (int)cd->frame_format); + return 0; +} + +static int bt_codec_reset(struct processing_module *mod) +{ + struct bt_codec_comp_data *cd = module_get_private_data(mod); + bool is_enc = (mod->dev->direction == SOF_IPC_STREAM_PLAYBACK); + + bt_codec_state_init(&cd->state, cd->codec_type, is_enc, cd->pcm_rate); + return 0; +} + +static int bt_codec_process_s16(struct bt_codec_comp_data *cd, + struct sof_source *source, struct sof_sink *sink, + uint32_t wire_frames, size_t src_bytes, size_t sink_bytes) +{ + const int16_t *x, *x_start, *x_end; + int16_t *y, *y_start, *y_end; + size_t x_size, y_size; + uint32_t w, s; + int ret; + + ret = source_get_data_s16(source, src_bytes, &x, &x_start, &x_size); + if (ret) + return ret; + + ret = sink_get_buffer_s16(sink, sink_bytes, &y, &y_start, &y_size); + if (ret) { + source_release_data(source, 0); + return ret; + } + + x_end = x_start + x_size; + y_end = y_start + y_size; + + if (cd->state.is_encoder) { + for (w = 0; w < wire_frames; w++) { + int16_t pcm[2]; + + for (s = 0; s < cd->pcm_step; s++) { + pcm[s] = *x++; + if (x >= x_end) + x = x_start; + } + *y++ = bt_codec_process_enc_step(&cd->state, pcm); + if (y >= y_end) + y = y_start; + } + } else { + for (w = 0; w < wire_frames; w++) { + int16_t cw16 = *x++; + int16_t pcm[2]; + + if (x >= x_end) + x = x_start; + + bt_codec_process_dec_step(&cd->state, cw16, pcm); + for (s = 0; s < cd->pcm_step; s++) { + *y++ = pcm[s]; + if (y >= y_end) + y = y_start; + } + } + } + + source_release_data(source, src_bytes); + sink_commit_buffer(sink, sink_bytes); + return 0; +} + +static int bt_codec_process_s32(struct bt_codec_comp_data *cd, + struct sof_source *source, struct sof_sink *sink, + uint32_t wire_frames, size_t src_bytes, size_t sink_bytes) +{ + const int32_t *x, *x_start, *x_end; + int32_t *y, *y_start, *y_end; + size_t x_size, y_size; + uint32_t w, s; + int ret; + + ret = source_get_data_s32(source, src_bytes, &x, &x_start, &x_size); + if (ret) + return ret; + + ret = sink_get_buffer_s32(sink, sink_bytes, &y, &y_start, &y_size); + if (ret) { + source_release_data(source, 0); + return ret; + } + + x_end = x_start + x_size; + y_end = y_start + y_size; + + if (cd->state.is_encoder) { + for (w = 0; w < wire_frames; w++) { + int16_t pcm[2]; + int16_t cw16; + + for (s = 0; s < cd->pcm_step; s++) { + pcm[s] = sat_int16(Q_SHIFT_RND(*x++, 31, 15)); + if (x >= x_end) + x = x_start; + } + cw16 = bt_codec_process_enc_step(&cd->state, pcm); + *y++ = (int32_t)cw16 << 16; + if (y >= y_end) + y = y_start; + } + } else { + for (w = 0; w < wire_frames; w++) { + int16_t cw16 = (int16_t)((uint32_t)*x++ >> 16); + int16_t pcm[2]; + + if (x >= x_end) + x = x_start; + + bt_codec_process_dec_step(&cd->state, cw16, pcm); + for (s = 0; s < cd->pcm_step; s++) { + *y++ = (int32_t)pcm[s] << 16; + if (y >= y_end) + y = y_start; + } + } + } + + source_release_data(source, src_bytes); + sink_commit_buffer(sink, sink_bytes); + return 0; +} + +static int bt_codec_process(struct processing_module *mod, + struct sof_source **sources, int num_of_sources, + struct sof_sink **sinks, int num_of_sinks) +{ + struct bt_codec_comp_data *cd = module_get_private_data(mod); + struct sof_source *source = sources[0]; + struct sof_sink *sink = sinks[0]; + uint32_t src_frames = source_get_data_frames_available(source); + uint32_t sink_frames = sink_get_free_frames(sink); + uint32_t wire_frames; + size_t src_bytes, sink_bytes; + + (void)num_of_sources; + (void)num_of_sinks; + + if (cd->state.is_encoder) { + wire_frames = MIN(src_frames / cd->pcm_step, sink_frames); + src_bytes = wire_frames * cd->pcm_step * cd->frame_bytes; + sink_bytes = wire_frames * cd->frame_bytes; + } else { + wire_frames = MIN(src_frames, sink_frames / cd->pcm_step); + src_bytes = wire_frames * cd->frame_bytes; + sink_bytes = wire_frames * cd->pcm_step * cd->frame_bytes; + } + + if (!wire_frames) + return 0; + + if (cd->frame_format == SOF_IPC_FRAME_S16_LE) + return bt_codec_process_s16(cd, source, sink, wire_frames, + src_bytes, sink_bytes); + + if (cd->frame_format == SOF_IPC_FRAME_S32_LE) + return bt_codec_process_s32(cd, source, sink, wire_frames, + src_bytes, sink_bytes); + + return -EINVAL; +} + +static const struct module_interface bt_lc3_interface = { + .init = bt_lc3_init, + .prepare = bt_codec_prepare, + .process = bt_codec_process, + .reset = bt_codec_reset, + .free = bt_codec_free, +}; + +static const struct module_interface bt_msbc_interface = { + .init = bt_msbc_init, + .prepare = bt_codec_prepare, + .process = bt_codec_process, + .reset = bt_codec_reset, + .free = bt_codec_free, +}; + +static const struct module_interface bt_g722_interface = { + .init = bt_g722_init, + .prepare = bt_codec_prepare, + .process = bt_codec_process, + .reset = bt_codec_reset, + .free = bt_codec_free, +}; + +static const struct module_interface bt_sb4_interface = { + .init = bt_sb4_init, + .prepare = bt_codec_prepare, + .process = bt_codec_process, + .reset = bt_codec_reset, + .free = bt_codec_free, +}; + +DECLARE_TR_CTX(bt_lc3_tr, SOF_UUID(bt_lc3_uuid), LOG_LEVEL_INFO); +DECLARE_MODULE_ADAPTER(bt_lc3_interface, bt_lc3_uuid, bt_lc3_tr); +SOF_MODULE_INIT(bt_lc3, sys_comp_module_bt_lc3_interface_init); + +DECLARE_TR_CTX(bt_msbc_tr, SOF_UUID(bt_msbc_uuid), LOG_LEVEL_INFO); +DECLARE_MODULE_ADAPTER(bt_msbc_interface, bt_msbc_uuid, bt_msbc_tr); +SOF_MODULE_INIT(bt_msbc, sys_comp_module_bt_msbc_interface_init); + +DECLARE_TR_CTX(bt_g722_tr, SOF_UUID(bt_g722_uuid), LOG_LEVEL_INFO); +DECLARE_MODULE_ADAPTER(bt_g722_interface, bt_g722_uuid, bt_g722_tr); +SOF_MODULE_INIT(bt_g722, sys_comp_module_bt_g722_interface_init); + +DECLARE_TR_CTX(bt_sb4_tr, SOF_UUID(bt_sb4_uuid), LOG_LEVEL_INFO); +DECLARE_MODULE_ADAPTER(bt_sb4_interface, bt_sb4_uuid, bt_sb4_tr); +SOF_MODULE_INIT(bt_sb4, sys_comp_module_bt_sb4_interface_init); diff --git a/src/audio/bt_codec/bt_codec.toml b/src/audio/bt_codec/bt_codec.toml new file mode 100644 index 000000000000..e638d4a2a901 --- /dev/null +++ b/src/audio/bt_codec/bt_codec.toml @@ -0,0 +1,71 @@ +#ifndef LOAD_TYPE +#define LOAD_TYPE "0" +#endif + + REM # BT LC3 Transparent codec module config + [[module.entry]] + name = "BT_LC3" + uuid = UUIDREG_STR_BT_LC3 + affinity_mask = "0x1" + instance_count = "8" + domain_types = "0" + load_type = LOAD_TYPE + module_type = "9" + init_config = "1" + auto_start = "0" + sched_caps = [1, 0x00008000] + pin = [0, 0, 0xfeef, 0xf, 0xf, 0x45ff, 1, 0, 0xfeef, 0xf, 0xf, 0x1ff] + mod_cfg = [0, 0, 0, 0, 16384, 1000000, 256, 256, 0, 0, 0] + + index = __COUNTER__ + + REM # BT mSBC Transparent codec module config + [[module.entry]] + name = "BT_MSBC" + uuid = UUIDREG_STR_BT_MSBC + affinity_mask = "0x1" + instance_count = "8" + domain_types = "0" + load_type = LOAD_TYPE + module_type = "9" + init_config = "1" + auto_start = "0" + sched_caps = [1, 0x00008000] + pin = [0, 0, 0xfeef, 0xf, 0xf, 0x45ff, 1, 0, 0xfeef, 0xf, 0xf, 0x1ff] + mod_cfg = [0, 0, 0, 0, 16384, 1000000, 256, 256, 0, 0, 0] + + index = __COUNTER__ + + REM # BT G722 Transparent codec module config + [[module.entry]] + name = "BT_G722" + uuid = UUIDREG_STR_BT_G722 + affinity_mask = "0x1" + instance_count = "8" + domain_types = "0" + load_type = LOAD_TYPE + module_type = "9" + init_config = "1" + auto_start = "0" + sched_caps = [1, 0x00008000] + pin = [0, 0, 0xfeef, 0xf, 0xf, 0x45ff, 1, 0, 0xfeef, 0xf, 0xf, 0x1ff] + mod_cfg = [0, 0, 0, 0, 16384, 1000000, 256, 256, 0, 0, 0] + + index = __COUNTER__ + + REM # BT SB4 Transparent codec module config + [[module.entry]] + name = "BT_SB4" + uuid = UUIDREG_STR_BT_SB4 + affinity_mask = "0x1" + instance_count = "8" + domain_types = "0" + load_type = LOAD_TYPE + module_type = "9" + init_config = "1" + auto_start = "0" + sched_caps = [1, 0x00008000] + pin = [0, 0, 0xfeef, 0xf, 0xf, 0x45ff, 1, 0, 0xfeef, 0xf, 0xf, 0x1ff] + mod_cfg = [0, 0, 0, 0, 16384, 1000000, 256, 256, 0, 0, 0] + + index = __COUNTER__ diff --git a/src/audio/bt_codec/bt_codec_algos.c b/src/audio/bt_codec/bt_codec_algos.c new file mode 100644 index 000000000000..a480fa18c20f --- /dev/null +++ b/src/audio/bt_codec/bt_codec_algos.c @@ -0,0 +1,1233 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Intel Corporation. All rights reserved. +// +// Bluetooth Transparent Mode (0x03) DSP Codec Algorithms for SOF: +// 1. BT_CODEC_LC3 - Zephyr liblc3 (7.5 ms @ 16 kHz WB / 8 kHz NB) +// 2. BT_CODEC_MSBC - Bluetooth SIG HFP v1.8 / A2DP v1.3 H2-framed mSBC +// 3. BT_CODEC_G722 - ITU-T Recommendation G.722 SB-ADPCM +// 4. BT_CODEC_SB4 - Experimental 4-band 24-tap tree-QMF subband ADPCM + +#include "bt_codec_algos.h" +#include + +/* ======================================================================== + * Shared Cache-Line-Aligned CRC-8 (0x1D) and 24-Tap QMF Tables + * ======================================================================== + */ +const uint8_t bt_crc8_poly1d_table[256] __aligned(PLATFORM_DCACHE_ALIGN) = { + 0x00, 0x1d, 0x3a, 0x27, 0x74, 0x69, 0x4e, 0x53, + 0xe8, 0xf5, 0xd2, 0xcf, 0x9c, 0x81, 0xa6, 0xbb, + 0xcd, 0xd0, 0xf7, 0xea, 0xb9, 0xa4, 0x83, 0x9e, + 0x25, 0x38, 0x1f, 0x02, 0x51, 0x4c, 0x6b, 0x76, + 0x87, 0x9a, 0xbd, 0xa0, 0xf3, 0xee, 0xc9, 0xd4, + 0x6f, 0x72, 0x55, 0x48, 0x1b, 0x06, 0x21, 0x3c, + 0x4a, 0x57, 0x70, 0x6d, 0x3e, 0x23, 0x04, 0x19, + 0xa2, 0xbf, 0x98, 0x85, 0xd6, 0xcb, 0xec, 0xf1, + 0x13, 0x0e, 0x29, 0x34, 0x67, 0x7a, 0x5d, 0x40, + 0xfb, 0xe6, 0xc1, 0xdc, 0x8f, 0x92, 0xb5, 0xa8, + 0xde, 0xc3, 0xe4, 0xf9, 0xaa, 0xb7, 0x90, 0x8d, + 0x36, 0x2b, 0x0c, 0x11, 0x42, 0x5f, 0x78, 0x65, + 0x94, 0x89, 0xae, 0xb3, 0xe0, 0xfd, 0xda, 0xc7, + 0x7c, 0x61, 0x46, 0x5b, 0x08, 0x15, 0x32, 0x2f, + 0x59, 0x44, 0x63, 0x7e, 0x2d, 0x30, 0x17, 0x0a, + 0xb1, 0xac, 0x8b, 0x96, 0xc5, 0xd8, 0xff, 0xe2, + 0x26, 0x3b, 0x1c, 0x01, 0x52, 0x4f, 0x68, 0x75, + 0xce, 0xd3, 0xf4, 0xe9, 0xba, 0xa7, 0x80, 0x9d, + 0xeb, 0xf6, 0xd1, 0xcc, 0x9f, 0x82, 0xa5, 0xb8, + 0x03, 0x1e, 0x39, 0x24, 0x77, 0x6a, 0x4d, 0x50, + 0xa1, 0xbc, 0x9b, 0x86, 0xd5, 0xc8, 0xef, 0xf2, + 0x49, 0x54, 0x73, 0x6e, 0x3d, 0x20, 0x07, 0x1a, + 0x6c, 0x71, 0x56, 0x4b, 0x18, 0x05, 0x22, 0x3f, + 0x84, 0x99, 0xbe, 0xa3, 0xf0, 0xed, 0xca, 0xd7, + 0x35, 0x28, 0x0f, 0x12, 0x41, 0x5c, 0x7b, 0x66, + 0xdd, 0xc0, 0xe7, 0xfa, 0xa9, 0xb4, 0x93, 0x8e, + 0xf8, 0xe5, 0xc2, 0xdf, 0x8c, 0x91, 0xb6, 0xab, + 0x10, 0x0d, 0x2a, 0x37, 0x64, 0x79, 0x5e, 0x43, + 0xb2, 0xaf, 0x88, 0x95, 0xc6, 0xdb, 0xfc, 0xe1, + 0x5a, 0x47, 0x60, 0x7d, 0x2e, 0x33, 0x14, 0x09, + 0x7f, 0x62, 0x45, 0x58, 0x0b, 0x16, 0x31, 0x2c, + 0x97, 0x8a, 0xad, 0xb0, 0xe3, 0xfe, 0xd9, 0xc4, +}; + +/* ITU-T Rec. G.722 24-tap symmetric quadrature mirror filter (half-symmetric 12 taps) */ +static const int16_t bt_qmf24_coeffs[12] __aligned(PLATFORM_DCACHE_ALIGN) = { + 3, -11, 12, 32, -210, 951, 3876, -805, 362, -156, 53, -11, +}; + +static inline void bt_qmf24_tx(int32_t *x, int32_t s0, int32_t s1, + int32_t *out_low, int32_t *out_high, int shift) +{ + int32_t sumeven = 0; + int32_t sumodd = 0; + int i; + + for (i = 0; i < 22; i++) + x[i] = x[i + 2]; + x[22] = s0; + x[23] = s1; + + for (i = 0; i < 12; i++) { + sumodd += x[2 * i] * bt_qmf24_coeffs[i]; + sumeven += x[2 * i + 1] * bt_qmf24_coeffs[11 - i]; + } + + *out_low = (sumeven + sumodd) >> shift; + *out_high = (sumeven - sumodd) >> shift; +} + +static inline void bt_qmf24_rx(int32_t *x, int32_t rlow, int32_t rhigh, + int32_t *out0, int32_t *out1, int shift) +{ + int32_t xout1 = 0; + int32_t xout2 = 0; + int i; + + for (i = 0; i < 22; i++) + x[i] = x[i + 2]; + x[22] = rlow + rhigh; + x[23] = rlow - rhigh; + + for (i = 0; i < 12; i++) { + xout2 += x[2 * i] * bt_qmf24_coeffs[i]; + xout1 += x[2 * i + 1] * bt_qmf24_coeffs[11 - i]; + } + + *out0 = xout1 >> shift; + *out1 = xout2 >> shift; +} + +/* ======================================================================== + * 1. SOF LC3 (Zephyr liblc3, 7.5 ms @ 16 kHz WB or 8 kHz NB) + * ======================================================================== + */ +void bt_lc3_encode_block(struct bt_codec_state *st, const int16_t *pcm_in, uint8_t *pkt_out) +{ + pkt_out[0] = BT_LC3_SYNC_0; + pkt_out[1] = BT_LC3_SYNC_1; + lc3_encode(st->u.lc3.enc, LC3_PCM_FORMAT_S16, pcm_in, 1, + BT_LC3_PAYLOAD_BYTES, pkt_out + 2); + pkt_out[59] = bt_frame_crc8(pkt_out, 59); +} + +void bt_lc3_decode_block(struct bt_codec_state *st, const uint8_t *pkt_in, int16_t *pcm_out) +{ + lc3_decode(st->u.lc3.dec, pkt_in ? (pkt_in + 2) : NULL, + pkt_in ? BT_LC3_PAYLOAD_BYTES : 0, + LC3_PCM_FORMAT_S16, pcm_out, 1); +} + +/* ======================================================================== + * 2. SOF mSBC (Bluetooth SIG HFP v1.8 / A2DP v1.3 80-tap Filterbank mSBC) + * ======================================================================== + */ +#define BT_MSBC_BLOCKS 15 +#define BT_MSBC_SUBBANDS 8 +#define BT_MSBC_BITPOOL 26 + +static const uint8_t bt_msbc_h2_sn[4] __aligned(PLATFORM_DCACHE_ALIGN) = { + 0x08, 0x38, 0xC8, 0xF8, +}; + +/* A2DP v1.3 Section 12.6.3.1 LOUDNESS offset8 table for 16 kHz */ +static const int8_t bt_msbc_offset8_16k[8] __aligned(PLATFORM_DCACHE_ALIGN) = { + -2, 0, 0, 0, 0, 0, 0, 1, +}; + +/* 1 / (2^nbits - 1) in Q28 for division-free subband dequantization (nbits = 1..16) */ +static const int32_t bt_msbc_recip_q28[16] __aligned(PLATFORM_DCACHE_ALIGN) = { + 0x0FFFFFFF, 0x05555556, 0x02492492, 0x01111111, + 0x00842108, 0x00410410, 0x00204081, 0x00101010, + 0x00080402, 0x00040100, 0x00020040, 0x00010010, + 0x00008004, 0x00004001, 0x00002000, 0x00001000, +}; + +/* A2DP v1.3 Table 12.30 80-tap prototype window Proto_8_80 in Q16 (synthesis D = -C in Q13) */ +static const int16_t bt_msbc_proto80[80] __aligned(PLATFORM_DCACHE_ALIGN) = { + 0, 10, 22, 36, 54, 75, 97, 117, + 132, 138, 131, 106, 59, -12, -108, -229, + 371, 526, 685, 835, 960, 1042, 1063, 1004, + 848, 580, 192, -322, -959, -1711, -2561, -3486, + 4456, 5438, 6395, 7287, 8078, 8734, 9224, 9528, + 9631, 9528, 9224, 8734, 8078, 7287, 6395, 5438, + -4456, -3486, -2561, -1711, -959, -322, 192, 580, + 848, 1004, 1063, 1042, 960, 835, 685, 526, + -371, -229, -108, -12, 59, 106, 131, 138, + 132, 117, 97, 75, 54, 36, 22, 10, +}; + +/* Analysis matrix M[m][j] = round(cos((m + 0.5) * (j - 4) * pi / 8) * 8192) */ +static const int16_t bt_msbc_ana_mat[8][16] __aligned(PLATFORM_DCACHE_ALIGN) = { + { 5793, 6811, 7568, 8035, 8192, 8035, 7568, 6811, + 5793, 4551, 3135, 1598, 0, -1598, -3135, -4551 }, + { -5793, -1598, 3135, 6811, 8192, 6811, 3135, -1598, + -5793, -8035, -7568, -4551, 0, 4551, 7568, 8035 }, + { -5793, -8035, -3135, 4551, 8192, 4551, -3135, -8035, + -5793, 1598, 7568, 6811, 0, -6811, -7568, -1598 }, + { 5793, -4551, -7568, 1598, 8192, 1598, -7568, -4551, + 5793, 6811, -3135, -8035, 0, 8035, 3135, -6811 }, + { 5793, 4551, -7568, -1598, 8192, -1598, -7568, 4551, + 5793, -6811, -3135, 8035, 0, -8035, 3135, 6811 }, + { -5793, 8035, -3135, -4551, 8192, -4551, -3135, 8035, + -5793, -1598, 7568, -6811, 0, 6811, -7568, 1598 }, + { -5793, 1598, 3135, -6811, 8192, -6811, 3135, 1598, + -5793, 8035, -7568, 4551, 0, -4551, 7568, -8035 }, + { 5793, -6811, 7568, -8035, 8192, -8035, 7568, -6811, + 5793, -4551, 3135, -1598, 0, 1598, -3135, 4551 }, +}; + +/* Synthesis matrix N[k][m] = round(cos((m + 0.5) * (k + 4) * pi / 8) * 8192) */ +static const int16_t bt_msbc_syn_mat[16][8] __aligned(PLATFORM_DCACHE_ALIGN) = { + { 5793, -5793, -5793, 5793, 5793, -5793, -5793, 5793 }, + { 4551, -8035, 1598, 6811, -6811, -1598, 8035, -4551 }, + { 3135, -7568, 7568, -3135, -3135, 7568, -7568, 3135 }, + { 1598, -4551, 6811, -8035, 8035, -6811, 4551, -1598 }, + { 0, 0, 0, 0, 0, 0, 0, 0 }, + { -1598, 4551, -6811, 8035, -8035, 6811, -4551, 1598 }, + { -3135, 7568, -7568, 3135, 3135, -7568, 7568, -3135 }, + { -4551, 8035, -1598, -6811, 6811, 1598, -8035, 4551 }, + { -5793, 5793, 5793, -5793, -5793, 5793, 5793, -5793 }, + { -6811, 1598, 8035, 4551, -4551, -8035, -1598, 6811 }, + { -7568, -3135, 3135, 7568, 7568, 3135, -3135, -7568 }, + { -8035, -6811, -4551, -1598, 1598, 4551, 6811, 8035 }, + { -8192, -8192, -8192, -8192, -8192, -8192, -8192, -8192 }, + { -8035, -6811, -4551, -1598, 1598, 4551, 6811, 8035 }, + { -7568, -3135, 3135, 7568, 7568, 3135, -3135, -7568 }, + { -6811, 1598, 8035, 4551, -4551, -8035, -1598, 6811 }, +}; + +/* A2DP v1.3 Section 12.6.3.1 LOUDNESS bit allocation for mSBC (8 subbands, bitpool = 26) */ +static void bt_msbc_compute_bits(const uint8_t scf[8], uint8_t nbits[8]) +{ + int bitneeds[8]; + int max_bitneed = -5; + int bitcount = 0; + int bitslice; + int bc = 0; + int isb; + + for (isb = 0; isb < BT_MSBC_SUBBANDS; isb++) { + int bn = scf[isb] ? ((int)scf[isb] - (int)bt_msbc_offset8_16k[isb]) : -5; + + if (bn > 0) + bn >>= 1; + if (bn > max_bitneed) + max_bitneed = bn; + bitneeds[isb] = bn; + } + + bitslice = max_bitneed + 1; + while (bc < BT_MSBC_BITPOOL) { + int bs = bitslice--; + + bitcount = bc; + if (bitcount == BT_MSBC_BITPOOL) + break; + for (isb = 0; isb < BT_MSBC_SUBBANDS; isb++) { + int bn = bitneeds[isb]; + + if (bn >= bs && bn < bs + 15) + bc++; + if (bn == bs) + bc++; + } + } + + for (isb = 0; isb < BT_MSBC_SUBBANDS; isb++) { + int nb = bitneeds[isb] - bitslice; + + nbits[isb] = (uint8_t)(nb < 2 ? 0 : (nb > 16 ? 16 : nb)); + } + + for (isb = 0; isb < BT_MSBC_SUBBANDS && bitcount < BT_MSBC_BITPOOL; isb++) { + int add = 0; + + if (nbits[isb] > 0 && nbits[isb] < 16) + add = 1; + else if (bitneeds[isb] == bitslice + 1 && BT_MSBC_BITPOOL > bitcount + 1) + add = 2; + nbits[isb] = (uint8_t)(nbits[isb] + add); + bitcount += add; + } + + for (isb = 0; isb < BT_MSBC_SUBBANDS && bitcount < BT_MSBC_BITPOOL; isb++) { + if (nbits[isb] < 16) { + nbits[isb]++; + bitcount++; + } + } +} + +static void bt_msbc_analyze_8(int16_t x[80], const int16_t pcm[8], int16_t sb_out[8]) +{ + int32_t y[16]; + int i, j, m; + + memmove(&x[8], &x[0], 72 * sizeof(int16_t)); + for (i = 0; i < 8; i++) + x[7 - i] = pcm[i]; + + for (j = 0; j < 16; j++) { + int32_t acc = 0; + + for (i = 0; i < 5; i++) + acc += (int32_t)x[j + 16 * i] * (int32_t)bt_msbc_proto80[j + 16 * i]; + y[j] = acc >> 16; + } + + for (m = 0; m < 8; m++) { + int32_t acc = 0; + + for (j = 0; j < 16; j++) + acc += y[j] * (int32_t)bt_msbc_ana_mat[m][j]; + sb_out[m] = sat_int16((acc + 4096) >> 13); + } +} + +static void bt_msbc_synthesize_8(int32_t v[160], const int32_t sb_in[8], int16_t pcm_out[8]) +{ + int32_t u[80]; + int i, j, k, m; + + memmove(&v[16], &v[0], 144 * sizeof(int32_t)); + for (k = 0; k < 16; k++) { + int32_t acc = 0; + + for (m = 0; m < 8; m++) + acc += sb_in[m] * (int32_t)bt_msbc_syn_mat[k][m]; + v[k] = acc >> 13; + } + + for (i = 0; i < 5; i++) { + for (j = 0; j < 8; j++) { + u[16 * i + j] = v[32 * i + j]; + u[16 * i + 8 + j] = v[32 * i + 24 + j]; + } + } + + for (j = 0; j < 8; j++) { + int32_t acc = 0; + + for (i = 0; i < 10; i++) + acc += u[j + 8 * i] * (-(int32_t)bt_msbc_proto80[j + 8 * i]); + pcm_out[j] = sat_int16((acc + 4096) >> 13); + } +} + +void bt_msbc_encode_block(struct bt_codec_state *st, const int16_t *pcm_in, uint8_t *pkt_out) +{ + int16_t pcm16k_buf[BT_TRANSP_WB_SAMPLES]; + const int16_t *pcm16k = pcm_in; + int16_t sb[BT_MSBC_BLOCKS][BT_MSBC_SUBBANDS]; + uint8_t scf[BT_MSBC_SUBBANDS]; + uint8_t nbits[BT_MSBC_SUBBANDS]; + uint32_t bit_pos = 0; + int blk, band, k; + + if (st->pcm_rate == 8000) { + for (k = 0; k < BT_TRANSP_WIRE_SAMPLES; k++) { + int32_t s0, s1; + + bt_qmf24_rx(st->u.msbc.qmf_x, (int32_t)pcm_in[k] >> 1, 0, &s0, &s1, 11); + pcm16k_buf[2 * k] = st->u.msbc.qmf_prev; + pcm16k_buf[2 * k + 1] = sat_int16(s0); + st->u.msbc.qmf_prev = sat_int16(s1); + } + pcm16k = pcm16k_buf; + } + + for (blk = 0; blk < BT_MSBC_BLOCKS; blk++) + bt_msbc_analyze_8(st->u.msbc.enc_x, &pcm16k[blk * 8], sb[blk]); + + for (band = 0; band < BT_MSBC_SUBBANDS; band++) { + uint32_t mag = 0; + uint8_t s = 0; + + for (blk = 0; blk < BT_MSBC_BLOCKS; blk++) { + int32_t v = sb[blk][band]; + + mag |= (uint32_t)(v < 0 ? ~v : v); + } + while (s < 15 && (mag >> (s + 1)) > 0) + s++; + scf[band] = s; + } + + bt_msbc_compute_bits(scf, nbits); + + /* Official HFP v1.8 Transparent mSBC 60-byte packet: + * [0..1] H2 synchronization header (0x01, SN) + * [2..58] 57-byte mSBC frame (0xAD, 0x00, 0x00, CRC8, scf[4], audio[49]) + * [59] 0x00 trailing HFP padding byte + */ + memset(pkt_out, 0, BT_TRANSP_PACKET_BYTES); + pkt_out[0] = 0x01; + pkt_out[1] = bt_msbc_h2_sn[st->frame_seq & 3u]; + st->frame_seq++; + + pkt_out[2] = 0xAD; + pkt_out[3] = 0x00; + pkt_out[4] = 0x00; + for (band = 0; band < 4; band++) + pkt_out[6 + band] = (uint8_t)((scf[2 * band] << 4) | (scf[2 * band + 1] & 0x0F)); + pkt_out[5] = bt_msbc_crc8(&pkt_out[3], &pkt_out[6]); + + for (blk = 0; blk < BT_MSBC_BLOCKS; blk++) { + for (band = 0; band < BT_MSBC_SUBBANDS; band++) { + uint8_t nb = nbits[band]; + int32_t levels, q; + + if (!nb) + continue; + levels = (1 << nb) - 1; + q = ((((int32_t)sb[blk][band] * levels) >> (scf[band] + 1)) + levels) >> 1; + if (q < 0) + q = 0; + if (q > levels) + q = levels; + bt_put_bits(pkt_out + 10, &bit_pos, (uint32_t)q, nb); + } + } +} + +void bt_msbc_decode_block(struct bt_codec_state *st, const uint8_t *pkt_in, int16_t *pcm_out) +{ + int16_t pcm16k_buf[BT_TRANSP_WB_SAMPLES]; + int16_t *dst16k = (st->pcm_rate == 16000) ? pcm_out : pcm16k_buf; + uint8_t scf[BT_MSBC_SUBBANDS]; + uint8_t nbits[BT_MSBC_SUBBANDS]; + uint32_t bit_pos = 0; + int blk, band, k; + + for (band = 0; band < 4; band++) { + scf[2 * band] = (pkt_in[6 + band] >> 4) & 0x0F; + scf[2 * band + 1] = pkt_in[6 + band] & 0x0F; + } + + bt_msbc_compute_bits(scf, nbits); + + for (blk = 0; blk < BT_MSBC_BLOCKS; blk++) { + int32_t sb[BT_MSBC_SUBBANDS]; + + for (band = 0; band < BT_MSBC_SUBBANDS; band++) { + uint8_t nb = nbits[band]; + + if (!nb) { + sb[band] = 0; + } else { + int32_t q = (int32_t)bt_get_bits(pkt_in + 10, &bit_pos, nb); + int64_t scaled = (int64_t)((q << 1) | 1) * + (int64_t)bt_msbc_recip_q28[nb - 1]; + int shr = 28 - ((int)scf[band] + 1); + + sb[band] = (int32_t)((scaled - (1LL << 28)) >> shr); + } + } + bt_msbc_synthesize_8(st->u.msbc.dec_v, sb, &dst16k[blk * 8]); + } + + if (st->pcm_rate == 8000) { + for (k = 0; k < BT_TRANSP_WIRE_SAMPLES; k++) { + int32_t xlow, xhigh; + + bt_qmf24_tx(st->u.msbc.qmf_x, pcm16k_buf[2 * k], + pcm16k_buf[2 * k + 1], &xlow, &xhigh, 14); + pcm_out[k] = sat_int16(xlow << 1); + } + } +} + +/* ======================================================================== + * 3. SOF G.722 (ITU-T Recommendation G.722 SB-ADPCM) + * - Raw Mode 1/2/3 (64/56/48 kbps) + Framed Transparent Mode 2 (56 kbps) + * ======================================================================== + */ +static const int16_t bt_g722_q6[32] __aligned(PLATFORM_DCACHE_ALIGN) = { + 0, 35, 72, 110, 150, 190, 233, 276, + 323, 370, 422, 473, 530, 587, 650, 714, + 786, 858, 940, 1023, 1121, 1219, 1339, 1458, + 1612, 1765, 1980, 2195, 2557, 2919, 0, 0, +}; + +static const uint8_t bt_g722_iln[32] __aligned(PLATFORM_DCACHE_ALIGN) = { + 0, 63, 62, 31, 30, 29, 28, 27, + 26, 25, 24, 23, 22, 21, 20, 19, + 18, 17, 16, 15, 14, 13, 12, 11, + 10, 9, 8, 7, 6, 5, 4, 0, +}; + +static const uint8_t bt_g722_ilp[32] __aligned(PLATFORM_DCACHE_ALIGN) = { + 0, 61, 60, 59, 58, 57, 56, 55, + 54, 53, 52, 51, 50, 49, 48, 47, + 46, 45, 44, 43, 42, 41, 40, 39, + 38, 37, 36, 35, 34, 33, 32, 0, +}; + +static const int16_t bt_g722_wl[8] __aligned(PLATFORM_DCACHE_ALIGN) = { + -60, -30, 58, 172, 334, 538, 1198, 3042, +}; + +static const uint8_t bt_g722_rl42[16] __aligned(PLATFORM_DCACHE_ALIGN) = { + 0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0, +}; + +static const int16_t bt_g722_ilb[32] __aligned(PLATFORM_DCACHE_ALIGN) = { + 2048, 2093, 2139, 2186, 2233, 2282, 2332, 2383, + 2435, 2489, 2543, 2599, 2656, 2714, 2774, 2834, + 2896, 2960, 3025, 3091, 3158, 3228, 3298, 3371, + 3444, 3520, 3597, 3676, 3756, 3838, 3922, 4008, +}; + +static const int16_t bt_g722_qm4[16] __aligned(PLATFORM_DCACHE_ALIGN) = { + 0, -20456, -12896, -8968, + -6288, -4240, -2584, -1200, + 20456, 12896, 8968, 6288, + 4240, 2584, 1200, 0, +}; + +static const int16_t bt_g722_qm5[32] __aligned(PLATFORM_DCACHE_ALIGN) = { + -280, -280, -23352, -17560, + -14120, -11664, -9752, -8184, + -6864, -5712, -4696, -3784, + -2960, -2208, -1520, -880, + 23352, 17560, 14120, 11664, + 9752, 8184, 6864, 5712, + 4696, 3784, 2960, 2208, + 1520, 880, 280, -280, +}; + +static const int16_t bt_g722_qm6[64] __aligned(PLATFORM_DCACHE_ALIGN) = { + -136, -136, -136, -136, + -24808, -21904, -19008, -16704, + -14984, -13512, -12280, -11192, + -10232, -9360, -8576, -7856, + -7192, -6576, -6000, -5456, + -4944, -4464, -4008, -3576, + -3168, -2776, -2400, -2032, + -1688, -1360, -1040, -728, + 24808, 21904, 19008, 16704, + 14984, 13512, 12280, 11192, + 10232, 9360, 8576, 7856, + 7192, 6576, 6000, 5456, + 4944, 4464, 4008, 3576, + 3168, 2776, 2400, 2032, + 1688, 1360, 1040, 728, + 432, 136, -432, -136, +}; + +static const int16_t bt_g722_qm2[4] __aligned(PLATFORM_DCACHE_ALIGN) = { + -7408, -1616, 7408, 1616, +}; + +static const uint8_t bt_g722_ihn[3] __aligned(PLATFORM_DCACHE_ALIGN) = { 0, 1, 0 }; +static const uint8_t bt_g722_ihp[3] __aligned(PLATFORM_DCACHE_ALIGN) = { 0, 3, 2 }; +static const int16_t bt_g722_wh[3] __aligned(PLATFORM_DCACHE_ALIGN) = { 0, -214, 798 }; +static const uint8_t bt_g722_rh2[4] __aligned(PLATFORM_DCACHE_ALIGN) = { 2, 1, 2, 1 }; + +/* ITU-T G.722 Block 4: 2-pole / 6-zero adaptive predictor update */ +static void bt_g722_block4(struct bt_g722_band *band, int32_t d) +{ + int32_t wd1, wd2, wd3; + int i; + + band->d[0] = d; + band->r[0] = sat_int16(band->s + d); + band->p[0] = sat_int16(band->sz + d); + + for (i = 0; i < 3; i++) + band->sg[i] = band->p[i] >> 15; + + wd1 = sat_int16(band->a[1] << 2); + wd2 = (band->sg[0] == band->sg[1]) ? -wd1 : wd1; + if (wd2 > 32767) + wd2 = 32767; + + wd3 = (wd2 >> 7) + ((band->sg[0] == band->sg[2]) ? 128 : -128); + wd3 += (band->a[2] * 32512) >> 15; + if (wd3 > 12288) + wd3 = 12288; + else if (wd3 < -12288) + wd3 = -12288; + band->ap[2] = wd3; + + band->sg[0] = band->p[0] >> 15; + band->sg[1] = band->p[1] >> 15; + wd1 = (band->sg[0] == band->sg[1]) ? 192 : -192; + wd2 = (band->a[1] * 32640) >> 15; + band->ap[1] = sat_int16(wd1 + wd2); + + wd3 = sat_int16(15360 - band->ap[2]); + if (band->ap[1] > wd3) + band->ap[1] = wd3; + else if (band->ap[1] < -wd3) + band->ap[1] = -wd3; + + wd1 = (d == 0) ? 0 : 128; + band->sg[0] = d >> 15; + for (i = 1; i < 7; i++) { + band->sg[i] = band->d[i] >> 15; + wd2 = (band->sg[i] == band->sg[0]) ? wd1 : -wd1; + wd3 = (band->b[i] * 32640) >> 15; + band->bp[i] = sat_int16(wd2 + wd3); + } + + for (i = 6; i > 0; i--) { + band->d[i] = band->d[i - 1]; + band->b[i] = band->bp[i]; + } + + for (i = 2; i > 0; i--) { + band->r[i] = band->r[i - 1]; + band->p[i] = band->p[i - 1]; + band->a[i] = band->ap[i]; + } + + wd1 = sat_int16(band->r[1] + band->r[1]); + wd1 = (band->a[1] * wd1) >> 15; + wd2 = sat_int16(band->r[2] + band->r[2]); + wd2 = (band->a[2] * wd2) >> 15; + band->sp = sat_int16(wd1 + wd2); + + band->sz = 0; + for (i = 6; i > 0; i--) { + wd1 = sat_int16(band->d[i] + band->d[i]); + band->sz += (band->b[i] * wd1) >> 15; + } + band->sz = sat_int16(band->sz); + band->s = sat_int16(band->sp + band->sz); +} + +static uint8_t bt_g722_encode_pair(struct bt_codec_state *st, int32_t xlow, int32_t xhigh, + bool eight_k, uint8_t bits_per_cw) +{ + struct bt_g722_band *b0 = &st->u.g722.band[0]; + struct bt_g722_band *b1 = &st->u.g722.band[1]; + int32_t el, wd, wd1, wd2, wd3, dlow, eh, dhigh; + uint8_t ilow, ril, il4, mih, ihigh, ih2, code; + int i; + + el = sat_int16(xlow - b0->s); + wd = (el >= 0) ? el : -(el + 1); + for (i = 1; i < 30; i++) { + wd1 = ((int32_t)bt_g722_q6[i] * b0->det) >> 12; + if (wd < wd1) + break; + } + ilow = (el < 0) ? bt_g722_iln[i] : bt_g722_ilp[i]; + + ril = ilow >> 2; + wd2 = bt_g722_qm4[ril]; + dlow = (b0->det * wd2) >> 15; + + il4 = bt_g722_rl42[ril]; + wd = (b0->nb * 127) >> 7; + b0->nb = wd + bt_g722_wl[il4]; + if (b0->nb < 0) + b0->nb = 0; + else if (b0->nb > 18432) + b0->nb = 18432; + + wd1 = (b0->nb >> 6) & 31; + wd2 = 8 - (b0->nb >> 11); + wd3 = (wd2 < 0) ? ((int32_t)bt_g722_ilb[wd1] << -wd2) : + ((int32_t)bt_g722_ilb[wd1] >> wd2); + b0->det = wd3 << 2; + bt_g722_block4(b0, dlow); + + if (eight_k) { + code = (uint8_t)(0xC0u | ilow); + } else { + eh = sat_int16(xhigh - b1->s); + wd = (eh >= 0) ? eh : -(eh + 1); + wd1 = (564 * b1->det) >> 12; + mih = (wd >= wd1) ? 2 : 1; + ihigh = (eh < 0) ? bt_g722_ihn[mih] : bt_g722_ihp[mih]; + + wd2 = bt_g722_qm2[ihigh]; + dhigh = (b1->det * wd2) >> 15; + + ih2 = bt_g722_rh2[ihigh]; + wd = (b1->nb * 127) >> 7; + b1->nb = wd + bt_g722_wh[ih2]; + if (b1->nb < 0) + b1->nb = 0; + else if (b1->nb > 22528) + b1->nb = 22528; + + wd1 = (b1->nb >> 6) & 31; + wd2 = 10 - (b1->nb >> 11); + wd3 = (wd2 < 0) ? ((int32_t)bt_g722_ilb[wd1] << -wd2) : + ((int32_t)bt_g722_ilb[wd1] >> wd2); + b1->det = wd3 << 2; + bt_g722_block4(b1, dhigh); + code = (uint8_t)((ihigh << 6) | ilow); + } + + return code >> (8u - bits_per_cw); +} + +static void bt_g722_decode_pair(struct bt_codec_state *st, uint8_t code, + bool eight_k, uint8_t bits_per_cw, + int32_t *out_rlow, int32_t *out_rhigh) +{ + struct bt_g722_band *b0 = &st->u.g722.band[0]; + struct bt_g722_band *b1 = &st->u.g722.band[1]; + int32_t wd1, wd2, wd3, rlow, dlowt, dhigh, rhigh = 0; + uint8_t ihigh; + + switch (bits_per_cw) { + case 6: + wd1 = code & 0x0F; + ihigh = (code >> 4) & 0x03; + wd2 = bt_g722_qm4[wd1]; + break; + case 7: + wd1 = code & 0x1F; + ihigh = (code >> 5) & 0x03; + wd2 = bt_g722_qm5[wd1]; + wd1 >>= 1; + break; + default: + wd1 = code & 0x3F; + ihigh = (code >> 6) & 0x03; + wd2 = bt_g722_qm6[wd1]; + wd1 >>= 2; + break; + } + + wd2 = (b0->det * wd2) >> 15; + rlow = b0->s + wd2; + if (rlow > 16383) + rlow = 16383; + else if (rlow < -16384) + rlow = -16384; + + wd2 = bt_g722_qm4[wd1]; + dlowt = (b0->det * wd2) >> 15; + + wd2 = bt_g722_rl42[wd1]; + wd1 = ((b0->nb * 127) >> 7) + bt_g722_wl[wd2]; + if (wd1 < 0) + wd1 = 0; + else if (wd1 > 18432) + wd1 = 18432; + b0->nb = wd1; + + wd1 = (b0->nb >> 6) & 31; + wd2 = 8 - (b0->nb >> 11); + wd3 = (wd2 < 0) ? ((int32_t)bt_g722_ilb[wd1] << -wd2) : + ((int32_t)bt_g722_ilb[wd1] >> wd2); + b0->det = wd3 << 2; + bt_g722_block4(b0, dlowt); + + if (!eight_k) { + wd2 = bt_g722_qm2[ihigh]; + dhigh = (b1->det * wd2) >> 15; + rhigh = dhigh + b1->s; + if (rhigh > 16383) + rhigh = 16383; + else if (rhigh < -16384) + rhigh = -16384; + + wd2 = bt_g722_rh2[ihigh]; + wd1 = ((b1->nb * 127) >> 7) + bt_g722_wh[wd2]; + if (wd1 < 0) + wd1 = 0; + else if (wd1 > 22528) + wd1 = 22528; + b1->nb = wd1; + + wd1 = (b1->nb >> 6) & 31; + wd2 = 10 - (b1->nb >> 11); + wd3 = (wd2 < 0) ? ((int32_t)bt_g722_ilb[wd1] << -wd2) : + ((int32_t)bt_g722_ilb[wd1] >> wd2); + b1->det = wd3 << 2; + bt_g722_block4(b1, dhigh); + } + + *out_rlow = rlow; + *out_rhigh = rhigh; +} + +void bt_g722_encode_raw(struct bt_codec_state *st, const int16_t *pcm_in, + uint32_t num_samples, uint8_t *g722_out, uint8_t bits_per_cw) +{ + bool eight_k = (st->pcm_rate == 8000); + uint32_t n = eight_k ? num_samples : (num_samples / 2u); + uint32_t i; + + for (i = 0; i < n; i++) { + int32_t xlow, xhigh = 0; + + if (eight_k) + xlow = (int32_t)pcm_in[i] >> 1; + else + bt_qmf24_tx(st->u.g722.qmf_x, pcm_in[2 * i], pcm_in[2 * i + 1], + &xlow, &xhigh, 14); + + g722_out[i] = bt_g722_encode_pair(st, xlow, xhigh, eight_k, bits_per_cw); + } +} + +void bt_g722_decode_raw(struct bt_codec_state *st, const uint8_t *g722_in, + uint32_t num_codewords, int16_t *pcm_out, uint8_t bits_per_cw) +{ + bool eight_k = (st->pcm_rate == 8000); + uint32_t i; + + for (i = 0; i < num_codewords; i++) { + int32_t rlow, rhigh; + + bt_g722_decode_pair(st, g722_in[i], eight_k, bits_per_cw, &rlow, &rhigh); + if (eight_k) { + pcm_out[i] = sat_int16(rlow << 1); + } else { + int32_t s0, s1; + + bt_qmf24_rx(st->u.g722.qmf_x, rlow, rhigh, &s0, &s1, 11); + pcm_out[2 * i] = sat_int16(s0); + pcm_out[2 * i + 1] = sat_int16(s1); + } + } +} + +/* Framed Bluetooth Transparent G.722 60-byte packet: + * [0..1] Sync word (0x72, 0x27) + * [2..54] 60 ITU-T G.722 Mode-2 (56 kbps, 7-bit) subband codewords (420 bits) + * [55..58] Zero padding + * [59] CRC-8 (0x1D) + */ +void bt_g722_encode_block(struct bt_codec_state *st, const int16_t *pcm_in, uint8_t *pkt_out) +{ + uint8_t cw[BT_TRANSP_WIRE_SAMPLES]; + uint32_t bit_pos = 0; + int i; + + memset(pkt_out, 0, BT_TRANSP_PACKET_BYTES); + pkt_out[0] = BT_G722_SYNC_0; + pkt_out[1] = BT_G722_SYNC_1; + + bt_g722_encode_raw(st, pcm_in, st->pcm_frame_samples, cw, 7); + for (i = 0; i < BT_TRANSP_WIRE_SAMPLES; i++) + bt_put_bits(pkt_out + 2, &bit_pos, cw[i], 7); + + pkt_out[59] = bt_frame_crc8(pkt_out, 59); +} + +void bt_g722_decode_block(struct bt_codec_state *st, const uint8_t *pkt_in, int16_t *pcm_out) +{ + uint8_t cw[BT_TRANSP_WIRE_SAMPLES]; + uint32_t bit_pos = 0; + int i; + + for (i = 0; i < BT_TRANSP_WIRE_SAMPLES; i++) + cw[i] = (uint8_t)bt_get_bits(pkt_in + 2, &bit_pos, 7); + + bt_g722_decode_raw(st, cw, BT_TRANSP_WIRE_SAMPLES, pcm_out, 7); +} + +/* ======================================================================== + * 4. SOF SB4 (Experimental BSD-3-Clause 4-Band 24-Tap Tree-QMF Subband ADPCM) + * - 2-stage 24-tap QMF splits 120 wideband samples into 30 x 4 subbands + * (LL: 6 bits, LH: 4 bits, HL: 3 bits, HH: 2 bits = 15 bits/group = 450 bits) + * ======================================================================== + */ +static const int16_t bt_sb4_step_table[89] __aligned(PLATFORM_DCACHE_ALIGN) = { + 7, 8, 9, 10, 11, 12, 13, 14, 16, 17, + 19, 21, 23, 25, 28, 31, 34, 37, 41, 45, + 50, 55, 60, 66, 73, 80, 88, 97, 107, 118, + 130, 143, 157, 173, 190, 209, 230, 253, 279, 307, + 337, 371, 408, 449, 494, 544, 598, 658, 724, 796, + 876, 963, 1060, 1166, 1282, 1411, 1552, 1707, 1878, 2066, + 2272, 2499, 2749, 3024, 3327, 3660, 4026, 4428, 4871, 5358, + 5894, 6484, 7132, 7845, 8630, 9493, 10442, 11487, 12635, 13899, + 15289, 16818, 18500, 20350, 22385, 24623, 27086, 29794, 32767, +}; + +static const int8_t bt_sb4_idx6[32] __aligned(PLATFORM_DCACHE_ALIGN) = { + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, + 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, +}; + +static const int8_t bt_sb4_idx4[8] __aligned(PLATFORM_DCACHE_ALIGN) = { + -1, -1, -1, -1, 2, 4, 6, 8, +}; + +static const int8_t bt_sb4_idx2[2] __aligned(PLATFORM_DCACHE_ALIGN) = { + -1, 2, +}; + +static uint8_t bt_sb4_adpcm_enc(int32_t sample, int32_t *pred, int32_t *idx, uint8_t nbits) +{ + int32_t p = (*pred * 253) >> 8; + int32_t diff = sample - p; + int32_t step = bt_sb4_step_table[*idx]; + uint8_t mag_bits = nbits - 1u; + uint8_t sign_mask = (uint8_t)(1u << mag_bits); + uint8_t code = 0; + int32_t vpdiff = step >> mag_bits; + int b; + + if (diff < 0) { + code = sign_mask; + diff = -diff; + } + for (b = (int)mag_bits - 1; b >= 0; b--) { + int32_t s = step >> (mag_bits - 1 - b); + + if (diff >= s) { + code |= (uint8_t)(1u << b); + diff -= s; + vpdiff += s; + } + } + + *pred = sat_int16(p + ((code & sign_mask) ? -vpdiff : vpdiff)); + + *idx = (*idx * 63) >> 6; + if (nbits == 6) + *idx += bt_sb4_idx6[code & 0x1F]; + else if (nbits == 4) + *idx += bt_sb4_idx4[code & 0x07]; + else if (nbits == 3) + *idx += (code & 0x03) >= 2 ? 2 : -1; + else + *idx += bt_sb4_idx2[code & 0x01]; + + if (*idx < 0) + *idx = 0; + else if (*idx > 88) + *idx = 88; + + return code; +} + +static int32_t bt_sb4_adpcm_dec(uint8_t code, int32_t *pred, int32_t *idx, uint8_t nbits) +{ + int32_t p = (*pred * 253) >> 8; + int32_t step = bt_sb4_step_table[*idx]; + uint8_t mag_bits = nbits - 1u; + uint8_t sign_mask = (uint8_t)(1u << mag_bits); + int32_t vpdiff = step >> mag_bits; + int b; + + for (b = (int)mag_bits - 1; b >= 0; b--) { + if (code & (1u << b)) + vpdiff += step >> (mag_bits - 1 - b); + } + + *pred = sat_int16(p + ((code & sign_mask) ? -vpdiff : vpdiff)); + + *idx = (*idx * 63) >> 6; + if (nbits == 6) + *idx += bt_sb4_idx6[code & 0x1F]; + else if (nbits == 4) + *idx += bt_sb4_idx4[code & 0x07]; + else if (nbits == 3) + *idx += (code & 0x03) >= 2 ? 2 : -1; + else + *idx += bt_sb4_idx2[code & 0x01]; + + if (*idx < 0) + *idx = 0; + else if (*idx > 88) + *idx = 88; + + return *pred; +} + +void bt_sb4_encode_block(struct bt_codec_state *st, const int16_t *pcm_in, uint8_t *pkt_out) +{ + int16_t pcm16k_buf[BT_TRANSP_WB_SAMPLES]; + const int16_t *pcm16k = pcm_in; + uint32_t bit_pos = 0; + int grp, k; + + if (st->pcm_rate == 8000) { + for (k = 0; k < BT_TRANSP_WIRE_SAMPLES; k++) { + int32_t s0, s1; + + bt_qmf24_rx(st->u.sb4.qmf_8k_x, (int32_t)pcm_in[k] >> 1, 0, + &s0, &s1, 11); + pcm16k_buf[2 * k] = sat_int16(s0); + pcm16k_buf[2 * k + 1] = sat_int16(s1); + } + pcm16k = pcm16k_buf; + } + + memset(pkt_out, 0, BT_TRANSP_PACKET_BYTES); + pkt_out[0] = BT_SB4_SYNC_0; + pkt_out[1] = BT_SB4_SYNC_1; + + for (grp = 0; grp < 30; grp++) { + int32_t l0, h0, l1, h1; + int32_t sb[4]; + uint8_t q0, q1, q2, q3; + uint32_t cw15; + + bt_qmf24_tx(st->u.sb4.qmf_root_x, + pcm16k[4 * grp + 0], pcm16k[4 * grp + 1], &l0, &h0, 13); + bt_qmf24_tx(st->u.sb4.qmf_root_x, + pcm16k[4 * grp + 2], pcm16k[4 * grp + 3], &l1, &h1, 13); + + bt_qmf24_tx(st->u.sb4.qmf_low_x, l0, l1, &sb[0], &sb[1], 13); + bt_qmf24_tx(st->u.sb4.qmf_high_x, h0, h1, &sb[2], &sb[3], 13); + + q0 = bt_sb4_adpcm_enc(sb[0], &st->u.sb4.pred[0], &st->u.sb4.idx[0], 6); + q1 = bt_sb4_adpcm_enc(sb[1], &st->u.sb4.pred[1], &st->u.sb4.idx[1], 4); + q2 = bt_sb4_adpcm_enc(sb[2], &st->u.sb4.pred[2], &st->u.sb4.idx[2], 3); + q3 = bt_sb4_adpcm_enc(sb[3], &st->u.sb4.pred[3], &st->u.sb4.idx[3], 2); + + cw15 = ((uint32_t)(q0 & 0x3Fu) << 9) | + ((uint32_t)(q1 & 0x0Fu) << 5) | + ((uint32_t)(q2 & 0x07u) << 2) | + (uint32_t)(q3 & 0x03u); + bt_put_bits(pkt_out + 2, &bit_pos, cw15, 15); + } + + pkt_out[59] = bt_frame_crc8(pkt_out, 59); +} + +void bt_sb4_decode_block(struct bt_codec_state *st, const uint8_t *pkt_in, int16_t *pcm_out) +{ + int16_t pcm16k_buf[BT_TRANSP_WB_SAMPLES]; + int16_t *dst16k = (st->pcm_rate == 16000) ? pcm_out : pcm16k_buf; + uint32_t bit_pos = 0; + int grp, k; + + for (grp = 0; grp < 30; grp++) { + uint32_t cw15 = bt_get_bits(pkt_in + 2, &bit_pos, 15); + uint8_t q0 = (uint8_t)((cw15 >> 9) & 0x3Fu); + uint8_t q1 = (uint8_t)((cw15 >> 5) & 0x0Fu); + uint8_t q2 = (uint8_t)((cw15 >> 2) & 0x07u); + uint8_t q3 = (uint8_t)(cw15 & 0x03u); + int32_t sb0, sb1, sb2, sb3; + int32_t l0, l1, h0, h1; + int32_t s0, s1, s2, s3; + + sb0 = bt_sb4_adpcm_dec(q0, &st->u.sb4.pred[0], &st->u.sb4.idx[0], 6); + sb1 = bt_sb4_adpcm_dec(q1, &st->u.sb4.pred[1], &st->u.sb4.idx[1], 4); + sb2 = bt_sb4_adpcm_dec(q2, &st->u.sb4.pred[2], &st->u.sb4.idx[2], 3); + sb3 = bt_sb4_adpcm_dec(q3, &st->u.sb4.pred[3], &st->u.sb4.idx[3], 2); + + bt_qmf24_rx(st->u.sb4.qmf_low_x, sb0, sb1, &l0, &l1, 12); + bt_qmf24_rx(st->u.sb4.qmf_high_x, sb2, sb3, &h0, &h1, 12); + + bt_qmf24_rx(st->u.sb4.qmf_root_x, l0, h0, &s0, &s1, 12); + bt_qmf24_rx(st->u.sb4.qmf_root_x, l1, h1, &s2, &s3, 12); + + dst16k[4 * grp + 0] = sat_int16(s0); + dst16k[4 * grp + 1] = sat_int16(s1); + dst16k[4 * grp + 2] = sat_int16(s2); + dst16k[4 * grp + 3] = sat_int16(s3); + } + + if (st->pcm_rate == 8000) { + for (k = 0; k < BT_TRANSP_WIRE_SAMPLES; k++) { + int32_t xlow, xhigh; + + bt_qmf24_tx(st->u.sb4.qmf_8k_x, pcm16k_buf[2 * k], + pcm16k_buf[2 * k + 1], &xlow, &xhigh, 14); + pcm_out[k] = sat_int16(xlow << 1); + } + } +} + +/* ======================================================================== + * Unified Initialization & Streaming Dispatch + * ======================================================================== + */ +void bt_codec_state_init(struct bt_codec_state *st, enum bt_codec_type type, + bool is_encoder, uint32_t pcm_rate) +{ + memset(st, 0, sizeof(*st)); + st->type = type; + st->is_encoder = is_encoder; + st->pcm_rate = (pcm_rate == 16000) ? 16000 : 8000; + st->pcm_step = (st->pcm_rate == 16000) ? 2u : 1u; + st->pcm_frame_samples = BT_TRANSP_WIRE_SAMPLES * st->pcm_step; + + if (type == BT_CODEC_LC3) { + if (is_encoder) + st->u.lc3.enc = lc3_setup_encoder(7500, (int)st->pcm_rate, 0, + &st->u.lc3.enc_mem); + else + st->u.lc3.dec = lc3_setup_decoder(7500, (int)st->pcm_rate, 0, + &st->u.lc3.dec_mem); + } else if (type == BT_CODEC_G722) { + st->u.g722.band[0].det = 32; + st->u.g722.band[1].det = 8; + } +} + +bool bt_codec_valid_frame(enum bt_codec_type type, const uint8_t *pkt) +{ + switch (type) { + case BT_CODEC_LC3: + return pkt[0] == BT_LC3_SYNC_0 && + pkt[1] == BT_LC3_SYNC_1 && + pkt[59] == bt_frame_crc8(pkt, 59); + case BT_CODEC_MSBC: + return pkt[0] == 0x01 && + (pkt[1] == 0x08 || pkt[1] == 0x38 || + pkt[1] == 0xC8 || pkt[1] == 0xF8) && + pkt[2] == 0xAD && + pkt[3] == 0x00 && + pkt[4] == 0x00 && + pkt[5] == bt_msbc_crc8(&pkt[3], &pkt[6]) && + (pkt[58] & 0x03) == 0x00 && + pkt[59] == 0x00; + case BT_CODEC_G722: + return pkt[0] == BT_G722_SYNC_0 && + pkt[1] == BT_G722_SYNC_1 && + pkt[59] == bt_frame_crc8(pkt, 59); + case BT_CODEC_SB4: + return pkt[0] == BT_SB4_SYNC_0 && + pkt[1] == BT_SB4_SYNC_1 && + pkt[59] == bt_frame_crc8(pkt, 59); + } + + return false; +} + +static void bt_codec_encode_block_dispatch(struct bt_codec_state *st, + const int16_t *pcm_in, uint8_t *pkt_out) +{ + switch (st->type) { + case BT_CODEC_LC3: + bt_lc3_encode_block(st, pcm_in, pkt_out); + break; + case BT_CODEC_MSBC: + bt_msbc_encode_block(st, pcm_in, pkt_out); + break; + case BT_CODEC_G722: + bt_g722_encode_block(st, pcm_in, pkt_out); + break; + case BT_CODEC_SB4: + bt_sb4_encode_block(st, pcm_in, pkt_out); + break; + } +} + +static void bt_codec_decode_block_dispatch(struct bt_codec_state *st, + const uint8_t *pkt_in, int16_t *pcm_out) +{ + switch (st->type) { + case BT_CODEC_LC3: + bt_lc3_decode_block(st, pkt_in, pcm_out); + break; + case BT_CODEC_MSBC: + bt_msbc_decode_block(st, pkt_in, pcm_out); + break; + case BT_CODEC_G722: + bt_g722_decode_block(st, pkt_in, pcm_out); + break; + case BT_CODEC_SB4: + bt_sb4_decode_block(st, pkt_in, pcm_out); + break; + } +} + +/* Process 1 wire step in PLAYBACK (Encoder) direction: + * Consumes st->pcm_step PCM samples (1 @ 8 kHz, 2 @ 16 kHz) and produces + * 1 SSP2-BT Transparent 8 kHz word (encoded byte << 8). + */ +int16_t bt_codec_process_enc_step(struct bt_codec_state *st, const int16_t *pcm_in) +{ + uint8_t out_byte = 0; + uint32_t s; + + for (s = 0; s < st->pcm_step; s++) + st->enc_in_buf[st->enc_in_count++] = pcm_in[s]; + + if (st->enc_in_count >= st->pcm_frame_samples) { + uint8_t pkt[BT_TRANSP_PACKET_BYTES]; + int i; + + bt_codec_encode_block_dispatch(st, st->enc_in_buf, pkt); + for (i = 0; i < BT_TRANSP_PACKET_BYTES; i++) { + st->enc_out_fifo[st->enc_fifo_w & BT_TRANSP_ENC_FIFO_MASK] = pkt[i]; + st->enc_fifo_w++; + } + st->enc_in_count = 0; + } + + if (st->enc_fifo_r != st->enc_fifo_w) { + out_byte = st->enc_out_fifo[st->enc_fifo_r & BT_TRANSP_ENC_FIFO_MASK]; + st->enc_fifo_r++; + } + + return (int16_t)((uint16_t)out_byte << 8); +} + +/* Process 1 wire step in CAPTURE (Decoder) direction: + * Consumes 1 SSP2-BT Transparent 8 kHz word (rx_byte = word >> 8) and produces + * st->pcm_step decoded PCM samples (1 @ 8 kHz, 2 @ 16 kHz). + */ +void bt_codec_process_dec_step(struct bt_codec_state *st, int16_t ssp_word, int16_t *pcm_out) +{ + uint8_t rx_byte = (uint8_t)((uint16_t)ssp_word >> 8); + uint32_t plc_len = 4u * st->pcm_frame_samples; + uint32_t s; + + st->dec_bytes_since_valid++; + st->dec_pkt_buf[st->dec_pkt_pos++] = rx_byte; + if (st->dec_pkt_pos == BT_TRANSP_PACKET_BYTES) { + if (bt_codec_valid_frame(st->type, st->dec_pkt_buf)) { + int16_t decoded[BT_TRANSP_WB_SAMPLES]; + uint32_t i; + + if (st->dec_bytes_since_valid == BT_TRANSP_PACKET_BYTES) + st->dec_consec_valid++; + else + st->dec_consec_valid = 1; + st->dec_bytes_since_valid = 0; + + bt_codec_decode_block_dispatch(st, st->dec_pkt_buf, decoded); + for (i = 0; i < st->pcm_frame_samples; i++) { + st->dec_out_fifo[st->dec_fifo_w & + BT_TRANSP_DEC_FIFO_MASK] = decoded[i]; + st->dec_fifo_w++; + st->plc_hist[st->plc_pos % plc_len] = decoded[i]; + st->plc_pos++; + } + st->dec_valid_frames++; + st->dec_pkt_pos = 0; + } else { + if (st->dec_valid_frames > 0 && + (st->dec_bytes_since_valid % BT_TRANSP_PACKET_BYTES) == 0) { + st->dec_consec_valid = 0; + if (st->type == BT_CODEC_LC3) { + int16_t decoded[BT_TRANSP_WB_SAMPLES]; + uint32_t i; + + bt_lc3_decode_block(st, NULL, decoded); + for (i = 0; i < st->pcm_frame_samples; i++) { + st->dec_out_fifo[st->dec_fifo_w & + BT_TRANSP_DEC_FIFO_MASK] = + decoded[i]; + st->dec_fifo_w++; + st->plc_hist[st->plc_pos % plc_len] = decoded[i]; + st->plc_pos++; + } + } + } + memmove(st->dec_pkt_buf, st->dec_pkt_buf + 1, + BT_TRANSP_PACKET_BYTES - 1); + st->dec_pkt_pos = BT_TRANSP_PACKET_BYTES - 1; + } + } + + for (s = 0; s < st->pcm_step; s++) { + if (st->dec_fifo_r != st->dec_fifo_w) { + pcm_out[s] = st->dec_out_fifo[st->dec_fifo_r & + BT_TRANSP_DEC_FIFO_MASK]; + st->dec_fifo_r++; + } else if (st->dec_valid_frames >= 2) { + pcm_out[s] = st->plc_hist[st->plc_pos % plc_len]; + st->plc_pos++; + } else { + pcm_out[s] = 0; + } + } +} diff --git a/src/audio/bt_codec/bt_codec_algos.h b/src/audio/bt_codec/bt_codec_algos.h new file mode 100644 index 000000000000..8969accea747 --- /dev/null +++ b/src/audio/bt_codec/bt_codec_algos.h @@ -0,0 +1,204 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Intel Corporation. All rights reserved. + * + * Bluetooth Transparent Mode (0x03) DSP Codec Algorithms for SOF: + * 1. BT_CODEC_LC3 - Zephyr liblc3 (7.5 ms @ 16 kHz WB / 8 kHz NB, 57B frame + 2B sync + CRC8) + * 2. BT_CODEC_MSBC - Bluetooth HFP v1.8 / A2DP v1.3 H2-framed mSBC (80-tap filterbank, 60B pkt) + * 3. BT_CODEC_G722 - ITU-T Rec. G.722 SB-ADPCM (24-tap QMF + 2-pole/6-zero predictor) + * 4. BT_CODEC_SB4 - Experimental 4-band 24-tap tree-QMF subband ADPCM (BSD-3-Clause) + */ + +#ifndef __SOF_AUDIO_BT_CODEC_ALGOS_H__ +#define __SOF_AUDIO_BT_CODEC_ALGOS_H__ + +#include +#include +#include +#include +#include +#include +#include + +#define BT_TRANSP_WIRE_SAMPLES 60 +#define BT_TRANSP_WB_SAMPLES 120 +#define BT_TRANSP_PACKET_BYTES 60 +#define BT_TRANSP_ENC_FIFO_SIZE 256 +#define BT_TRANSP_ENC_FIFO_MASK (BT_TRANSP_ENC_FIFO_SIZE - 1) +#define BT_TRANSP_DEC_FIFO_SIZE 512 +#define BT_TRANSP_DEC_FIFO_MASK (BT_TRANSP_DEC_FIFO_SIZE - 1) +#define BT_TRANSP_PLC_MAX_SAMPLES 480 + +#define BT_LC3_SYNC_0 0x5A +#define BT_LC3_SYNC_1 0xA5 +#define BT_LC3_PAYLOAD_BYTES 57 + +#define BT_G722_SYNC_0 0x72 +#define BT_G722_SYNC_1 0x27 + +#define BT_SB4_SYNC_0 0x53 +#define BT_SB4_SYNC_1 0x42 + +enum bt_codec_type { + BT_CODEC_LC3 = 0, + BT_CODEC_MSBC = 1, + BT_CODEC_G722 = 2, + BT_CODEC_SB4 = 3, +}; + +struct bt_g722_band { + int32_t s; + int32_t sp; + int32_t sz; + int32_t r[3]; + int32_t a[3]; + int32_t ap[3]; + int32_t p[3]; + int32_t d[7]; + int32_t b[7]; + int32_t bp[7]; + int32_t sg[7]; + int32_t nb; + int32_t det; +}; + +struct bt_codec_state { + enum bt_codec_type type; + bool is_encoder; + uint32_t pcm_rate; + uint32_t pcm_step; + uint32_t pcm_frame_samples; + + /* Streaming PCM block staging (60 samples @ 8 kHz or 120 samples @ 16 kHz) */ + int16_t enc_in_buf[BT_TRANSP_WB_SAMPLES]; + uint32_t enc_in_count; + uint8_t enc_out_fifo[BT_TRANSP_ENC_FIFO_SIZE]; + uint32_t enc_fifo_r; + uint32_t enc_fifo_w; + + uint8_t dec_pkt_buf[BT_TRANSP_PACKET_BYTES]; + uint32_t dec_pkt_pos; + int16_t dec_out_fifo[BT_TRANSP_DEC_FIFO_SIZE]; + uint32_t dec_fifo_r; + uint32_t dec_fifo_w; + int16_t plc_hist[BT_TRANSP_PLC_MAX_SAMPLES]; + uint32_t plc_pos; + uint32_t dec_valid_frames; + uint32_t dec_consec_valid; + uint32_t dec_bytes_since_valid; + + uint32_t frame_seq; + + /* Per-codec state (one active codec type per component instance) */ + union { + struct { + lc3_encoder_t enc; + lc3_decoder_t dec; + union { + LC3_ENCODER_MEM_T(7500, 16000) enc_mem; + LC3_DECODER_MEM_T(7500, 16000) dec_mem; + }; + } lc3; + + struct { + int16_t enc_x[80]; + int32_t dec_v[160]; + int32_t qmf_x[24]; + int16_t qmf_prev; + } msbc; + + struct { + int32_t qmf_x[24]; + struct bt_g722_band band[2]; + } g722; + + struct { + int32_t qmf_root_x[24]; + int32_t qmf_low_x[24]; + int32_t qmf_high_x[24]; + int32_t qmf_8k_x[24]; + int32_t pred[4]; + int32_t idx[4]; + } sb4; + } u; +}; + +/* Cache-line-aligned CRC-8 table for polynomial x^8 + x^4 + x^3 + x^2 + 1 (0x1D) */ +extern const uint8_t bt_crc8_poly1d_table[256]; + +static inline uint8_t bt_crc8_1d(uint8_t crc, const uint8_t *data, size_t len) +{ + size_t i; + + for (i = 0; i < len; i++) + crc = bt_crc8_poly1d_table[crc ^ data[i]]; + + return crc; +} + +static inline uint8_t bt_frame_crc8(const uint8_t *data, size_t len) +{ + return bt_crc8_1d(0xA5, data, len); +} + +static inline uint8_t bt_msbc_crc8(const uint8_t *hdr2, const uint8_t *sf4) +{ + uint8_t crc = bt_crc8_1d(0x0F, hdr2, 2); + + return bt_crc8_1d(crc, sf4, 4); +} + +static inline void bt_put_bits(uint8_t *buf, uint32_t *bit_pos, uint32_t val, uint8_t nbits) +{ + int b; + + for (b = (int)nbits - 1; b >= 0; b--) { + uint32_t byte_idx = (*bit_pos) >> 3; + uint32_t bit_idx = 7u - ((*bit_pos) & 7u); + + if ((val >> b) & 1u) + buf[byte_idx] |= (uint8_t)(1u << bit_idx); + (*bit_pos)++; + } +} + +static inline uint32_t bt_get_bits(const uint8_t *buf, uint32_t *bit_pos, uint8_t nbits) +{ + uint32_t val = 0; + uint8_t b; + + for (b = 0; b < nbits; b++) { + uint32_t byte_idx = (*bit_pos) >> 3; + uint32_t bit_idx = 7u - ((*bit_pos) & 7u); + + val = (val << 1) | ((buf[byte_idx] >> bit_idx) & 1u); + (*bit_pos)++; + } + return val; +} + +void bt_codec_state_init(struct bt_codec_state *st, enum bt_codec_type type, + bool is_encoder, uint32_t pcm_rate); + +bool bt_codec_valid_frame(enum bt_codec_type type, const uint8_t *pkt); + +void bt_lc3_encode_block(struct bt_codec_state *st, const int16_t *pcm_in, uint8_t *pkt_out); +void bt_lc3_decode_block(struct bt_codec_state *st, const uint8_t *pkt_in, int16_t *pcm_out); + +void bt_msbc_encode_block(struct bt_codec_state *st, const int16_t *pcm_in, uint8_t *pkt_out); +void bt_msbc_decode_block(struct bt_codec_state *st, const uint8_t *pkt_in, int16_t *pcm_out); + +void bt_g722_encode_raw(struct bt_codec_state *st, const int16_t *pcm_in, + uint32_t num_samples, uint8_t *g722_out, uint8_t bits_per_cw); +void bt_g722_decode_raw(struct bt_codec_state *st, const uint8_t *g722_in, + uint32_t num_codewords, int16_t *pcm_out, uint8_t bits_per_cw); +void bt_g722_encode_block(struct bt_codec_state *st, const int16_t *pcm_in, uint8_t *pkt_out); +void bt_g722_decode_block(struct bt_codec_state *st, const uint8_t *pkt_in, int16_t *pcm_out); + +void bt_sb4_encode_block(struct bt_codec_state *st, const int16_t *pcm_in, uint8_t *pkt_out); +void bt_sb4_decode_block(struct bt_codec_state *st, const uint8_t *pkt_in, int16_t *pcm_out); + +int16_t bt_codec_process_enc_step(struct bt_codec_state *st, const int16_t *pcm_in); +void bt_codec_process_dec_step(struct bt_codec_state *st, int16_t ssp_word, int16_t *pcm_out); + +#endif /* __SOF_AUDIO_BT_CODEC_ALGOS_H__ */ diff --git a/src/audio/bt_codec/lc3_math_shim.h b/src/audio/bt_codec/lc3_math_shim.h new file mode 100644 index 000000000000..19cdd315501c --- /dev/null +++ b/src/audio/bt_codec/lc3_math_shim.h @@ -0,0 +1,176 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Intel Corporation. All rights reserved. + * + * Self-contained IEEE-754 single-precision math helpers for liblc3 under + * Zephyr CONFIG_MINIMAL_LIBC (no external libm link dependency required). + */ + +#ifndef __SOF_AUDIO_BT_CODEC_LC3_MATH_SHIM_H__ +#define __SOF_AUDIO_BT_CODEC_LC3_MATH_SHIM_H__ + +#include +#include + +#ifndef INFINITY +#define INFINITY (__builtin_inff()) +#endif + +#ifndef NAN +#define NAN (__builtin_nanf("")) +#endif + +static inline float sof_lc3_fabsf(float x) +{ + return __builtin_fabsf(x); +} + +static inline float sof_lc3_fminf(float a, float b) +{ + return a < b ? a : b; +} + +static inline float sof_lc3_fmaxf(float a, float b) +{ + return a > b ? a : b; +} + +static inline float sof_lc3_truncf(float x) +{ + union { + float f; + uint32_t u; + } v = { .f = x }; + + /* For |x| >= 2^23 (biased exponent >= 150), x has no fractional bits */ + if (((v.u >> 23) & 0xFFu) >= 150u) + return x; + + return (float)(int32_t)x; +} + +static inline float sof_lc3_floorf(float x) +{ + float t = sof_lc3_truncf(x); + + return t - (x < t ? 1.0f : 0.0f); +} + +static inline float sof_lc3_ceilf(float x) +{ + float t = sof_lc3_truncf(x); + + return t + (x > t ? 1.0f : 0.0f); +} + +static inline float sof_lc3_roundf(float x) +{ + union { + float f; + uint32_t u; + } v = { .f = x }; + + if (((v.u >> 23) & 0xFFu) >= 150u) + return x; + + return x >= 0.0f ? sof_lc3_truncf(x + 0.5f) : sof_lc3_truncf(x - 0.5f); +} + +static inline float sof_lc3_ldexpf(float x, int exp) +{ + union { + float f; + uint32_t u; + } v = { .f = x }; + int e; + + if (x == 0.0f) + return 0.0f; + e = (int)((v.u >> 23) & 0xFF) + exp; + if (e <= 0) + return 0.0f; + if (e >= 255) + e = 254; + v.u = (v.u & 0x807FFFFFu) | ((uint32_t)e << 23); + return v.f; +} + +static inline float sof_lc3_frexpf(float x, int *exp) +{ + union { + float f; + uint32_t u; + } v = { .f = x }; + int e; + + if (x == 0.0f) { + *exp = 0; + return 0.0f; + } + e = (int)((v.u >> 23) & 0xFF); + if (e == 0) { + *exp = 0; + return 0.0f; + } + *exp = e - 126; + v.u = (v.u & 0x807FFFFFu) | (126u << 23); + return v.f; +} + +static inline float sof_lc3_sqrtf(float x) +{ + union { + float f; + uint32_t u; + } v; + float y; + + if (x <= 0.0f) + return 0.0f; + v.f = x; + v.u = 0x5f3759dfu - (v.u >> 1); + y = v.f; + y = y * (1.5f - 0.5f * x * y * y); + y = y * (1.5f - 0.5f * x * y * y); + y = y * (1.5f - 0.5f * x * y * y); + return x * y; +} + +static inline float sof_lc3_log10f(float x) +{ + int e = 0; + float m, y; + + if (x <= 0.0f) + return -38.0f; + m = sof_lc3_frexpf(x, &e); + y = (((-1.29479677f * m + 5.11769018f) * m - 8.42295281f) * m + + 8.10557963f) * m - 3.50567360f; + return 0.30102999566f * ((float)e + y); +} + +#undef fabsf +#undef fminf +#undef fmaxf +#undef truncf +#undef floorf +#undef ceilf +#undef roundf +#undef ldexpf +#undef frexpf +#undef sqrtf +#undef log10f + +#define fabsf sof_lc3_fabsf +#define fminf sof_lc3_fminf +#define fmaxf sof_lc3_fmaxf +#define truncf sof_lc3_truncf +#define floorf sof_lc3_floorf +#define ceilf sof_lc3_ceilf +#define roundf sof_lc3_roundf +#define ldexpf sof_lc3_ldexpf +#define frexpf sof_lc3_frexpf +#define sqrtf sof_lc3_sqrtf +#define log10f sof_lc3_log10f + +#endif /* __SOF_AUDIO_BT_CODEC_LC3_MATH_SHIM_H__ */ diff --git a/tools/rimage/config/ptl.toml.h b/tools/rimage/config/ptl.toml.h index 88e4a153797e..b1544cb12835 100644 --- a/tools/rimage/config/ptl.toml.h +++ b/tools/rimage/config/ptl.toml.h @@ -205,5 +205,9 @@ index = __COUNTER__ #include