diff --git a/.github/workflows/build_firmware.yaml b/.github/workflows/build_firmware.yaml index 8a70d08f..e64e82dd 100644 --- a/.github/workflows/build_firmware.yaml +++ b/.github/workflows/build_firmware.yaml @@ -51,9 +51,9 @@ jobs: - name: Install Zephyr SDK run: | - wget -q https://github.com/zephyrproject-rtos/sdk-ng/releases/download/v0.17.0/zephyr-sdk-0.17.0_linux-x86_64_minimal.tar.xz - tar xf zephyr-sdk-0.17.0_linux-x86_64_minimal.tar.xz -C ~/ - ~/zephyr-sdk-0.17.0/setup.sh -c -t arm-zephyr-eabi + wget -q https://github.com/zephyrproject-rtos/sdk-ng/releases/download/v1.0.1/zephyr-sdk-1.0.1_linux-x86_64_minimal.tar.xz + tar xf zephyr-sdk-1.0.1_linux-x86_64_minimal.tar.xz -C ~/ + ~/zephyr-sdk-1.0.1/setup.sh -c -t arm-zephyr-eabi - name: Initialize Zephyr Workspace # Set up the Zephyr workspace and install the Python dependencies @@ -102,7 +102,16 @@ jobs: - name: Check And Prepare Build Output run: | mkdir -p $(pwd)/workflow-board-temp/build-output - cp $(pwd)/zephyr-workspace/build/ipc_radio/zephyr/zephyr.elf $(pwd)/workflow-board-temp/build-output/openearable_v2_firmware.elf + firmware_dir="$(pwd)/workflow-board-temp/build-output/openearable_v2_firmware" + mkdir -p "$firmware_dir/build_fota" "$firmware_dir/tools/flash" + cp zephyr-workspace/build/merged_openearable_v2_nrf5340_cpuapp.hex "$firmware_dir/build_fota/" + cp zephyr-workspace/build/merged_openearable_v2_nrf5340_cpunet.hex "$firmware_dir/build_fota/" + cp zephyr-workspace/build/open-earable-v2/zephyr/zephyr.elf "$firmware_dir/openearable_v2_cpuapp.elf" + cp zephyr-workspace/build/ipc_radio/zephyr/zephyr.elf "$firmware_dir/openearable_v2_cpunet.elf" + cp zephyr-workspace/open-earable-v2/tools/flash/flash_fota.sh "$firmware_dir/tools/flash/" + printf '%s\n' 'From this directory, run: bash tools/flash/flash_fota.sh --snr ' \ + 'The build_fota images include both cores and their bootloaders. The ELFs are for debugging.' \ + > "$firmware_dir/FLASH.txt" cp $(pwd)/zephyr-workspace/build/dfu_application.zip $(pwd)/workflow-board-temp/build-output/openearable_v2_fota.zip (cd $(pwd)/workflow-board-temp/build-output && unzip openearable_v2_fota.zip -d openearable_v2_fota && rm openearable_v2_fota.zip) @@ -110,8 +119,8 @@ jobs: uses: actions/upload-artifact@v4 id: artifact-upload-step-1 with: - name: openearable_v2_firmware.elf - path: workflow-board-temp/build-output/openearable_v2_firmware.elf + name: openearable_v2_firmware.zip + path: workflow-board-temp/build-output/openearable_v2_firmware/ - name: Upload Build Artifact (FOTA build) uses: actions/upload-artifact@v4 @@ -126,5 +135,5 @@ jobs: with: message: | Build output available: - [openearable_v2_firmware.elf.zip](${{ steps.artifact-upload-step-1.outputs.artifact-url }}) + [openearable_v2_firmware.zip](${{ steps.artifact-upload-step-1.outputs.artifact-url }}) [openearable_v2_fota.zip](${{ steps.artifact-upload-step-2.outputs.artifact-url }}) diff --git a/.github/workflows/codechecker.yaml b/.github/workflows/codechecker.yaml index 3f605c08..5595877d 100644 --- a/.github/workflows/codechecker.yaml +++ b/.github/workflows/codechecker.yaml @@ -14,7 +14,7 @@ concurrency: env: CODECHECKER_VERSION: "6.28.2" - ZEPHYR_SDK_VERSION: "0.17.0" + ZEPHYR_SDK_VERSION: "1.0.1" jobs: codechecker: @@ -62,7 +62,16 @@ jobs: wget --quiet \ "https://github.com/zephyrproject-rtos/sdk-ng/releases/download/v${ZEPHYR_SDK_VERSION}/zephyr-sdk-${ZEPHYR_SDK_VERSION}_linux-x86_64_minimal.tar.xz" tar xf "zephyr-sdk-${ZEPHYR_SDK_VERSION}_linux-x86_64_minimal.tar.xz" - "${PWD}/zephyr-sdk-${ZEPHYR_SDK_VERSION}/setup.sh" -c -t arm-zephyr-eabi + for attempt in 1 2 3; do + "${PWD}/zephyr-sdk-${ZEPHYR_SDK_VERSION}/setup.sh" -c -t arm-zephyr-eabi && break + if [ "${attempt}" -eq 3 ]; then + echo "::error::Failed to install arm-zephyr-eabi toolchain after ${attempt} attempts." + exit 20 + fi + echo "Retrying Zephyr toolchain installation (attempt $((attempt + 1))/3)..." + rm -rf "${PWD}/zephyr-sdk-${ZEPHYR_SDK_VERSION}/gnu/arm-zephyr-eabi" + sleep 5 + done - name: Initialize Zephyr workspace working-directory: zephyr-workspace diff --git a/.github/workflows/compiler_warnings.yaml b/.github/workflows/compiler_warnings.yaml index 7e605d59..839201c7 100644 --- a/.github/workflows/compiler_warnings.yaml +++ b/.github/workflows/compiler_warnings.yaml @@ -30,9 +30,9 @@ jobs: - name: Set up Zephyr workspace run: | pip3 install west - wget -q https://github.com/zephyrproject-rtos/sdk-ng/releases/download/v0.17.0/zephyr-sdk-0.17.0_linux-x86_64_minimal.tar.xz - tar xf zephyr-sdk-0.17.0_linux-x86_64_minimal.tar.xz -C ~/ - ~/zephyr-sdk-0.17.0/setup.sh -c -t arm-zephyr-eabi + wget -q https://github.com/zephyrproject-rtos/sdk-ng/releases/download/v1.0.1/zephyr-sdk-1.0.1_linux-x86_64_minimal.tar.xz + tar xf zephyr-sdk-1.0.1_linux-x86_64_minimal.tar.xz -C ~/ + ~/zephyr-sdk-1.0.1/setup.sh -c -t arm-zephyr-eabi cd zephyr-workspace west init -l open-earable-v2 west update --narrow -o=--depth=1 diff --git a/.github/workflows/prerelease_open_pr_builds.yaml b/.github/workflows/prerelease_open_pr_builds.yaml index a29b2d04..ebbdb8b9 100644 --- a/.github/workflows/prerelease_open_pr_builds.yaml +++ b/.github/workflows/prerelease_open_pr_builds.yaml @@ -58,7 +58,7 @@ jobs: num=$(echo "$pr" | jq -r .number) sha=$(echo "$pr" | jq -r .headRefOid) title=$(echo "$pr" | jq -r .title) - safe_title=$(echo "$title" | sed 's/[^a-zA-Z0-9_-]/_/g' | cut -c1-50) + safe_title=$(echo "$title" | sed 's/[^a-zA-Z0-9_.-]/_/g' | cut -c1-50) run_id=$(gh run list \ --repo "${{ github.repository }}" \ --workflow "$BUILD_WORKFLOW_NAME" \ @@ -73,8 +73,13 @@ jobs: fi out="collected/pr-$num" rm -rf "$out" && mkdir -p "$out" - if ! gh run download "$run_id" --repo "${{ github.repository }}" -n "openearable_v2_firmware.elf" -D "$out/elf" 2>&1; then - echo "PR #$num: failed to download firmware.elf artifact (may be expired)" + if gh run download "$run_id" --repo "${{ github.repository }}" -n "openearable_v2_firmware.zip" -D "$out/firmware" 2>&1; then + (cd "$out/firmware" && zip -r "../openearable_v2_firmware.zip" .) + elif gh run download "$run_id" --repo "${{ github.repository }}" -n "openearable_v2_firmware.elf" -D "$out/elf" 2>&1; then + # Older PRs still publish a single ELF rather than the flash bundle. + mv "$out/elf/openearable_v2_firmware.elf" "$out/openearable_v2_firmware.elf" + else + echo "PR #$num: failed to download firmware artifact (may be expired)" rm -rf "$out" continue fi @@ -84,8 +89,7 @@ jobs: continue fi (cd "$out/fota" && zip -r "../openearable_v2_fota.zip" .) - mv "$out/elf/openearable_v2_firmware.elf" "$out/openearable_v2_firmware.elf" - rm -rf "$out/elf" "$out/fota" + rm -rf "$out/firmware" "$out/elf" "$out/fota" echo "$safe_title" > "$out/title.txt" diff --git a/.github/workflows/release_firmware.yaml b/.github/workflows/release_firmware.yaml index ebe90ad5..9e4a94e7 100644 --- a/.github/workflows/release_firmware.yaml +++ b/.github/workflows/release_firmware.yaml @@ -25,7 +25,11 @@ jobs: - name: Download Firmware Artifact (Firmware) uses: actions/download-artifact@v4 with: - name: openearable_v2_firmware.elf + name: openearable_v2_firmware.zip + path: ./openearable_v2_firmware + + - name: Zip complete firmware package + run: (cd openearable_v2_firmware && zip -r ../openearable_v2_firmware.zip .) - name: Create Directory for FOTA run: mkdir -p openearable_v2_fota @@ -52,10 +56,10 @@ jobs: # v2.2.8 -> 2.2.8 version="${RELEASE_TAG#v}" - firmware_name="openearable_v2_firmware_${version}.elf" + firmware_name="openearable_v2_firmware_${version}.zip" fota_name="openearable_v2_fota_${version}.zip" - mv openearable_v2_firmware.elf "$firmware_name" + mv openearable_v2_firmware.zip "$firmware_name" mv openearable_v2_fota.zip "$fota_name" echo "firmware=$firmware_name" >> "$GITHUB_OUTPUT" diff --git a/.github/workflows/unit_tests.yaml b/.github/workflows/unit_tests.yaml index 68adca61..a9b3ff8f 100644 --- a/.github/workflows/unit_tests.yaml +++ b/.github/workflows/unit_tests.yaml @@ -31,6 +31,7 @@ jobs: with: # Keep the application below the west workspace root. path: zephyr-workspace/open-earable-v2 + submodules: recursive - name: Install host dependencies uses: awalsh128/cache-apt-pkgs-action@latest diff --git a/CMakeLists.txt b/CMakeLists.txt index 2d73b3c9..51debf12 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,13 +12,21 @@ add_compile_definitions(GATEWAY=2) find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) -project(openearable_v2_1) +project(openearable_v2) + +include(cmake/sd_spi_fast_write.cmake) + +# NCS 3.4.1 gates this bootutil hook on a bootloader-only Kconfig symbol. +# SMP must use its nRF53 hook too: the radio primary slot is not readable here. +if(CONFIG_MCUMGR_GRP_IMG_NRF5340_BOOTUTIL_HOOK) + target_compile_definitions(mcuboot_util PRIVATE CONFIG_NRF53_MULTI_IMAGE_UPDATE=1) +endif() #set(OVERLAY_CONFIG # ${CMAKE_CURRENT_SOURCE_DIR}/unicast_server/overlay-unicast_server.conf #) -string(TIMESTAMP NRF5340_AUDIO_CORE_APP_COMP_DATE "%a %b %d %H:%M:%S %Y") +string(TIMESTAMP NRF_AUDIO_CORE_APP_COMP_DATE "%a %b %d %H:%M:%S %Y") # Generate fw_info_app.c configure_file("${CMAKE_CURRENT_SOURCE_DIR}/src/utils/fw_info_app.c.in" @@ -45,6 +53,7 @@ zephyr_library_include_directories( include src/audio src/Battery + src/buttons src/Wire src/bluetooth src/drivers @@ -59,7 +68,7 @@ zephyr_library_include_directories( ) zephyr_library_include_directories(app PRIVATE - ${ZEPHYR_NRF_MODULE_DIR}/boards/teco/openearable_v2_nrf5340) + ${CMAKE_CURRENT_SOURCE_DIR}/boards/teco/openearable_v2) # Application sources add_subdirectory(src/audio) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 28dfd513..4d0b784d 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -33,11 +33,11 @@ Use the same toolchain versions the repository expects. 1. Install Visual Studio Code and the nRF Connect for VS Code extension. 2. Install the J-Link Software and Documentation Package. 3. Install `nrfutil` and ensure it is available on your `PATH`. -4. Install nRF Connect SDK `v3.0.1`. -5. Install toolchain `v3.0.1`. +4. Install nRF Connect SDK `v3.4.1`. +5. Install toolchain `v3.4.1`. 6. Open this repository as an application in the nRF Connect extension. -The manifest in [west.yml](west.yml) pins the workspace to `sdk-nrf` `v3.0.1`. Keep documentation and local validation aligned with that version unless the repository is explicitly upgraded. +The manifest in [west.yml](west.yml) pins the workspace to `sdk-nrf` `v3.4.1`. Keep documentation and local validation aligned with that version unless the repository is explicitly upgraded. ## Build And Flash @@ -156,7 +156,7 @@ without sysbuild using Clang Static Analyzer and Cppcheck. Style findings, Clang-Tidy, formatting, and automatic fixes are not enabled. The analyzer configuration is in [.codechecker.json](.codechecker.json). -Local execution is optional. In an nRF Connect SDK `v3.0.1` Linux or WSL +Local execution is optional. In an nRF Connect SDK `v3.4.1` Linux or WSL workspace, install CodeChecker `6.28.2`, Clang, and Cppcheck, then run: ```bash @@ -171,8 +171,7 @@ west build --no-sysbuild --board openearable_v2/nrf5340/cpuapp \ ``` Reports are written to `/sca/codechecker/`. See Nordic's -[CodeChecker documentation for nRF Connect SDK 3.0.1](https://nrfconnectdocs.nordicsemi.com/ncs/3.0.1/zephyr/develop/sca/codechecker.html) -or the corresponding [upstream Zephyr 4.0 documentation](https://docs.zephyrproject.org/4.0.0/develop/sca/codechecker.html) +[CodeChecker documentation for nRF Connect SDK 3.4.1](https://nrfconnectdocs.nordicsemi.com/ncs/3.4.1/zephyr/develop/sca/codechecker.html) for the available options. ## Validation Before Opening A Pull Request diff --git a/Kconfig b/Kconfig index 9106caf9..ef8744cd 100644 --- a/Kconfig +++ b/Kconfig @@ -4,11 +4,9 @@ # SPDX-License-Identifier: LicenseRef-Nordic-5-Clause # -menuconfig NRF5340_AUDIO - bool "nRF5340 Audio [EXPERIMENTAL]" +menuconfig APP_NRF_AUDIO + bool "nRF Audio [EXPERIMENTAL]" select EXPERIMENTAL - -if NRF5340_AUDIO config CUSTOM_BROADCASTER bool "Allow custom broadcasters" @@ -25,7 +23,7 @@ config AUDIO_DEV as a HEADSET device. Setting to 2 will compile as a GATEWAY. -choice NRF5340_AUDIO_TRANSPORT_MODE +choice NRF_AUDIO_TRANSPORT_MODE prompt "Choose BIS or CIS for ISO transport" default TRANSPORT_CIS if WALKIE_TALKIE_DEMO default TRANSPORT_CIS @@ -79,6 +77,4 @@ config PRINT_STACK_USAGE_MS endmenu # Log levels #----------------------------------------------------------------------------# -endif # NRF5340_AUDIO - source "Kconfig.zephyr" diff --git a/Kconfig.defaults b/Kconfig.defaults index c07438d6..f48f824b 100644 --- a/Kconfig.defaults +++ b/Kconfig.defaults @@ -11,11 +11,11 @@ config MAIN_THREAD_PRIORITY default 10 config MAIN_STACK_SIZE - default 1800 if SD_CARD_PLAYBACK - default 1600 + default 2500 if SD_CARD_PLAYBACK + default 2000 config SYSTEM_WORKQUEUE_STACK_SIZE - default 1200 + default 2000 # As long as thread names are used, config must be set to "y" config THREAD_NAME @@ -48,7 +48,7 @@ config REGULATOR config CONTIN_ARRAY default y -config NRFX_I2S0 +config NRFX_I2S default y config PCM_MIX @@ -57,9 +57,6 @@ config PCM_MIX config PSCM default y -config DATA_FIFO - default y - # Enable NRFX_CLOCK for ACLK control config NRFX_CLOCK default y @@ -67,12 +64,6 @@ config NRFX_CLOCK config I2C default y -choice LIBC_IMPLEMENTATION - # NOTE: Since we are not using minimal libc, error codes from - # minimal libc are not used - default NEWLIB_LIBC -endchoice - # Audio codec LC3 related defines # FPU_SHARING enables preservation of the hardware floating point registers # across context switches to allow multiple threads to perform concurrent @@ -110,9 +101,6 @@ config FILE_SYSTEM config FAT_FILESYSTEM_ELM default y -config FS_FATFS_HAS_RTC - default y - config FS_FATFS_LFN default y choice FS_FATFS_LFN_MODE @@ -124,8 +112,6 @@ endchoice config FS_FATFS_EXFAT default y -config WATCHDOG - default y - -config TASK_WDT +# The SD logger owns a 32768-byte ring; Zephyr now defaults to a 32767-byte limit. +config RING_BUFFER_LARGE default y diff --git a/Kconfig.sysbuild b/Kconfig.sysbuild index ee679c15..6be9c8ef 100644 --- a/Kconfig.sysbuild +++ b/Kconfig.sysbuild @@ -7,4 +7,7 @@ config NRF_DEFAULT_IPC_RADIO default y -source "${ZEPHYR_BASE}/share/sysbuild/Kconfig" +config PARTITION_MANAGER + default n + +source "share/sysbuild/Kconfig" diff --git a/README.md b/README.md index 1c032739..c75f5a17 100644 --- a/README.md +++ b/README.md @@ -39,11 +39,11 @@ 5. **Install the Toolchain via nRF Connect** - Open the **nRF Connect** tab in VS Code. - Click **"Install Toolchain"**. - - Select and install **version 3.0.1**. + - Select and install **version 3.4.1**. 6. **Install the nRF Connect SDK** - In the **nRF Connect** tab, select **"Manage SDK"**. - - Install **SDK version 3.0.1**. + - Install **SDK version 3.4.1**. 7. **Clone or Update the Firmware Repository and Open It in VS Code** - For a new checkout, clone the repository together with its submodules: @@ -65,7 +65,9 @@ - In the **APPLICATIONS** section of the nRF Connect extension tab: - Select the `open-earable-2` application. - Click **"+ Add build configuration"** to set up a new build. - - Select the SDK version 3.0.1, toolchain version 3.0.1, and `open-earable-2/nrf5340/cpuapp` as board target. + - Select the SDK version 3.4.1, toolchain version 3.4.1, and `openearable_v2/nrf5340/cpuapp` as board target. + - After switching SDK versions, use a new build directory or a pristine build. + - The firmware manifest pins nRF Connect SDK **v3.4.1** (Nordic Zephyr **4.4.2**). Use the matching **v3.4.1 Nordic toolchain**; CI uses **Zephyr SDK 1.0.1**. - To build **with FOTA** (firmware over-the-air update functionality): - Leave the `Base configuration files (Kconfig fragments)` dropdown empty. - as `Extra CMAKE arguments` set `-DFILE_SUFFIX="fota"`. @@ -73,6 +75,7 @@ - To build **without FOTA**: - Select `prj.conf` as the `Base configuration files (Kconfig fragments)`. - Do not set any of the FOTA flags described above. + - **Migrating from SDK 3.0.1:** use a full wired sysbuild flash, including MCUboot, the network-core bootloader, and both core images. SDK 3.4.1 replaces Partition Manager with fixed devicetree partitions at the same addresses. Its FOTA configuration uses overwrite-only updates, so it does not provide automatic rollback. An OTA-only migration using the older bootloader has not been validated. 9. **J-Link Setup** - Wire your J-Link to the debugging breakout PCB as shown below. @@ -84,6 +87,7 @@ 11. **Build and Flash** - Click on `Generate and Build` and wait for the application to build (this will take some time) + - Rebuild existing configurations to generate the per-core `merged_openearable_v2_nrf5340_cpuapp.hex` and `merged_openearable_v2_nrf5340_cpunet.hex` files used by the flash scripts. Both scripts also accept the previous `merged.hex` / `merged_CPUNET.hex` layout. - Make sure your device is charged or powered via USB. If the battery is fully discharged, the charging management IC will no longer supply power to the MCU from the battery, so you won’t be able to flash the MCU unless the battery is charged or the device is directly powered via USB. - Open a new terminal in VS Code and run the following command from the root of the `open-earable-v2` directory to flash the FOTA build. Make sure to set the serial number of your J-Link (right click your J-Link in the `CONNECTED DEVICES` tab of the nRF connect extension and copy the serial number). ```bash diff --git a/VERSION b/VERSION index d0530d25..d3e37008 100644 --- a/VERSION +++ b/VERSION @@ -1,5 +1,5 @@ VERSION_MAJOR = 2 -VERSION_MINOR = 2 -PATCHLEVEL = 9 +VERSION_MINOR = 3 +PATCHLEVEL = 0 VERSION_TWEAK = 0 EXTRAVERSION = diff --git a/boards/nrf5340_audio_dk_nrf5340_cpuapp.conf b/boards/nrf5340_audio_dk_nrf5340_cpuapp.conf index ccddc56b..407297ba 100644 --- a/boards/nrf5340_audio_dk_nrf5340_cpuapp.conf +++ b/boards/nrf5340_audio_dk_nrf5340_cpuapp.conf @@ -4,6 +4,6 @@ # SPDX-License-Identifier: LicenseRef-Nordic-5-Clause # -CONFIG_NRF5340_AUDIO_CS47L63_DRIVER=y -CONFIG_NRF5340_AUDIO_POWER_MEASUREMENT=y -CONFIG_NRF5340_AUDIO_SD_CARD_MODULE=y +CONFIG_NRF_AUDIO_CS47L63_DRIVER=y +CONFIG_NRF_AUDIO_SD_CARD_MODULE=y +CONFIG_UDC_DRIVER_HIGH_SPEED_SUPPORT_ENABLED=n diff --git a/boards/nrf5340_audio_dk_nrf5340_cpuapp_release.conf b/boards/nrf5340_audio_dk_nrf5340_cpuapp_release.conf index 068697c6..b813a3e0 100644 --- a/boards/nrf5340_audio_dk_nrf5340_cpuapp_release.conf +++ b/boards/nrf5340_audio_dk_nrf5340_cpuapp_release.conf @@ -4,5 +4,5 @@ # SPDX-License-Identifier: LicenseRef-Nordic-5-Clause # -CONFIG_NRF5340_AUDIO_CS47L63_DRIVER=y -CONFIG_NRF5340_AUDIO_SD_CARD_MODULE=y +CONFIG_NRF_AUDIO_CS47L63_DRIVER=y +CONFIG_NRF_AUDIO_SD_CARD_MODULE=y diff --git a/boards/nrf5340dk_nrf5340_cpuapp.overlay b/boards/nrf5340dk_nrf5340_cpuapp.overlay index 9c36cd5f..edb52322 100644 --- a/boards/nrf5340dk_nrf5340_cpuapp.overlay +++ b/boards/nrf5340dk_nrf5340_cpuapp.overlay @@ -4,15 +4,169 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ -&usbd { - hs_0: hs_0 { - compatible = "usb-audio-hs"; - mic-feature-mute; - mic-channel-l; - mic-channel-r; - - hp-feature-mute; - hp-channel-l; - hp-channel-r; +#include + +/ { + audioleds: audioleds { + compatible = "gpio-leds"; + status = "okay"; + + led_conn_status: led_conn_status { + gpios = <&gpio0 28 GPIO_ACTIVE_LOW>; + label = "Green LED 0"; + }; + + led_sync_status: led_sync_status { + gpios = <&gpio0 29 GPIO_ACTIVE_LOW>; + label = "Green LED 1"; + }; + + led_app_status: led_app_status { + gpios = <&gpio0 30 GPIO_ACTIVE_LOW>; + label = "Green LED 2"; + }; + }; + + ramdisk0: ramdisk0 { + compatible = "zephyr,ram-disk"; + disk-name = "RAM"; + sector-size = <512>; + sector-count = <80>; + }; +}; + +/ { + uac2_headset: usb_audio2 { + compatible = "zephyr,uac2"; + status = "okay"; + full-speed; + audio-function = ; + + uac_aclk: aclk { + compatible = "zephyr,uac2-clock-source"; + clock-type = "internal-programmable"; + frequency-control = "read-only"; + sampling-frequencies = <48000>; + sof-synchronized; + }; + + out_terminal: out_terminal { + compatible = "zephyr,uac2-input-terminal"; + clock-source = <&uac_aclk>; + terminal-type = ; + front-left; + front-right; + }; + + headphones_output: headphones { + compatible = "zephyr,uac2-output-terminal"; + data-source = <&out_terminal>; + clock-source = <&uac_aclk>; + terminal-type = ; + assoc-terminal = <&mic_input>; + }; + + in_terminal: in_terminal { + compatible = "zephyr,uac2-output-terminal"; + data-source = <&mic_input>; + clock-source = <&uac_aclk>; + terminal-type = ; + }; + + mic_input: microphone { + compatible = "zephyr,uac2-input-terminal"; + clock-source = <&uac_aclk>; + terminal-type = ; + front-left; + front-right; + }; + + as_iso_out: out_interface { + compatible = "zephyr,uac2-audio-streaming"; + linked-terminal = <&out_terminal>; + subslot-size = <2>; + bit-resolution = <16>; + }; + + as_iso_in: in_interface { + compatible = "zephyr,uac2-audio-streaming"; + linked-terminal = <&in_terminal>; + subslot-size = <2>; + bit-resolution = <16>; + }; }; }; + +/ { + uac2_headphones: usb_audio2 { + compatible = "zephyr,uac2"; + status = "okay"; + full-speed; + audio-function = ; + + hp_uac_aclk: hp_aclk { + compatible = "zephyr,uac2-clock-source"; + clock-type = "internal-programmable"; + frequency-control = "read-only"; + sampling-frequencies = <48000>; + sof-synchronized; + }; + + hp_out_terminal: out_terminal { + compatible = "zephyr,uac2-input-terminal"; + clock-source = <&hp_uac_aclk>; + terminal-type = ; + front-left; + front-right; + }; + + hp_headphones_output: hp_headphones_output { + compatible = "zephyr,uac2-output-terminal"; + data-source = <&hp_out_terminal>; + clock-source = <&hp_uac_aclk>; + terminal-type = ; + }; + + hp_as_iso_out: out_interface { + compatible = "zephyr,uac2-audio-streaming"; + linked-terminal = <&hp_out_terminal>; + subslot-size = <2>; + bit-resolution = <16>; + }; + }; +}; + +&pinctrl { + i2s0_default: i2s0_default { + group1 { + psels = ; + nordic,drive-mode = ; + }; + + group2 { + psels = , + , + , + ; + }; + }; + + i2s0_sleep: i2s0_sleep { + group1 { + psels = , + , + , + , + ; + low-power-enable; + }; + }; +}; + +&i2s0 { + compatible = "nordic,nrf-i2s"; + status = "okay"; + pinctrl-0 = <&i2s0_default>; + pinctrl-1 = <&i2s0_sleep>; + pinctrl-names = "default", "sleep"; +}; diff --git a/boards/openearable_v2_nrf5340_cpuapp.overlay b/boards/openearable_v2_nrf5340_cpuapp.overlay new file mode 100644 index 00000000..81f0dc1b --- /dev/null +++ b/boards/openearable_v2_nrf5340_cpuapp.overlay @@ -0,0 +1,21 @@ +/* Preserve the non-FOTA settings layout used before the SDK upgrade. */ +/delete-node/ &boot_partition; +/delete-node/ &slot0_partition; +/delete-node/ &slot1_partition; +/delete-node/ &storage_partition; + +&flash0 { + partitions { + slot0_partition: partition@0 { + compatible = "zephyr,mapped-partition"; + label = "image-0"; + reg = <0x00000000 0x000fc000>; + }; + + storage_partition: partition@fc000 { + compatible = "zephyr,mapped-partition"; + label = "storage"; + reg = <0x000fc000 0x00002000>; + }; + }; +}; diff --git a/boards/openearable_v2_nrf5340_cpuapp_fota.overlay b/boards/openearable_v2_nrf5340_cpuapp_fota.overlay index aa9893d8..e8897a9f 100644 --- a/boards/openearable_v2_nrf5340_cpuapp_fota.overlay +++ b/boards/openearable_v2_nrf5340_cpuapp_fota.overlay @@ -4,6 +4,33 @@ }; }; +/delete-node/ &boot_partition; +/delete-node/ &slot0_partition; +/delete-node/ &slot1_partition; +/delete-node/ &storage_partition; + +&flash0 { + partitions { + boot_partition: partition@0 { + compatible = "zephyr,mapped-partition"; + label = "mcuboot"; + reg = <0x00000000 0x00010000>; + }; + + slot0_partition: partition@10000 { + compatible = "zephyr,mapped-partition"; + label = "image-0"; + reg = <0x00010000 0x000e0000>; + }; + + storage_partition: partition@f0000 { + compatible = "zephyr,mapped-partition"; + label = "storage"; + reg = <0x000f0000 0x00010000>; + }; + }; +}; + &qspi { status = "disabled"; }; @@ -29,6 +56,22 @@ t-exit-dpd = <5000>; dpd-wakeup-sequence = <30000 20 45000>; mxicy,mx25r-power-mode = "high-performance"; + + partitions { + compatible = "fixed-partitions"; + #address-cells = <1>; + #size-cells = <1>; + + slot1_partition: partition@0 { + label = "image-1"; + reg = <0x00000000 0x000e0000>; + }; + + slot3_partition: partition@e0000 { + label = "image-3"; + reg = <0x000e0000 0x00040000>; + }; + }; }; }; @@ -44,4 +87,4 @@ chosen { zephyr,uart-mcumgr = &cdc_acm_uart0; }; -}; \ No newline at end of file +}; diff --git a/boards/teco/openearable_v2/Kconfig.defconfig b/boards/teco/openearable_v2/Kconfig.defconfig index 62e572e2..c3a1be1a 100644 --- a/boards/teco/openearable_v2/Kconfig.defconfig +++ b/boards/teco/openearable_v2/Kconfig.defconfig @@ -1,4 +1,7 @@ -if BOARD_OPENEARABLE_V2_NRF5340_CPUAPP || BOARD_OPENEARABLE_V2_NRF5340_CPUAPP_NS +config HW_STACK_PROTECTION + default ARCH_HAS_STACK_PROTECTION + +if BOARD_OPENEARABLE_V2_NRF5340_CPUAPP || BOARD_OPENEARABLE_V2_NRF5340_CPUAPP_NS # Code Partition: # @@ -23,31 +26,11 @@ if BOARD_OPENEARABLE_V2_NRF5340_CPUAPP || BOARD_OPENEARABLE_V2_NRF5340_CPUAPP_N # # For the non-secure version of the board, the firmware image SRAM is # always restricted to the allocated non-secure SRAM partition. -DT_CHOSEN_Z_CODE_PARTITION := zephyr,code-partition -DT_CHOSEN_Z_SRAM_PARTITION := zephyr,sram-secure-partition - -if BOARD_OPENEARABLE_V2_NRF5340_CPUAPP && TRUSTED_EXECUTION_SECURE - -config FLASH_LOAD_SIZE - default $(dt_chosen_reg_size_hex,$(DT_CHOSEN_Z_CODE_PARTITION)) - -config SRAM_SIZE - default $(dt_chosen_reg_size_int,$(DT_CHOSEN_Z_SRAM_PARTITION),0,K) - -endif # BOARD_OPENEARABLE_V2_NRF5340_CPUAPP && TRUSTED_EXECUTION_SECURE - -if BOARD_OPENEARABLE_V2_NRF5340_CPUAPP_NS - -config FLASH_LOAD_OFFSET - default $(dt_chosen_reg_addr_hex,$(DT_CHOSEN_Z_CODE_PARTITION)) - -config FLASH_LOAD_SIZE - default $(dt_chosen_reg_size_hex,$(DT_CHOSEN_Z_CODE_PARTITION)) - -endif # BOARD_OPENEARABLE_V2_NRF5340_CPUAPP_NS +config BOARD_OPENEARABLE_V2 + select USE_DT_CODE_PARTITION if BOARD_OPENEARABLE_V2_NRF5340_CPUAPP_NS config BT_HCI_IPC - default y if BT + default y if BT_HCI config HEAP_MEM_POOL_ADD_SIZE_BOARD int diff --git a/boards/teco/openearable_v2/Kconfig.openearable_v2 b/boards/teco/openearable_v2/Kconfig.openearable_v2 index 18eed0ce..c3a09730 100644 --- a/boards/teco/openearable_v2/Kconfig.openearable_v2 +++ b/boards/teco/openearable_v2/Kconfig.openearable_v2 @@ -1,4 +1,4 @@ config BOARD_OPENEARABLE_V2 select SOC_NRF5340_CPUAPP_QKAA if BOARD_OPENEARABLE_V2_NRF5340_CPUAPP select SOC_NRF5340_CPUAPP_QKAA if BOARD_OPENEARABLE_V2_NRF5340_CPUAPP_NS - select SOC_NRF5340_CPUNET_QKAA if BOARD_OPENEARABLE_V2_NRF5340_CPUNET \ No newline at end of file + select SOC_NRF5340_CPUNET_QKAA if BOARD_OPENEARABLE_V2_NRF5340_CPUNET diff --git a/boards/teco/openearable_v2/board.yml b/boards/teco/openearable_v2/board.yml index d76f352d..9b8741f2 100644 --- a/boards/teco/openearable_v2/board.yml +++ b/boards/teco/openearable_v2/board.yml @@ -1,8 +1,9 @@ board: name: openearable_v2 + full_name: OpenEarable V2 vendor: teco socs: - - name: nrf5340 + - name: 'nrf5340' variants: - - name: ns - cpucluster: cpuapp \ No newline at end of file + - name: 'ns' + cpucluster: 'cpuapp' diff --git a/boards/teco/openearable_v2/board_init.c b/boards/teco/openearable_v2/board_init.c index 04b59477..9612a196 100644 --- a/boards/teco/openearable_v2/board_init.c +++ b/boards/teco/openearable_v2/board_init.c @@ -2,9 +2,6 @@ #include #include #include // ✅ Correct Power Management API -#include -#include -LOG_MODULE_REGISTER(board_init, LOG_LEVEL_DBG); //#include "nrf5340_audio_common.h" @@ -23,22 +20,6 @@ const struct device *const ls_1_8 = DEVICE_DT_GET(load_switch_1_8_id); const struct device *const ls_3_3 = DEVICE_DT_GET(load_switch_3_3_id); const struct device *const ls_sd = DEVICE_DT_GET(load_switch_sd_id); -static int app_core_hfclk_divider_init(void) -{ - int ret = nrfx_clock_divider_set(NRF_CLOCK_DOMAIN_HFCLK, NRF_CLOCK_HFCLK_DIV_1); - - ret -= NRFX_ERROR_BASE_NUM; - if (ret) { - LOG_WRN("Failed to set app core HFCLK divider to DIV_1: %d", ret); - return ret; - } - - LOG_DBG("App core HFCLK divider set to DIV_1"); - - return 0; -} -SYS_INIT(app_core_hfclk_divider_init, POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT); - struct load_switch_data { struct gpio_dt_spec ctrl_pin; int delay_us; diff --git a/boards/teco/openearable_v2/openearable_v2_nrf5340_cpuapp.dts b/boards/teco/openearable_v2/openearable_v2_nrf5340_cpuapp.dts index a20984d5..fdcfa460 100644 --- a/boards/teco/openearable_v2/openearable_v2_nrf5340_cpuapp.dts +++ b/boards/teco/openearable_v2/openearable_v2_nrf5340_cpuapp.dts @@ -23,4 +23,7 @@ &nfct { status = "disabled"; -}; \ No newline at end of file +}; + +/* Include the Zephyr 4.x devicetree-based flash and SRAM layout. */ +#include diff --git a/boards/teco/openearable_v2/openearable_v2_nrf5340_cpuapp_common.dts b/boards/teco/openearable_v2/openearable_v2_nrf5340_cpuapp_common.dts index d3796b69..65b66b71 100644 --- a/boards/teco/openearable_v2/openearable_v2_nrf5340_cpuapp_common.dts +++ b/boards/teco/openearable_v2/openearable_v2_nrf5340_cpuapp_common.dts @@ -125,7 +125,7 @@ }; adau1860: adau1860@64 { - compatible = "analog,adau1860"; + compatible = "adi,adau1860"; reg = <0x64>; enable-gpios = <&gpio0 4 GPIO_ACTIVE_HIGH>; }; @@ -232,6 +232,10 @@ compatible = "nordic,nrf-spim"; status = "okay"; cs-gpios = <&gpio0 11 GPIO_ACTIVE_LOW>,<&gpio1 1 GPIO_ACTIVE_LOW>; + /* Sample on the clock edge: the inherited 31.25 ns delay can sample + * the next bit of a high-speed SD card's response at 16 MHz. + */ + rx-delay = <0>; pinctrl-0 = <&spi4_default>; pinctrl-1 = <&spi4_sleep>; @@ -262,8 +266,4 @@ nfct-pins-as-gpios; }; - /* Include partition configuration file */ - //#include "openearable_v2_nrf5340_cpuapp_partition_conf.dts" - #include #include "openearable_v2_nrf5340_shared.dtsi" - \ No newline at end of file diff --git a/boards/teco/openearable_v2/openearable_v2_nrf5340_cpuapp_ns.dts b/boards/teco/openearable_v2/openearable_v2_nrf5340_cpuapp_ns.dts index 76f30998..150341a6 100644 --- a/boards/teco/openearable_v2/openearable_v2_nrf5340_cpuapp_ns.dts +++ b/boards/teco/openearable_v2/openearable_v2_nrf5340_cpuapp_ns.dts @@ -5,7 +5,7 @@ */ /dts-v1/; -#include +#include #include "openearable_v2_nrf5340_cpuapp_common.dts" / { @@ -13,8 +13,11 @@ compatible = "raytac,raytac-mdbt53-db-40-cpuapp"; chosen { - zephyr,sram = &sram0_ns; + zephyr,sram = &sram0_ns_app; zephyr,flash = &flash0; zephyr,code-partition = &slot0_ns_partition; }; }; + +/* Include the Zephyr 4.x TF-M-compatible flash and SRAM layout. */ +#include diff --git a/boards/teco/openearable_v2/openearable_v2_nrf5340_cpunet.dts b/boards/teco/openearable_v2/openearable_v2_nrf5340_cpunet.dts index f4541e26..488861b7 100644 --- a/boards/teco/openearable_v2/openearable_v2_nrf5340_cpunet.dts +++ b/boards/teco/openearable_v2/openearable_v2_nrf5340_cpunet.dts @@ -37,23 +37,27 @@ &flash1 { partitions { - compatible = "fixed-partitions"; #address-cells = <1>; #size-cells = <1>; + ranges; boot_partition: partition@0 { + compatible = "zephyr,mapped-partition"; label = "mcuboot"; reg = <0x00000000 0xc000>; }; slot0_partition: partition@c000 { + compatible = "zephyr,mapped-partition"; label = "image-0"; reg = <0x0000C000 0x12000>; }; slot1_partition: partition@1e000 { + compatible = "zephyr,mapped-partition"; label = "image-1"; reg = <0x0001E000 0x12000>; }; storage_partition: partition@3a000 { + compatible = "zephyr,mapped-partition"; label = "storage"; reg = <0x0003a000 0x6000>; }; @@ -61,5 +65,5 @@ }; /* Include default shared RAM configuration file */ -#include -#include "openearable_v2_nrf5340_shared.dtsi" \ No newline at end of file +#include +#include "openearable_v2_nrf5340_shared.dtsi" diff --git a/cmake/sd_spi_fast_write.cmake b/cmake/sd_spi_fast_write.cmake new file mode 100644 index 00000000..c8d56653 --- /dev/null +++ b/cmake/sd_spi_fast_write.cmake @@ -0,0 +1,44 @@ +# Keep the SDK installation unchanged. Build a patched copy of its SD driver. +if(NOT CONFIG_OPENEARABLE_SD_SPI_FAST_WRITE) + return() +endif() + +set(sd_spi_source "${ZEPHYR_BASE}/drivers/sdhc/sdhc_spi.c") +set(sd_spi_patch "${CMAKE_CURRENT_LIST_DIR}/../patches/sdhc-spi-fast-write.patch") +set(sd_spi_build "${CMAKE_CURRENT_BINARY_DIR}/sd-spi-fast-write") +file(MAKE_DIRECTORY "${sd_spi_build}/drivers/sdhc") +configure_file("${sd_spi_source}" "${sd_spi_build}/drivers/sdhc/sdhc_spi.c" COPYONLY) +set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS "${sd_spi_patch}") + +find_package(Git REQUIRED) +execute_process( + COMMAND "${GIT_EXECUTABLE}" "--git-dir=${sd_spi_build}/.git" apply --unsafe-paths + "--directory=${sd_spi_build}" "${sd_spi_patch}" + WORKING_DIRECTORY "${sd_spi_build}" + RESULT_VARIABLE sd_spi_patch_result + ERROR_VARIABLE sd_spi_patch_error +) +if(NOT sd_spi_patch_result EQUAL 0) + message(FATAL_ERROR "Cannot apply SD SPI patch to this SDK: ${sd_spi_patch_error}") +endif() +# An explicit Git directory prevents discovery of a parent checkout, where +# git apply can silently skip paths under an ignored build directory. +file(SHA256 "${sd_spi_source}" sd_spi_original_hash) +file(SHA256 "${sd_spi_build}/drivers/sdhc/sdhc_spi.c" sd_spi_patched_hash) +if(sd_spi_original_hash STREQUAL sd_spi_patched_hash) + message(FATAL_ERROR "SD SPI patch was skipped; refusing an unpatched build") +endif() + +get_target_property(sd_spi_sources drivers__sdhc SOURCES) +set(sd_spi_replaced FALSE) +foreach(source IN LISTS sd_spi_sources) + if(source MATCHES "(^|/)sdhc_spi\\.c$") + list(REMOVE_ITEM sd_spi_sources "${source}") + list(APPEND sd_spi_sources "${sd_spi_build}/drivers/sdhc/sdhc_spi.c") + set(sd_spi_replaced TRUE) + endif() +endforeach() +if(NOT sd_spi_replaced) + message(FATAL_ERROR "SDK SD SPI driver source was not found") +endif() +set_property(TARGET drivers__sdhc PROPERTY SOURCES "${sd_spi_sources}") diff --git a/dts/bindings/analog,adau1860.yaml b/dts/bindings/adi,adau1860.yaml similarity index 74% rename from dts/bindings/analog,adau1860.yaml rename to dts/bindings/adi,adau1860.yaml index 394419b6..5c207b38 100644 --- a/dts/bindings/analog,adau1860.yaml +++ b/dts/bindings/adi,adau1860.yaml @@ -6,9 +6,9 @@ include: [sensor-device.yaml, i2c-device.yaml] -compatible: "analog,adau1860" +compatible: "adi,adau1860" properties: enable-gpios: type: phandle-array - description: DSP enable \ No newline at end of file + description: DSP enable diff --git a/include/openearable_common.h b/include/openearable_common.h index 40a1c38c..7bad7f27 100644 --- a/include/openearable_common.h +++ b/include/openearable_common.h @@ -10,7 +10,7 @@ #define ZBUS_READ_TIMEOUT_MS K_MSEC(100) #define ZBUS_ADD_OBS_TIMEOUT_MS K_MSEC(200) -#define SENSOR_DATA_FIXED_LENGTH 38 +#define SENSOR_DATA_FIXED_LENGTH 98 #define millis() k_cyc_to_ms_floor64(k_uptime_ticks()) diff --git a/include/zbus_common.h b/include/zbus_common.h index 80bfddf4..a9d2ea6d 100644 --- a/include/zbus_common.h +++ b/include/zbus_common.h @@ -13,6 +13,7 @@ #define ZBUS_READ_TIMEOUT_MS K_MSEC(100) #define ZBUS_ADD_OBS_TIMEOUT_MS K_MSEC(200) +#define ZBUS_STREAMS_NUM_MAX 8 /***** Messages for zbus ******/ @@ -35,6 +36,7 @@ enum le_audio_evt_type { LE_AUDIO_EVT_SYNC_LOST, LE_AUDIO_EVT_NO_VALID_CFG, LE_AUDIO_EVT_COORD_SET_DISCOVERED, + LE_AUDIO_EVT_DISCOVERY_COMPLETE, }; struct le_audio_msg { @@ -45,6 +47,16 @@ struct le_audio_msg { uint8_t set_size; uint8_t const *sirk; struct stream_index idx; + struct bt_bap_stream *stream; +}; + +enum tx_data_status { + STATUS_NOT_SET = 0, /* Initial status */ + STATUS_SENT_WITH_TS, /* Data sent with timestamp */ + STATUS_SENT_WITHOUT_TS, /* Data sent without timestamp */ + STATUS_OVERRUN_FLUSHED, /* Data overrun, flushed */ + STATUS_UNDERRUN_EMPTY_SDU_ON_AIR, /* Data underrun, empty SDU on air */ + STATUS_ERROR, /* Error occurred (e.g. failed to send) */ }; /** @@ -53,14 +65,20 @@ struct le_audio_msg { */ struct sdu_ref_msg { uint32_t tx_sync_ts_us; + bool tx_sync_ts_us_valid; uint32_t curr_ts_us; + enum tx_data_status status; bool adjust; + struct stream_index idx[ZBUS_STREAMS_NUM_MAX]; + uint8_t num_idx; }; enum bt_mgmt_evt_type { BT_MGMT_EXT_ADV_WITH_PA_READY = 1, BT_MGMT_CONNECTED, BT_MGMT_SECURITY_CHANGED, + BT_MGMT_PAIRING_COMPLETE, + BT_MGMT_BOND_DELETED, BT_MGMT_PA_SYNCED, BT_MGMT_PA_SYNC_LOST, BT_MGMT_DISCONNECTED, @@ -76,6 +94,8 @@ struct bt_mgmt_msg { struct bt_le_per_adv_sync *pa_sync; uint32_t broadcast_id; uint8_t pa_sync_term_reason; + bt_addr_le_t addr; + bt_addr_le_t identity; }; enum volume_evt_type { diff --git a/patches/sdhc-spi-fast-write.patch b/patches/sdhc-spi-fast-write.patch new file mode 100644 index 00000000..3b8ad46a --- /dev/null +++ b/patches/sdhc-spi-fast-write.patch @@ -0,0 +1,183 @@ +--- a/drivers/sdhc/sdhc_spi.c ++++ b/drivers/sdhc/sdhc_spi.c +@@ -15,6 +15,7 @@ + #include + #include + #include ++#include + + LOG_MODULE_REGISTER(sdhc_spi, CONFIG_SDHC_LOG_LEVEL); + +@@ -103,6 +104,8 @@ + struct spi_config cfg_a; + struct spi_config cfg_b; + uint8_t scratch[MAX_CMD_READ]; ++ uint32_t write_frequency; ++ bool speed_checked; + }; + + /* Receives a block of bytes */ +@@ -209,19 +212,24 @@ + { + const struct sdhc_spi_config *config = dev->config; + struct sdhc_spi_data *data = dev->data; +- int ret; +- uint8_t response; +- +- while (timeout_ms > 0) { +- ret = sdhc_spi_rx(config->spi_dev, data->spi_cfg, &response, 1); +- if (ret) { +- return ret; +- } +- if (response == 0xFF) { ++ const int64_t deadline = k_uptime_get() + timeout_ms; ++ uint8_t response[32]; ++ ++ while (k_uptime_get() < deadline) { ++ /* Busy releases to all ones until the next command. Clock a small ++ * block rather than scheduling a DMA transfer for every byte. ++ */ ++ int ret = sdhc_spi_rx(config->spi_dev, data->spi_cfg, ++ response, sizeof(response)); ++ if (ret) { ++ return ret; ++ } ++ if (response[sizeof(response) - 1] == 0xFF) { + return 0; + } +- k_msleep(k_ticks_to_ms_floor32(interval_ticks)); +- timeout_ms -= k_ticks_to_ms_floor32(interval_ticks); ++ if (interval_ticks > 0) { ++ k_sleep(K_TICKS(interval_ticks)); ++ } + } + return -ETIMEDOUT; + } +@@ -541,6 +549,64 @@ + return ret; + } + ++/* CMD6 changes card timing without changing its SPI protocol or file format. ++ * Only write blocks use the higher clock: nRF5340 receive remains <= 16 MHz. ++ */ ++static int sdhc_spi_enable_fast_write(const struct device *dev) ++{ ++ const struct sdhc_spi_config *config = dev->config; ++ struct sdhc_spi_data *dev_data = dev->data; ++ uint8_t status[64]; ++ struct sdhc_command cmd = { ++ .opcode = SD_SWITCH, ++ .arg = 0x00fffff1U, ++ .response_type = SD_SPI_RSP_TYPE_R1, ++ .timeout_ms = CONFIG_SD_CMD_TIMEOUT, ++ }; ++ struct sdhc_data data = { ++ .block_size = sizeof(status), ++ .blocks = 1, ++ .data = status, ++ .timeout_ms = CONFIG_SD_DATA_TIMEOUT, ++ }; ++ int ret; ++ ++ if (dev_data->speed_checked || config->spi_max_freq < 32000000U || ++ dev_data->spi_cfg->frequency <= 16000000U || ++ nrf_clock_hfclk_div_get(NRF_CLOCK) != NRF_CLOCK_HFCLK_DIV_1) { ++ return 0; ++ } ++ ++ /* Check support, then request high-speed timing, both at the old clock. */ ++ for (int mode = 0; mode < 2; mode++) { ++ cmd.arg = 0x00fffff1U | ((uint32_t)mode << 31); ++ ret = sdhc_spi_send_cmd(dev, &cmd, true); ++ if (ret == -EINVAL && (cmd.response[0] & SD_SPI_R1ILLEGAL_CMD_ERR)) { ++ dev_data->speed_checked = true; ++ return 0; ++ } ++ if (ret) { ++ return ret; ++ } ++ ret = sdhc_spi_read_data(dev, &data); ++ if (ret) { ++ return ret; ++ } ++ if (!(status[13] & BIT(1)) || ++ (status[17] == 1 && (status[29] & BIT(1)))) { ++ dev_data->speed_checked = true; ++ return 0; ++ } ++ } ++ ++ dev_data->speed_checked = true; ++ if ((status[16] & 0x0f) == 1) { ++ dev_data->write_frequency = 32000000U; ++ LOG_INF("SD high-speed writes enabled: 32 MHz TX, 16 MHz RX"); ++ } ++ return 0; ++} ++ + /* Handles writing data to SD SPI device */ + static int sdhc_spi_write_data(const struct device *dev, struct sdhc_data *data) + { +@@ -584,7 +650,17 @@ + /* Build the CRC for this data block */ + sys_put_be16(crc16_itu_t(0, write_location, data->block_size), + crc); ++ /* Keep the same SPI_LOCK_ON owner and CS asserted. The NCS 3.4.1 ++ * nrfx driver compares configuration values, so it applies each ++ * frequency change even though the config pointer is unchanged. ++ */ ++ uint32_t read_frequency = dev_data->spi_cfg->frequency; ++ ++ if (dev_data->write_frequency != 0) { ++ dev_data->spi_cfg->frequency = dev_data->write_frequency; ++ } + ret = spi_write(config->spi_dev, dev_data->spi_cfg, &tx); ++ dev_data->spi_cfg->frequency = read_frequency; + if (ret) { + return ret; + } +@@ -648,6 +724,14 @@ + /* Request SPI bus to be active */ + if (pm_device_runtime_get(config->spi_dev) < 0) { + return -EIO; ++ } ++ ++ if (data != NULL && (cmd->opcode == SD_WRITE_SINGLE_BLOCK || ++ cmd->opcode == SD_WRITE_MULTIPLE_BLOCK)) { ++ ret = sdhc_spi_enable_fast_write(dev); ++ if (ret) { ++ goto release; ++ } + } + + if (data == NULL) { +@@ -687,6 +771,7 @@ + } while ((ret != 0) && (retries > 0)); + } + ++release: + /* Release SPI bus */ + ret2 = spi_release(config->spi_dev, dev_data->spi_cfg); + +@@ -701,6 +786,10 @@ + const struct sdhc_spi_config *cfg = dev->config; + struct sdhc_spi_data *data = dev->data; + ++ if (ios->clock <= SDMMC_CLOCK_400KHZ || ios->power_mode == SDHC_POWER_OFF) { ++ data->write_frequency = 0; ++ data->speed_checked = false; ++ } + if (ios->clock != data->spi_cfg->frequency) { + if (ios->clock > cfg->spi_max_freq) { + return -ENOTSUP; +@@ -800,7 +889,9 @@ + { + struct sdhc_spi_data *data = dev->data; + +- /* Reset host I/O */ ++ /* Reset host I/O and renegotiate after a card reset. */ ++ data->write_frequency = 0; ++ data->speed_checked = false; + data->spi_cfg->frequency = SDMMC_CLOCK_400KHZ; + return 0; + } diff --git a/prj.conf b/prj.conf index 1a747255..9d3a1667 100644 --- a/prj.conf +++ b/prj.conf @@ -6,14 +6,20 @@ # CONFIG_STATIC_INIT_GNU=y -# nRF5340 Audio -CONFIG_NRF5340_AUDIO=y +# nRF Audio +CONFIG_APP_NRF_AUDIO=y +CONFIG_USE_DT_CODE_PARTITION=y CONFIG_SAMPLE_RATE_CONVERTER=y CONFIG_SAMPLE_RATE_CONVERTER_FILTER_SIMPLE=y +# General nRF Audio runtime requirements +CONFIG_BT=y +CONFIG_NET_BUF_POOL_USAGE=y +CONFIG_BT_RX_STACK_SIZE=2500 +CONFIG_POLL=y + # General CONFIG_DEBUG=y -CONFIG_DEBUG_INFO=y CONFIG_ASSERT=y CONFIG_STACK_USAGE=y CONFIG_THREAD_RUNTIME_STATS=y @@ -25,11 +31,12 @@ CONFIG_SERIAL=y # Logging CONFIG_LOG=y -CONFIG_NEWLIB_LIBC_FLOAT_PRINTF=y CONFIG_LOG_TAG_MAX_LEN=2 CONFIG_LOG_TAG_DEFAULT="--" CONFIG_LOG_BACKEND_UART=y CONFIG_LOG_BUFFER_SIZE=4096 +CONFIG_LOG_RUNTIME_DEFAULT_LEVEL=4 +CONFIG_BT_SECURITY_ERR_TO_STR=y # Use this for debugging thread usage #CONFIG_LOG_THREAD_ID_PREFIX=y @@ -72,12 +79,12 @@ CONFIG_SD_LOG_LEVEL_OFF=y # Suppress LOG_INF messages from hci_core CONFIG_BT_HCI_CORE_LOG_LEVEL_WRN=y -CONFIG_NRF5340_AUDIO_ADAU1860_DRIVER=y +CONFIG_NRF_AUDIO_ADAU1860_DRIVER=y CONFIG_NRF5340_AUDIO_POWER_MEASUREMENT=n -CONFIG_NRF5340_AUDIO_SD_CARD_MODULE=y +CONFIG_NRF_AUDIO_SD_CARD_MODULE=y #Custom -CONFIG_NRFX_PWM1=y +CONFIG_NRFX_PWM=y CONFIG_I2C=y CONFIG_I2C_NRFX=y @@ -98,9 +105,10 @@ CONFIG_AUTO_OFF_TIMEOUT_AGGRESSIVE=5 CONFIG_AUTO_OFF_TIMEOUT_BALANCED=15 CONFIG_AUTO_OFF_TIMEOUT_MINIMAL=30 -CONFIG_PM=y # may cause delay issues for ISO stream ? CONFIG_PM_DEVICE=y CONFIG_PM_DEVICE_RUNTIME=y +# PowerManager explicitly enables runtime PM for the managed load switches. +CONFIG_PM_DEVICE_RUNTIME_DEFAULT_ENABLE=n CONFIG_BT_DEVICE_NAME="OpenEarable" @@ -117,11 +125,11 @@ CONFIG_CMSIS_DSP=y CONFIG_CMSIS_DSP_TRANSFORM=y CONFIG_CMSIS_DSP_COMPLEXMATH=y -CONFIG_NEWLIB_LIBC=y -# Maximum seal-check waveform, duplex LC3, and an 8 KiB runtime reserve. -CONFIG_NEWLIB_LIBC_MIN_REQUIRED_HEAP_SIZE=115200 -CONFIG_NEWLIB_LIBC_FLOAT_PRINTF=y -CONFIG_NEWLIB_LIBC_FLOAT_SCANF=y +CONFIG_PICOLIBC=y +CONFIG_PICOLIBC_IO_FLOAT=y +# Seal-check waveform + LC3 sessions + 8 KiB reserve, as before the SDK upgrade. +CONFIG_COMMON_LIBC_MALLOC_ARENA_SIZE=-1 +CONFIG_AUDIO_MIN_RUNTIME_HEAP_SIZE=115200 CONFIG_CPP=y CONFIG_GLIBCXX_LIBCPP=y @@ -140,16 +148,24 @@ CONFIG_UART_LINE_CTRL=y CONFIG_BT_USER_DATA_LEN_UPDATE=y CONFIG_BT_USER_PHY_UPDATE=y -CONFIG_BT_CTLR_DATA_LENGTH_MAX=251 +# Negotiate the link settings previously requested by connected_cb. +CONFIG_BT_AUTO_DATA_LEN_UPDATE=y +CONFIG_BT_AUTO_PHY_PERIPHERAL_2M=y + CONFIG_BT_BUF_ACL_RX_SIZE=502 CONFIG_BT_BUF_ACL_RX_COUNT_EXTRA=8 CONFIG_BT_BUF_ACL_TX_SIZE=502 CONFIG_BT_L2CAP_TX_MTU=498 -CONFIG_BT_CTLR_PHY_2M=y -CONFIG_BT_CTLR_PHY_CODED=n -CONFIG_BT_ISO_TX_BUF_COUNT=1 -CONFIG_BT_ISO_RX_BUF_COUNT=1 +CONFIG_BT_ISO_TX_BUF_COUNT=3 +# Full advertised mono LC3 range: 124 kbit/s at 10 ms = 155 bytes. +CONFIG_BT_ISO_TX_MTU=155 +CONFIG_BT_ISO_RX_MTU=155 +CONFIG_BT_ISO_RX_BUF_COUNT=4 +CONFIG_BT_BUF_ACL_TX_COUNT=12 +CONFIG_BT_ATT_TX_COUNT=12 + +CONFIG_SPEED_OPTIMIZATIONS=y #CONFIG_BT_BUF_ACL_TX_COUNT=20 #CONFIG_BT_L2CAP_TX_BUF_COUNT=20 diff --git a/prj_fota.conf b/prj_fota.conf index 7294933b..9acc7190 100644 --- a/prj_fota.conf +++ b/prj_fota.conf @@ -6,14 +6,19 @@ # CONFIG_STATIC_INIT_GNU=y -# nRF5340 Audio -CONFIG_NRF5340_AUDIO=y +# nRF Audio +CONFIG_APP_NRF_AUDIO=y CONFIG_SAMPLE_RATE_CONVERTER=y CONFIG_SAMPLE_RATE_CONVERTER_FILTER_SIMPLE=y +# General nRF Audio runtime requirements +CONFIG_BT=y +CONFIG_NET_BUF_POOL_USAGE=y +CONFIG_BT_RX_STACK_SIZE=2500 +CONFIG_POLL=y + # General CONFIG_DEBUG=y -CONFIG_DEBUG_INFO=y CONFIG_ASSERT=y CONFIG_STACK_USAGE=y CONFIG_THREAD_RUNTIME_STATS=y @@ -25,11 +30,12 @@ CONFIG_SERIAL=y # Logging CONFIG_LOG=y -CONFIG_NEWLIB_LIBC_FLOAT_PRINTF=y CONFIG_LOG_TAG_MAX_LEN=2 CONFIG_LOG_TAG_DEFAULT="--" CONFIG_LOG_BACKEND_UART=y CONFIG_LOG_BUFFER_SIZE=4096 +CONFIG_LOG_RUNTIME_DEFAULT_LEVEL=4 +CONFIG_BT_SECURITY_ERR_TO_STR=y # Use this for debugging thread usage #CONFIG_LOG_THREAD_ID_PREFIX=y @@ -85,12 +91,12 @@ CONFIG_FLASH_MAP=y CONFIG_SPI_NOR=y CONFIG_SPI_NOR_SFDP_DEVICETREE=y -CONFIG_NRF5340_AUDIO_ADAU1860_DRIVER=y +CONFIG_NRF_AUDIO_ADAU1860_DRIVER=y CONFIG_NRF5340_AUDIO_POWER_MEASUREMENT=n -CONFIG_NRF5340_AUDIO_SD_CARD_MODULE=y +CONFIG_NRF_AUDIO_SD_CARD_MODULE=y #Custom -CONFIG_NRFX_PWM1=y +CONFIG_NRFX_PWM=y CONFIG_I2C=y CONFIG_I2C_NRFX=y @@ -100,6 +106,8 @@ CONFIG_POWEROFF=y #CONFIG_PM=y CONFIG_PM_DEVICE=y CONFIG_PM_DEVICE_RUNTIME=y +# PowerManager explicitly enables runtime PM for the managed load switches. +CONFIG_PM_DEVICE_RUNTIME_DEFAULT_ENABLE=n CONFIG_BT_DEVICE_NAME="OpenEarable" @@ -112,11 +120,11 @@ CONFIG_SFLOAT=y CONFIG_FPU=y -CONFIG_NEWLIB_LIBC=y -# Maximum seal-check waveform, duplex LC3, and an 8 KiB runtime reserve. -CONFIG_NEWLIB_LIBC_MIN_REQUIRED_HEAP_SIZE=115200 -CONFIG_NEWLIB_LIBC_FLOAT_PRINTF=y -CONFIG_NEWLIB_LIBC_FLOAT_SCANF=y +CONFIG_PICOLIBC=y +CONFIG_PICOLIBC_IO_FLOAT=y +# Seal-check waveform + LC3 sessions + 8 KiB reserve, as before the SDK upgrade. +CONFIG_COMMON_LIBC_MALLOC_ARENA_SIZE=-1 +CONFIG_AUDIO_MIN_RUNTIME_HEAP_SIZE=115200 CONFIG_CPP=y CONFIG_GLIBCXX_LIBCPP=y @@ -135,22 +143,24 @@ CONFIG_UART_LINE_CTRL=y CONFIG_BT_USER_DATA_LEN_UPDATE=y CONFIG_BT_USER_PHY_UPDATE=y -CONFIG_BT_CTLR_DATA_LENGTH_MAX=251 +# Negotiate the link settings previously requested by connected_cb. +CONFIG_BT_AUTO_DATA_LEN_UPDATE=y +CONFIG_BT_AUTO_PHY_PERIPHERAL_2M=y + CONFIG_BT_BUF_ACL_RX_SIZE=502 CONFIG_BT_BUF_ACL_RX_COUNT_EXTRA=8 CONFIG_BT_BUF_ACL_TX_SIZE=502 CONFIG_BT_L2CAP_TX_MTU=498 -CONFIG_BT_CTLR_PHY_2M=y -CONFIG_BT_CTLR_PHY_CODED=n - -CONFIG_BT_ISO_TX_BUF_COUNT=4 -CONFIG_BT_ISO_RX_BUF_COUNT=8 -CONFIG_BT_CTLR_RX_BUFFERS=8 -#CONFIG_BT_CTLR_TX_BUFFERS=8 +CONFIG_BT_ISO_TX_BUF_COUNT=3 +# Full advertised mono LC3 range: 124 kbit/s at 10 ms = 155 bytes. +CONFIG_BT_ISO_TX_MTU=155 +CONFIG_BT_ISO_RX_MTU=155 +CONFIG_BT_ISO_RX_BUF_COUNT=4 +CONFIG_BT_BUF_ACL_TX_COUNT=12 +CONFIG_BT_ATT_TX_COUNT=12 -CONFIG_BT_CTLR_ISO_TX_BUFFERS=16 -CONFIG_BT_CTLR_ISO_RX_BUFFERS=16 +CONFIG_SPEED_OPTIMIZATIONS=y #CONFIG_BT_ISO_RX_MTU=498 #CONFIG_BT_ISO_TX_MTU=498 @@ -232,6 +242,7 @@ CONFIG_MCUMGR_GRP_OS=y CONFIG_MCUMGR_GRP_IMG=y # Configure MCUMGR transport to UART +CONFIG_UART_MCUMGR=y CONFIG_MCUMGR_TRANSPORT_UART=y # Dependencies diff --git a/prj_release.conf b/prj_release.conf index 1adec71c..47defd3d 100644 --- a/prj_release.conf +++ b/prj_release.conf @@ -4,8 +4,8 @@ # SPDX-License-Identifier: LicenseRef-Nordic-5-Clause # -# nRF5340 Audio -CONFIG_NRF5340_AUDIO=y +# nRF Audio +CONFIG_APP_NRF_AUDIO=y CONFIG_SAMPLE_RATE_CONVERTER=y CONFIG_SAMPLE_RATE_CONVERTER_FILTER_SIMPLE=y @@ -18,17 +18,21 @@ CONFIG_THREAD_MONITOR=n CONFIG_SERIAL=n CONFIG_CONSOLE=n CONFIG_PRINTK=n +CONFIG_BT_SETTINGS_USE_PRINTK=n +CONFIG_NCS_BOOT_BANNER=n CONFIG_UART_CONSOLE=n CONFIG_BOOT_BANNER=n +CONFIG_NET_BUF_POOL_USAGE=y +CONFIG_BT_RX_STACK_SIZE=2500 +CONFIG_POLL=y -# Bluetooth -# Default value from src/bluetooth/Kconfig.defaults. -# BT_PRIVACY is default turned off to ease the development. -# Should be turned on before production. -# CONFIG_BT_PRIVACY=y +# BT related configs +CONFIG_BT_ISO_TX_BUF_COUNT=3 +CONFIG_BT_BUF_ACL_TX_COUNT=3 +CONFIG_BT_ATT_TX_COUNT=12 -# USB -# Default values from src/modules/Kconfig.defaults. -# USB VID and PID must be changed before production. -# CONFIG_USB_DEVICE_VID=0x1915 -# CONFIG_USB_DEVICE_PID=0x530A +CONFIG_SPEED_OPTIMIZATIONS=y + +# Enable link-time-optimizations to improve performance and reduce size +CONFIG_LTO=y +CONFIG_ISR_TABLES_LOCAL_DECLARATION=y diff --git a/protocol b/protocol index 53221c84..bf61cbe7 160000 --- a/protocol +++ b/protocol @@ -1 +1 @@ -Subproject commit 53221c847f2c00924ed80e8f0b1b0c90d3a5eba0 +Subproject commit bf61cbe72922045ff850dbdbabfaece8c7a38177 diff --git a/sample.yaml b/sample.yaml index 7db32333..ba0a5a2b 100644 --- a/sample.yaml +++ b/sample.yaml @@ -1,6 +1,6 @@ sample: - name: nRF5340 Audio application - description: LE Audio and Auracast implementation example + name: OpenEarable V2 application + description: OpenEarable V2 LE Audio application common: integration_platforms: - openearable_v2/nrf5340/cpuapp @@ -9,17 +9,14 @@ common: build_only: true tags: ci_build sysbuild tests: - applications.nrf5340_audio.default: - extra_args: [] - applications.nrf5340_audio.headset_unicast: - extra_args: FILE_SUFFIX=release CONFIG_AUDIO_DEV=1 - applications.nrf5340_audio.gateway_unicast: - extra_args: FILE_SUFFIX=release CONFIG_AUDIO_DEV=2 - applications.nrf5340_audio.headset_broadcast: - extra_args: FILE_SUFFIX=release CONFIG_AUDIO_DEV=1 CONFIG_TRANSPORT_BIS=y - applications.nrf5340_audio.gateway_broadcast: - extra_args: FILE_SUFFIX=release CONFIG_AUDIO_DEV=2 CONFIG_TRANSPORT_BIS=y - applications.nrf5340_audio.headset_unicast_sd_card: - extra_args: FILE_SUFFIX=release CONFIG_AUDIO_DEV=1 CONFIG_SD_CARD_PLAYBACK=y - applications.nrf5340_audio.headset_dfu: - extra_args: FILE_SUFFIX=release CONFIG_AUDIO_DEV=1 FILE_SUFFIX=fota + applications.openearable_v2.unicast_debug: + extra_args: + - EXTRA_CONF_FILE=unicast_server/overlay-unicast_server.conf + applications.openearable_v2.unicast_release: + extra_args: + - FILE_SUFFIX=release + - EXTRA_CONF_FILE=unicast_server/overlay-unicast_server.conf + applications.openearable_v2.unicast_fota: + extra_args: + - FILE_SUFFIX=fota + - EXTRA_CONF_FILE=unicast_server/overlay-unicast_server.conf diff --git a/src/Battery/BQ27220.cpp b/src/Battery/BQ27220.cpp index cf21a764..63761f7f 100644 --- a/src/Battery/BQ27220.cpp +++ b/src/Battery/BQ27220.cpp @@ -411,6 +411,12 @@ void BQ27220::setup(const battery_settings &_battery_settings, bool init) { // sleep current (void)write_RAM(0x9217, 1); + // Reduce SLEEP-mode sampling from the 20 s defaults to save off-state power. + // Sleep Voltage Time (0x921E, also temperature) and Sleep Current Time + // (0x921F) are adjacent one-byte intervals in seconds. + uint8_t sleep_intervals[] = {60, 60}; + (void)write_RAM(0x921E, sleep_intervals, sizeof(sleep_intervals)); + // dischage current trd (void)write_RAM(0x9228, 2); // charge current trd diff --git a/src/Battery/PowerManager.cpp b/src/Battery/PowerManager.cpp index 77ba0760..62f0bbf5 100644 --- a/src/Battery/PowerManager.cpp +++ b/src/Battery/PowerManager.cpp @@ -324,7 +324,12 @@ int PowerManager::begin() { // check setup op_state state = fuel_gauge.operation_state(); - if (state.SEC != BQ27220::SEALED) { + bool setup_fuel_gauge = state.SEC != BQ27220::SEALED; +#if defined(CONFIG_BOOTLOADER_MCUBOOT) && defined(CONFIG_SETUP_FUEL_GAUGE) + setup_fuel_gauge |= !boot_is_img_confirmed(); +#endif + // Finish the unseal/configuration sequence before charge_ctrl_delayable can read the gauge. + if (setup_fuel_gauge) { //battery_controller.setup(); fuel_gauge.setup(_battery_settings); } @@ -436,9 +441,6 @@ int PowerManager::begin() { sys_reboot(SYS_REBOOT_COLD); } LOG_INF("Image confirmed"); - #ifdef CONFIG_SETUP_FUEL_GAUGE - fuel_gauge.setup(_battery_settings); - #endif } #endif @@ -561,8 +563,6 @@ void bt_disconnect_handler(struct bt_conn *conn, void * data) { } void PowerManager::reboot() { - int ret; - // disconnect devices uint8_t data = BT_HCI_ERR_REMOTE_USER_TERM_CONN; bt_conn_foreach(BT_CONN_TYPE_ALL, bt_disconnect_handler, &data); @@ -571,9 +571,7 @@ void PowerManager::reboot() { stop_sensor_manager(); - ret = bt_mgmt_stop_watchdog(); - ERR_CHK(ret); - + (void)bt_mgmt_stop_watchdog(); dac.end(); sys_reboot(SYS_REBOOT_COLD); @@ -625,10 +623,8 @@ int PowerManager::power_down(bool fault) { } else { LOG_INF("Power off"); } - LOG_PANIC(); - (void)bt_mgmt_stop_watchdog(); - //ERR_CHK(ret); + LOG_PANIC(); dac.end(); diff --git a/src/ParseInfo/DefaultSensors.h b/src/ParseInfo/DefaultSensors.h index e6daef36..0a55dc11 100644 --- a/src/ParseInfo/DefaultSensors.h +++ b/src/ParseInfo/DefaultSensors.h @@ -60,10 +60,11 @@ SensorComponentGroup imuGroups[IMU_GROUP_COUNT] = { // ============= BoneConductionIMU ============= #define BONE_CONDUCTION_ACC_COUNT 3 +// Raw BMA580 counts at +/-2 g: divide by 16384 to convert LSB to g. SensorComponent boneConductionIMUComponents[BONE_CONDUCTION_ACC_COUNT] = { - { .name = "X", .unit = "g", .parseType = PARSE_TYPE_INT16 }, - { .name = "Y", .unit = "g", .parseType = PARSE_TYPE_INT16 }, - { .name = "Z", .unit = "g", .parseType = PARSE_TYPE_INT16 }, + { .name = "X", .unit = "LSB", .parseType = PARSE_TYPE_INT16 }, + { .name = "Y", .unit = "LSB", .parseType = PARSE_TYPE_INT16 }, + { .name = "Z", .unit = "LSB", .parseType = PARSE_TYPE_INT16 }, }; #define BONE_CONDUCTION_IMU_GROUP_COUNT 1 @@ -107,7 +108,7 @@ SensorComponent baroTempComponents[BARO_TEMP_COUNT] = { #define BARO_PRESSURE_COUNT 1 SensorComponent baroPressureComponents[BARO_PRESSURE_COUNT] = { - { .name = "Pressure", .unit = "kPa", .parseType = PARSE_TYPE_FLOAT }, + { .name = "Pressure", .unit = "Pa", .parseType = PARSE_TYPE_FLOAT }, }; #define BARO_GROUP_COUNT 2 @@ -174,8 +175,8 @@ SensorScheme defaultSensors[SENSOR_COUNT] = { .availableOptions = DATA_STREAMING | DATA_STORAGE | FREQUENCIES_DEFINED, .frequencyOptions = { .frequencyCount = sizeof(Temp::sample_rates.reg_vals), - .defaultFrequencyIndex = 4, - .maxBleFrequencyIndex = 7, + .defaultFrequencyIndex = 3, + .maxBleFrequencyIndex = 6, .frequencies = Temp::sample_rates.sample_rates, }, }, @@ -189,8 +190,8 @@ SensorScheme defaultSensors[SENSOR_COUNT] = { .availableOptions = DATA_STREAMING | DATA_STORAGE | FREQUENCIES_DEFINED, .frequencyOptions = { .frequencyCount = sizeof(Baro::sample_rates.reg_vals), - .defaultFrequencyIndex = 12, - .maxBleFrequencyIndex = 17, + .defaultFrequencyIndex = 0, + .maxBleFrequencyIndex = 3, .frequencies = Baro::sample_rates.sample_rates, }, }, @@ -204,8 +205,8 @@ SensorScheme defaultSensors[SENSOR_COUNT] = { .availableOptions = DATA_STREAMING | DATA_STORAGE | FREQUENCIES_DEFINED, .frequencyOptions = { .frequencyCount = sizeof(BoneConduction::sample_rates.reg_vals), - .defaultFrequencyIndex = 2, - .maxBleFrequencyIndex = 6, + .defaultFrequencyIndex = 1, + .maxBleFrequencyIndex = 5, .frequencies = BoneConduction::sample_rates.sample_rates, }, }, diff --git a/src/ParseInfo/SensorScheme.cpp b/src/ParseInfo/SensorScheme.cpp index 6639af5a..a3350e6b 100644 --- a/src/ParseInfo/SensorScheme.cpp +++ b/src/ParseInfo/SensorScheme.cpp @@ -386,7 +386,8 @@ size_t getParseInfoStorageSize() { return size; } -ssize_t serializeParseInfoStorage(char* buffer, size_t bufferSize) { +ssize_t serializeParseInfoStorage(char* buffer, size_t bufferSize, + const struct sensor_config* configs, size_t configCount) { if ((parseInfoSchemeStruct == NULL) || (parseInfoScheme == NULL) || (buffer == NULL)) { return -ENODATA; } @@ -415,7 +416,18 @@ ssize_t serializeParseInfoStorage(char* buffer, size_t bufferSize) { memcpy(buffer, &encodedSchemeSize, sizeof(encodedSchemeSize)); buffer += sizeof(encodedSchemeSize); - ssize_t writtenSize = serializeSensorScheme(scheme, buffer, bufferSize - (buffer - bufferStart)); + // Snapshot the selected rates for this file without changing BLE parse info. + SensorScheme schemeForHeader = *scheme; + for (size_t j = 0; j < configCount; j++) { + if (configs[j].sensorId == scheme->id && + configs[j].sampleRateIndex < scheme->configOptions.frequencyOptions.frequencyCount) { + schemeForHeader.configOptions.frequencyOptions.defaultFrequencyIndex = + configs[j].sampleRateIndex; + break; + } + } + ssize_t writtenSize = serializeSensorScheme(&schemeForHeader, buffer, + bufferSize - (buffer - bufferStart)); if (writtenSize < 0) { return writtenSize; } diff --git a/src/ParseInfo/SensorScheme.h b/src/ParseInfo/SensorScheme.h index 6aad24a2..bd721fc6 100644 --- a/src/ParseInfo/SensorScheme.h +++ b/src/ParseInfo/SensorScheme.h @@ -81,7 +81,9 @@ size_t getParseInfoStorageSize(); * @param bufferSize Available bytes in @p buffer. * @return Number of bytes written on success, or a negative errno value. */ -ssize_t serializeParseInfoStorage(char* buffer, size_t bufferSize); +struct sensor_config; +ssize_t serializeParseInfoStorage(char* buffer, size_t bufferSize, + const struct sensor_config* configs, size_t configCount); float getSampleRateForSensorId(uint8_t id, uint8_t frequencyIndex); float getSampleRateForSensor(struct SensorScheme* sensorScheme, uint8_t frequencyIndex); diff --git a/src/SD_Card/SDLogger/SDLogger.cpp b/src/SD_Card/SDLogger/SDLogger.cpp index 4635c1da..ef54042e 100644 --- a/src/SD_Card/SDLogger/SDLogger.cpp +++ b/src/SD_Card/SDLogger/SDLogger.cpp @@ -46,6 +46,9 @@ uint8_t buffer[BUFFER_SIZE]; // Ring Buffer Speicher static atomic_t g_stop_writing; // 1 while end()/flush/close is in progress static atomic_t g_sd_removed; // 1 if SD was removed while recording +static constexpr int64_t LOG_SYNC_INTERVAL_MS = 5 * 1000; +static int64_t last_sync_uptime_ms; + uint32_t count_max_buffer_fill = 0; struct k_poll_signal logger_sig; @@ -54,6 +57,16 @@ static struct k_poll_event logger_evt = namespace { +// Keep a ring claim, its file write and its finish in one consumer transaction. +// Producers use ring_mutex only, so card latency does not block acquisition. +class FileLock { +public: + FileLock() { k_mutex_lock(&file_mutex, K_FOREVER); } + ~FileLock() { k_mutex_unlock(&file_mutex); } + FileLock(const FileLock &) = delete; + FileLock &operator=(const FileLock &) = delete; +}; + constexpr uint8_t OE_HEADER_SIDE_LEFT = 0x00; constexpr uint8_t OE_HEADER_SIDE_RIGHT = 0x01; constexpr uint8_t OE_HEADER_SIDE_UNKNOWN = 0xFF; @@ -125,9 +138,16 @@ void SDLogger::sensor_sd_task() { int ret; while (1) { - ret = k_poll(&logger_evt, 1, K_FOREVER); + k_timeout_t timeout; + { + FileLock lock; + const int64_t remaining = LOG_SYNC_INTERVAL_MS - + (k_uptime_get() - last_sync_uptime_ms); + timeout = sdlogger.is_open ? K_MSEC(MAX(remaining, 1)) : K_FOREVER; + } + ret = k_poll(&logger_evt, 1, timeout); - if (ret < 0) { + if (ret < 0 && ret != -EAGAIN) { LOG_ERR("k_poll failed: %d", ret); continue; } @@ -136,13 +156,13 @@ void SDLogger::sensor_sd_task() { int result; k_poll_signal_check(&logger_sig, &signaled, &result); - if (signaled == 0) { + if (signaled == 0 && ret != -EAGAIN) { LOG_DBG("Poll woke up without signal"); continue; } - // If a close/flush is in progress, do not write concurrently. - if (atomic_get(&g_stop_writing)) { + FileLock lock; + if (atomic_get(&g_stop_writing) || !sdlogger.is_open) { reset_logger_signal(); continue; } @@ -163,6 +183,22 @@ void SDLogger::sensor_sd_task() { return; } + if (k_uptime_get() - last_sync_uptime_ms >= LOG_SYNC_INTERVAL_MS) { + // Drain a bounded snapshot ending at a complete record. Producers + // can keep filling the ring while this thread writes and syncs. + ret = sdlogger.drain_buffer(); + if (ret >= 0) { + ret = sdlogger.sd_card->sync(); + } + last_sync_uptime_ms = k_uptime_get(); + if (ret < 0) { + state_indicator.set_sd_state(SD_FAULT); + LOG_ERR("Failed to checkpoint SD recording: %d", ret); + } + reset_logger_signal(); + continue; + } + uint32_t fill = ring_buf_size_get(&ring_buffer); if (fill >= SD_BLOCK_SIZE) { @@ -172,7 +208,7 @@ void SDLogger::sensor_sd_task() { uint8_t *data = nullptr; - // Claim up to one SD block from the ring buffer under lock. + // Claim complete SD blocks from the ring buffer under lock. k_mutex_lock(&ring_mutex, K_FOREVER); uint32_t claimed = ring_buf_get_claim(&ring_buffer, &data, fill - (fill % SD_BLOCK_SIZE)); k_mutex_unlock(&ring_mutex); @@ -185,30 +221,47 @@ void SDLogger::sensor_sd_task() { // Write the claimed bytes under file lock. size_t write_size = claimed; + const off_t position = sdlogger.sd_card->tell(); + if (position >= 0 && write_size >= 512) { + // The variable-size file header and checkpoints need not end on + // a sector boundary. Align the end of regular writes so FatFS + // can use full-sector transfers on the next iteration. + write_size -= (position + write_size) % 512; + } int written; k_mutex_lock(&file_mutex, K_FOREVER); written = sdlogger.sd_card->write((char*)data, &write_size, false); k_mutex_unlock(&file_mutex); - if (written < 0) { + if (written <= 0) { state_indicator.set_sd_state(SD_FAULT); LOG_ERR("SD write failed: %d", written); - // Do not advance the ring buffer on error. - // Wakeups will continue; user can call end(). + // Release the claim without consuming bytes, allowing a retry. + k_mutex_lock(&ring_mutex, K_FOREVER); + ring_buf_get_finish(&ring_buffer, 0); + k_mutex_unlock(&ring_mutex); reset_logger_signal(); continue; } // Advance ring buffer by the number of bytes actually written. k_mutex_lock(&ring_mutex, K_FOREVER); - ring_buf_get_finish(&ring_buffer, (uint32_t)written); + ret = ring_buf_get_finish(&ring_buffer, (uint32_t)written); k_mutex_unlock(&ring_mutex); + if (ret < 0) { + atomic_set(&g_stop_writing, 1); + state_indicator.set_sd_state(SD_FAULT); + LOG_ERR("Failed to finish SD buffer read: %d", ret); + } } else { k_yield(); } reset_logger_signal(); + if (ring_buf_size_get(&ring_buffer) >= SD_BLOCK_SIZE) { + k_poll_signal_raise(&logger_sig, 0); + } STACK_USAGE_PRINT("sensor_msg_thread", &sdlogger.thread_data); } @@ -262,7 +315,8 @@ int SDLogger::init() { * Opens a file for logging with .oe extension appended to the filename. * Returns -EBUSY if logger is already open or -ENODEV if SD card not initialized. */ -int SDLogger::begin(const std::string& filename) { +int SDLogger::begin(const std::string& filename, const sensor_config* configs, size_t config_count) { + FileLock lock; int ret; if (is_open) { @@ -302,10 +356,13 @@ int SDLogger::begin(const std::string& filename) { ring_buf_reset(&ring_buffer); k_mutex_unlock(&ring_mutex); - ret = write_header(); + ret = write_header(configs, config_count); + if (ret >= 0) { + ret = sd_card->sync(); + } if (ret < 0) { state_indicator.set_sd_state(SD_FAULT); - LOG_ERR("Failed to write header: %d", ret); + LOG_ERR("Failed to persist header: %d", ret); k_mutex_lock(&file_mutex, K_FOREVER); sd_card->close_file(); k_mutex_unlock(&file_mutex); @@ -314,12 +371,13 @@ int SDLogger::begin(const std::string& filename) { return ret; } + last_sync_uptime_ms = k_uptime_get(); k_poll_signal_raise(&logger_sig, 0); return 0; } -int SDLogger::write_header() { +int SDLogger::write_header(const sensor_config* configs, size_t config_count) { const size_t parse_info_size = getParseInfoStorageSize(); if (parse_info_size == 0) { LOG_ERR("Parse info scheme is unavailable"); @@ -349,7 +407,7 @@ int SDLogger::write_header() { ssize_t serialized_size = serializeParseInfoStorage( reinterpret_cast(header_buffer + sizeof(FileHeader)), - parse_info_size + parse_info_size, configs, config_count ); if (serialized_size < 0) { k_free(header_buffer); @@ -403,6 +461,11 @@ int SDLogger::write_sensor_data(const void* const* data_blocks, const size_t* le if (k_mutex_lock(&ring_mutex, K_NO_WAIT) != 0) { return -EAGAIN; } + // Stop may have begun after the check above but before taking ring_mutex. + if (atomic_get(&g_stop_writing) || atomic_get(&g_sd_removed)) { + k_mutex_unlock(&ring_mutex); + return -ENODEV; + } // Ensure there is enough space; if not, free up room by discarding oldest bytes // in SD_BLOCK_SIZE chunks to keep SD writer alignment and minimize partial writes. @@ -449,24 +512,26 @@ int SDLogger::flush() { // Prevent SD thread from writing concurrently atomic_set(&g_stop_writing, 1); k_poll_signal_raise(&logger_sig, 0); + FileLock lock; + + return drain_buffer(); +} + +int SDLogger::drain_buffer() { + FileLock lock; + k_mutex_lock(&ring_mutex, K_FOREVER); + uint32_t remaining = ring_buf_size_get(&ring_buffer); + k_mutex_unlock(&ring_mutex); uint32_t total_written = 0; - for (;;) { + while (remaining > 0) { uint8_t *data = nullptr; - uint32_t fill; - k_mutex_lock(&ring_mutex, K_FOREVER); - fill = ring_buf_size_get(&ring_buffer); - if (fill == 0) { - k_mutex_unlock(&ring_mutex); - break; - } - - uint32_t claimed = ring_buf_get_claim(&ring_buffer, &data, fill); + uint32_t claimed = ring_buf_get_claim(&ring_buffer, &data, remaining); k_mutex_unlock(&ring_mutex); if (claimed == 0 || data == nullptr) { - break; + return -EIO; } size_t req = claimed; @@ -475,17 +540,26 @@ int SDLogger::flush() { written = sd_card->write((char*)data, &req, false); k_mutex_unlock(&file_mutex); - if (written < 0) { + if (written <= 0) { state_indicator.set_sd_state(SD_FAULT); LOG_ERR("Failed to flush SD buffer: %d", written); - break; + k_mutex_lock(&ring_mutex, K_FOREVER); + ring_buf_get_finish(&ring_buffer, 0); + k_mutex_unlock(&ring_mutex); + return written < 0 ? written : -EIO; } k_mutex_lock(&ring_mutex, K_FOREVER); - ring_buf_get_finish(&ring_buffer, (uint32_t)written); + int ret = ring_buf_get_finish(&ring_buffer, (uint32_t)written); k_mutex_unlock(&ring_mutex); + if (ret < 0) { + state_indicator.set_sd_state(SD_FAULT); + LOG_ERR("Failed to finish SD buffer flush: %d", ret); + return ret; + } total_written += (uint32_t)written; + remaining -= (uint32_t)written; if ((uint32_t)written < claimed) { k_yield(); @@ -496,6 +570,7 @@ int SDLogger::flush() { } int SDLogger::end() { + FileLock lock; int ret; if (!is_open) { diff --git a/src/SD_Card/SDLogger/SDLogger.h b/src/SD_Card/SDLogger/SDLogger.h index 0b037e87..17a6dbf0 100644 --- a/src/SD_Card/SDLogger/SDLogger.h +++ b/src/SD_Card/SDLogger/SDLogger.h @@ -10,13 +10,15 @@ constexpr size_t SD_BLOCK_SIZE = 4096; -constexpr size_t BUFFER_BLOCK_COUNT = 8; // Number of blocks in the buffer +constexpr size_t BUFFER_BLOCK_COUNT = 9; // Absorb brief card stalls at full sensor rates. constexpr size_t BUFFER_SIZE = SD_BLOCK_SIZE * BUFFER_BLOCK_COUNT; // BUFFER_SIZE must always be a multiple of SD_BLOCK_SIZE to ensure proper block alignment // without requiring padding. The SensorLogger implementation assumes this relationship // and will not work correctly otherwise. static_assert(BUFFER_SIZE % SD_BLOCK_SIZE == 0, "BUFFER_SIZE must be a multiple of SD_BLOCK_SIZE"); +static_assert(BUFFER_SIZE <= RING_BUFFER_MAX_SIZE, + "SD logger requires CONFIG_RING_BUFFER_LARGE"); // Forward declare the work handler //static void sd_work_handler(struct k_work* work); @@ -32,8 +34,9 @@ class SDLogger { //size_t buffer_pos = 0; std::string current_file; - int write_header(); // Write the file header, device metadata, and embedded parse metadata. + int write_header(const sensor_config* configs, size_t config_count); int flush(); // Flush any buffered data to the SD card + int drain_buffer(); // Drain a complete-record snapshot without stopping producers static constexpr uint16_t SENSOR_LOG_VERSION = 0x0003; @@ -69,7 +72,7 @@ class SDLogger { * @param filename Base filename without extension (.oe will be appended) * @return 0 on success, negative error code on failure */ - int begin(const std::string& filename); + int begin(const std::string& filename, const sensor_config* configs, size_t config_count); /** * @brief Write sensor data to the log file diff --git a/src/SD_Card/SD_Card_Manager/SD_Card_Manager.cpp b/src/SD_Card/SD_Card_Manager/SD_Card_Manager.cpp index eb2a8ac8..a3c49d15 100644 --- a/src/SD_Card/SD_Card_Manager/SD_Card_Manager.cpp +++ b/src/SD_Card/SD_Card_Manager/SD_Card_Manager.cpp @@ -494,6 +494,13 @@ int SDCardManager::close_file() { return 0; } +off_t SDCardManager::tell() { + k_mutex_lock(&m_sem_sd_mngr_oper_ongoing, K_FOREVER); + const off_t position = tracked_file.is_open ? fs_tell(&tracked_file.filep) : -EBADF; + k_mutex_unlock(&m_sem_sd_mngr_oper_ongoing); + return position; +} + ssize_t SDCardManager::write(char *buf, size_t *buf_size, bool sync) { if (!this->tracked_file.is_open) { LOG_ERR("File is not open"); diff --git a/src/SD_Card/SD_Card_Manager/SD_Card_Manager.h b/src/SD_Card/SD_Card_Manager/SD_Card_Manager.h index 804cc504..f0144380 100644 --- a/src/SD_Card/SD_Card_Manager/SD_Card_Manager.h +++ b/src/SD_Card/SD_Card_Manager/SD_Card_Manager.h @@ -107,6 +107,7 @@ class SDCardManager { * @return 0 on success, < 0 on error */ int sync(); + off_t tell(); /** * @brief Remove a file or directory. * diff --git a/src/SensorManager/BMA580/BMA580_Sensor.cpp b/src/SensorManager/BMA580/BMA580_Sensor.cpp index a3e4cc76..63533b53 100644 --- a/src/SensorManager/BMA580/BMA580_Sensor.cpp +++ b/src/SensorManager/BMA580/BMA580_Sensor.cpp @@ -69,7 +69,7 @@ BMA5_INTF_RET_TYPE bma5_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t l if (ret) LOG_WRN("I2C read failed: %d\n", ret); device_info->i2c_dev->release(); - return 0; + return ret == 0 ? BMA5_INTF_RET_SUCCESS : -1; } /*! @@ -86,7 +86,7 @@ BMA5_INTF_RET_TYPE bma5_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uin device_info->i2c_dev->release(); - return 0; + return ret == 0 ? BMA5_INTF_RET_SUCCESS : -1; } /*! @@ -237,7 +237,10 @@ int8_t BMA580::get_accel_and_int_settings(struct bma5_dev *device) } /*! - * @brief This internal API gets FIFO configurations. + * @brief This internal API sets FIFO configurations and verifies the readback. + * + * The BMA580 silently limits the FIFO to 512 bytes while the feature engine + * is enabled, so verify that the requested FIFO size was accepted. */ int8_t BMA580::get_fifo_conf(const struct bma5_fifo_conf *fifo_config, struct bma5_dev *device) { @@ -253,13 +256,17 @@ int8_t BMA580::get_fifo_conf(const struct bma5_fifo_conf *fifo_config, struct bm rslt = bma5_get_fifo_conf(&read_fifo_conf, device); bma5_check_rslt("bma5_get_fifo_conf", rslt); + if (read_fifo_conf.fifo_size != fifo_config->fifo_size) { + LOG_ERR("FIFO size mismatch: requested 0x%02X, readback 0x%02X " + "(feature engine likely still enabled)", + fifo_config->fifo_size, read_fifo_conf.fifo_size); + } + return rslt; } int BMA580::init(int odr, int watermark_level) { int8_t rslt; - struct bma580_int_map int_map, get_int_map; - //struct bma5_fifo_conf fifo_conf; /* Assign context parameter selection */ enum bma5_context context = BMA5_HEARABLE; @@ -277,19 +284,11 @@ int BMA580::init(int odr, int watermark_level) { bma5_check_rslt("bma580_init", rslt); LOG_DBG("Chip ID :0x%X", dev.chip_id); - /* Map generic interrupts to hardware interrupt pin of the sensor */ - rslt = bma580_get_int_map(&int_map, &dev); - bma5_check_rslt("bma580_get_int_map", rslt); - - /* Set FIFO full interrupt to INT2 */ - //int_map.fifo_full_int_map = BMA580_FIFO_FULL_INT_MAP_INT2; - int_map.fifo_wm_int_map = BMA580_FIFO_WM_INT_MAP_INT1; - - rslt = bma580_set_int_map(&int_map, &dev); - bma5_check_rslt("bma580_set_int_map", rslt); - - rslt = bma580_get_int_map(&get_int_map, &dev); - bma5_check_rslt("bma580_get_int_map", rslt); + /* The feature engine and FIFO share 1024 bytes of RAM. None of the + * feature-engine functions are used here, so disable it to make the full + * RAM available to the FIFO. INT1/INT2 are unused; the FIFO is polled. */ + rslt = bma5_set_feat_eng_conf(BMA5_FEAT_ENG_CTRL_DISABLE, &dev); + bma5_check_rslt("bma5_set_feat_eng_conf", rslt); rslt = get_accel_and_int_settings(&dev); bma5_check_rslt("get_accel_and_int_settings", rslt); @@ -298,17 +297,16 @@ int BMA580::init(int odr, int watermark_level) { rslt = bma5_get_fifo_conf(&fifo_conf, &dev); bma5_check_rslt("bma5_get_fifo_conf", rslt); - rslt = bma5_get_fifo_conf(&fifo_conf, &dev); - bma5_check_rslt("bma5_get_fifo_conf", rslt); - fifo_conf.fifo_cfg = BMA5_FIFO_CFG_ENABLE; fifo_conf.fifo_acc_x = BMA5_FIFO_ACC_X_ENABLE; fifo_conf.fifo_acc_y = BMA5_FIFO_ACC_Y_ENABLE; fifo_conf.fifo_acc_z = BMA5_FIFO_ACC_Z_ENABLE; fifo_conf.fifo_compression = BMA5_FIFO_COMPRESSION_ACC_16BIT; fifo_conf.fifo_sensor_time = BMA5_FIFO_SENSOR_TIME_OFF; - fifo_conf.fifo_size = BMA5_FIFO_SIZE_MAX_512_BYTES; - fifo_conf.fifo_stop_on_full = BMA5_ENABLE; //BMA5_ENABLE + fifo_conf.fifo_size = BMA5_FIFO_SIZE_MAX_1024_BYTES; + /* Keep sampling if servicing is delayed instead of leaving the FIFO + * permanently stopped with stale data. Oldest samples may be overwritten. */ + fifo_conf.fifo_stop_on_full = BMA5_DISABLE; rslt = get_fifo_conf(&fifo_conf, &dev); bma5_check_rslt("get_fifo_conf", rslt); @@ -336,32 +334,38 @@ int BMA580::stop() { int BMA580::read(bma5_sens_fifo_axes_data_16_bit *fifo_accel_data) { int8_t rslt = BMA5_OK; - uint8_t n_status = 1; - struct bma580_int_status_types int_status{}; fifoframe.fifo_avail_frames = 0; - int_status.int_src = BMA580_INT_STATUS_INT1; - - /* Get fifo full interrupt 2 status */ - rslt = bma580_get_int_status(&int_status, n_status, &dev); - bma5_check_rslt("bma580_get_int_status", rslt); - - if (int_status.int_status.fifo_wm_int_status & BMA5_ENABLE) + // The FIFO fill level counts six-byte XYZ payloads, excluding the header + // generated for each frame on the bus. Read complete seven-byte frames. + uint8_t level[2] = {}; + rslt = bma5_get_regs(BMA5_REG_FIFO_LEVEL_0, level, sizeof(level), &dev); + if (rslt != BMA5_OK) return rslt; + const unsigned frames = MIN(((level[0] | ((level[1] & 7) << 8)) / 6), + sizeof(fifo_data) / 7); + if (frames == 0) return 0; + fifoframe.acc_byte_start_idx = 0; + fifoframe.fifo_avail_len = frames * 7; + // The generic register helper has a 128-byte temporary buffer. FIFO reads + // must go directly into our full-size member buffer, as in the Bosch FIFO API. + dev.intf_rslt = dev.bus_read(BMA5_REG_FIFO_DATA_OUT, fifo_data, + fifoframe.fifo_avail_len, dev.intf_ptr); + rslt = dev.intf_rslt == BMA5_INTF_RET_SUCCESS ? BMA5_OK : BMA5_E_COM_FAIL; + bma5_check_rslt("FIFO read", rslt); + + if (rslt == BMA5_OK && fifoframe.fifo_avail_len > 0) { - /* Read FIFO data */ - rslt = bma5_read_fifo_data(&fifoframe, &fifo_conf, &dev); - bma5_check_rslt("bma5_read_fifo_data", rslt); - - /* Set fifo full interrupt 2 status */ - rslt = bma580_set_int_status(&int_status, n_status, &dev); - bma5_check_rslt("bma580_get_int_status\n", rslt); - - if (rslt == BMA5_OK) - { - /* Parse the FIFO data to extract accelerometer and sensortime data from the FIFO buffer */ - (void)bma5_extract_acc_sens_time_16_bit(fifo_accel_data, &fifoframe, &fifo_conf, &dev); + // The configured format is XYZ without sensor-time bytes. Reject a + // different frame layout before the vendor parser can overrun output. + for (unsigned i = 0; i < fifoframe.fifo_avail_len; i += 7) { + if ((fifo_data[i] & 0x0F) != + (BMA5_FIFO_ACC_X_MSK | BMA5_FIFO_ACC_Y_MSK | BMA5_FIFO_ACC_Z_MSK)) { + return BMA5_E_COM_FAIL; + } } + /* Parse accelerometer data from the FIFO buffer. */ + (void)bma5_extract_acc_sens_time_16_bit(fifo_accel_data, &fifoframe, &fifo_conf, &dev); } return fifoframe.fifo_avail_frames; diff --git a/src/SensorManager/BMA580/BMA580_Sensor.h b/src/SensorManager/BMA580/BMA580_Sensor.h index 1ff93349..b5d90d4f 100644 --- a/src/SensorManager/BMA580/BMA580_Sensor.h +++ b/src/SensorManager/BMA580/BMA580_Sensor.h @@ -43,8 +43,8 @@ /******************************************************************************/ /*! Macro definition */ -/*! FIFO raw data buffer size */ -#define BMA580_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(520) +/*! 1024 stored payload bytes hold 170 XYZ frames, each with a wire header. */ +#define BMA580_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(1190) /*! Number of accel frames to be extracted from FIFO * Calculation: diff --git a/src/SensorManager/BMP388/Adafruit_BMP3XX.cpp b/src/SensorManager/BMP388/Adafruit_BMP3XX.cpp index 9a11960f..cb8cb924 100644 --- a/src/SensorManager/BMP388/Adafruit_BMP3XX.cpp +++ b/src/SensorManager/BMP388/Adafruit_BMP3XX.cpp @@ -300,6 +300,92 @@ bool Adafruit_BMP3XX::performReading(void) { return true; } +bool Adafruit_BMP3XX::startContinuous(uint8_t odr) { + if (!setOutputDataRate(odr)) + return false; + + the_sensor.settings.temp_en = BMP3_ENABLE; + the_sensor.settings.press_en = BMP3_ENABLE; + const uint16_t settings = BMP3_SEL_TEMP_EN | BMP3_SEL_PRESS_EN | + BMP3_SEL_TEMP_OS | BMP3_SEL_PRESS_OS | BMP3_SEL_IIR_FILTER | BMP3_SEL_ODR; + if (bmp3_set_sensor_settings(settings, &the_sensor) != BMP3_OK) + return false; + + fifo = {}; + fifo.data.buffer = fifo_buffer; + fifo.settings.mode = BMP3_ENABLE; + fifo.settings.press_en = BMP3_ENABLE; + fifo.settings.temp_en = BMP3_ENABLE; + the_sensor.fifo = &fifo; + const uint16_t fifo_settings = BMP3_SEL_FIFO_MODE | BMP3_SEL_FIFO_STOP_ON_FULL_EN | + BMP3_SEL_FIFO_TIME_EN | BMP3_SEL_FIFO_PRESS_EN | BMP3_SEL_FIFO_TEMP_EN | + BMP3_SEL_FIFO_DOWN_SAMPLING | BMP3_SEL_FIFO_FILTER_EN; + if (bmp3_set_fifo_settings(fifo_settings, &the_sensor) != BMP3_OK || + bmp3_fifo_flush(&the_sensor) != BMP3_OK) + return false; + + the_sensor.settings.op_mode = BMP3_MODE_NORMAL; + return bmp3_set_op_mode(&the_sensor) == BMP3_OK; +} + +bool Adafruit_BMP3XX::readContinuous(void) { + uint8_t status; + if (bmp3_get_regs(BMP3_REG_SENS_STATUS, &status, 1, &the_sensor) != BMP3_OK) + return false; + + const uint8_t ready = BMP3_STATUS_DRDY_PRESS_MSK | BMP3_STATUS_DRDY_TEMP_MSK; + if ((status & ready) != ready) + return false; + + struct bmp3_data data; + if (bmp3_get_sensor_data(BMP3_PRESS | BMP3_TEMP, &data, &the_sensor) != BMP3_OK) + return false; + + temperature = data.temperature; + pressure = data.pressure; + return true; +} + +// Read the complete FIFO snapshot into member storage. The Bosch parser assumes +// complete frames; validate their lengths before handing it the received bytes. +int Adafruit_BMP3XX::readFifo(struct bmp3_data *samples, uint8_t capacity) { + uint16_t length = 0; + if (samples == nullptr || capacity == 0 || + bmp3_get_fifo_length(&length, &the_sensor) != BMP3_OK) + return -1; + if (length == 0) return 0; + if (length > sizeof(fifo_buffer)) return -1; + if (the_sensor.read(BMP3_REG_FIFO_DATA, fifo_buffer, length, + the_sensor.intf_ptr) != BMP3_INTF_RET_SUCCESS) + return -1; + + unsigned frames = 0; + for (unsigned i = 0; i < length;) { + const uint8_t header = fifo_buffer[i]; + const unsigned size = header == BMP3_FIFO_TEMP_PRESS_FRAME ? 7 : + header == BMP3_FIFO_CONFIG_CHANGE ? 2 : 0; + if (size == 0 || size > length - i) return -1; + if (header == BMP3_FIFO_TEMP_PRESS_FRAME) ++frames; + i += size; + } + if (frames > capacity) return -1; + fifo.data.byte_count = length; + fifo.data.start_idx = 0; + fifo.data.parsed_frames = 0; + fifo.data.config_change = 0; + fifo.data.config_err = 0; + fifo.data.req_frames = capacity; + if (bmp3_extract_fifo_data(samples, &the_sensor) != BMP3_OK || + fifo.data.config_err || fifo.data.parsed_frames != frames) + return -1; + return frames; +} + +bool Adafruit_BMP3XX::stopContinuous(void) { + the_sensor.settings.op_mode = BMP3_MODE_SLEEP; + return bmp3_set_op_mode(&the_sensor) == BMP3_OK; +} + /**************************************************************************/ /*! @brief Setter for Temperature oversampling @@ -431,7 +517,7 @@ int8_t i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, dev_info->i2c_dev->release(); - return 0; + return ret; } static void delay_usec(uint32_t us, void *intf_ptr) { diff --git a/src/SensorManager/BMP388/Adafruit_BMP3XX.h b/src/SensorManager/BMP388/Adafruit_BMP3XX.h index a9843b00..cef97c8c 100644 --- a/src/SensorManager/BMP388/Adafruit_BMP3XX.h +++ b/src/SensorManager/BMP388/Adafruit_BMP3XX.h @@ -68,6 +68,11 @@ class Adafruit_BMP3XX { /// Perform a reading in blocking mode bool performReading(void); + bool startContinuous(uint8_t odr); + bool readContinuous(void); + int readFifo(struct bmp3_data *samples, uint8_t capacity); + bool stopContinuous(void); + /// Temperature (Celsius) assigned after calling performReading() double temperature; /// Pressure (Pascals) assigned after calling performReading() @@ -87,6 +92,8 @@ class Adafruit_BMP3XX { //uint8_t spixfer(uint8_t x); struct bmp3_dev the_sensor; + struct bmp3_fifo fifo{}; + uint8_t fifo_buffer[512]{}; bool detect(int address); diff --git a/src/SensorManager/BMX160/BMX160.cpp b/src/SensorManager/BMX160/BMX160.cpp new file mode 100644 index 00000000..64bef4cd --- /dev/null +++ b/src/SensorManager/BMX160/BMX160.cpp @@ -0,0 +1,287 @@ +#include "BMX160.h" + +#include +#include +#include + +LOG_MODULE_REGISTER(BMX160, CONFIG_MAIN_LOG_LEVEL); + +namespace { +constexpr float EARTH_GRAVITY = 9.80665f; +constexpr float ACCEL_SCALE_2G = (2.0f * EARTH_GRAVITY) / 32768.0f; +constexpr float GYRO_SCALE_2000_DPS = 2000.0f / 32768.0f; +constexpr uint8_t FIFO_FRAME_BYTES = 21; // header + 8-byte mag + 6-byte gyro + 6-byte accel +constexpr uint8_t BMX160_CHIP_ID = 0xD8; +} + +BMX160 *BMX160::instance = nullptr; + +BMX160::BMX160(TWIM *i2c) + : _i2c(i2c), _addr(DT_REG_ADDR(DT_NODELABEL(bmx160))) +{ + instance = this; +} + +int8_t BMX160::bmiRead(uint8_t, uint8_t reg_addr, uint8_t *data, uint16_t len) +{ + return instance == nullptr ? BMI160_E_NULL_PTR : instance->busRead(reg_addr, data, len); +} + +int8_t BMX160::bmiWrite(uint8_t, uint8_t reg_addr, uint8_t *data, uint16_t len) +{ + return instance == nullptr ? BMI160_E_NULL_PTR : instance->busWrite(reg_addr, data, len); +} + +void BMX160::bmiDelay(uint32_t period_ms) +{ + k_msleep(period_ms); +} + +int8_t BMX160::bmmRead(uint8_t reg_addr, uint8_t *data, uint32_t len, void *intf_ptr) +{ + auto *self = static_cast(intf_ptr); + if (self == nullptr || len > UINT16_MAX) { + return BMM150_E_NULL_PTR; + } + return bmi160_aux_read(reg_addr, data, static_cast(len), &self->_bmi); +} + +int8_t BMX160::bmmWrite(uint8_t reg_addr, const uint8_t *data, uint32_t len, void *intf_ptr) +{ + auto *self = static_cast(intf_ptr); + if (self == nullptr || len > UINT16_MAX) { + return BMM150_E_NULL_PTR; + } + return bmi160_aux_write(reg_addr, const_cast(data), static_cast(len), &self->_bmi); +} + +void BMX160::bmmDelay(uint32_t period_us, void *) +{ + k_usleep(period_us); +} + +int8_t BMX160::busRead(uint8_t reg_addr, uint8_t *data, uint16_t len) +{ + _i2c->aquire(); + const int ret = i2c_burst_read(_i2c->master, _addr, reg_addr, data, len); + _i2c->release(); + + return ret == 0 ? BMI160_OK : BMI160_E_COM_FAIL; +} + +int8_t BMX160::busWrite(uint8_t reg_addr, const uint8_t *data, uint16_t len) +{ + _i2c->aquire(); + const int ret = i2c_burst_write(_i2c->master, _addr, reg_addr, data, len); + _i2c->release(); + + return ret == 0 ? BMI160_OK : BMI160_E_COM_FAIL; +} + +bool BMX160::init() +{ + _i2c->begin(); + + _bmi = {}; + _bmi.id = _addr; + _bmi.intf = BMI160_I2C_INTF; + _bmi.read = bmiRead; + _bmi.write = bmiWrite; + _bmi.delay_ms = bmiDelay; + _bmi.read_write_len = FIFO_CAPACITY_BYTES; + + int8_t result = bmi160_init(&_bmi); + // The BMX160's integrated BMI160-compatible die reports 0xD8 instead of + // the standalone BMI160's 0xD1 expected by Bosch's unmodified API. + if (result == BMI160_E_DEV_NOT_FOUND && _bmi.chip_id == BMX160_CHIP_ID) { + LOG_INF("BMX160 chip ID 0x%02x accepted for Bosch BMI160 API", _bmi.chip_id); + result = bmi160_soft_reset(&_bmi); + } + if (result != BMI160_OK) { + LOG_ERR("Bosch BMI160 init failed: %d (chip ID 0x%02x)", result, _bmi.chip_id); + return false; + } + + _bmi.accel_cfg.odr = BMI160_ACCEL_ODR_100HZ; + _bmi.accel_cfg.range = BMI160_ACCEL_RANGE_2G; + _bmi.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4; + _bmi.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; + _bmi.gyro_cfg.odr = BMI160_GYRO_ODR_100HZ; + _bmi.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS; + _bmi.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE; + _bmi.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; + + result = bmi160_set_sens_conf(&_bmi); + if (result != BMI160_OK) { + LOG_ERR("BMI160 sensor configuration failed: %d", result); + return false; + } + + if (configureMagnetometer() != 0) { + return false; + } + + _fifo.data = _fifo_data; + _bmi.fifo = &_fifo; + + result = bmi160_set_fifo_config(BMI160_FIFO_HEADER | BMI160_FIFO_AUX | + BMI160_FIFO_GYRO | BMI160_FIFO_ACCEL, + BMI160_ENABLE, &_bmi); + if (result != BMI160_OK) { + LOG_ERR("BMI160 FIFO configuration failed: %d", result); + return false; + } + + result = bmi160_set_fifo_flush(&_bmi); + if (result != BMI160_OK) { + LOG_ERR("BMI160 FIFO flush failed: %d", result); + return false; + } + + LOG_INF("Bosch BMI160/BMM150 driver initialized with MGA FIFO"); + return true; +} + +int BMX160::configureMagnetometer() +{ + _bmi.aux_cfg.aux_sensor_enable = BMI160_ENABLE; + _bmi.aux_cfg.aux_i2c_addr = BMM150_DEFAULT_I2C_ADDRESS; + _bmi.aux_cfg.manual_enable = BMI160_ENABLE; + _bmi.aux_cfg.aux_rd_burst_len = BMI160_AUX_READ_LEN_3; + _bmi.aux_cfg.aux_odr = BMI160_AUX_ODR_100HZ; + + int8_t result = bmi160_aux_init(&_bmi); + if (result != BMI160_OK) { + LOG_ERR("BMI160 auxiliary interface init failed: %d", result); + return result; + } + + _bmm = {}; + _bmm.intf = BMM150_I2C_INTF; + _bmm.intf_ptr = this; + _bmm.read = bmmRead; + _bmm.write = bmmWrite; + _bmm.delay_us = bmmDelay; + + result = bmm150_init(&_bmm); + if (result != BMM150_OK) { + LOG_ERR("Bosch BMM150 init through BMI160 AUX failed: %d", result); + return result; + } + + _bmm_settings = {}; + _bmm_settings.preset_mode = BMM150_PRESETMODE_REGULAR; + result = bmm150_set_presetmode(&_bmm_settings, &_bmm); + if (result != BMM150_OK) { + LOG_ERR("BMM150 regular preset failed: %d", result); + return result; + } + + // This must be the final BMM150 write before auto mode. The BMI160 repeats + // it after each AUX read, triggering the next single measurement. + _bmm_settings.pwr_mode = BMM150_POWERMODE_FORCED; + result = bmm150_set_op_mode(&_bmm_settings, &_bmm); + if (result != BMM150_OK) { + LOG_ERR("BMM150 forced mode configuration failed: %d", result); + return result; + } + + uint8_t data_start = BMM150_REG_DATA_X_LSB; + result = bmi160_set_aux_auto_mode(&data_start, &_bmi); + if (result != BMI160_OK) { + LOG_ERR("BMI160 AUX auto mode failed: %d", result); + } + return result; +} + +uint8_t BMX160::auxOdrFor(float sample_rate_hz) +{ + if (sample_rate_hz <= 25.0f) return BMI160_AUX_ODR_25HZ; + if (sample_rate_hz <= 50.0f) return BMI160_AUX_ODR_50HZ; + return BMI160_AUX_ODR_100HZ; +} + +int BMX160::start(uint8_t odr, float sample_rate_hz, uint8_t buffered_samples) +{ + _bmi.accel_cfg.odr = odr; + _bmi.gyro_cfg.odr = odr; + _bmi.aux_cfg.aux_odr = auxOdrFor(sample_rate_hz); + + int8_t result = bmi160_set_sens_conf(&_bmi); + if (result == BMI160_OK) { + uint8_t data_start = BMM150_REG_DATA_X_LSB; + result = bmi160_set_aux_auto_mode(&data_start, &_bmi); + } + if (result == BMI160_OK) { + const uint16_t watermark_bytes = std::min( + FIFO_CAPACITY_BYTES - FIFO_FRAME_BYTES, + static_cast(buffered_samples) * FIFO_FRAME_BYTES); + result = bmi160_set_fifo_wm(static_cast((watermark_bytes + 3U) / 4U), &_bmi); + } + if (result == BMI160_OK) { + result = bmi160_set_fifo_flush(&_bmi); + } + return result; +} + +int BMX160::stop() +{ + int8_t result = bmi160_set_fifo_flush(&_bmi); + _bmi.accel_cfg.power = BMI160_ACCEL_SUSPEND_MODE; + _bmi.gyro_cfg.power = BMI160_GYRO_SUSPEND_MODE; + const int8_t power_result = bmi160_set_power_mode(&_bmi); + return result == BMI160_OK ? power_result : result; +} + +int BMX160::read(BMX160Sample *samples, uint8_t max_samples) +{ + if (samples == nullptr || max_samples == 0) { + return BMI160_E_NULL_PTR; + } + + _fifo.length = sizeof(_fifo_data); + int8_t result = bmi160_get_fifo_data(&_bmi); + if (result != BMI160_OK) { + return result; + } + + uint8_t accel_count = std::min(max_samples, MAX_FIFO_SAMPLES); + uint8_t gyro_count = accel_count; + uint8_t aux_count = accel_count; + + result = bmi160_extract_accel(_accel, &accel_count, &_bmi); + if (result == BMI160_OK) result = bmi160_extract_gyro(_gyro, &gyro_count, &_bmi); + if (result == BMI160_OK) result = bmi160_extract_aux(_aux, &aux_count, &_bmi); + if (result != BMI160_OK) { + return result; + } + + const uint8_t sample_count = std::min(accel_count, gyro_count); + if (sample_count == 0 || aux_count == 0) { + return 0; + } + + for (uint8_t i = 0; i < sample_count; ++i) { + samples[i].accel[0] = _accel[i].x * ACCEL_SCALE_2G; + samples[i].accel[1] = _accel[i].y * ACCEL_SCALE_2G; + samples[i].accel[2] = _accel[i].z * ACCEL_SCALE_2G; + samples[i].gyro[0] = _gyro[i].x * GYRO_SCALE_2000_DPS; + samples[i].gyro[1] = _gyro[i].y * GYRO_SCALE_2000_DPS; + samples[i].gyro[2] = _gyro[i].z * GYRO_SCALE_2000_DPS; + + const uint8_t mag_index = sample_count <= aux_count + ? i + : std::min(aux_count - 1, + static_cast(i) * aux_count / sample_count); + struct bmm150_mag_data mag = {}; + result = bmm150_aux_mag_data(_aux[mag_index].data, &mag, &_bmm); + if (result != BMM150_OK) { + return result; + } + samples[i].mag[0] = mag.x; + samples[i].mag[1] = mag.y; + samples[i].mag[2] = mag.z; + } + + return sample_count; +} diff --git a/src/SensorManager/BMX160/BMX160.h b/src/SensorManager/BMX160/BMX160.h new file mode 100644 index 00000000..741b3591 --- /dev/null +++ b/src/SensorManager/BMX160/BMX160.h @@ -0,0 +1,61 @@ +#ifndef BMX160_H +#define BMX160_H + +#include + +#include + +extern "C" { +#include "bosch/bmi160.h" +#include "bosch/bmm150.h" +} + +struct BMX160Sample { + float accel[3]; + float gyro[3]; + float mag[3]; +}; + +class BMX160 { +public: + static constexpr uint16_t FIFO_CAPACITY_BYTES = 1024; + static constexpr uint8_t MAX_FIFO_SAMPLES = 48; + + explicit BMX160(TWIM *i2c); + + bool init(); + int start(uint8_t odr, float sample_rate_hz, uint8_t buffered_samples); + int stop(); + int read(BMX160Sample *samples, uint8_t max_samples); + +private: + static BMX160 *instance; + + static int8_t bmiRead(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len); + static int8_t bmiWrite(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len); + static void bmiDelay(uint32_t period_ms); + + static int8_t bmmRead(uint8_t reg_addr, uint8_t *data, uint32_t len, void *intf_ptr); + static int8_t bmmWrite(uint8_t reg_addr, const uint8_t *data, uint32_t len, void *intf_ptr); + static void bmmDelay(uint32_t period_us, void *intf_ptr); + + int8_t busRead(uint8_t reg_addr, uint8_t *data, uint16_t len); + int8_t busWrite(uint8_t reg_addr, const uint8_t *data, uint16_t len); + int configureMagnetometer(); + static uint8_t auxOdrFor(float sample_rate_hz); + + TWIM *_i2c; + uint8_t _addr; + + struct bmi160_dev _bmi = {}; + struct bmm150_dev _bmm = {}; + struct bmi160_fifo_frame _fifo = {}; + struct bmm150_settings _bmm_settings = {}; + + uint8_t _fifo_data[FIFO_CAPACITY_BYTES] = {}; + struct bmi160_sensor_data _accel[MAX_FIFO_SAMPLES] = {}; + struct bmi160_sensor_data _gyro[MAX_FIFO_SAMPLES] = {}; + struct bmi160_aux_data _aux[MAX_FIFO_SAMPLES] = {}; +}; + +#endif diff --git a/src/SensorManager/BMX160/CMakeLists.txt b/src/SensorManager/BMX160/CMakeLists.txt index d287a1d4..3af05235 100644 --- a/src/SensorManager/BMX160/CMakeLists.txt +++ b/src/SensorManager/BMX160/CMakeLists.txt @@ -5,5 +5,9 @@ # target_sources(app PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/DFRobot_BMX160.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/BMX160.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/bosch/bmi160.c + ${CMAKE_CURRENT_SOURCE_DIR}/bosch/bmm150.c ) + +target_compile_definitions(app PRIVATE BMM150_USE_FLOATING_POINT) diff --git a/src/SensorManager/BMX160/DFRobot_BMX160.cpp b/src/SensorManager/BMX160/DFRobot_BMX160.cpp deleted file mode 100644 index 659b38a2..00000000 --- a/src/SensorManager/BMX160/DFRobot_BMX160.cpp +++ /dev/null @@ -1,391 +0,0 @@ -/*! - * @file DFRobot_BMX160.cpp - * @brief define DFRobot_BMX160 class infrastructure, the implementation of basic methods - * @copyright Copyright (c) 2010 DFRobot Co.Ltd (http://www.dfrobot.com) - * @license The MIT License (MIT) - * @author [luoyufeng] (yufeng.luo@dfrobot.com) - * @maintainer [Fary](feng.yang@dfrobot.com) - * @version V1.0 - * @date 2021-10-20 - * @url https://github.com/DFRobot/DFRobot_BMX160 - */ -#include "DFRobot_BMX160.h" - -#include -LOG_MODULE_REGISTER(BMX160, CONFIG_MAIN_LOG_LEVEL); - -#define delay(ms) k_msleep(ms) -#define malloc(a) k_malloc(a) - -DFRobot_BMX160::DFRobot_BMX160(TWIM *i2c) : _i2c(i2c) -{ - Obmx160 = (sBmx160Dev_t *)malloc(sizeof(sBmx160Dev_t)); - Oaccel = ( sBmx160SensorData_t*)malloc(sizeof( sBmx160SensorData_t)); - Ogyro = ( sBmx160SensorData_t*)malloc(sizeof( sBmx160SensorData_t)); - Omagn = ( sBmx160SensorData_t*)malloc(sizeof( sBmx160SensorData_t)); -} - -const uint8_t int_mask_lookup_table[13] = { - BMX160_INT1_SLOPE_MASK, - BMX160_INT1_SLOPE_MASK, - BMX160_INT2_LOW_STEP_DETECT_MASK, - BMX160_INT1_DOUBLE_TAP_MASK, - BMX160_INT1_SINGLE_TAP_MASK, - BMX160_INT1_ORIENT_MASK, - BMX160_INT1_FLAT_MASK, - BMX160_INT1_HIGH_G_MASK, - BMX160_INT1_LOW_G_MASK, - BMX160_INT1_NO_MOTION_MASK, - BMX160_INT2_DATA_READY_MASK, - BMX160_INT2_FIFO_FULL_MASK, - BMX160_INT2_FIFO_WM_MASK -}; - -bool DFRobot_BMX160::begin() -{ - _i2c->begin(); - - if (scan() == true){ - softReset(); - writeBmxReg(BMX160_COMMAND_REG_ADDR, 0x11); - delay(50); - /* Set gyro to normal mode */ - writeBmxReg(BMX160_COMMAND_REG_ADDR, 0x15); - delay(100); - /* Set mag to normal mode */ - writeBmxReg(BMX160_COMMAND_REG_ADDR, 0x19); - delay(10); - setMagnConf(); - return true; - } else return false; -} - -void DFRobot_BMX160::setLowPower(){ - softReset(); - delay(100); - setMagnConf(); - delay(100); - writeBmxReg(BMX160_COMMAND_REG_ADDR, 0x12); - delay(100); - /* Set gyro to normal mode */ - writeBmxReg(BMX160_COMMAND_REG_ADDR, 0x17); - delay(100); - /* Set mag to normal mode */ - writeBmxReg(BMX160_COMMAND_REG_ADDR, 0x1B); - delay(100); -} - -void DFRobot_BMX160::wakeUp(){ - softReset(); - delay(100); - setMagnConf(); - delay(100); - writeBmxReg(BMX160_COMMAND_REG_ADDR, 0x11); - delay(100); - /* Set gyro to normal mode */ - writeBmxReg(BMX160_COMMAND_REG_ADDR, 0x15); - delay(100); - /* Set mag to normal mode */ - writeBmxReg(BMX160_COMMAND_REG_ADDR, 0x19); - delay(100); -} - -bool DFRobot_BMX160::softReset() -{ - return _softReset(Obmx160) == BMX160_OK; -} - -int8_t DFRobot_BMX160:: _softReset(sBmx160Dev_t *dev) -{ - int8_t rslt=BMX160_OK; - uint8_t data = BMX160_SOFT_RESET_CMD; - if (dev==NULL){ - rslt = BMX160_E_NULL_PTR; - } - writeBmxReg(BMX160_COMMAND_REG_ADDR, data); - delay(BMX160_SOFT_RESET_DELAY_MS); - if (rslt == BMX160_OK){ - DFRobot_BMX160::defaultParamSettg(dev); - } - return rslt; -} - -void DFRobot_BMX160::defaultParamSettg(sBmx160Dev_t *dev) -{ - // Initializing accel and gyro params with - dev->gyroCfg.bw = BMX160_GYRO_BW_NORMAL_MODE; - dev->gyroCfg.odr = BMX160_GYRO_ODR_100HZ; - dev->gyroCfg.power = BMX160_GYRO_SUSPEND_MODE; - dev->gyroCfg.range = BMX160_GYRO_RANGE_2000_DPS; - - dev->accelCfg.bw = BMX160_ACCEL_BW_NORMAL_AVG4; - dev->accelCfg.odr = BMX160_ACCEL_ODR_100HZ; - dev->accelCfg.power = BMX160_ACCEL_SUSPEND_MODE; - dev->accelCfg.range = BMX160_ACCEL_RANGE_2G; - - dev->magnCfg.odr = BMX160_MAGN_ODR_100HZ; - dev->magnCfg.power = BMX160_MAGN_SUSPEND_MODE; - - dev->prevMagnCfg = dev->magnCfg; - dev->prevGyroCfg = dev->gyroCfg; - dev->prevAccelCfg = dev->accelCfg; -} - -bool DFRobot_BMX160::setMagnConf() -{ - if (setBmi160AuxMagnConf()) { - LOG_INF("BMM150 magnetometer configured through BMI160 auxiliary interface"); - return true; - } - - LOG_WRN("BMM150 auxiliary probe failed; using BMX160 magnetometer interface fallback"); - setBmx160MagnConf(); - - return false; -} - -bool DFRobot_BMX160::setBmi160AuxMagnConf() -{ - uint8_t reg_val = 0; - uint8_t chip_id = 0; - - if (!writeBmxReg(BMX160_COMMAND_REG_ADDR, BMX160_MAGN_NORMAL_MODE)) { - return false; - } - delay(1); - - if (!readReg(BMX160_IF_CONF_ADDR, ®_val, 1)) { - return false; - } - - reg_val |= BMX160_IF_CONF_SECONDARY_IF_EN; - if (!writeBmxReg(BMX160_IF_CONF_ADDR, reg_val)) { - return false; - } - delay(BMX160_MAGN_COM_DELAY); - - if (!setBmi160AuxMode(true, 0x00)) { - return false; - } - - if (!writeBmm150Reg(BMM150_POWER_CONTROL_ADDR, BMM150_POWER_CONTROL_ENABLE)) { - return false; - } - delay(BMX160_MAGN_COM_DELAY); - - if (!readBmm150Reg(BMM150_CHIP_ID_ADDR, &chip_id) || chip_id != BMM150_CHIP_ID) { - LOG_WRN("BMM150 chip id mismatch: 0x%02x", chip_id); - return false; - } - - if (!writeBmm150Reg(BMM150_REP_XY_ADDR, BMM150_REP_XY_REGULAR) || - !writeBmm150Reg(BMM150_REP_Z_ADDR, BMM150_REP_Z_REGULAR) || - !writeBmm150Reg(BMM150_OP_MODE_ADDR, BMM150_OP_MODE_FORCED) || - !setBmi160AuxReadAddr(BMM150_DATA_X_LSB_ADDR) || - !writeBmxReg(BMX160_MAGN_CONFIG_ADDR, BMX160_MAGN_ODR_100HZ) || - !setBmi160AuxMode(false, 0x03)) { - return false; - } - - delay(50); - - return true; -} - -void DFRobot_BMX160::setBmx160MagnConf() -{ - // puts magnetometer into mag_if setup mode - writeBmxReg(BMX160_MAGN_IF_0_ADDR, 0x80); - delay(50); - // Sleep mode - writeBmxReg(BMX160_MAGN_IF_3_ADDR, 0x01); - writeBmxReg(BMX160_MAGN_IF_2_ADDR, 0x4B); - // REPXY regular preset - writeBmxReg(BMX160_MAGN_IF_3_ADDR, 0x04); - writeBmxReg(BMX160_MAGN_IF_2_ADDR, 0x51); - // REPZ regular preset - writeBmxReg(BMX160_MAGN_IF_3_ADDR, 0x0E); - writeBmxReg(BMX160_MAGN_IF_2_ADDR, 0x52); - // Prepare MAG_IF[1-3] for mag_if data mode - writeBmxReg(BMX160_MAGN_IF_3_ADDR, 0x02); - writeBmxReg(BMX160_MAGN_IF_2_ADDR, 0x4C); - writeBmxReg(BMX160_MAGN_IF_1_ADDR, 0x42); - // sets the sampling rate t0 100Hz - writeBmxReg(BMX160_MAGN_CONFIG_ADDR, 0x08); - // puts magnetometer into mag_if data mode sets data length of read burst operation to 8 bytes - writeBmxReg(BMX160_MAGN_IF_0_ADDR, 0x03); - delay(50); -} - -bool DFRobot_BMX160::setBmi160AuxMode(bool manual, uint8_t read_burst_len) -{ - uint8_t aux_if[2] = { - static_cast(BMX160_MAGN_BMM150_I2C_ADDR << 1), - static_cast((manual ? BMX160_MANUAL_MODE_EN_MSK : 0x00) | - (read_burst_len & BMX160_MAGN_READ_BURST_MSK)) - }; - - return writeReg(BMX160_AUX_IF_0_ADDR, aux_if, sizeof(aux_if)); -} - -bool DFRobot_BMX160::setBmi160AuxReadAddr(uint8_t reg) -{ - return writeBmxReg(BMX160_AUX_IF_2_ADDR, reg); -} - -bool DFRobot_BMX160::writeBmm150Reg(uint8_t reg, uint8_t value) -{ - if (!writeBmxReg(BMX160_AUX_IF_4_ADDR, value)) { - return false; - } - delay(BMX160_MAGN_COM_DELAY); - - if (!writeBmxReg(BMX160_AUX_IF_3_ADDR, reg)) { - return false; - } - delay(BMX160_MAGN_COM_DELAY); - - return true; -} - -bool DFRobot_BMX160::readBmm150Reg(uint8_t reg, uint8_t *value) -{ - if (!setBmi160AuxReadAddr(reg)) { - return false; - } - delay(BMX160_MAGN_COM_DELAY); - - return readReg(BMX160_MAG_DATA_ADDR, value, 1); -} - -void DFRobot_BMX160::setGyroRange(eGyroRange_t bits){ - switch (bits){ - case eGyroRange_125DPS: - gyroRange = BMX160_GYRO_SENSITIVITY_125DPS; - break; - case eGyroRange_250DPS: - gyroRange = BMX160_GYRO_SENSITIVITY_250DPS; - break; - case eGyroRange_500DPS: - gyroRange = BMX160_GYRO_SENSITIVITY_500DPS; - break; - case eGyroRange_1000DPS: - gyroRange = BMX160_GYRO_SENSITIVITY_1000DPS; - break; - case eGyroRange_2000DPS: - gyroRange = BMX160_GYRO_SENSITIVITY_2000DPS; - break; - default: - gyroRange = BMX160_GYRO_SENSITIVITY_2000DPS; - break; - } - writeBmxReg(BMX160_GYRO_RANGE_ADDR, bits); -} - -void DFRobot_BMX160::setAccelRange(eAccelRange_t bits){ - switch (bits){ - case eAccelRange_2G: - accelRange = BMX160_ACCEL_MG_LSB_2G * EARTH_ACC; - break; - case eAccelRange_4G: - accelRange = BMX160_ACCEL_MG_LSB_4G * EARTH_ACC; - break; - case eAccelRange_8G: - accelRange = BMX160_ACCEL_MG_LSB_8G * EARTH_ACC; - break; - case eAccelRange_16G: - accelRange = BMX160_ACCEL_MG_LSB_16G * EARTH_ACC; - break; - default: - accelRange = BMX160_ACCEL_MG_LSB_2G * EARTH_ACC; - break; - } - - writeBmxReg(BMX160_ACCEL_RANGE_ADDR, bits); -} - -void DFRobot_BMX160::setMagnODR(uint8_t val){ - writeBmxReg(BMX160_MAGN_CONFIG_ADDR, BMX160_MAGN_ODR_MASK & val); -} - -void DFRobot_BMX160::setGyroODR(uint8_t val){ - writeBmxReg(BMX160_GYRO_CONFIG_ADDR, BMX160_GYRO_ODR_MASK & val); -} - -void DFRobot_BMX160::setAccelODR(uint8_t val){ - writeBmxReg(BMX160_ACCEL_CONFIG_ADDR, BMX160_ACCEL_ODR_MASK & val); -} - -void DFRobot_BMX160::getAllData(sBmx160SensorData_t *magn, sBmx160SensorData_t *gyro, sBmx160SensorData_t *accel){ - - uint8_t data[23] = {0}; - int16_t x=0,y=0,z=0; - // put your main code here, to run repeatedly: - readReg(BMX160_MAG_DATA_ADDR, data, 23); - if(magn){ - x = (int16_t) (((uint16_t)data[1] << 8) | data[0]); - y = (int16_t) (((uint16_t)data[3] << 8) | data[2]); - z = (int16_t) (((uint16_t)data[5] << 8) | data[4]); - magn->x = x * BMX160_MAGN_UT_LSB; - magn->y = y * BMX160_MAGN_UT_LSB; - magn->z = z * BMX160_MAGN_UT_LSB; - } - if(gyro){ - x = (int16_t) (((uint16_t)data[9] << 8) | data[8]); - y = (int16_t) (((uint16_t)data[11] << 8) | data[10]); - z = (int16_t) (((uint16_t)data[13] << 8) | data[12]); - gyro->x = x * gyroRange; - gyro->y = y * gyroRange; - gyro->z = z * gyroRange; - } - if(accel){ - x = (int16_t) (((uint16_t)data[15] << 8) | data[14]); - y = (int16_t) (((uint16_t)data[17] << 8) | data[16]); - z = (int16_t) (((uint16_t)data[19] << 8) | data[18]); - accel->x = x * accelRange; - accel->y = y * accelRange; - accel->z = z * accelRange; - } -} - -bool DFRobot_BMX160::writeBmxReg(uint8_t reg, uint8_t value) -{ - uint8_t buffer[1] = {value}; - return writeReg(reg, buffer, 1); -} - -bool DFRobot_BMX160::writeReg(uint8_t reg, uint8_t *pBuf, uint16_t len) -{ - _i2c->aquire(); - - int ret = i2c_burst_write(_i2c->master, _addr, reg, pBuf, len); - if (ret) LOG_WRN("I2C write failed: %d", ret); - - _i2c->release(); - - return ret == 0; -} - -bool DFRobot_BMX160::readReg(uint8_t reg, uint8_t *pBuf, uint16_t len) -{ - _i2c->aquire(); - - int ret = i2c_burst_read(_i2c->master, _addr, reg, pBuf, len); - if (ret) LOG_WRN("I2C read failed: %d", ret); - - _i2c->release(); - - return ret == 0; -} - -bool DFRobot_BMX160::scan() -{ - _i2c->aquire(); - - uint8_t dummy = 0; - int ret = i2c_write(_i2c->master, &dummy, 0, _addr); - - _i2c->release(); - - return (ret == 0); -} diff --git a/src/SensorManager/BMX160/DFRobot_BMX160.h b/src/SensorManager/BMX160/DFRobot_BMX160.h deleted file mode 100644 index f6f48a51..00000000 --- a/src/SensorManager/BMX160/DFRobot_BMX160.h +++ /dev/null @@ -1,1148 +0,0 @@ -/*! - * @file DFRobot_BMX160.h - * @brief DFRobot_BMX160 class infrastructure - * @copyright Copyright (c) 2010 DFRobot Co.Ltd (http://www.dfrobot.com) - * @license The MIT License (MIT) - * @author [luoyufeng] (yufeng.luo@dfrobot.com) - * @maintainer [Fary](feng.yang@dfrobot.com) - * @version V1.0 - * @date 2021-10-20 - * @url https://github.com/DFRobot/DFRobot_BMX160 - */ -//#include -#include - -#define LITTLE_ENDIAN 1 - -/** Mask definitions */ -#define BMX160_MAGN_ODR_MASK 0x0F -#define BMX160_ACCEL_BW_MASK 0x70 -#define BMX160_ACCEL_ODR_MASK 0x0F -#define BMX160_ACCEL_UNDERSAMPLING_MASK 0x80 -#define BMX160_ACCEL_RANGE_MASK 0x0F -#define BMX160_GYRO_BW_MASK 0x30 -#define BMX160_GYRO_ODR_MASK 0x0F -#define BMX160_GYRO_RANGE_MSK 0x07 - -/** Mask definitions for INT_EN registers */ -#define BMX160_ANY_MOTION_X_INT_EN_MASK 0x01 -#define BMX160_HIGH_G_X_INT_EN_MASK 0x01 -#define BMX160_NO_MOTION_X_INT_EN_MASK 0x01 -#define BMX160_ANY_MOTION_Y_INT_EN_MASK 0x02 -#define BMX160_HIGH_G_Y_INT_EN_MASK 0x02 -#define BMX160_NO_MOTION_Y_INT_EN_MASK 0x02 -#define BMX160_ANY_MOTION_Z_INT_EN_MASK 0x04 -#define BMX160_HIGH_G_Z_INT_EN_MASK 0x04 -#define BMX160_NO_MOTION_Z_INT_EN_MASK 0x04 -#define BMX160_SIG_MOTION_INT_EN_MASK 0x07 -#define BMX160_ANY_MOTION_ALL_INT_EN_MASK 0x07 -#define BMX160_STEP_DETECT_INT_EN_MASK 0x08 -#define BMX160_DOUBLE_TAP_INT_EN_MASK 0x10 -#define BMX160_SINGLE_TAP_INT_EN_MASK 0x20 -#define BMX160_FIFO_FULL_INT_EN_MASK 0x20 -#define BMX160_ORIENT_INT_EN_MASK 0x40 -#define BMX160_FIFO_WATERMARK_INT_EN_MASK 0x40 -#define BMX160_LOW_G_INT_EN_MASK 0x08 -#define BMX160_STEP_DETECT_EN_MASK 0x08 -#define BMX160_FLAT_INT_EN_MASK 0x80 -#define BMX160_DATA_RDY_INT_EN_MASK 0x10 - -/** Mask definitions for INT_OUT_CTRL register */ -#define BMX160_INT1_EDGE_CTRL_MASK 0x01 -#define BMX160_INT1_OUTPUT_MODE_MASK 0x04 -#define BMX160_INT1_OUTPUT_TYPE_MASK 0x02 -#define BMX160_INT1_OUTPUT_EN_MASK 0x08 -#define BMX160_INT2_EDGE_CTRL_MASK 0x10 -#define BMX160_INT2_OUTPUT_MODE_MASK 0x40 -#define BMX160_INT2_OUTPUT_TYPE_MASK 0x20 -#define BMX160_INT2_OUTPUT_EN_MASK 0x80 - -/** Mask definitions for INT_LATCH register */ -#define BMX160_INT1_INPUT_EN_MASK 0x10 -#define BMX160_INT2_INPUT_EN_MASK 0x20 -#define BMX160_INT_LATCH_MASK 0x0F - -/** Mask definitions for INT_MAP register */ -#define BMX160_INT1_LOW_G_MASK 0x01 -#define BMX160_INT1_HIGH_G_MASK 0x02 -#define BMX160_INT1_SLOPE_MASK 0x04 -#define BMX160_INT1_NO_MOTION_MASK 0x08 -#define BMX160_INT1_DOUBLE_TAP_MASK 0x10 -#define BMX160_INT1_SINGLE_TAP_MASK 0x20 -#define BMX160_INT1_FIFO_FULL_MASK 0x20 -#define BMX160_INT1_FIFO_WM_MASK 0x40 -#define BMX160_INT1_ORIENT_MASK 0x40 -#define BMX160_INT1_FLAT_MASK 0x80 -#define BMX160_INT1_DATA_READY_MASK 0x80 -#define BMX160_INT2_LOW_G_MASK 0x01 -#define BMX160_INT1_LOW_STEP_DETECT_MASK 0x01 -#define BMX160_INT2_LOW_STEP_DETECT_MASK 0x01 -#define BMX160_INT2_HIGH_G_MASK 0x02 -#define BMX160_INT2_FIFO_FULL_MASK 0x02 -#define BMX160_INT2_FIFO_WM_MASK 0x04 -#define BMX160_INT2_SLOPE_MASK 0x04 -#define BMX160_INT2_DATA_READY_MASK 0x08 -#define BMX160_INT2_NO_MOTION_MASK 0x08 -#define BMX160_INT2_DOUBLE_TAP_MASK 0x10 -#define BMX160_INT2_SINGLE_TAP_MASK 0x20 -#define BMX160_INT2_ORIENT_MASK 0x40 -#define BMX160_INT2_FLAT_MASK 0x80 - -/** Mask definitions for INT_DATA register */ -#define BMX160_TAP_SRC_INT_MASK 0x08 -#define BMX160_LOW_HIGH_SRC_INT_MASK 0x80 -#define BMX160_MOTION_SRC_INT_MASK 0x80 - -/** Mask definitions for INT_MOTION register */ -#define BMX160_SLOPE_INT_DUR_MASK 0x03 -#define BMX160_NO_MOTION_INT_DUR_MASK 0xFC -#define BMX160_NO_MOTION_SEL_BIT_MASK 0x01 - -/** Mask definitions for INT_TAP register */ -#define BMX160_TAP_DUR_MASK 0x07 -#define BMX160_TAP_SHOCK_DUR_MASK 0x40 -#define BMX160_TAP_QUIET_DUR_MASK 0x80 -#define BMX160_TAP_THRES_MASK 0x1F - -/** Mask definitions for INT_FLAT register */ -#define BMX160_FLAT_THRES_MASK 0x3F -#define BMX160_FLAT_HOLD_TIME_MASK 0x30 -#define BMX160_FLAT_HYST_MASK 0x07 - -/** Mask definitions for INT_LOWHIGH register */ -#define BMX160_LOW_G_HYST_MASK 0x03 -#define BMX160_LOW_G_LOW_MODE_MASK 0x04 -#define BMX160_HIGH_G_HYST_MASK 0xC0 - -/** Mask definitions for INT_SIG_MOTION register */ -#define BMX160_SIG_MOTION_SEL_MASK 0x02 -#define BMX160_SIG_MOTION_SKIP_MASK 0x0C -#define BMX160_SIG_MOTION_PROOF_MASK 0x30 - -/** Mask definitions for INT_ORIENT register */ -#define BMX160_ORIENT_MODE_MASK 0x03 -#define BMX160_ORIENT_BLOCK_MASK 0x0C -#define BMX160_ORIENT_HYST_MASK 0xF0 -#define BMX160_ORIENT_THETA_MASK 0x3F -#define BMX160_ORIENT_UD_ENABLE 0x40 -#define BMX160_AXES_EN_MASK 0x80 - -/** Mask definitions for FIFO_CONFIG register */ -#define BMX160_FIFO_GYRO 0x80 -#define BMX160_FIFO_ACCEL 0x40 -#define BMX160_FIFO_MAGN 0x20 -#define BMX160_FIFO_TAG_INT1 0x08 -#define BMX160_FIFO_TAG_INT2 0x04 -#define BMX160_FIFO_TIME 0x02 -#define BMX160_FIFO_HEADER 0x10 -#define BMX160_FIFO_CONFIG_1_MASK 0xFE - - -/** Mask definitions for STEP_CONF register */ -#define BMX160_STEP_COUNT_EN_BIT_MASK 0x08 -#define BMX160_STEP_DETECT_MIN_THRES_MASK 0x18 -#define BMX160_STEP_DETECT_STEPTIME_MIN_MASK 0x07 -#define BMX160_STEP_MIN_BUF_MASK 0x07 - -/** Mask definition for FIFO Header Data Tag */ -#define BMX160_FIFO_TAG_INTR_MASK 0xFC - -/** Fifo byte counter mask definitions */ -#define BMX160_FIFO_BYTE_COUNTER_MASK 0x07 - -/** Enable/disable bit value */ -#define BMX160_ENABLE 0x01 -#define BMX160_DISABLE 0x00 - -/** Latch Duration */ -#define BMX160_LATCH_DUR_NONE 0x00 -#define BMX160_LATCH_DUR_312_5_MICRO_SEC 0x01 -#define BMX160_LATCH_DUR_625_MICRO_SEC 0x02 -#define BMX160_LATCH_DUR_1_25_MILLI_SEC 0x03 -#define BMX160_LATCH_DUR_2_5_MILLI_SEC 0x04 -#define BMX160_LATCH_DUR_5_MILLI_SEC 0x05 -#define BMX160_LATCH_DUR_10_MILLI_SEC 0x06 -#define BMX160_LATCH_DUR_20_MILLI_SEC 0x07 -#define BMX160_LATCH_DUR_40_MILLI_SEC 0x08 -#define BMX160_LATCH_DUR_80_MILLI_SEC 0x09 -#define BMX160_LATCH_DUR_160_MILLI_SEC 0x0A -#define BMX160_LATCH_DUR_320_MILLI_SEC 0x0B -#define BMX160_LATCH_DUR_640_MILLI_SEC 0x0C -#define BMX160_LATCH_DUR_1_28_SEC 0x0D -#define BMX160_LATCH_DUR_2_56_SEC 0x0E -#define BMX160_LATCHED 0x0F - -/** bmx160 Register map */ -#define BMX160_CHIP_ID_ADDR 0x00 -#define BMX160_ERROR_REG_ADDR 0x02 -#define BMX160_MAG_DATA_ADDR 0x04 -#define BMX160_GYRO_DATA_ADDR 0x0C -#define BMX160_ACCEL_DATA_ADDR 0x12 -#define BMX160_STATUS_ADDR 0x1B -#define BMX160_INT_STATUS_ADDR 0x1C -#define BMX160_FIFO_LENGTH_ADDR 0x22 -#define BMX160_FIFO_DATA_ADDR 0x24 -#define BMX160_ACCEL_CONFIG_ADDR 0x40 -#define BMX160_ACCEL_RANGE_ADDR 0x41 -#define BMX160_GYRO_CONFIG_ADDR 0x42 -#define BMX160_GYRO_RANGE_ADDR 0x43 -#define BMX160_MAGN_CONFIG_ADDR 0x44 -#define BMX160_FIFO_DOWN_ADDR 0x45 -#define BMX160_FIFO_CONFIG_0_ADDR 0x46 -#define BMX160_FIFO_CONFIG_1_ADDR 0x47 -#define BMX160_MAGN_RANGE_ADDR 0x4B -#define BMX160_MAGN_IF_0_ADDR 0x4C -#define BMX160_MAGN_IF_1_ADDR 0x4D -#define BMX160_MAGN_IF_2_ADDR 0x4E -#define BMX160_MAGN_IF_3_ADDR 0x4F -#define BMX160_INT_ENABLE_0_ADDR 0x50 -#define BMX160_INT_ENABLE_1_ADDR 0x51 -#define BMX160_INT_ENABLE_2_ADDR 0x52 -#define BMX160_INT_OUT_CTRL_ADDR 0x53 -#define BMX160_INT_LATCH_ADDR 0x54 -#define BMX160_INT_MAP_0_ADDR 0x55 -#define BMX160_INT_MAP_1_ADDR 0x56 -#define BMX160_INT_MAP_2_ADDR 0x57 -#define BMX160_INT_DATA_0_ADDR 0x58 -#define BMX160_INT_DATA_1_ADDR 0x59 -#define BMX160_INT_LOWHIGH_0_ADDR 0x5A -#define BMX160_INT_LOWHIGH_1_ADDR 0x5B -#define BMX160_INT_LOWHIGH_2_ADDR 0x5C -#define BMX160_INT_LOWHIGH_3_ADDR 0x5D -#define BMX160_INT_LOWHIGH_4_ADDR 0x5E -#define BMX160_INT_MOTION_0_ADDR 0x5F -#define BMX160_INT_MOTION_1_ADDR 0x60 -#define BMX160_INT_MOTION_2_ADDR 0x61 -#define BMX160_INT_MOTION_3_ADDR 0x62 -#define BMX160_INT_TAP_0_ADDR 0x63 -#define BMX160_INT_TAP_1_ADDR 0x64 -#define BMX160_INT_ORIENT_0_ADDR 0x65 -#define BMX160_INT_ORIENT_1_ADDR 0x66 -#define BMX160_INT_FLAT_0_ADDR 0x67 -#define BMX160_INT_FLAT_1_ADDR 0x68 -#define BMX160_FOC_CONF_ADDR 0x69 -#define BMX160_CONF_ADDR 0x6A - -#define BMX160_IF_CONF_ADDR 0x6B -#define BMX160_SELF_TEST_ADDR 0x6D -#define BMX160_OFFSET_ADDR 0x71 -#define BMX160_OFFSET_CONF_ADDR 0x77 -#define BMX160_INT_STEP_CNT_0_ADDR 0x78 -#define BMX160_INT_STEP_CONFIG_0_ADDR 0x7A -#define BMX160_INT_STEP_CONFIG_1_ADDR 0x7B -#define BMX160_COMMAND_REG_ADDR 0x7E -#define BMX160_SPI_COMM_TEST_ADDR 0x7F -#define BMX160_INTL_PULLUP_CONF_ADDR 0x85 - -/* BMI160-style auxiliary interface used by development BMX160/BMM150 hardware */ -#define BMX160_AUX_IF_0_ADDR 0x4B -#define BMX160_AUX_IF_1_ADDR 0x4C -#define BMX160_AUX_IF_2_ADDR 0x4D -#define BMX160_AUX_IF_3_ADDR 0x4E -#define BMX160_AUX_IF_4_ADDR 0x4F -#define BMX160_IF_CONF_SECONDARY_IF_EN 0x20 - -/* BMM150 registers behind the BMX160/BMI160 auxiliary interface */ -#define BMM150_CHIP_ID_ADDR 0x40 -#define BMM150_CHIP_ID 0x32 -#define BMM150_DATA_X_LSB_ADDR 0x42 -#define BMM150_POWER_CONTROL_ADDR 0x4B -#define BMM150_OP_MODE_ADDR 0x4C -#define BMM150_REP_XY_ADDR 0x51 -#define BMM150_REP_Z_ADDR 0x52 -#define BMM150_POWER_CONTROL_ENABLE 0x01 -#define BMM150_OP_MODE_FORCED 0x02 -#define BMM150_REP_XY_REGULAR 0x04 -#define BMM150_REP_Z_REGULAR 0x0E - -/** Error code definitions */ -#define BMX160_OK 0 -#define BMX160_E_NULL_PTR -1 -#define BMX160_E_COM_FAIL -2 -#define BMX160_E_DEV_NOT_FOUND -3 -#define BMX160_E_OUT_OF_RANGE -4 -#define BMX160_E_INVALID_INPUT -5 -#define BMX160_E_ACCEL_ODR_BW_INVALID -6 -#define BMX160_E_GYRO_ODR_BW_INVALID -7 -#define BMX160_E_LWP_PRE_FLTR_INT_INVALID -8 -#define BMX160_E_LWP_PRE_FLTR_INVALID -9 -#define BMX160_E_MAGN_NOT_FOUND -10 -#define BMX160_FOC_FAILURE -11 -#define BMX160_ERR_CHOOSE -12 - -/**\name API warning codes */ -#define BMX160_W_GYRO_SELF_TEST_FAIL (1) -#define BMX160_W_ACCEl_SELF_TEST_FAIL (2) - -/** bmx160 unique chip identifier */ -#define BMX160_CHIP_ID 0xD8 - -/** Soft reset command */ -#define BMX160_SOFT_RESET_CMD 0xb6 -#define BMX160_SOFT_RESET_DELAY_MS 15 -/** Start FOC command */ -#define BMX160_START_FOC_CMD 0x03 -/** NVM backup enabling command */ -#define BMX160_NVM_BACKUP_EN 0xA0 - -/* Delay in ms settings */ -#define BMX160_ACCEL_DELAY_MS 5 -#define BMX160_GYRO_DELAY_MS 81 -#define BMX160_ONE_MS_DELAY 1 -#define BMX160_MAGN_COM_DELAY 10 -#define BMX160_GYRO_SELF_TEST_DELAY 20 -#define BMX160_ACCEL_SELF_TEST_DELAY 50 - -/** Self test configurations */ -#define BMX160_ACCEL_SELF_TEST_CONFIG 0x2C -#define BMX160_ACCEL_SELF_TEST_POSITIVE_EN 0x0D -#define BMX160_ACCEL_SELF_TEST_NEGATIVE_EN 0x09 -#define BMX160_ACCEL_SELF_TEST_LIMIT 8192 - -/** Power mode settings */ -/* Accel power mode */ -#define BMX160_ACCEL_NORMAL_MODE 0x11 -#define BMX160_ACCEL_LOWPOWER_MODE 0x12 -#define BMX160_ACCEL_SUSPEND_MODE 0x10 - -/* Gyro power mode */ -#define BMX160_GYRO_SUSPEND_MODE 0x14 -#define BMX160_GYRO_NORMAL_MODE 0x15 -#define BMX160_GYRO_FASTSTARTUP_MODE 0x17 - -/* Magn power mode */ -#define BMX160_MAGN_SUSPEND_MODE 0x18 -#define BMX160_MAGN_NORMAL_MODE 0x19 -#define BMX160_MAGN_LOWPOWER_MODE 0x1A - -/** Range settings */ -/* Accel Range */ -#define BMX160_ACCEL_RANGE_2G 0x03 -#define BMX160_ACCEL_RANGE_4G 0x05 -#define BMX160_ACCEL_RANGE_8G 0x08 -#define BMX160_ACCEL_RANGE_16G 0x0C - -/* Gyro Range */ -#define BMX160_GYRO_RANGE_2000_DPS 0x00 -#define BMX160_GYRO_RANGE_1000_DPS 0x01 -#define BMX160_GYRO_RANGE_500_DPS 0x02 -#define BMX160_GYRO_RANGE_250_DPS 0x03 -#define BMX160_GYRO_RANGE_125_DPS 0x04 - - -#define BMX160_ACCEL_MG_LSB_2G 0.000061035F ///< Macro for mg per LSB at +/- 2g sensitivity (1 LSB = 0.000061035mg) */ -#define BMX160_ACCEL_MG_LSB_4G 0.000122070F ///< Macro for mg per LSB at +/- 4g sensitivity (1 LSB = 0.000122070mg) */ -#define BMX160_ACCEL_MG_LSB_8G 0.000244141F ///< Macro for mg per LSB at +/- 8g sensitivity (1 LSB = 0.000244141mg) */ -#define BMX160_ACCEL_MG_LSB_16G 0.000488281F ///< Macro for mg per LSB at +/- 16g sensitivity (1 LSB = 0.000488281mg) */ - -#define BMX160_GYRO_SENSITIVITY_125DPS 0.0038110F ///< Gyroscope sensitivity at 125dps */ -#define BMX160_GYRO_SENSITIVITY_250DPS 0.0076220F ///< Gyroscope sensitivity at 250dps */ -#define BMX160_GYRO_SENSITIVITY_500DPS 0.0152439F ///< Gyroscope sensitivity at 500dps */ -#define BMX160_GYRO_SENSITIVITY_1000DPS 0.0304878F ///< Gyroscope sensitivity at 1000dps */ -#define BMX160_GYRO_SENSITIVITY_2000DPS 0.0609756F ///< Gyroscope sensitivity at 2000dps */ - -/** Bandwidth settings */ -/* Accel Bandwidth */ -#define BMX160_ACCEL_BW_OSR4_AVG1 0x00 -#define BMX160_ACCEL_BW_OSR2_AVG2 0x01 -#define BMX160_ACCEL_BW_NORMAL_AVG4 0x02 -#define BMX160_ACCEL_BW_RES_AVG8 0x03 -#define BMX160_ACCEL_BW_RES_AVG16 0x04 -#define BMX160_ACCEL_BW_RES_AVG32 0x05 -#define BMX160_ACCEL_BW_RES_AVG64 0x06 -#define BMX160_ACCEL_BW_RES_AVG128 0x07 - -#define BMX160_GYRO_BW_OSR4_MODE 0x00 -#define BMX160_GYRO_BW_OSR2_MODE 0x01 -#define BMX160_GYRO_BW_NORMAL_MODE 0x02 - -#define BMX160_MAGN_UT_LSB (0.3F) ///< Macro for micro tesla (uT) per LSB (1 LSB = 0.1uT) */ -/* Output Data Rate settings */ -/* Accel Output data rate */ -#define BMX160_ACCEL_ODR_RESERVED 0x00 -#define BMX160_ACCEL_ODR_0_78HZ 0x01 -#define BMX160_ACCEL_ODR_1_56HZ 0x02 -#define BMX160_ACCEL_ODR_3_12HZ 0x03 -#define BMX160_ACCEL_ODR_6_25HZ 0x04 -#define BMX160_ACCEL_ODR_12_5HZ 0x05 -#define BMX160_ACCEL_ODR_25HZ 0x06 -#define BMX160_ACCEL_ODR_50HZ 0x07 -#define BMX160_ACCEL_ODR_100HZ 0x08 -#define BMX160_ACCEL_ODR_200HZ 0x09 -#define BMX160_ACCEL_ODR_400HZ 0x0A -#define BMX160_ACCEL_ODR_800HZ 0x0B -#define BMX160_ACCEL_ODR_1600HZ 0x0C -#define BMX160_ACCEL_ODR_RESERVED0 0x0D -#define BMX160_ACCEL_ODR_RESERVED1 0x0E -#define BMX160_ACCEL_ODR_RESERVED2 0x0F - -/* Gyro Output data rate */ -#define BMX160_GYRO_ODR_RESERVED 0x00 -#define BMX160_GYRO_ODR_25HZ 0x06 -#define BMX160_GYRO_ODR_50HZ 0x07 -#define BMX160_GYRO_ODR_100HZ 0x08 -#define BMX160_GYRO_ODR_200HZ 0x09 -#define BMX160_GYRO_ODR_400HZ 0x0A -#define BMX160_GYRO_ODR_800HZ 0x0B -#define BMX160_GYRO_ODR_1600HZ 0x0C -#define BMX160_GYRO_ODR_3200HZ 0x0D - -/* Magnetometer sensor Output data rate */ -#define BMX160_MAGN_ODR_RESERVED 0x00 -#define BMX160_MAGN_ODR_0_78HZ 0x01 -#define BMX160_MAGN_ODR_1_56HZ 0x02 -#define BMX160_MAGN_ODR_3_12HZ 0x03 -#define BMX160_MAGN_ODR_6_25HZ 0x04 -#define BMX160_MAGN_ODR_12_5HZ 0x05 -#define BMX160_MAGN_ODR_25HZ 0x06 -#define BMX160_MAGN_ODR_50HZ 0x07 -#define BMX160_MAGN_ODR_100HZ 0x08 -#define BMX160_MAGN_ODR_200HZ 0x09 -#define BMX160_MAGN_ODR_400HZ 0x0A -#define BMX160_MAGN_ODR_800HZ 0x0B - -/* Maximum limits definition */ -#define BMX160_MAGN_ODR_MAX 14 -#define BMX160_MAGN_BW_MAX 2 -#define BMX160_MAGN_RANGE_MAX 8 -#define BMX160_GYRO_ODR_MAX 13 -#define BMX160_GYRO_BW_MAX 2 -#define BMX160_GYRO_RANGE_MAX 4 -#define BMX160_ACCEL_ODR_MAX 15 -#define BMX160_ACCEL_BW_MAX 2 -#define BMX160_ACCEL_RANGE_MAX 12 - - - -/** FIFO_CONFIG Definitions */ -#define BMX160_FIFO_TIME_ENABLE 0x02 -#define BMX160_FIFO_TAG_INT2_ENABLE 0x04 -#define BMX160_FIFO_TAG_INT1_ENABLE 0x08 -#define BMX160_FIFO_HEAD_ENABLE 0x10 -#define BMX160_FIFO_M_ENABLE 0x20 -#define BMX160_FIFO_A_ENABLE 0x40 -#define BMX160_FIFO_M_A_ENABLE 0x60 -#define BMX160_FIFO_G_ENABLE 0x80 -#define BMX160_FIFO_M_G_ENABLE 0xA0 -#define BMX160_FIFO_G_A_ENABLE 0xC0 -#define BMX160_FIFO_M_G_A_ENABLE 0xE0 - - -/* Accel, gyro and magn. sensor length and also their combined - * length definitions in FIFO */ -#define BMX160_FIFO_G_LENGTH 6 -#define BMX160_FIFO_A_LENGTH 6 -#define BMX160_FIFO_M_LENGTH 8 -#define BMX160_FIFO_GA_LENGTH 12 -#define BMX160_FIFO_MA_LENGTH 14 -#define BMX160_FIFO_MG_LENGTH 14 -#define BMX160_FIFO_MGA_LENGTH 20 - - -/** FIFO Header Data definitions */ -#define BMX160_FIFO_HEAD_SKIP_FRAME 0x40 -#define BMX160_FIFO_HEAD_SENSOR_TIME 0x44 -#define BMX160_FIFO_HEAD_INPUT_CONFIG 0x48 -#define BMX160_FIFO_HEAD_OVER_READ 0x80 -#define BMX160_FIFO_HEAD_A 0x84 -#define BMX160_FIFO_HEAD_G 0x88 -#define BMX160_FIFO_HEAD_G_A 0x8C -#define BMX160_FIFO_HEAD_M 0x90 -#define BMX160_FIFO_HEAD_M_A 0x94 -#define BMX160_FIFO_HEAD_M_G 0x98 -#define BMX160_FIFO_HEAD_M_G_A 0x9C - - -/** FIFO sensor time length definitions */ -#define BMX160_SENSOR_TIME_LENGTH 3) - - -/** FIFO DOWN selection */ -/* Accel fifo down-sampling values*/ -#define BMX160_ACCEL_FIFO_DOWN_ZERO 0x00 -#define BMX160_ACCEL_FIFO_DOWN_ONE 0x10 -#define BMX160_ACCEL_FIFO_DOWN_TWO 0x20 -#define BMX160_ACCEL_FIFO_DOWN_THREE 0x30 -#define BMX160_ACCEL_FIFO_DOWN_FOUR 0x40 -#define BMX160_ACCEL_FIFO_DOWN_FIVE 0x50 -#define BMX160_ACCEL_FIFO_DOWN_SIX 0x60 -#define BMX160_ACCEL_FIFO_DOWN_SEVEN 0x70 - -/* Gyro fifo down-smapling values*/ -#define BMX160_GYRO_FIFO_DOWN_ZERO 0x00 -#define BMX160_GYRO_FIFO_DOWN_ONE 0x01 -#define BMX160_GYRO_FIFO_DOWN_TWO 0x02 -#define BMX160_GYRO_FIFO_DOWN_THREE 0x03 -#define BMX160_GYRO_FIFO_DOWN_FOUR 0x04 -#define BMX160_GYRO_FIFO_DOWN_FIVE 0x05 -#define BMX160_GYRO_FIFO_DOWN_SIX 0x06 -#define BMX160_GYRO_FIFO_DOWN_SEVEN 0x07 - - -#define BMX160_ACCEL_FIFO_FILT_EN 0x80 //< Accel Fifo filter enable*/ - -#define BMX160_GYRO_FIFO_FILT_EN 0x08 //< Gyro Fifo filter enable*/ - -/** Definitions to check validity of FIFO frames */ -#define FIFO_CONFIG_MSB_CHECK 0x80 -#define FIFO_CONFIG_LSB_CHECK 0x00 - -/*! bmx160 accel FOC configurations */ -#define BMX160_FOC_ACCEL_DISABLED 0x00 -#define BMX160_FOC_ACCEL_POSITIVE_G 0x01 -#define BMX160_FOC_ACCEL_NEGATIVE_G 0x02 -#define BMX160_FOC_ACCEL_0G 0x03 - -/** Array Parameter DefinItions */ -#define BMX160_SENSOR_TIME_LSB_BYTE 0 -#define BMX160_SENSOR_TIME_XLSB_BYTE 1 -#define BMX160_SENSOR_TIME_MSB_BYTE 2 - - -/** Interface settings */ -#define BMX160_SPI_INTF 1 -#define BMX160_I2C_INTF 0 -#define BMX160_SPI_RD_MASK 0x80 -#define BMX160_SPI_WR_MASK 0x7F - -/* Sensor & time select definition*/ -#define BMX160_MAG_SEL 0x01 -#define BMX160_GYRO_SEL 0x02 -#define BMX160_ACCEL_SEL 0x03 -#define BMX160_TIME_SEL 0x04 - - -#define BMX160_SEN_SEL_MASK 0x07 //< Sensor select mask*/ -#define BMX160_ERR_REG_MASK 0x0F //< Error code mask */ -#define BMX160_I2C_ADDR 0x68 //< bmx160 I2C address */ -#define BMX160_MAGN_BMM150_I2C_ADDR 0x10 //< bmx160 secondary IF address */ - -/** bmx160 Length definitions */ -#define BMX160_ONE 1 -#define BMX160_TWO 2 -#define BMX160_THREE 3 -#define BMX160_FOUR 4 -#define BMX160_FIVE 5 - - -#define BMX160_FIFO_LEVEL_MARGIN 16 //< bmx160 fifo level Margin */ -#define BMX160_FIFO_FLUSH_VALUE 0xB0 //< bmx160 fifo flush Command */ - -/** bmx160 offset values for xyz axes of accel */ -#define BMX160_ACCEL_MIN_OFFSET -128 -#define BMX160_ACCEL_MAX_OFFSET 127 - -/** bmx160 offset values for xyz axes of gyro */ -#define BMX160_GYRO_MIN_OFFSET -512 -#define BMX160_GYRO_MAX_OFFSET 511 - -/** bmx160 fifo full interrupt position and mask */ -#define BMX160_FIFO_FULL_INT_POS 5 -#define BMX160_FIFO_FULL_INT_MSK 0x20 -#define BMX160_FIFO_WTM_INT_POS 6 -#define BMX160_FIFO_WTM_INT_MSK 0x40 - -#define BMX160_FIFO_FULL_INT_PIN1_POS 5 -#define BMX160_FIFO_FULL_INT_PIN1_MSK 0x20 -#define BMX160_FIFO_FULL_INT_PIN2_POS 1 -#define BMX160_FIFO_FULL_INT_PIN2_MSK 0x02 - -#define BMX160_FIFO_WTM_INT_PIN1_POS 6 -#define BMX160_FIFO_WTM_INT_PIN1_MSK 0x40 -#define BMX160_FIFO_WTM_INT_PIN2_POS 2 -#define BMX160_FIFO_WTM_INT_PIN2_MSK 0x04 - -#define BMX160_MANUAL_MODE_EN_POS 7 -#define BMX160_MANUAL_MODE_EN_MSK 0x80 -#define BMX160_MAGN_READ_BURST_POS 0 -#define BMX160_MAGN_READ_BURST_MSK 0x03 - -#define BMX160_GYRO_SELF_TEST_POS 4 -#define BMX160_GYRO_SELF_TEST_MSK 0x10 -#define BMX160_GYRO_SELF_TEST_STATUS_POS 1 -#define BMX160_GYRO_SELF_TEST_STATUS_MSK 0x02 - -#define BMX160_GYRO_FOC_EN_POS 6 -#define BMX160_GYRO_FOC_EN_MSK 0x40 - -#define BMX160_ACCEL_FOC_X_CONF_POS 4 -#define BMX160_ACCEL_FOC_X_CONF_MSK 0x30 - -#define BMX160_ACCEL_FOC_Y_CONF_POS 2 -#define BMX160_ACCEL_FOC_Y_CONF_MSK 0x0C - -#define BMX160_ACCEL_FOC_Z_CONF_MSK 0x03 - -#define BMX160_FOC_STATUS_POS 3 -#define BMX160_FOC_STATUS_MSK 0x08 - -#define BMX160_GYRO_OFFSET_X_MSK 0x03 - -#define BMX160_GYRO_OFFSET_Y_POS 2 -#define BMX160_GYRO_OFFSET_Y_MSK 0x0C - -#define BMX160_GYRO_OFFSET_Z_POS 4 -#define BMX160_GYRO_OFFSET_Z_MSK 0x30 - -#define BMX160_GYRO_OFFSET_EN_POS 7 -#define BMX160_GYRO_OFFSET_EN_MSK 0x80 - -#define BMX160_ACCEL_OFFSET_EN_POS 6 -#define BMX160_ACCEL_OFFSET_EN_MSK 0x40 - - -#define BMX160_GYRO_OFFSET_POS 8 -#define BMX160_GYRO_OFFSET_MSK 0x0300 - -#define BMX160_NVM_UPDATE_POS 1 -#define BMX160_NVM_UPDATE_MSK 0x02 - -#define BMX160_NVM_STATUS_POS 4 -#define BMX160_NVM_STATUS_MSK 0x10 - -/* BIT SLICE GET AND SET FUNCTIONS */ -#define BMX160_GET_BITS(regvar, bitname)\ - ((regvar & bitname##_MSK) >> bitname##_POS) -#define BMX160_SET_BITS(regvar, bitname, val)\ - ((regvar & ~bitname##_MSK) | \ - ((val<> 8) - -/** - * @struct sBmx160FifoFrame_t - * @brief This structure holds the information for usage of FIFO by the user. - */ -typedef struct { - uint8_t *data; /**< Data buffer of user defined length is to be mapped here */ - uint16_t length; /**< While calling the API "BMX160_get_fifo_data" , length stores number of bytes in FIFO to be read (specified by user as input) and after execution of the API ,number of FIFO data bytes available is provided as an output to user */ - uint8_t fifoTimeEnable; /**< FIFO time enable */ - uint8_t fifoHeaderEnable; /**< Enabling of the FIFO header to stream in header mode */ - uint8_t fifoDataEnable; /**< Streaming of the Accelerometer, Gyroscope sensor data or both in FIFO */ - uint16_t accelByteStartIdx;/**< Will be equal to length when no more frames are there to parse */ - uint16_t gyroByteStartIdx; /**< Will be equal to length when no more frames are there to parse */ - uint16_t magnByteStartIdx; /**< Will be equal to length when no more frames are there to parse */ - uint32_t sensorTime; /**< Value of FIFO sensor time time */ - uint8_t skippedFrameCount; /**< Value of Skipped frame counts */ -}sBmx160FifoFrame_t; - -/** - * @enum eBmx160AnySigMotionActiveInterruptState_t - * @brief bmx160 active state of any & sig motion interrupt. - */ -typedef enum { - eBmx160BothAnySigMotionDisabled = -1, /**< Both any & sig motion are disabled */ - eBmx160AnyMotionEnabled, /**< Any-motion selected */ - eBmx160SigMotionEnabled /**< Sig-motion selected */ -}eBmx160AnySigMotionActiveInterruptState_t; - - -/** - * @struct sBmx160Cfg_t - * @brief bmx160 sensor configuration structure - */ -typedef struct { - uint8_t power; /**< power mode */ - uint8_t odr; /**< output data rate */ - uint8_t range; /**< range */ - uint8_t bw; /**< bandwidth */ -}sBmx160Cfg_t; - - - -/* type definitions */ -typedef int8_t (*bmx160ComFptrT)(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len); -typedef void (*bmx160DelayFptrT)(uint32_t period); - -/** - * @struct sBmx160Dev_t - */ -typedef struct { - uint8_t chipId; /**< Chip Id */ - uint8_t id; /**< Device Id */ - uint8_t interface; /**< 0 - I2C , 1 - SPI Interface */ - eBmx160AnySigMotionActiveInterruptState_t any_sig_sel; /**< Hold active interrupts status for any and sig motion 0 - Any-motion enable, 1 - Sig-motion enable, -1 neither any-motion nor sig-motion selected */ - sBmx160Cfg_t magnCfg; /**< Structure to configure Magnetometer sensor */ - sBmx160Cfg_t prevMagnCfg; /**< Structure to hold previous/old magn config parameters.This is used at driver level to prevent overwriting of same data, hence user does not change it in the code */ - sBmx160Cfg_t accelCfg; /**< Structure to configure Accel sensor */ - sBmx160Cfg_t prevAccelCfg; /**< Structure to hold previous/old accel config parameters.This is used at driver level to prevent overwriting of same data, hence user does not change it in the code */ - sBmx160Cfg_t gyroCfg; /**< Structure to configure Gyro sensor */ - sBmx160Cfg_t prevGyroCfg; /**< Structure to hold previous/old gyro config parameters. This is used at driver level to prevent overwriting of same data, hence user does not change it in the code */ - sBmx160FifoFrame_t *fifo; /**< FIFO related configurations */ - bmx160ComFptrT read; /**< Read function pointer */ - bmx160ComFptrT write; /**< Write function pointer */ - bmx160DelayFptrT delayMs; /**< Delay function pointer */ -}sBmx160Dev_t; - -/** - * @struct sBmx160SensorData_t - * @brief bmx160 sensor data structure which comprises of accel data - */ -typedef struct { - float x; /**< X-axis sensor data */ - float y; /**< Y-axis sensor data */ - float z; /**< Z-axis sensor data */ - uint32_t sensortime; /**< sensor time */ -}sBmx160SensorData_t; // __attribute__((packed)); - -/** - * @enum eBmx160IntChannel_t - * @brief Interrupt channel - */ - typedef enum { - BMX160_INT_CHANNEL_NONE, /**< Un-map both channels */ - BMX160_INT_CHANNEL_1, /**< interrupt Channel 1 */ - BMX160_INT_CHANNEL_2, /**< interrupt Channel 2 */ - BMX160_INT_CHANNEL_BOTH /**< Map both channels */ -}eBmx160IntChannel_t; - -/** - * @enum eBmx160IntTypes_t - * @brief Select Interrupt - */ -typedef enum { - BMX160_ACC_ANY_MOTION_INT, /**< Slope/Any-motion interrupt */ - BMX160_ACC_SIG_MOTION_INT, /**< Significant motion interrupt */ - BMX160_STEP_DETECT_INT, /**< Step detector interrupt */ - BMX160_ACC_DOUBLE_TAP_INT, /**< double tap interrupt */ - BMX160_ACC_SINGLE_TAP_INT, /**< single tap interrupt */ - BMX160_ACC_ORIENT_INT, /**< orientation interrupt */ - BMX160_ACC_FLAT_INT, /**< flat interrupt */ - BMX160_ACC_HIGH_G_INT, /**< high-g interrupt */ - BMX160_ACC_LOW_G_INT, /**< low-g interrupt */ - BMX160_ACC_SLOW_NO_MOTION_INT, /**< slow/no-motion interrupt */ - BMX160_ACC_GYRO_DATA_RDY_INT, /**< data ready interrupt */ - BMX160_ACC_GYRO_FIFO_FULL_INT, /**< fifo full interrupt */ - BMX160_ACC_GYRO_FIFO_WATERMARK_INT /**< fifo watermark interrupt */ -}eBmx160IntTypes_t; - -/** - * @struct sBmx160IntPinSettg_t - * @brief Structure configuring Interrupt pins - */ -typedef struct { -#if LITTLE_ENDIAN == 1 - uint16_t outputEn :1; /**< To enable either INT1 or INT2 pin as output. 0- output disabled ,1- output enabled */ - uint16_t outputMode :1; /**< 0 - push-pull 1- open drain,only valid if outputEn is set 1 */ - uint16_t outputType :1; /**< 0 - active low , 1 - active high level.if outputEn is 1,this applies to interrupts,else PMU_trigger */ - uint16_t edgeCtrl :1; /**< 0 - level trigger , 1 - edge trigger */ - uint16_t inputEn :1; /**< To enable either INT1 or INT2 pin as input. 0 - input disabled ,1 - input enabled */ - uint16_t latchDur :4; /**< latch duration*/ -#elif BIG_ENDIAN == 1 - uint16_t latchDur : 4; /**< latch duration*/ - uint16_t inputEn : 1; /**< Latched,non-latched or temporary interrupt modes */ - uint16_t edgeCtrl : 1; /**< 1 - edge trigger, 0 - level trigger */ - uint16_t outputType : 1; /**< 0 - active low , 1 - active high level. if outputEn is 1,this applies to interrupts,else PMU_trigger */ - uint16_t outputMode : 1; /**< 0 - push-pull , 1 - open drain,only valid if outputEn is set 1 */ - uint16_t outputEn : 1; /**< To enable either INT1 or INT2 pin as output. 0 - output disabled , 1 - output enabled */ -#endif -}sBmx160IntPinSettg_t; - -/** - * @struct sBmx160AccTapIntCfg_t - * @brief Tap interrupt structure - */ -typedef struct { -#if LITTLE_ENDIAN == 1 - uint16_t tapThr :5; /**< tap threshold */ - uint16_t tapShock :1; /**< tap shock */ - uint16_t tapQuiet :1; /**< tap quiet */ - uint16_t tapDur :3; /**< tap duration */ - uint16_t tapDataSrc :1; /**< data source 0- filter & 1 pre-filter*/ - uint16_t tapEn :1; /**< tap enable, 1 - enable, 0 - disable */ -#elif BIG_ENDIAN == 1 - uint16_t tapEn :1; /**< tap enable, 1 - enable, 0 - disable */ - uint16_t tapDataSrc :1; /**< data source 0- filter & 1 pre-filter*/ - uint16_t tapDur : 3; /**< tap duration */ - uint16_t tapQuiet : 1; /**< tap quiet */ - uint16_t tapShock : 1; /**< tap shock */ - uint16_t tapThr : 5; /**< tap threshold */ -#endif -}sBmx160AccTapIntCfg_t; - -/** - * @struct sBmx160AccAnyMotIntCfg_t - * @brief Slope interrupt structure - */ -typedef struct { -#if LITTLE_ENDIAN == 1 - uint8_t anymotionEn :1; /**< 1 any-motion enable, 0 - any-motion disable */ - uint8_t anymotionX :1; /**< slope interrupt x, 1 - enable, 0 - disable */ - uint8_t anymotionY :1; /**< slope interrupt y, 1 - enable, 0 - disable */ - uint8_t anymotionZ :1; /**< slope interrupt z, 1 - enable, 0 - disable */ - uint8_t anymotionDur :2; /**< slope duration */ - uint8_t anymotionDataSrc :1; /**< data source 0- filter & 1 pre-filter*/ - uint8_t anymotionThr; /**< slope threshold */ -#elif BIG_ENDIAN == 1 - uint8_t anymotionThr; /**< slope threshold */ - uint8_t anymotionDataSrc :1; /**< data source 0- filter & 1 pre-filter*/ - uint8_t anymotionDur : 2; /**< slope duration */ - uint8_t anymotionZ : 1; /**< slope interrupt z, 1 - enable, 0 - disable */ - uint8_t anymotionY : 1; /**< slope interrupt y, 1 - enable, 0 - disable */ - uint8_t anymotionX : 1; /**< slope interrupt x, 1 - enable, 0 - disable */ - uint8_t anymotionEn :1; /**< 1 any-motion enable, 0 - any-motion disable */ -#endif -}sBmx160AccAnyMotIntCfg_t; - -/** - * @struct sBmx160AccSigMotIntCfg_t - * @brief Significant motion interrupt structure - */ -typedef struct { -#if LITTLE_ENDIAN == 1 - uint8_t sigMotSkip :2; /**< skip time of sig-motion interrupt */ - uint8_t sigMotProof :2; /**< proof time of sig-motion interrupt */ - uint8_t sigDataSrc :1; /**< data source 0- filter & 1 pre-filter*/ - uint8_t sigEn :1; /**< 1 - enable sig, 0 - disable sig & enable anymotion */ - uint8_t sigMotThres; /**< sig-motion threshold */ -#elif BIG_ENDIAN == 1 - uint8_t sigMotThres; /**< sig-motion threshold */ - uint8_t sigEn :1; /**< 1 - enable sig, 0 - disable sig & enable anymotion */ - uint8_t sigDataSrc :1; /**< data source 0- filter & 1 pre-filter*/ - uint8_t sigMotProof : 2;/**< proof time of sig-motion interrupt */ - uint8_t sigMotSkip : 2; /**< skip time of sig-motion interrupt */ -#endif -}sBmx160AccSigMotIntCfg_t; - -/** - * @struct sBmx160AccStepDetectIntCfg_t - * @brief Step detector interrupt structure - */ -typedef struct { -#if LITTLE_ENDIAN == 1 - uint16_t stepDetectorEn :1; /**< 1- step detector enable, 0- step detector disable */ - uint16_t minThreshold :2; /**< minimum threshold */ - uint16_t steptimeMin :3; /**< minimal detectable step time */ - uint16_t stepDetectorMode :2; /**< enable step counter mode setting */ - uint16_t stepMinBuf :3; /**< minimum step buffer size*/ -#elif BIG_ENDIAN == 1 - uint16_t stepMinBuf :3; /**< minimum step buffer size*/ - uint16_t stepDetectorMode : 2; /**< enable step counter mode setting */ - uint16_t steptimeMin : 3; /**< minimal detectable step time */ - uint16_t minThreshold : 2; /**< minimum threshold */ - uint16_t stepDetectorEn :1; /**< 1- step detector enable, 0- step detector disable */ -#endif -}sBmx160AccStepDetectIntCfg_t; - -/** - * @struct sBmx160AccNoMotionIntCfg_t - * @brief No motion interrupt structure - */ -typedef struct { -#if LITTLE_ENDIAN == 1 - uint16_t noMotionX :1; /**< no motion interrupt x */ - uint16_t noMotionY :1; /**< no motion interrupt y */ - uint16_t noMotionZ :1; /**< no motion interrupt z */ - uint16_t noMotionDur :6; /**< no motion duration */ - uint16_t noMotionSel :1; /**< no motion sel , 1 - enable no-motion ,0- enable slow-motion */ - uint16_t noMotionSrc :1; /**< data source 0- filter & 1 pre-filter*/ - uint8_t noMotionThres; /**< no motion threshold */ -#elif BIG_ENDIAN == 1 - uint8_t noMotionThres; /**< no motion threshold */ - uint16_t noMotionSrc :1; /**< data source 0- filter & 1 pre-filter*/ - uint16_t noMotionSel : 1; /**< no motion sel , 1 - enable no-motion ,0- enable slow-motion */ - uint16_t noMotionDur : 6; /**< no motion duration */ - uint16_t noMotionZ :1; /**, commit + `252ac2859ac1d2010915b7a7cfcbf501d7adfb40` +- `bmm150.c`, `bmm150.h`, `bmm150_defs.h`: + , commit + `0dce0617873cda1f6d51f6b7b961fdc2641e0c7c` + +Both APIs are distributed under the BSD 3-Clause license. The corresponding +license texts are stored in `BMI160_LICENSE` and `BMM150_LICENSE`. + +Project-specific integration, FIFO configuration, bus locking and conversion +live in the adjacent `BMX160.cpp` wrapper. diff --git a/src/SensorManager/BMX160/bosch/bmi160.c b/src/SensorManager/BMX160/bosch/bmi160.c new file mode 100644 index 00000000..f7033f6a --- /dev/null +++ b/src/SensorManager/BMX160/bosch/bmi160.c @@ -0,0 +1,6436 @@ +/** +* Copyright (c) 2021 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi160.c +* @date 2021-10-05 +* @version v3.9.2 +* +*/ + +#include "bmi160.h" + +/* Below look up table follows the enum bmi160_int_types. + * Hence any change should match to the enum bmi160_int_types + */ +const uint8_t int_mask_lookup_table[13] = { + BMI160_INT1_SLOPE_MASK, BMI160_INT1_SLOPE_MASK, BMI160_INT2_LOW_STEP_DETECT_MASK, BMI160_INT1_DOUBLE_TAP_MASK, + BMI160_INT1_SINGLE_TAP_MASK, BMI160_INT1_ORIENT_MASK, BMI160_INT1_FLAT_MASK, BMI160_INT1_HIGH_G_MASK, + BMI160_INT1_LOW_G_MASK, BMI160_INT1_NO_MOTION_MASK, BMI160_INT2_DATA_READY_MASK, BMI160_INT2_FIFO_FULL_MASK, + BMI160_INT2_FIFO_WM_MASK +}; + +/*********************************************************************/ +/* Static function declarations */ + +/*! + * @brief This API configures the pins to fire the + * interrupt signal when it occurs + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_intr_pin_config(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the any-motion interrupt of the sensor. + * This interrupt occurs when accel values exceeds preset threshold + * for a certain period of time. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_any_motion_int(struct bmi160_int_settg *int_config, struct bmi160_dev *dev); + +/*! + * @brief This API sets tap interrupts.Interrupt is fired when + * tap movements happen. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_tap_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the data ready interrupt for both accel and gyro. + * This interrupt occurs when new accel and gyro data come. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_gyro_data_ready_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the significant motion interrupt of the sensor.This + * interrupt occurs when there is change in user location. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_sig_motion_int(struct bmi160_int_settg *int_config, struct bmi160_dev *dev); + +/*! + * @brief This API sets the no motion/slow motion interrupt of the sensor. + * Slow motion is similar to any motion interrupt.No motion interrupt + * occurs when slope bet. two accel values falls below preset threshold + * for preset duration. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_no_motion_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the step detection interrupt.This interrupt + * occurs when the single step causes accel values to go above + * preset threshold. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_step_detect_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the orientation interrupt of the sensor.This + * interrupt occurs when there is orientation change in the sensor + * with respect to gravitational field vector g. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_orientation_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the flat interrupt of the sensor.This interrupt + * occurs in case of flat orientation + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_flat_detect_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the low-g interrupt of the sensor.This interrupt + * occurs during free-fall. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_low_g_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the high-g interrupt of the sensor.The interrupt + * occurs if the absolute value of acceleration data of any enabled axis + * exceeds the programmed threshold and the sign of the value does not + * change for a preset duration. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_high_g_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the default configuration parameters of accel & gyro. + * Also maintain the previous state of configurations. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static void default_param_settg(struct bmi160_dev *dev); + +/*! + * @brief This API is used to validate the device structure pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t null_ptr_check(const struct bmi160_dev *dev); + +/*! + * @brief This API set the accel configuration. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_conf(struct bmi160_dev *dev); + +/*! + * @brief This API gets the accel configuration. + * + * @param[out] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t get_accel_conf(struct bmi160_dev *dev); + +/*! + * @brief This API check the accel configuration. + * + * @param[in] data : Pointer to store the updated accel config. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t check_accel_config(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API process the accel odr. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_accel_odr(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API process the accel bandwidth. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_accel_bw(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API process the accel range. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_accel_range(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API checks the invalid settings for ODR & Bw for Accel and Gyro. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t check_invalid_settg(const struct bmi160_dev *dev); + +/*! + * @brief This API set the gyro configuration. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_gyro_conf(struct bmi160_dev *dev); + +/*! + * @brief This API get the gyro configuration. + * + * @param[out] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t get_gyro_conf(struct bmi160_dev *dev); + +/*! + * @brief This API check the gyro configuration. + * + * @param[in] data : Pointer to store the updated gyro config. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t check_gyro_config(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API process the gyro odr. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_gyro_odr(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API process the gyro bandwidth. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_gyro_bw(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API process the gyro range. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_gyro_range(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the accel power mode. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_pwr(struct bmi160_dev *dev); + +/*! + * @brief This API process the undersampling setting of Accel. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_under_sampling(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the gyro power mode. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_gyro_pwr(struct bmi160_dev *dev); + +/*! + * @brief This API reads accel data along with sensor time if time is requested + * by user. Kindly refer the user guide(README.md) for more info. + * + * @param[in] len : len to read no of bytes + * @param[out] accel : Structure pointer to store accel data + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t get_accel_data(uint8_t len, struct bmi160_sensor_data *accel, const struct bmi160_dev *dev); + +/*! + * @brief This API reads accel data along with sensor time if time is requested + * by user. Kindly refer the user guide(README.md) for more info. + * + * @param[in] len : len to read no of bytes + * @param[out] gyro : Structure pointer to store accel data + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t get_gyro_data(uint8_t len, struct bmi160_sensor_data *gyro, const struct bmi160_dev *dev); + +/*! + * @brief This API reads accel and gyro data along with sensor time + * if time is requested by user. + * Kindly refer the user guide(README.md) for more info. + * + * @param[in] len : len to read no of bytes + * @param[out] accel : Structure pointer to store accel data + * @param[out] gyro : Structure pointer to store accel data + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t get_accel_gyro_data(uint8_t len, + struct bmi160_sensor_data *accel, + struct bmi160_sensor_data *gyro, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the any-motion interrupt for accel. + * + * @param[in] any_motion_int_cfg : Structure instance of + * bmi160_acc_any_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_accel_any_motion_int(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + struct bmi160_dev *dev); + +/*! + * @brief This API disable the sig-motion interrupt. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t disable_sig_motion_int(const struct bmi160_dev *dev); + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for any-motion interrupt. + * + * @param[in] any_motion_int_cfg : Structure instance of + * bmi160_acc_any_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_any_motion_src(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the duration and threshold of + * any-motion interrupt. + * + * @param[in] any_motion_int_cfg : Structure instance of + * bmi160_acc_any_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_any_dur_threshold(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure necessary setting of any-motion interrupt. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] any_motion_int_cfg : Structure instance of + * bmi160_acc_any_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_any_motion_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API enable the data ready interrupt. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_data_ready_int(const struct bmi160_dev *dev); + +/*! + * @brief This API enables the no motion/slow motion interrupt. + * + * @param[in] no_mot_int_cfg : Structure instance of + * bmi160_acc_no_motion_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_no_motion_int(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the interrupt PIN setting for + * no motion/slow motion interrupt. + * + * @param[in] int_config : structure instance of bmi160_int_settg. + * @param[in] no_mot_int_cfg : Structure instance of + * bmi160_acc_no_motion_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_no_motion_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the source of interrupt for no motion. + * + * @param[in] no_mot_int_cfg : Structure instance of + * bmi160_acc_no_motion_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_no_motion_data_src(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the duration and threshold of + * no motion/slow motion interrupt along with selection of no/slow motion. + * + * @param[in] no_mot_int_cfg : Structure instance of + * bmi160_acc_no_motion_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_no_motion_dur_thr(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the sig-motion motion interrupt. + * + * @param[in] sig_mot_int_cfg : Structure instance of + * bmi160_acc_sig_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_sig_motion_int(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, struct bmi160_dev *dev); + +/*! + * @brief This API configure the interrupt PIN setting for + * significant motion interrupt. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] sig_mot_int_cfg : Structure instance of + * bmi160_acc_sig_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_sig_motion_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for sig motion interrupt. + * + * @param[in] sig_mot_int_cfg : Structure instance of + * bmi160_acc_sig_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_sig_motion_data_src(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the threshold, skip and proof time of + * sig motion interrupt. + * + * @param[in] sig_mot_int_cfg : Structure instance of + * bmi160_acc_sig_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_sig_dur_threshold(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the step detector interrupt. + * + * @param[in] step_detect_int_cfg : Structure instance of + * bmi160_acc_step_detect_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_step_detect_int(const struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the step detector parameter. + * + * @param[in] step_detect_int_cfg : Structure instance of + * bmi160_acc_step_detect_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_step_detect(const struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the single/double tap interrupt. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_tap_int(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_tap_int_cfg *tap_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the interrupt PIN setting for + * tap interrupt. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] tap_int_cfg : Structure instance of bmi160_acc_tap_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_tap_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_tap_int_cfg *tap_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for tap interrupt. + * + * @param[in] tap_int_cfg : Structure instance of bmi160_acc_tap_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_tap_data_src(const struct bmi160_acc_tap_int_cfg *tap_int_cfg, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the parameters of tap interrupt. + * Threshold, quite, shock, and duration. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] tap_int_cfg : Structure instance of bmi160_acc_tap_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_tap_param(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_tap_int_cfg *tap_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API enable the external mode configuration. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_sec_if(const struct bmi160_dev *dev); + +/*! + * @brief This API configure the ODR of the auxiliary sensor. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_aux_odr(const struct bmi160_dev *dev); + +/*! + * @brief This API maps the actual burst read length set by user. + * + * @param[in] len : Pointer to store the read length. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t map_read_len(uint16_t *len, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the settings of auxiliary sensor. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_aux_settg(const struct bmi160_dev *dev); + +/*! + * @brief This API extract the read data from auxiliary sensor. + * + * @param[in] map_len : burst read value. + * @param[in] reg_addr : Address of register to read. + * @param[in] aux_data : Pointer to store the read data. + * @param[in] len : length to read the data. + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t extract_aux_read(uint16_t map_len, + uint8_t reg_addr, + uint8_t *aux_data, + uint16_t len, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the orient interrupt. + * + * @param[in] orient_int_cfg : Structure instance of bmi160_acc_orient_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_orient_int(const struct bmi160_acc_orient_int_cfg *orient_int_cfg, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the necessary setting of orientation interrupt. + * + * @param[in] orient_int_cfg : Structure instance of bmi160_acc_orient_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_orient_int_settg(const struct bmi160_acc_orient_int_cfg *orient_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the flat interrupt. + * + * @param[in] flat_int : Structure instance of bmi160_acc_flat_detect_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_flat_int(const struct bmi160_acc_flat_detect_int_cfg *flat_int, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the necessary setting of flat interrupt. + * + * @param[in] flat_int : Structure instance of bmi160_acc_flat_detect_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_flat_int_settg(const struct bmi160_acc_flat_detect_int_cfg *flat_int, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the Low-g interrupt. + * + * @param[in] low_g_int : Structure instance of bmi160_acc_low_g_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_low_g_int(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the source of data(filter & pre-filter) for low-g interrupt. + * + * @param[in] low_g_int : Structure instance of bmi160_acc_low_g_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_low_g_data_src(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the necessary setting of low-g interrupt. + * + * @param[in] low_g_int : Structure instance of bmi160_acc_low_g_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_low_g_int_settg(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev); + +/*! + * @brief This API enables the high-g interrupt. + * + * @param[in] high_g_int_cfg : Structure instance of bmi160_acc_high_g_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_high_g_int(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for high-g interrupt. + * + * @param[in] high_g_int_cfg : Structure instance of bmi160_acc_high_g_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_high_g_data_src(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the necessary setting of high-g interrupt. + * + * @param[in] high_g_int_cfg : Structure instance of bmi160_acc_high_g_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_high_g_int_settg(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the behavioural setting of interrupt pin. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_int_out_ctrl(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the mode(input enable, latch or non-latch) of interrupt pin. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_int_latch(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API performs the self test for accelerometer of BMI160 + * + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t perform_accel_self_test(struct bmi160_dev *dev); + +/*! + * @brief This API enables to perform the accel self test by setting proper + * configurations to facilitate accel self test + * + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_accel_self_test(struct bmi160_dev *dev); + +/*! + * @brief This API performs accel self test with positive excitation + * + * @param[in] accel_pos : Structure pointer to store accel data + * for positive excitation + * @param[in] dev : structure instance of bmi160_dev + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t accel_self_test_positive_excitation(struct bmi160_sensor_data *accel_pos, const struct bmi160_dev *dev); + +/*! + * @brief This API performs accel self test with negative excitation + * + * @param[in] accel_neg : Structure pointer to store accel data + * for negative excitation + * @param[in] dev : structure instance of bmi160_dev + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t accel_self_test_negative_excitation(struct bmi160_sensor_data *accel_neg, const struct bmi160_dev *dev); + +/*! + * @brief This API validates the accel self test results + * + * @param[in] accel_pos : Structure pointer to store accel data + * for positive excitation + * @param[in] accel_neg : Structure pointer to store accel data + * for negative excitation + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error / +ve value -> Self test fail + */ +static int8_t validate_accel_self_test(const struct bmi160_sensor_data *accel_pos, + const struct bmi160_sensor_data *accel_neg); + +/*! + * @brief This API performs the self test for gyroscope of BMI160 + * + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t perform_gyro_self_test(const struct bmi160_dev *dev); + +/*! + * @brief This API enables the self test bit to trigger self test for gyro + * + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_gyro_self_test(const struct bmi160_dev *dev); + +/*! + * @brief This API validates the self test results of gyro + * + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t validate_gyro_self_test(const struct bmi160_dev *dev); + +/*! + * @brief This API sets FIFO full interrupt of the sensor.This interrupt + * occurs when the FIFO is full and the next full data sample would cause + * a FIFO overflow, which may delete the old samples. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t set_fifo_full_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This enable the FIFO full interrupt engine. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_fifo_full_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets FIFO watermark interrupt of the sensor.The FIFO + * watermark interrupt is fired, when the FIFO fill level is above a fifo + * watermark. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t set_fifo_watermark_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This enable the FIFO watermark interrupt engine. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_fifo_wtm_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to reset the FIFO related configurations + * in the fifo_frame structure. + * + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void reset_fifo_data_structure(const struct bmi160_dev *dev); + +/*! + * @brief This API is used to read number of bytes filled + * currently in FIFO buffer. + * + * @param[in] bytes_to_read : Number of bytes available in FIFO at the + * instant which is obtained from FIFO counter. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + * @retval Any non zero value -> Fail + * + */ +static int8_t get_fifo_byte_counter(uint16_t *bytes_to_read, struct bmi160_dev const *dev); + +/*! + * @brief This API is used to compute the number of bytes of accel FIFO data + * which is to be parsed in header-less mode + * + * @param[out] data_index : The start index for parsing data + * @param[out] data_read_length : Number of bytes to be parsed + * @param[in] acc_frame_count : Number of accelerometer frames to be read + * @param[in] dev : Structure instance of bmi160_dev. + * + */ +static void get_accel_len_to_parse(uint16_t *data_index, + uint16_t *data_read_length, + const uint8_t *acc_frame_count, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data in both header mode and header-less mode. + * It updates the idx value which is used to store the index of + * the current data byte which is parsed. + * + * @param[in,out] acc : structure instance of sensor data + * @param[in,out] idx : Index value of number of bytes parsed + * @param[in,out] acc_idx : Index value of accelerometer data + * (x,y,z axes) frames parsed + * @param[in] frame_info : It consists of either fifo_data_enable + * parameter in header-less mode or + * frame header data in header mode + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_accel_frame(struct bmi160_sensor_data *acc, + uint16_t *idx, + uint8_t *acc_idx, + uint8_t frame_info, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data and store it in the instance of the structure bmi160_sensor_data. + * + * @param[in,out] accel_data : structure instance of sensor data + * @param[in,out] data_start_index : Index value of number of bytes parsed + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_accel_data(struct bmi160_sensor_data *accel_data, + uint16_t data_start_index, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data in header mode. + * + * @param[in,out] accel_data : Structure instance of sensor data + * @param[in,out] accel_length : Number of accelerometer frames + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void extract_accel_header_mode(struct bmi160_sensor_data *accel_data, + uint8_t *accel_length, + const struct bmi160_dev *dev); + +/*! + * @brief This API computes the number of bytes of gyro FIFO data + * which is to be parsed in header-less mode + * + * @param[out] data_index : The start index for parsing data + * @param[out] data_read_length : No of bytes to be parsed from FIFO buffer + * @param[in] gyro_frame_count : Number of Gyro data frames to be read + * @param[in] dev : Structure instance of bmi160_dev. + */ +static void get_gyro_len_to_parse(uint16_t *data_index, + uint16_t *data_read_length, + const uint8_t *gyro_frame_count, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the gyroscope's data from the + * FIFO data in both header mode and header-less mode. + * It updates the idx value which is used to store the index of + * the current data byte which is parsed. + * + * @param[in,out] gyro : structure instance of sensor data + * @param[in,out] idx : Index value of number of bytes parsed + * @param[in,out] gyro_idx : Index value of gyro data + * (x,y,z axes) frames parsed + * @param[in] frame_info : It consists of either fifo_data_enable + * parameter in header-less mode or + * frame header data in header mode + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_gyro_frame(struct bmi160_sensor_data *gyro, + uint16_t *idx, + uint8_t *gyro_idx, + uint8_t frame_info, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the gyro data from the + * FIFO data and store it in the instance of the structure bmi160_sensor_data. + * + * @param[in,out] gyro_data : structure instance of sensor data + * @param[in,out] data_start_index : Index value of number of bytes parsed + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_gyro_data(struct bmi160_sensor_data *gyro_data, + uint16_t data_start_index, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the gyro data from the + * FIFO data in header mode. + * + * @param[in,out] gyro_data : Structure instance of sensor data + * @param[in,out] gyro_length : Number of gyro frames + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void extract_gyro_header_mode(struct bmi160_sensor_data *gyro_data, + uint8_t *gyro_length, + const struct bmi160_dev *dev); + +/*! + * @brief This API computes the number of bytes of aux FIFO data + * which is to be parsed in header-less mode + * + * @param[out] data_index : The start index for parsing data + * @param[out] data_read_length : No of bytes to be parsed from FIFO buffer + * @param[in] aux_frame_count : Number of Aux data frames to be read + * @param[in] dev : Structure instance of bmi160_dev. + */ +static void get_aux_len_to_parse(uint16_t *data_index, + uint16_t *data_read_length, + const uint8_t *aux_frame_count, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the aux's data from the + * FIFO data in both header mode and header-less mode. + * It updates the idx value which is used to store the index of + * the current data byte which is parsed + * + * @param[in,out] aux_data : structure instance of sensor data + * @param[in,out] idx : Index value of number of bytes parsed + * @param[in,out] aux_index : Index value of gyro data + * (x,y,z axes) frames parsed + * @param[in] frame_info : It consists of either fifo_data_enable + * parameter in header-less mode or + * frame header data in header mode + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_aux_frame(struct bmi160_aux_data *aux_data, + uint16_t *idx, + uint8_t *aux_index, + uint8_t frame_info, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the aux data from the + * FIFO data and store it in the instance of the structure bmi160_aux_data. + * + * @param[in,out] aux_data : structure instance of sensor data + * @param[in,out] data_start_index : Index value of number of bytes parsed + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_aux_data(struct bmi160_aux_data *aux_data, uint16_t data_start_index, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the aux data from the + * FIFO data in header mode. + * + * @param[in,out] aux_data : Structure instance of sensor data + * @param[in,out] aux_length : Number of aux frames + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void extract_aux_header_mode(struct bmi160_aux_data *aux_data, uint8_t *aux_length, + const struct bmi160_dev *dev); + +/*! + * @brief This API checks the presence of non-valid frames in the read fifo data. + * + * @param[in,out] data_index : The index of the current data to + * be parsed from fifo data + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void check_frame_validity(uint16_t *data_index, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to move the data index ahead of the + * current_frame_length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + * + * @param[in,out] data_index : Index of the FIFO data which + * is to be moved ahead of the + * current_frame_length + * @param[in] current_frame_length : Number of bytes in a particular frame + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse and store the sensor time from the + * FIFO data in the structure instance dev. + * + * @param[in,out] data_index : Index of the FIFO data which + * has the sensor time. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_sensortime_frame(uint16_t *data_index, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse and store the skipped_frame_count from + * the FIFO data in the structure instance dev. + * + * @param[in,out] data_index : Index of the FIFO data which + * has the skipped frame count. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_skipped_frame(uint16_t *data_index, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to get the FOC status from the sensor + * + * @param[in,out] foc_status : Result of FOC status. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t get_foc_status(uint8_t *foc_status, struct bmi160_dev const *dev); + +/*! + * @brief This API is used to configure the offset enable bits in the sensor + * + * @param[in,out] foc_conf : Structure instance of bmi160_foc_conf which + * has the FOC and offset configurations + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t configure_offset_enable(const struct bmi160_foc_conf *foc_conf, struct bmi160_dev const *dev); + +/*! + * @brief This API is used to trigger the FOC in the sensor + * + * @param[in,out] offset : Structure instance of bmi160_offsets which + * reads and stores the offset values after FOC + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t trigger_foc(struct bmi160_offsets *offset, struct bmi160_dev const *dev); + +/*! + * @brief This API is used to map/unmap the Dataready(Accel & Gyro), FIFO full + * and FIFO watermark interrupt + * + * @param[in] int_config : Structure instance of bmi160_int_settg which + * stores the interrupt type and interrupt channel + * configurations to map/unmap the interrupt pins + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t map_hardware_interrupt(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to map/unmap the Any/Sig motion, Step det/Low-g, + * Double tap, Single tap, Orientation, Flat, High-G, Nomotion interrupt pins. + * + * @param[in] int_config : Structure instance of bmi160_int_settg which + * stores the interrupt type and interrupt channel + * configurations to map/unmap the interrupt pins + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t map_feature_interrupt(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*********************** User function definitions ****************************/ + +/*! + * @brief This API reads the data from the given register address + * of sensor. + */ +int8_t bmi160_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->read == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else if (len == 0) + { + rslt = BMI160_E_READ_WRITE_LENGTH_INVALID; + } + else + { + /* Configuring reg_addr for SPI Interface */ + if (dev->intf == BMI160_SPI_INTF) + { + reg_addr = (reg_addr | BMI160_SPI_RD_MASK); + } + + rslt = dev->read(dev->id, reg_addr, data, len); + } + + return rslt; +} + +/*! + * @brief This API writes the given data to the register address + * of sensor. + */ +int8_t bmi160_set_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + uint8_t count = 0; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->write == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else if (len == 0) + { + rslt = BMI160_E_READ_WRITE_LENGTH_INVALID; + } + else + { + /* Configuring reg_addr for SPI Interface */ + if (dev->intf == BMI160_SPI_INTF) + { + reg_addr = (reg_addr & BMI160_SPI_WR_MASK); + } + + if ((dev->prev_accel_cfg.power == BMI160_ACCEL_NORMAL_MODE) || + (dev->prev_gyro_cfg.power == BMI160_GYRO_NORMAL_MODE)) + { + rslt = dev->write(dev->id, reg_addr, data, len); + + /* Kindly refer bmi160 data sheet section 3.2.4 */ + dev->delay_ms(1); + + } + else + { + /*Burst write is not allowed in + * suspend & low power mode */ + for (; count < len; count++) + { + rslt = dev->write(dev->id, reg_addr, &data[count], 1); + reg_addr++; + + /* Kindly refer bmi160 data sheet section 3.2.4 */ + dev->delay_ms(1); + + } + } + + if (rslt != BMI160_OK) + { + rslt = BMI160_E_COM_FAIL; + } + } + + return rslt; +} + +/*! + * @brief This API is the entry point for sensor.It performs + * the selection of I2C/SPI read mechanism according to the + * selected interface and reads the chip-id of bmi160 sensor. + */ +int8_t bmi160_init(struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data; + uint8_t try = 3; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + + /* Dummy read of 0x7F register to enable SPI Interface + * if SPI is used */ + if ((rslt == BMI160_OK) && (dev->intf == BMI160_SPI_INTF)) + { + rslt = bmi160_get_regs(BMI160_SPI_COMM_TEST_ADDR, &data, 1, dev); + } + + if (rslt == BMI160_OK) + { + /* Assign chip id as zero */ + dev->chip_id = 0; + + while ((try--) && (dev->chip_id != BMI160_CHIP_ID)) + { + /* Read chip_id */ + rslt = bmi160_get_regs(BMI160_CHIP_ID_ADDR, &dev->chip_id, 1, dev); + } + + if ((rslt == BMI160_OK) && (dev->chip_id == BMI160_CHIP_ID)) + { + dev->any_sig_sel = BMI160_BOTH_ANY_SIG_MOTION_DISABLED; + + /* Soft reset */ + rslt = bmi160_soft_reset(dev); + } + else + { + rslt = BMI160_E_DEV_NOT_FOUND; + } + } + + return rslt; +} + +/*! + * @brief This API resets and restarts the device. + * All register values are overwritten with default parameters. + */ +int8_t bmi160_soft_reset(struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = BMI160_SOFT_RESET_CMD; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Reset the device */ + rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &data, 1, dev); + dev->delay_ms(BMI160_SOFT_RESET_DELAY_MS); + if ((rslt == BMI160_OK) && (dev->intf == BMI160_SPI_INTF)) + { + /* Dummy read of 0x7F register to enable SPI Interface + * if SPI is used */ + rslt = bmi160_get_regs(BMI160_SPI_COMM_TEST_ADDR, &data, 1, dev); + } + + if (rslt == BMI160_OK) + { + /* Update the default parameters */ + default_param_settg(dev); + } + } + + return rslt; +} + +/*! + * @brief This API configures the power mode, range and bandwidth + * of sensor. + */ +int8_t bmi160_set_sens_conf(struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = set_accel_conf(dev); + if (rslt == BMI160_OK) + { + rslt = set_gyro_conf(dev); + if (rslt == BMI160_OK) + { + /* write power mode for accel and gyro */ + rslt = bmi160_set_power_mode(dev); + if (rslt == BMI160_OK) + { + rslt = check_invalid_settg(dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API gets accel and gyro configurations. + */ +int8_t bmi160_get_sens_conf(struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = get_accel_conf(dev); + if (rslt == BMI160_OK) + { + rslt = get_gyro_conf(dev); + } + } + + return rslt; +} + +/*! + * @brief This API sets the power mode of the sensor. + */ +int8_t bmi160_set_power_mode(struct bmi160_dev *dev) +{ + int8_t rslt = 0; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = set_accel_pwr(dev); + if (rslt == BMI160_OK) + { + rslt = set_gyro_pwr(dev); + } + } + + return rslt; +} + +/*! + * @brief This API gets the power mode of the sensor. + */ +int8_t bmi160_get_power_mode(struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t power_mode = 0; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_get_regs(BMI160_PMU_STATUS_ADDR, &power_mode, 1, dev); + if (rslt == BMI160_OK) + { + /* Power mode of the accel, gyro sensor is obtained */ + dev->gyro_cfg.power = BMI160_GET_BITS(power_mode, BMI160_GYRO_POWER_MODE); + dev->accel_cfg.power = BMI160_GET_BITS(power_mode, BMI160_ACCEL_POWER_MODE); + } + } + + return rslt; +} + +/*! + * @brief This API reads sensor data, stores it in + * the bmi160_sensor_data structure pointer passed by the user. + */ +int8_t bmi160_get_sensor_data(uint8_t select_sensor, + struct bmi160_sensor_data *accel, + struct bmi160_sensor_data *gyro, + const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + uint8_t time_sel; + uint8_t sen_sel; + uint8_t len = 0; + + /*Extract the sensor and time select information*/ + sen_sel = select_sensor & BMI160_SEN_SEL_MASK; + time_sel = ((sen_sel & BMI160_TIME_SEL) >> 2); + sen_sel = sen_sel & (BMI160_ACCEL_SEL | BMI160_GYRO_SEL); + if (time_sel == 1) + { + len = 3; + } + + /* Null-pointer check */ + if (dev != NULL) + { + switch (sen_sel) + { + case BMI160_ACCEL_ONLY: + + /* Null-pointer check */ + if (accel == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = get_accel_data(len, accel, dev); + } + + break; + case BMI160_GYRO_ONLY: + + /* Null-pointer check */ + if (gyro == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = get_gyro_data(len, gyro, dev); + } + + break; + case BMI160_BOTH_ACCEL_AND_GYRO: + + /* Null-pointer check */ + if ((gyro == NULL) || (accel == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = get_accel_gyro_data(len, accel, gyro, dev); + } + + break; + default: + rslt = BMI160_E_INVALID_INPUT; + break; + } + } + else + { + rslt = BMI160_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API configures the necessary interrupt based on + * the user settings in the bmi160_int_settg structure instance. + */ +int8_t bmi160_set_int_config(struct bmi160_int_settg *int_config, struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + switch (int_config->int_type) + { + case BMI160_ACC_ANY_MOTION_INT: + + /*Any-motion interrupt*/ + rslt = set_accel_any_motion_int(int_config, dev); + break; + case BMI160_ACC_SIG_MOTION_INT: + + /* Significant motion interrupt */ + rslt = set_accel_sig_motion_int(int_config, dev); + break; + case BMI160_ACC_SLOW_NO_MOTION_INT: + + /* Slow or no motion interrupt */ + rslt = set_accel_no_motion_int(int_config, dev); + break; + case BMI160_ACC_DOUBLE_TAP_INT: + case BMI160_ACC_SINGLE_TAP_INT: + + /* Double tap and single tap Interrupt */ + rslt = set_accel_tap_int(int_config, dev); + break; + case BMI160_STEP_DETECT_INT: + + /* Step detector interrupt */ + rslt = set_accel_step_detect_int(int_config, dev); + break; + case BMI160_ACC_ORIENT_INT: + + /* Orientation interrupt */ + rslt = set_accel_orientation_int(int_config, dev); + break; + case BMI160_ACC_FLAT_INT: + + /* Flat detection interrupt */ + rslt = set_accel_flat_detect_int(int_config, dev); + break; + case BMI160_ACC_LOW_G_INT: + + /* Low-g interrupt */ + rslt = set_accel_low_g_int(int_config, dev); + break; + case BMI160_ACC_HIGH_G_INT: + + /* High-g interrupt */ + rslt = set_accel_high_g_int(int_config, dev); + break; + case BMI160_ACC_GYRO_DATA_RDY_INT: + + /* Data ready interrupt */ + rslt = set_accel_gyro_data_ready_int(int_config, dev); + break; + case BMI160_ACC_GYRO_FIFO_FULL_INT: + + /* Fifo full interrupt */ + rslt = set_fifo_full_int(int_config, dev); + break; + case BMI160_ACC_GYRO_FIFO_WATERMARK_INT: + + /* Fifo water-mark interrupt */ + rslt = set_fifo_watermark_int(int_config, dev); + break; + case BMI160_FIFO_TAG_INT_PIN: + + /* Fifo tagging feature support */ + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + break; + default: + break; + } + + return rslt; +} + +/*! + * @brief This API enables or disable the step counter feature. + * 1 - enable step counter (0 - disable) + */ +int8_t bmi160_set_step_counter(uint8_t step_cnt_enable, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_get_regs(BMI160_INT_STEP_CONFIG_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + if (step_cnt_enable == BMI160_ENABLE) + { + data |= (uint8_t)(step_cnt_enable << 3); + } + else + { + data &= ~BMI160_STEP_COUNT_EN_BIT_MASK; + } + + rslt = bmi160_set_regs(BMI160_INT_STEP_CONFIG_1_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API reads the step counter value. + */ +int8_t bmi160_read_step_counter(uint16_t *step_val, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data[2] = { 0, 0 }; + uint16_t msb = 0; + uint8_t lsb = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_get_regs(BMI160_INT_STEP_CNT_0_ADDR, data, 2, dev); + if (rslt == BMI160_OK) + { + lsb = data[0]; + msb = data[1] << 8; + *step_val = msb | lsb; + } + } + + return rslt; +} + +/*! + * @brief This API reads the mention no of byte of data from the given + * register address of auxiliary sensor. + */ +int8_t bmi160_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + uint16_t map_len = 0; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->read == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + if (dev->aux_cfg.aux_sensor_enable == BMI160_ENABLE) + { + rslt = map_read_len(&map_len, dev); + if (rslt == BMI160_OK) + { + rslt = extract_aux_read(map_len, reg_addr, aux_data, len, dev); + } + } + else + { + rslt = BMI160_E_INVALID_INPUT; + } + } + + return rslt; +} + +/*! + * @brief This API writes the mention no of byte of data to the given + * register address of auxiliary sensor. + */ +int8_t bmi160_aux_write(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + uint8_t count = 0; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->write == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + for (; count < len; count++) + { + /* set data to write */ + rslt = bmi160_set_regs(BMI160_AUX_IF_4_ADDR, aux_data, 1, dev); + dev->delay_ms(BMI160_AUX_COM_DELAY); + if (rslt == BMI160_OK) + { + /* set address to write */ + rslt = bmi160_set_regs(BMI160_AUX_IF_3_ADDR, ®_addr, 1, dev); + dev->delay_ms(BMI160_AUX_COM_DELAY); + if (rslt == BMI160_OK && (count < len - 1)) + { + aux_data++; + reg_addr++; + } + } + } + } + + return rslt; +} + +/*! + * @brief This API initialize the auxiliary sensor + * in order to access it. + */ +int8_t bmi160_aux_init(const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + if (dev->aux_cfg.aux_sensor_enable == BMI160_ENABLE) + { + /* Configures the auxiliary sensor interface settings */ + rslt = config_aux_settg(dev); + } + else + { + rslt = BMI160_E_INVALID_INPUT; + } + } + + return rslt; +} + +/*! + * @brief This API is used to setup the auxiliary sensor of bmi160 in auto mode + * Thus enabling the auto update of 8 bytes of data from auxiliary sensor + * to BMI160 register address 0x04 to 0x0B + */ +int8_t bmi160_set_aux_auto_mode(uint8_t *data_addr, struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + if (dev->aux_cfg.aux_sensor_enable == BMI160_ENABLE) + { + /* Write the aux. address to read in 0x4D of BMI160*/ + rslt = bmi160_set_regs(BMI160_AUX_IF_2_ADDR, data_addr, 1, dev); + dev->delay_ms(BMI160_AUX_COM_DELAY); + if (rslt == BMI160_OK) + { + /* Configure the polling ODR for + * auxiliary sensor */ + rslt = config_aux_odr(dev); + if (rslt == BMI160_OK) + { + /* Disable the aux. manual mode, i.e aux. + * sensor is in auto-mode (data-mode) */ + dev->aux_cfg.manual_enable = BMI160_DISABLE; + rslt = bmi160_config_aux_mode(dev); + + /* Auxiliary sensor data is obtained + * in auto mode from this point */ + } + } + } + else + { + rslt = BMI160_E_INVALID_INPUT; + } + } + + return rslt; +} + +/*! + * @brief This API configures the 0x4C register and settings like + * Auxiliary sensor manual enable/ disable and aux burst read length. + */ +int8_t bmi160_config_aux_mode(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t aux_if[2] = { (uint8_t)(dev->aux_cfg.aux_i2c_addr * 2), 0 }; + + rslt = bmi160_get_regs(BMI160_AUX_IF_1_ADDR, &aux_if[1], 1, dev); + if (rslt == BMI160_OK) + { + /* update the Auxiliary interface to manual/auto mode */ + aux_if[1] = BMI160_SET_BITS(aux_if[1], BMI160_MANUAL_MODE_EN, dev->aux_cfg.manual_enable); + + /* update the burst read length defined by user */ + aux_if[1] = BMI160_SET_BITS_POS_0(aux_if[1], BMI160_AUX_READ_BURST, dev->aux_cfg.aux_rd_burst_len); + + /* Set the secondary interface address and manual mode + * along with burst read length */ + rslt = bmi160_set_regs(BMI160_AUX_IF_0_ADDR, &aux_if[0], 2, dev); + dev->delay_ms(BMI160_AUX_COM_DELAY); + } + + return rslt; +} + +/*! + * @brief This API is used to read the raw uncompensated auxiliary sensor + * data of 8 bytes from BMI160 register address 0x04 to 0x0B + */ +int8_t bmi160_read_aux_data_auto_mode(uint8_t *aux_data, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + if ((dev->aux_cfg.aux_sensor_enable == BMI160_ENABLE) && (dev->aux_cfg.manual_enable == BMI160_DISABLE)) + { + /* Read the aux. sensor's raw data */ + rslt = bmi160_get_regs(BMI160_AUX_DATA_ADDR, aux_data, 8, dev); + } + else + { + rslt = BMI160_E_INVALID_INPUT; + } + } + + return rslt; +} + +/*! + * @brief This is used to perform self test of accel/gyro of the BMI160 sensor + */ +int8_t bmi160_perform_self_test(uint8_t select_sensor, struct bmi160_dev *dev) +{ + int8_t rslt; + int8_t self_test_rslt = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + + /* Proceed if null check is fine */ + switch (select_sensor) + { + case BMI160_ACCEL_ONLY: + rslt = perform_accel_self_test(dev); + break; + case BMI160_GYRO_ONLY: + + /* Set the power mode as normal mode */ + dev->gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; + rslt = bmi160_set_power_mode(dev); + + /* Perform gyro self test */ + if (rslt == BMI160_OK) + { + /* Perform gyro self test */ + rslt = perform_gyro_self_test(dev); + } + + break; + default: + rslt = BMI160_E_INVALID_INPUT; + break; + } + + /* Check to ensure bus error does not occur */ + if (rslt >= BMI160_OK) + { + /* Store the status of self test result */ + self_test_rslt = rslt; + + /* Perform soft reset */ + rslt = bmi160_soft_reset(dev); + + } + + /* Check to ensure bus operations are success */ + if (rslt == BMI160_OK) + { + /* Restore self_test_rslt as return value */ + rslt = self_test_rslt; + } + } + + return rslt; +} + +/*! + * @brief This API reads the data from fifo buffer. + */ +int8_t bmi160_get_fifo_data(struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + uint16_t bytes_to_read = 0; + uint16_t user_fifo_len = 0; + + /* check the bmi160 structure as NULL*/ + if ((dev == NULL) || (dev->fifo->data == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + reset_fifo_data_structure(dev); + + /* get current FIFO fill-level*/ + rslt = get_fifo_byte_counter(&bytes_to_read, dev); + if (rslt == BMI160_OK) + { + user_fifo_len = dev->fifo->length; + if ((dev->fifo->length > bytes_to_read)) + { + /* Handling the case where user requests + * more data than available in FIFO */ + dev->fifo->length = bytes_to_read; + } + + if ((dev->fifo->fifo_time_enable == BMI160_FIFO_TIME_ENABLE) && + (bytes_to_read + BMI160_FIFO_BYTES_OVERREAD <= user_fifo_len)) + { + /* Handling case of sensor time availability*/ + dev->fifo->length = dev->fifo->length + BMI160_FIFO_BYTES_OVERREAD; + } + + /* read only the filled bytes in the FIFO Buffer */ + rslt = bmi160_get_regs(BMI160_FIFO_DATA_ADDR, dev->fifo->data, dev->fifo->length, dev); + } + } + + return rslt; +} + +/*! + * @brief This API writes fifo_flush command to command register.This + * action clears all data in the Fifo without changing fifo configuration + * settings + */ +int8_t bmi160_set_fifo_flush(const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t data = BMI160_FIFO_FLUSH_VALUE; + uint8_t reg_addr = BMI160_COMMAND_REG_ADDR; + + /* Check the bmi160_dev structure for NULL address*/ + if (dev == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_set_regs(reg_addr, &data, BMI160_ONE, dev); + } + + return rslt; +} + +/*! + * @brief This API sets the FIFO configuration in the sensor. + */ +int8_t bmi160_set_fifo_config(uint8_t config, uint8_t enable, struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + uint8_t data = 0; + uint8_t reg_addr = BMI160_FIFO_CONFIG_1_ADDR; + uint8_t fifo_config = config & BMI160_FIFO_CONFIG_1_MASK; + + /* Check the bmi160_dev structure for NULL address*/ + if (dev == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_get_regs(reg_addr, &data, BMI160_ONE, dev); + if (rslt == BMI160_OK) + { + if (fifo_config > 0) + { + if (enable == BMI160_ENABLE) + { + data = data | fifo_config; + } + else + { + data = data & (~fifo_config); + } + } + + /* write fifo frame content configuration*/ + rslt = bmi160_set_regs(reg_addr, &data, BMI160_ONE, dev); + if (rslt == BMI160_OK) + { + /* read fifo frame content configuration*/ + rslt = bmi160_get_regs(reg_addr, &data, BMI160_ONE, dev); + if (rslt == BMI160_OK) + { + /* extract fifo header enabled status */ + dev->fifo->fifo_header_enable = data & BMI160_FIFO_HEAD_ENABLE; + + /* extract accel/gyr/aux. data enabled status */ + dev->fifo->fifo_data_enable = data & BMI160_FIFO_M_G_A_ENABLE; + + /* extract fifo sensor time enabled status */ + dev->fifo->fifo_time_enable = data & BMI160_FIFO_TIME_ENABLE; + } + } + } + } + + return rslt; +} + +/*! @brief This API is used to configure the down sampling ratios of + * the accel and gyro data for FIFO.Also, it configures filtered or + * pre-filtered data for accel and gyro. + * + */ +int8_t bmi160_set_fifo_down(uint8_t fifo_down, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t data = 0; + uint8_t reg_addr = BMI160_FIFO_DOWN_ADDR; + + /* Check the bmi160_dev structure for NULL address*/ + if (dev == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_get_regs(reg_addr, &data, BMI160_ONE, dev); + if (rslt == BMI160_OK) + { + data = data | fifo_down; + rslt = bmi160_set_regs(reg_addr, &data, BMI160_ONE, dev); + } + } + + return rslt; +} + +/*! + * @brief This API sets the FIFO watermark level in the sensor. + * + */ +int8_t bmi160_set_fifo_wm(uint8_t fifo_wm, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t data = fifo_wm; + uint8_t reg_addr = BMI160_FIFO_CONFIG_0_ADDR; + + /* Check the bmi160_dev structure for NULL address*/ + if (dev == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_set_regs(reg_addr, &data, BMI160_ONE, dev); + } + + return rslt; +} + +/*! + * @brief This API parses and extracts the accelerometer frames from + * FIFO data read by the "bmi160_get_fifo_data" API and stores it in + * the "accel_data" structure instance. + */ +int8_t bmi160_extract_accel(struct bmi160_sensor_data *accel_data, uint8_t *accel_length, struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + uint16_t data_index = 0; + uint16_t data_read_length = 0; + uint8_t accel_index = 0; + uint8_t fifo_data_enable = 0; + + if (dev == NULL || dev->fifo == NULL || dev->fifo->data == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Parsing the FIFO data in header-less mode */ + if (dev->fifo->fifo_header_enable == 0) + { + /* Number of bytes to be parsed from FIFO */ + get_accel_len_to_parse(&data_index, &data_read_length, accel_length, dev); + for (; data_index < data_read_length;) + { + /*Check for the availability of next two bytes of FIFO data */ + check_frame_validity(&data_index, dev); + fifo_data_enable = dev->fifo->fifo_data_enable; + unpack_accel_frame(accel_data, &data_index, &accel_index, fifo_data_enable, dev); + } + + /* update number of accel data read*/ + *accel_length = accel_index; + + /*update the accel byte index*/ + dev->fifo->accel_byte_start_idx = data_index; + } + else + { + /* Parsing the FIFO data in header mode */ + extract_accel_header_mode(accel_data, accel_length, dev); + } + } + + return rslt; +} + +/*! + * @brief This API parses and extracts the gyro frames from + * FIFO data read by the "bmi160_get_fifo_data" API and stores it in + * the "gyro_data" structure instance. + */ +int8_t bmi160_extract_gyro(struct bmi160_sensor_data *gyro_data, uint8_t *gyro_length, struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + uint16_t data_index = 0; + uint16_t data_read_length = 0; + uint8_t gyro_index = 0; + uint8_t fifo_data_enable = 0; + + if (dev == NULL || dev->fifo->data == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Parsing the FIFO data in header-less mode */ + if (dev->fifo->fifo_header_enable == 0) + { + /* Number of bytes to be parsed from FIFO */ + get_gyro_len_to_parse(&data_index, &data_read_length, gyro_length, dev); + for (; data_index < data_read_length;) + { + /*Check for the availability of next two bytes of FIFO data */ + check_frame_validity(&data_index, dev); + fifo_data_enable = dev->fifo->fifo_data_enable; + unpack_gyro_frame(gyro_data, &data_index, &gyro_index, fifo_data_enable, dev); + } + + /* update number of gyro data read */ + *gyro_length = gyro_index; + + /* update the gyro byte index */ + dev->fifo->gyro_byte_start_idx = data_index; + } + else + { + /* Parsing the FIFO data in header mode */ + extract_gyro_header_mode(gyro_data, gyro_length, dev); + } + } + + return rslt; +} + +/*! + * @brief This API parses and extracts the aux frames from + * FIFO data read by the "bmi160_get_fifo_data" API and stores it in + * the "aux_data" structure instance. + */ +int8_t bmi160_extract_aux(struct bmi160_aux_data *aux_data, uint8_t *aux_len, struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + uint16_t data_index = 0; + uint16_t data_read_length = 0; + uint8_t aux_index = 0; + uint8_t fifo_data_enable = 0; + + if ((dev == NULL) || (dev->fifo->data == NULL) || (aux_data == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Parsing the FIFO data in header-less mode */ + if (dev->fifo->fifo_header_enable == 0) + { + /* Number of bytes to be parsed from FIFO */ + get_aux_len_to_parse(&data_index, &data_read_length, aux_len, dev); + for (; data_index < data_read_length;) + { + /* Check for the availability of next two + * bytes of FIFO data */ + check_frame_validity(&data_index, dev); + fifo_data_enable = dev->fifo->fifo_data_enable; + unpack_aux_frame(aux_data, &data_index, &aux_index, fifo_data_enable, dev); + } + + /* update number of aux data read */ + *aux_len = aux_index; + + /* update the aux byte index */ + dev->fifo->aux_byte_start_idx = data_index; + } + else + { + /* Parsing the FIFO data in header mode */ + extract_aux_header_mode(aux_data, aux_len, dev); + } + } + + return rslt; +} + +/*! + * @brief This API starts the FOC of accel and gyro + * + * @note FOC should not be used in low-power mode of sensor + * + * @note Accel FOC targets values of +1g , 0g , -1g + * Gyro FOC always targets value of 0 dps + */ +int8_t bmi160_start_foc(const struct bmi160_foc_conf *foc_conf, + struct bmi160_offsets *offset, + struct bmi160_dev const *dev) +{ + int8_t rslt; + uint8_t data; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Set the offset enable bits */ + rslt = configure_offset_enable(foc_conf, dev); + if (rslt == BMI160_OK) + { + /* Read the FOC config from the sensor */ + rslt = bmi160_get_regs(BMI160_FOC_CONF_ADDR, &data, 1, dev); + if (rslt != BMI160_OK) + { + return rslt; + } + + /* Set the FOC config for gyro */ + data = BMI160_SET_BITS(data, BMI160_GYRO_FOC_EN, foc_conf->foc_gyr_en); + + /* Set the FOC config for accel xyz axes */ + data = BMI160_SET_BITS(data, BMI160_ACCEL_FOC_X_CONF, foc_conf->foc_acc_x); + data = BMI160_SET_BITS(data, BMI160_ACCEL_FOC_Y_CONF, foc_conf->foc_acc_y); + data = BMI160_SET_BITS_POS_0(data, BMI160_ACCEL_FOC_Z_CONF, foc_conf->foc_acc_z); + if (rslt == BMI160_OK) + { + /* Set the FOC config in the sensor */ + rslt = bmi160_set_regs(BMI160_FOC_CONF_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* Procedure to trigger + * FOC and check status */ + rslt = trigger_foc(offset, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API reads and stores the offset values of accel and gyro + */ +int8_t bmi160_get_offsets(struct bmi160_offsets *offset, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data[7]; + uint8_t lsb, msb; + int16_t offset_msb, offset_lsb; + int16_t offset_data; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Read the FOC config from the sensor */ + rslt = bmi160_get_regs(BMI160_OFFSET_ADDR, data, 7, dev); + + /* Accel offsets */ + offset->off_acc_x = (int8_t)data[0]; + offset->off_acc_y = (int8_t)data[1]; + offset->off_acc_z = (int8_t)data[2]; + + /* Gyro x-axis offset */ + lsb = data[3]; + msb = BMI160_GET_BITS_POS_0(data[6], BMI160_GYRO_OFFSET_X); + offset_msb = (int16_t)(msb << 14); + offset_lsb = lsb << 6; + offset_data = offset_msb | offset_lsb; + + /* Divide by 64 to get the Right shift by 6 value */ + offset->off_gyro_x = (int16_t)(offset_data / 64); + + /* Gyro y-axis offset */ + lsb = data[4]; + msb = BMI160_GET_BITS(data[6], BMI160_GYRO_OFFSET_Y); + offset_msb = (int16_t)(msb << 14); + offset_lsb = lsb << 6; + offset_data = offset_msb | offset_lsb; + + /* Divide by 64 to get the Right shift by 6 value */ + offset->off_gyro_y = (int16_t)(offset_data / 64); + + /* Gyro z-axis offset */ + lsb = data[5]; + msb = BMI160_GET_BITS(data[6], BMI160_GYRO_OFFSET_Z); + offset_msb = (int16_t)(msb << 14); + offset_lsb = lsb << 6; + offset_data = offset_msb | offset_lsb; + + /* Divide by 64 to get the Right shift by 6 value */ + offset->off_gyro_z = (int16_t)(offset_data / 64); + } + + return rslt; +} + +/*! + * @brief This API writes the offset values of accel and gyro to + * the sensor but these values will be reset on POR or soft reset. + */ +int8_t bmi160_set_offsets(const struct bmi160_foc_conf *foc_conf, + const struct bmi160_offsets *offset, + struct bmi160_dev const *dev) +{ + int8_t rslt; + uint8_t data[7]; + uint8_t x_msb, y_msb, z_msb; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Update the accel offset */ + data[0] = (uint8_t)offset->off_acc_x; + data[1] = (uint8_t)offset->off_acc_y; + data[2] = (uint8_t)offset->off_acc_z; + + /* Update the LSB of gyro offset */ + data[3] = BMI160_GET_LSB(offset->off_gyro_x); + data[4] = BMI160_GET_LSB(offset->off_gyro_y); + data[5] = BMI160_GET_LSB(offset->off_gyro_z); + + /* Update the MSB of gyro offset */ + x_msb = BMI160_GET_BITS(offset->off_gyro_x, BMI160_GYRO_OFFSET); + y_msb = BMI160_GET_BITS(offset->off_gyro_y, BMI160_GYRO_OFFSET); + z_msb = BMI160_GET_BITS(offset->off_gyro_z, BMI160_GYRO_OFFSET); + data[6] = (uint8_t)(z_msb << 4 | y_msb << 2 | x_msb); + + /* Set the offset enable/disable for gyro and accel */ + data[6] = BMI160_SET_BITS(data[6], BMI160_GYRO_OFFSET_EN, foc_conf->gyro_off_en); + data[6] = BMI160_SET_BITS(data[6], BMI160_ACCEL_OFFSET_EN, foc_conf->acc_off_en); + + /* Set the offset config and values in the sensor */ + rslt = bmi160_set_regs(BMI160_OFFSET_ADDR, data, 7, dev); + } + + return rslt; +} + +/*! + * @brief This API writes the image registers values to NVM which is + * stored even after POR or soft reset + */ +int8_t bmi160_update_nvm(struct bmi160_dev const *dev) +{ + int8_t rslt; + uint8_t data; + uint8_t cmd = BMI160_NVM_BACKUP_EN; + + /* Read the nvm_prog_en configuration */ + rslt = bmi160_get_regs(BMI160_CONF_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + data = BMI160_SET_BITS(data, BMI160_NVM_UPDATE, 1); + + /* Set the nvm_prog_en bit in the sensor */ + rslt = bmi160_set_regs(BMI160_CONF_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* Update NVM */ + rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &cmd, 1, dev); + if (rslt == BMI160_OK) + { + /* Check for NVM ready status */ + rslt = bmi160_get_regs(BMI160_STATUS_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + data = BMI160_GET_BITS(data, BMI160_NVM_STATUS); + if (data != BMI160_ENABLE) + { + /* Delay to update NVM */ + dev->delay_ms(25); + } + } + } + } + } + + return rslt; +} + +/*! + * @brief This API gets the interrupt status from the sensor. + */ +int8_t bmi160_get_int_status(enum bmi160_int_status_sel int_status_sel, + union bmi160_int_status *int_status, + struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + + /* To get the status of all interrupts */ + if (int_status_sel == BMI160_INT_STATUS_ALL) + { + rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR, &int_status->data[0], 4, dev); + } + else + { + if (int_status_sel & BMI160_INT_STATUS_0) + { + rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR, &int_status->data[0], 1, dev); + } + + if (int_status_sel & BMI160_INT_STATUS_1) + { + rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR + 1, &int_status->data[1], 1, dev); + } + + if (int_status_sel & BMI160_INT_STATUS_2) + { + rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR + 2, &int_status->data[2], 1, dev); + } + + if (int_status_sel & BMI160_INT_STATUS_3) + { + rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR + 3, &int_status->data[3], 1, dev); + } + } + + return rslt; +} + +/*********************** Local function definitions ***************************/ + +/*! + * @brief This API sets the any-motion interrupt of the sensor. + * This interrupt occurs when accel values exceeds preset threshold + * for a certain period of time. + */ +static int8_t set_accel_any_motion_int(struct bmi160_int_settg *int_config, struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg = &(int_config->int_type_cfg.acc_any_motion_int); + rslt = enable_accel_any_motion_int(any_motion_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_any_motion_int_settg(int_config, any_motion_int_cfg, dev); + } + } + + return rslt; +} + +/*! + * @brief This API sets tap interrupts.Interrupt is fired when + * tap movements happen. + */ +static int8_t set_accel_tap_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_tap_int_cfg *tap_int_cfg = &(int_config->int_type_cfg.acc_tap_int); + rslt = enable_tap_int(int_config, tap_int_cfg, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_tap_int_settg(int_config, tap_int_cfg, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the data ready interrupt for both accel and gyro. + * This interrupt occurs when new accel and gyro data comes. + */ +static int8_t set_accel_gyro_data_ready_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = enable_data_ready_int(dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_hardware_interrupt(int_config, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the significant motion interrupt of the sensor.This + * interrupt occurs when there is change in user location. + */ +static int8_t set_accel_sig_motion_int(struct bmi160_int_settg *int_config, struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg = &(int_config->int_type_cfg.acc_sig_motion_int); + rslt = enable_sig_motion_int(sig_mot_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_sig_motion_int_settg(int_config, sig_mot_int_cfg, dev); + } + } + + return rslt; +} + +/*! + * @brief This API sets the no motion/slow motion interrupt of the sensor. + * Slow motion is similar to any motion interrupt.No motion interrupt + * occurs when slope bet. two accel values falls below preset threshold + * for preset duration. + */ +static int8_t set_accel_no_motion_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg = &(int_config->int_type_cfg.acc_no_motion_int); + rslt = enable_no_motion_int(no_mot_int_cfg, dev); + if (rslt == BMI160_OK) + { + /* Configure the INT PIN settings*/ + rslt = config_no_motion_int_settg(int_config, no_mot_int_cfg, dev); + } + } + + return rslt; +} + +/*! + * @brief This API sets the step detection interrupt.This interrupt + * occurs when the single step causes accel values to go above + * preset threshold. + */ +static int8_t set_accel_step_detect_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg = &(int_config->int_type_cfg.acc_step_detect_int); + rslt = enable_step_detect_int(step_detect_int_cfg, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_step_detect(step_detect_int_cfg, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the orientation interrupt of the sensor.This + * interrupt occurs when there is orientation change in the sensor + * with respect to gravitational field vector g. + */ +static int8_t set_accel_orientation_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_orient_int_cfg *orient_int_cfg = &(int_config->int_type_cfg.acc_orient_int); + rslt = enable_orient_int(orient_int_cfg, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + /* map INT pin to orient interrupt */ + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + /* configure the + * orientation setting*/ + rslt = config_orient_int_settg(orient_int_cfg, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the flat interrupt of the sensor.This interrupt + * occurs in case of flat orientation + */ +static int8_t set_accel_flat_detect_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_flat_detect_int_cfg *flat_detect_int = &(int_config->int_type_cfg.acc_flat_int); + + /* enable the flat interrupt */ + rslt = enable_flat_int(flat_detect_int, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + /* map INT pin to flat interrupt */ + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + /* configure the flat setting*/ + rslt = config_flat_int_settg(flat_detect_int, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the low-g interrupt of the sensor.This interrupt + * occurs during free-fall. + */ +static int8_t set_accel_low_g_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_low_g_int_cfg *low_g_int = &(int_config->int_type_cfg.acc_low_g_int); + + /* Enable the low-g interrupt*/ + rslt = enable_low_g_int(low_g_int, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + /* Map INT pin to low-g interrupt */ + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + /* configure the data source + * for low-g interrupt*/ + rslt = config_low_g_data_src(low_g_int, dev); + if (rslt == BMI160_OK) + { + rslt = config_low_g_int_settg(low_g_int, dev); + } + } + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the high-g interrupt of the sensor.The interrupt + * occurs if the absolute value of acceleration data of any enabled axis + * exceeds the programmed threshold and the sign of the value does not + * change for a preset duration. + */ +static int8_t set_accel_high_g_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_high_g_int_cfg *high_g_int_cfg = &(int_config->int_type_cfg.acc_high_g_int); + + /* Enable the high-g interrupt */ + rslt = enable_high_g_int(high_g_int_cfg, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + /* Map INT pin to high-g interrupt */ + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + /* configure the data source + * for high-g interrupt*/ + rslt = config_high_g_data_src(high_g_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_high_g_int_settg(high_g_int_cfg, dev); + } + } + } + } + } + + return rslt; +} + +/*! + * @brief This API configures the pins to fire the + * interrupt signal when it occurs. + */ +static int8_t set_intr_pin_config(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* configure the behavioural settings of interrupt pin */ + rslt = config_int_out_ctrl(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_int_latch(int_config, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi160_dev *dev) +{ + int8_t rslt; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Device structure is fine */ + rslt = BMI160_OK; + } + + return rslt; +} + +/*! + * @brief This API sets the default configuration parameters of accel & gyro. + * Also maintain the previous state of configurations. + */ +static void default_param_settg(struct bmi160_dev *dev) +{ + /* Initializing accel and gyro params with + * default values */ + dev->accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4; + dev->accel_cfg.odr = BMI160_ACCEL_ODR_100HZ; + dev->accel_cfg.power = BMI160_ACCEL_SUSPEND_MODE; + dev->accel_cfg.range = BMI160_ACCEL_RANGE_2G; + dev->gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE; + dev->gyro_cfg.odr = BMI160_GYRO_ODR_100HZ; + dev->gyro_cfg.power = BMI160_GYRO_SUSPEND_MODE; + dev->gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS; + + /* To maintain the previous state of accel configuration */ + dev->prev_accel_cfg = dev->accel_cfg; + + /* To maintain the previous state of gyro configuration */ + dev->prev_gyro_cfg = dev->gyro_cfg; +} + +/*! + * @brief This API set the accel configuration. + */ +static int8_t set_accel_conf(struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data[2] = { 0 }; + + rslt = check_accel_config(data, dev); + if (rslt == BMI160_OK) + { + /* Write output data rate and bandwidth */ + rslt = bmi160_set_regs(BMI160_ACCEL_CONFIG_ADDR, &data[0], 1, dev); + if (rslt == BMI160_OK) + { + dev->prev_accel_cfg.odr = dev->accel_cfg.odr; + dev->prev_accel_cfg.bw = dev->accel_cfg.bw; + + /* write accel range */ + rslt = bmi160_set_regs(BMI160_ACCEL_RANGE_ADDR, &data[1], 1, dev); + if (rslt == BMI160_OK) + { + dev->prev_accel_cfg.range = dev->accel_cfg.range; + } + } + } + + return rslt; +} + +/*! + * @brief This API gets the accel configuration. + */ +static int8_t get_accel_conf(struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data[2] = { 0 }; + + /* Get accel configurations */ + rslt = bmi160_get_regs(BMI160_ACCEL_CONFIG_ADDR, data, 2, dev); + if (rslt == BMI160_OK) + { + dev->accel_cfg.odr = (data[0] & BMI160_ACCEL_ODR_MASK); + dev->accel_cfg.bw = (data[0] & BMI160_ACCEL_BW_MASK) >> BMI160_ACCEL_BW_POS; + dev->accel_cfg.range = (data[1] & BMI160_ACCEL_RANGE_MASK); + } + + return rslt; +} + +/*! + * @brief This API check the accel configuration. + */ +static int8_t check_accel_config(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* read accel Output data rate and bandwidth */ + rslt = bmi160_get_regs(BMI160_ACCEL_CONFIG_ADDR, data, 2, dev); + if (rslt == BMI160_OK) + { + rslt = process_accel_odr(&data[0], dev); + if (rslt == BMI160_OK) + { + rslt = process_accel_bw(&data[0], dev); + if (rslt == BMI160_OK) + { + rslt = process_accel_range(&data[1], dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API process the accel odr. + */ +static int8_t process_accel_odr(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t temp = 0; + uint8_t odr = 0; + + if (dev->accel_cfg.odr <= BMI160_ACCEL_ODR_1600HZ) + { + if (dev->accel_cfg.odr != dev->prev_accel_cfg.odr) + { + odr = (uint8_t)dev->accel_cfg.odr; + temp = *data & ~BMI160_ACCEL_ODR_MASK; + + /* Adding output data rate */ + *data = temp | (odr & BMI160_ACCEL_ODR_MASK); + } + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API process the accel bandwidth. + */ +static int8_t process_accel_bw(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t temp = 0; + uint8_t bw = 0; + + if (dev->accel_cfg.bw <= BMI160_ACCEL_BW_RES_AVG128) + { + if (dev->accel_cfg.bw != dev->prev_accel_cfg.bw) + { + bw = (uint8_t)dev->accel_cfg.bw; + temp = *data & ~BMI160_ACCEL_BW_MASK; + + /* Adding bandwidth */ + *data = temp | ((bw << 4) & BMI160_ACCEL_BW_MASK); + } + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API process the accel range. + */ +static int8_t process_accel_range(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t temp = 0; + uint8_t range = 0; + + if (dev->accel_cfg.range <= BMI160_ACCEL_RANGE_16G) + { + if (dev->accel_cfg.range != dev->prev_accel_cfg.range) + { + range = (uint8_t)dev->accel_cfg.range; + temp = *data & ~BMI160_ACCEL_RANGE_MASK; + + /* Adding range */ + *data = temp | (range & BMI160_ACCEL_RANGE_MASK); + } + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API checks the invalid settings for ODR & Bw for + * Accel and Gyro. + */ +static int8_t check_invalid_settg(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + + /* read the error reg */ + rslt = bmi160_get_regs(BMI160_ERROR_REG_ADDR, &data, 1, dev); + data = data >> 1; + data = data & BMI160_ERR_REG_MASK; + if (data == 1) + { + rslt = BMI160_E_ACCEL_ODR_BW_INVALID; + } + else if (data == 2) + { + rslt = BMI160_E_GYRO_ODR_BW_INVALID; + } + else if (data == 3) + { + rslt = BMI160_E_LWP_PRE_FLTR_INT_INVALID; + } + else if (data == 7) + { + rslt = BMI160_E_LWP_PRE_FLTR_INVALID; + } + + return rslt; +} +static int8_t set_gyro_conf(struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data[2] = { 0 }; + + rslt = check_gyro_config(data, dev); + if (rslt == BMI160_OK) + { + /* Write output data rate and bandwidth */ + rslt = bmi160_set_regs(BMI160_GYRO_CONFIG_ADDR, &data[0], 1, dev); + if (rslt == BMI160_OK) + { + dev->prev_gyro_cfg.odr = dev->gyro_cfg.odr; + dev->prev_gyro_cfg.bw = dev->gyro_cfg.bw; + + /* Write gyro range */ + rslt = bmi160_set_regs(BMI160_GYRO_RANGE_ADDR, &data[1], 1, dev); + if (rslt == BMI160_OK) + { + dev->prev_gyro_cfg.range = dev->gyro_cfg.range; + } + } + } + + return rslt; +} + +/*! + * @brief This API gets the gyro configuration. + */ +static int8_t get_gyro_conf(struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data[2] = { 0 }; + + /* Get accel configurations */ + rslt = bmi160_get_regs(BMI160_GYRO_CONFIG_ADDR, data, 2, dev); + if (rslt == BMI160_OK) + { + dev->gyro_cfg.odr = (data[0] & BMI160_GYRO_ODR_MASK); + dev->gyro_cfg.bw = (data[0] & BMI160_GYRO_BW_MASK) >> BMI160_GYRO_BW_POS; + dev->gyro_cfg.range = (data[1] & BMI160_GYRO_RANGE_MASK); + } + + return rslt; +} + +/*! + * @brief This API check the gyro configuration. + */ +static int8_t check_gyro_config(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* read gyro Output data rate and bandwidth */ + rslt = bmi160_get_regs(BMI160_GYRO_CONFIG_ADDR, data, 2, dev); + if (rslt == BMI160_OK) + { + rslt = process_gyro_odr(&data[0], dev); + if (rslt == BMI160_OK) + { + rslt = process_gyro_bw(&data[0], dev); + if (rslt == BMI160_OK) + { + rslt = process_gyro_range(&data[1], dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API process the gyro odr. + */ +static int8_t process_gyro_odr(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t temp = 0; + uint8_t odr = 0; + + if (dev->gyro_cfg.odr <= BMI160_GYRO_ODR_3200HZ) + { + if (dev->gyro_cfg.odr != dev->prev_gyro_cfg.odr) + { + odr = (uint8_t)dev->gyro_cfg.odr; + temp = (*data & ~BMI160_GYRO_ODR_MASK); + + /* Adding output data rate */ + *data = temp | (odr & BMI160_GYRO_ODR_MASK); + } + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API process the gyro bandwidth. + */ +static int8_t process_gyro_bw(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t temp = 0; + uint8_t bw = 0; + + if (dev->gyro_cfg.bw <= BMI160_GYRO_BW_NORMAL_MODE) + { + bw = (uint8_t)dev->gyro_cfg.bw; + temp = *data & ~BMI160_GYRO_BW_MASK; + + /* Adding bandwidth */ + *data = temp | ((bw << 4) & BMI160_GYRO_BW_MASK); + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API process the gyro range. + */ +static int8_t process_gyro_range(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t temp = 0; + uint8_t range = 0; + + if (dev->gyro_cfg.range <= BMI160_GYRO_RANGE_125_DPS) + { + if (dev->gyro_cfg.range != dev->prev_gyro_cfg.range) + { + range = (uint8_t)dev->gyro_cfg.range; + temp = *data & ~BMI160_GYRO_RANGE_MASK; + + /* Adding range */ + *data = temp | (range & BMI160_GYRO_RANGE_MASK); + } + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API sets the accel power. + */ +static int8_t set_accel_pwr(struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t data = 0; + + if ((dev->accel_cfg.power >= BMI160_ACCEL_SUSPEND_MODE) && (dev->accel_cfg.power <= BMI160_ACCEL_LOWPOWER_MODE)) + { + if (dev->accel_cfg.power != dev->prev_accel_cfg.power) + { + rslt = process_under_sampling(&data, dev); + if (rslt == BMI160_OK) + { + /* Write accel power */ + rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &dev->accel_cfg.power, 1, dev); + + /* Add delay of 3.8 ms - refer data sheet table 24*/ + if (dev->prev_accel_cfg.power == BMI160_ACCEL_SUSPEND_MODE) + { + dev->delay_ms(BMI160_ACCEL_DELAY_MS); + } + + dev->prev_accel_cfg.power = dev->accel_cfg.power; + } + } + } + else + { + rslt = BMI160_E_INVALID_CONFIG; + } + + return rslt; +} + +/*! + * @brief This API process the undersampling setting of Accel. + */ +static int8_t process_under_sampling(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t pre_filter[2] = { 0 }; + + rslt = bmi160_get_regs(BMI160_ACCEL_CONFIG_ADDR, data, 1, dev); + if (rslt == BMI160_OK) + { + if (dev->accel_cfg.power == BMI160_ACCEL_LOWPOWER_MODE) + { + temp = *data & ~BMI160_ACCEL_UNDERSAMPLING_MASK; + + /* Set under-sampling parameter */ + *data = temp | ((1 << 7) & BMI160_ACCEL_UNDERSAMPLING_MASK); + + /* Write data */ + rslt = bmi160_set_regs(BMI160_ACCEL_CONFIG_ADDR, data, 1, dev); + + /* Disable the pre-filter data in low power mode */ + if (rslt == BMI160_OK) + { + /* Disable the Pre-filter data*/ + rslt = bmi160_set_regs(BMI160_INT_DATA_0_ADDR, pre_filter, 2, dev); + } + } + else if (*data & BMI160_ACCEL_UNDERSAMPLING_MASK) + { + temp = *data & ~BMI160_ACCEL_UNDERSAMPLING_MASK; + + /* Disable under-sampling parameter if already enabled */ + *data = temp; + + /* Write data */ + rslt = bmi160_set_regs(BMI160_ACCEL_CONFIG_ADDR, data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API sets the gyro power mode. + */ +static int8_t set_gyro_pwr(struct bmi160_dev *dev) +{ + int8_t rslt = 0; + + if ((dev->gyro_cfg.power == BMI160_GYRO_SUSPEND_MODE) || (dev->gyro_cfg.power == BMI160_GYRO_NORMAL_MODE) || + (dev->gyro_cfg.power == BMI160_GYRO_FASTSTARTUP_MODE)) + { + if (dev->gyro_cfg.power != dev->prev_gyro_cfg.power) + { + /* Write gyro power */ + rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &dev->gyro_cfg.power, 1, dev); + if (dev->prev_gyro_cfg.power == BMI160_GYRO_SUSPEND_MODE) + { + /* Delay of 80 ms - datasheet Table 24 */ + dev->delay_ms(BMI160_GYRO_DELAY_MS); + } + else if ((dev->prev_gyro_cfg.power == BMI160_GYRO_FASTSTARTUP_MODE) && + (dev->gyro_cfg.power == BMI160_GYRO_NORMAL_MODE)) + { + /* This delay is required for transition from + * fast-startup mode to normal mode - datasheet Table 3 */ + dev->delay_ms(10); + } + else + { + /* do nothing */ + } + + dev->prev_gyro_cfg.power = dev->gyro_cfg.power; + } + } + else + { + rslt = BMI160_E_INVALID_CONFIG; + } + + return rslt; +} + +/*! + * @brief This API reads accel data along with sensor time if time is requested + * by user. Kindly refer the user guide(README.md) for more info. + */ +static int8_t get_accel_data(uint8_t len, struct bmi160_sensor_data *accel, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t idx = 0; + uint8_t data_array[9] = { 0 }; + uint8_t time_0 = 0; + uint16_t time_1 = 0; + uint32_t time_2 = 0; + uint8_t lsb; + uint8_t msb; + int16_t msblsb; + + /* read accel sensor data along with time if requested */ + rslt = bmi160_get_regs(BMI160_ACCEL_DATA_ADDR, data_array, 6 + len, dev); + if (rslt == BMI160_OK) + { + /* Accel Data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + accel->x = msblsb; /* Data in X axis */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + accel->y = msblsb; /* Data in Y axis */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + accel->z = msblsb; /* Data in Z axis */ + if (len == 3) + { + time_0 = data_array[idx++]; + time_1 = (uint16_t)(data_array[idx++] << 8); + time_2 = (uint32_t)(data_array[idx++] << 16); + accel->sensortime = (uint32_t)(time_2 | time_1 | time_0); + } + else + { + accel->sensortime = 0; + } + } + else + { + rslt = BMI160_E_COM_FAIL; + } + + return rslt; +} + +/*! + * @brief This API reads accel data along with sensor time if time is requested + * by user. Kindly refer the user guide(README.md) for more info. + */ +static int8_t get_gyro_data(uint8_t len, struct bmi160_sensor_data *gyro, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t idx = 0; + uint8_t data_array[15] = { 0 }; + uint8_t time_0 = 0; + uint16_t time_1 = 0; + uint32_t time_2 = 0; + uint8_t lsb; + uint8_t msb; + int16_t msblsb; + + if (len == 0) + { + /* read gyro data only */ + rslt = bmi160_get_regs(BMI160_GYRO_DATA_ADDR, data_array, 6, dev); + if (rslt == BMI160_OK) + { + /* Gyro Data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->x = msblsb; /* Data in X axis */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->y = msblsb; /* Data in Y axis */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->z = msblsb; /* Data in Z axis */ + gyro->sensortime = 0; + } + else + { + rslt = BMI160_E_COM_FAIL; + } + } + else + { + /* read gyro sensor data along with time */ + rslt = bmi160_get_regs(BMI160_GYRO_DATA_ADDR, data_array, 12 + len, dev); + if (rslt == BMI160_OK) + { + /* Gyro Data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->x = msblsb; /* gyro X axis data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->y = msblsb; /* gyro Y axis data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->z = msblsb; /* gyro Z axis data */ + idx = idx + 6; + time_0 = data_array[idx++]; + time_1 = (uint16_t)(data_array[idx++] << 8); + time_2 = (uint32_t)(data_array[idx++] << 16); + gyro->sensortime = (uint32_t)(time_2 | time_1 | time_0); + } + else + { + rslt = BMI160_E_COM_FAIL; + } + } + + return rslt; +} + +/*! + * @brief This API reads accel and gyro data along with sensor time + * if time is requested by user. + * Kindly refer the user guide(README.md) for more info. + */ +static int8_t get_accel_gyro_data(uint8_t len, + struct bmi160_sensor_data *accel, + struct bmi160_sensor_data *gyro, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t idx = 0; + uint8_t data_array[15] = { 0 }; + uint8_t time_0 = 0; + uint16_t time_1 = 0; + uint32_t time_2 = 0; + uint8_t lsb; + uint8_t msb; + int16_t msblsb; + + /* read both accel and gyro sensor data + * along with time if requested */ + rslt = bmi160_get_regs(BMI160_GYRO_DATA_ADDR, data_array, 12 + len, dev); + if (rslt == BMI160_OK) + { + /* Gyro Data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->x = msblsb; /* gyro X axis data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->y = msblsb; /* gyro Y axis data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->z = msblsb; /* gyro Z axis data */ + /* Accel Data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + accel->x = (int16_t)msblsb; /* accel X axis data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + accel->y = (int16_t)msblsb; /* accel Y axis data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + accel->z = (int16_t)msblsb; /* accel Z axis data */ + if (len == 3) + { + time_0 = data_array[idx++]; + time_1 = (uint16_t)(data_array[idx++] << 8); + time_2 = (uint32_t)(data_array[idx++] << 16); + accel->sensortime = (uint32_t)(time_2 | time_1 | time_0); + gyro->sensortime = (uint32_t)(time_2 | time_1 | time_0); + } + else + { + accel->sensortime = 0; + gyro->sensortime = 0; + } + } + else + { + rslt = BMI160_E_COM_FAIL; + } + + return rslt; +} + +/*! + * @brief This API enables the any-motion interrupt for accel. + */ +static int8_t enable_accel_any_motion_int(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable any motion x, any motion y, any motion z + * in Int Enable 0 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + if (any_motion_int_cfg->anymotion_en == BMI160_ENABLE) + { + temp = data & ~BMI160_ANY_MOTION_X_INT_EN_MASK; + + /* Adding Any_motion x axis */ + data = temp | (any_motion_int_cfg->anymotion_x & BMI160_ANY_MOTION_X_INT_EN_MASK); + temp = data & ~BMI160_ANY_MOTION_Y_INT_EN_MASK; + + /* Adding Any_motion y axis */ + data = temp | ((any_motion_int_cfg->anymotion_y << 1) & BMI160_ANY_MOTION_Y_INT_EN_MASK); + temp = data & ~BMI160_ANY_MOTION_Z_INT_EN_MASK; + + /* Adding Any_motion z axis */ + data = temp | ((any_motion_int_cfg->anymotion_z << 2) & BMI160_ANY_MOTION_Z_INT_EN_MASK); + + /* any-motion feature selected*/ + dev->any_sig_sel = BMI160_ANY_MOTION_ENABLED; + } + else + { + data = data & ~BMI160_ANY_MOTION_ALL_INT_EN_MASK; + + /* neither any-motion feature nor sig-motion selected */ + dev->any_sig_sel = BMI160_BOTH_ANY_SIG_MOTION_DISABLED; + } + + /* write data to Int Enable 0 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API disable the sig-motion interrupt. + */ +static int8_t disable_sig_motion_int(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Disabling Significant motion interrupt if enabled */ + rslt = bmi160_get_regs(BMI160_INT_MOTION_3_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = (data & BMI160_SIG_MOTION_SEL_MASK); + if (temp) + { + temp = data & ~BMI160_SIG_MOTION_SEL_MASK; + data = temp; + + /* Write data to register */ + rslt = bmi160_set_regs(BMI160_INT_MOTION_3_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API is used to map/unmap the Any/Sig motion, Step det/Low-g, + * Double tap, Single tap, Orientation, Flat, High-G, Nomotion interrupt pins. + */ +static int8_t map_feature_interrupt(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data[3] = { 0, 0, 0 }; + uint8_t temp[3] = { 0, 0, 0 }; + + rslt = bmi160_get_regs(BMI160_INT_MAP_0_ADDR, data, 3, dev); + if (rslt == BMI160_OK) + { + temp[0] = data[0] & ~int_mask_lookup_table[int_config->int_type]; + temp[2] = data[2] & ~int_mask_lookup_table[int_config->int_type]; + switch (int_config->int_channel) + { + case BMI160_INT_CHANNEL_NONE: + data[0] = temp[0]; + data[2] = temp[2]; + break; + case BMI160_INT_CHANNEL_1: + data[0] = temp[0] | int_mask_lookup_table[int_config->int_type]; + data[2] = temp[2]; + break; + case BMI160_INT_CHANNEL_2: + data[2] = temp[2] | int_mask_lookup_table[int_config->int_type]; + data[0] = temp[0]; + break; + case BMI160_INT_CHANNEL_BOTH: + data[0] = temp[0] | int_mask_lookup_table[int_config->int_type]; + data[2] = temp[2] | int_mask_lookup_table[int_config->int_type]; + break; + default: + rslt = BMI160_E_OUT_OF_RANGE; + } + if (rslt == BMI160_OK) + { + rslt = bmi160_set_regs(BMI160_INT_MAP_0_ADDR, data, 3, dev); + } + } + + return rslt; +} + +/*! + * @brief This API is used to map/unmap the Dataready(Accel & Gyro), FIFO full + * and FIFO watermark interrupt. + */ +static int8_t map_hardware_interrupt(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + rslt = bmi160_get_regs(BMI160_INT_MAP_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~int_mask_lookup_table[int_config->int_type]; + temp = temp & ~((uint8_t)(int_mask_lookup_table[int_config->int_type] << 4)); + switch (int_config->int_channel) + { + case BMI160_INT_CHANNEL_NONE: + data = temp; + break; + case BMI160_INT_CHANNEL_1: + data = temp | (uint8_t)((int_mask_lookup_table[int_config->int_type]) << 4); + break; + case BMI160_INT_CHANNEL_2: + data = temp | int_mask_lookup_table[int_config->int_type]; + break; + case BMI160_INT_CHANNEL_BOTH: + data = temp | int_mask_lookup_table[int_config->int_type]; + data = data | (uint8_t)((int_mask_lookup_table[int_config->int_type]) << 4); + break; + default: + rslt = BMI160_E_OUT_OF_RANGE; + } + if (rslt == BMI160_OK) + { + rslt = bmi160_set_regs(BMI160_INT_MAP_1_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for any-motion interrupt. + */ +static int8_t config_any_motion_src(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Configure Int data 1 register to add source of interrupt */ + rslt = bmi160_get_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_MOTION_SRC_INT_MASK; + data = temp | ((any_motion_int_cfg->anymotion_data_src << 7) & BMI160_MOTION_SRC_INT_MASK); + + /* Write data to DATA 1 address */ + rslt = bmi160_set_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the duration and threshold of + * any-motion interrupt. + */ +static int8_t config_any_dur_threshold(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + uint8_t data_array[2] = { 0 }; + uint8_t dur; + + /* Configure Int Motion 0 register */ + rslt = bmi160_get_regs(BMI160_INT_MOTION_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* slope duration */ + dur = (uint8_t)any_motion_int_cfg->anymotion_dur; + temp = data & ~BMI160_SLOPE_INT_DUR_MASK; + data = temp | (dur & BMI160_MOTION_SRC_INT_MASK); + data_array[0] = data; + + /* add slope threshold */ + data_array[1] = any_motion_int_cfg->anymotion_thr; + + /* INT MOTION 0 and INT MOTION 1 address lie consecutively, + * hence writing data to respective registers at one go */ + + /* Writing to Int_motion 0 and + * Int_motion 1 Address simultaneously */ + rslt = bmi160_set_regs(BMI160_INT_MOTION_0_ADDR, data_array, 2, dev); + } + + return rslt; +} + +/*! + * @brief This API configure necessary setting of any-motion interrupt. + */ +static int8_t config_any_motion_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = disable_sig_motion_int(dev); + if (rslt == BMI160_OK) + { + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_any_motion_src(any_motion_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_any_dur_threshold(any_motion_int_cfg, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API enable the data ready interrupt. + */ +static int8_t enable_data_ready_int(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable data ready interrupt in Int Enable 1 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_DATA_RDY_INT_EN_MASK; + data = temp | ((1 << 4) & BMI160_DATA_RDY_INT_EN_MASK); + + /* Writing data to INT ENABLE 1 Address */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API enables the no motion/slow motion interrupt. + */ +static int8_t enable_no_motion_int(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable no motion x, no motion y, no motion z + * in Int Enable 2 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_2_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + if (no_mot_int_cfg->no_motion_x == 1) + { + temp = data & ~BMI160_NO_MOTION_X_INT_EN_MASK; + + /* Adding No_motion x axis */ + data = temp | (1 & BMI160_NO_MOTION_X_INT_EN_MASK); + } + + if (no_mot_int_cfg->no_motion_y == 1) + { + temp = data & ~BMI160_NO_MOTION_Y_INT_EN_MASK; + + /* Adding No_motion x axis */ + data = temp | ((1 << 1) & BMI160_NO_MOTION_Y_INT_EN_MASK); + } + + if (no_mot_int_cfg->no_motion_z == 1) + { + temp = data & ~BMI160_NO_MOTION_Z_INT_EN_MASK; + + /* Adding No_motion x axis */ + data = temp | ((1 << 2) & BMI160_NO_MOTION_Z_INT_EN_MASK); + } + + /* write data to Int Enable 2 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_2_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the interrupt PIN setting for + * no motion/slow motion interrupt. + */ +static int8_t config_no_motion_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_no_motion_data_src(no_mot_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_no_motion_dur_thr(no_mot_int_cfg, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API configure the source of interrupt for no motion. + */ +static int8_t config_no_motion_data_src(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Configure Int data 1 register to add source of interrupt */ + rslt = bmi160_get_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_MOTION_SRC_INT_MASK; + data = temp | ((no_mot_int_cfg->no_motion_src << 7) & BMI160_MOTION_SRC_INT_MASK); + + /* Write data to DATA 1 address */ + rslt = bmi160_set_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the duration and threshold of + * no motion/slow motion interrupt along with selection of no/slow motion. + */ +static int8_t config_no_motion_dur_thr(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + uint8_t temp_1 = 0; + uint8_t reg_addr; + uint8_t data_array[2] = { 0 }; + + /* Configuring INT_MOTION register */ + reg_addr = BMI160_INT_MOTION_0_ADDR; + rslt = bmi160_get_regs(reg_addr, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_NO_MOTION_INT_DUR_MASK; + + /* Adding no_motion duration */ + data = temp | ((no_mot_int_cfg->no_motion_dur << 2) & BMI160_NO_MOTION_INT_DUR_MASK); + + /* Write data to NO_MOTION 0 address */ + rslt = bmi160_set_regs(reg_addr, &data, 1, dev); + if (rslt == BMI160_OK) + { + reg_addr = BMI160_INT_MOTION_3_ADDR; + rslt = bmi160_get_regs(reg_addr, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_NO_MOTION_SEL_BIT_MASK; + + /* Adding no_motion_sel bit */ + temp_1 = (no_mot_int_cfg->no_motion_sel & BMI160_NO_MOTION_SEL_BIT_MASK); + data = (temp | temp_1); + data_array[1] = data; + + /* Adding no motion threshold */ + data_array[0] = no_mot_int_cfg->no_motion_thres; + reg_addr = BMI160_INT_MOTION_2_ADDR; + + /* writing data to INT_MOTION 2 and INT_MOTION 3 + * address simultaneously */ + rslt = bmi160_set_regs(reg_addr, data_array, 2, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API enables the sig-motion motion interrupt. + */ +static int8_t enable_sig_motion_int(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* For significant motion,enable any motion x,any motion y, + * any motion z in Int Enable 0 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + if (sig_mot_int_cfg->sig_en == BMI160_ENABLE) + { + temp = data & ~BMI160_SIG_MOTION_INT_EN_MASK; + data = temp | (7 & BMI160_SIG_MOTION_INT_EN_MASK); + + /* sig-motion feature selected*/ + dev->any_sig_sel = BMI160_SIG_MOTION_ENABLED; + } + else + { + data = data & ~BMI160_SIG_MOTION_INT_EN_MASK; + + /* neither any-motion feature nor sig-motion selected */ + dev->any_sig_sel = BMI160_BOTH_ANY_SIG_MOTION_DISABLED; + } + + /* write data to Int Enable 0 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the interrupt PIN setting for + * significant motion interrupt. + */ +static int8_t config_sig_motion_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_sig_motion_data_src(sig_mot_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_sig_dur_threshold(sig_mot_int_cfg, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for sig motion interrupt. + */ +static int8_t config_sig_motion_data_src(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Configure Int data 1 register to add source of interrupt */ + rslt = bmi160_get_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_MOTION_SRC_INT_MASK; + data = temp | ((sig_mot_int_cfg->sig_data_src << 7) & BMI160_MOTION_SRC_INT_MASK); + + /* Write data to DATA 1 address */ + rslt = bmi160_set_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the threshold, skip and proof time of + * sig motion interrupt. + */ +static int8_t config_sig_dur_threshold(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data; + uint8_t temp = 0; + + /* Configuring INT_MOTION registers */ + + /* Write significant motion threshold. + * This threshold is same as any motion threshold */ + data = sig_mot_int_cfg->sig_mot_thres; + + /* Write data to INT_MOTION 1 address */ + rslt = bmi160_set_regs(BMI160_INT_MOTION_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + rslt = bmi160_get_regs(BMI160_INT_MOTION_3_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_SIG_MOTION_SKIP_MASK; + + /* adding skip time of sig_motion interrupt*/ + data = temp | ((sig_mot_int_cfg->sig_mot_skip << 2) & BMI160_SIG_MOTION_SKIP_MASK); + temp = data & ~BMI160_SIG_MOTION_PROOF_MASK; + + /* adding proof time of sig_motion interrupt */ + data = temp | ((sig_mot_int_cfg->sig_mot_proof << 4) & BMI160_SIG_MOTION_PROOF_MASK); + + /* configure the int_sig_mot_sel bit to select + * significant motion interrupt */ + temp = data & ~BMI160_SIG_MOTION_SEL_MASK; + data = temp | ((sig_mot_int_cfg->sig_en << 1) & BMI160_SIG_MOTION_SEL_MASK); + rslt = bmi160_set_regs(BMI160_INT_MOTION_3_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API enables the step detector interrupt. + */ +static int8_t enable_step_detect_int(const struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable data ready interrupt in Int Enable 2 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_2_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_STEP_DETECT_INT_EN_MASK; + data = temp | ((step_detect_int_cfg->step_detector_en << 3) & BMI160_STEP_DETECT_INT_EN_MASK); + + /* Writing data to INT ENABLE 2 Address */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_2_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the step detector parameter. + */ +static int8_t config_step_detect(const struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t data_array[2] = { 0 }; + + if (step_detect_int_cfg->step_detector_mode == BMI160_STEP_DETECT_NORMAL) + { + /* Normal mode setting */ + data_array[0] = 0x15; + data_array[1] = 0x03; + } + else if (step_detect_int_cfg->step_detector_mode == BMI160_STEP_DETECT_SENSITIVE) + { + /* Sensitive mode setting */ + data_array[0] = 0x2D; + data_array[1] = 0x00; + } + else if (step_detect_int_cfg->step_detector_mode == BMI160_STEP_DETECT_ROBUST) + { + /* Robust mode setting */ + data_array[0] = 0x1D; + data_array[1] = 0x07; + } + else if (step_detect_int_cfg->step_detector_mode == BMI160_STEP_DETECT_USER_DEFINE) + { + /* Non recommended User defined setting */ + /* Configuring STEP_CONFIG register */ + rslt = bmi160_get_regs(BMI160_INT_STEP_CONFIG_0_ADDR, &data_array[0], 2, dev); + if (rslt == BMI160_OK) + { + temp = data_array[0] & ~BMI160_STEP_DETECT_MIN_THRES_MASK; + + /* Adding min_threshold */ + data_array[0] = temp | ((step_detect_int_cfg->min_threshold << 3) & BMI160_STEP_DETECT_MIN_THRES_MASK); + temp = data_array[0] & ~BMI160_STEP_DETECT_STEPTIME_MIN_MASK; + + /* Adding steptime_min */ + data_array[0] = temp | ((step_detect_int_cfg->steptime_min) & BMI160_STEP_DETECT_STEPTIME_MIN_MASK); + temp = data_array[1] & ~BMI160_STEP_MIN_BUF_MASK; + + /* Adding steptime_min */ + data_array[1] = temp | ((step_detect_int_cfg->step_min_buf) & BMI160_STEP_MIN_BUF_MASK); + } + } + + /* Write data to STEP_CONFIG register */ + rslt = bmi160_set_regs(BMI160_INT_STEP_CONFIG_0_ADDR, data_array, 2, dev); + + return rslt; +} + +/*! + * @brief This API enables the single/double tap interrupt. + */ +static int8_t enable_tap_int(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_tap_int_cfg *tap_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable single tap or double tap interrupt in Int Enable 0 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + if (int_config->int_type == BMI160_ACC_SINGLE_TAP_INT) + { + temp = data & ~BMI160_SINGLE_TAP_INT_EN_MASK; + data = temp | ((tap_int_cfg->tap_en << 5) & BMI160_SINGLE_TAP_INT_EN_MASK); + } + else + { + temp = data & ~BMI160_DOUBLE_TAP_INT_EN_MASK; + data = temp | ((tap_int_cfg->tap_en << 4) & BMI160_DOUBLE_TAP_INT_EN_MASK); + } + + /* Write to Enable 0 Address */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the interrupt PIN setting for + * tap interrupt. + */ +static int8_t config_tap_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_tap_int_cfg *tap_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_tap_data_src(tap_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_tap_param(int_config, tap_int_cfg, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for tap interrupt. + */ +static int8_t config_tap_data_src(const struct bmi160_acc_tap_int_cfg *tap_int_cfg, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Configure Int data 0 register to add source of interrupt */ + rslt = bmi160_get_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_TAP_SRC_INT_MASK; + data = temp | ((tap_int_cfg->tap_data_src << 3) & BMI160_TAP_SRC_INT_MASK); + + /* Write data to Data 0 address */ + rslt = bmi160_set_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the parameters of tap interrupt. + * Threshold, quite, shock, and duration. + */ +static int8_t config_tap_param(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_tap_int_cfg *tap_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t data = 0; + uint8_t data_array[2] = { 0 }; + uint8_t count = 0; + uint8_t dur, shock, quiet, thres; + + /* Configure tap 0 register for tap shock,tap quiet duration + * in case of single tap interrupt */ + rslt = bmi160_get_regs(BMI160_INT_TAP_0_ADDR, data_array, 2, dev); + if (rslt == BMI160_OK) + { + data = data_array[count]; + if (int_config->int_type == BMI160_ACC_DOUBLE_TAP_INT) + { + dur = (uint8_t)tap_int_cfg->tap_dur; + temp = (data & ~BMI160_TAP_DUR_MASK); + + /* Add tap duration data in case of + * double tap interrupt */ + data = temp | (dur & BMI160_TAP_DUR_MASK); + } + + shock = (uint8_t)tap_int_cfg->tap_shock; + temp = data & ~BMI160_TAP_SHOCK_DUR_MASK; + data = temp | ((shock << 6) & BMI160_TAP_SHOCK_DUR_MASK); + quiet = (uint8_t)tap_int_cfg->tap_quiet; + temp = data & ~BMI160_TAP_QUIET_DUR_MASK; + data = temp | ((quiet << 7) & BMI160_TAP_QUIET_DUR_MASK); + data_array[count++] = data; + data = data_array[count]; + thres = (uint8_t)tap_int_cfg->tap_thr; + temp = data & ~BMI160_TAP_THRES_MASK; + data = temp | (thres & BMI160_TAP_THRES_MASK); + data_array[count++] = data; + + /* TAP 0 and TAP 1 address lie consecutively, + * hence writing data to respective registers at one go */ + + /* Writing to Tap 0 and Tap 1 Address simultaneously */ + rslt = bmi160_set_regs(BMI160_INT_TAP_0_ADDR, data_array, count, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the secondary interface. + */ +static int8_t config_sec_if(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t if_conf = 0; + uint8_t cmd = BMI160_AUX_NORMAL_MODE; + + /* set the aux power mode to normal*/ + rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &cmd, 1, dev); + if (rslt == BMI160_OK) + { + /* 0.5ms delay - refer datasheet table 24*/ + dev->delay_ms(1); + rslt = bmi160_get_regs(BMI160_IF_CONF_ADDR, &if_conf, 1, dev); + if_conf |= (uint8_t)(1 << 5); + if (rslt == BMI160_OK) + { + /*enable the secondary interface also*/ + rslt = bmi160_set_regs(BMI160_IF_CONF_ADDR, &if_conf, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API configure the ODR of the auxiliary sensor. + */ +static int8_t config_aux_odr(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t aux_odr; + + rslt = bmi160_get_regs(BMI160_AUX_ODR_ADDR, &aux_odr, 1, dev); + if (rslt == BMI160_OK) + { + aux_odr = (uint8_t)(dev->aux_cfg.aux_odr); + + /* Set the secondary interface ODR + * i.e polling rate of secondary sensor */ + rslt = bmi160_set_regs(BMI160_AUX_ODR_ADDR, &aux_odr, 1, dev); + dev->delay_ms(BMI160_AUX_COM_DELAY); + } + + return rslt; +} + +/*! + * @brief This API maps the actual burst read length set by user. + */ +static int8_t map_read_len(uint16_t *len, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + switch (dev->aux_cfg.aux_rd_burst_len) + { + case BMI160_AUX_READ_LEN_0: + *len = 1; + break; + case BMI160_AUX_READ_LEN_1: + *len = 2; + break; + case BMI160_AUX_READ_LEN_2: + *len = 6; + break; + case BMI160_AUX_READ_LEN_3: + *len = 8; + break; + default: + rslt = BMI160_E_INVALID_INPUT; + break; + } + + return rslt; +} + +/*! + * @brief This API configure the settings of auxiliary sensor. + */ +static int8_t config_aux_settg(const struct bmi160_dev *dev) +{ + int8_t rslt; + + rslt = config_sec_if(dev); + if (rslt == BMI160_OK) + { + /* Configures the auxiliary interface settings */ + rslt = bmi160_config_aux_mode(dev); + } + + return rslt; +} + +/*! + * @brief This API extract the read data from auxiliary sensor. + */ +static int8_t extract_aux_read(uint16_t map_len, + uint8_t reg_addr, + uint8_t *aux_data, + uint16_t len, + const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + uint8_t data[8] = { 0, }; + uint8_t read_addr = BMI160_AUX_DATA_ADDR; + uint8_t count = 0; + uint8_t read_count; + uint8_t read_len = (uint8_t)map_len; + + for (; count < len;) + { + /* set address to read */ + rslt = bmi160_set_regs(BMI160_AUX_IF_2_ADDR, ®_addr, 1, dev); + dev->delay_ms(BMI160_AUX_COM_DELAY); + if (rslt == BMI160_OK) + { + rslt = bmi160_get_regs(read_addr, data, map_len, dev); + if (rslt == BMI160_OK) + { + read_count = 0; + + /* if read len is less the burst read len + * mention by user*/ + if (len < map_len) + { + read_len = (uint8_t)len; + } + else if ((len - count) < map_len) + { + read_len = (uint8_t)(len - count); + } + + for (; read_count < read_len; read_count++) + { + aux_data[count + read_count] = data[read_count]; + } + + reg_addr += (uint8_t)map_len; + count += (uint8_t)map_len; + } + else + { + rslt = BMI160_E_COM_FAIL; + break; + } + } + } + + return rslt; +} + +/*! + * @brief This API enables the orient interrupt. + */ +static int8_t enable_orient_int(const struct bmi160_acc_orient_int_cfg *orient_int_cfg, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable data ready interrupt in Int Enable 0 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_ORIENT_INT_EN_MASK; + data = temp | ((orient_int_cfg->orient_en << 6) & BMI160_ORIENT_INT_EN_MASK); + + /* write data to Int Enable 0 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the necessary setting of orientation interrupt. + */ +static int8_t config_orient_int_settg(const struct bmi160_acc_orient_int_cfg *orient_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + uint8_t data_array[2] = { 0, 0 }; + + /* Configuring INT_ORIENT registers */ + rslt = bmi160_get_regs(BMI160_INT_ORIENT_0_ADDR, data_array, 2, dev); + if (rslt == BMI160_OK) + { + data = data_array[0]; + temp = data & ~BMI160_ORIENT_MODE_MASK; + + /* Adding Orientation mode */ + data = temp | ((orient_int_cfg->orient_mode) & BMI160_ORIENT_MODE_MASK); + temp = data & ~BMI160_ORIENT_BLOCK_MASK; + + /* Adding Orientation blocking */ + data = temp | ((orient_int_cfg->orient_blocking << 2) & BMI160_ORIENT_BLOCK_MASK); + temp = data & ~BMI160_ORIENT_HYST_MASK; + + /* Adding Orientation hysteresis */ + data = temp | ((orient_int_cfg->orient_hyst << 4) & BMI160_ORIENT_HYST_MASK); + data_array[0] = data; + data = data_array[1]; + temp = data & ~BMI160_ORIENT_THETA_MASK; + + /* Adding Orientation threshold */ + data = temp | ((orient_int_cfg->orient_theta) & BMI160_ORIENT_THETA_MASK); + temp = data & ~BMI160_ORIENT_UD_ENABLE; + + /* Adding Orient_ud_en */ + data = temp | ((orient_int_cfg->orient_ud_en << 6) & BMI160_ORIENT_UD_ENABLE); + temp = data & ~BMI160_AXES_EN_MASK; + + /* Adding axes_en */ + data = temp | ((orient_int_cfg->axes_ex << 7) & BMI160_AXES_EN_MASK); + data_array[1] = data; + + /* Writing data to INT_ORIENT 0 and INT_ORIENT 1 + * registers simultaneously */ + rslt = bmi160_set_regs(BMI160_INT_ORIENT_0_ADDR, data_array, 2, dev); + } + + return rslt; +} + +/*! + * @brief This API enables the flat interrupt. + */ +static int8_t enable_flat_int(const struct bmi160_acc_flat_detect_int_cfg *flat_int, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable flat interrupt in Int Enable 0 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_FLAT_INT_EN_MASK; + data = temp | ((flat_int->flat_en << 7) & BMI160_FLAT_INT_EN_MASK); + + /* write data to Int Enable 0 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the necessary setting of flat interrupt. + */ +static int8_t config_flat_int_settg(const struct bmi160_acc_flat_detect_int_cfg *flat_int, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + uint8_t data_array[2] = { 0, 0 }; + + /* Configuring INT_FLAT register */ + rslt = bmi160_get_regs(BMI160_INT_FLAT_0_ADDR, data_array, 2, dev); + if (rslt == BMI160_OK) + { + data = data_array[0]; + temp = data & ~BMI160_FLAT_THRES_MASK; + + /* Adding flat theta */ + data = temp | ((flat_int->flat_theta) & BMI160_FLAT_THRES_MASK); + data_array[0] = data; + data = data_array[1]; + temp = data & ~BMI160_FLAT_HOLD_TIME_MASK; + + /* Adding flat hold time */ + data = temp | ((flat_int->flat_hold_time << 4) & BMI160_FLAT_HOLD_TIME_MASK); + temp = data & ~BMI160_FLAT_HYST_MASK; + + /* Adding flat hysteresis */ + data = temp | ((flat_int->flat_hy) & BMI160_FLAT_HYST_MASK); + data_array[1] = data; + + /* Writing data to INT_FLAT 0 and INT_FLAT 1 + * registers simultaneously */ + rslt = bmi160_set_regs(BMI160_INT_FLAT_0_ADDR, data_array, 2, dev); + } + + return rslt; +} + +/*! + * @brief This API enables the Low-g interrupt. + */ +static int8_t enable_low_g_int(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable low-g interrupt in Int Enable 1 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_LOW_G_INT_EN_MASK; + data = temp | ((low_g_int->low_en << 3) & BMI160_LOW_G_INT_EN_MASK); + + /* write data to Int Enable 0 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for low-g interrupt. + */ +static int8_t config_low_g_data_src(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Configure Int data 0 register to add source of interrupt */ + rslt = bmi160_get_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_LOW_HIGH_SRC_INT_MASK; + data = temp | ((low_g_int->low_data_src << 7) & BMI160_LOW_HIGH_SRC_INT_MASK); + + /* Write data to Data 0 address */ + rslt = bmi160_set_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the necessary setting of low-g interrupt. + */ +static int8_t config_low_g_int_settg(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t data_array[3] = { 0, 0, 0 }; + + /* Configuring INT_LOWHIGH register for low-g interrupt */ + rslt = bmi160_get_regs(BMI160_INT_LOWHIGH_2_ADDR, &data_array[2], 1, dev); + if (rslt == BMI160_OK) + { + temp = data_array[2] & ~BMI160_LOW_G_HYST_MASK; + + /* Adding low-g hysteresis */ + data_array[2] = temp | (low_g_int->low_hyst & BMI160_LOW_G_HYST_MASK); + temp = data_array[2] & ~BMI160_LOW_G_LOW_MODE_MASK; + + /* Adding low-mode */ + data_array[2] = temp | ((low_g_int->low_mode << 2) & BMI160_LOW_G_LOW_MODE_MASK); + + /* Adding low-g threshold */ + data_array[1] = low_g_int->low_thres; + + /* Adding low-g interrupt delay */ + data_array[0] = low_g_int->low_dur; + + /* Writing data to INT_LOWHIGH 0,1,2 registers simultaneously*/ + rslt = bmi160_set_regs(BMI160_INT_LOWHIGH_0_ADDR, data_array, 3, dev); + } + + return rslt; +} + +/*! + * @brief This API enables the high-g interrupt. + */ +static int8_t enable_high_g_int(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable low-g interrupt in Int Enable 1 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* Adding high-g X-axis */ + temp = data & ~BMI160_HIGH_G_X_INT_EN_MASK; + data = temp | (high_g_int_cfg->high_g_x & BMI160_HIGH_G_X_INT_EN_MASK); + + /* Adding high-g Y-axis */ + temp = data & ~BMI160_HIGH_G_Y_INT_EN_MASK; + data = temp | ((high_g_int_cfg->high_g_y << 1) & BMI160_HIGH_G_Y_INT_EN_MASK); + + /* Adding high-g Z-axis */ + temp = data & ~BMI160_HIGH_G_Z_INT_EN_MASK; + data = temp | ((high_g_int_cfg->high_g_z << 2) & BMI160_HIGH_G_Z_INT_EN_MASK); + + /* write data to Int Enable 0 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for high-g interrupt. + */ +static int8_t config_high_g_data_src(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Configure Int data 0 register to add source of interrupt */ + rslt = bmi160_get_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_LOW_HIGH_SRC_INT_MASK; + data = temp | ((high_g_int_cfg->high_data_src << 7) & BMI160_LOW_HIGH_SRC_INT_MASK); + + /* Write data to Data 0 address */ + rslt = bmi160_set_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the necessary setting of high-g interrupt. + */ +static int8_t config_high_g_int_settg(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t data_array[3] = { 0, 0, 0 }; + + rslt = bmi160_get_regs(BMI160_INT_LOWHIGH_2_ADDR, &data_array[0], 1, dev); + if (rslt == BMI160_OK) + { + temp = data_array[0] & ~BMI160_HIGH_G_HYST_MASK; + + /* Adding high-g hysteresis */ + data_array[0] = temp | ((high_g_int_cfg->high_hy << 6) & BMI160_HIGH_G_HYST_MASK); + + /* Adding high-g duration */ + data_array[1] = high_g_int_cfg->high_dur; + + /* Adding high-g threshold */ + data_array[2] = high_g_int_cfg->high_thres; + rslt = bmi160_set_regs(BMI160_INT_LOWHIGH_2_ADDR, data_array, 3, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the behavioural setting of interrupt pin. + */ +static int8_t config_int_out_ctrl(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t data = 0; + + /* Configuration of output interrupt signals on pins INT1 and INT2 are + * done in BMI160_INT_OUT_CTRL_ADDR register*/ + rslt = bmi160_get_regs(BMI160_INT_OUT_CTRL_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* updating the interrupt pin structure to local structure */ + const struct bmi160_int_pin_settg *intr_pin_sett = &(int_config->int_pin_settg); + + /* Configuring channel 1 */ + if (int_config->int_channel == BMI160_INT_CHANNEL_1) + { + /* Output enable */ + temp = data & ~BMI160_INT1_OUTPUT_EN_MASK; + data = temp | ((intr_pin_sett->output_en << 3) & BMI160_INT1_OUTPUT_EN_MASK); + + /* Output mode */ + temp = data & ~BMI160_INT1_OUTPUT_MODE_MASK; + data = temp | ((intr_pin_sett->output_mode << 2) & BMI160_INT1_OUTPUT_MODE_MASK); + + /* Output type */ + temp = data & ~BMI160_INT1_OUTPUT_TYPE_MASK; + data = temp | ((intr_pin_sett->output_type << 1) & BMI160_INT1_OUTPUT_TYPE_MASK); + + /* edge control */ + temp = data & ~BMI160_INT1_EDGE_CTRL_MASK; + data = temp | ((intr_pin_sett->edge_ctrl) & BMI160_INT1_EDGE_CTRL_MASK); + } + else + { + /* Configuring channel 2 */ + /* Output enable */ + temp = data & ~BMI160_INT2_OUTPUT_EN_MASK; + data = temp | ((intr_pin_sett->output_en << 7) & BMI160_INT2_OUTPUT_EN_MASK); + + /* Output mode */ + temp = data & ~BMI160_INT2_OUTPUT_MODE_MASK; + data = temp | ((intr_pin_sett->output_mode << 6) & BMI160_INT2_OUTPUT_MODE_MASK); + + /* Output type */ + temp = data & ~BMI160_INT2_OUTPUT_TYPE_MASK; + data = temp | ((intr_pin_sett->output_type << 5) & BMI160_INT2_OUTPUT_TYPE_MASK); + + /* edge control */ + temp = data & ~BMI160_INT2_EDGE_CTRL_MASK; + data = temp | ((intr_pin_sett->edge_ctrl << 4) & BMI160_INT2_EDGE_CTRL_MASK); + } + + rslt = bmi160_set_regs(BMI160_INT_OUT_CTRL_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the mode(input enable, latch or non-latch) of interrupt pin. + */ +static int8_t config_int_latch(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t data = 0; + + /* Configuration of latch on pins INT1 and INT2 are done in + * BMI160_INT_LATCH_ADDR register*/ + rslt = bmi160_get_regs(BMI160_INT_LATCH_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* updating the interrupt pin structure to local structure */ + const struct bmi160_int_pin_settg *intr_pin_sett = &(int_config->int_pin_settg); + if (int_config->int_channel == BMI160_INT_CHANNEL_1) + { + /* Configuring channel 1 */ + /* Input enable */ + temp = data & ~BMI160_INT1_INPUT_EN_MASK; + data = temp | ((intr_pin_sett->input_en << 4) & BMI160_INT1_INPUT_EN_MASK); + } + else + { + /* Configuring channel 2 */ + /* Input enable */ + temp = data & ~BMI160_INT2_INPUT_EN_MASK; + data = temp | ((intr_pin_sett->input_en << 5) & BMI160_INT2_INPUT_EN_MASK); + } + + /* In case of latch interrupt,update the latch duration */ + + /* Latching holds the interrupt for the amount of latch + * duration time */ + temp = data & ~BMI160_INT_LATCH_MASK; + data = temp | (intr_pin_sett->latch_dur & BMI160_INT_LATCH_MASK); + + /* OUT_CTRL_INT and LATCH_INT address lie consecutively, + * hence writing data to respective registers at one go */ + rslt = bmi160_set_regs(BMI160_INT_LATCH_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API performs the self test for accelerometer of BMI160 + */ +static int8_t perform_accel_self_test(struct bmi160_dev *dev) +{ + int8_t rslt; + struct bmi160_sensor_data accel_pos, accel_neg; + + /* Enable Gyro self test bit */ + rslt = enable_accel_self_test(dev); + if (rslt == BMI160_OK) + { + /* Perform accel self test with positive excitation */ + rslt = accel_self_test_positive_excitation(&accel_pos, dev); + if (rslt == BMI160_OK) + { + /* Perform accel self test with negative excitation */ + rslt = accel_self_test_negative_excitation(&accel_neg, dev); + if (rslt == BMI160_OK) + { + /* Validate the self test result */ + rslt = validate_accel_self_test(&accel_pos, &accel_neg); + } + } + } + + return rslt; +} + +/*! + * @brief This API enables to perform the accel self test by setting proper + * configurations to facilitate accel self test + */ +static int8_t enable_accel_self_test(struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Set the Accel power mode as normal mode */ + dev->accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; + + /* Set the sensor range configuration as 8G */ + dev->accel_cfg.range = BMI160_ACCEL_RANGE_8G; + rslt = bmi160_set_sens_conf(dev); + if (rslt == BMI160_OK) + { + /* Accel configurations are set to facilitate self test + * acc_odr - 1600Hz ; acc_bwp = 2 ; acc_us = 0 */ + reg_data = BMI160_ACCEL_SELF_TEST_CONFIG; + rslt = bmi160_set_regs(BMI160_ACCEL_CONFIG_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API performs accel self test with positive excitation + */ +static int8_t accel_self_test_positive_excitation(struct bmi160_sensor_data *accel_pos, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Enable accel self test with positive self-test excitation + * and with amplitude of deflection set as high */ + reg_data = BMI160_ACCEL_SELF_TEST_POSITIVE_EN; + rslt = bmi160_set_regs(BMI160_SELF_TEST_ADDR, ®_data, 1, dev); + if (rslt == BMI160_OK) + { + /* Read the data after a delay of 50ms - refer datasheet 2.8.1 accel self test*/ + dev->delay_ms(BMI160_ACCEL_SELF_TEST_DELAY); + rslt = bmi160_get_sensor_data(BMI160_ACCEL_ONLY, accel_pos, NULL, dev); + } + + return rslt; +} + +/*! + * @brief This API performs accel self test with negative excitation + */ +static int8_t accel_self_test_negative_excitation(struct bmi160_sensor_data *accel_neg, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Enable accel self test with negative self-test excitation + * and with amplitude of deflection set as high */ + reg_data = BMI160_ACCEL_SELF_TEST_NEGATIVE_EN; + rslt = bmi160_set_regs(BMI160_SELF_TEST_ADDR, ®_data, 1, dev); + if (rslt == BMI160_OK) + { + /* Read the data after a delay of 50ms */ + dev->delay_ms(BMI160_ACCEL_SELF_TEST_DELAY); + rslt = bmi160_get_sensor_data(BMI160_ACCEL_ONLY, accel_neg, NULL, dev); + } + + return rslt; +} + +/*! + * @brief This API validates the accel self test results + */ +static int8_t validate_accel_self_test(const struct bmi160_sensor_data *accel_pos, + const struct bmi160_sensor_data *accel_neg) +{ + int8_t rslt; + + /* Validate the results of self test */ + if (((accel_neg->x - accel_pos->x) > BMI160_ACCEL_SELF_TEST_LIMIT) && + ((accel_neg->y - accel_pos->y) > BMI160_ACCEL_SELF_TEST_LIMIT) && + ((accel_neg->z - accel_pos->z) > BMI160_ACCEL_SELF_TEST_LIMIT)) + { + /* Self test pass condition */ + rslt = BMI160_OK; + } + else + { + rslt = BMI160_W_ACCEl_SELF_TEST_FAIL; + } + + return rslt; +} + +/*! + * @brief This API performs the self test for gyroscope of BMI160 + */ +static int8_t perform_gyro_self_test(const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Enable Gyro self test bit */ + rslt = enable_gyro_self_test(dev); + if (rslt == BMI160_OK) + { + /* Validate the gyro self test a delay of 50ms */ + dev->delay_ms(50); + + /* Validate the gyro self test results */ + rslt = validate_gyro_self_test(dev); + } + + return rslt; +} + +/*! + * @brief This API enables the self test bit to trigger self test for Gyro + */ +static int8_t enable_gyro_self_test(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Enable the Gyro self test bit to trigger the self test */ + rslt = bmi160_get_regs(BMI160_SELF_TEST_ADDR, ®_data, 1, dev); + if (rslt == BMI160_OK) + { + reg_data = BMI160_SET_BITS(reg_data, BMI160_GYRO_SELF_TEST, 1); + rslt = bmi160_set_regs(BMI160_SELF_TEST_ADDR, ®_data, 1, dev); + if (rslt == BMI160_OK) + { + /* Delay to enable gyro self test */ + dev->delay_ms(15); + } + } + + return rslt; +} + +/*! + * @brief This API validates the self test results of Gyro + */ +static int8_t validate_gyro_self_test(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Validate the Gyro self test result */ + rslt = bmi160_get_regs(BMI160_STATUS_ADDR, ®_data, 1, dev); + if (rslt == BMI160_OK) + { + + reg_data = BMI160_GET_BITS(reg_data, BMI160_GYRO_SELF_TEST_STATUS); + if (reg_data == BMI160_ENABLE) + { + /* Gyro self test success case */ + rslt = BMI160_OK; + } + else + { + rslt = BMI160_W_GYRO_SELF_TEST_FAIL; + } + } + + return rslt; +} + +/*! + * @brief This API sets FIFO full interrupt of the sensor.This interrupt + * occurs when the FIFO is full and the next full data sample would cause + * a FIFO overflow, which may delete the old samples. + */ +static int8_t set_fifo_full_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /*enable the fifo full interrupt */ + rslt = enable_fifo_full_int(int_config, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_hardware_interrupt(int_config, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This enable the FIFO full interrupt engine. + */ +static int8_t enable_fifo_full_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + + rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + data = BMI160_SET_BITS(data, BMI160_FIFO_FULL_INT, int_config->fifo_full_int_en); + + /* Writing data to INT ENABLE 1 Address */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API sets FIFO watermark interrupt of the sensor.The FIFO + * watermark interrupt is fired, when the FIFO fill level is above a fifo + * watermark. + */ +static int8_t set_fifo_watermark_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Enable fifo-watermark interrupt in Int Enable 1 register */ + rslt = enable_fifo_wtm_int(int_config, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_hardware_interrupt(int_config, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This enable the FIFO watermark interrupt engine. + */ +static int8_t enable_fifo_wtm_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + + rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + data = BMI160_SET_BITS(data, BMI160_FIFO_WTM_INT, int_config->fifo_wtm_int_en); + + /* Writing data to INT ENABLE 1 Address */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API is used to reset the FIFO related configurations + * in the fifo_frame structure. + */ +static void reset_fifo_data_structure(const struct bmi160_dev *dev) +{ + /*Prepare for next FIFO read by resetting FIFO's + * internal data structures*/ + dev->fifo->accel_byte_start_idx = 0; + dev->fifo->gyro_byte_start_idx = 0; + dev->fifo->aux_byte_start_idx = 0; + dev->fifo->sensor_time = 0; + dev->fifo->skipped_frame_count = 0; +} + +/*! + * @brief This API is used to read fifo_byte_counter value (i.e) + * current fill-level in Fifo buffer. + */ +static int8_t get_fifo_byte_counter(uint16_t *bytes_to_read, struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + uint8_t data[2]; + uint8_t addr = BMI160_FIFO_LENGTH_ADDR; + + rslt = bmi160_get_regs(addr, data, 2, dev); + if (rslt != BMI160_OK) + { + return rslt; + } + data[1] = data[1] & BMI160_FIFO_BYTE_COUNTER_MASK; + + /* Available data in FIFO is stored in bytes_to_read*/ + *bytes_to_read = (((uint16_t)data[1] << 8) | ((uint16_t)data[0])); + + return rslt; +} + +/*! + * @brief This API is used to compute the number of bytes of accel FIFO data + * which is to be parsed in header-less mode + */ +static void get_accel_len_to_parse(uint16_t *data_index, + uint16_t *data_read_length, + const uint8_t *acc_frame_count, + const struct bmi160_dev *dev) +{ + /* Data start index */ + *data_index = dev->fifo->accel_byte_start_idx; + if (dev->fifo->fifo_data_enable == BMI160_FIFO_A_ENABLE) + { + *data_read_length = (*acc_frame_count) * BMI160_FIFO_A_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_G_A_ENABLE) + { + *data_read_length = (*acc_frame_count) * BMI160_FIFO_GA_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_A_ENABLE) + { + *data_read_length = (*acc_frame_count) * BMI160_FIFO_MA_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_A_ENABLE) + { + *data_read_length = (*acc_frame_count) * BMI160_FIFO_MGA_LENGTH; + } + else + { + /* When accel is not enabled ,there will be no accel data. + * so we update the data index as complete */ + *data_index = dev->fifo->length; + } + + if (*data_read_length > dev->fifo->length) + { + /* Handling the case where more data is requested + * than that is available*/ + *data_read_length = dev->fifo->length; + } +} + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data in both header mode and header-less mode. + * It updates the idx value which is used to store the index of + * the current data byte which is parsed. + */ +static void unpack_accel_frame(struct bmi160_sensor_data *acc, + uint16_t *idx, + uint8_t *acc_idx, + uint8_t frame_info, + const struct bmi160_dev *dev) +{ + switch (frame_info) + { + case BMI160_FIFO_HEAD_A: + case BMI160_FIFO_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_A_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into the structure instance "acc" */ + unpack_accel_data(&acc[*acc_idx], *idx, dev); + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_A_LENGTH; + (*acc_idx)++; + break; + case BMI160_FIFO_HEAD_G_A: + case BMI160_FIFO_G_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_GA_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into structure instance "acc"*/ + unpack_accel_data(&acc[*acc_idx], *idx + BMI160_FIFO_G_LENGTH, dev); + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_GA_LENGTH; + (*acc_idx)++; + break; + case BMI160_FIFO_HEAD_M_A: + case BMI160_FIFO_M_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_MA_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into structure instance "acc"*/ + unpack_accel_data(&acc[*acc_idx], *idx + BMI160_FIFO_M_LENGTH, dev); + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_MA_LENGTH; + (*acc_idx)++; + break; + case BMI160_FIFO_HEAD_M_G_A: + case BMI160_FIFO_M_G_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_MGA_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into structure instance "acc"*/ + unpack_accel_data(&acc[*acc_idx], *idx + BMI160_FIFO_MG_LENGTH, dev); + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_MGA_LENGTH; + (*acc_idx)++; + break; + case BMI160_FIFO_HEAD_M: + case BMI160_FIFO_M_ENABLE: + (*idx) = (*idx) + BMI160_FIFO_M_LENGTH; + break; + case BMI160_FIFO_HEAD_G: + case BMI160_FIFO_G_ENABLE: + (*idx) = (*idx) + BMI160_FIFO_G_LENGTH; + break; + case BMI160_FIFO_HEAD_M_G: + case BMI160_FIFO_M_G_ENABLE: + (*idx) = (*idx) + BMI160_FIFO_MG_LENGTH; + break; + default: + break; + } +} + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data and store it in the instance of the structure bmi160_sensor_data. + */ +static void unpack_accel_data(struct bmi160_sensor_data *accel_data, + uint16_t data_start_index, + const struct bmi160_dev *dev) +{ + uint16_t data_lsb; + uint16_t data_msb; + + /* Accel raw x data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + accel_data->x = (int16_t)((data_msb << 8) | data_lsb); + + /* Accel raw y data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + accel_data->y = (int16_t)((data_msb << 8) | data_lsb); + + /* Accel raw z data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + accel_data->z = (int16_t)((data_msb << 8) | data_lsb); +} + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data in header mode. + */ +static void extract_accel_header_mode(struct bmi160_sensor_data *accel_data, + uint8_t *accel_length, + const struct bmi160_dev *dev) +{ + uint8_t frame_header = 0; + uint16_t data_index; + uint8_t accel_index = 0; + + for (data_index = dev->fifo->accel_byte_start_idx; data_index < dev->fifo->length;) + { + /* extracting Frame header */ + frame_header = (dev->fifo->data[data_index] & BMI160_FIFO_TAG_INTR_MASK); + + /*Index is moved to next byte where the data is starting*/ + data_index++; + switch (frame_header) + { + /* Accel frame */ + case BMI160_FIFO_HEAD_A: + case BMI160_FIFO_HEAD_M_A: + case BMI160_FIFO_HEAD_G_A: + case BMI160_FIFO_HEAD_M_G_A: + unpack_accel_frame(accel_data, &data_index, &accel_index, frame_header, dev); + break; + case BMI160_FIFO_HEAD_M: + move_next_frame(&data_index, BMI160_FIFO_M_LENGTH, dev); + break; + case BMI160_FIFO_HEAD_G: + move_next_frame(&data_index, BMI160_FIFO_G_LENGTH, dev); + break; + case BMI160_FIFO_HEAD_M_G: + move_next_frame(&data_index, BMI160_FIFO_MG_LENGTH, dev); + break; + + /* Sensor time frame */ + case BMI160_FIFO_HEAD_SENSOR_TIME: + unpack_sensortime_frame(&data_index, dev); + break; + + /* Skip frame */ + case BMI160_FIFO_HEAD_SKIP_FRAME: + unpack_skipped_frame(&data_index, dev); + break; + + /* Input config frame */ + case BMI160_FIFO_HEAD_INPUT_CONFIG: + move_next_frame(&data_index, 1, dev); + break; + case BMI160_FIFO_HEAD_OVER_READ: + + /* Update the data index as complete in case of Over read */ + data_index = dev->fifo->length; + break; + default: + break; + } + if (*accel_length == accel_index) + { + /* Number of frames to read completed */ + break; + } + } + + /*Update number of accel data read*/ + *accel_length = accel_index; + + /*Update the accel frame index*/ + dev->fifo->accel_byte_start_idx = data_index; +} + +/*! + * @brief This API computes the number of bytes of gyro FIFO data + * which is to be parsed in header-less mode + */ +static void get_gyro_len_to_parse(uint16_t *data_index, + uint16_t *data_read_length, + const uint8_t *gyro_frame_count, + const struct bmi160_dev *dev) +{ + /* Data start index */ + *data_index = dev->fifo->gyro_byte_start_idx; + if (dev->fifo->fifo_data_enable == BMI160_FIFO_G_ENABLE) + { + *data_read_length = (*gyro_frame_count) * BMI160_FIFO_G_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_G_A_ENABLE) + { + *data_read_length = (*gyro_frame_count) * BMI160_FIFO_GA_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_ENABLE) + { + *data_read_length = (*gyro_frame_count) * BMI160_FIFO_MG_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_A_ENABLE) + { + *data_read_length = (*gyro_frame_count) * BMI160_FIFO_MGA_LENGTH; + } + else + { + /* When gyro is not enabled ,there will be no gyro data. + * so we update the data index as complete */ + *data_index = dev->fifo->length; + } + + if (*data_read_length > dev->fifo->length) + { + /* Handling the case where more data is requested + * than that is available*/ + *data_read_length = dev->fifo->length; + } +} + +/*! + * @brief This API is used to parse the gyroscope's data from the + * FIFO data in both header mode and header-less mode. + * It updates the idx value which is used to store the index of + * the current data byte which is parsed. + */ +static void unpack_gyro_frame(struct bmi160_sensor_data *gyro, + uint16_t *idx, + uint8_t *gyro_idx, + uint8_t frame_info, + const struct bmi160_dev *dev) +{ + switch (frame_info) + { + case BMI160_FIFO_HEAD_G: + case BMI160_FIFO_G_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_G_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into structure instance "gyro"*/ + unpack_gyro_data(&gyro[*gyro_idx], *idx, dev); + + /*Move the data index*/ + (*idx) = (*idx) + BMI160_FIFO_G_LENGTH; + (*gyro_idx)++; + break; + case BMI160_FIFO_HEAD_G_A: + case BMI160_FIFO_G_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_GA_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /* Unpack the data array into structure instance "gyro" */ + unpack_gyro_data(&gyro[*gyro_idx], *idx, dev); + + /* Move the data index */ + *idx = *idx + BMI160_FIFO_GA_LENGTH; + (*gyro_idx)++; + break; + case BMI160_FIFO_HEAD_M_G_A: + case BMI160_FIFO_M_G_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_MGA_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into structure instance "gyro"*/ + unpack_gyro_data(&gyro[*gyro_idx], *idx + BMI160_FIFO_M_LENGTH, dev); + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_MGA_LENGTH; + (*gyro_idx)++; + break; + case BMI160_FIFO_HEAD_M_A: + case BMI160_FIFO_M_A_ENABLE: + + /* Move the data index */ + *idx = *idx + BMI160_FIFO_MA_LENGTH; + break; + case BMI160_FIFO_HEAD_M: + case BMI160_FIFO_M_ENABLE: + (*idx) = (*idx) + BMI160_FIFO_M_LENGTH; + break; + case BMI160_FIFO_HEAD_M_G: + case BMI160_FIFO_M_G_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_MG_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into structure instance "gyro"*/ + unpack_gyro_data(&gyro[*gyro_idx], *idx + BMI160_FIFO_M_LENGTH, dev); + + /*Move the data index*/ + (*idx) = (*idx) + BMI160_FIFO_MG_LENGTH; + (*gyro_idx)++; + break; + case BMI160_FIFO_HEAD_A: + case BMI160_FIFO_A_ENABLE: + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_A_LENGTH; + break; + default: + break; + } +} + +/*! + * @brief This API is used to parse the gyro data from the + * FIFO data and store it in the instance of the structure bmi160_sensor_data. + */ +static void unpack_gyro_data(struct bmi160_sensor_data *gyro_data, + uint16_t data_start_index, + const struct bmi160_dev *dev) +{ + uint16_t data_lsb; + uint16_t data_msb; + + /* Gyro raw x data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + gyro_data->x = (int16_t)((data_msb << 8) | data_lsb); + + /* Gyro raw y data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + gyro_data->y = (int16_t)((data_msb << 8) | data_lsb); + + /* Gyro raw z data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + gyro_data->z = (int16_t)((data_msb << 8) | data_lsb); +} + +/*! + * @brief This API is used to parse the gyro data from the + * FIFO data in header mode. + */ +static void extract_gyro_header_mode(struct bmi160_sensor_data *gyro_data, + uint8_t *gyro_length, + const struct bmi160_dev *dev) +{ + uint8_t frame_header = 0; + uint16_t data_index; + uint8_t gyro_index = 0; + + for (data_index = dev->fifo->gyro_byte_start_idx; data_index < dev->fifo->length;) + { + /* extracting Frame header */ + frame_header = (dev->fifo->data[data_index] & BMI160_FIFO_TAG_INTR_MASK); + + /*Index is moved to next byte where the data is starting*/ + data_index++; + switch (frame_header) + { + /* GYRO frame */ + case BMI160_FIFO_HEAD_G: + case BMI160_FIFO_HEAD_G_A: + case BMI160_FIFO_HEAD_M_G: + case BMI160_FIFO_HEAD_M_G_A: + unpack_gyro_frame(gyro_data, &data_index, &gyro_index, frame_header, dev); + break; + case BMI160_FIFO_HEAD_A: + move_next_frame(&data_index, BMI160_FIFO_A_LENGTH, dev); + break; + case BMI160_FIFO_HEAD_M: + move_next_frame(&data_index, BMI160_FIFO_M_LENGTH, dev); + break; + case BMI160_FIFO_HEAD_M_A: + move_next_frame(&data_index, BMI160_FIFO_M_LENGTH, dev); + break; + + /* Sensor time frame */ + case BMI160_FIFO_HEAD_SENSOR_TIME: + unpack_sensortime_frame(&data_index, dev); + break; + + /* Skip frame */ + case BMI160_FIFO_HEAD_SKIP_FRAME: + unpack_skipped_frame(&data_index, dev); + break; + + /* Input config frame */ + case BMI160_FIFO_HEAD_INPUT_CONFIG: + move_next_frame(&data_index, 1, dev); + break; + case BMI160_FIFO_HEAD_OVER_READ: + + /* Update the data index as complete in case of over read */ + data_index = dev->fifo->length; + break; + default: + break; + } + if (*gyro_length == gyro_index) + { + /*Number of frames to read completed*/ + break; + } + } + + /*Update number of gyro data read*/ + *gyro_length = gyro_index; + + /*Update the gyro frame index*/ + dev->fifo->gyro_byte_start_idx = data_index; +} + +/*! + * @brief This API computes the number of bytes of aux FIFO data + * which is to be parsed in header-less mode + */ +static void get_aux_len_to_parse(uint16_t *data_index, + uint16_t *data_read_length, + const uint8_t *aux_frame_count, + const struct bmi160_dev *dev) +{ + /* Data start index */ + *data_index = dev->fifo->gyro_byte_start_idx; + if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_ENABLE) + { + *data_read_length = (*aux_frame_count) * BMI160_FIFO_M_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_A_ENABLE) + { + *data_read_length = (*aux_frame_count) * BMI160_FIFO_MA_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_ENABLE) + { + *data_read_length = (*aux_frame_count) * BMI160_FIFO_MG_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_A_ENABLE) + { + *data_read_length = (*aux_frame_count) * BMI160_FIFO_MGA_LENGTH; + } + else + { + /* When aux is not enabled ,there will be no aux data. + * so we update the data index as complete */ + *data_index = dev->fifo->length; + } + + if (*data_read_length > dev->fifo->length) + { + /* Handling the case where more data is requested + * than that is available */ + *data_read_length = dev->fifo->length; + } +} + +/*! + * @brief This API is used to parse the aux's data from the + * FIFO data in both header mode and header-less mode. + * It updates the idx value which is used to store the index of + * the current data byte which is parsed + */ +static void unpack_aux_frame(struct bmi160_aux_data *aux_data, + uint16_t *idx, + uint8_t *aux_index, + uint8_t frame_info, + const struct bmi160_dev *dev) +{ + switch (frame_info) + { + case BMI160_FIFO_HEAD_M: + case BMI160_FIFO_M_ENABLE: + + /* Partial read, then skip the data */ + if ((*idx + BMI160_FIFO_M_LENGTH) > dev->fifo->length) + { + /* Update the data index as complete */ + *idx = dev->fifo->length; + break; + } + + /* Unpack the data array into structure instance */ + unpack_aux_data(&aux_data[*aux_index], *idx, dev); + + /* Move the data index */ + *idx = *idx + BMI160_FIFO_M_LENGTH; + (*aux_index)++; + break; + case BMI160_FIFO_HEAD_M_A: + case BMI160_FIFO_M_A_ENABLE: + + /* Partial read, then skip the data */ + if ((*idx + BMI160_FIFO_MA_LENGTH) > dev->fifo->length) + { + /* Update the data index as complete */ + *idx = dev->fifo->length; + break; + } + + /* Unpack the data array into structure instance */ + unpack_aux_data(&aux_data[*aux_index], *idx, dev); + + /* Move the data index */ + *idx = *idx + BMI160_FIFO_MA_LENGTH; + (*aux_index)++; + break; + case BMI160_FIFO_HEAD_M_G: + case BMI160_FIFO_M_G_ENABLE: + + /* Partial read, then skip the data */ + if ((*idx + BMI160_FIFO_MG_LENGTH) > dev->fifo->length) + { + /* Update the data index as complete */ + *idx = dev->fifo->length; + break; + } + + /* Unpack the data array into structure instance */ + unpack_aux_data(&aux_data[*aux_index], *idx, dev); + + /* Move the data index */ + (*idx) = (*idx) + BMI160_FIFO_MG_LENGTH; + (*aux_index)++; + break; + case BMI160_FIFO_HEAD_M_G_A: + case BMI160_FIFO_M_G_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_MGA_LENGTH) > dev->fifo->length) + { + /* Update the data index as complete */ + *idx = dev->fifo->length; + break; + } + + /* Unpack the data array into structure instance */ + unpack_aux_data(&aux_data[*aux_index], *idx, dev); + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_MGA_LENGTH; + (*aux_index)++; + break; + case BMI160_FIFO_HEAD_G: + case BMI160_FIFO_G_ENABLE: + + /* Move the data index */ + (*idx) = (*idx) + BMI160_FIFO_G_LENGTH; + break; + case BMI160_FIFO_HEAD_G_A: + case BMI160_FIFO_G_A_ENABLE: + + /* Move the data index */ + *idx = *idx + BMI160_FIFO_GA_LENGTH; + break; + case BMI160_FIFO_HEAD_A: + case BMI160_FIFO_A_ENABLE: + + /* Move the data index */ + *idx = *idx + BMI160_FIFO_A_LENGTH; + break; + default: + break; + } +} + +/*! + * @brief This API is used to parse the aux data from the + * FIFO data and store it in the instance of the structure bmi160_aux_data. + */ +static void unpack_aux_data(struct bmi160_aux_data *aux_data, uint16_t data_start_index, const struct bmi160_dev *dev) +{ + /* Aux data bytes */ + aux_data->data[0] = dev->fifo->data[data_start_index++]; + aux_data->data[1] = dev->fifo->data[data_start_index++]; + aux_data->data[2] = dev->fifo->data[data_start_index++]; + aux_data->data[3] = dev->fifo->data[data_start_index++]; + aux_data->data[4] = dev->fifo->data[data_start_index++]; + aux_data->data[5] = dev->fifo->data[data_start_index++]; + aux_data->data[6] = dev->fifo->data[data_start_index++]; + aux_data->data[7] = dev->fifo->data[data_start_index++]; +} + +/*! + * @brief This API is used to parse the aux data from the + * FIFO data in header mode. + */ +static void extract_aux_header_mode(struct bmi160_aux_data *aux_data, uint8_t *aux_length, const struct bmi160_dev *dev) +{ + uint8_t frame_header = 0; + uint16_t data_index; + uint8_t aux_index = 0; + + for (data_index = dev->fifo->aux_byte_start_idx; data_index < dev->fifo->length;) + { + /* extracting Frame header */ + frame_header = (dev->fifo->data[data_index] & BMI160_FIFO_TAG_INTR_MASK); + + /*Index is moved to next byte where the data is starting*/ + data_index++; + switch (frame_header) + { + /* Aux frame */ + case BMI160_FIFO_HEAD_M: + case BMI160_FIFO_HEAD_M_A: + case BMI160_FIFO_HEAD_M_G: + case BMI160_FIFO_HEAD_M_G_A: + unpack_aux_frame(aux_data, &data_index, &aux_index, frame_header, dev); + break; + case BMI160_FIFO_HEAD_G: + move_next_frame(&data_index, BMI160_FIFO_G_LENGTH, dev); + break; + case BMI160_FIFO_HEAD_G_A: + move_next_frame(&data_index, BMI160_FIFO_GA_LENGTH, dev); + break; + case BMI160_FIFO_HEAD_A: + move_next_frame(&data_index, BMI160_FIFO_A_LENGTH, dev); + break; + + /* Sensor time frame */ + case BMI160_FIFO_HEAD_SENSOR_TIME: + unpack_sensortime_frame(&data_index, dev); + break; + + /* Skip frame */ + case BMI160_FIFO_HEAD_SKIP_FRAME: + unpack_skipped_frame(&data_index, dev); + break; + + /* Input config frame */ + case BMI160_FIFO_HEAD_INPUT_CONFIG: + move_next_frame(&data_index, 1, dev); + break; + case BMI160_FIFO_HEAD_OVER_READ: + + /* Update the data index as complete in case + * of over read */ + data_index = dev->fifo->length; + break; + default: + + /* Update the data index as complete in case of + * getting other headers like 0x00 */ + data_index = dev->fifo->length; + break; + } + if (*aux_length == aux_index) + { + /*Number of frames to read completed*/ + break; + } + } + + /* Update number of aux data read */ + *aux_length = aux_index; + + /* Update the aux frame index */ + dev->fifo->aux_byte_start_idx = data_index; +} + +/*! + * @brief This API checks the presence of non-valid frames in the read fifo data. + */ +static void check_frame_validity(uint16_t *data_index, const struct bmi160_dev *dev) +{ + if ((*data_index + 2) < dev->fifo->length) + { + /* Check if FIFO is empty */ + if ((dev->fifo->data[*data_index] == FIFO_CONFIG_MSB_CHECK) && + (dev->fifo->data[*data_index + 1] == FIFO_CONFIG_LSB_CHECK)) + { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } + } +} + +/*! + * @brief This API is used to move the data index ahead of the + * current_frame_length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + */ +static void move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi160_dev *dev) +{ + /*Partial read, then move the data index to last data*/ + if ((*data_index + current_frame_length) > dev->fifo->length) + { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } + else + { + /*Move the data index to next frame*/ + *data_index = *data_index + current_frame_length; + } +} + +/*! + * @brief This API is used to parse and store the sensor time from the + * FIFO data in the structure instance dev. + */ +static void unpack_sensortime_frame(uint16_t *data_index, const struct bmi160_dev *dev) +{ + uint32_t sensor_time_byte3 = 0; + uint16_t sensor_time_byte2 = 0; + uint8_t sensor_time_byte1 = 0; + + /*Partial read, then move the data index to last data*/ + if ((*data_index + BMI160_SENSOR_TIME_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } + else + { + sensor_time_byte3 = dev->fifo->data[(*data_index) + BMI160_SENSOR_TIME_MSB_BYTE] << 16; + sensor_time_byte2 = dev->fifo->data[(*data_index) + BMI160_SENSOR_TIME_XLSB_BYTE] << 8; + sensor_time_byte1 = dev->fifo->data[(*data_index)]; + + /* Sensor time */ + dev->fifo->sensor_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); + *data_index = (*data_index) + BMI160_SENSOR_TIME_LENGTH; + } +} + +/*! + * @brief This API is used to parse and store the skipped_frame_count from + * the FIFO data in the structure instance dev. + */ +static void unpack_skipped_frame(uint16_t *data_index, const struct bmi160_dev *dev) +{ + /*Partial read, then move the data index to last data*/ + if (*data_index >= dev->fifo->length) + { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } + else + { + dev->fifo->skipped_frame_count = dev->fifo->data[*data_index]; + + /*Move the data index*/ + *data_index = (*data_index) + 1; + } +} + +/*! + * @brief This API is used to get the FOC status from the sensor + */ +static int8_t get_foc_status(uint8_t *foc_status, struct bmi160_dev const *dev) +{ + int8_t rslt; + uint8_t data; + + /* Read the FOC status from sensor */ + rslt = bmi160_get_regs(BMI160_STATUS_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* Get the foc_status bit */ + *foc_status = BMI160_GET_BITS(data, BMI160_FOC_STATUS); + } + + return rslt; +} + +/*! + * @brief This API is used to configure the offset enable bits in the sensor + */ +static int8_t configure_offset_enable(const struct bmi160_foc_conf *foc_conf, struct bmi160_dev const *dev) +{ + int8_t rslt; + uint8_t data; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Read the FOC config from the sensor */ + rslt = bmi160_get_regs(BMI160_OFFSET_CONF_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* Set the offset enable/disable for gyro */ + data = BMI160_SET_BITS(data, BMI160_GYRO_OFFSET_EN, foc_conf->gyro_off_en); + + /* Set the offset enable/disable for accel */ + data = BMI160_SET_BITS(data, BMI160_ACCEL_OFFSET_EN, foc_conf->acc_off_en); + + /* Set the offset config in the sensor */ + rslt = bmi160_set_regs(BMI160_OFFSET_CONF_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +static int8_t trigger_foc(struct bmi160_offsets *offset, struct bmi160_dev const *dev) +{ + int8_t rslt; + uint8_t foc_status = BMI160_ENABLE; + uint8_t cmd = BMI160_START_FOC_CMD; + uint8_t timeout = 0; + uint8_t data_array[20]; + + /* Start the FOC process */ + rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &cmd, 1, dev); + if (rslt == BMI160_OK) + { + /* Check the FOC status*/ + rslt = get_foc_status(&foc_status, dev); + + if ((rslt != BMI160_OK) || (foc_status != BMI160_ENABLE)) + { + while ((foc_status != BMI160_ENABLE) && (timeout < 11)) + { + /* Maximum time of 250ms is given in 10 + * steps of 25ms each - 250ms refer datasheet 2.9.1 */ + dev->delay_ms(25); + + /* Check the FOC status*/ + rslt = get_foc_status(&foc_status, dev); + timeout++; + } + + if ((rslt == BMI160_OK) && (foc_status == BMI160_ENABLE)) + { + /* Get offset values from sensor */ + rslt = bmi160_get_offsets(offset, dev); + } + else + { + /* FOC failure case */ + rslt = BMI160_E_FOC_FAILURE; + } + } + + if (rslt == BMI160_OK) + { + /* Read registers 0x04-0x17 */ + rslt = bmi160_get_regs(BMI160_GYRO_DATA_ADDR, data_array, 20, dev); + } + } + + return rslt; +} diff --git a/src/SensorManager/BMX160/bosch/bmi160.h b/src/SensorManager/BMX160/bosch/bmi160.h new file mode 100644 index 00000000..add21427 --- /dev/null +++ b/src/SensorManager/BMX160/bosch/bmi160.h @@ -0,0 +1,969 @@ +/** +* Copyright (c) 2021 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi160.h +* @date 2021-10-05 +* @version v3.9.2 +* +*/ + +/*! + * @defgroup bmi160 BMI160 + */ + +#ifndef BMI160_H_ +#define BMI160_H_ + +/*************************** C++ guard macro *****************************/ +#ifdef __cplusplus +extern "C" { +#endif + +#include "bmi160_defs.h" +#ifdef __KERNEL__ +#include +#else +#include +#include +#include +#endif + +/*********************** User function prototypes ************************/ + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmi160ApiInit + * \page bmi160_api_bmi160_init bmi160_init + * \code + * int8_t bmi160_init(struct bmi160_dev *dev); + * \endcode + * @details This API is the entry point for sensor.It performs + * the selection of I2C/SPI read mechanism according to the + * selected interface and reads the chip-id of bmi160 sensor. + * + * @param[in,out] dev : Structure instance of bmi160_dev + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_init(struct bmi160_dev *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiRegs Registers + * @brief Read data from the given register address of sensor + */ + +/*! + * \ingroup bmi160ApiRegs + * \page bmi160_api_bmi160_get_regs bmi160_get_regs + * \code + * int8_t bmi160_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev); + * \endcode + * @details This API reads the data from the given register address of sensor. + * + * @param[in] reg_addr : Register address from where the data to be read + * @param[out] data : Pointer to data buffer to store the read data. + * @param[in] len : No of bytes of data to be read. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note For most of the registers auto address increment applies, with the + * exception of a few special registers, which trap the address. For e.g., + * Register address - 0x24(BMI160_FIFO_DATA_ADDR) + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiRegs + * \page bmi160_api_bmi160_set_regs bmi160_set_regs + * \code + * int8_t bmi160_set_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev); + * \endcode + * @details This API writes the given data to the register address + * of sensor. + * + * @param[in] reg_addr : Register address from where the data to be written. + * @param[in] data : Pointer to data buffer which is to be written + * in the sensor. + * @param[in] len : No of bytes of data to write.. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_set_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiSoftreset Soft reset + * @brief Perform soft reset of the sensor + */ + +/*! + * \ingroup bmi160ApiSoftreset + * \page bmi160_api_bmi160_soft_reset bmi160_soft_reset + * \code + * int8_t bmi160_soft_reset(struct bmi160_dev *dev); + * \endcode + * @details This API resets and restarts the device. + * All register values are overwritten with default parameters. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_soft_reset(struct bmi160_dev *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiConfig Configuration + * @brief Configuration of the sensor + */ + +/*! + * \ingroup bmi160ApiConfig + * \page bmi160_api_bmi160_set_sens_conf bmi160_set_sens_conf + * \code + * int8_t bmi160_set_sens_conf(struct bmi160_dev *dev); + * \endcode + * @details This API configures the power mode, range and bandwidth + * of sensor. + * + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_set_sens_conf(struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiConfig + * \page bmi160_api_bmi160_get_sens_conf bmi160_get_sens_conf + * \code + * int8_t bmi160_get_sens_conf(struct bmi160_dev *dev); + * \endcode + * @details This API gets accel and gyro configurations. + * + * @param[out] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_get_sens_conf(struct bmi160_dev *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiPowermode Power mode + * @brief Set / Get power mode of the sensor + */ + +/*! + * \ingroup bmi160ApiPowermode + * \page bmi160_api_bmi160_set_power_mode bmi160_set_power_mode + * \code + * int8_t bmi160_set_power_mode(struct bmi160_dev *dev); + * \endcode + * @details This API sets the power mode of the sensor. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_set_power_mode(struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiPowermode + * \page bmi160_api_bmi160_get_power_mode bmi160_get_power_mode + * \code + * int8_t bmi160_get_power_mode(struct bmi160_dev *dev); + * \endcode + * @details This API gets the power mode of the sensor. + * + * @param[in] dev : Structure instance of bmi160_dev + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_get_power_mode(struct bmi160_dev *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiData Sensor Data + * @brief Read sensor data + */ + +/*! + * \ingroup bmi160ApiData + * \page bmi160_api_bmi160_get_sensor_data bmi160_get_sensor_data + * \code + * int8_t bmi160_get_sensor_data(uint8_t select_sensor, + * struct bmi160_sensor_data *accel, + * struct bmi160_sensor_data *gyro, + * const struct bmi160_dev *dev); + * + * \endcode + * @details This API reads sensor data, stores it in + * the bmi160_sensor_data structure pointer passed by the user. + * The user can ask for accel data ,gyro data or both sensor + * data using bmi160_select_sensor enum + * + * @param[in] select_sensor : enum to choose accel,gyro or both sensor data + * @param[out] accel : Structure pointer to store accel data + * @param[out] gyro : Structure pointer to store gyro data + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_get_sensor_data(uint8_t select_sensor, + struct bmi160_sensor_data *accel, + struct bmi160_sensor_data *gyro, + const struct bmi160_dev *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiInt Interrupt configuration + * @brief Set interrupt configuration of the sensor + */ + +/*! + * \ingroup bmi160ApiInt + * \page bmi160_api_bmi160_set_int_config bmi160_set_int_config + * \code + * int8_t bmi160_set_int_config(struct bmi160_int_settg *int_config, struct bmi160_dev *dev); + * \endcode + * @details This API configures the necessary interrupt based on + * the user settings in the bmi160_int_settg structure instance. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_set_int_config(struct bmi160_int_settg *int_config, struct bmi160_dev *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiStepC Step counter + * @brief Step counter operations + */ + +/*! + * \ingroup bmi160ApiStepC + * \page bmi160_api_bmi160_set_step_counter bmi160_set_step_counter + * \code + * int8_t bmi160_set_step_counter(uint8_t step_cnt_enable, const struct bmi160_dev *dev); + * \endcode + * @details This API enables the step counter feature. + * + * @param[in] step_cnt_enable : value to enable or disable + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_set_step_counter(uint8_t step_cnt_enable, const struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiStepC + * \page bmi160_api_bmi160_read_step_counter bmi160_read_step_counter + * \code + * int8_t bmi160_read_step_counter(uint16_t *step_val, const struct bmi160_dev *dev); + * \endcode + * @details This API reads the step counter value. + * + * @param[in] step_val : Pointer to store the step counter value. + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_read_step_counter(uint16_t *step_val, const struct bmi160_dev *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiAux Auxiliary sensor + * @brief Auxiliary sensor operations + */ + +/*! + * \ingroup bmi160ApiAux + * \page bmi160_api_bmi160_aux_read bmi160_aux_read + * \code + * int8_t bmi160_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev); + * \endcode + * @details This API reads the mention no of byte of data from the given + * register address of auxiliary sensor. + * + * @param[in] reg_addr : Address of register to read. + * @param[in] aux_data : Pointer to store the read data. + * @param[in] len : No of bytes to read. + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiAux + * \page bmi160_api_bmi160_aux_write bmi160_aux_write + * \code + * int8_t bmi160_aux_write(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev); + * \endcode + * @details This API writes the mention no of byte of data to the given + * register address of auxiliary sensor. + * + * @param[in] reg_addr : Address of register to write. + * @param[in] aux_data : Pointer to write data. + * @param[in] len : No of bytes to write. + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_aux_write(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiAux + * \page bmi160_api_bmi160_aux_init bmi160_aux_init + * \code + * int8_t bmi160_aux_init(const struct bmi160_dev *dev); + * \endcode + * @details This API initialize the auxiliary sensor + * in order to access it. + * + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_aux_init(const struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiAux + * \page bmi160_api_bmi160_set_aux_auto_mode bmi160_set_aux_auto_mode + * \code + * int8_t bmi160_set_aux_auto_mode(uint8_t *data_addr, struct bmi160_dev *dev); + * \endcode + * @details This API is used to setup the auxiliary sensor of bmi160 in auto mode + * Thus enabling the auto update of 8 bytes of data from auxiliary sensor + * to BMI160 register address 0x04 to 0x0B + * + * @param[in] data_addr : Starting address of aux. sensor's data register + * (BMI160 registers 0x04 to 0x0B will be updated + * with 8 bytes of data from auxiliary sensor + * starting from this register address.) + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note : Set the value of auxiliary polling rate by setting + * dev->aux_cfg.aux_odr to the required value from the table + * before calling this API + * + *@verbatim + * dev->aux_cfg.aux_odr | Auxiliary ODR (Hz) + * -----------------------|----------------------- + * BMI160_AUX_ODR_0_78HZ | 25/32 + * BMI160_AUX_ODR_1_56HZ | 25/16 + * BMI160_AUX_ODR_3_12HZ | 25/8 + * BMI160_AUX_ODR_6_25HZ | 25/4 + * BMI160_AUX_ODR_12_5HZ | 25/2 + * BMI160_AUX_ODR_25HZ | 25 + * BMI160_AUX_ODR_50HZ | 50 + * BMI160_AUX_ODR_100HZ | 100 + * BMI160_AUX_ODR_200HZ | 200 + * BMI160_AUX_ODR_400HZ | 400 + * BMI160_AUX_ODR_800HZ | 800 + *@endverbatim + * + * @note : Other values of dev->aux_cfg.aux_odr are reserved and not for use + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_set_aux_auto_mode(uint8_t *data_addr, struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiAux + * \page bmi160_api_bmi160_config_aux_mode bmi160_config_aux_mode + * \code + * int8_t bmi160_config_aux_mode(const struct bmi160_dev *dev); + * \endcode + * @details This API configures the 0x4C register and settings like + * Auxiliary sensor manual enable/ disable and aux burst read length. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_config_aux_mode(const struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiAux + * \page bmi160_api_bmi160_read_aux_data_auto_mode bmi160_read_aux_data_auto_mode + * \code + * int8_t bmi160_read_aux_data_auto_mode(uint8_t *aux_data, const struct bmi160_dev *dev); + * \endcode + * @details This API is used to read the raw uncompensated auxiliary sensor + * data of 8 bytes from BMI160 register address 0x04 to 0x0B + * + * @param[in] aux_data : Pointer to user array of length 8 bytes + * Ensure that the aux_data array is of + * length 8 bytes + * @param[in] dev : Structure instance of bmi160_dev + * + * @retval zero -> Success / -ve value -> Error + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_read_aux_data_auto_mode(uint8_t *aux_data, const struct bmi160_dev *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiSelfTest Self test + * @brief Perform self test of the sensor + */ + +/*! + * \ingroup bmi160ApiSelfTest + * \page bmi160_api_bmi160_perform_self_test bmi160_perform_self_test + * \code + * int8_t bmi160_perform_self_test(uint8_t select_sensor, struct bmi160_dev *dev); + * \endcode + * @details This is used to perform self test of accel/gyro of the BMI160 sensor + * + * @param[in] select_sensor : enum to choose accel or gyro for self test + * @param[in] dev : Structure instance of bmi160_dev + * + * @note self test can be performed either for accel/gyro at any instant. + * + *@verbatim + * value of select_sensor | Inference + *----------------------------------|-------------------------------- + * BMI160_ACCEL_ONLY | Accel self test enabled + * BMI160_GYRO_ONLY | Gyro self test enabled + * BMI160_BOTH_ACCEL_AND_GYRO | NOT TO BE USED + *@endverbatim + * + * @note The return value of this API gives us the result of self test. + * + * @note Performing self test does soft reset of the sensor, User can + * set the desired settings after performing the self test. + * + * @return Result of API execution status + * @retval BMI160_OK Self test success + * @retval BMI160_W_GYRO_SELF_TEST_FAIL Gyro self test fail + * @retval BMI160_W_ACCEl_SELF_TEST_FAIL Accel self test fail + */ +int8_t bmi160_perform_self_test(uint8_t select_sensor, struct bmi160_dev *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiFIFO FIFO + * @brief FIFO operations of the sensor + */ + +/*! + * \ingroup bmi160ApiFIFO + * \page bmi160_api_bmi160_get_fifo_data bmi160_get_fifo_data + * \code + * int8_t bmi160_get_fifo_data(struct bmi160_dev const *dev); + * \endcode + * @details This API reads data from the fifo buffer. + * + * @note User has to allocate the FIFO buffer along with + * corresponding fifo length from his side before calling this API + * as mentioned in the readme.md + * + * @note User must specify the number of bytes to read from the FIFO in + * dev->fifo->length , It will be updated by the number of bytes actually + * read from FIFO after calling this API + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval Zero Success + * @retval Negative Error + */ +int8_t bmi160_get_fifo_data(struct bmi160_dev const *dev); + +/*! + * \ingroup bmi160ApiFIFO + * \page bmi160_api_bmi160_set_fifo_flush bmi160_set_fifo_flush + * \code + * int8_t bmi160_set_fifo_flush(const struct bmi160_dev *dev); + * \endcode + * @details This API writes fifo_flush command to command register.This + * action clears all data in the Fifo without changing fifo configuration + * settings. + * + * @param[in] dev : Structure instance of bmi160_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_set_fifo_flush(const struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiFIFO + * \page bmi160_api_bmi160_set_fifo_config bmi160_set_fifo_config + * \code + * int8_t bmi160_set_fifo_config(uint8_t config, uint8_t enable, struct bmi160_dev const *dev); + * \endcode + * @details This API sets the FIFO configuration in the sensor. + * + * @param[in] config : variable used to specify the FIFO + * configurations which are to be enabled or disabled in the sensor. + * + * @note : User can set either set one or more or all FIFO configurations + * by ORing the below mentioned macros. + * + *@verbatim + * config | Value + * ------------------------|--------------------------- + * BMI160_FIFO_TIME | 0x02 + * BMI160_FIFO_TAG_INT2 | 0x04 + * BMI160_FIFO_TAG_INT1 | 0x08 + * BMI160_FIFO_HEADER | 0x10 + * BMI160_FIFO_AUX | 0x20 + * BMI160_FIFO_ACCEL | 0x40 + * BMI160_FIFO_GYRO | 0x80 + *@endverbatim + * + * @param[in] enable : Parameter used to enable or disable the above + * FIFO configuration + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return status of bus communication result + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_set_fifo_config(uint8_t config, uint8_t enable, struct bmi160_dev const *dev); + +/*! + * \ingroup bmi160ApiFIFO + * \page bmi160_api_bmi160_set_fifo_down bmi160_set_fifo_down + * \code + * int8_t bmi160_set_fifo_down(uint8_t fifo_down, const struct bmi160_dev *dev); + * \endcode + * @details This API is used to configure the down sampling ratios of + * the accel and gyro data for FIFO.Also, it configures filtered or + * pre-filtered data for the fifo for accel and gyro. + * + * @param[in] fifo_down : variable used to specify the FIFO down + * configurations which are to be enabled or disabled in the sensor. + * + * @note The user must select one among the following macros to + * select down-sampling ratio for accel + * + *@verbatim + * config | Value + * -------------------------------------|--------------------------- + * BMI160_ACCEL_FIFO_DOWN_ZERO | 0x00 + * BMI160_ACCEL_FIFO_DOWN_ONE | 0x10 + * BMI160_ACCEL_FIFO_DOWN_TWO | 0x20 + * BMI160_ACCEL_FIFO_DOWN_THREE | 0x30 + * BMI160_ACCEL_FIFO_DOWN_FOUR | 0x40 + * BMI160_ACCEL_FIFO_DOWN_FIVE | 0x50 + * BMI160_ACCEL_FIFO_DOWN_SIX | 0x60 + * BMI160_ACCEL_FIFO_DOWN_SEVEN | 0x70 + *@endverbatim + * + * @note The user must select one among the following macros to + * select down-sampling ratio for gyro + * + *@verbatim + * config | Value + * -------------------------------------|--------------------------- + * BMI160_GYRO_FIFO_DOWN_ZERO | 0x00 + * BMI160_GYRO_FIFO_DOWN_ONE | 0x01 + * BMI160_GYRO_FIFO_DOWN_TWO | 0x02 + * BMI160_GYRO_FIFO_DOWN_THREE | 0x03 + * BMI160_GYRO_FIFO_DOWN_FOUR | 0x04 + * BMI160_GYRO_FIFO_DOWN_FIVE | 0x05 + * BMI160_GYRO_FIFO_DOWN_SIX | 0x06 + * BMI160_GYRO_FIFO_DOWN_SEVEN | 0x07 + *@endverbatim + * + * @note The user can enable filtered accel data by the following macro + * + *@verbatim + * config | Value + * -------------------------------------|--------------------------- + * BMI160_ACCEL_FIFO_FILT_EN | 0x80 + *@endverbatim + * + * @note The user can enable filtered gyro data by the following macro + * + *@verbatim + * config | Value + * -------------------------------------|--------------------------- + * BMI160_GYRO_FIFO_FILT_EN | 0x08 + *@endverbatim + * + * @note : By ORing the above mentioned macros, the user can select + * the required FIFO down config settings + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return status of bus communication result + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_set_fifo_down(uint8_t fifo_down, const struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiFIFO + * \page bmi160_api_bmi160_set_fifo_wm bmi160_set_fifo_wm + * \code + * int8_t bmi160_set_fifo_wm(uint8_t fifo_wm, const struct bmi160_dev *dev); + * \endcode + * @details This API sets the FIFO watermark level in the sensor. + * + * @note The FIFO watermark is issued when the FIFO fill level is + * equal or above the watermark level and units of watermark is 4 bytes. + * + * @param[in] fifo_wm : Variable used to set the FIFO water mark level + * @param[in] dev : Structure instance of bmi160_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_set_fifo_wm(uint8_t fifo_wm, const struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiFIFO + * \page bmi160_api_bmi160_extract_accel bmi160_extract_accel + * \code + * int8_t bmi160_extract_accel(struct bmi160_sensor_data *accel_data, uint8_t *accel_length, struct bmi160_dev const + **dev); + * \endcode + * @details This API parses and extracts the accelerometer frames from + * FIFO data read by the "bmi160_get_fifo_data" API and stores it in + * the "accel_data" structure instance. + * + * @note The bmi160_extract_accel API should be called only after + * reading the FIFO data by calling the bmi160_get_fifo_data() API. + * + * @param[out] accel_data : Structure instance of bmi160_sensor_data + * where the accelerometer data in FIFO is stored. + * @param[in,out] accel_length : Number of valid accelerometer frames + * (x,y,z axes data) read out from fifo. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note accel_length is updated with the number of valid accelerometer + * frames extracted from fifo (1 accel frame = 6 bytes) at the end of + * execution of this API. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_extract_accel(struct bmi160_sensor_data *accel_data, uint8_t *accel_length, struct bmi160_dev const *dev); + +/*! + * \ingroup bmi160ApiFIFO + * \page bmi160_api_bmi160_extract_gyro bmi160_extract_gyro + * \code + * int8_t bmi160_extract_gyro(struct bmi160_sensor_data *gyro_data, uint8_t *gyro_length, struct bmi160_dev const *dev); + * \endcode + * @details This API parses and extracts the gyro frames from + * FIFO data read by the "bmi160_get_fifo_data" API and stores it in + * the "gyro_data" structure instance. + * + * @note The bmi160_extract_gyro API should be called only after + * reading the FIFO data by calling the bmi160_get_fifo_data() API. + * + * @param[out] gyro_data : Structure instance of bmi160_sensor_data + * where the gyro data in FIFO is stored. + * @param[in,out] gyro_length : Number of valid gyro frames + * (x,y,z axes data) read out from fifo. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note gyro_length is updated with the number of valid gyro + * frames extracted from fifo (1 gyro frame = 6 bytes) at the end of + * execution of this API. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_extract_gyro(struct bmi160_sensor_data *gyro_data, uint8_t *gyro_length, struct bmi160_dev const *dev); + +/*! + * \ingroup bmi160ApiFIFO + * \page bmi160_api_bmi160_extract_aux bmi160_extract_aux + * \code + * int8_t bmi160_extract_aux(struct bmi160_aux_data *aux_data, uint8_t *aux_len, struct bmi160_dev const *dev); + * \endcode + * @details This API parses and extracts the aux frames from + * FIFO data read by the "bmi160_get_fifo_data" API and stores it in + * the bmi160_aux_data structure instance. + * + * @note The bmi160_extract_aux API should be called only after + * reading the FIFO data by calling the bmi160_get_fifo_data() API. + * + * @param[out] aux_data : Structure instance of bmi160_aux_data + * where the aux data in FIFO is stored. + * @param[in,out] aux_len : Number of valid aux frames (8bytes) + * read out from FIFO. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note aux_len is updated with the number of valid aux + * frames extracted from fifo (1 aux frame = 8 bytes) at the end of + * execution of this API. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_extract_aux(struct bmi160_aux_data *aux_data, uint8_t *aux_len, struct bmi160_dev const *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiFOC FOC + * @brief Start FOC of accel and gyro sensors + */ + +/*! + * \ingroup bmi160ApiFOC + * \page bmi160_api_bmi160_start_foc bmi160_start_foc + * \code + * int8_t bmi160_start_foc(const struct bmi160_foc_conf *foc_conf, + * \endcode + * @details This API starts the FOC of accel and gyro + * + * @note FOC should not be used in low-power mode of sensor + * + * @note Accel FOC targets values of +1g , 0g , -1g + * Gyro FOC always targets value of 0 dps + * + * @param[in] foc_conf : Structure instance of bmi160_foc_conf which + * has the FOC configuration + * @param[in,out] offset : Structure instance to store Offset + * values read from sensor + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note Pre-requisites for triggering FOC in accel , Set the following, + * Enable the acc_off_en + * Ex : foc_conf.acc_off_en = BMI160_ENABLE; + * + * Set the desired target values of FOC to each axes (x,y,z) by using the + * following macros + * - BMI160_FOC_ACCEL_DISABLED + * - BMI160_FOC_ACCEL_POSITIVE_G + * - BMI160_FOC_ACCEL_NEGATIVE_G + * - BMI160_FOC_ACCEL_0G + * + * Ex : foc_conf.foc_acc_x = BMI160_FOC_ACCEL_0G; + * foc_conf.foc_acc_y = BMI160_FOC_ACCEL_0G; + * foc_conf.foc_acc_z = BMI160_FOC_ACCEL_POSITIVE_G; + * + * @note Pre-requisites for triggering FOC in gyro , + * Set the following parameters, + * + * Ex : foc_conf.foc_gyr_en = BMI160_ENABLE; + * foc_conf.gyro_off_en = BMI160_ENABLE; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +int8_t bmi160_start_foc(const struct bmi160_foc_conf *foc_conf, + struct bmi160_offsets *offset, + struct bmi160_dev const *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiOffsets Offsets + * @brief Set / Get offset values of accel and gyro sensors + */ + +/*! + * \ingroup bmi160ApiOffsets + * \page bmi160_api_bmi160_get_offsets bmi160_get_offsets + * \code + * int8_t bmi160_get_offsets(struct bmi160_offsets *offset, const struct bmi160_dev *dev); + * \endcode + * @details This API reads and stores the offset values of accel and gyro + * + * @param[in,out] offset : Structure instance of bmi160_offsets in which + * the offset values are read and stored + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +int8_t bmi160_get_offsets(struct bmi160_offsets *offset, const struct bmi160_dev *dev); + +/*! + * \ingroup bmi160ApiOffsets + * \page bmi160_api_bmi160_set_offsets bmi160_set_offsets + * \code + * int8_t bmi160_set_offsets(const struct bmi160_foc_conf *foc_conf, + * const struct bmi160_offsets *offset, + * struct bmi160_dev const *dev); + * \endcode + * @details This API writes the offset values of accel and gyro to + * the sensor but these values will be reset on POR or soft reset. + * + * @param[in] foc_conf : Structure instance of bmi160_foc_conf which + * has the FOC configuration + * @param[in] offset : Structure instance in which user updates offset + * values which are to be written in the sensor + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note Offsets can be set by user like offset->off_acc_x = 10; + * where 1LSB = 3.9mg and for gyro 1LSB = 0.061degrees/second + * + * @note BMI160 offset values for xyz axes of accel should be within range of + * BMI160_ACCEL_MIN_OFFSET (-128) to BMI160_ACCEL_MAX_OFFSET (127) + * + * @note BMI160 offset values for xyz axes of gyro should be within range of + * BMI160_GYRO_MIN_OFFSET (-512) to BMI160_GYRO_MAX_OFFSET (511) + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +int8_t bmi160_set_offsets(const struct bmi160_foc_conf *foc_conf, + const struct bmi160_offsets *offset, + struct bmi160_dev const *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiNVM NVM + * @brief Write image registers values to NVM + */ + +/*! + * \ingroup bmi160ApiNVM + * \page bmi160_api_bmi160_update_nvm bmi160_update_nvm + * \code + * int8_t bmi160_update_nvm(struct bmi160_dev const *dev); + * \endcode + * @details This API writes the image registers values to NVM which is + * stored even after POR or soft reset + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +int8_t bmi160_update_nvm(struct bmi160_dev const *dev); + +/** + * \ingroup bmi160 + * \defgroup bmi160ApiInts Interrupt status + * @brief Read interrupt status from the sensor + */ + +/*! + * \ingroup bmi160ApiInts + * \page bmi160_api_bmi160_get_int_status bmi160_get_int_status + * \code + * int8_t bmi160_get_int_status(enum bmi160_int_status_sel int_status_sel, + * union bmi160_int_status *int_status, + * struct bmi160_dev const *dev); + * \endcode + * @details This API gets the interrupt status from the sensor. + * + * @param[in] int_status_sel : Enum variable to select either individual or all the + * interrupt status bits. + * @param[in] int_status : pointer variable to get the interrupt status + * from the sensor. + * param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +int8_t bmi160_get_int_status(enum bmi160_int_status_sel int_status_sel, + union bmi160_int_status *int_status, + struct bmi160_dev const *dev); + +/*************************** C++ guard macro *****************************/ +#ifdef __cplusplus +} +#endif + +#endif /* BMI160_H_ */ diff --git a/src/SensorManager/BMX160/bosch/bmi160_defs.h b/src/SensorManager/BMX160/bosch/bmi160_defs.h new file mode 100644 index 00000000..02fcce79 --- /dev/null +++ b/src/SensorManager/BMX160/bosch/bmi160_defs.h @@ -0,0 +1,1647 @@ +/** +* Copyright (c) 2021 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi160_defs.h +* @date 2021-10-05 +* @version v3.9.2 +* +*/ + +#ifndef BMI160_DEFS_H_ +#define BMI160_DEFS_H_ + +/*************************** C types headers *****************************/ +#ifdef __KERNEL__ +#include +#include +#else +#include +#include +#endif + +/*************************** Common macros *****************************/ + +#if !defined(UINT8_C) && !defined(INT8_C) +#define INT8_C(x) S8_C(x) +#define UINT8_C(x) U8_C(x) +#endif + +#if !defined(UINT16_C) && !defined(INT16_C) +#define INT16_C(x) S16_C(x) +#define UINT16_C(x) U16_C(x) +#endif + +#if !defined(INT32_C) && !defined(UINT32_C) +#define INT32_C(x) S32_C(x) +#define UINT32_C(x) U32_C(x) +#endif + +#if !defined(INT64_C) && !defined(UINT64_C) +#define INT64_C(x) S64_C(x) +#define UINT64_C(x) U64_C(x) +#endif + +/**@}*/ +/**\name C standard macros */ +#ifndef NULL +#ifdef __cplusplus +#define NULL 0 +#else +#define NULL ((void *) 0) +#endif +#endif + +/*************************** Sensor macros *****************************/ +/* Test for an endian machine */ +#ifndef __ORDER_LITTLE_ENDIAN__ +#define __ORDER_LITTLE_ENDIAN__ 0 +#endif + +#ifndef __BYTE_ORDER__ +#define __BYTE_ORDER__ __ORDER_LITTLE_ENDIAN__ +#endif + +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ +#ifndef LITTLE_ENDIAN +#define LITTLE_ENDIAN 1 +#endif +#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ +#ifndef BIG_ENDIAN +#define BIG_ENDIAN 1 +#endif +#else +#error "Code does not support Endian format of the processor" +#endif + +/** Mask definitions */ +#define BMI160_ACCEL_BW_MASK UINT8_C(0x70) +#define BMI160_ACCEL_ODR_MASK UINT8_C(0x0F) +#define BMI160_ACCEL_UNDERSAMPLING_MASK UINT8_C(0x80) +#define BMI160_ACCEL_RANGE_MASK UINT8_C(0x0F) +#define BMI160_GYRO_BW_MASK UINT8_C(0x30) +#define BMI160_GYRO_ODR_MASK UINT8_C(0x0F) +#define BMI160_GYRO_RANGE_MASK UINT8_C(0x07) + +#define BMI160_ACCEL_BW_POS UINT8_C(4) +#define BMI160_GYRO_BW_POS UINT8_C(4) + +/** Mask definitions for INT_EN registers */ +#define BMI160_ANY_MOTION_X_INT_EN_MASK UINT8_C(0x01) +#define BMI160_HIGH_G_X_INT_EN_MASK UINT8_C(0x01) +#define BMI160_NO_MOTION_X_INT_EN_MASK UINT8_C(0x01) +#define BMI160_ANY_MOTION_Y_INT_EN_MASK UINT8_C(0x02) +#define BMI160_HIGH_G_Y_INT_EN_MASK UINT8_C(0x02) +#define BMI160_NO_MOTION_Y_INT_EN_MASK UINT8_C(0x02) +#define BMI160_ANY_MOTION_Z_INT_EN_MASK UINT8_C(0x04) +#define BMI160_HIGH_G_Z_INT_EN_MASK UINT8_C(0x04) +#define BMI160_NO_MOTION_Z_INT_EN_MASK UINT8_C(0x04) +#define BMI160_SIG_MOTION_INT_EN_MASK UINT8_C(0x07) +#define BMI160_ANY_MOTION_ALL_INT_EN_MASK UINT8_C(0x07) +#define BMI160_STEP_DETECT_INT_EN_MASK UINT8_C(0x08) +#define BMI160_DOUBLE_TAP_INT_EN_MASK UINT8_C(0x10) +#define BMI160_SINGLE_TAP_INT_EN_MASK UINT8_C(0x20) +#define BMI160_FIFO_FULL_INT_EN_MASK UINT8_C(0x20) +#define BMI160_ORIENT_INT_EN_MASK UINT8_C(0x40) +#define BMI160_FIFO_WATERMARK_INT_EN_MASK UINT8_C(0x40) +#define BMI160_LOW_G_INT_EN_MASK UINT8_C(0x08) +#define BMI160_STEP_DETECT_EN_MASK UINT8_C(0x08) +#define BMI160_FLAT_INT_EN_MASK UINT8_C(0x80) +#define BMI160_DATA_RDY_INT_EN_MASK UINT8_C(0x10) + +/** PMU status Macros */ +#define BMI160_AUX_PMU_SUSPEND UINT8_C(0x00) +#define BMI160_AUX_PMU_NORMAL UINT8_C(0x01) +#define BMI160_AUX_PMU_LOW_POWER UINT8_C(0x02) + +#define BMI160_GYRO_PMU_SUSPEND UINT8_C(0x00) +#define BMI160_GYRO_PMU_NORMAL UINT8_C(0x01) +#define BMI160_GYRO_PMU_FSU UINT8_C(0x03) + +#define BMI160_ACCEL_PMU_SUSPEND UINT8_C(0x00) +#define BMI160_ACCEL_PMU_NORMAL UINT8_C(0x01) +#define BMI160_ACCEL_PMU_LOW_POWER UINT8_C(0x02) + +/** Mask definitions for INT_OUT_CTRL register */ +#define BMI160_INT1_EDGE_CTRL_MASK UINT8_C(0x01) +#define BMI160_INT1_OUTPUT_MODE_MASK UINT8_C(0x04) +#define BMI160_INT1_OUTPUT_TYPE_MASK UINT8_C(0x02) +#define BMI160_INT1_OUTPUT_EN_MASK UINT8_C(0x08) +#define BMI160_INT2_EDGE_CTRL_MASK UINT8_C(0x10) +#define BMI160_INT2_OUTPUT_MODE_MASK UINT8_C(0x40) +#define BMI160_INT2_OUTPUT_TYPE_MASK UINT8_C(0x20) +#define BMI160_INT2_OUTPUT_EN_MASK UINT8_C(0x80) + +/** Mask definitions for INT_LATCH register */ +#define BMI160_INT1_INPUT_EN_MASK UINT8_C(0x10) +#define BMI160_INT2_INPUT_EN_MASK UINT8_C(0x20) +#define BMI160_INT_LATCH_MASK UINT8_C(0x0F) + +/** Mask definitions for INT_MAP register */ +#define BMI160_INT1_LOW_G_MASK UINT8_C(0x01) +#define BMI160_INT1_HIGH_G_MASK UINT8_C(0x02) +#define BMI160_INT1_SLOPE_MASK UINT8_C(0x04) +#define BMI160_INT1_NO_MOTION_MASK UINT8_C(0x08) +#define BMI160_INT1_DOUBLE_TAP_MASK UINT8_C(0x10) +#define BMI160_INT1_SINGLE_TAP_MASK UINT8_C(0x20) +#define BMI160_INT1_FIFO_FULL_MASK UINT8_C(0x20) +#define BMI160_INT1_FIFO_WM_MASK UINT8_C(0x40) +#define BMI160_INT1_ORIENT_MASK UINT8_C(0x40) +#define BMI160_INT1_FLAT_MASK UINT8_C(0x80) +#define BMI160_INT1_DATA_READY_MASK UINT8_C(0x80) +#define BMI160_INT2_LOW_G_MASK UINT8_C(0x01) +#define BMI160_INT1_LOW_STEP_DETECT_MASK UINT8_C(0x01) +#define BMI160_INT2_LOW_STEP_DETECT_MASK UINT8_C(0x01) +#define BMI160_INT2_HIGH_G_MASK UINT8_C(0x02) +#define BMI160_INT2_FIFO_FULL_MASK UINT8_C(0x02) +#define BMI160_INT2_FIFO_WM_MASK UINT8_C(0x04) +#define BMI160_INT2_SLOPE_MASK UINT8_C(0x04) +#define BMI160_INT2_DATA_READY_MASK UINT8_C(0x08) +#define BMI160_INT2_NO_MOTION_MASK UINT8_C(0x08) +#define BMI160_INT2_DOUBLE_TAP_MASK UINT8_C(0x10) +#define BMI160_INT2_SINGLE_TAP_MASK UINT8_C(0x20) +#define BMI160_INT2_ORIENT_MASK UINT8_C(0x40) +#define BMI160_INT2_FLAT_MASK UINT8_C(0x80) + +/** Mask definitions for INT_DATA register */ +#define BMI160_TAP_SRC_INT_MASK UINT8_C(0x08) +#define BMI160_LOW_HIGH_SRC_INT_MASK UINT8_C(0x80) +#define BMI160_MOTION_SRC_INT_MASK UINT8_C(0x80) + +/** Mask definitions for INT_MOTION register */ +#define BMI160_SLOPE_INT_DUR_MASK UINT8_C(0x03) +#define BMI160_NO_MOTION_INT_DUR_MASK UINT8_C(0xFC) +#define BMI160_NO_MOTION_SEL_BIT_MASK UINT8_C(0x01) + +/** Mask definitions for INT_TAP register */ +#define BMI160_TAP_DUR_MASK UINT8_C(0x07) +#define BMI160_TAP_SHOCK_DUR_MASK UINT8_C(0x40) +#define BMI160_TAP_QUIET_DUR_MASK UINT8_C(0x80) +#define BMI160_TAP_THRES_MASK UINT8_C(0x1F) + +/** Mask definitions for INT_FLAT register */ +#define BMI160_FLAT_THRES_MASK UINT8_C(0x3F) +#define BMI160_FLAT_HOLD_TIME_MASK UINT8_C(0x30) +#define BMI160_FLAT_HYST_MASK UINT8_C(0x07) + +/** Mask definitions for INT_LOWHIGH register */ +#define BMI160_LOW_G_HYST_MASK UINT8_C(0x03) +#define BMI160_LOW_G_LOW_MODE_MASK UINT8_C(0x04) +#define BMI160_HIGH_G_HYST_MASK UINT8_C(0xC0) + +/** Mask definitions for INT_SIG_MOTION register */ +#define BMI160_SIG_MOTION_SEL_MASK UINT8_C(0x02) +#define BMI160_SIG_MOTION_SKIP_MASK UINT8_C(0x0C) +#define BMI160_SIG_MOTION_PROOF_MASK UINT8_C(0x30) + +/** Mask definitions for INT_ORIENT register */ +#define BMI160_ORIENT_MODE_MASK UINT8_C(0x03) +#define BMI160_ORIENT_BLOCK_MASK UINT8_C(0x0C) +#define BMI160_ORIENT_HYST_MASK UINT8_C(0xF0) +#define BMI160_ORIENT_THETA_MASK UINT8_C(0x3F) +#define BMI160_ORIENT_UD_ENABLE UINT8_C(0x40) +#define BMI160_AXES_EN_MASK UINT8_C(0x80) + +/** Mask definitions for FIFO_CONFIG register */ +#define BMI160_FIFO_GYRO UINT8_C(0x80) +#define BMI160_FIFO_ACCEL UINT8_C(0x40) +#define BMI160_FIFO_AUX UINT8_C(0x20) +#define BMI160_FIFO_TAG_INT1 UINT8_C(0x08) +#define BMI160_FIFO_TAG_INT2 UINT8_C(0x04) +#define BMI160_FIFO_TIME UINT8_C(0x02) +#define BMI160_FIFO_HEADER UINT8_C(0x10) +#define BMI160_FIFO_CONFIG_1_MASK UINT8_C(0xFE) + +/** Mask definitions for STEP_CONF register */ +#define BMI160_STEP_COUNT_EN_BIT_MASK UINT8_C(0x08) +#define BMI160_STEP_DETECT_MIN_THRES_MASK UINT8_C(0x18) +#define BMI160_STEP_DETECT_STEPTIME_MIN_MASK UINT8_C(0x07) +#define BMI160_STEP_MIN_BUF_MASK UINT8_C(0x07) + +/** Mask definition for FIFO Header Data Tag */ +#define BMI160_FIFO_TAG_INTR_MASK UINT8_C(0xFC) + +/** Fifo byte counter mask definitions */ +#define BMI160_FIFO_BYTE_COUNTER_MASK UINT8_C(0x07) + +/** Enable/disable bit value */ +#define BMI160_ENABLE UINT8_C(0x01) +#define BMI160_DISABLE UINT8_C(0x00) + +/** Latch Duration */ +#define BMI160_LATCH_DUR_NONE UINT8_C(0x00) +#define BMI160_LATCH_DUR_312_5_MICRO_SEC UINT8_C(0x01) +#define BMI160_LATCH_DUR_625_MICRO_SEC UINT8_C(0x02) +#define BMI160_LATCH_DUR_1_25_MILLI_SEC UINT8_C(0x03) +#define BMI160_LATCH_DUR_2_5_MILLI_SEC UINT8_C(0x04) +#define BMI160_LATCH_DUR_5_MILLI_SEC UINT8_C(0x05) +#define BMI160_LATCH_DUR_10_MILLI_SEC UINT8_C(0x06) +#define BMI160_LATCH_DUR_20_MILLI_SEC UINT8_C(0x07) +#define BMI160_LATCH_DUR_40_MILLI_SEC UINT8_C(0x08) +#define BMI160_LATCH_DUR_80_MILLI_SEC UINT8_C(0x09) +#define BMI160_LATCH_DUR_160_MILLI_SEC UINT8_C(0x0A) +#define BMI160_LATCH_DUR_320_MILLI_SEC UINT8_C(0x0B) +#define BMI160_LATCH_DUR_640_MILLI_SEC UINT8_C(0x0C) +#define BMI160_LATCH_DUR_1_28_SEC UINT8_C(0x0D) +#define BMI160_LATCH_DUR_2_56_SEC UINT8_C(0x0E) +#define BMI160_LATCHED UINT8_C(0x0F) + +/** BMI160 Register map */ +#define BMI160_CHIP_ID_ADDR UINT8_C(0x00) +#define BMI160_ERROR_REG_ADDR UINT8_C(0x02) +#define BMI160_PMU_STATUS_ADDR UINT8_C(0x03) +#define BMI160_AUX_DATA_ADDR UINT8_C(0x04) +#define BMI160_GYRO_DATA_ADDR UINT8_C(0x0C) +#define BMI160_ACCEL_DATA_ADDR UINT8_C(0x12) +#define BMI160_STATUS_ADDR UINT8_C(0x1B) +#define BMI160_INT_STATUS_ADDR UINT8_C(0x1C) +#define BMI160_FIFO_LENGTH_ADDR UINT8_C(0x22) +#define BMI160_FIFO_DATA_ADDR UINT8_C(0x24) +#define BMI160_ACCEL_CONFIG_ADDR UINT8_C(0x40) +#define BMI160_ACCEL_RANGE_ADDR UINT8_C(0x41) +#define BMI160_GYRO_CONFIG_ADDR UINT8_C(0x42) +#define BMI160_GYRO_RANGE_ADDR UINT8_C(0x43) +#define BMI160_AUX_ODR_ADDR UINT8_C(0x44) +#define BMI160_FIFO_DOWN_ADDR UINT8_C(0x45) +#define BMI160_FIFO_CONFIG_0_ADDR UINT8_C(0x46) +#define BMI160_FIFO_CONFIG_1_ADDR UINT8_C(0x47) +#define BMI160_AUX_IF_0_ADDR UINT8_C(0x4B) +#define BMI160_AUX_IF_1_ADDR UINT8_C(0x4C) +#define BMI160_AUX_IF_2_ADDR UINT8_C(0x4D) +#define BMI160_AUX_IF_3_ADDR UINT8_C(0x4E) +#define BMI160_AUX_IF_4_ADDR UINT8_C(0x4F) +#define BMI160_INT_ENABLE_0_ADDR UINT8_C(0x50) +#define BMI160_INT_ENABLE_1_ADDR UINT8_C(0x51) +#define BMI160_INT_ENABLE_2_ADDR UINT8_C(0x52) +#define BMI160_INT_OUT_CTRL_ADDR UINT8_C(0x53) +#define BMI160_INT_LATCH_ADDR UINT8_C(0x54) +#define BMI160_INT_MAP_0_ADDR UINT8_C(0x55) +#define BMI160_INT_MAP_1_ADDR UINT8_C(0x56) +#define BMI160_INT_MAP_2_ADDR UINT8_C(0x57) +#define BMI160_INT_DATA_0_ADDR UINT8_C(0x58) +#define BMI160_INT_DATA_1_ADDR UINT8_C(0x59) +#define BMI160_INT_LOWHIGH_0_ADDR UINT8_C(0x5A) +#define BMI160_INT_LOWHIGH_1_ADDR UINT8_C(0x5B) +#define BMI160_INT_LOWHIGH_2_ADDR UINT8_C(0x5C) +#define BMI160_INT_LOWHIGH_3_ADDR UINT8_C(0x5D) +#define BMI160_INT_LOWHIGH_4_ADDR UINT8_C(0x5E) +#define BMI160_INT_MOTION_0_ADDR UINT8_C(0x5F) +#define BMI160_INT_MOTION_1_ADDR UINT8_C(0x60) +#define BMI160_INT_MOTION_2_ADDR UINT8_C(0x61) +#define BMI160_INT_MOTION_3_ADDR UINT8_C(0x62) +#define BMI160_INT_TAP_0_ADDR UINT8_C(0x63) +#define BMI160_INT_TAP_1_ADDR UINT8_C(0x64) +#define BMI160_INT_ORIENT_0_ADDR UINT8_C(0x65) +#define BMI160_INT_ORIENT_1_ADDR UINT8_C(0x66) +#define BMI160_INT_FLAT_0_ADDR UINT8_C(0x67) +#define BMI160_INT_FLAT_1_ADDR UINT8_C(0x68) +#define BMI160_FOC_CONF_ADDR UINT8_C(0x69) +#define BMI160_CONF_ADDR UINT8_C(0x6A) + +#define BMI160_IF_CONF_ADDR UINT8_C(0x6B) +#define BMI160_SELF_TEST_ADDR UINT8_C(0x6D) +#define BMI160_OFFSET_ADDR UINT8_C(0x71) +#define BMI160_OFFSET_CONF_ADDR UINT8_C(0x77) +#define BMI160_INT_STEP_CNT_0_ADDR UINT8_C(0x78) +#define BMI160_INT_STEP_CONFIG_0_ADDR UINT8_C(0x7A) +#define BMI160_INT_STEP_CONFIG_1_ADDR UINT8_C(0x7B) +#define BMI160_COMMAND_REG_ADDR UINT8_C(0x7E) +#define BMI160_SPI_COMM_TEST_ADDR UINT8_C(0x7F) +#define BMI160_INTL_PULLUP_CONF_ADDR UINT8_C(0x85) + +/** Error code definitions */ +#define BMI160_OK INT8_C(0) +#define BMI160_E_NULL_PTR INT8_C(-1) +#define BMI160_E_COM_FAIL INT8_C(-2) +#define BMI160_E_DEV_NOT_FOUND INT8_C(-3) +#define BMI160_E_OUT_OF_RANGE INT8_C(-4) +#define BMI160_E_INVALID_INPUT INT8_C(-5) +#define BMI160_E_ACCEL_ODR_BW_INVALID INT8_C(-6) +#define BMI160_E_GYRO_ODR_BW_INVALID INT8_C(-7) +#define BMI160_E_LWP_PRE_FLTR_INT_INVALID INT8_C(-8) +#define BMI160_E_LWP_PRE_FLTR_INVALID INT8_C(-9) +#define BMI160_E_AUX_NOT_FOUND INT8_C(-10) +#define BMI160_E_FOC_FAILURE INT8_C(-11) +#define BMI160_E_READ_WRITE_LENGTH_INVALID INT8_C(-12) +#define BMI160_E_INVALID_CONFIG INT8_C(-13) + +/**\name API warning codes */ +#define BMI160_W_GYRO_SELF_TEST_FAIL INT8_C(1) +#define BMI160_W_ACCEl_SELF_TEST_FAIL INT8_C(2) + +/** BMI160 unique chip identifier */ +#define BMI160_CHIP_ID UINT8_C(0xD1) + +/** Soft reset command */ +#define BMI160_SOFT_RESET_CMD UINT8_C(0xb6) +#define BMI160_SOFT_RESET_DELAY_MS UINT8_C(1) + +/** Start FOC command */ +#define BMI160_START_FOC_CMD UINT8_C(0x03) + +/** NVM backup enabling command */ +#define BMI160_NVM_BACKUP_EN UINT8_C(0xA0) + +/* Delay in ms settings */ +#define BMI160_ACCEL_DELAY_MS UINT8_C(5) +#define BMI160_GYRO_DELAY_MS UINT8_C(80) +#define BMI160_ONE_MS_DELAY UINT8_C(1) +#define BMI160_AUX_COM_DELAY UINT8_C(10) +#define BMI160_GYRO_SELF_TEST_DELAY UINT8_C(20) +#define BMI160_ACCEL_SELF_TEST_DELAY UINT8_C(50) + +/** Self test configurations */ +#define BMI160_ACCEL_SELF_TEST_CONFIG UINT8_C(0x2C) +#define BMI160_ACCEL_SELF_TEST_POSITIVE_EN UINT8_C(0x0D) +#define BMI160_ACCEL_SELF_TEST_NEGATIVE_EN UINT8_C(0x09) +#define BMI160_ACCEL_SELF_TEST_LIMIT UINT16_C(8192) + +/** Power mode settings */ +/* Accel power mode */ +#define BMI160_ACCEL_NORMAL_MODE UINT8_C(0x11) +#define BMI160_ACCEL_LOWPOWER_MODE UINT8_C(0x12) +#define BMI160_ACCEL_SUSPEND_MODE UINT8_C(0x10) + +/* Gyro power mode */ +#define BMI160_GYRO_SUSPEND_MODE UINT8_C(0x14) +#define BMI160_GYRO_NORMAL_MODE UINT8_C(0x15) +#define BMI160_GYRO_FASTSTARTUP_MODE UINT8_C(0x17) + +/* Aux power mode */ +#define BMI160_AUX_SUSPEND_MODE UINT8_C(0x18) +#define BMI160_AUX_NORMAL_MODE UINT8_C(0x19) +#define BMI160_AUX_LOWPOWER_MODE UINT8_C(0x1A) + +/** Range settings */ +/* Accel Range */ +#define BMI160_ACCEL_RANGE_2G UINT8_C(0x03) +#define BMI160_ACCEL_RANGE_4G UINT8_C(0x05) +#define BMI160_ACCEL_RANGE_8G UINT8_C(0x08) +#define BMI160_ACCEL_RANGE_16G UINT8_C(0x0C) + +/* Gyro Range */ +#define BMI160_GYRO_RANGE_2000_DPS UINT8_C(0x00) +#define BMI160_GYRO_RANGE_1000_DPS UINT8_C(0x01) +#define BMI160_GYRO_RANGE_500_DPS UINT8_C(0x02) +#define BMI160_GYRO_RANGE_250_DPS UINT8_C(0x03) +#define BMI160_GYRO_RANGE_125_DPS UINT8_C(0x04) + +/** Bandwidth settings */ +/* Accel Bandwidth */ +#define BMI160_ACCEL_BW_OSR4_AVG1 UINT8_C(0x00) +#define BMI160_ACCEL_BW_OSR2_AVG2 UINT8_C(0x01) +#define BMI160_ACCEL_BW_NORMAL_AVG4 UINT8_C(0x02) +#define BMI160_ACCEL_BW_RES_AVG8 UINT8_C(0x03) +#define BMI160_ACCEL_BW_RES_AVG16 UINT8_C(0x04) +#define BMI160_ACCEL_BW_RES_AVG32 UINT8_C(0x05) +#define BMI160_ACCEL_BW_RES_AVG64 UINT8_C(0x06) +#define BMI160_ACCEL_BW_RES_AVG128 UINT8_C(0x07) + +#define BMI160_GYRO_BW_OSR4_MODE UINT8_C(0x00) +#define BMI160_GYRO_BW_OSR2_MODE UINT8_C(0x01) +#define BMI160_GYRO_BW_NORMAL_MODE UINT8_C(0x02) + +/* Output Data Rate settings */ +/* Accel Output data rate */ +#define BMI160_ACCEL_ODR_RESERVED UINT8_C(0x00) +#define BMI160_ACCEL_ODR_0_78HZ UINT8_C(0x01) +#define BMI160_ACCEL_ODR_1_56HZ UINT8_C(0x02) +#define BMI160_ACCEL_ODR_3_12HZ UINT8_C(0x03) +#define BMI160_ACCEL_ODR_6_25HZ UINT8_C(0x04) +#define BMI160_ACCEL_ODR_12_5HZ UINT8_C(0x05) +#define BMI160_ACCEL_ODR_25HZ UINT8_C(0x06) +#define BMI160_ACCEL_ODR_50HZ UINT8_C(0x07) +#define BMI160_ACCEL_ODR_100HZ UINT8_C(0x08) +#define BMI160_ACCEL_ODR_200HZ UINT8_C(0x09) +#define BMI160_ACCEL_ODR_400HZ UINT8_C(0x0A) +#define BMI160_ACCEL_ODR_800HZ UINT8_C(0x0B) +#define BMI160_ACCEL_ODR_1600HZ UINT8_C(0x0C) +#define BMI160_ACCEL_ODR_RESERVED0 UINT8_C(0x0D) +#define BMI160_ACCEL_ODR_RESERVED1 UINT8_C(0x0E) +#define BMI160_ACCEL_ODR_RESERVED2 UINT8_C(0x0F) + +/* Gyro Output data rate */ +#define BMI160_GYRO_ODR_RESERVED UINT8_C(0x00) +#define BMI160_GYRO_ODR_25HZ UINT8_C(0x06) +#define BMI160_GYRO_ODR_50HZ UINT8_C(0x07) +#define BMI160_GYRO_ODR_100HZ UINT8_C(0x08) +#define BMI160_GYRO_ODR_200HZ UINT8_C(0x09) +#define BMI160_GYRO_ODR_400HZ UINT8_C(0x0A) +#define BMI160_GYRO_ODR_800HZ UINT8_C(0x0B) +#define BMI160_GYRO_ODR_1600HZ UINT8_C(0x0C) +#define BMI160_GYRO_ODR_3200HZ UINT8_C(0x0D) + +/* Auxiliary sensor Output data rate */ +#define BMI160_AUX_ODR_RESERVED UINT8_C(0x00) +#define BMI160_AUX_ODR_0_78HZ UINT8_C(0x01) +#define BMI160_AUX_ODR_1_56HZ UINT8_C(0x02) +#define BMI160_AUX_ODR_3_12HZ UINT8_C(0x03) +#define BMI160_AUX_ODR_6_25HZ UINT8_C(0x04) +#define BMI160_AUX_ODR_12_5HZ UINT8_C(0x05) +#define BMI160_AUX_ODR_25HZ UINT8_C(0x06) +#define BMI160_AUX_ODR_50HZ UINT8_C(0x07) +#define BMI160_AUX_ODR_100HZ UINT8_C(0x08) +#define BMI160_AUX_ODR_200HZ UINT8_C(0x09) +#define BMI160_AUX_ODR_400HZ UINT8_C(0x0A) +#define BMI160_AUX_ODR_800HZ UINT8_C(0x0B) + +/** FIFO_CONFIG Definitions */ +#define BMI160_FIFO_TIME_ENABLE UINT8_C(0x02) +#define BMI160_FIFO_TAG_INT2_ENABLE UINT8_C(0x04) +#define BMI160_FIFO_TAG_INT1_ENABLE UINT8_C(0x08) +#define BMI160_FIFO_HEAD_ENABLE UINT8_C(0x10) +#define BMI160_FIFO_M_ENABLE UINT8_C(0x20) +#define BMI160_FIFO_A_ENABLE UINT8_C(0x40) +#define BMI160_FIFO_M_A_ENABLE UINT8_C(0x60) +#define BMI160_FIFO_G_ENABLE UINT8_C(0x80) +#define BMI160_FIFO_M_G_ENABLE UINT8_C(0xA0) +#define BMI160_FIFO_G_A_ENABLE UINT8_C(0xC0) +#define BMI160_FIFO_M_G_A_ENABLE UINT8_C(0xE0) + +/* Macro to specify the number of bytes over-read from the + * FIFO in order to get the sensor time at the end of FIFO */ +#ifndef BMI160_FIFO_BYTES_OVERREAD +#define BMI160_FIFO_BYTES_OVERREAD UINT8_C(25) +#endif + +/* Accel, gyro and aux. sensor length and also their combined + * length definitions in FIFO */ +#define BMI160_FIFO_G_LENGTH UINT8_C(6) +#define BMI160_FIFO_A_LENGTH UINT8_C(6) +#define BMI160_FIFO_M_LENGTH UINT8_C(8) +#define BMI160_FIFO_GA_LENGTH UINT8_C(12) +#define BMI160_FIFO_MA_LENGTH UINT8_C(14) +#define BMI160_FIFO_MG_LENGTH UINT8_C(14) +#define BMI160_FIFO_MGA_LENGTH UINT8_C(20) + +/** FIFO Header Data definitions */ +#define BMI160_FIFO_HEAD_SKIP_FRAME UINT8_C(0x40) +#define BMI160_FIFO_HEAD_SENSOR_TIME UINT8_C(0x44) +#define BMI160_FIFO_HEAD_INPUT_CONFIG UINT8_C(0x48) +#define BMI160_FIFO_HEAD_OVER_READ UINT8_C(0x80) +#define BMI160_FIFO_HEAD_A UINT8_C(0x84) +#define BMI160_FIFO_HEAD_G UINT8_C(0x88) +#define BMI160_FIFO_HEAD_G_A UINT8_C(0x8C) +#define BMI160_FIFO_HEAD_M UINT8_C(0x90) +#define BMI160_FIFO_HEAD_M_A UINT8_C(0x94) +#define BMI160_FIFO_HEAD_M_G UINT8_C(0x98) +#define BMI160_FIFO_HEAD_M_G_A UINT8_C(0x9C) + +/** FIFO sensor time length definitions */ +#define BMI160_SENSOR_TIME_LENGTH UINT8_C(3) + +/** FIFO DOWN selection */ +/* Accel fifo down-sampling values*/ +#define BMI160_ACCEL_FIFO_DOWN_ZERO UINT8_C(0x00) +#define BMI160_ACCEL_FIFO_DOWN_ONE UINT8_C(0x10) +#define BMI160_ACCEL_FIFO_DOWN_TWO UINT8_C(0x20) +#define BMI160_ACCEL_FIFO_DOWN_THREE UINT8_C(0x30) +#define BMI160_ACCEL_FIFO_DOWN_FOUR UINT8_C(0x40) +#define BMI160_ACCEL_FIFO_DOWN_FIVE UINT8_C(0x50) +#define BMI160_ACCEL_FIFO_DOWN_SIX UINT8_C(0x60) +#define BMI160_ACCEL_FIFO_DOWN_SEVEN UINT8_C(0x70) + +/* Gyro fifo down-smapling values*/ +#define BMI160_GYRO_FIFO_DOWN_ZERO UINT8_C(0x00) +#define BMI160_GYRO_FIFO_DOWN_ONE UINT8_C(0x01) +#define BMI160_GYRO_FIFO_DOWN_TWO UINT8_C(0x02) +#define BMI160_GYRO_FIFO_DOWN_THREE UINT8_C(0x03) +#define BMI160_GYRO_FIFO_DOWN_FOUR UINT8_C(0x04) +#define BMI160_GYRO_FIFO_DOWN_FIVE UINT8_C(0x05) +#define BMI160_GYRO_FIFO_DOWN_SIX UINT8_C(0x06) +#define BMI160_GYRO_FIFO_DOWN_SEVEN UINT8_C(0x07) + +/* Accel Fifo filter enable*/ +#define BMI160_ACCEL_FIFO_FILT_EN UINT8_C(0x80) + +/* Gyro Fifo filter enable*/ +#define BMI160_GYRO_FIFO_FILT_EN UINT8_C(0x08) + +/** Definitions to check validity of FIFO frames */ +#define FIFO_CONFIG_MSB_CHECK UINT8_C(0x80) +#define FIFO_CONFIG_LSB_CHECK UINT8_C(0x00) + +/*! BMI160 accel FOC configurations */ +#define BMI160_FOC_ACCEL_DISABLED UINT8_C(0x00) +#define BMI160_FOC_ACCEL_POSITIVE_G UINT8_C(0x01) +#define BMI160_FOC_ACCEL_NEGATIVE_G UINT8_C(0x02) +#define BMI160_FOC_ACCEL_0G UINT8_C(0x03) + +/** Array Parameter DefinItions */ +#define BMI160_SENSOR_TIME_LSB_BYTE UINT8_C(0) +#define BMI160_SENSOR_TIME_XLSB_BYTE UINT8_C(1) +#define BMI160_SENSOR_TIME_MSB_BYTE UINT8_C(2) + +/** Interface settings */ +#define BMI160_SPI_INTF UINT8_C(1) +#define BMI160_I2C_INTF UINT8_C(0) +#define BMI160_SPI_RD_MASK UINT8_C(0x80) +#define BMI160_SPI_WR_MASK UINT8_C(0x7F) + +/* Sensor & time select definition*/ +#define BMI160_ACCEL_SEL UINT8_C(0x01) +#define BMI160_GYRO_SEL UINT8_C(0x02) +#define BMI160_TIME_SEL UINT8_C(0x04) + +/* Sensor select mask*/ +#define BMI160_SEN_SEL_MASK UINT8_C(0x07) + +/* Error code mask */ +#define BMI160_ERR_REG_MASK UINT8_C(0x0F) + +/* BMI160 I2C address */ +#define BMI160_I2C_ADDR UINT8_C(0x68) + +/* BMI160 secondary IF address */ +#define BMI160_AUX_BMM150_I2C_ADDR UINT8_C(0x10) + +/** BMI160 Length definitions */ +#define BMI160_ONE UINT8_C(1) +#define BMI160_TWO UINT8_C(2) +#define BMI160_THREE UINT8_C(3) +#define BMI160_FOUR UINT8_C(4) +#define BMI160_FIVE UINT8_C(5) + +/** BMI160 fifo level Margin */ +#define BMI160_FIFO_LEVEL_MARGIN UINT8_C(16) + +/** BMI160 fifo flush Command */ +#define BMI160_FIFO_FLUSH_VALUE UINT8_C(0xB0) + +/** BMI160 offset values for xyz axes of accel */ +#define BMI160_ACCEL_MIN_OFFSET INT8_C(-128) +#define BMI160_ACCEL_MAX_OFFSET INT8_C(127) + +/** BMI160 offset values for xyz axes of gyro */ +#define BMI160_GYRO_MIN_OFFSET INT16_C(-512) +#define BMI160_GYRO_MAX_OFFSET INT16_C(511) + +/** BMI160 fifo full interrupt position and mask */ +#define BMI160_FIFO_FULL_INT_POS UINT8_C(5) +#define BMI160_FIFO_FULL_INT_MSK UINT8_C(0x20) +#define BMI160_FIFO_WTM_INT_POS UINT8_C(6) +#define BMI160_FIFO_WTM_INT_MSK UINT8_C(0x40) + +#define BMI160_FIFO_FULL_INT_PIN1_POS UINT8_C(5) +#define BMI160_FIFO_FULL_INT_PIN1_MSK UINT8_C(0x20) +#define BMI160_FIFO_FULL_INT_PIN2_POS UINT8_C(1) +#define BMI160_FIFO_FULL_INT_PIN2_MSK UINT8_C(0x02) + +#define BMI160_FIFO_WTM_INT_PIN1_POS UINT8_C(6) +#define BMI160_FIFO_WTM_INT_PIN1_MSK UINT8_C(0x40) +#define BMI160_FIFO_WTM_INT_PIN2_POS UINT8_C(2) +#define BMI160_FIFO_WTM_INT_PIN2_MSK UINT8_C(0x04) + +#define BMI160_MANUAL_MODE_EN_POS UINT8_C(7) +#define BMI160_MANUAL_MODE_EN_MSK UINT8_C(0x80) +#define BMI160_AUX_READ_BURST_POS UINT8_C(0) +#define BMI160_AUX_READ_BURST_MSK UINT8_C(0x03) + +#define BMI160_GYRO_SELF_TEST_POS UINT8_C(4) +#define BMI160_GYRO_SELF_TEST_MSK UINT8_C(0x10) +#define BMI160_GYRO_SELF_TEST_STATUS_POS UINT8_C(1) +#define BMI160_GYRO_SELF_TEST_STATUS_MSK UINT8_C(0x02) + +#define BMI160_GYRO_FOC_EN_POS UINT8_C(6) +#define BMI160_GYRO_FOC_EN_MSK UINT8_C(0x40) + +#define BMI160_ACCEL_FOC_X_CONF_POS UINT8_C(4) +#define BMI160_ACCEL_FOC_X_CONF_MSK UINT8_C(0x30) + +#define BMI160_ACCEL_FOC_Y_CONF_POS UINT8_C(2) +#define BMI160_ACCEL_FOC_Y_CONF_MSK UINT8_C(0x0C) + +#define BMI160_ACCEL_FOC_Z_CONF_MSK UINT8_C(0x03) + +#define BMI160_FOC_STATUS_POS UINT8_C(3) +#define BMI160_FOC_STATUS_MSK UINT8_C(0x08) + +#define BMI160_GYRO_OFFSET_X_MSK UINT8_C(0x03) + +#define BMI160_GYRO_OFFSET_Y_POS UINT8_C(2) +#define BMI160_GYRO_OFFSET_Y_MSK UINT8_C(0x0C) + +#define BMI160_GYRO_OFFSET_Z_POS UINT8_C(4) +#define BMI160_GYRO_OFFSET_Z_MSK UINT8_C(0x30) + +#define BMI160_GYRO_OFFSET_EN_POS UINT8_C(7) +#define BMI160_GYRO_OFFSET_EN_MSK UINT8_C(0x80) + +#define BMI160_ACCEL_OFFSET_EN_POS UINT8_C(6) +#define BMI160_ACCEL_OFFSET_EN_MSK UINT8_C(0x40) + +#define BMI160_GYRO_OFFSET_POS UINT16_C(8) +#define BMI160_GYRO_OFFSET_MSK UINT16_C(0x0300) + +#define BMI160_NVM_UPDATE_POS UINT8_C(1) +#define BMI160_NVM_UPDATE_MSK UINT8_C(0x02) + +#define BMI160_NVM_STATUS_POS UINT8_C(4) +#define BMI160_NVM_STATUS_MSK UINT8_C(0x10) + +#define BMI160_MAG_POWER_MODE_MSK UINT8_C(0x03) + +#define BMI160_ACCEL_POWER_MODE_MSK UINT8_C(0x30) +#define BMI160_ACCEL_POWER_MODE_POS UINT8_C(4) + +#define BMI160_GYRO_POWER_MODE_MSK UINT8_C(0x0C) +#define BMI160_GYRO_POWER_MODE_POS UINT8_C(2) + +/* BIT SLICE GET AND SET FUNCTIONS */ +#define BMI160_GET_BITS(regvar, bitname) \ + ((regvar & bitname##_MSK) >> bitname##_POS) +#define BMI160_SET_BITS(regvar, bitname, val) \ + ((regvar & ~bitname##_MSK) | \ + ((val << bitname##_POS) & bitname##_MSK)) + +#define BMI160_SET_BITS_POS_0(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MSK)) | \ + (data & bitname##_MSK)) + +#define BMI160_GET_BITS_POS_0(reg_data, bitname) (reg_data & (bitname##_MSK)) + +/**\name UTILITY MACROS */ +#define BMI160_SET_LOW_BYTE UINT16_C(0x00FF) +#define BMI160_SET_HIGH_BYTE UINT16_C(0xFF00) + +#define BMI160_GET_LSB(var) (uint8_t)(var & BMI160_SET_LOW_BYTE) +#define BMI160_GET_MSB(var) (uint8_t)((var & BMI160_SET_HIGH_BYTE) >> 8) + +/*****************************************************************************/ +/* type definitions */ + +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific read functions of the user + */ +typedef int8_t (*bmi160_read_fptr_t)(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len); + +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific write functions of the user + */ +typedef int8_t (*bmi160_write_fptr_t)(uint8_t dev_addr, uint8_t reg_addr, uint8_t *read_data, uint16_t len); +typedef void (*bmi160_delay_fptr_t)(uint32_t period); + +/*************************** Data structures *********************************/ + +/*! + * @brief bmi160 interrupt status selection enum. + */ +enum bmi160_int_status_sel { + BMI160_INT_STATUS_0 = 1, + BMI160_INT_STATUS_1 = 2, + BMI160_INT_STATUS_2 = 4, + BMI160_INT_STATUS_3 = 8, + BMI160_INT_STATUS_ALL = 15 +}; + +/*! + * @brief bmi160 interrupt status bits structure + */ +struct bmi160_int_status_bits +{ +#ifdef LITTLE_ENDIAN + + uint32_t step : 1; + uint32_t sigmot : 1; + uint32_t anym : 1; + + /* pmu trigger will be handled later */ + uint32_t pmu_trigger_reserved : 1; + uint32_t d_tap : 1; + uint32_t s_tap : 1; + uint32_t orient : 1; + uint32_t flat_int : 1; + uint32_t reserved : 2; + uint32_t high_g : 1; + uint32_t low_g : 1; + uint32_t drdy : 1; + uint32_t ffull : 1; + uint32_t fwm : 1; + uint32_t nomo : 1; + uint32_t anym_first_x : 1; + uint32_t anym_first_y : 1; + uint32_t anym_first_z : 1; + uint32_t anym_sign : 1; + uint32_t tap_first_x : 1; + uint32_t tap_first_y : 1; + uint32_t tap_first_z : 1; + uint32_t tap_sign : 1; + uint32_t high_first_x : 1; + uint32_t high_first_y : 1; + uint32_t high_first_z : 1; + uint32_t high_sign : 1; + uint32_t orient_1_0 : 2; + uint32_t orient_2 : 1; + uint32_t flat : 1; +#else + uint32_t high_first_x : 1; + uint32_t high_first_y : 1; + uint32_t high_first_z : 1; + uint32_t high_sign : 1; + uint32_t orient_1_0 : 2; + uint32_t orient_2 : 1; + uint32_t flat : 1; + uint32_t anym_first_x : 1; + uint32_t anym_first_y : 1; + uint32_t anym_first_z : 1; + uint32_t anym_sign : 1; + uint32_t tap_first_x : 1; + uint32_t tap_first_y : 1; + uint32_t tap_first_z : 1; + uint32_t tap_sign : 1; + uint32_t reserved : 2; + uint32_t high_g : 1; + uint32_t low_g : 1; + uint32_t drdy : 1; + uint32_t ffull : 1; + uint32_t fwm : 1; + uint32_t nomo : 1; + uint32_t step : 1; + uint32_t sigmot : 1; + uint32_t anym : 1; + + /* pmu trigger will be handled later */ + uint32_t pmu_trigger_reserved : 1; + uint32_t d_tap : 1; + uint32_t s_tap : 1; + uint32_t orient : 1; + uint32_t flat_int : 1; +#endif +}; + +/*! + * @brief bmi160 interrupt status structure + */ +union bmi160_int_status +{ + uint8_t data[4]; + struct bmi160_int_status_bits bit; +}; + +/*! + * @brief bmi160 sensor data structure which comprises of accel data + */ +struct bmi160_sensor_data +{ + /*! X-axis sensor data */ + int16_t x; + + /*! Y-axis sensor data */ + int16_t y; + + /*! Z-axis sensor data */ + int16_t z; + + /*! sensor time */ + uint32_t sensortime; +}; + +/*! + * @brief bmi160 aux data structure which comprises of 8 bytes of accel data + */ +struct bmi160_aux_data +{ + /*! Auxiliary data */ + uint8_t data[8]; +}; + +/*! + * @brief bmi160 FOC configuration structure + */ +struct bmi160_foc_conf +{ + /*! Enabling FOC in gyro + * Assignable macros : + * - BMI160_ENABLE + * - BMI160_DISABLE + */ + uint8_t foc_gyr_en; + + /*! Accel FOC configurations + * Assignable macros : + * - BMI160_FOC_ACCEL_DISABLED + * - BMI160_FOC_ACCEL_POSITIVE_G + * - BMI160_FOC_ACCEL_NEGATIVE_G + * - BMI160_FOC_ACCEL_0G + */ + uint8_t foc_acc_x; + uint8_t foc_acc_y; + uint8_t foc_acc_z; + + /*! Enabling offset compensation for accel in data registers + * Assignable macros : + * - BMI160_ENABLE + * - BMI160_DISABLE + */ + uint8_t acc_off_en; + + /*! Enabling offset compensation for gyro in data registers + * Assignable macros : + * - BMI160_ENABLE + * - BMI160_DISABLE + */ + uint8_t gyro_off_en; +}; + +/*! + * @brief bmi160 accel gyro offsets + */ +struct bmi160_offsets +{ + /*! Accel offset for x axis */ + int8_t off_acc_x; + + /*! Accel offset for y axis */ + int8_t off_acc_y; + + /*! Accel offset for z axis */ + int8_t off_acc_z; + + /*! Gyro offset for x axis */ + int16_t off_gyro_x; + + /*! Gyro offset for y axis */ + int16_t off_gyro_y; + + /*! Gyro offset for z axis */ + int16_t off_gyro_z; +}; + +/*! + * @brief FIFO aux. sensor data structure + */ +struct bmi160_aux_fifo_data +{ + /*! The value of aux. sensor x LSB data */ + uint8_t aux_x_lsb; + + /*! The value of aux. sensor x MSB data */ + uint8_t aux_x_msb; + + /*! The value of aux. sensor y LSB data */ + uint8_t aux_y_lsb; + + /*! The value of aux. sensor y MSB data */ + uint8_t aux_y_msb; + + /*! The value of aux. sensor z LSB data */ + uint8_t aux_z_lsb; + + /*! The value of aux. sensor z MSB data */ + uint8_t aux_z_msb; + + /*! The value of aux. sensor r for BMM150 LSB data */ + uint8_t aux_r_y2_lsb; + + /*! The value of aux. sensor r for BMM150 MSB data */ + uint8_t aux_r_y2_msb; +}; + +/*! + * @brief bmi160 sensor select structure + */ +enum bmi160_select_sensor { + BMI160_ACCEL_ONLY = 1, + BMI160_GYRO_ONLY, + BMI160_BOTH_ACCEL_AND_GYRO +}; + +/*! + * @brief bmi160 sensor step detector mode structure + */ +enum bmi160_step_detect_mode { + BMI160_STEP_DETECT_NORMAL, + BMI160_STEP_DETECT_SENSITIVE, + BMI160_STEP_DETECT_ROBUST, + + /*! Non recommended User defined setting */ + BMI160_STEP_DETECT_USER_DEFINE +}; + +/*! + * @brief enum for auxiliary burst read selection + */ +enum bmi160_aux_read_len { + BMI160_AUX_READ_LEN_0, + BMI160_AUX_READ_LEN_1, + BMI160_AUX_READ_LEN_2, + BMI160_AUX_READ_LEN_3 +}; + +/*! + * @brief bmi160 sensor configuration structure + */ +struct bmi160_cfg +{ + /*! power mode */ + uint8_t power; + + /*! output data rate */ + uint8_t odr; + + /*! range */ + uint8_t range; + + /*! bandwidth */ + uint8_t bw; +}; + +/*! + * @brief Aux sensor configuration structure + */ +struct bmi160_aux_cfg +{ + /*! Aux sensor, 1 - enable 0 - disable */ + uint8_t aux_sensor_enable : 1; + + /*! Aux manual/auto mode status */ + uint8_t manual_enable : 1; + + /*! Aux read burst length */ + uint8_t aux_rd_burst_len : 2; + + /*! output data rate */ + uint8_t aux_odr : 4; + + /*! i2c addr of auxiliary sensor */ + uint8_t aux_i2c_addr; +}; + +/*! + * @brief bmi160 interrupt channel selection structure + */ +enum bmi160_int_channel { + /*! Un-map both channels */ + BMI160_INT_CHANNEL_NONE, + + /*! interrupt Channel 1 */ + BMI160_INT_CHANNEL_1, + + /*! interrupt Channel 2 */ + BMI160_INT_CHANNEL_2, + + /*! Map both channels */ + BMI160_INT_CHANNEL_BOTH +}; +enum bmi160_int_types { + /*! Slope/Any-motion interrupt */ + BMI160_ACC_ANY_MOTION_INT, + + /*! Significant motion interrupt */ + BMI160_ACC_SIG_MOTION_INT, + + /*! Step detector interrupt */ + BMI160_STEP_DETECT_INT, + + /*! double tap interrupt */ + BMI160_ACC_DOUBLE_TAP_INT, + + /*! single tap interrupt */ + BMI160_ACC_SINGLE_TAP_INT, + + /*! orientation interrupt */ + BMI160_ACC_ORIENT_INT, + + /*! flat interrupt */ + BMI160_ACC_FLAT_INT, + + /*! high-g interrupt */ + BMI160_ACC_HIGH_G_INT, + + /*! low-g interrupt */ + BMI160_ACC_LOW_G_INT, + + /*! slow/no-motion interrupt */ + BMI160_ACC_SLOW_NO_MOTION_INT, + + /*! data ready interrupt */ + BMI160_ACC_GYRO_DATA_RDY_INT, + + /*! fifo full interrupt */ + BMI160_ACC_GYRO_FIFO_FULL_INT, + + /*! fifo watermark interrupt */ + BMI160_ACC_GYRO_FIFO_WATERMARK_INT, + + /*! fifo tagging feature support */ + BMI160_FIFO_TAG_INT_PIN +}; + +/*! + * @brief bmi160 active state of any & sig motion interrupt. + */ +enum bmi160_any_sig_motion_active_interrupt_state { + /*! Both any & sig motion are disabled */ + BMI160_BOTH_ANY_SIG_MOTION_DISABLED = -1, + + /*! Any-motion selected */ + BMI160_ANY_MOTION_ENABLED, + + /*! Sig-motion selected */ + BMI160_SIG_MOTION_ENABLED +}; +struct bmi160_acc_tap_int_cfg +{ +#ifdef LITTLE_ENDIAN + + /*! tap threshold */ + uint16_t tap_thr : 5; + + /*! tap shock */ + uint16_t tap_shock : 1; + + /*! tap quiet */ + uint16_t tap_quiet : 1; + + /*! tap duration */ + uint16_t tap_dur : 3; + + /*! data source 0- filter & 1 pre-filter*/ + uint16_t tap_data_src : 1; + + /*! tap enable, 1 - enable, 0 - disable */ + uint16_t tap_en : 1; +#else + + /*! tap enable, 1 - enable, 0 - disable */ + uint16_t tap_en : 1; + + /*! data source 0- filter & 1 pre-filter*/ + uint16_t tap_data_src : 1; + + /*! tap duration */ + uint16_t tap_dur : 3; + + /*! tap quiet */ + uint16_t tap_quiet : 1; + + /*! tap shock */ + uint16_t tap_shock : 1; + + /*! tap threshold */ + uint16_t tap_thr : 5; +#endif +}; +struct bmi160_acc_any_mot_int_cfg +{ +#ifdef LITTLE_ENDIAN + + /*! 1 any-motion enable, 0 - any-motion disable */ + uint8_t anymotion_en : 1; + + /*! slope interrupt x, 1 - enable, 0 - disable */ + uint8_t anymotion_x : 1; + + /*! slope interrupt y, 1 - enable, 0 - disable */ + uint8_t anymotion_y : 1; + + /*! slope interrupt z, 1 - enable, 0 - disable */ + uint8_t anymotion_z : 1; + + /*! slope duration */ + uint8_t anymotion_dur : 2; + + /*! data source 0- filter & 1 pre-filter*/ + uint8_t anymotion_data_src : 1; + + /*! slope threshold */ + uint8_t anymotion_thr; +#else + + /*! slope threshold */ + uint8_t anymotion_thr; + + /*! data source 0- filter & 1 pre-filter*/ + uint8_t anymotion_data_src : 1; + + /*! slope duration */ + uint8_t anymotion_dur : 2; + + /*! slope interrupt z, 1 - enable, 0 - disable */ + uint8_t anymotion_z : 1; + + /*! slope interrupt y, 1 - enable, 0 - disable */ + uint8_t anymotion_y : 1; + + /*! slope interrupt x, 1 - enable, 0 - disable */ + uint8_t anymotion_x : 1; + + /*! 1 any-motion enable, 0 - any-motion disable */ + uint8_t anymotion_en : 1; +#endif +}; +struct bmi160_acc_sig_mot_int_cfg +{ +#ifdef LITTLE_ENDIAN + + /*! skip time of sig-motion interrupt */ + uint8_t sig_mot_skip : 2; + + /*! proof time of sig-motion interrupt */ + uint8_t sig_mot_proof : 2; + + /*! data source 0- filter & 1 pre-filter*/ + uint8_t sig_data_src : 1; + + /*! 1 - enable sig, 0 - disable sig & enable anymotion */ + uint8_t sig_en : 1; + + /*! sig-motion threshold */ + uint8_t sig_mot_thres; +#else + + /*! sig-motion threshold */ + uint8_t sig_mot_thres; + + /*! 1 - enable sig, 0 - disable sig & enable anymotion */ + uint8_t sig_en : 1; + + /*! data source 0- filter & 1 pre-filter*/ + uint8_t sig_data_src : 1; + + /*! proof time of sig-motion interrupt */ + uint8_t sig_mot_proof : 2; + + /*! skip time of sig-motion interrupt */ + uint8_t sig_mot_skip : 2; +#endif +}; +struct bmi160_acc_step_detect_int_cfg +{ +#ifdef LITTLE_ENDIAN + + /*! 1- step detector enable, 0- step detector disable */ + uint16_t step_detector_en : 1; + + /*! minimum threshold */ + uint16_t min_threshold : 2; + + /*! minimal detectable step time */ + uint16_t steptime_min : 3; + + /*! enable step counter mode setting */ + uint16_t step_detector_mode : 2; + + /*! minimum step buffer size*/ + uint16_t step_min_buf : 3; +#else + + /*! minimum step buffer size*/ + uint16_t step_min_buf : 3; + + /*! enable step counter mode setting */ + uint16_t step_detector_mode : 2; + + /*! minimal detectable step time */ + uint16_t steptime_min : 3; + + /*! minimum threshold */ + uint16_t min_threshold : 2; + + /*! 1- step detector enable, 0- step detector disable */ + uint16_t step_detector_en : 1; +#endif +}; +struct bmi160_acc_no_motion_int_cfg +{ +#ifdef LITTLE_ENDIAN + + /*! no motion interrupt x */ + uint16_t no_motion_x : 1; + + /*! no motion interrupt y */ + uint16_t no_motion_y : 1; + + /*! no motion interrupt z */ + uint16_t no_motion_z : 1; + + /*! no motion duration */ + uint16_t no_motion_dur : 6; + + /*! no motion sel , 1 - enable no-motion ,0- enable slow-motion */ + uint16_t no_motion_sel : 1; + + /*! data source 0- filter & 1 pre-filter*/ + uint16_t no_motion_src : 1; + + /*! no motion threshold */ + uint8_t no_motion_thres; +#else + + /*! no motion threshold */ + uint8_t no_motion_thres; + + /*! data source 0- filter & 1 pre-filter*/ + uint16_t no_motion_src : 1; + + /*! no motion sel , 1 - enable no-motion ,0- enable slow-motion */ + uint16_t no_motion_sel : 1; + + /*! no motion duration */ + uint16_t no_motion_dur : 6; + + /* no motion interrupt z */ + uint16_t no_motion_z : 1; + + /*! no motion interrupt y */ + uint16_t no_motion_y : 1; + + /*! no motion interrupt x */ + uint16_t no_motion_x : 1; +#endif +}; +struct bmi160_acc_orient_int_cfg +{ +#ifdef LITTLE_ENDIAN + + /*! thresholds for switching between the different orientations */ + uint16_t orient_mode : 2; + + /*! blocking_mode */ + uint16_t orient_blocking : 2; + + /*! Orientation interrupt hysteresis */ + uint16_t orient_hyst : 4; + + /*! Orientation interrupt theta */ + uint16_t orient_theta : 6; + + /*! Enable/disable Orientation interrupt */ + uint16_t orient_ud_en : 1; + + /*! exchange x- and z-axis in algorithm ,0 - z, 1 - x */ + uint16_t axes_ex : 1; + + /*! 1 - orient enable, 0 - orient disable */ + uint8_t orient_en : 1; +#else + + /*! 1 - orient enable, 0 - orient disable */ + uint8_t orient_en : 1; + + /*! exchange x- and z-axis in algorithm ,0 - z, 1 - x */ + uint16_t axes_ex : 1; + + /*! Enable/disable Orientation interrupt */ + uint16_t orient_ud_en : 1; + + /*! Orientation interrupt theta */ + uint16_t orient_theta : 6; + + /*! Orientation interrupt hysteresis */ + uint16_t orient_hyst : 4; + + /*! blocking_mode */ + uint16_t orient_blocking : 2; + + /*! thresholds for switching between the different orientations */ + uint16_t orient_mode : 2; +#endif +}; +struct bmi160_acc_flat_detect_int_cfg +{ +#ifdef LITTLE_ENDIAN + + /*! flat threshold */ + uint16_t flat_theta : 6; + + /*! flat interrupt hysteresis */ + uint16_t flat_hy : 3; + + /*! delay time for which the flat value must remain stable for the + * flat interrupt to be generated */ + uint16_t flat_hold_time : 2; + + /*! 1 - flat enable, 0 - flat disable */ + uint16_t flat_en : 1; +#else + + /*! 1 - flat enable, 0 - flat disable */ + uint16_t flat_en : 1; + + /*! delay time for which the flat value must remain stable for the + * flat interrupt to be generated */ + uint16_t flat_hold_time : 2; + + /*! flat interrupt hysteresis */ + uint16_t flat_hy : 3; + + /*! flat threshold */ + uint16_t flat_theta : 6; +#endif +}; +struct bmi160_acc_low_g_int_cfg +{ +#ifdef LITTLE_ENDIAN + + /*! low-g interrupt trigger delay */ + uint8_t low_dur; + + /*! low-g interrupt trigger threshold */ + uint8_t low_thres; + + /*! hysteresis of low-g interrupt */ + uint8_t low_hyst : 2; + + /*! 0 - single-axis mode ,1 - axis-summing mode */ + uint8_t low_mode : 1; + + /*! data source 0- filter & 1 pre-filter */ + uint8_t low_data_src : 1; + + /*! 1 - enable low-g, 0 - disable low-g */ + uint8_t low_en : 1; +#else + + /*! 1 - enable low-g, 0 - disable low-g */ + uint8_t low_en : 1; + + /*! data source 0- filter & 1 pre-filter */ + uint8_t low_data_src : 1; + + /*! 0 - single-axis mode ,1 - axis-summing mode */ + uint8_t low_mode : 1; + + /*! hysteresis of low-g interrupt */ + uint8_t low_hyst : 2; + + /*! low-g interrupt trigger threshold */ + uint8_t low_thres; + + /*! low-g interrupt trigger delay */ + uint8_t low_dur; +#endif +}; +struct bmi160_acc_high_g_int_cfg +{ +#ifdef LITTLE_ENDIAN + + /*! High-g interrupt x, 1 - enable, 0 - disable */ + uint8_t high_g_x : 1; + + /*! High-g interrupt y, 1 - enable, 0 - disable */ + uint8_t high_g_y : 1; + + /*! High-g interrupt z, 1 - enable, 0 - disable */ + uint8_t high_g_z : 1; + + /*! High-g hysteresis */ + uint8_t high_hy : 2; + + /*! data source 0- filter & 1 pre-filter */ + uint8_t high_data_src : 1; + + /*! High-g threshold */ + uint8_t high_thres; + + /*! High-g duration */ + uint8_t high_dur; +#else + + /*! High-g duration */ + uint8_t high_dur; + + /*! High-g threshold */ + uint8_t high_thres; + + /*! data source 0- filter & 1 pre-filter */ + uint8_t high_data_src : 1; + + /*! High-g hysteresis */ + uint8_t high_hy : 2; + + /*! High-g interrupt z, 1 - enable, 0 - disable */ + uint8_t high_g_z : 1; + + /*! High-g interrupt y, 1 - enable, 0 - disable */ + uint8_t high_g_y : 1; + + /*! High-g interrupt x, 1 - enable, 0 - disable */ + uint8_t high_g_x : 1; +#endif +}; +struct bmi160_int_pin_settg +{ +#ifdef LITTLE_ENDIAN + + /*! To enable either INT1 or INT2 pin as output. + * 0- output disabled ,1- output enabled */ + uint16_t output_en : 1; + + /*! 0 - push-pull 1- open drain,only valid if output_en is set 1 */ + uint16_t output_mode : 1; + + /*! 0 - active low , 1 - active high level. + * if output_en is 1,this applies to interrupts,else PMU_trigger */ + uint16_t output_type : 1; + + /*! 0 - level trigger , 1 - edge trigger */ + uint16_t edge_ctrl : 1; + + /*! To enable either INT1 or INT2 pin as input. + * 0 - input disabled ,1 - input enabled */ + uint16_t input_en : 1; + + /*! latch duration*/ + uint16_t latch_dur : 4; +#else + + /*! latch duration*/ + uint16_t latch_dur : 4; + + /*! Latched,non-latched or temporary interrupt modes */ + uint16_t input_en : 1; + + /*! 1 - edge trigger, 0 - level trigger */ + uint16_t edge_ctrl : 1; + + /*! 0 - active low , 1 - active high level. + * if output_en is 1,this applies to interrupts,else PMU_trigger */ + uint16_t output_type : 1; + + /*! 0 - push-pull , 1 - open drain,only valid if output_en is set 1 */ + uint16_t output_mode : 1; + + /*! To enable either INT1 or INT2 pin as output. + * 0 - output disabled , 1 - output enabled */ + uint16_t output_en : 1; +#endif +}; +union bmi160_int_type_cfg +{ + /*! Tap interrupt structure */ + struct bmi160_acc_tap_int_cfg acc_tap_int; + + /*! Slope interrupt structure */ + struct bmi160_acc_any_mot_int_cfg acc_any_motion_int; + + /*! Significant motion interrupt structure */ + struct bmi160_acc_sig_mot_int_cfg acc_sig_motion_int; + + /*! Step detector interrupt structure */ + struct bmi160_acc_step_detect_int_cfg acc_step_detect_int; + + /*! No motion interrupt structure */ + struct bmi160_acc_no_motion_int_cfg acc_no_motion_int; + + /*! Orientation interrupt structure */ + struct bmi160_acc_orient_int_cfg acc_orient_int; + + /*! Flat interrupt structure */ + struct bmi160_acc_flat_detect_int_cfg acc_flat_int; + + /*! Low-g interrupt structure */ + struct bmi160_acc_low_g_int_cfg acc_low_g_int; + + /*! High-g interrupt structure */ + struct bmi160_acc_high_g_int_cfg acc_high_g_int; +}; +struct bmi160_int_settg +{ + /*! Interrupt channel */ + enum bmi160_int_channel int_channel; + + /*! Select Interrupt */ + enum bmi160_int_types int_type; + + /*! Structure configuring Interrupt pins */ + struct bmi160_int_pin_settg int_pin_settg; + + /*! Union configures required interrupt */ + union bmi160_int_type_cfg int_type_cfg; + + /*! FIFO FULL INT 1-enable, 0-disable */ + uint8_t fifo_full_int_en : 1; + + /*! FIFO WTM INT 1-enable, 0-disable */ + uint8_t fifo_wtm_int_en : 1; +}; + +/*! + * @brief This structure holds the information for usage of + * FIFO by the user. + */ +struct bmi160_fifo_frame +{ + /*! Data buffer of user defined length is to be mapped here */ + uint8_t *data; + + /*! While calling the API "bmi160_get_fifo_data" , length stores + * number of bytes in FIFO to be read (specified by user as input) + * and after execution of the API ,number of FIFO data bytes + * available is provided as an output to user + */ + uint16_t length; + + /*! FIFO time enable */ + uint8_t fifo_time_enable; + + /*! Enabling of the FIFO header to stream in header mode */ + uint8_t fifo_header_enable; + + /*! Streaming of the Accelerometer, Gyroscope + * sensor data or both in FIFO */ + uint8_t fifo_data_enable; + + /*! Will be equal to length when no more frames are there to parse */ + uint16_t accel_byte_start_idx; + + /*! Will be equal to length when no more frames are there to parse */ + uint16_t gyro_byte_start_idx; + + /*! Will be equal to length when no more frames are there to parse */ + uint16_t aux_byte_start_idx; + + /*! Value of FIFO sensor time time */ + uint32_t sensor_time; + + /*! Value of Skipped frame counts */ + uint8_t skipped_frame_count; +}; +struct bmi160_dev +{ + /*! Chip Id */ + uint8_t chip_id; + + /*! Device Id */ + uint8_t id; + + /*! 0 - I2C , 1 - SPI Interface */ + uint8_t intf; + + /*! Hold active interrupts status for any and sig motion + * 0 - Any-motion enable, 1 - Sig-motion enable, + * -1 neither any-motion nor sig-motion selected */ + enum bmi160_any_sig_motion_active_interrupt_state any_sig_sel; + + /*! Structure to configure Accel sensor */ + struct bmi160_cfg accel_cfg; + + /*! Structure to hold previous/old accel config parameters. + * This is used at driver level to prevent overwriting of same + * data, hence user does not change it in the code */ + struct bmi160_cfg prev_accel_cfg; + + /*! Structure to configure Gyro sensor */ + struct bmi160_cfg gyro_cfg; + + /*! Structure to hold previous/old gyro config parameters. + * This is used at driver level to prevent overwriting of same + * data, hence user does not change it in the code */ + struct bmi160_cfg prev_gyro_cfg; + + /*! Structure to configure the auxiliary sensor */ + struct bmi160_aux_cfg aux_cfg; + + /*! Structure to hold previous/old aux config parameters. + * This is used at driver level to prevent overwriting of same + * data, hence user does not change it in the code */ + struct bmi160_aux_cfg prev_aux_cfg; + + /*! FIFO related configurations */ + struct bmi160_fifo_frame *fifo; + + /*! Read function pointer */ + bmi160_read_fptr_t read; + + /*! Write function pointer */ + bmi160_write_fptr_t write; + + /*! Delay function pointer */ + bmi160_delay_fptr_t delay_ms; + + /*! User set read/write length */ + uint16_t read_write_len; +}; + +#endif /* BMI160_DEFS_H_ */ diff --git a/src/SensorManager/BMX160/bosch/bmm150.c b/src/SensorManager/BMX160/bosch/bmm150.c new file mode 100644 index 00000000..3af10488 --- /dev/null +++ b/src/SensorManager/BMX160/bosch/bmm150.c @@ -0,0 +1,2187 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmm150.c +* @date 2020-06-03 +* @version v2.0.0 +* +*/ + +/*! @file bmm150.c + * @brief Sensor driver for BMM150 sensor */ +#include "bmm150.h" + +/************************** Internal macros *******************************/ +/* Sensor ODR, Repetition and axes enable/disable settings */ +#define MODE_SETTING_SEL UINT16_C(0x000F) + +/* Interrupt pin settings like polarity,latch and int_pin enable */ +#define INTERRUPT_PIN_SETTING_SEL UINT16_C(0x01F0) + +/* Settings to enable/disable interrupts */ +#define INTERRUPT_CONFIG_SEL UINT16_C(0x1E00) + +/* Interrupt settings for configuring threshold values */ +#define INTERRUPT_THRESHOLD_CONFIG_SEL UINT16_C(0x6000) + +/********************** Static function declarations ************************/ + +/*! + * @brief This internal API is used to validate the device pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t null_ptr_check(const struct bmm150_dev *dev); + +/*! + * @brief This internal API sets/resets the power control bit of 0x4B register. + * + * @param[in] pwrcntrl_bit : Variable used to select/deselect the suspend mode. + * @param[in,out] dev : Structure instance of bmm150_dev + * + * pwrcntrl_bit | power mode + * -----------------|------------------------- + * 0x00 | Suspend mode + * 0x01 | Sleep/Active modes + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t set_power_control_bit(uint8_t pwrcntrl_bit, struct bmm150_dev *dev); + +/*! + * @brief This internal API reads the trim registers of the sensor and stores + * the trim values in the "trim_data" of device structure. + * + * @param[in,out] dev : Structure instance of bmm150_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t read_trim_registers(struct bmm150_dev *dev); + +/*! + * @brief This internal API writes the op_mode value in the Opmode bits + * (bits 1 and 2) of 0x4C register. + * + * op_mode | Power mode + * ------------|----------------------- + * 0x00 | BMM150_POWERMODE_NORMAL + * 0x01 | BMM150_POWERMODE_FORCED + * 0x03 | BMM150_POWERMODE_SLEEP + * + * @param[in,out] dev : Structure instance of bmm150_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t write_op_mode(uint8_t op_mode, struct bmm150_dev *dev); + +/*! + * @brief This internal API sets the device from suspend to sleep mode + * by setting the power control bit to '1' of 0x4B register + * + * @param[in,out] dev : Structure instance of bmm150_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t suspend_to_sleep_mode(struct bmm150_dev *dev); + +/*! + * @brief This internal API sets the xy repetition value in the 0x51 register. + * + * @param[in] settings : Structure instance of bmm150_settings. + * @param[in,out] dev : Structure instance of bmm150_dev + * + * settings->xy_rep | nXY(XY Repetitions) + * -------------------------|----------------------- + * 0x00 | 1 + * 0x01 | 3 + * 0x02 | 5 + * . | . + * . | . + * 0xFF | 511 + * + * @note number of XY Repetitions nXY = 1+2(settings->xy_rep) + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t set_xy_rep(const struct bmm150_settings *settings, struct bmm150_dev *dev); + +/*! + * @brief This internal API sets the z repetition value in the 0x52 register. + * + * @param[in] settings : Structure instance of bmm150_settings. + * @param[in,out] dev : Structure instance of bmm150_dev + * + * settings->z_rep | nZ(Z Repetitions) + * -------------------------|----------------------- + * 0x00 | 1 + * 0x01 | 2 + * 0x02 | 3 + * . | . + * . | . + * 0xFF | 256 + * + * @note number of Z Repetitions nZ = 1+(settings->z_rep) + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t set_z_rep(const struct bmm150_settings *settings, struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to set the output data rate of the sensor + * + * @param[in] settings : Structure instance of bmm150_settings. + * @param[in] dev : Structure instance of bmm150_dev. + * + * settings->data_rate | Data rate (ODR) + * -------------------------|----------------------- + * 0x00 | BMM150_DATA_RATE_10HZ + * 0x01 | BMM150_DATA_RATE_02HZ + * 0x02 | BMM150_DATA_RATE_06HZ + * 0x03 | BMM150_DATA_RATE_08HZ + * 0x04 | BMM150_DATA_RATE_15HZ + * 0x05 | BMM150_DATA_RATE_20HZ + * 0x06 | BMM150_DATA_RATE_25HZ + * 0x07 | BMM150_DATA_RATE_30HZ + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t set_odr(const struct bmm150_settings *settings, struct bmm150_dev *dev); + +/*! + * @brief This internal API sets the preset mode ODR and repetition settings. + * @param[in] settings : Structure instance of bmm150_settings + * @param[in] dev : Structure instance of bmm150_dev + * + * API settings | Representation + * ---------------------|------------------------------ + * settings->data_rate | Output Data Rate (ODR) + * settings->xy_rep | XY repetition value + * settings->z_rep | Z-repetition value + * + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t set_odr_xyz_rep(const struct bmm150_settings *settings, struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to enable or disable the magnetic + * measurement of x,y,z axes based on the value of xyz_axes_control. + * + * @param[in] settings : Structure instance of bmm150_settings. + * @param[in] dev : Structure instance of bmm150_dev. + * + * settings->xyz_axes_control | Measurement axes/channel + * ---------------------------|-------------------------- + * Bit 0 | X - Channel + * Bit 1 | Y - Channel + * Bit 2 | Z - Channel + * + * @note Setting 1 - Disables Channel measurement + * @note Setting 0 - Enables Channel measurement + * + * settings->xyz_axes_control | Measurement axes Enabled/disabled + * -------------------------------|------------------------------------ + * 0x01 | Disables X axis (Y,Z axes enabled) + * 0x02 | Disables Y axis (X,Z axes enabled) + * 0x04 | Disables Z axis (X,Y axes enabled) + * 0x07 | Disables all X,Y,Z axes measurement + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t set_control_measurement_xyz(const struct bmm150_settings *settings, struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to identify the settings which the user + * wants to modify in the sensor. + * + * @param[in] sub_settings : Contains the settings subset to identify particular + * group of settings which the user is interested to change. + * @param[in] settings : Contains the user specified settings. + * + * @return Indicates whether user is interested to modify the settings which + * are related to sub_settings. + * @retval True -> User wants to modify this group of settings + * @retval False -> User does not want to modify this group of settings + */ +static uint8_t are_settings_changed(uint16_t sub_settings, uint16_t settings); + +/*! + * @brief This API sets the ODR , measurement axes control , + * repetition values of xy,z. + * + * @param[in] desired_settings : Contains the settings which user wants to + * change. + * @param[in] settings : Structure instance of bmm150_settings. + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t mode_settings(uint16_t desired_settings, const struct bmm150_settings *settings, struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to parse and store the sensor + * settings in the device structure + * + * @param[in] reg_data : Pointer of an array consisting all sensor + * setting data from 0x4B to 0x52 registers. + * @param[in] settings : Structure instance of bmm150_settings. + * + */ +static void parse_setting(const uint8_t *reg_data, struct bmm150_settings *settings); + +/*! + * @brief This API is used to enable the interrupts and map them to the + * corresponding interrupt pins and specify the pin characteristics like the + * polarity , latch settings for the interrupt pins. + * + * @note The other interrupts can be latched or non-latched but, + * Data ready interrupt is always cleared after reading out the data + * + * @param[in] desired_settings : Contains the settings which user wants to + * change. + * @param[in] settings : Structure instance of bmm150_settings. + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t interrupt_pin_settings(uint16_t desired_settings, + const struct bmm150_settings *settings, + struct bmm150_dev *dev); + +/*! + * @brief This API is used to enable data overrun , overflow interrupts and + * enable/disable high/low threshold interrupts for x,y,z axis based on the + * threshold values set by the user in the High threshold (0x50) and + * Low threshold (0x4F) registers. + * + * @param[in] desired_settings : Contains the settings which user wants to + * change. + * @param[in] settings : Structure instance of bmm150_settings. + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t interrupt_config(uint16_t desired_settings, const struct bmm150_settings *settings, + struct bmm150_dev *dev); + +/*! + * @brief This API is used to write the user specified High/Low threshold value + * as a reference to generate the high/low threshold interrupt. + * + * @param[in] desired_settings : Contains the settings which user wants to + * change. + * @param[in] settings : Structure instance of bmm150_settings. + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t interrupt_threshold_settings(uint16_t desired_settings, + const struct bmm150_settings *settings, + struct bmm150_dev *dev); + +#ifdef BMM150_USE_FLOATING_POINT + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer X axis data in float. + * + * @param[in] mag_data_x : The value of raw X data + * @param[in] data_rhall : The value of raw RHALL data + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of compensated X data value in float + */ +static float compensate_x(int16_t mag_data_x, uint16_t data_rhall, const struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer Y axis data in float. + * + * @param[in] mag_data_y : The value of raw Y data + * @param[in] data_rhall : The value of raw RHALL data + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of compensated Y data value in float + */ +static float compensate_y(int16_t mag_data_y, uint16_t data_rhall, const struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer Z axis data in float. + * + * @param[in] mag_data_z : The value of raw Z data + * @param[in] data_rhall : The value of raw RHALL data + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of compensated Z data value in float + */ +static float compensate_z(int16_t mag_data_z, uint16_t data_rhall, const struct bmm150_dev *dev); + +#else + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer X axis data in int16_t. + * + * @param[in] mag_data_x : The value of raw X data + * @param[in] data_rhall : The value of raw RHALL data + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of compensated X data value in int16_t format + */ +static int16_t compensate_x(int16_t mag_data_x, uint16_t data_rhall, const struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer Y axis data in int16_t. + * + * @param[in] mag_data_y : The value of raw Y data + * @param[in] data_rhall : The value of raw RHALL data + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of compensated Y data value in int16_t format + */ +static int16_t compensate_y(int16_t mag_data_y, uint16_t data_rhall, const struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer Z axis data in int16_t. + * + * @param[in] mag_data_z : The value of raw Z data + * @param[in] data_rhall : The value of raw RHALL data + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of compensated Z data value in int16_t format + */ +static int16_t compensate_z(int16_t mag_data_z, uint16_t data_rhall, const struct bmm150_dev *dev); + +#endif + +/*! + * @brief This internal API is used to perform the normal self test + * of the sensor and return the self test result as return value + * + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t perform_normal_self_test(struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to enable the normal self test by setting + * the Self Test bit (bit0) of the 0x4C register, + * which triggers the start of self test + * + * @param[out] self_test_enable : The value of self test bit0 in 0x4C register + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t enable_normal_self_test(uint8_t *self_test_enable, struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to validate the results of normal self test + * by using the self test status available in the bit0 of registers 0x42,0x44 + * and 0x46. + * + * @param[in] dev : Structure instance of bmm150_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t validate_normal_self_test(struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to perform advanced self test for Z axis + * + * @param[in] dev : Structure instance of bmm150_dev + * + * @return Result of API execution status + * @retval zero -> Success / +ve value -> Warning / -ve value -> Error + * + * Return value | Status of self-test + *----------------------|--------------------------- + * 0 | BMM150_OK + * 8 | BMM150_W_ADV_SELF_TEST_FAIL + */ +static int8_t perform_adv_self_test(struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to set the desired power mode , + * axes control and repetition settings for advanced self test + * + * @param[in] dev : Structure instance of bmm150_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t adv_self_test_settings(struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to set the positive or negative value of + * self-test current and obtain the corresponding magnetometer z axis data + * + * @param[in] self_test_current : Self test current either positive/negative + * @param[out] data_z : Z-axis Magnetometer data + * @param[in] dev : Structure instance of bmm150_dev + * + * self_test_current | Self-test current Direction + *-------------------------|------------------------------ + * 0x03 | BMM150_ENABLE_POSITIVE_CURRENT + * 0x02 | BMM150_ENABLE_NEGATIVE_CURRENT + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t adv_self_test_measurement(uint8_t self_test_current, int16_t *data_z, struct bmm150_dev *dev); + +/*! + * @brief This internal API is used to get the difference between the + * Z axis mag data obtained by positive and negative self-test current + * and validate whether the advanced self test is done successfully or not. + * + * @param[in] positive_data_z : Z-axis Mag data by positive self-test current + * @param[in] negative_data_z : Z-axis Mag data by negative self-test current + * + * + * @return Result of API execution status + * @retval zero -> Success / +ve value -> Warning / -ve value -> Error + * + * Return value | Status of self-test + *----------------------|--------------------------- + * 0 | BMM150_OK + * 8 | BMM150_W_ADV_SELF_TEST_FAIL + */ +static int8_t validate_adv_self_test(int16_t positive_data_z, int16_t negative_data_z); + +/*! + * @brief This internal API is used to set the self test current value in + * the Adv. ST bits (bit6 and bit7) of 0x4C register + * + * @param[in] self_test_current : Self test current value (+ve/-ve) + * @param[in] dev : Structure instance of bmm150_dev + * + * self_test_current | Self-test current Direction + *-------------------------|------------------------------ + * 0x00 | BMM150_DISABLE_SELF_TEST_CURRENT + * 0x02 | BMM150_ENABLE_NEGATIVE_CURRENT + * 0x03 | BMM150_ENABLE_POSITIVE_CURRENT + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t set_adv_self_test_current(uint8_t self_test_current, struct bmm150_dev *dev); + +/********************** Global function definitions ************************/ + +/*! + * @brief This API is the entry point, Call this API before using other APIs. + * This API reads the chip-id of the sensor which is the first step to + * verify the sensor and updates the trim parameters of the sensor. + */ +int8_t bmm150_init(struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t chip_id = 0; + + /* Power up the sensor from suspend to sleep mode */ + rslt = set_power_control_bit(BMM150_POWER_CNTRL_ENABLE, dev); + + if (rslt == BMM150_OK) + { + /* Start-up time delay of 3ms */ + dev->delay_us(BMM150_START_UP_TIME, dev->intf_ptr); + + /* Chip ID of the sensor is read */ + rslt = bmm150_get_regs(BMM150_REG_CHIP_ID, &chip_id, 1, dev); + + /* Proceed if everything is fine until now */ + if (rslt == BMM150_OK) + { + /* Check for chip id validity */ + if (chip_id == BMM150_CHIP_ID) + { + dev->chip_id = chip_id; + + /* Function to update trim values */ + rslt = read_trim_registers(dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API writes the given data to the register address + * of the sensor. + */ +int8_t bmm150_set_regs(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, struct bmm150_dev *dev) +{ + int8_t rslt; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Proceed if null check is fine */ + if ((rslt == BMM150_OK) && (reg_data != NULL) && (len != 0)) + { + /* Write the data to the reg_addr */ + + /* SPI write requires to set The MSB of reg_addr as 0 + * but in default the MSB is always 0 + */ + dev->intf_rslt = dev->write(reg_addr, reg_data, len, dev->intf_ptr); + } + else + { + rslt = BMM150_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the data from the given register address of the sensor. + */ +int8_t bmm150_get_regs(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, struct bmm150_dev *dev) +{ + int8_t rslt; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Proceed if null check is fine */ + if ((rslt == BMM150_OK) && (reg_data != NULL)) + { + if (dev->intf != BMM150_I2C_INTF) + { + /* If interface selected is SPI */ + reg_addr = reg_addr | 0x80; + } + + /* Read the data from the reg_addr */ + dev->intf_rslt = dev->read(reg_addr, reg_data, len, dev->intf_ptr); + } + else + { + rslt = BMM150_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API is used to perform soft-reset of the sensor + * where all the registers are reset to their default values except 0x4B. + */ +int8_t bmm150_soft_reset(struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + rslt = bmm150_get_regs(BMM150_REG_POWER_CONTROL, ®_data, 1, dev); + + if (rslt == BMM150_OK) + { + reg_data = reg_data | BMM150_SET_SOFT_RESET; + rslt = bmm150_set_regs(BMM150_REG_POWER_CONTROL, ®_data, 1, dev); + dev->delay_us(BMM150_DELAY_SOFT_RESET, dev->intf_ptr); + } + + return rslt; +} + +/*! + * @brief This API is used to set the power mode of the sensor. + */ +int8_t bmm150_set_op_mode(const struct bmm150_settings *settings, struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t pwr_mode; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Proceed if null check is fine */ + if (rslt == BMM150_OK) + { + pwr_mode = settings->pwr_mode; + + /* Select the power mode to set */ + switch (pwr_mode) + { + case BMM150_POWERMODE_NORMAL: + + /* If the sensor is in suspend mode + * put the device to sleep mode + */ + rslt = suspend_to_sleep_mode(dev); + if (rslt == BMM150_OK) + { + /* write the op mode */ + rslt = write_op_mode(pwr_mode, dev); + } + + break; + case BMM150_POWERMODE_FORCED: + + /* If the sensor is in suspend mode + * put the device to sleep mode + */ + rslt = suspend_to_sleep_mode(dev); + if (rslt == BMM150_OK) + { + /* write the op mode */ + rslt = write_op_mode(pwr_mode, dev); + } + + break; + case BMM150_POWERMODE_SLEEP: + + /* If the sensor is in suspend mode + * put the device to sleep mode + */ + rslt = suspend_to_sleep_mode(dev); + if (rslt == BMM150_OK) + { + /* write the op mode */ + rslt = write_op_mode(pwr_mode, dev); + } + + break; + case BMM150_POWERMODE_SUSPEND: + + /* Set the power control bit to zero */ + rslt = set_power_control_bit(BMM150_POWER_CNTRL_DISABLE, dev); + break; + default: + rslt = BMM150_E_INVALID_CONFIG; + break; + } + } + + return rslt; +} + +/*! + * @brief This API is used to get the power mode of the sensor. + */ +int8_t bmm150_get_op_mode(uint8_t *op_mode, struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Proceed if null check is fine */ + if (rslt == BMM150_OK) + { + if (dev->pwr_cntrl_bit == BMM150_POWER_CNTRL_DISABLE) + { + /* Power mode set is suspend mode */ + *op_mode = BMM150_POWERMODE_SUSPEND; + } + else + { + /* Power mode set is stored in the op_mode */ + rslt = bmm150_get_regs(BMM150_REG_OP_MODE, ®_data, 1, dev); + *op_mode = BMM150_GET_BITS(reg_data, BMM150_OP_MODE); + } + } + + return rslt; +} + +/*! + * @brief This API is used to set the preset mode of the sensor. + */ +int8_t bmm150_set_presetmode(struct bmm150_settings *settings, struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t preset_mode; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Proceed if null check is fine */ + if (rslt == BMM150_OK) + { + preset_mode = settings->preset_mode; + + switch (preset_mode) + { + case BMM150_PRESETMODE_LOWPOWER: + + /* Set the data rate x,y,z repetition + * for Low Power mode + */ + settings->data_rate = BMM150_DATA_RATE_10HZ; + settings->xy_rep = BMM150_REPXY_LOWPOWER; + settings->z_rep = BMM150_REPZ_LOWPOWER; + rslt = set_odr_xyz_rep(settings, dev); + break; + case BMM150_PRESETMODE_REGULAR: + + /* Set the data rate x,y,z repetition + * for Regular mode + */ + settings->data_rate = BMM150_DATA_RATE_10HZ; + settings->xy_rep = BMM150_REPXY_REGULAR; + settings->z_rep = BMM150_REPZ_REGULAR; + rslt = set_odr_xyz_rep(settings, dev); + break; + case BMM150_PRESETMODE_HIGHACCURACY: + + /* Set the data rate x,y,z repetition + * for High Accuracy mode * + */ + settings->data_rate = BMM150_DATA_RATE_20HZ; + settings->xy_rep = BMM150_REPXY_HIGHACCURACY; + settings->z_rep = BMM150_REPZ_HIGHACCURACY; + rslt = set_odr_xyz_rep(settings, dev); + break; + case BMM150_PRESETMODE_ENHANCED: + + /* Set the data rate x,y,z repetition + * for Enhanced Accuracy mode + */ + settings->data_rate = BMM150_DATA_RATE_10HZ; + settings->xy_rep = BMM150_REPXY_ENHANCED; + settings->z_rep = BMM150_REPZ_ENHANCED; + rslt = set_odr_xyz_rep(settings, dev); + break; + default: + rslt = BMM150_E_INVALID_CONFIG; + break; + } + } + + return rslt; +} + +/*! + * @brief This API sets the sensor settings based on the desired_settings + * and the dev structure configuration + */ +int8_t bmm150_set_sensor_settings(uint16_t desired_settings, + const struct bmm150_settings *settings, + struct bmm150_dev *dev) +{ + int8_t rslt; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Proceed if null check is fine */ + if (rslt == BMM150_OK) + { + if (are_settings_changed(MODE_SETTING_SEL, desired_settings)) + { + /* ODR, Control measurement, XY,Z repetition values */ + rslt = mode_settings(desired_settings, settings, dev); + } + + if ((!rslt) && are_settings_changed(INTERRUPT_PIN_SETTING_SEL, desired_settings)) + { + /* Interrupt pin settings */ + rslt = interrupt_pin_settings(desired_settings, settings, dev); + } + + if ((!rslt) && are_settings_changed(INTERRUPT_CONFIG_SEL, desired_settings)) + { + /* Interrupt configuration settings */ + rslt = interrupt_config(desired_settings, settings, dev); + } + + if ((!rslt) && are_settings_changed(INTERRUPT_THRESHOLD_CONFIG_SEL, desired_settings)) + { + /* Interrupt threshold settings */ + rslt = interrupt_threshold_settings(desired_settings, settings, dev); + } + } + + return rslt; +} + +/*! + * @brief This API gets the sensor settings and updates the dev structure + */ +int8_t bmm150_get_sensor_settings(struct bmm150_settings *settings, struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t setting[BMM150_LEN_SETTING_DATA] = { 0 }; + + /* Read the entire sensor settings */ + rslt = bmm150_get_regs(BMM150_REG_POWER_CONTROL, setting, BMM150_LEN_SETTING_DATA, dev); + + if (rslt == BMM150_OK) + { + /* Parse and store the settings */ + parse_setting(setting, settings); + } + + return rslt; +} + +/*! + * @brief This API is used to read the magnetometer data from registers + * 0x42 to 0x49 and update the dev structure with the + * compensated mag data in micro-tesla. + */ +int8_t bmm150_read_mag_data(struct bmm150_mag_data *mag_data, struct bmm150_dev *dev) +{ + int8_t rslt; + int16_t msb_data; + uint8_t reg_data[BMM150_LEN_XYZR_DATA] = { 0 }; + struct bmm150_raw_mag_data raw_mag_data; + + /* Read the mag data registers */ + rslt = bmm150_get_regs(BMM150_REG_DATA_X_LSB, reg_data, BMM150_LEN_XYZR_DATA, dev); + + if (rslt == BMM150_OK) + { + /* Mag X axis data */ + reg_data[0] = BMM150_GET_BITS(reg_data[0], BMM150_DATA_X); + + /* Shift the MSB data to left by 5 bits */ + /* Multiply by 32 to get the shift left by 5 value */ + msb_data = ((int16_t)((int8_t)reg_data[1])) * 32; + + /* Raw mag X axis data */ + raw_mag_data.raw_datax = (int16_t)(msb_data | reg_data[0]); + + /* Mag Y axis data */ + reg_data[2] = BMM150_GET_BITS(reg_data[2], BMM150_DATA_Y); + + /* Shift the MSB data to left by 5 bits */ + /* Multiply by 32 to get the shift left by 5 value */ + msb_data = ((int16_t)((int8_t)reg_data[3])) * 32; + + /* Raw mag Y axis data */ + raw_mag_data.raw_datay = (int16_t)(msb_data | reg_data[2]); + + /* Mag Z axis data */ + reg_data[4] = BMM150_GET_BITS(reg_data[4], BMM150_DATA_Z); + + /* Shift the MSB data to left by 7 bits */ + /* Multiply by 128 to get the shift left by 7 value */ + msb_data = ((int16_t)((int8_t)reg_data[5])) * 128; + + /* Raw mag Z axis data */ + raw_mag_data.raw_dataz = (int16_t)(msb_data | reg_data[4]); + + /* Mag R-HALL data */ + reg_data[6] = BMM150_GET_BITS(reg_data[6], BMM150_DATA_RHALL); + raw_mag_data.raw_data_r = (uint16_t)(((uint16_t)reg_data[7] << 6) | reg_data[6]); + + /* Compensated Mag X data in int16_t format */ + mag_data->x = compensate_x(raw_mag_data.raw_datax, raw_mag_data.raw_data_r, dev); + + /* Compensated Mag Y data in int16_t format */ + mag_data->y = compensate_y(raw_mag_data.raw_datay, raw_mag_data.raw_data_r, dev); + + /* Compensated Mag Z data in int16_t format */ + mag_data->z = compensate_z(raw_mag_data.raw_dataz, raw_mag_data.raw_data_r, dev); + } + + return rslt; +} + +/*! + * @brief This API is used to perform the complete self test + * (both normal and advanced) for the BMM150 sensor + */ +int8_t bmm150_perform_self_test(uint8_t self_test_mode, struct bmm150_dev *dev) +{ + int8_t rslt; + int8_t self_test_rslt = 0; + struct bmm150_settings settings; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Proceed if null check is fine */ + if (rslt == BMM150_OK) + { + switch (self_test_mode) + { + case BMM150_SELF_TEST_NORMAL: + + /* Set the sensor in sleep mode */ + settings.pwr_mode = BMM150_POWERMODE_SLEEP; + rslt = bmm150_set_op_mode(&settings, dev); + + if (rslt == BMM150_OK) + { + /* Perform the normal self test */ + rslt = perform_normal_self_test(dev); + } + + break; + case BMM150_SELF_TEST_ADVANCED: + + /* Perform the advanced self test */ + rslt = perform_adv_self_test(dev); + + /* Check to ensure bus error does not occur */ + if (rslt >= BMM150_OK) + { + /* Store the status of self test result */ + self_test_rslt = rslt; + + /* Perform soft reset */ + rslt = bmm150_soft_reset(dev); + } + + /* Check to ensure bus operations are success */ + if (rslt == BMM150_OK) + { + /* Restore self_test_rslt as return value */ + rslt = self_test_rslt; + } + + break; + default: + rslt = BMM150_E_INVALID_CONFIG; + break; + } + } + + return rslt; +} + +/*! + * @brief This API is used to get the status flags of all interrupt + * which is used to check for the assertion of interrupts + */ +int8_t bmm150_get_interrupt_status(struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t interrupt_status; + uint8_t data_ready_status; + + /* Read the data ready status from the register 0x48 */ + rslt = bmm150_get_regs(BMM150_REG_DATA_READY_STATUS, &data_ready_status, 1, dev); + + if (rslt == BMM150_OK) + { + /* Read the interrupt status from the register 0x50 */ + rslt = bmm150_get_regs(BMM150_REG_INTERRUPT_STATUS, &interrupt_status, 1, dev); + + if (rslt == BMM150_OK) + { + /* Mask and store the data ready status bit */ + data_ready_status = BMM150_GET_BITS_POS_0(data_ready_status, BMM150_DRDY_STATUS); + + /* Store the entire interrupt status in dev */ + dev->int_status = (data_ready_status << 8) | interrupt_status; + } + } + + return rslt; +} + +/****************************************************************************/ +/**\name BMM150 as Auxiliary Mag */ + +/*! + * @brief This API is used to compensate the raw mag data + */ +int8_t bmm150_aux_mag_data(uint8_t *aux_data, struct bmm150_mag_data *mag_data, const struct bmm150_dev *dev) +{ + int8_t rslt; + int16_t msb_data; + struct bmm150_raw_mag_data raw_mag_data; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Proceed if null check is fine */ + if ((rslt == BMM150_OK) && (aux_data != NULL)) + { + /* Mag X axis data */ + aux_data[0] = BMM150_GET_BITS(aux_data[0], BMM150_DATA_X); + + /* Shift the MSB data to left by 5 bits */ + /* Multiply by 32 to get the shift left by 5 value */ + msb_data = ((int16_t)((int8_t)aux_data[1])) * 32; + + /* Raw mag X axis data */ + raw_mag_data.raw_datax = (int16_t)(msb_data | aux_data[0]); + + /* Mag Y axis data */ + aux_data[2] = BMM150_GET_BITS(aux_data[2], BMM150_DATA_Y); + + /* Shift the MSB data to left by 5 bits */ + /* Multiply by 32 to get the shift left by 5 value */ + msb_data = ((int16_t)((int8_t)aux_data[3])) * 32; + + /* Raw mag Y axis data */ + raw_mag_data.raw_datay = (int16_t)(msb_data | aux_data[2]); + + /* Mag Z axis data */ + aux_data[4] = BMM150_GET_BITS(aux_data[4], BMM150_DATA_Z); + + /* Shift the MSB data to left by 7 bits */ + /* Multiply by 128 to get the shift left by 7 value */ + msb_data = ((int16_t)((int8_t)aux_data[5])) * 128; + + /* Raw mag Z axis data */ + raw_mag_data.raw_dataz = (int16_t)(msb_data | aux_data[4]); + + /* Mag R-HALL data */ + aux_data[6] = BMM150_GET_BITS(aux_data[6], BMM150_DATA_RHALL); + raw_mag_data.raw_data_r = (uint16_t)(((uint16_t)aux_data[7] << 6) | aux_data[6]); + + /* Compensated Mag X data in int16_t format */ + mag_data->x = compensate_x(raw_mag_data.raw_datax, raw_mag_data.raw_data_r, dev); + + /* Compensated Mag Y data in int16_t format */ + mag_data->y = compensate_y(raw_mag_data.raw_datay, raw_mag_data.raw_data_r, dev); + + /* Compensated Mag Z data in int16_t format */ + mag_data->z = compensate_z(raw_mag_data.raw_dataz, raw_mag_data.raw_data_r, dev); + } + + return rslt; +} + +/****************************************************************************/ +/**\name INTERNAL APIs */ + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmm150_dev *dev) +{ + int8_t rslt; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL) || + (dev->intf_ptr == NULL)) + { + /* Device structure pointer is not valid */ + rslt = BMM150_E_NULL_PTR; + } + else + { + /* Device structure is fine */ + rslt = BMM150_OK; + } + + return rslt; +} + +/*! + * @brief This internal API sets/resets the power control bit of 0x4B register. + */ +static int8_t set_power_control_bit(uint8_t pwrcntrl_bit, struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t reg_data = 0; + + /* Power control register 0x4B is read */ + rslt = bmm150_get_regs(BMM150_REG_POWER_CONTROL, ®_data, 1, dev); + + /* Proceed if everything is fine until now */ + if (rslt == BMM150_OK) + { + /* Sets the value of power control bit */ + reg_data = BMM150_SET_BITS_POS_0(reg_data, BMM150_PWR_CNTRL, pwrcntrl_bit); + rslt = bmm150_set_regs(BMM150_REG_POWER_CONTROL, ®_data, 1, dev); + + if (rslt == BMM150_OK) + { + /* Store the power control bit + * value in dev structure + */ + dev->pwr_cntrl_bit = pwrcntrl_bit; + } + } + + return rslt; +} + +/*! + * @brief This internal API reads the trim registers of the sensor and stores + * the trim values in the "trim_data" of device structure. + */ +static int8_t read_trim_registers(struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t trim_x1y1[2] = { 0 }; + uint8_t trim_xyz_data[4] = { 0 }; + uint8_t trim_xy1xy2[10] = { 0 }; + uint16_t temp_msb = 0; + + /* Trim register value is read */ + rslt = bmm150_get_regs(BMM150_DIG_X1, trim_x1y1, 2, dev); + + if (rslt == BMM150_OK) + { + rslt = bmm150_get_regs(BMM150_DIG_Z4_LSB, trim_xyz_data, 4, dev); + + if (rslt == BMM150_OK) + { + rslt = bmm150_get_regs(BMM150_DIG_Z2_LSB, trim_xy1xy2, 10, dev); + + if (rslt == BMM150_OK) + { + /* Trim data which is read is updated + * in the device structure + */ + dev->trim_data.dig_x1 = (int8_t)trim_x1y1[0]; + dev->trim_data.dig_y1 = (int8_t)trim_x1y1[1]; + dev->trim_data.dig_x2 = (int8_t)trim_xyz_data[2]; + dev->trim_data.dig_y2 = (int8_t)trim_xyz_data[3]; + temp_msb = ((uint16_t)trim_xy1xy2[3]) << 8; + dev->trim_data.dig_z1 = (uint16_t)(temp_msb | trim_xy1xy2[2]); + temp_msb = ((uint16_t)trim_xy1xy2[1]) << 8; + dev->trim_data.dig_z2 = (int16_t)(temp_msb | trim_xy1xy2[0]); + temp_msb = ((uint16_t)trim_xy1xy2[7]) << 8; + dev->trim_data.dig_z3 = (int16_t)(temp_msb | trim_xy1xy2[6]); + temp_msb = ((uint16_t)trim_xyz_data[1]) << 8; + dev->trim_data.dig_z4 = (int16_t)(temp_msb | trim_xyz_data[0]); + dev->trim_data.dig_xy1 = trim_xy1xy2[9]; + dev->trim_data.dig_xy2 = (int8_t)trim_xy1xy2[8]; + temp_msb = ((uint16_t)(trim_xy1xy2[5] & 0x7F)) << 8; + dev->trim_data.dig_xyz1 = (uint16_t)(temp_msb | trim_xy1xy2[4]); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API writes the op_mode value in the Opmode bits + * (bits 1 and 2) of 0x4C register. + */ +static int8_t write_op_mode(uint8_t op_mode, struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Read the 0x4C register */ + rslt = bmm150_get_regs(BMM150_REG_OP_MODE, ®_data, 1, dev); + + if (rslt == BMM150_OK) + { + /* Set the op_mode value in Opmode bits of 0x4C */ + reg_data = BMM150_SET_BITS(reg_data, BMM150_OP_MODE, op_mode); + rslt = bmm150_set_regs(BMM150_REG_OP_MODE, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API sets the device from suspend to sleep mode + * by setting the power control bit to '1' of 0x4B register + */ +static int8_t suspend_to_sleep_mode(struct bmm150_dev *dev) +{ + int8_t rslt; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Proceed if null check is fine */ + if (rslt == BMM150_OK) + { + if (dev->pwr_cntrl_bit == BMM150_POWER_CNTRL_DISABLE) + { + rslt = set_power_control_bit(BMM150_POWER_CNTRL_ENABLE, dev); + + /* Start-up time delay of 3ms */ + dev->delay_us(BMM150_START_UP_TIME, dev->intf_ptr); + } + } + + return rslt; +} + +/*! + * @brief This internal API sets the xy repetition value in the 0x51 register. + */ +static int8_t set_xy_rep(const struct bmm150_settings *settings, struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t rep_xy; + + /* Set the xy repetition */ + rep_xy = settings->xy_rep; + rslt = bmm150_set_regs(BMM150_REG_REP_XY, &rep_xy, 1, dev); + + return rslt; +} + +/*! + * @brief This internal API sets the z repetition value in the 0x52 register. + */ +static int8_t set_z_rep(const struct bmm150_settings *settings, struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t rep_z; + + /* Set the z repetition */ + rep_z = settings->z_rep; + rslt = bmm150_set_regs(BMM150_REG_REP_Z, &rep_z, 1, dev); + + return rslt; +} + +/*! + * @brief This internal API is used to set the output data rate of the sensor. + */ +static int8_t set_odr(const struct bmm150_settings *settings, struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Read the 0x4C register */ + rslt = bmm150_get_regs(BMM150_REG_OP_MODE, ®_data, 1, dev); + + if (rslt == BMM150_OK) + { + /* Set the ODR value */ + reg_data = BMM150_SET_BITS(reg_data, BMM150_ODR, settings->data_rate); + rslt = bmm150_set_regs(BMM150_REG_OP_MODE, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API sets the preset mode ODR and repetition settings. + */ +static int8_t set_odr_xyz_rep(const struct bmm150_settings *settings, struct bmm150_dev *dev) +{ + int8_t rslt; + + /* Set the ODR */ + rslt = set_odr(settings, dev); + + if (rslt == BMM150_OK) + { + /* Set the XY-repetitions number */ + rslt = set_xy_rep(settings, dev); + + if (rslt == BMM150_OK) + { + /* Set the Z-repetitions number */ + rslt = set_z_rep(settings, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable or disable the magnetic + * measurement of x,y,z axes based on the value of xyz_axes_control. + */ +static int8_t set_control_measurement_xyz(const struct bmm150_settings *settings, struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + rslt = bmm150_get_regs(BMM150_REG_AXES_ENABLE, ®_data, 1, dev); + + if (rslt == BMM150_OK) + { + /* Set the axes to be enabled/disabled */ + reg_data = BMM150_SET_BITS(reg_data, BMM150_CONTROL_MEASURE, settings->xyz_axes_control); + rslt = bmm150_set_regs(BMM150_REG_AXES_ENABLE, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to identify the settings which the user + * wants to modify in the sensor. + */ +static uint8_t are_settings_changed(uint16_t sub_settings, uint16_t desired_settings) +{ + uint8_t settings_changed; + + if (sub_settings & desired_settings) + { + /* User wants to modify this particular settings */ + settings_changed = BMM150_TRUE; + } + else + { + /* User don't want to modify this particular settings */ + settings_changed = BMM150_FALSE; + } + + return settings_changed; +} + +/*! + * @brief This API sets the ODR , measurement axes control , + * repetition values of xy,z. + */ +static int8_t mode_settings(uint16_t desired_settings, const struct bmm150_settings *settings, struct bmm150_dev *dev) +{ + int8_t rslt = BMM150_E_INVALID_CONFIG; + + if (desired_settings & BMM150_SEL_DATA_RATE) + { + /* Sets the ODR */ + rslt = set_odr(settings, dev); + } + + if (desired_settings & BMM150_SEL_CONTROL_MEASURE) + { + /* Enables/Disables the control measurement axes */ + rslt = set_control_measurement_xyz(settings, dev); + } + + if (desired_settings & BMM150_SEL_XY_REP) + { + /* Sets the XY repetition */ + rslt = set_xy_rep(settings, dev); + } + + if (desired_settings & BMM150_SEL_Z_REP) + { + /* Sets the Z repetition */ + rslt = set_z_rep(settings, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse and store the sensor + * settings in the device structure + */ +static void parse_setting(const uint8_t *reg_data, struct bmm150_settings *settings) +{ + /* Parse all the w/r registers and update the + * current sensor settings in the dev structure + */ + settings->z_rep = reg_data[7]; + settings->xy_rep = reg_data[6]; + settings->int_settings.high_threshold = reg_data[5]; + settings->int_settings.low_threshold = reg_data[4]; + settings->xyz_axes_control = BMM150_GET_BITS(reg_data[3], BMM150_CONTROL_MEASURE); + settings->int_settings.drdy_pin_en = BMM150_GET_BITS(reg_data[3], BMM150_DRDY_EN); + settings->int_settings.int_pin_en = BMM150_GET_BITS(reg_data[3], BMM150_INT_PIN_EN); + settings->int_settings.drdy_polarity = BMM150_GET_BITS(reg_data[3], BMM150_DRDY_POLARITY); + settings->int_settings.int_latch = BMM150_GET_BITS(reg_data[3], BMM150_INT_LATCH); + settings->int_settings.int_polarity = BMM150_GET_BITS_POS_0(reg_data[3], BMM150_INT_POLARITY); + settings->int_settings.data_overrun_en = BMM150_GET_BITS(reg_data[2], BMM150_DATA_OVERRUN_INT); + settings->int_settings.overflow_int_en = BMM150_GET_BITS(reg_data[2], BMM150_OVERFLOW_INT); + settings->int_settings.high_int_en = BMM150_GET_BITS(reg_data[2], BMM150_HIGH_THRESHOLD_INT); + settings->int_settings.low_int_en = BMM150_GET_BITS_POS_0(reg_data[2], BMM150_LOW_THRESHOLD_INT); + settings->data_rate = BMM150_GET_BITS(reg_data[1], BMM150_ODR); +} + +/*! + * @brief This API is used to enable the interrupts and map them to the + * corresponding interrupt pins and specify the pin characteristics like the + * polarity , latch settings for the interrupt pins. + */ +static int8_t interrupt_pin_settings(uint16_t desired_settings, + const struct bmm150_settings *settings, + struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + struct bmm150_int_ctrl_settings int_settings; + + rslt = bmm150_get_regs(BMM150_REG_AXES_ENABLE, ®_data, 1, dev); + + if (rslt == BMM150_OK) + { + int_settings = settings->int_settings; + if (desired_settings & BMM150_SEL_DRDY_PIN_EN) + { + /* Enables the Data ready interrupt and + * maps it to the DRDY pin of the sensor + */ + reg_data = BMM150_SET_BITS(reg_data, BMM150_DRDY_EN, int_settings.drdy_pin_en); + } + + if (desired_settings & BMM150_SEL_INT_PIN_EN) + { + /* Sets interrupt pin enable */ + reg_data = BMM150_SET_BITS(reg_data, BMM150_INT_PIN_EN, int_settings.int_pin_en); + } + + if (desired_settings & BMM150_SEL_DRDY_POLARITY) + { + /* Sets Data ready pin's polarity */ + reg_data = BMM150_SET_BITS(reg_data, BMM150_DRDY_POLARITY, int_settings.drdy_polarity); + } + + if (desired_settings & BMM150_SEL_INT_LATCH) + { + /* Sets Interrupt in latched or non-latched mode */ + reg_data = BMM150_SET_BITS(reg_data, BMM150_INT_LATCH, int_settings.int_latch); + } + + if (desired_settings & BMM150_SEL_INT_POLARITY) + { + /* Sets Interrupt pin's polarity */ + reg_data = BMM150_SET_BITS_POS_0(reg_data, BMM150_INT_POLARITY, int_settings.int_polarity); + } + + /* Set the interrupt configurations in the 0x4E register */ + rslt = bmm150_set_regs(BMM150_REG_AXES_ENABLE, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API is used to enable data overrun , overflow interrupts and + * enable/disable high/low threshold interrupts for x,y,z axis based on the + * threshold values set by the user in the High threshold (0x50) and + * Low threshold (0x4F) registers. + */ +static int8_t interrupt_config(uint16_t desired_settings, const struct bmm150_settings *settings, + struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + struct bmm150_int_ctrl_settings int_settings; + + rslt = bmm150_get_regs(BMM150_REG_INT_CONFIG, ®_data, 1, dev); + + if (rslt == BMM150_OK) + { + int_settings = settings->int_settings; + if (desired_settings & BMM150_SEL_DATA_OVERRUN_INT) + { + /* Sets Data overrun interrupt */ + reg_data = BMM150_SET_BITS(reg_data, BMM150_DATA_OVERRUN_INT, int_settings.data_overrun_en); + } + + if (desired_settings & BMM150_SEL_OVERFLOW_INT) + { + /* Sets Data overflow interrupt */ + reg_data = BMM150_SET_BITS(reg_data, BMM150_OVERFLOW_INT, int_settings.overflow_int_en); + } + + if (desired_settings & BMM150_SEL_HIGH_THRESHOLD_INT) + { + /* Sets high threshold interrupt */ + reg_data = BMM150_SET_BITS(reg_data, BMM150_HIGH_THRESHOLD_INT, int_settings.high_int_en); + } + + if (desired_settings & BMM150_SEL_LOW_THRESHOLD_INT) + { + /* Sets low threshold interrupt */ + reg_data = BMM150_SET_BITS_POS_0(reg_data, BMM150_LOW_THRESHOLD_INT, int_settings.low_int_en); + } + + /* Set the interrupt configurations in the 0x4D register */ + rslt = bmm150_set_regs(BMM150_REG_INT_CONFIG, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API is used to write the user specified High/Low threshold value + * as a reference to generate the high/low threshold interrupt. + */ +static int8_t interrupt_threshold_settings(uint16_t desired_settings, + const struct bmm150_settings *settings, + struct bmm150_dev *dev) +{ + int8_t rslt = BMM150_E_INVALID_CONFIG; + uint8_t reg_data; + + if (desired_settings & BMM150_SEL_LOW_THRESHOLD_SETTING) + { + /* Sets the Low threshold value to trigger interrupt */ + reg_data = settings->int_settings.low_threshold; + rslt = bmm150_set_regs(BMM150_REG_LOW_THRESHOLD, ®_data, 1, dev); + } + + if (desired_settings & BMM150_SEL_HIGH_THRESHOLD_SETTING) + { + /* Sets the High threshold value to trigger interrupt */ + reg_data = settings->int_settings.high_threshold; + rslt = bmm150_set_regs(BMM150_REG_HIGH_THRESHOLD, ®_data, 1, dev); + } + + return rslt; +} + +#ifdef BMM150_USE_FLOATING_POINT + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer x axis data(micro-tesla) in float. + */ +static float compensate_x(int16_t mag_data_x, uint16_t data_rhall, const struct bmm150_dev *dev) +{ + float retval = 0; + float process_comp_x0; + float process_comp_x1; + float process_comp_x2; + float process_comp_x3; + float process_comp_x4; + + /* Overflow condition check */ + if ((mag_data_x != BMM150_OVERFLOW_ADCVAL_XYAXES_FLIP) && (data_rhall != 0) && (dev->trim_data.dig_xyz1 != 0)) + { + /* Processing compensation equations */ + process_comp_x0 = (((float)dev->trim_data.dig_xyz1) * 16384.0f / data_rhall); + retval = (process_comp_x0 - 16384.0f); + process_comp_x1 = ((float)dev->trim_data.dig_xy2) * (retval * retval / 268435456.0f); + process_comp_x2 = process_comp_x1 + retval * ((float)dev->trim_data.dig_xy1) / 16384.0f; + process_comp_x3 = ((float)dev->trim_data.dig_x2) + 160.0f; + process_comp_x4 = mag_data_x * ((process_comp_x2 + 256.0f) * process_comp_x3); + retval = ((process_comp_x4 / 8192.0f) + (((float)dev->trim_data.dig_x1) * 8.0f)) / 16.0f; + } + else + { + /* Overflow, set output to 0.0f */ + retval = BMM150_OVERFLOW_OUTPUT_FLOAT; + } + + return retval; +} + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer y axis data(micro-tesla) in float. + */ +static float compensate_y(int16_t mag_data_y, uint16_t data_rhall, const struct bmm150_dev *dev) +{ + float retval = 0; + float process_comp_y0; + float process_comp_y1; + float process_comp_y2; + float process_comp_y3; + float process_comp_y4; + + /* Overflow condition check */ + if ((mag_data_y != BMM150_OVERFLOW_ADCVAL_XYAXES_FLIP) && (data_rhall != 0) && (dev->trim_data.dig_xyz1 != 0)) + { + /* Processing compensation equations */ + process_comp_y0 = ((float)dev->trim_data.dig_xyz1) * 16384.0f / data_rhall; + retval = process_comp_y0 - 16384.0f; + process_comp_y1 = ((float)dev->trim_data.dig_xy2) * (retval * retval / 268435456.0f); + process_comp_y2 = process_comp_y1 + retval * ((float)dev->trim_data.dig_xy1) / 16384.0f; + process_comp_y3 = ((float)dev->trim_data.dig_y2) + 160.0f; + process_comp_y4 = mag_data_y * (((process_comp_y2) + 256.0f) * process_comp_y3); + retval = ((process_comp_y4 / 8192.0f) + (((float)dev->trim_data.dig_y1) * 8.0f)) / 16.0f; + } + else + { + /* Overflow, set output to 0.0f */ + retval = BMM150_OVERFLOW_OUTPUT_FLOAT; + } + + return retval; +} + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer z axis data(micro-tesla) in float. + */ +static float compensate_z(int16_t mag_data_z, uint16_t data_rhall, const struct bmm150_dev *dev) +{ + float retval = 0; + float process_comp_z0; + float process_comp_z1; + float process_comp_z2; + float process_comp_z3; + float process_comp_z4; + float process_comp_z5; + + /* Overflow condition check */ + if ((mag_data_z != BMM150_OVERFLOW_ADCVAL_ZAXIS_HALL) && (dev->trim_data.dig_z2 != 0) && + (dev->trim_data.dig_z1 != 0) && (dev->trim_data.dig_xyz1 != 0) && (data_rhall != 0)) + { + /* Processing compensation equations */ + process_comp_z0 = ((float)mag_data_z) - ((float)dev->trim_data.dig_z4); + process_comp_z1 = ((float)data_rhall) - ((float)dev->trim_data.dig_xyz1); + process_comp_z2 = (((float)dev->trim_data.dig_z3) * process_comp_z1); + process_comp_z3 = ((float)dev->trim_data.dig_z1) * ((float)data_rhall) / 32768.0f; + process_comp_z4 = ((float)dev->trim_data.dig_z2) + process_comp_z3; + process_comp_z5 = (process_comp_z0 * 131072.0f) - process_comp_z2; + retval = (process_comp_z5 / ((process_comp_z4) * 4.0f)) / 16.0f; + } + else + { + /* Overflow, set output to 0.0f */ + retval = BMM150_OVERFLOW_OUTPUT_FLOAT; + } + + return retval; +} + +#else + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer X axis data(micro-tesla) in int16_t. + */ +static int16_t compensate_x(int16_t mag_data_x, uint16_t data_rhall, const struct bmm150_dev *dev) +{ + int16_t retval; + uint16_t process_comp_x0 = 0; + int32_t process_comp_x1; + uint16_t process_comp_x2; + int32_t process_comp_x3; + int32_t process_comp_x4; + int32_t process_comp_x5; + int32_t process_comp_x6; + int32_t process_comp_x7; + int32_t process_comp_x8; + int32_t process_comp_x9; + int32_t process_comp_x10; + + /* Overflow condition check */ + if (mag_data_x != BMM150_OVERFLOW_ADCVAL_XYAXES_FLIP) + { + if (data_rhall != 0) + { + /* Availability of valid data */ + process_comp_x0 = data_rhall; + } + else if (dev->trim_data.dig_xyz1 != 0) + { + process_comp_x0 = dev->trim_data.dig_xyz1; + } + else + { + process_comp_x0 = 0; + } + + if (process_comp_x0 != 0) + { + /* Processing compensation equations */ + process_comp_x1 = ((int32_t)dev->trim_data.dig_xyz1) * 16384; + process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000); + retval = ((int16_t)process_comp_x2); + process_comp_x3 = (((int32_t)retval) * ((int32_t)retval)); + process_comp_x4 = (((int32_t)dev->trim_data.dig_xy2) * (process_comp_x3 / 128)); + process_comp_x5 = (int32_t)(((int16_t)dev->trim_data.dig_xy1) * 128); + process_comp_x6 = ((int32_t)retval) * process_comp_x5; + process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000)); + process_comp_x8 = ((int32_t)(((int16_t)dev->trim_data.dig_x2) + ((int16_t)0xA0))); + process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096); + process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9; + retval = ((int16_t)(process_comp_x10 / 8192)); + retval = (retval + (((int16_t)dev->trim_data.dig_x1) * 8)) / 16; + } + else + { + retval = BMM150_OVERFLOW_OUTPUT; + } + } + else + { + /* Overflow condition */ + retval = BMM150_OVERFLOW_OUTPUT; + } + + return retval; +} + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer Y axis data(micro-tesla) in int16_t. + */ +static int16_t compensate_y(int16_t mag_data_y, uint16_t data_rhall, const struct bmm150_dev *dev) +{ + int16_t retval; + uint16_t process_comp_y0 = 0; + int32_t process_comp_y1; + uint16_t process_comp_y2; + int32_t process_comp_y3; + int32_t process_comp_y4; + int32_t process_comp_y5; + int32_t process_comp_y6; + int32_t process_comp_y7; + int32_t process_comp_y8; + int32_t process_comp_y9; + + /* Overflow condition check */ + if (mag_data_y != BMM150_OVERFLOW_ADCVAL_XYAXES_FLIP) + { + if (data_rhall != 0) + { + /* Availability of valid data */ + process_comp_y0 = data_rhall; + } + else if (dev->trim_data.dig_xyz1 != 0) + { + process_comp_y0 = dev->trim_data.dig_xyz1; + } + else + { + process_comp_y0 = 0; + } + + if (process_comp_y0 != 0) + { + /* Processing compensation equations */ + process_comp_y1 = (((int32_t)dev->trim_data.dig_xyz1) * 16384) / process_comp_y0; + process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000); + retval = ((int16_t)process_comp_y2); + process_comp_y3 = ((int32_t) retval) * ((int32_t)retval); + process_comp_y4 = ((int32_t)dev->trim_data.dig_xy2) * (process_comp_y3 / 128); + process_comp_y5 = ((int32_t)(((int16_t)dev->trim_data.dig_xy1) * 128)); + process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512); + process_comp_y7 = ((int32_t)(((int16_t)dev->trim_data.dig_y2) + ((int16_t)0xA0))); + process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096); + process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8); + retval = (int16_t)(process_comp_y9 / 8192); + retval = (retval + (((int16_t)dev->trim_data.dig_y1) * 8)) / 16; + } + else + { + retval = BMM150_OVERFLOW_OUTPUT; + } + } + else + { + /* Overflow condition */ + retval = BMM150_OVERFLOW_OUTPUT; + } + + return retval; +} + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer Z axis data(micro-tesla) in int16_t. + */ +static int16_t compensate_z(int16_t mag_data_z, uint16_t data_rhall, const struct bmm150_dev *dev) +{ + int32_t retval; + int16_t process_comp_z0; + int32_t process_comp_z1; + int32_t process_comp_z2; + int32_t process_comp_z3; + int16_t process_comp_z4; + + if (mag_data_z != BMM150_OVERFLOW_ADCVAL_ZAXIS_HALL) + { + if ((dev->trim_data.dig_z2 != 0) && (dev->trim_data.dig_z1 != 0) && (data_rhall != 0) && + (dev->trim_data.dig_xyz1 != 0)) + { + /*Processing compensation equations */ + process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) dev->trim_data.dig_xyz1); + process_comp_z1 = (((int32_t)dev->trim_data.dig_z3) * ((int32_t)(process_comp_z0))) / 4; + process_comp_z2 = (((int32_t)(mag_data_z - dev->trim_data.dig_z4)) * 32768); + process_comp_z3 = ((int32_t)dev->trim_data.dig_z1) * (((int16_t)data_rhall) * 2); + process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536); + retval = ((process_comp_z2 - process_comp_z1) / (dev->trim_data.dig_z2 + process_comp_z4)); + + /* Saturate result to +/- 2 micro-tesla */ + if (retval > BMM150_POSITIVE_SATURATION_Z) + { + retval = BMM150_POSITIVE_SATURATION_Z; + } + else if (retval < BMM150_NEGATIVE_SATURATION_Z) + { + retval = BMM150_NEGATIVE_SATURATION_Z; + } + + /* Conversion of LSB to micro-tesla */ + retval = retval / 16; + } + else + { + retval = BMM150_OVERFLOW_OUTPUT; + } + } + else + { + /* Overflow condition */ + retval = BMM150_OVERFLOW_OUTPUT; + } + + return (int16_t)retval; +} + +#endif + +/*! + * @brief This internal API is used to perform the normal self test + * of the sensor and return the self test result as return value + */ +static int8_t perform_normal_self_test(struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t self_test_bit; + + /* Triggers the start of normal self test */ + rslt = enable_normal_self_test(&self_test_bit, dev); + + /* Check for self test completion status */ + if ((rslt == BMM150_OK) && (self_test_bit == 0)) + { + /* Validates the self test results for all 3 axes */ + rslt = validate_normal_self_test(dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable the normal self test by setting + * the Self Test bit (bit0) of the 0x4C register, + * which triggers the start of self test + */ +static int8_t enable_normal_self_test(uint8_t *self_test_enable, struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + uint8_t self_test_val; + + /* Read the data from register 0x4C */ + rslt = bmm150_get_regs(BMM150_REG_OP_MODE, ®_data, 1, dev); + + if (rslt == BMM150_OK) + { + /* Set the Self Test bit(bit0) of the 0x4C register */ + self_test_val = 1; + reg_data = BMM150_SET_BITS_POS_0(reg_data, BMM150_SELF_TEST, self_test_val); + + /* Write the data to 0x4C register to trigger self test */ + rslt = bmm150_set_regs(BMM150_REG_OP_MODE, ®_data, 1, dev); + dev->delay_us(BMM150_DELAY_NORMAL_SELF_TEST, dev->intf_ptr); + + if (rslt == BMM150_OK) + { + /* Read the data from register 0x4C */ + rslt = bmm150_get_regs(BMM150_REG_OP_MODE, ®_data, 1, dev); + + /* Self Test bit(bit0) is stored in self_test_enable, + * It will be reset to zero after the self test is over + */ + *self_test_enable = BMM150_GET_BITS_POS_0(reg_data, BMM150_SELF_TEST); + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to validate the results of normal self test + * by using the self test status available in the bit0 of registers 0x42,0x44 + * and 0x46. + */ +static int8_t validate_normal_self_test(struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t status; + uint8_t self_test_rslt[5]; + + /* Read the data from register 0x42 to 0x46 */ + rslt = bmm150_get_regs(BMM150_REG_DATA_X_LSB, self_test_rslt, BMM150_LEN_SELF_TEST, dev); + + if (rslt == BMM150_OK) + { + /* Parse and get the self test status bits */ + /* X-Self-Test (bit0) of 0x42 register is stored */ + self_test_rslt[0] = BMM150_GET_BITS_POS_0(self_test_rslt[0], BMM150_SELF_TEST); + + /* Y-Self-Test (bit0) of 0x44 register is stored */ + self_test_rslt[2] = BMM150_GET_BITS_POS_0(self_test_rslt[2], BMM150_SELF_TEST); + + /* Z-Self-Test (bit0) of 0x46 register is stored */ + self_test_rslt[4] = BMM150_GET_BITS_POS_0(self_test_rslt[4], BMM150_SELF_TEST); + + /* Combine the self test status and store it in the first + * 3 bits of the status variable for processing + */ + status = (uint8_t)((self_test_rslt[4] << 2) | (self_test_rslt[2] << 1) | self_test_rslt[0]); + + /* Validate status and store Self test result in "rslt" */ + if (status == BMM150_SELF_TEST_STATUS_SUCCESS) + { + /* Self test is success when all status bits are set */ + rslt = BMM150_OK; + } + else + { + if (status == BMM150_SELF_TEST_STATUS_XYZ_FAIL) + { + /* Self test - all axis fail condition */ + rslt = BMM150_W_NORMAL_SELF_TEST_XYZ_FAIL; + } + else + { + /* Self test - some axis fail condition */ + rslt = (int8_t)status; + } + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to perform advanced self test for Z axis + */ +static int8_t perform_adv_self_test(struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t self_test_current; + int16_t positive_data_z; + int16_t negative_data_z; + + /* Set the desired power mode ,axes control and repetition settings */ + rslt = adv_self_test_settings(dev); + + if (rslt == BMM150_OK) + { + /* Measure the Z axes data with positive self-test current */ + self_test_current = BMM150_ENABLE_POSITIVE_CURRENT; + rslt = adv_self_test_measurement(self_test_current, &positive_data_z, dev); + + if (rslt == BMM150_OK) + { + /* Measure the Z axes data with + * negative self-test current + */ + self_test_current = BMM150_ENABLE_NEGATIVE_CURRENT; + rslt = adv_self_test_measurement(self_test_current, &negative_data_z, dev); + + if (rslt == BMM150_OK) + { + /* Disable self-test current */ + self_test_current = BMM150_DISABLE_SELF_TEST_CURRENT; + rslt = set_adv_self_test_current(self_test_current, dev); + + if (rslt == BMM150_OK) + { + /* Validate the advanced self test */ + rslt = validate_adv_self_test(positive_data_z, negative_data_z); + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to set the desired power mode , + * axes control and repetition settings for advanced self test + */ +static int8_t adv_self_test_settings(struct bmm150_dev *dev) +{ + int8_t rslt; + struct bmm150_settings settings; + + /* Set the power mode as sleep mode */ + settings.pwr_mode = BMM150_POWERMODE_SLEEP; + rslt = bmm150_set_op_mode(&settings, dev); + + if (rslt == BMM150_OK) + { + /* Disable XY-axis measurement */ + settings.xyz_axes_control = BMM150_DISABLE_XY_AXIS; + rslt = set_control_measurement_xyz(&settings, dev); + + if (rslt == BMM150_OK) + { + /* Repetition value is set as 0x04 */ + settings.z_rep = BMM150_SELF_TEST_REP_Z; + rslt = set_z_rep(&settings, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to set the positive or negative value of + * self-test current and obtain the corresponding magnetometer z axis data + */ +static int8_t adv_self_test_measurement(uint8_t self_test_current, int16_t *data_z, struct bmm150_dev *dev) +{ + int8_t rslt; + struct bmm150_settings settings; + struct bmm150_mag_data mag_data; + + /* Set the advanced self test current as positive or + * negative based on the value of parameter "self_test_current" + */ + rslt = set_adv_self_test_current(self_test_current, dev); + + if (rslt == BMM150_OK) + { + /* Set the device in forced mode */ + settings.pwr_mode = BMM150_POWERMODE_FORCED; + rslt = bmm150_set_op_mode(&settings, dev); + + /* Delay to ensure measurement is complete */ + dev->delay_us(BMM150_DELAY_ADV_SELF_TEST, dev->intf_ptr); + + if (rslt == BMM150_OK) + { + /* Read Mag data and store the value of Z axis data */ + rslt = bmm150_read_mag_data(&mag_data, dev); + + if (rslt == BMM150_OK) + { + /* Mag Z axis data is stored */ + *data_z = mag_data.z; + } + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to get the difference between the + * Z axis mag data obtained by positive and negative self-test current + * and validate whether the advanced self test is done successfully or not. + */ +static int8_t validate_adv_self_test(int16_t positive_data_z, int16_t negative_data_z) +{ + int32_t adv_self_test_rslt; + int8_t rslt; + + /* Advanced self test difference between the Z axis mag data + * obtained by the positive and negative self-test current + */ + adv_self_test_rslt = positive_data_z - negative_data_z; + + /* Advanced self test validation */ + /*Value of adv_self_test_rslt should be in between 180-240 micro-tesla */ + if ((adv_self_test_rslt > 180) && (adv_self_test_rslt < 240)) + { + /* Advanced self test success */ + rslt = BMM150_OK; + } + else + { + /* Advanced self test fail */ + rslt = BMM150_W_ADV_SELF_TEST_FAIL; + } + + return rslt; +} + +/*! + * @brief This internal API is used to set the self test current value in + * the Adv. ST bits (bit6 and bit7) of 0x4C register + */ +static int8_t set_adv_self_test_current(uint8_t self_test_current, struct bmm150_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Read the 0x4C register */ + rslt = bmm150_get_regs(BMM150_REG_OP_MODE, ®_data, 1, dev); + + if (rslt == BMM150_OK) + { + /* Set the self test current value in the Adv. ST bits + * (bit6 and bit7) of 0x4c register + */ + reg_data = BMM150_SET_BITS(reg_data, BMM150_ADV_SELF_TEST, self_test_current); + rslt = bmm150_set_regs(BMM150_REG_OP_MODE, ®_data, 1, dev); + } + + return rslt; +} diff --git a/src/SensorManager/BMX160/bosch/bmm150.h b/src/SensorManager/BMX160/bosch/bmm150.h new file mode 100644 index 00000000..01f3aabe --- /dev/null +++ b/src/SensorManager/BMX160/bosch/bmm150.h @@ -0,0 +1,479 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmm150.h +* @date 2020-06-03 +* @version v2.0.0 +* +*/ + +/*! + * @defgroup bmm150 BMM150 + */ + +#ifndef _BMM150_H +#define _BMM150_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/********************************************************************/ +/* header files */ + +#include "bmm150_defs.h" + +/********************************************************************/ +/* (extern) variable declarations */ +/********************************************************************/ +/* function prototype declarations */ + +/** + * \ingroup bmm150 + * \defgroup bmm150ApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmm150ApiInit + * \page bmm150_api_bmm150_init bmm150_init + * \code + * int8_t bmm150_init(struct bmm150_dev *dev); + * \endcode + * @details This API is the entry point, Call this API before using other APIs. + * This API reads the chip-id of the sensor which is the first step to + * verify the sensor and also it configures the read mechanism of SPI and + * I2C interface. + * + * @param[in,out] dev : Structure instance of bmm150_dev + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +int8_t bmm150_init(struct bmm150_dev *dev); + +/** + * \ingroup bmm150 + * \defgroup bmm150ApiRegs Registers + * @brief Read / Write data to the given register address of the sensor + */ + +/*! + * \ingroup bmm150ApiRegs + * \page bmm150_api_bmm150_set_regs bmm150_set_regs + * \code + * int8_t bmm150_set_regs(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, struct bmm150_dev *dev); + * \endcode + * @details This API writes the given data to the register address + * of the sensor. + * + * @param[in] reg_addr : Register address from where the data to be written. + * @param[in] reg_data : Pointer to data buffer which is to be written + * in the reg_addr of sensor. + * @param[in] len : No of bytes of data to write.. + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +int8_t bmm150_set_regs(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, struct bmm150_dev *dev); + +/*! + * \ingroup bmm150ApiRegs + * \page bmm150_api_bmm150_get_regs bmm150_get_regs + * \code + * int8_t bmm150_get_regs(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, struct bmm150_dev *dev) + * \endcode + * @details This API reads the data from the given register address of sensor. + * + * @param[in] reg_addr : Register address from where the data to be read + * @param[out] reg_data : Pointer to data buffer to store the read data. + * @param[in] len : No of bytes of data to be read. + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +int8_t bmm150_get_regs(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, struct bmm150_dev *dev); + +/** + * \ingroup bmm150 + * \defgroup bmm150ApiSoftreset Soft reset + * @brief Perform soft reset of the sensor + */ + +/*! + * \ingroup bmm150ApiSoftreset + * \page bmm150_api_bmm150_soft_reset bmm150_soft_reset + * \code + * int8_t bmm150_soft_reset(struct bmm150_dev *dev); + * \endcode + * @details This API is used to perform soft-reset of the sensor + * where all the registers are reset to their default values except 0x4B. + * + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +int8_t bmm150_soft_reset(struct bmm150_dev *dev); + +/** + * \ingroup bmm150 + * \defgroup bmm150ApiPowermode Power mode + * @brief Set / Get power mode of the sensor + */ + +/*! + * \ingroup bmm150ApiPowermode + * \page bmm150_api_bmm150_set_op_mode bmm150_set_op_mode + * \code + * int8_t bmm150_set_op_mode(const struct bmm150_settings *settings, struct bmm150_dev *dev); + * \endcode + * @details This API is used to set the power mode of the sensor. + * + * @param[in] settings : Structure instance of bmm150_settings. + * @param[in] dev : Structure instance of bmm150_dev. + * + *@verbatim + * settings->pwr_mode | Power mode + * ------------------------|----------------------- + * 0x00 | BMM150_POWERMODE_NORMAL + * 0x01 | BMM150_POWERMODE_FORCED + * 0x03 | BMM150_POWERMODE_SLEEP + * 0x04 | BMM150_POWERMODE_SUSPEND + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +int8_t bmm150_set_op_mode(const struct bmm150_settings *settings, struct bmm150_dev *dev); + +/*! + * \ingroup bmm150ApiPowermode + * \page bmm150_api_bmm150_get_op_mode bmm150_get_op_mode + * \code + * int8_t bmm150_get_op_mode(uint8_t *op_mode, struct bmm150_dev *dev); + * \endcode + * @details This API is used to get the power mode of the sensor. + * + * @param[out] op_mode : power mode of the sensor. + * @param[in] dev : Structure instance of bmm150_dev. + * + *@verbatim + * op_mode | Power mode + * -------------|----------------------- + * 0x00 | BMM150_POWERMODE_NORMAL + * 0x01 | BMM150_POWERMODE_FORCED + * 0x03 | BMM150_POWERMODE_SLEEP + * 0x04 | BMM150_POWERMODE_SUSPEND + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +int8_t bmm150_get_op_mode(uint8_t *op_mode, struct bmm150_dev *dev); + +/** + * \ingroup bmm150 + * \defgroup bmm150ApiPresetmode Preset mode + * @brief Set preset mode of the sensor + */ + +/*! + * \ingroup bmm150ApiPresetmode + * \page bmm150_api_bmm150_set_presetmode bmm150_set_presetmode + * \code + * int8_t bmm150_set_presetmode(struct bmm150_settings *settings, struct bmm150_dev *dev); + * \endcode + * @details This API is used to set the preset mode of the sensor. + * + * @param[in] settings : Structure instance of bmm150_settings. + * @param[in] dev : Structure instance of bmm150_dev. + * + *@verbatim + * settings->preset_mode | Preset mode + * ---------------------------|---------------------------------- + * 0x01 | BMM150_PRESETMODE_LOWPOWER + * 0x02 | BMM150_PRESETMODE_REGULAR + * 0x03 | BMM150_PRESETMODE_HIGHACCURACY + * 0x04 | BMM150_PRESETMODE_ENHANCED + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +int8_t bmm150_set_presetmode(struct bmm150_settings *settings, struct bmm150_dev *dev); + +/** + * \ingroup bmm150 + * \defgroup bmm150ApiSensorSettings Sensor Settings + * @brief Set / Get sensor settings of the sensor + */ + +/*! + * \ingroup bmm150ApiSensorSettings + * \page bmm150_api_bmm150_set_sensor_settings bmm150_set_sensor_settings + * \code + * int8_t bmm150_set_sensor_settings(uint16_t desired_settings, const struct bmm150_settings *settings, struct bmm150_dev *dev); + * \endcode + * @details This API sets the sensor settings based on the desired_settings + * and the dev structure configuration + * + * @param[in] desired_settings : Selection macro for selecting the setting. + * @param[in] settings : Structure instance of bmm150_settings. + * @param[in] dev : Structure instance of bmm150_dev. + * + * @note Assign the sensor setting macros (multiple macros can be + * set by doing a bitwise-OR operation) to the desired_settings parameter + * of this API to perform the corresponding setting. + * + * @note threshold interrupt for each axes are set by using bitwise AND + * operation of the following macros + * - BMM150_THRESHOLD_X + * - BMM150_THRESHOLD_Y + * - BMM150_THRESHOLD_Z + * + *@verbatim + * desired_settings | Selected sensor setting macros + * -------------------|-------------------------------- + * 0x0001 | BMM150_SEL_DATA_RATE + * 0x0002 | BMM150_SEL_CONTROL_MEASURE + * 0x0004 | BMM150_SEL_XY_REP + * 0x0008 | BMM150_SEL_Z_REP + * 0x0010 | BMM150_SEL_DRDY_PIN_EN + * 0x0020 | BMM150_SEL_INT_PIN_EN + * 0x0040 | BMM150_SEL_DRDY_POLARITY + * 0x0080 | BMM150_SEL_INT_LATCH + * 0x0100 | BMM150_SEL_INT_POLARITY + * 0x0200 | BMM150_SEL_DATA_OVERRUN_INT + * 0x0400 | BMM150_SEL_OVERFLOW_INT + * 0x0800 | BMM150_SEL_HIGH_THRESHOLD_INT + * 0x1000 | BMM150_SEL_LOW_THRESHOLD_INT + * 0x2000 | BMM150_SEL_LOW_THRESHOLD_SETTING + * 0x4000 | BMM150_SEL_HIGH_THRESHOLD_SETTING + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +int8_t bmm150_set_sensor_settings(uint16_t desired_settings, + const struct bmm150_settings *settings, + struct bmm150_dev *dev); + +/*! + * \ingroup bmm150ApiSensorSettings + * \page bmm150_api_bmm150_get_sensor_settings bmm150_get_sensor_settings + * \code + * int8_t bmm150_get_sensor_settings(struct bmm150_settings *settings, struct bmm150_dev *dev); + * \endcode + * @details This API gets all the sensor settings and updates the dev structure + * + * @param[in] settings : Structure instance of bmm150_settings. + * @param[in] dev : Structure instance of bmm150_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +int8_t bmm150_get_sensor_settings(struct bmm150_settings *settings, struct bmm150_dev *dev); + +/** + * \ingroup bmm150 + * \defgroup bmm150ApiMagData Read magnetometer data + * @brief Read magnetometer data + */ + +/*! + * \ingroup bmm150ApiMagData + * \page bmm150_api_bmm150_read_mag_data bmm150_read_mag_data + * \code + * int8_t bbmm150_read_mag_data(struct bmm150_mag_data *mag_data, struct bmm150_dev *dev); + * \endcode + * @details This API reads the magnetometer data from registers 0x42 to 0x49 + * and updates the dev structure with compensated mag data in micro-tesla + * + * @param[in] mag_data : Structure instance of bmm150_mag_data. + * @param[in,out] dev : Structure instance of bmm150_dev. + * + * @note The output mag data can be obtained either in int16_t or float format + * using this API. + * @note Enable the macro "BMM150_USE_FLOATING_POINT" in the bmm150_defs.h + * file and call this API to get the mag data in float, + * disable this macro to get the mag data in int16_t format + * + *@verbatim + * Mag data output(micro-tesla) | Mag data in dev structure(int16_t/float) + * --------------------------------|------------------------------------------ + * X-axis data | mag_data->x + * Y-axis data | mag_data->y + * Z-axis data | mag_data->z + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +int8_t bmm150_read_mag_data(struct bmm150_mag_data *mag_data, struct bmm150_dev *dev); + +/** + * \ingroup bmm150 + * \defgroup bmm150ApiSelftest Self test + * @brief Perform self test + */ + +/*! + * \ingroup bmm150ApiSelftest + * \page bmm150_api_bmm150_perform_self_test bmm150_perform_self_test + * \code + * int8_t bmm150_perform_self_test(uint8_t self_test_mode, struct bmm150_dev *dev); + * \endcode + * @details This API is used to perform the complete self test + * (both normal and advanced) for the BMM150 sensor + * + * @param[in] self_test_mode : The type of self test to be performed + * @param[in] dev : Structure instance of bmm150_dev. + * + *@verbatim + * self_test_mode | Self test enabled + * --------------------|-------------------------- + * 0 | BMM150_SELF_TEST_NORMAL + * 1 | BMM150_SELF_TEST_ADVANCED + *@endverbatim + * + * @note The return value of this API gives us the result of self test. + * + * @note Performing advanced self test does soft reset of the sensor, User can + * set the desired settings after performing the advanced self test. + * + * @return Result of API execution status and self test result. + * @retval 0 BMM150_OK + * @retval 1 BMM150_W_NORMAL_SELF_TEST_YZ_FAIL + * @retval 2 BMM150_W_NORMAL_SELF_TEST_XZ_FAIL + * @retval 3 BMM150_W_NORMAL_SELF_TEST_Z_FAIL + * @retval 4 BMM150_W_NORMAL_SELF_TEST_XY_FAIL + * @retval 5 BMM150_W_NORMAL_SELF_TEST_Y_FAIL + * @retval 6 BMM150_W_NORMAL_SELF_TEST_X_FAIL + * @retval 7 BMM150_W_NORMAL_SELF_TEST_XYZ_FAIL + * @retval 8 BMM150_W_ADV_SELF_TEST_FAIL + */ +int8_t bmm150_perform_self_test(uint8_t self_test_mode, struct bmm150_dev *dev); + +/** + * \ingroup bmm150 + * \defgroup bmm150ApiInt Interrupt status + * @brief Obtain interrupt staus flags + */ + +/*! + * \ingroup bmm150ApiInt + * \page bmm150_api_bmm150_get_interrupt_status bmm150_get_interrupt_status + * \code + * int8_t bmm150_get_interrupt_status(struct bmm150_dev *dev); + * \endcode + * @details This API obtains the status flags of all interrupt + * which is used to check for the assertion of interrupts + * + * @param[in,out] dev : Structure instance of bmm150_dev. + * + * @note The status flags of all the interrupts are stored in the + * dev->int_status. + * + * @note The value of dev->int_status is performed a bitwise AND operation + * with predefined interrupt status macros to find the interrupt status + * which is either set or reset. + * + * Ex. + * if (dev->int_status & BMM150_INT_ASSERTED_DRDY) + * { + * Occurrence of data ready interrupt + * } else { + * No interrupt occurred + * } + * + * @return Result of API execution status and self test result. + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +int8_t bmm150_get_interrupt_status(struct bmm150_dev *dev); + +/** + * \ingroup bmm150 + * \defgroup bmm150ApiAux Compensate magnetometer data + * @brief Compensation of magnetometer data + */ + +/*! + * \ingroup bmm150ApiAux + * \page bmm150_api_bmm150_aux_mag_data bmm150_aux_mag_data + * \code + * int8_t bmm150_aux_mag_data(uint8_t *aux_data, struct bmm150_mag_data *mag_data, const struct bmm150_dev *dev); + * \endcode + * @details This API is used to compensate the raw mag data + * + * @param[in] aux_data : Raw mag data obtained from BMI160 registers + * @param[in] mag_data : Structure instance of bmm150_mag_data. + * @param[in,out] dev : Structure instance of bmm150_dev. + * + * @return Result of API execution status and self test result. + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +int8_t bmm150_aux_mag_data(uint8_t *aux_data, struct bmm150_mag_data *mag_data, const struct bmm150_dev *dev); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* _BMM150_H */ diff --git a/src/SensorManager/BMX160/bosch/bmm150_defs.h b/src/SensorManager/BMX160/bosch/bmm150_defs.h new file mode 100644 index 00000000..80c96ab8 --- /dev/null +++ b/src/SensorManager/BMX160/bosch/bmm150_defs.h @@ -0,0 +1,656 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmm150_defs.h +* @date 2020-06-03 +* @version v2.0.0 +* +*/ + +/*! \file bmm150_defs.h */ + +#ifndef _BMM150_DEFS_H +#define _BMM150_DEFS_H + +/******************************************************************************/ +/*! @name Header includes */ +/******************************************************************************/ +#ifdef __KERNEL__ +#include +#else +#include +#include +#endif + +/******************************************************************************/ +/*! @name Common macros */ +/******************************************************************************/ +#ifdef __KERNEL__ +#if (LONG_MAX) > 0x7fffffff +#define __have_long64 1 +#elif (LONG_MAX) == 0x7fffffff +#define __have_long32 1 +#endif +#endif + +#if !defined(UINT8_C) +#define INT8_C(x) x +#if (INT_MAX) > 0x7f +#define UINT8_C(x) x +#else +#define UINT8_C(x) x##U +#endif +#endif + +#if !defined(UINT16_C) +#define INT16_C(x) x +#if (INT_MAX) > 0x7fff +#define UINT16_C(x) x +#else +#define UINT16_C(x) x##U +#endif +#endif + +#if !defined(INT32_C) && !defined(UINT32_C) +#if __have_long32 +#define INT32_C(x) x##L +#define UINT32_C(x) x##UL +#else +#define INT32_C(x) x +#define UINT32_C(x) x##U +#endif +#endif + +#if !defined(INT64_C) && !defined(UINT64_C) +#if __have_long64 +#define INT64_C(x) x##L +#define UINT64_C(x) x##UL +#else +#define INT64_C(x) x##LL +#define UINT64_C(x) x##ULL +#endif +#endif + +/*! @name C standard macros */ +#ifndef NULL +#ifdef __cplusplus +#define NULL 0 +#else +#define NULL ((void *) 0) +#endif +#endif + +/******************************************************************************/ +/*! @name Compiler switch macros Definitions */ +/******************************************************************************/ +#ifndef BMM150_USE_FLOATING_POINT /*< Check if floating point (using BMM150_USE_FLOATING_POINT) is enabled */ +#ifndef BMM150_USE_FIXED_POINT /*< If floating point is not enabled then enable BMM150_USE_FIXED_POINT */ +#define BMM150_USE_FIXED_POINT +#endif +#endif + +/******************************************************************************/ +/*! @name General Macro Definitions */ +/******************************************************************************/ +/*! @name API success code */ +#define BMM150_OK INT8_C(0) + +/*! @name To define TRUE or FALSE */ +#define BMM150_TRUE UINT8_C(1) +#define BMM150_FALSE UINT8_C(0) + +/*! @name API error codes */ +#define BMM150_E_NULL_PTR INT8_C(-1) +#define BMM150_E_DEV_NOT_FOUND INT8_C(-2) +#define BMM150_E_INVALID_CONFIG INT8_C(-3) +#define BMM150_E_COM_FAIL INT8_C(-4) + +/*! @name API warning codes */ +#define BMM150_W_NORMAL_SELF_TEST_YZ_FAIL INT8_C(1) +#define BMM150_W_NORMAL_SELF_TEST_XZ_FAIL INT8_C(2) +#define BMM150_W_NORMAL_SELF_TEST_Z_FAIL INT8_C(3) +#define BMM150_W_NORMAL_SELF_TEST_XY_FAIL INT8_C(4) +#define BMM150_W_NORMAL_SELF_TEST_Y_FAIL INT8_C(5) +#define BMM150_W_NORMAL_SELF_TEST_X_FAIL INT8_C(6) +#define BMM150_W_NORMAL_SELF_TEST_XYZ_FAIL INT8_C(7) +#define BMM150_W_ADV_SELF_TEST_FAIL INT8_C(8) + +/*! @name CHIP ID & SOFT RESET VALUES */ +#define BMM150_CHIP_ID UINT8_C(0x32) +#define BMM150_SET_SOFT_RESET UINT8_C(0x82) + +/*! @name POWER MODE DEFINTIONS */ +#define BMM150_POWERMODE_NORMAL UINT8_C(0x00) +#define BMM150_POWERMODE_FORCED UINT8_C(0x01) +#define BMM150_POWERMODE_SLEEP UINT8_C(0x03) +#define BMM150_POWERMODE_SUSPEND UINT8_C(0x04) + +/*! @name Power mode settings */ +#define BMM150_POWER_CNTRL_DISABLE UINT8_C(0x00) +#define BMM150_POWER_CNTRL_ENABLE UINT8_C(0x01) + +/*! @name I2C ADDRESS */ +#define BMM150_DEFAULT_I2C_ADDRESS UINT8_C(0x10) +#define BMM150_I2C_ADDRESS_CSB_LOW_SDO_HIGH UINT8_C(0x11) +#define BMM150_I2C_ADDRESS_CSB_HIGH_SDO_LOW UINT8_C(0x12) +#define BMM150_I2C_ADDRESS_CSB_HIGH_SDO_HIGH UINT8_C(0x13) + +/*! @name Sensor delay time settings */ +#define BMM150_DELAY_SOFT_RESET UINT8_C(1000) +#define BMM150_DELAY_NORMAL_SELF_TEST UINT8_C(2000) +#define BMM150_START_UP_TIME UINT8_C(3000) +#define BMM150_DELAY_ADV_SELF_TEST UINT8_C(4000) + +/*! @name ENABLE/DISABLE DEFINITIONS */ +#define BMM150_XYZ_CHANNEL_ENABLE UINT8_C(0x00) +#define BMM150_XYZ_CHANNEL_DISABLE UINT8_C(0x07) + +/*! @name Register Address */ +#define BMM150_REG_CHIP_ID UINT8_C(0x40) +#define BMM150_REG_DATA_X_LSB UINT8_C(0x42) +#define BMM150_REG_DATA_READY_STATUS UINT8_C(0x48) +#define BMM150_REG_INTERRUPT_STATUS UINT8_C(0x4A) +#define BMM150_REG_POWER_CONTROL UINT8_C(0x4B) +#define BMM150_REG_OP_MODE UINT8_C(0x4C) +#define BMM150_REG_INT_CONFIG UINT8_C(0x4D) +#define BMM150_REG_AXES_ENABLE UINT8_C(0x4E) +#define BMM150_REG_LOW_THRESHOLD UINT8_C(0x4F) +#define BMM150_REG_HIGH_THRESHOLD UINT8_C(0x50) +#define BMM150_REG_REP_XY UINT8_C(0x51) +#define BMM150_REG_REP_Z UINT8_C(0x52) + +/*! @name Macros to select the sensor settings to be set by the user + * These values are internal for API implementation. Don't relate this to + * data sheet. + */ +#define BMM150_SEL_DATA_RATE UINT16_C(1) +#define BMM150_SEL_CONTROL_MEASURE UINT16_C(1 << 1) +#define BMM150_SEL_XY_REP UINT16_C(1 << 2) +#define BMM150_SEL_Z_REP UINT16_C(1 << 3) +#define BMM150_SEL_DRDY_PIN_EN UINT16_C(1 << 4) +#define BMM150_SEL_INT_PIN_EN UINT16_C(1 << 5) +#define BMM150_SEL_DRDY_POLARITY UINT16_C(1 << 6) +#define BMM150_SEL_INT_LATCH UINT16_C(1 << 7) +#define BMM150_SEL_INT_POLARITY UINT16_C(1 << 8) +#define BMM150_SEL_DATA_OVERRUN_INT UINT16_C(1 << 9) +#define BMM150_SEL_OVERFLOW_INT UINT16_C(1 << 10) +#define BMM150_SEL_HIGH_THRESHOLD_INT UINT16_C(1 << 11) +#define BMM150_SEL_LOW_THRESHOLD_INT UINT16_C(1 << 12) +#define BMM150_SEL_LOW_THRESHOLD_SETTING UINT16_C(1 << 13) +#define BMM150_SEL_HIGH_THRESHOLD_SETTING UINT16_C(1 << 14) + +/*! @name DATA RATE DEFINITIONS */ +#define BMM150_DATA_RATE_10HZ UINT8_C(0x00) +#define BMM150_DATA_RATE_02HZ UINT8_C(0x01) +#define BMM150_DATA_RATE_06HZ UINT8_C(0x02) +#define BMM150_DATA_RATE_08HZ UINT8_C(0x03) +#define BMM150_DATA_RATE_15HZ UINT8_C(0x04) +#define BMM150_DATA_RATE_20HZ UINT8_C(0x05) +#define BMM150_DATA_RATE_25HZ UINT8_C(0x06) +#define BMM150_DATA_RATE_30HZ UINT8_C(0x07) +#define BMM150_ODR_MAX UINT8_C(0x07) +#define BMM150_ODR_MSK UINT8_C(0x38) +#define BMM150_ODR_POS UINT8_C(0x03) + +/*! @name TRIM REGISTERS */ +/* Trim Extended Registers */ +#define BMM150_DIG_X1 UINT8_C(0x5D) +#define BMM150_DIG_Y1 UINT8_C(0x5E) +#define BMM150_DIG_Z4_LSB UINT8_C(0x62) +#define BMM150_DIG_Z4_MSB UINT8_C(0x63) +#define BMM150_DIG_X2 UINT8_C(0x64) +#define BMM150_DIG_Y2 UINT8_C(0x65) +#define BMM150_DIG_Z2_LSB UINT8_C(0x68) +#define BMM150_DIG_Z2_MSB UINT8_C(0x69) +#define BMM150_DIG_Z1_LSB UINT8_C(0x6A) +#define BMM150_DIG_Z1_MSB UINT8_C(0x6B) +#define BMM150_DIG_XYZ1_LSB UINT8_C(0x6C) +#define BMM150_DIG_XYZ1_MSB UINT8_C(0x6D) +#define BMM150_DIG_Z3_LSB UINT8_C(0x6E) +#define BMM150_DIG_Z3_MSB UINT8_C(0x6F) +#define BMM150_DIG_XY2 UINT8_C(0x70) +#define BMM150_DIG_XY1 UINT8_C(0x71) + +/*! @name Threshold interrupt setting macros for x,y,z axes selection */ +#define BMM150_THRESHOLD_X UINT8_C(0x06) +#define BMM150_THRESHOLD_Y UINT8_C(0x05) +#define BMM150_THRESHOLD_Z UINT8_C(0x03) + +#define BMM150_HIGH_THRESHOLD_INT_MSK UINT8_C(0x38) +#define BMM150_HIGH_THRESHOLD_INT_POS UINT8_C(0x03) +#define BMM150_LOW_THRESHOLD_INT_MSK UINT8_C(0x07) + +/*! @name User configurable interrupt setting macros */ +#define BMM150_INT_ENABLE UINT8_C(0x01) +#define BMM150_INT_DISABLE UINT8_C(0x00) +#define BMM150_ACTIVE_HIGH_POLARITY UINT8_C(0x01) +#define BMM150_ACTIVE_LOW_POLARITY UINT8_C(0x00) +#define BMM150_LATCHED UINT8_C(0x01) +#define BMM150_NON_LATCHED UINT8_C(0x00) + +/*! @name Interrupt status */ +#define BMM150_INT_THRESHOLD_X_LOW UINT16_C(0x0001) +#define BMM150_INT_THRESHOLD_Y_LOW UINT16_C(0x0002) +#define BMM150_INT_THRESHOLD_Z_LOW UINT16_C(0x0004) +#define BMM150_INT_THRESHOLD_X_HIGH UINT16_C(0x0008) +#define BMM150_INT_THRESHOLD_Y_HIGH UINT16_C(0x0010) +#define BMM150_INT_THRESHOLD_Z_HIGH UINT16_C(0x0020) +#define BMM150_INT_DATA_OVERFLOW UINT16_C(0x0040) +#define BMM150_INT_DATA_OVERRUN UINT16_C(0x0080) +#define BMM150_INT_DATA_READY UINT16_C(0x0100) + +#define BMM150_DRDY_EN_MSK UINT8_C(0x80) +#define BMM150_DRDY_EN_POS UINT8_C(0x07) +#define BMM150_DRDY_POLARITY_MSK UINT8_C(0x04) +#define BMM150_DRDY_POLARITY_POS UINT8_C(0x02) +#define BMM150_INT_PIN_EN_MSK UINT8_C(0x40) +#define BMM150_INT_PIN_EN_POS UINT8_C(0x06) +#define BMM150_INT_LATCH_MSK UINT8_C(0x02) +#define BMM150_INT_LATCH_POS UINT8_C(0x01) +#define BMM150_INT_POLARITY_MSK UINT8_C(0x01) +#define BMM150_DRDY_STATUS_MSK UINT8_C(0x01) + +/*! @name Interrupt status macros */ +#define BMM150_INT_ASSERTED_DRDY UINT16_C(0x0100) +#define BMM150_INT_ASSERTED_LOW_THRES UINT16_C(0x0007) +#define BMM150_INT_ASSERTED_HIGH_THRES UINT16_C(0x0380) + +/*! @name Power control bit macros */ +#define BMM150_PWR_CNTRL_MSK UINT8_C(0x01) +#define BMM150_CONTROL_MEASURE_MSK UINT8_C(0x38) +#define BMM150_CONTROL_MEASURE_POS UINT8_C(0x03) +#define BMM150_POWER_CONTROL_BIT_MSK UINT8_C(0x01) +#define BMM150_POWER_CONTROL_BIT_POS UINT8_C(0x00) + +/*! @name Data macros */ +#define BMM150_DATA_X_MSK UINT8_C(0xF8) +#define BMM150_DATA_X_POS UINT8_C(0x03) + +#define BMM150_DATA_Y_MSK UINT8_C(0xF8) +#define BMM150_DATA_Y_POS UINT8_C(0x03) + +#define BMM150_DATA_Z_MSK UINT8_C(0xFE) +#define BMM150_DATA_Z_POS UINT8_C(0x01) + +#define BMM150_DATA_RHALL_MSK UINT8_C(0xFC) +#define BMM150_DATA_RHALL_POS UINT8_C(0x02) + +#define BMM150_DATA_OVERRUN_INT_MSK UINT8_C(0x80) +#define BMM150_DATA_OVERRUN_INT_POS UINT8_C(0x07) + +#define BMM150_OVERFLOW_INT_MSK UINT8_C(0x40) +#define BMM150_OVERFLOW_INT_POS UINT8_C(0x06) + +/*! @name OVERFLOW DEFINITIONS */ +#define BMM150_OVERFLOW_ADCVAL_XYAXES_FLIP INT16_C(-4096) +#define BMM150_OVERFLOW_ADCVAL_ZAXIS_HALL INT16_C(-16384) +#define BMM150_OVERFLOW_OUTPUT INT16_C(-32768) +#define BMM150_NEGATIVE_SATURATION_Z INT16_C(-32767) +#define BMM150_POSITIVE_SATURATION_Z INT16_C(32767) +#ifdef BMM150_USE_FLOATING_POINT +#define BMM150_OVERFLOW_OUTPUT_FLOAT 0.0f +#endif + +/*! @name PRESET MODE DEFINITIONS */ +#define BMM150_PRESETMODE_LOWPOWER UINT8_C(0x01) +#define BMM150_PRESETMODE_REGULAR UINT8_C(0x02) +#define BMM150_PRESETMODE_HIGHACCURACY UINT8_C(0x03) +#define BMM150_PRESETMODE_ENHANCED UINT8_C(0x04) + +#define BMM150_OP_MODE_MSK UINT8_C(0x06) +#define BMM150_OP_MODE_POS UINT8_C(0x01) + +/*! @name PRESET MODES - REPETITIONS-XY RATES */ +#define BMM150_REPXY_LOWPOWER UINT8_C(0x01) +#define BMM150_REPXY_REGULAR UINT8_C(0x04) +#define BMM150_REPXY_ENHANCED UINT8_C(0x07) +#define BMM150_REPXY_HIGHACCURACY UINT8_C(0x17) + +/*! @name PRESET MODES - REPETITIONS-Z RATES */ +#define BMM150_REPZ_LOWPOWER UINT8_C(0x01) +#define BMM150_REPZ_REGULAR UINT8_C(0x07) +#define BMM150_REPZ_ENHANCED UINT8_C(0x0D) +#define BMM150_REPZ_HIGHACCURACY UINT8_C(0x29) + +/*! @name Self test settings */ +#define BMM150_DISABLE_XY_AXIS UINT8_C(0x03) +#define BMM150_SELF_TEST_REP_Z UINT8_C(0x04) + +/*! @name Self test selection macros */ +#define BMM150_SELF_TEST_NORMAL UINT8_C(0) +#define BMM150_SELF_TEST_ADVANCED UINT8_C(1) + +/*! @name Advanced self-test current settings */ +#define BMM150_DISABLE_SELF_TEST_CURRENT UINT8_C(0x00) +#define BMM150_ENABLE_NEGATIVE_CURRENT UINT8_C(0x02) +#define BMM150_ENABLE_POSITIVE_CURRENT UINT8_C(0x03) + +/*! @name Normal self-test status */ +#define BMM150_SELF_TEST_STATUS_XYZ_FAIL UINT8_C(0x00) +#define BMM150_SELF_TEST_STATUS_SUCCESS UINT8_C(0x07) + +#define BMM150_SELF_TEST_MSK UINT8_C(0x01) +#define BMM150_ADV_SELF_TEST_MSK UINT8_C(0xC0) +#define BMM150_ADV_SELF_TEST_POS UINT8_C(0x06) + +/*! @name Register read lengths */ +#define BMM150_LEN_SELF_TEST UINT8_C(5) +#define BMM150_LEN_SETTING_DATA UINT8_C(8) +#define BMM150_LEN_XYZR_DATA UINT8_C(8) + +/*! @name Boundary check macros */ +#define BMM150_BOUNDARY_MAXIMUM UINT8_C(0) +#define BMM150_BOUNDARY_MINIMUM UINT8_C(1) + +/*! @name Macro to SET and GET BITS of a register*/ +#define BMM150_SET_BITS(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MSK)) | \ + ((data << bitname##_POS) & bitname##_MSK)) + +#define BMM150_GET_BITS(reg_data, bitname) ((reg_data & (bitname##_MSK)) >> \ + (bitname##_POS)) + +#define BMM150_SET_BITS_POS_0(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MSK)) | \ + (data & bitname##_MSK)) + +#define BMM150_GET_BITS_POS_0(reg_data, bitname) (reg_data & (bitname##_MSK)) + +/********************************************************/ + +/*! + * @brief Interface selection Enums + */ +enum bmm150_intf { + /*! SPI interface */ + BMM150_SPI_INTF, + /*! I2C interface */ + BMM150_I2C_INTF +}; + +/******************************************************************************/ +/*! @name Function Pointers */ +/******************************************************************************/ +#ifndef BMM150_INTF_RET_TYPE +#define BMM150_INTF_RET_TYPE int8_t +#endif + +#ifndef BMM150_INTF_RET_SUCCESS +#define BMM150_INTF_RET_SUCCESS INT8_C(0) +#endif + +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific read functions of the user + * + * @param[in] reg_addr : 8bit register address of the sensor + * @param[out] reg_data : Data from the specified address + * @param[in] length : Length of the reg_data array + * @param[in,out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related callbacks + * @retval 0 for Success + * @retval Non-zero for Failure + */ +typedef BMM150_INTF_RET_TYPE (*bmm150_read_fptr_t)(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, + void *intf_ptr); + +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific write functions of the user + * + * @param[in] reg_addr : 8bit register address of the sensor + * @param[out] reg_data : Data to the specified address + * @param[in] length : Length of the reg_data array + * @param[in,out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related callbacks + * @retval 0 for Success + * @retval Non-zero for Failure + * + */ +typedef BMM150_INTF_RET_TYPE (*bmm150_write_fptr_t)(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, + void *intf_ptr); + +/*! + * @brief Delay function pointer which should be mapped to + * delay function of the user + * + * @param period : The time period in microseconds + * @param[in,out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related callbacks + */ +typedef void (*bmm150_delay_us_fptr_t)(uint32_t period, void *intf_ptr); + +/******************************************************************************/ +/*! @name Structure Declarations */ +/******************************************************************************/ + +/*! + * @brief bmm150 trim data structure + */ +struct bmm150_trim_registers +{ + /*! trim x1 data */ + int8_t dig_x1; + + /*! trim y1 data */ + int8_t dig_y1; + + /*! trim x2 data */ + int8_t dig_x2; + + /*! trim y2 data */ + int8_t dig_y2; + + /*! trim z1 data */ + uint16_t dig_z1; + + /*! trim z2 data */ + int16_t dig_z2; + + /*! trim z3 data */ + int16_t dig_z3; + + /*! trim z4 data */ + int16_t dig_z4; + + /*! trim xy1 data */ + uint8_t dig_xy1; + + /*! trim xy2 data */ + int8_t dig_xy2; + + /*! trim xyz1 data */ + uint16_t dig_xyz1; +}; + +/*! + * @brief bmm150 interrupt pin settings + */ +struct bmm150_int_ctrl_settings +{ + /*! Data ready interrupt enable */ + uint8_t drdy_pin_en; + + /*! Threshold and overflow interrupts enable */ + uint8_t int_pin_en; + + /*! Data ready interrupt polarity Active high/low */ + uint8_t drdy_polarity; + + /*! Interrupt pin - Latched or Non-latched */ + uint8_t int_latch; + + /*! Interrupt polarity Active high/low */ + uint8_t int_polarity; + + /*! Data overrun interrupt enable */ + uint8_t data_overrun_en; + + /*! Overflow interrupt enable */ + uint8_t overflow_int_en; + + /*! high interrupt enable/disable axis selection */ + uint8_t high_int_en; + + /*! low interrupt enable/disable axis selection */ + uint8_t low_int_en; + + /*! low threshold limit */ + uint8_t low_threshold; + + /*! high threshold limit */ + uint8_t high_threshold; +}; + +/*! + * @brief bmm150 sensor settings + */ +struct bmm150_settings +{ + /*! Control measurement of XYZ axes */ + uint8_t xyz_axes_control; + + /*! Power mode of sensor */ + uint8_t pwr_mode; + + /*! Data rate value (ODR) */ + uint8_t data_rate; + + /*! XY Repetitions */ + uint8_t xy_rep; + + /*! Z Repetitions */ + uint8_t z_rep; + + /*! Preset mode of sensor */ + uint8_t preset_mode; + + /*! Interrupt configuration settings */ + struct bmm150_int_ctrl_settings int_settings; +}; + +/*! + * @brief bmm150 un-compensated (raw) magnetometer data + */ +struct bmm150_raw_mag_data +{ + /*! Raw mag X data */ + int16_t raw_datax; + + /*! Raw mag Y data */ + int16_t raw_datay; + + /*! Raw mag Z data */ + int16_t raw_dataz; + + /*! Raw mag resistance value */ + uint16_t raw_data_r; +}; + +#ifdef BMM150_USE_FLOATING_POINT + +/*! + * @brief bmm150 compensated magnetometer data in float + */ +struct bmm150_mag_data +{ + /*! compensated mag X data */ + float x; + + /*! compensated mag Y data */ + float y; + + /*! compensated mag Z data */ + float z; +}; + +#else + +/*! + * @brief bmm150 compensated magnetometer data in int16_t format + */ +struct bmm150_mag_data +{ + /*! compensated mag X data */ + int16_t x; + + /*! compensated mag Y data */ + int16_t y; + + /*! compensated mag Z data */ + int16_t z; +}; + +#endif + +/*! + * @brief bmm150 device structure + */ +struct bmm150_dev +{ + /*! Chip Id */ + uint8_t chip_id; + + /*! SPI/I2C Interface */ + enum bmm150_intf intf; + + /*! + * The interface pointer is used to enable the user + * to link their interface descriptors for reference during the + * implementation of the read and write interfaces to the + * hardware. + */ + void *intf_ptr; + + /*! Variable that holds result of read/write function */ + BMM150_INTF_RET_TYPE intf_rslt; + + /*! Bus read function pointer */ + bmm150_read_fptr_t read; + + /*! Bus write function pointer */ + bmm150_write_fptr_t write; + + /*! delay(in us) function pointer */ + bmm150_delay_us_fptr_t delay_us; + + /*! Trim registers */ + struct bmm150_trim_registers trim_data; + + /*! Interrupt status */ + uint16_t int_status; + + /*! Power control bit value */ + uint8_t pwr_cntrl_bit; +}; + +#endif /* BMM150_DEFS_H_ */ diff --git a/src/SensorManager/Baro.cpp b/src/SensorManager/Baro.cpp index f8340a08..4f698989 100644 --- a/src/SensorManager/Baro.cpp +++ b/src/SensorManager/Baro.cpp @@ -15,54 +15,41 @@ Adafruit_BMP3XX Baro::bmp; Baro Baro::sensor; -static int baro_initial_discard = 1; - // Initialisierung der SampleRateSettings für Baro (BMP3) -const SampleRateSetting<18> Baro::sample_rates = { - { BMP3_ODR_0_001_HZ, BMP3_ODR_0_003_HZ, BMP3_ODR_0_006_HZ, BMP3_ODR_0_01_HZ, - BMP3_ODR_0_02_HZ, BMP3_ODR_0_05_HZ, BMP3_ODR_0_1_HZ, BMP3_ODR_0_2_HZ, - BMP3_ODR_0_39_HZ, BMP3_ODR_0_78_HZ, BMP3_ODR_1_5_HZ, BMP3_ODR_3_1_HZ, - BMP3_ODR_6_25_HZ, BMP3_ODR_12_5_HZ, BMP3_ODR_25_HZ, BMP3_ODR_50_HZ, - BMP3_ODR_100_HZ, BMP3_ODR_200_HZ }, // reg_vals - - { 0.001, 0.003, 0.006, 0.01, 0.02, 0.05, 0.1, 0.2, - 0.39, 0.78, 1.5, 3.1, 6.25, 12.5, 25.0, 50.0, - 100.0, 200.0 }, // sample_rates - - { 0.001, 0.003, 0.006, 0.01, 0.02, 0.05, 0.1, 0.2, - 0.39, 0.78, 1.5, 3.1, 6.25, 12.5, 25.0, 50.0, - 100.0, 200.0 } // true_sample_rates +const SampleRateSetting<4> Baro::sample_rates = { + { BMP3_ODR_25_HZ, BMP3_ODR_50_HZ, BMP3_ODR_100_HZ, BMP3_ODR_200_HZ }, // reg_vals + { 25.0, 50.0, 100.0, 200.0 }, // sample_rates + { 25.0, 50.0, 100.0, 200.0 } // true_sample_rates }; void Baro::update_sensor(struct k_work *work) { ARG_UNUSED(work); int ret; - bmp.performReading(); - - if (baro_initial_discard > 0) { - baro_initial_discard--; - return; - } - - msg_baro.sd = sensor._sd_logging; - msg_baro.stream = sensor._ble_stream; - - msg_baro.data.id = ID_TEMP_BARO; - msg_baro.data.size = 2 * sizeof(float); - msg_baro.data.time = micros(); - - float data[2] = { - static_cast(bmp.temperature), - static_cast(bmp.pressure), - }; - - memcpy(msg_baro.data.data, data, 2 * sizeof(float)); - - ret = k_msgq_put(sensor_queue, &msg_baro, K_NO_WAIT); - if (ret) { - LOG_WRN("sensor msg queue full"); - } + if (!sensor._running) return; + const uint64_t read_start = micros(); + const int count = bmp.readFifo(sensor.samples, ARRAY_SIZE(sensor.samples)); + if (count < 0) { + LOG_WRN("Pressure FIFO read failed"); + return; + } + if (count == 0) return; + sensor.sample_clock.begin(read_start, count, sensor.sample_period_us, + ARRAY_SIZE(sensor.samples), CONFIG_SENSOR_CLOCK_ACCURACY); + for (int i = 0; i < count; ++i) { + msg_baro.sd = sensor._sd_logging; + msg_baro.stream = sensor._ble_stream; + msg_baro.data.id = ID_TEMP_BARO; + msg_baro.data.size = 2 * sizeof(float); + msg_baro.data.time = sensor.sample_clock.timestamp(i); + const float data[2] = {static_cast(sensor.samples[i].temperature), + static_cast(sensor.samples[i].pressure)}; + memcpy(msg_baro.data.data, data, sizeof(data)); + ret = k_msgq_put(sensor_queue, &msg_baro, K_NO_WAIT); + if (ret) { + LOG_WRN("sensor msg queue full"); + } + } } /** @@ -96,16 +83,20 @@ bool Baro::init(struct k_msgq * queue) { } void Baro::start(int sample_rate_idx) { - baro_initial_discard = 1; - - k_timeout_t t = K_USEC(1000000.0f / sample_rates.true_sample_rates[sample_rate_idx]); - - //bmp.set_interrogation_rate(setting.reg_val); - //bmp.start(); - - k_timer_start(&sensor.sensor_timer, K_NO_WAIT, t); + if (!_active) return; + const uint8_t odr = sample_rates.reg_vals[sample_rate_idx]; + if (!bmp.startContinuous(odr)) { + LOG_ERR("Failed to start pressure sampling"); + return; + } - _running = true; + sample_period_us = + 1000000.0 / static_cast(sample_rates.true_sample_rates[sample_rate_idx]); + sample_clock.reset(); + // Retain individual acquisition times while amortizing the bus reads. + const k_timeout_t interval = K_USEC(MAX(20000, sample_period_us)); + _running = true; + k_timer_start(&sensor.sensor_timer, interval, interval); } void Baro::stop() { @@ -115,6 +106,11 @@ void Baro::stop() { _running = false; k_timer_stop(&sensor.sensor_timer); + struct k_work_sync sync; + k_work_cancel_sync(&sensor.sensor_work, &sync); + if (!bmp.stopContinuous()) { + LOG_WRN("Failed to stop pressure sampling"); + } pm_device_runtime_put(ls_1_8); } diff --git a/src/SensorManager/Baro.h b/src/SensorManager/Baro.h index 42c81058..ba0ee859 100644 --- a/src/SensorManager/Baro.h +++ b/src/SensorManager/Baro.h @@ -2,6 +2,7 @@ #define BARO_H #include "EdgeMLSensor.h" +#include "FifoSampleClock.h" #include "openearable_common.h" #include "zbus_common.h" @@ -16,15 +17,18 @@ class Baro : public EdgeMlSensor { void start(int sample_rate_idx) override; void stop() override; - const static SampleRateSetting<18> sample_rates; + const static SampleRateSetting<4> sample_rates; private: static Adafruit_BMP3XX bmp; bool _active = false; + double sample_period_us = 0; + FifoSampleClock sample_clock; + struct bmp3_data samples[512 / 7]; static void update_sensor(struct k_work *work); static void sensor_timer_handler(struct k_timer *dummy); }; -#endif \ No newline at end of file +#endif diff --git a/src/SensorManager/BoneConduction.cpp b/src/SensorManager/BoneConduction.cpp index 26d4f648..860337b4 100644 --- a/src/SensorManager/BoneConduction.cpp +++ b/src/SensorManager/BoneConduction.cpp @@ -12,19 +12,21 @@ BoneConduction BoneConduction::sensor; static struct sensor_msg msg_bc; -const SampleRateSetting<10> BoneConduction::sample_rates = { - { BMA5_ACC_ODR_HZ_12P5, BMA5_ACC_ODR_HZ_25, BMA5_ACC_ODR_HZ_50, BMA5_ACC_ODR_HZ_100, +const SampleRateSetting<9> BoneConduction::sample_rates = { + { BMA5_ACC_ODR_HZ_25, BMA5_ACC_ODR_HZ_50, BMA5_ACC_ODR_HZ_100, BMA5_ACC_ODR_HZ_200, BMA5_ACC_ODR_HZ_400, BMA5_ACC_ODR_HZ_800, BMA5_ACC_ODR_HZ_1K6, BMA5_ACC_ODR_HZ_3K2, BMA5_ACC_ODR_HZ_6K4 }, // reg_vals - { 12.5, 25.0, 50.0, 100.0, 200.0, 400.0, 800.0, 1600.0, 3200.0, 6400.0 }, // sample_rates + { 25.0, 50.0, 100.0, 200.0, 400.0, 800.0, 1600.0, 3200.0, 6400.0 }, // sample_rates - { 12.5, 25.0, 50.0, 100.0, 200.0, 400.0, 800.0, 1600.0, 3200.0, 6400.0 } // true_sample_rates + { 25.0, 50.0, 100.0, 200.0, 400.0, 800.0, 1600.0, 3200.0, 6400.0 } // true_sample_rates }; bool BoneConduction::init(struct k_msgq * queue) { if (!_active) { pm_device_runtime_get(ls_1_8); + pm_device_runtime_get(ls_3_3); + k_msleep(50); _active = true; } @@ -32,6 +34,7 @@ bool BoneConduction::init(struct k_msgq * queue) { LOG_WRN("Could not find a valid bone conduction sensor, check wiring!"); _active = false; pm_device_runtime_put(ls_1_8); + pm_device_runtime_put(ls_3_3); return false; } @@ -49,19 +52,12 @@ void BoneConduction::reset() { void BoneConduction::update_sensor(struct k_work *work) { ARG_UNUSED(work); - uint64_t _time_stamp = micros(); - - BoneConduction::sensor._sample_count += (_time_stamp - BoneConduction::sensor._last_time_stamp) / BoneConduction::sensor.t_sample_us; - BoneConduction::sensor._last_time_stamp = _time_stamp; - - if (BoneConduction::sensor._sample_count < BoneConduction::sensor._num_samples_buffered * (1.f - CONFIG_SENSOR_CLOCK_ACCURACY / 100.f)) { - return; - } - + if (!sensor._running) return; int num_samples = sensor.bma580.read(sensor.fifo_acc_data); if (num_samples > 0) { - BoneConduction::sensor._sample_count = MAX(0, BoneConduction::sensor._num_samples_buffered - num_samples); + sensor.sample_clock.begin(micros(), num_samples, sensor.t_sample_us, + 1024 / (3 * sizeof(int16_t)), CONFIG_SENSOR_CLOCK_ACCURACY); } int written = 0; @@ -71,6 +67,7 @@ void BoneConduction::update_sensor(struct k_work *work) { while (written < num_samples) { int to_write = MIN((SENSOR_DATA_FIXED_LENGTH - sizeof(uint16_t)) / _size, num_samples - written); if (to_write <= 0) break; + if (sensor.sample_clock.period() > UINT16_MAX) to_write = 1; msg_bc.sd = sensor._sd_logging; msg_bc.stream = sensor._ble_stream; @@ -78,11 +75,10 @@ void BoneConduction::update_sensor(struct k_work *work) { msg_bc.data.id = ID_BONE_CONDUCTION; msg_bc.data.size = to_write * _size + sizeof(uint16_t); - uint64_t dt_us = (uint64_t)((double)(num_samples - written) * (double)BoneConduction::sensor.t_sample_us); - msg_bc.data.time = _time_stamp - dt_us; + msg_bc.data.time = sensor.sample_clock.timestamp(written); if (to_write > 1) { - uint16_t t_diff = BoneConduction::sensor.t_sample_us; + uint16_t t_diff = sensor.sample_clock.period(); for (int i = 0; i < to_write; i++) { memcpy(&msg_bc.data.data[i * _size], &sensor.fifo_acc_data[written + i], _size); } @@ -113,19 +109,16 @@ void BoneConduction::start(int sample_rate_idx) { t_sample_us = 1000000.0f / sample_rates.true_sample_rates[sample_rate_idx]; - k_timeout_t t = K_USEC(t_sample_us); - - int word_size = 3 * sizeof(int16_t) + 1; - _num_samples_buffered = MIN(MAX(1, (int) (CONFIG_SENSOR_LATENCY_MS * 1000.0f / t_sample_us)), 512 / word_size - 8); // Buffer size is 512 bytes + _num_samples_buffered = MIN(MAX(1, (int) (CONFIG_SENSOR_LATENCY_MS * 1000.0f / t_sample_us)), 1024 / (3 * sizeof(int16_t)) / 2); // Leave half the FIFO for scheduling latency - bma580.init(sample_rates.reg_vals[sample_rate_idx], _num_samples_buffered * word_size); + bma580.init(sample_rates.reg_vals[sample_rate_idx], _num_samples_buffered * 3 * sizeof(int16_t)); bma580.start(); - k_timer_start(&sensor.sensor_timer, K_NO_WAIT, t); - + /* Drain the FIFO once per buffered block instead of once per sample. */ + k_timeout_t t = K_USEC(t_sample_us * _num_samples_buffered); _running = true; - _sample_count = 0; - _last_time_stamp = micros(); + sample_clock.reset(); + k_timer_start(&sensor.sensor_timer, t, t); } void BoneConduction::stop() { @@ -135,8 +128,11 @@ void BoneConduction::stop() { _running = false; k_timer_stop(&sensor.sensor_timer); + struct k_work_sync sync; + k_work_cancel_sync(&sensor.sensor_work, &sync); bma580.stop(); pm_device_runtime_put(ls_1_8); + pm_device_runtime_put(ls_3_3); } diff --git a/src/SensorManager/BoneConduction.h b/src/SensorManager/BoneConduction.h index f285d2be..e6d07ae2 100644 --- a/src/SensorManager/BoneConduction.h +++ b/src/SensorManager/BoneConduction.h @@ -7,11 +7,13 @@ //#include "MAX30102/MAX30102.h" #include "BMA580/BMA580_Sensor.h" #include "EdgeMLSensor.h" +#include "FifoSampleClock.h" #include "openearable_common.h" #include "zbus_common.h" -#define ACCEL_FRAME_LEN UINT8_C(255) +// The 1024-byte FIFO can retain at most 170 six-byte XYZ payloads. +#define ACCEL_FRAME_LEN UINT8_C(170) class BoneConduction : public EdgeMlSensor { public: @@ -23,7 +25,7 @@ class BoneConduction : public EdgeMlSensor { void reset(); - const static SampleRateSetting<10> sample_rates; + const static SampleRateSetting<9> sample_rates; private: BMA580 bma580; @@ -42,10 +44,9 @@ class BoneConduction : public EdgeMlSensor { bool _active = false; float t_sample_us; + FifoSampleClock sample_clock; int _num_samples_buffered; - float _sample_count = 0; - uint64_t _last_time_stamp = 0; }; #endif diff --git a/src/SensorManager/CMakeLists.txt b/src/SensorManager/CMakeLists.txt index 36f09f91..8b592389 100644 --- a/src/SensorManager/CMakeLists.txt +++ b/src/SensorManager/CMakeLists.txt @@ -14,5 +14,4 @@ target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/Temp.cpp ${CMAKE_CURRENT_SOURCE_DIR}/BoneConduction.cpp ${CMAKE_CURRENT_SOURCE_DIR}/Microphone.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/Microphone.c ) diff --git a/src/SensorManager/EdgeMLSensor.h b/src/SensorManager/EdgeMLSensor.h index 875ee7cd..37a9ed1e 100644 --- a/src/SensorManager/EdgeMLSensor.h +++ b/src/SensorManager/EdgeMLSensor.h @@ -2,6 +2,7 @@ #define EDGE_ML_SENSOR_H #include +#include /* #define _EXTRACT_SAMPLE_RATES(samplerates, i, num_samplerates) \ @@ -60,8 +61,9 @@ class EdgeMlSensor { static k_msgq * sensor_queue; bool _sd_logging = false; - bool _ble_stream = true; + // Routing may change while the acquisition worker is publishing samples. + std::atomic _ble_stream{true}; bool _running = false; }; -#endif \ No newline at end of file +#endif diff --git a/src/SensorManager/FifoSampleClock.h b/src/SensorManager/FifoSampleClock.h new file mode 100644 index 00000000..cde279d3 --- /dev/null +++ b/src/SensorManager/FifoSampleClock.h @@ -0,0 +1,61 @@ +#ifndef FIFO_SAMPLE_CLOCK_H +#define FIFO_SAMPLE_CLOCK_H + +#include +#include + +// FIFO polling times are noisy observations of an independently clocked stream. +// Advance by samples, retaining fractional microseconds, and track clock error +// gradually instead of independently backdating each FIFO read. +class FifoSampleClock { +public: + void reset() { initialized = false; } + + void begin(uint64_t read_us, uint32_t count, double nominal_us, + uint32_t capacity, double accuracy_percent) { + const double span = count * nominal_us; + if (!initialized || read_us < previous_read) { + anchor(read_us, span, nominal_us); + } else { + const double observed = static_cast(read_us - epoch) - span; + const double error = observed - next; + // A pause longer than the FIFO can retain may have lost samples. + // Re-anchor forward, preserving the gap instead of filling it in. + if (read_us - previous_read > capacity * nominal_us && + error > 2 * nominal_us) { + anchor(read_us, span, nominal_us); + } else { + const double correction = error / std::max(count, 100000.0 / nominal_us); + const double limit = nominal_us * accuracy_percent / 100.0; + period_us = nominal_us + std::max(-limit, std::min(limit, correction)); + } + } + start = next; + next += count * period_us; + previous_read = read_us; + initialized = true; + } + + uint64_t timestamp(uint32_t index) const { + return epoch + static_cast(start + index * period_us); + } + + double period() const { return period_us; } + +private: + void anchor(uint64_t read_us, double span, double nominal_us) { + const uint64_t back = static_cast(span); + epoch = read_us > back ? read_us - back : 0; + next = 0; + period_us = nominal_us; + } + + bool initialized = false; + uint64_t epoch = 0; + uint64_t previous_read = 0; + double next = 0; + double start = 0; + double period_us = 0; +}; + +#endif diff --git a/src/SensorManager/IMU.cpp b/src/SensorManager/IMU.cpp index cf7e9e49..3c47eb69 100644 --- a/src/SensorManager/IMU.cpp +++ b/src/SensorManager/IMU.cpp @@ -3,7 +3,6 @@ #include "SensorManager.h" #include -#include #include #include @@ -11,13 +10,13 @@ LOG_MODULE_DECLARE(BMX160); static struct sensor_msg msg_imu; -DFRobot_BMX160 IMU::imu(&I2C3); +BMX160 IMU::imu(&I2C3); IMU IMU::sensor; const SampleRateSetting<6> IMU::sample_rates = { - { BMX160_GYRO_ODR_25HZ, BMX160_GYRO_ODR_50HZ, BMX160_GYRO_ODR_100HZ, - BMX160_GYRO_ODR_200HZ, BMX160_GYRO_ODR_400HZ, BMX160_GYRO_ODR_800HZ }, + { BMI160_GYRO_ODR_25HZ, BMI160_GYRO_ODR_50HZ, BMI160_GYRO_ODR_100HZ, + BMI160_GYRO_ODR_200HZ, BMI160_GYRO_ODR_400HZ, BMI160_GYRO_ODR_800HZ }, { 25, 50, 100, 200, 400, 800 }, @@ -26,31 +25,35 @@ const SampleRateSetting<6> IMU::sample_rates = { void IMU::update_sensor(struct k_work *work) { ARG_UNUSED(work); - int ret; + if (!sensor._running) return; + const int num_samples = imu.read(sensor.sample_buffer, sensor.MAX_BUFFERED_SAMPLES); + const uint64_t read_finished_us = micros(); - sBmx160SensorData_t magno_data; - sBmx160SensorData_t gyro_data; - sBmx160SensorData_t accel_data; - - imu.getAllData(&magno_data, &gyro_data, &accel_data); - - size_t size = 3 * sizeof(float); - - msg_imu.sd = sensor._sd_logging; - msg_imu.stream = sensor._ble_stream; - - msg_imu.data.id = ID_IMU; - msg_imu.data.size = 3 * size; - msg_imu.data.time = micros(); - - memcpy(msg_imu.data.data, &accel_data,size); - memcpy(msg_imu.data.data + size, &gyro_data, size); - memcpy(msg_imu.data.data + 2 * size, &magno_data, size); - - ret = k_msgq_put(sensor_queue, &msg_imu, K_NO_WAIT); - if (ret) { - LOG_WRN("sensor msg queue full"); + if (num_samples < 0) { + LOG_WRN("BMX160 FIFO read failed: %d", num_samples); + return; } + if (num_samples == 0) return; + sensor.sample_clock.begin(read_finished_us, num_samples, sensor.t_sample_us, + BMX160::MAX_FIFO_SAMPLES, CONFIG_SENSOR_CLOCK_ACCURACY); + + for (int i = 0; i < num_samples; ++i) { + msg_imu.sd = sensor._sd_logging; + msg_imu.stream = sensor._ble_stream; + msg_imu.data.id = ID_IMU; + msg_imu.data.size = 9 * sizeof(float); + msg_imu.data.time = sensor.sample_clock.timestamp(i); + + memcpy(msg_imu.data.data, sensor.sample_buffer[i].accel, 3 * sizeof(float)); + memcpy(msg_imu.data.data + 3 * sizeof(float), sensor.sample_buffer[i].gyro, 3 * sizeof(float)); + memcpy(msg_imu.data.data + 6 * sizeof(float), sensor.sample_buffer[i].mag, 3 * sizeof(float)); + + const int ret = k_msgq_put(sensor_queue, &msg_imu, K_NO_WAIT); + if (ret) { + LOG_WRN("sensor msg queue full"); + } + } + } /** @@ -68,15 +71,13 @@ bool IMU::init(struct k_msgq * queue) { _active = true; } - if (!imu.begin()) { // hardware I2C mode, can pass in address & alt Wire + if (!imu.init()) { LOG_ERR("Could not find a valid BMX160 sensor, check wiring!"); pm_device_runtime_put(ls_1_8); _active = false; return false; } - imu.setAccelRange(eAccelRange_2G); - sensor_queue = queue; k_work_init(&sensor.sensor_work, update_sensor); @@ -88,13 +89,27 @@ bool IMU::init(struct k_msgq * queue) { void IMU::start(int sample_rate_idx) { if (!_active) return; - k_timeout_t t = K_USEC(1000000.0f / sample_rates.true_sample_rates[sample_rate_idx]); + t_sample_us = 1e6f / sample_rates.true_sample_rates[sample_rate_idx]; + const uint8_t num_samples_buffered = MIN(MAX(1, (int)(CONFIG_SENSOR_LATENCY_MS * 1e3f / t_sample_us)), + (int)MAX_BUFFERED_SAMPLES); + + const int result = imu.start(sample_rates.reg_vals[sample_rate_idx], + sample_rates.true_sample_rates[sample_rate_idx], num_samples_buffered); + if (result != 0) { + LOG_ERR("BMX160 FIFO start failed: %d", result); + return; + } - imu.setAccelODR(sample_rates.reg_vals[sample_rate_idx]); + const uint32_t poll_period_us = (uint32_t)(num_samples_buffered * t_sample_us); + const k_timeout_t t = K_USEC(poll_period_us); + sample_clock.reset(); _running = true; - k_timer_start(&sensor.sensor_timer, K_NO_WAIT, t); + // Let the FIFO collect the first complete batch before the first burst read. + k_timer_start(&sensor.sensor_timer, t, t); + LOG_INF("BMX160 FIFO started: rate=%dHz buffered=%u poll=%uus", + (int)sample_rates.true_sample_rates[sample_rate_idx], num_samples_buffered, poll_period_us); } void IMU::stop() { @@ -104,9 +119,10 @@ void IMU::stop() { _running = false; k_timer_stop(&sensor.sensor_timer); + struct k_work_sync sync; + k_work_cancel_sync(&sensor.sensor_work, &sync); - // turn off imu (?) - imu.softReset(); + imu.stop(); pm_device_runtime_put(ls_1_8); } diff --git a/src/SensorManager/IMU.h b/src/SensorManager/IMU.h index 001a50e0..abc0d3e0 100644 --- a/src/SensorManager/IMU.h +++ b/src/SensorManager/IMU.h @@ -4,7 +4,8 @@ #include "EdgeMLSensor.h" #include "openearable_common.h" -#include "BMX160/DFRobot_BMX160.h" +#include "BMX160/BMX160.h" +#include "FifoSampleClock.h" class IMU : public EdgeMlSensor { public: @@ -16,10 +17,12 @@ class IMU : public EdgeMlSensor { const static SampleRateSetting<6> sample_rates; private: - static DFRobot_BMX160 imu; + static BMX160 imu; - //const static int num_sample_rates = 6; - //const static sample_rate_setting sample_rates[num_sample_rates]; + static constexpr uint8_t MAX_BUFFERED_SAMPLES = BMX160::MAX_FIFO_SAMPLES; + BMX160Sample sample_buffer[MAX_BUFFERED_SAMPLES] = {}; + float t_sample_us = 10000.0f; + FifoSampleClock sample_clock; static void sensor_timer_handler(struct k_timer *dummy); @@ -27,4 +30,4 @@ class IMU : public EdgeMlSensor { bool _active = false; }; -#endif \ No newline at end of file +#endif diff --git a/src/SensorManager/Kconfig b/src/SensorManager/Kconfig index 191b98a3..c7476281 100644 --- a/src/SensorManager/Kconfig +++ b/src/SensorManager/Kconfig @@ -41,6 +41,12 @@ config SENSOR_WORK_QUEUE_STACK_SIZE int "Stack size for power measurement thread" default 1024 +config SENSOR_CONFIG_STACK_SIZE + int "Stack size for sensor configuration worker" + default 4096 + help + Separate preemptible worker for sensor initialization and storage setup. + config SENSOR_GATT_NOTIFY_STACK_SIZE int "Stack size" default 2048 @@ -60,7 +66,7 @@ config SENSOR_SD_SUB_QUEUE_SIZE config SENSOR_GATT_SUB_QUEUE_SIZE int "Queue size for content control subscriber" - default 256 + default 32 endmenu # Zbus diff --git a/src/SensorManager/MAXM86161/MAXM86161.cpp b/src/SensorManager/MAXM86161/MAXM86161.cpp index 8774d01f..dc9387cf 100644 --- a/src/SensorManager/MAXM86161/MAXM86161.cpp +++ b/src/SensorManager/MAXM86161/MAXM86161.cpp @@ -1,10 +1,12 @@ #include "MAXM86161.h" +#include +#include #include #include LOG_MODULE_REGISTER(MAXM86161, 3); -char databuffer[32*BYTES_PER_CH*LED_NUM]; +uint8_t databuffer[32*BYTES_PER_CH*LED_NUM]; /*****************************************************************************/ // Constructor @@ -136,19 +138,32 @@ int MAXM86161::stop(void) return status; } -int MAXM86161::read(ppg_sample * buffer) { +int MAXM86161::read(ppg_sample *buffer, size_t buffer_capacity) { + if (buffer == nullptr || buffer_capacity == 0) { + return 0; + } + int status; int number_of_bytes; - int num_samples = 0; - int output_idx = -1; + int fifo_items = 0; + size_t output_count = 0; - status = _read_from_reg(REG_FIFO_DATA_COUNTER, num_samples); + status = _read_from_reg(REG_FIFO_DATA_COUNTER, fifo_items); if (status == 0){ - number_of_bytes = num_samples / LED_NUM * LED_NUM * BYTES_PER_CH; + int item_capacity = (int)(buffer_capacity * _exposure_count); + if (_pending_exposure_mask != 0) { + const int pending_items = __builtin_popcount((unsigned int)_pending_exposure_mask); + item_capacity -= pending_items; + } + int items_to_read = MIN(fifo_items, item_capacity); + number_of_bytes = items_to_read * BYTES_PER_CH; - (void)_read_block(REG_FIFO_DATA, number_of_bytes, (uint8_t *) databuffer); + status = _read_block(REG_FIFO_DATA, number_of_bytes, (uint8_t *) databuffer); + if (status != 0) { + return 0; + } - for (int i=0; i < num_samples / LED_NUM * LED_NUM; i++) { + for (int i = 0; i < items_to_read; i++) { int idx = BYTES_PER_CH * i; uint32_t val = databuffer[idx] << 16 | databuffer[idx + 1] << 8 | databuffer[idx+2]; @@ -156,16 +171,36 @@ int MAXM86161::read(ppg_sample * buffer) { uint8_t tag = val >> 19; val = val & ((1 << 19) - 1); - //LOG_INF("tag: %i, val: %i", tag, val); - - if (tag == 1) output_idx++; - if (tag > 6 || output_idx < 0) continue; - - buffer[output_idx][tag-1] = val; + /* Picket-fence replacement tags 13-15 correspond to exposures 1-3. */ + if (tag >= 13 && tag <= 15) { + tag -= 12; + } + + if (tag == 1) { + memset(_pending_sample, 0, sizeof(_pending_sample)); + _pending_exposure_mask = 0; + } + if (tag == 0 || tag > _exposure_count || + (_pending_exposure_mask == 0 && tag != 1)) { + continue; + } + + _pending_sample[_exposure_output_indices[tag - 1]] = val; + _pending_exposure_mask |= (uint8_t)(1U << (tag - 1)); + + const uint8_t complete_mask = (uint8_t)((1U << _exposure_count) - 1U); + if (_pending_exposure_mask == complete_mask) { + if (output_count >= buffer_capacity) { + break; + } + memcpy(buffer[output_count], _pending_sample, sizeof(ppg_sample)); + output_count++; + _pending_exposure_mask = 0; + } } } - return output_idx+1; + return (int)output_count; } @@ -176,7 +211,10 @@ int MAXM86161::set_interrogation_rate(int rate) int status; // Get value of register to avoid overwriting sample average value - _read_from_reg(REG_PPG_CONFIG2, existing_reg_values); + status = _read_from_reg(REG_PPG_CONFIG2, existing_reg_values); + if (status != 0) { + return status; + } // Set the appropriate bits, while leaving the others. existing_reg_values = _set_multiple_bits(existing_reg_values, MASK_SMP_AVE, rate, POS_PPG_SR); @@ -185,6 +223,18 @@ int MAXM86161::set_interrogation_rate(int rate) return status; } +int MAXM86161::get_interrogation_rate(int &rate) +{ + int register_value; + int status = _read_from_reg(REG_PPG_CONFIG2, register_value); + + if (status == 0) { + rate = (register_value & MASK_PPG_SR) >> POS_PPG_SR; + } + + return status; +} + int MAXM86161::set_sample_averaging(int average) { int existing_reg_values; @@ -245,7 +295,10 @@ int MAXM86161::set_ppg_tint(int time) int status; // Get value of register to avoid overwriting sample average value - _read_from_reg(REG_PPG_CONFIG1, existing_reg_values); + status = _read_from_reg(REG_PPG_CONFIG1, existing_reg_values); + if (status != 0) { + return status; + } // Set the appropriate bits, while leaving the others. existing_reg_values = _set_multiple_bits(existing_reg_values, MASK_PPG_TINT_WRITE, time, POS_PPG_TINT); @@ -255,6 +308,73 @@ int MAXM86161::set_ppg_tint(int time) } +int MAXM86161::get_ppg_tint(int &time) +{ + int register_value; + int status = _read_from_reg(REG_PPG_CONFIG1, register_value); + + if (status == 0) { + time = register_value & MASK_PPG_TINT; + } + + return status; +} + +int MAXM86161::set_exposure_count(uint8_t count) +{ + /* Keep IR as the highest-rate channel; preserve the red/IR pair at 2 kHz. */ + static const uint8_t sequence_registers[LED_NUM][3] = { + { 0x02, 0x00, 0x00 }, /* IR */ + { 0x32, 0x00, 0x00 }, /* IR, red */ + { 0x12, 0x03, 0x00 }, /* IR, green, red */ + { 0x12, 0x93, 0x00 }, /* IR, green, red, ambient */ + }; + static const uint8_t output_indices[LED_NUM][LED_NUM] = { + { 1, 0, 0, 0 }, + { 1, 0, 0, 0 }, + { 1, 2, 0, 0 }, + { 1, 2, 0, 3 }, + }; + + if (count < 1 || count > LED_NUM) { + return -EINVAL; + } + + const int registers[] = { REG_LED_SEQ1, REG_LED_SEQ2, REG_LED_SEQ3 }; + for (size_t i = 0; i < 3; ++i) { + int status = _write_to_reg(registers[i], sequence_registers[count - 1][i]); + if (status != 0) { + return status; + } + + int effective_value; + status = _read_from_reg(registers[i], effective_value); + if (status != 0) { + return status; + } + if (effective_value != sequence_registers[count - 1][i]) { + return -EIO; + } + } + + int fifo_config; + int status = _read_from_reg(REG_FIFO_CONFIG2, fifo_config); + if (status != 0) { + return status; + } + status = _write_to_reg(REG_FIFO_CONFIG2, fifo_config | 0x10); + if (status != 0) { + return status; + } + + _exposure_count = count; + memcpy(_exposure_output_indices, output_indices[count - 1], + sizeof(_exposure_output_indices)); + memset(_pending_sample, 0, sizeof(_pending_sample)); + _pending_exposure_mask = 0; + return 0; +} + /*******************************************************************************/ int MAXM86161::alc_on(void) { @@ -389,7 +509,7 @@ int MAXM86161::_write_to_reg(int address, int value) { _i2c->release(); - return 0; + return ret; } diff --git a/src/SensorManager/MAXM86161/MAXM86161.h b/src/SensorManager/MAXM86161/MAXM86161.h index 3037dd78..560c1406 100644 --- a/src/SensorManager/MAXM86161/MAXM86161.h +++ b/src/SensorManager/MAXM86161/MAXM86161.h @@ -6,6 +6,7 @@ //#include #include +#include #include @@ -34,6 +35,11 @@ typedef uint32_t ppg_sample[6]; +// Constants for reading data +#define BYTES_PER_CH 3 +#define LED_NUM 4 // 3 LEDs plus the ambient sample +#define FIFO_SIZE 128 + class MAXM86161 { public: /** @brief Constructor @@ -50,11 +56,13 @@ class MAXM86161 { /** @brief Stop collecting data samples */ int stop(void); /** @brief Read data from the sensor */ - int read(ppg_sample * buffer); + int read(ppg_sample *buffer, size_t buffer_capacity); // Configuration adjustments /** @brief Set the rate of the PPG sensor */ int set_interrogation_rate(int rate); + /** @brief Read back the effective rate selected by the sensor. */ + int get_interrogation_rate(int &rate); /** @brief Set the number of samples to average */ int set_sample_averaging(int average); /** @brief Set the LED current for all LEDs */ @@ -67,6 +75,10 @@ class MAXM86161 { int set_led3_current(int current); /** @brief Set the integration time for the photodiode */ int set_ppg_tint(int time); + /** @brief Read back the effective photodiode integration time. */ + int get_ppg_tint(int &time); + /** @brief Configure the number of optical exposures in each sample. */ + int set_exposure_count(uint8_t count); // Setting adjustments /** @brief Set the ALC on */ @@ -97,6 +109,11 @@ class MAXM86161 { uint8_t _addr = DT_REG_ADDR(DT_NODELABEL(maxm86161)); + uint8_t _exposure_count = LED_NUM; + uint8_t _exposure_output_indices[LED_NUM] = { 1, 2, 0, 3 }; + ppg_sample _pending_sample = {}; + uint8_t _pending_exposure_mask = 0; + // void _set_led_sequence(char sequence); int _read_from_reg(int address, int &data); @@ -118,12 +135,6 @@ class MAXM86161 { // Part ID of the MAXM86161 #define PPG_PART_ID 0x36 -// Constants for reading data -#define BYTES_PER_CH 3 -#define LED_NUM 4 // 3 LEDs plus the ambient sample -#define FIFO_SIZE 128 - - // Bit positions. #define POS_DATA_RDY_EN 6 @@ -145,6 +156,7 @@ class MAXM86161 { #define MASK_SMP_AVE 0b00000111 //Register 0x12 #define MASK_PPG_SR 0b11111000 // Register 0x12 #define MASK_PPG_TINT_WRITE 0b11111100 // Register 0x11 +#define MASK_PPG_TINT 0b00000011 // Register 0x11 #define MASK_PPG_LABEL 0x7FFFF; #define MASK_PPG_ID 0xF8; @@ -228,4 +240,4 @@ class MAXM86161 { #define REG_FIFO_TAG_MASK 0x1F -#endif /* __MAXM86161_H_*/ \ No newline at end of file +#endif /* __MAXM86161_H_*/ diff --git a/src/SensorManager/MLX90632/MLX90632.cpp b/src/SensorManager/MLX90632/MLX90632.cpp index b794e952..2de19b05 100644 --- a/src/SensorManager/MLX90632/MLX90632.cpp +++ b/src/SensorManager/MLX90632/MLX90632.cpp @@ -196,14 +196,6 @@ float MLX90632::getObjectTemp(status& returnError) //Write new_data = 0 clearNewData(); - gatherSensorTemp(returnError); - if (returnError != SENSOR_SUCCESS) - { - LOG_WRN("Sensor temperature not found"); - if(returnError == SENSOR_TIMEOUT_ERROR) LOG_WRN("Timeout"); - return (0.0); //Error - } - int16_t lowerRAM = 0; int16_t upperRAM = 0; @@ -237,28 +229,22 @@ float MLX90632::getObjectTemp(status& returnError) readRegister16(RAM_5, (uint16_t&)upperRAM); } - //Object temp requires 3 iterations - for (uint8_t i = 0 ; i < 3 ; i++) - { - double VRta = nineRAM + Gb * (sixRAM / 12.0); - - double AMB = (sixRAM / 12.0) / VRta * pow(2, 19); - - float S = (float)(lowerRAM + upperRAM) / 2.0f; - double VRto = nineRAM + Ka * (sixRAM / 12.0); - double Sto = ((double)S / 12.0) / VRto * pow(2, 19); - - double TAdut = (AMB - Eb) / Ea + 25.0; - - double ambientTempK = TAdut + 273.15; - - double bigFraction = Sto / (1 * Fa * Ha * (1 + Ga * (TOdut - TO0) + Fb * (TAdut - TA0))); - - double objectTemp = bigFraction + pow(ambientTempK, 4); - objectTemp = pow(objectTemp, 0.25); //Take 4th root - objectTemp = objectTemp - 273.15 - Hb; - - TO0 = objectTemp; + const double VRta = nineRAM + Gb * (sixRAM / 12.0); + const double AMB = (sixRAM / 12.0) / VRta * 524288.0; + const float S = (float)(lowerRAM + upperRAM) / 2.0f; + const double VRto = nineRAM + Ka * (sixRAM / 12.0); + const double Sto = ((double)S / 12.0) / VRto * 524288.0; + const double TAdut = (AMB - Eb) / Ea + 25.0; + const double ambientTempK = TAdut + 273.15; + const double ambientSquared = ambientTempK * ambientTempK; + const double ambientFourth = ambientSquared * ambientSquared; + + // Only the object-temperature estimate changes between iterations. A fourth + // root of this non-negative physical quantity is two square roots. + for (uint8_t i = 0; i < 3; ++i) { + const double bigFraction = Sto / (Fa * Ha * + (1 + Ga * (TOdut - TO0) + Fb * (TAdut - TA0))); + TO0 = sqrt(sqrt(bigFraction + ambientFourth)) - 273.15 - Hb; } return (TO0); @@ -586,7 +572,7 @@ void MLX90632::reset() { void MLX90632::setSampleRateRegVal(uint8_t val) { uint8_t originalMode = getMode(); - setMode(MODE_SLEEP); + setMode(MODE_HALT); // Keep the device awake while changing EEPROM. writeEEPROM(EE_MEAS_1, 0x800D | (val << 8)); writeEEPROM(EE_MEAS_2, 0x801D | (val << 8)); @@ -600,7 +586,7 @@ void MLX90632::setSampleRate(float sample_rate) { val = round(log2f(CLAMP(2 * sample_rate,1,128))); uint8_t originalMode = getMode(); - setMode(MODE_SLEEP); + setMode(MODE_HALT); // Keep the device awake while changing EEPROM. writeEEPROM(EE_MEAS_1, 0x800D | (val << 8)); writeEEPROM(EE_MEAS_2, 0x801D | (val << 8)); diff --git a/src/SensorManager/MLX90632/MLX90632.h b/src/SensorManager/MLX90632/MLX90632.h index 411659ea..281016d9 100644 --- a/src/SensorManager/MLX90632/MLX90632.h +++ b/src/SensorManager/MLX90632/MLX90632.h @@ -84,6 +84,7 @@ #define RAM_9 0x4008 //Three measurement modes available +#define MODE_HALT 0b00 #define MODE_SLEEP 0b01 #define MODE_STEP 0b10 #define MODE_CONTINUOUS 0b11 diff --git a/src/SensorManager/Microphone.cpp b/src/SensorManager/Microphone.cpp index ed1c4f1a..c0ec330e 100644 --- a/src/SensorManager/Microphone.cpp +++ b/src/SensorManager/Microphone.cpp @@ -11,25 +11,9 @@ #include "ADAU1860.h" -//#include - -#ifdef __cplusplus -extern "C" { -#endif - -#include -extern void init_fifo(); -extern void empty_fifo(); - -#ifdef __cplusplus -} -#endif - #include LOG_MODULE_REGISTER(microphone, CONFIG_LOG_DEFAULT_LEVEL); -extern struct data_fifo fifo_rx; - Microphone Microphone::sensor; const SampleRateSetting<9> Microphone::sample_rates = { @@ -48,8 +32,6 @@ bool Microphone::init(struct k_msgq * queue) { set_sensor_queue(queue); - init_fifo(); - return true; } @@ -59,13 +41,14 @@ void Microphone::start(int sample_rate_idx) { if (!_active) return; LOG_INF("Starting Microphone at %f Hz", (double)sample_rates.sample_rates[sample_rate_idx]); - record_to_sd(_sd_logging); + // Do not put samples from the previous rate into the newly opened file. + record_to_sd(false); - audio_datapath_aquire(&fifo_rx); + audio_datapath_aquire(NULL); audio_datapath_decimator_init(sample_rates.reg_vals[sample_rate_idx]); - record_to_sd(true); + record_to_sd(_sd_logging); _running = true; } diff --git a/src/SensorManager/PPG.cpp b/src/SensorManager/PPG.cpp index 7c1413f6..d9578e61 100644 --- a/src/SensorManager/PPG.cpp +++ b/src/SensorManager/PPG.cpp @@ -4,12 +4,11 @@ #include "math.h" #include "stdlib.h" +#include #include LOG_MODULE_DECLARE(MAXM86161); -#define LATENCY_MS 40 - PPG PPG::sensor; MAXM86161 PPG::ppg(&I2C2); @@ -27,6 +26,41 @@ const SampleRateSetting<16> PPG::sample_rates = { 32.000, 64.000, 128.000, 256.000, 512.000, 1024.000, 2048.000, 4096.000}, }; +namespace { + +struct ppg_timing { + int integration_time; + uint8_t exposure_count; +}; + +/* MAXM86161 data-sheet limits for four exposures per sample. */ +constexpr float max_rate_for_four_exposures[] = { + 1024.0f, /* 14.8 us */ + 512.0f, /* 29.4 us */ + 512.0f, /* 58.7 us */ + 400.0f, /* 117.3 us */ +}; + +ppg_timing select_timing(float requested_rate) +{ + for (int tint = 3; tint >= 0; --tint) { + if (requested_rate <= max_rate_for_four_exposures[tint]) { + return { tint, 4 }; + } + } + + if (requested_rate <= 2048.0f) { + return { 0, 2 }; + } + if (requested_rate <= 4096.0f) { + return { 0, 1 }; + } + + return { -ENOTSUP, 0 }; +} + +} // namespace + bool PPG::init(struct k_msgq * queue) { if (!_active) { pm_device_runtime_get(ls_1_8); @@ -57,64 +91,48 @@ bool PPG::init(struct k_msgq * queue) { void PPG::update_sensor(struct k_work *work) { ARG_UNUSED(work); - int int_status; - int status; - - uint64_t _time_stamp = micros(); - - PPG::sensor._sample_count += (_time_stamp - PPG::sensor._last_time_stamp) / PPG::sensor.t_sample_us; - PPG::sensor._last_time_stamp = _time_stamp; - - if (PPG::sensor._sample_count < PPG::sensor._num_samples_buffered * (1.f - CONFIG_SENSOR_CLOCK_ACCURACY / 100.f)) { - return; - } - - status = ppg.read_interrupt_state(int_status); - - if (status != 0) { - LOG_ERR("PPG read interrupt state failed: %d", status); - return; - } - - if(int_status & MAXM86161_INT_FULL) { // MAXM86161_INT_DATA_RDY - int num_samples = ppg.read(sensor.data_buffer); - - PPG::sensor._sample_count = MAX(0, PPG::sensor._num_samples_buffered - num_samples); - - int written = 0; - const int _size = 4 * sizeof(uint32_t); // red, ir, green, ambient - - while (written < num_samples) { - int to_write = MIN((SENSOR_DATA_FIXED_LENGTH - sizeof(uint16_t)) / _size, num_samples - written); - if (to_write <= 0) break; - - msg_ppg.sd = sensor._sd_logging; - msg_ppg.stream = sensor._ble_stream; - - msg_ppg.data.id = ID_PPG; - msg_ppg.data.size = to_write * _size + sizeof(uint16_t); - - const uint64_t dt_us = (uint64_t)((double)(num_samples - written) * (double)PPG::sensor.t_sample_us); - msg_ppg.data.time = _time_stamp - dt_us; - - if (to_write > 1) { - uint16_t t_diff = PPG::sensor.t_sample_us; - for (int i = 0; i < to_write; i++) { - memcpy(&msg_ppg.data.data[i * _size], &sensor.data_buffer[written + i], _size); - } - memcpy(&msg_ppg.data.data[msg_ppg.data.size - sizeof(uint16_t)], &t_diff, sizeof(uint16_t)); - } else { - memcpy(&msg_ppg.data.data, &sensor.data_buffer[written], _size); - } - - int ret = k_msgq_put(sensor_queue, &msg_ppg, K_NO_WAIT); - if (ret) { - LOG_WRN("sensor msg queue full"); + if (!sensor._running) return; + int num_samples = ppg.read(sensor.data_buffer, + sizeof(sensor.data_buffer) / sizeof(sensor.data_buffer[0])); + if (num_samples <= 0) return; + sensor.sample_clock.begin(micros(), num_samples, sensor.t_sample_us, + sensor.fifo_sample_capacity, + CONFIG_SENSOR_CLOCK_ACCURACY); + + int written = 0; + const int _size = 4 * sizeof(uint32_t); // red, ir, green, ambient + + while (written < num_samples) { + int to_write = MIN((SENSOR_DATA_FIXED_LENGTH - sizeof(uint16_t)) / _size, num_samples - written); + if (to_write <= 0) break; + if (sensor.sample_clock.period() > UINT16_MAX) to_write = 1; + + msg_ppg.sd = sensor._sd_logging; + msg_ppg.stream = sensor._ble_stream; + + msg_ppg.data.id = ID_PPG; + msg_ppg.data.size = to_write * _size + sizeof(uint16_t); + + msg_ppg.data.time = sensor.sample_clock.timestamp(written); + + if (to_write > 1) { + uint16_t t_diff = sensor.sample_clock.period(); + for (int i = 0; i < to_write; i++) { + memcpy(&msg_ppg.data.data[i * _size], &sensor.data_buffer[written + i], _size); } + memcpy(&msg_ppg.data.data[msg_ppg.data.size - sizeof(uint16_t)], &t_diff, sizeof(uint16_t)); + } else { + memcpy(&msg_ppg.data.data, &sensor.data_buffer[written], _size); + } - written += to_write; + int ret = k_msgq_put(sensor_queue, &msg_ppg, K_NO_WAIT); + if (ret) { + LOG_WRN("sensor msg queue full"); } + + written += to_write; } + } /** @@ -122,27 +140,79 @@ void PPG::update_sensor(struct k_work *work) { */ void PPG::sensor_timer_handler(struct k_timer *dummy) { ARG_UNUSED(dummy); - k_work_submit_to_queue(&sensor_work_q, &sensor.sensor_work); + k_work_submit_to_queue(&sensor_ppg_work_q, &sensor.sensor_work); } void PPG::start(int sample_rate_idx) { if (!_active) return; - t_sample_us = 1000000.0f / sample_rates.true_sample_rates[sample_rate_idx]; + const float requested_rate = sample_rates.true_sample_rates[sample_rate_idx]; + const int requested_rate_register = sample_rates.reg_vals[sample_rate_idx]; + const ppg_timing timing = select_timing(requested_rate); - k_timeout_t t = K_USEC(t_sample_us); + if (timing.integration_time < 0) { + LOG_ERR("PPG rate %.3f Hz is not supported", (double)requested_rate); + return; + } + + int ret = ppg.set_exposure_count(timing.exposure_count); + if (ret != 0) { + LOG_ERR("Failed to set PPG exposure count to %u: %d", timing.exposure_count, ret); + return; + } + + ret = ppg.set_ppg_tint(timing.integration_time); + if (ret != 0) { + LOG_ERR("Failed to set PPG integration time: %d", ret); + return; + } - _num_samples_buffered = MIN(MAX(1, (int) (CONFIG_SENSOR_LATENCY_MS * 1000.0f / t_sample_us)), FIFO_SIZE / LED_NUM - 2); + int effective_integration_time = -1; + ret = ppg.get_ppg_tint(effective_integration_time); + if (ret != 0 || effective_integration_time != timing.integration_time) { + LOG_ERR("PPG rejected integration time %d (effective %d, ret %d)", + timing.integration_time, effective_integration_time, ret); + return; + } + + ret = ppg.set_interrogation_rate(requested_rate_register); + if (ret != 0) { + LOG_ERR("Failed to set PPG sample rate: %d", ret); + return; + } + + int effective_rate_register = -1; + ret = ppg.get_interrogation_rate(effective_rate_register); + if (ret != 0 || effective_rate_register != requested_rate_register) { + LOG_ERR("PPG rejected rate register 0x%02x (effective 0x%02x, ret %d)", + requested_rate_register, effective_rate_register, ret); + return; + } + + t_sample_us = 1000000.0f / requested_rate; + + fifo_sample_capacity = FIFO_SIZE / timing.exposure_count; + const int work_buffer_capacity = + sizeof(sensor.data_buffer) / sizeof(sensor.data_buffer[0]) - 2; + _num_samples_buffered = MIN(MAX(1, (int)(CONFIG_SENSOR_LATENCY_MS * 1000.0f / t_sample_us)), + MIN(fifo_sample_capacity / 2, work_buffer_capacity)); - ppg.set_interrogation_rate(sample_rates.reg_vals[sample_rate_idx]); - ppg.set_watermark(FIFO_SIZE - _num_samples_buffered * LED_NUM); - ppg.start(); + ret = ppg.set_watermark(FIFO_SIZE - _num_samples_buffered * timing.exposure_count); + if (ret != 0) { + LOG_ERR("Failed to set PPG FIFO watermark: %d", ret); + return; + } - k_timer_start(&sensor.sensor_timer, K_NO_WAIT, t); + ret = ppg.start(); + if (ret != 0) { + LOG_ERR("Failed to start PPG: %d", ret); + return; + } _running = true; - _sample_count = 0; - _last_time_stamp = micros(); + sample_clock.reset(); + const k_timeout_t t = K_USEC(t_sample_us * _num_samples_buffered); + k_timer_start(&sensor.sensor_timer, t, t); } void PPG::stop() { @@ -152,6 +222,8 @@ void PPG::stop() { _running = false; k_timer_stop(&sensor.sensor_timer); + struct k_work_sync sync; + k_work_cancel_sync(&sensor.sensor_work, &sync); ppg.stop(); diff --git a/src/SensorManager/PPG.h b/src/SensorManager/PPG.h index 2fe4264f..2dc0f923 100644 --- a/src/SensorManager/PPG.h +++ b/src/SensorManager/PPG.h @@ -9,6 +9,7 @@ //#include "MAX30102/MAX30102.h" #include "MAXM86161/MAXM86161.h" #include "EdgeMLSensor.h" +#include "FifoSampleClock.h" #include "openearable_common.h" #include "zbus_common.h" @@ -39,12 +40,12 @@ class PPG : public EdgeMlSensor { ppg_sample data_buffer[64]; float t_sample_us; + FifoSampleClock sample_clock; + uint16_t fifo_sample_capacity; bool _active = false; int _num_samples_buffered; - float _sample_count = 0; - uint64_t _last_time_stamp = 0; }; #endif diff --git a/src/SensorManager/SensorManager.cpp b/src/SensorManager/SensorManager.cpp index d5d8364c..5838d51c 100644 --- a/src/SensorManager/SensorManager.cpp +++ b/src/SensorManager/SensorManager.cpp @@ -1,6 +1,7 @@ #include "SensorManager.h" #include +#include #include #include "macros_common.h" @@ -41,7 +42,9 @@ EdgeMlSensor * get_sensor(enum sensor_id id); static sensor_manager_state _state; -K_MSGQ_DEFINE(sensor_queue, sizeof(struct sensor_msg), 256, 4); +// Larger records batch more FIFO samples. Keep the queue's RAM budget bounded +// so the audio runtime heap retains its required headroom. +K_MSGQ_DEFINE(sensor_queue, sizeof(struct sensor_msg), 128, 4); K_MSGQ_DEFINE(config_queue, sizeof(struct sensor_config), 16, 4); K_THREAD_STACK_DEFINE(sensor_work_q_stack, CONFIG_SENSOR_WORK_QUEUE_STACK_SIZE); @@ -49,9 +52,8 @@ K_THREAD_STACK_DEFINE(sensor_work_q_stack, CONFIG_SENSOR_WORK_QUEUE_STACK_SIZE); ZBUS_CHAN_DEFINE(sensor_chan, struct sensor_msg, NULL, NULL, ZBUS_OBSERVERS_EMPTY, ZBUS_MSG_INIT(0)); -static struct k_poll_signal sensor_manager_sig; -static struct k_poll_event sensor_manager_evt = - K_POLL_EVENT_INITIALIZER(K_POLL_TYPE_SIGNAL, K_POLL_MODE_NOTIFY_ONLY, &sensor_manager_sig); +// Internal queue marker; consumed before publication, never written to BLE/SD. +static constexpr uint8_t SENSOR_QUEUE_BARRIER = UINT8_MAX; struct sensor_msg msg; @@ -60,8 +62,22 @@ struct k_thread sensor_publish; static k_tid_t sensor_pub_id; static struct k_work config_work; +static struct k_work_delayable sd_config_work; +// Profile writes arrived at most 92 ms apart in paired phone tests with music. +// Wait for a quiet window, leaving the old recording untouched in the meantime. +static constexpr int SD_CONFIG_QUIET_MS = 300; +static constexpr size_t SENSOR_CONFIG_SLOTS = 8; +static sensor_config pending_sd_configs[SENSOR_CONFIG_SLOTS]; +static bool pending_sd_valid[SENSOR_CONFIG_SLOTS]; +static atomic_t config_stopping; +static bool config_ready; +static struct k_work_q config_work_q; +K_THREAD_STACK_DEFINE(config_work_q_stack, CONFIG_SENSOR_CONFIG_STACK_SIZE); struct k_work_q sensor_work_q; +// PPG uses I2C2; the other polled sensors share I2C3 and one worker. +struct k_work_q sensor_ppg_work_q; +K_THREAD_STACK_DEFINE(sensor_ppg_work_q_stack, CONFIG_SENSOR_WORK_QUEUE_STACK_SIZE); K_THREAD_STACK_DEFINE(sensor_publish_thread_stack, CONFIG_SENSOR_PUB_STACK_SIZE); @@ -69,6 +85,21 @@ int active_sensors = 0; static const char sensor_manager_auto_off_token[] = "SensorManager"; static void config_work_handler(struct k_work *work); +static void sd_config_work_handler(struct k_work *work); + +static void drain_sensor_queue() { + if (_state == INIT) return; + + struct k_sem drained; + k_sem_init(&drained, 0, 1); + struct sensor_msg barrier = {}; + barrier.data.id = SENSOR_QUEUE_BARRIER; + struct k_sem *completion = &drained; + memcpy(barrier.data.data, &completion, sizeof(completion)); + // Each caller waits for its own marker, including an overlapping shutdown. + k_msgq_put(&sensor_queue, &barrier, K_FOREVER); + k_sem_take(&drained, K_FOREVER); +} void sensor_chan_update(void *p1, void *p2, void *p3) { ARG_UNUSED(p1); @@ -77,9 +108,16 @@ void sensor_chan_update(void *p1, void *p2, void *p3) { int ret; while (1) { - (void)k_poll(&sensor_manager_evt, 1, K_FOREVER); - - k_msgq_get(&sensor_queue, &msg, K_FOREVER); + ret = k_msgq_get(&sensor_queue, &msg, K_FOREVER); + if (ret) { + continue; + } + if (msg.data.id == SENSOR_QUEUE_BARRIER) { + struct k_sem *completion; + memcpy(&completion, msg.data.data, sizeof(completion)); + k_sem_give(completion); + continue; + } ret = zbus_chan_pub(&sensor_chan, &msg, K_FOREVER); //K_NO_WAIT if (ret) { @@ -94,6 +132,11 @@ void init_sensor_manager() { active_sensors = 0; k_work_queue_init(&sensor_work_q); + k_work_queue_init(&sensor_ppg_work_q); + k_work_queue_start(&sensor_ppg_work_q, sensor_ppg_work_q_stack, + K_THREAD_STACK_SIZEOF(sensor_ppg_work_q_stack), + K_PRIO_PREEMPT(CONFIG_SENSOR_WORK_QUEUE_PRIO), NULL); + k_thread_name_set(&sensor_ppg_work_q.thread, "sensor_ppg"); k_work_queue_start(&sensor_work_q, sensor_work_q_stack, K_THREAD_STACK_SIZEOF(sensor_work_q_stack), K_PRIO_PREEMPT(CONFIG_SENSOR_WORK_QUEUE_PRIO), @@ -104,8 +147,18 @@ void init_sensor_manager() { K_PRIO_PREEMPT(CONFIG_SENSOR_PUB_THREAD_PRIO), 0, K_FOREVER); // Thread ist initial suspendiert k_work_init(&config_work, config_work_handler); - - k_poll_signal_init(&sensor_manager_sig); + k_work_init_delayable(&sd_config_work, sd_config_work_handler); + /* Driver initialization can take hundreds of milliseconds. Keep it + * preemptible by audio decoding instead of using the cooperative system + * queue. It must also be separate from the polling queue, which sensor + * shutdown drains synchronously. + */ + k_work_queue_init(&config_work_q); + k_work_queue_start(&config_work_q, config_work_q_stack, + K_THREAD_STACK_SIZEOF(config_work_q_stack), + K_PRIO_PREEMPT(CONFIG_SENSOR_WORK_QUEUE_PRIO), NULL); + k_thread_name_set(&config_work_q.thread, "sensor_config"); + config_ready = true; sdlogger.init(); @@ -124,9 +177,8 @@ void start_sensor_manager() { LOG_DBG("Starting sensor manager"); - //empty message queue - k_msgq_purge(&sensor_queue); k_work_queue_unplug(&sensor_work_q); + k_work_queue_unplug(&sensor_ppg_work_q); ble_sensors.clear(); sd_sensors.clear(); @@ -135,12 +187,19 @@ void start_sensor_manager() { k_thread_start(sensor_pub_id); } - k_poll_signal_raise(&sensor_manager_sig, 0); - _state = RUNNING; } void stop_sensor_manager() { + if (config_ready && k_current_get() != &config_work_q.thread) { + // Power-down must not leave a delayed profile that restarts acquisition. + atomic_set(&config_stopping, 1); + struct k_work_sync sync; + k_work_cancel_delayable_sync(&sd_config_work, &sync); + k_work_flush(&config_work, &sync); + k_work_cancel_delayable_sync(&sd_config_work, &sync); + memset(pending_sd_valid, 0, sizeof(pending_sd_valid)); + } if (_state != RUNNING) return; LOG_DBG("Stopping sensor manager"); @@ -160,9 +219,10 @@ void stop_sensor_manager() { auto_off_manager.allow(sensor_manager_auto_off_token); k_work_queue_drain(&sensor_work_q, true); + k_work_queue_drain(&sensor_ppg_work_q, true); - //k_thread_suspend(sensor_pub_id); - k_poll_signal_reset(&sensor_manager_sig); + // Producers are stopped; let every accepted message reach the SD listener. + drain_sensor_queue(); _state = SUSPENDED; @@ -191,17 +251,9 @@ EdgeMlSensor * get_sensor(enum sensor_id id) { } } -// Worker-Funktion für die Sensor-Konfiguration -static void config_work_handler(struct k_work *work) { - ARG_UNUSED(work); - int ret; - struct sensor_config config; - - ret = k_msgq_get(&config_queue, &config, K_NO_WAIT); - if (ret != 0) { - LOG_INF("No config available"); - } - +// Apply one request; the worker below drains all requests, since k_work +// submissions coalesce while an earlier sensor reconfiguration is running. +static void apply_sensor_config(const struct sensor_config &config, bool stop_idle = true) { float sampleRate = getSampleRateForSensorId(config.sensorId, config.sampleRateIndex); if (sampleRate <= 0) { LOG_ERR("Invalid sample rate %f for sensor %i", (double)sampleRate, config.sensorId); @@ -215,6 +267,22 @@ static void config_work_handler(struct k_work *work) { return; } + struct sensor_config previous; + if (sensor->is_running() && + get_sensor_config_status(config.sensorId, &previous) == 0 && + previous.sampleRateIndex == config.sampleRateIndex && + (previous.storageOptions & DATA_STORAGE) == (config.storageOptions & DATA_STORAGE) && + (config.storageOptions & (DATA_STORAGE | DATA_STREAMING))) { + // Preserve the FIFO, sample clock and in-flight SD data for BLE-only + // routing changes or repeated configurations. SD changes still drain + // the stopped producer through the normal path below. + sensor->ble_stream(config.storageOptions & DATA_STREAMING); + if (config.storageOptions & DATA_STREAMING) ble_sensors.insert(config.sensorId); + else ble_sensors.erase(config.sensorId); + set_sensor_config_status(config); + return; + } + if (sensor->is_running()) { sensor->stop(); active_sensors--; @@ -241,23 +309,8 @@ static void config_work_handler(struct k_work *work) { if (config.storageOptions & DATA_STORAGE) { sd_sensors.insert(config.sensorId); - - if (!sdlogger.is_active()) { - const char *recording_name_prefix = get_sensor_recording_name(); - LOG_INF("Starting SDLogger with recording name prefix: %s", recording_name_prefix); - // Start SDLogger with timestamp-based filename - std::string filename = recording_name_prefix + std::to_string(micros()); - int logger_ret = sdlogger.begin(filename); - if (logger_ret == 0) state_indicator.set_sd_state(SD_RECORDING); - else LOG_ERR("Failed to start SDLogger, ret: %d", logger_ret); - } } else if (sd_sensors.find(config.sensorId) != sd_sensors.end()) { sd_sensors.erase(config.sensorId); - - if (sd_sensors.empty()) { - sdlogger.end(); - state_indicator.set_sd_state(SD_IDLE); - } } if (config.storageOptions & DATA_STREAMING) ble_sensors.insert(config.sensorId); @@ -269,17 +322,115 @@ static void config_work_handler(struct k_work *work) { set_sensor_config_status(config); + if (stop_idle && active_sensors == 0) stop_sensor_manager(); +} + +static bool sd_configuration_changed(const sensor_config &before, const sensor_config &after) { + return ((before.storageOptions ^ after.storageOptions) & DATA_STORAGE) || + ((after.storageOptions & DATA_STORAGE) && before.sampleRateIndex != after.sampleRateIndex); +} + +static void sd_config_work_handler(struct k_work *work) { + ARG_UNUSED(work); + if (atomic_get(&config_stopping)) return; + // A received write can still be queued behind this delayed work item. + if (k_msgq_num_used_get(&config_queue)) { + k_work_reschedule_for_queue(&config_work_q, &sd_config_work, K_MSEC(SD_CONFIG_QUIET_MS)); + return; + } + + sensor_config previous[SENSOR_CONFIG_SLOTS] = {}; + sensor_config desired[SENSOR_CONFIG_SLOTS] = {}; + bool apply[SENSOR_CONFIG_SLOTS] = {}; + const ParseInfoScheme *scheme = getParseInfoScheme(); + if (!scheme || scheme->sensorCount > SENSOR_CONFIG_SLOTS) return; + bool rotate = false; + bool record = false; + for (size_t i = 0; i < scheme->sensorCount; i++) { + const uint8_t id = scheme->sensorIds[i]; + if (id >= SENSOR_CONFIG_SLOTS || get_sensor_config_status(id, &previous[i])) return; + desired[i] = pending_sd_valid[id] ? pending_sd_configs[id] : previous[i]; + apply[i] = pending_sd_valid[id]; + rotate |= sd_configuration_changed(previous[i], desired[i]); + record |= (desired[i].storageOptions & DATA_STORAGE) != 0; + } + memset(pending_sd_valid, 0, sizeof(pending_sd_valid)); + const bool was_recording = sdlogger.is_active(); + rotate |= record && !was_recording; + int ret = 0; + if (rotate && was_recording) { + // Stop all SD producers before crossing the file boundary. BLE-only + // producers and playback continue; accepted samples are never purged. + for (size_t i = 0; i < scheme->sensorCount; i++) { + if (!(previous[i].storageOptions & DATA_STORAGE)) continue; + EdgeMlSensor *sensor = get_sensor((sensor_id)previous[i].sensorId); + if (sensor->is_running()) { + sensor->stop(); + active_sensors--; + } + apply[i] = true; + } + drain_sensor_queue(); + ret = sdlogger.end(); + sd_sensors.clear(); + } + if (rotate && record && ret == 0) { + const std::string filename = get_sensor_recording_name() + std::to_string(micros()); + ret = sdlogger.begin(filename, desired, scheme->sensorCount); + } + if (ret != 0) { + LOG_ERR("Failed to prepare SD configuration: %d", ret); + // A rejected initial start leaves the working configuration unchanged. + // If an existing file was closed, resume its BLE streams with SD off. + if (was_recording) { + for (size_t i = 0; i < scheme->sensorCount; i++) { + if (!(previous[i].storageOptions & DATA_STORAGE)) continue; + previous[i].storageOptions &= ~DATA_STORAGE; + apply_sensor_config(previous[i], false); + } + if (active_sensors == 0) stop_sensor_manager(); + } + state_indicator.set_sd_state(SD_FAULT); + notify_sensor_config_status(); + return; + } + for (size_t i = 0; i < scheme->sensorCount; i++) { + if (apply[i]) apply_sensor_config(desired[i], false); + } + if (rotate) state_indicator.set_sd_state(record ? SD_RECORDING : SD_IDLE); if (active_sensors == 0) stop_sensor_manager(); } +static void config_work_handler(struct k_work *work) { + ARG_UNUSED(work); + struct sensor_config config; + while (k_msgq_get(&config_queue, &config, K_NO_WAIT) == 0) { + if (atomic_get(&config_stopping)) continue; + sensor_config previous; + if (config.sensorId >= SENSOR_CONFIG_SLOTS || + get_sensor_config_status(config.sensorId, &previous) || + getSampleRateForSensorId(config.sensorId, config.sampleRateIndex) <= 0) { + notify_sensor_config_status(); + continue; + } + if (pending_sd_valid[config.sensorId] || sd_configuration_changed(previous, config) || + ((config.storageOptions & DATA_STORAGE) && !sdlogger.is_active())) { + pending_sd_configs[config.sensorId] = config; + pending_sd_valid[config.sensorId] = true; + k_work_reschedule_for_queue(&config_work_q, &sd_config_work, K_MSEC(SD_CONFIG_QUIET_MS)); + } else { + apply_sensor_config(config); + } + } +} + void config_sensor(struct sensor_config * config) { + if (atomic_get(&config_stopping)) return; int ret = k_msgq_put(&config_queue, config, K_NO_WAIT); if (ret) { LOG_ERR("Failed to put config in queue, ret: %d", ret); return; } - //k_work_queue_drain(&sensor_work_q, true); - k_work_submit(&config_work); - //k_work_queue_unplug(&sensor_work_q); + k_work_submit_to_queue(&config_work_q, &config_work); } diff --git a/src/SensorManager/SensorManager.h b/src/SensorManager/SensorManager.h index c02c144b..5fe22477 100644 --- a/src/SensorManager/SensorManager.h +++ b/src/SensorManager/SensorManager.h @@ -18,6 +18,7 @@ enum sensor_manager_state { }; extern struct k_work_q sensor_work_q; +extern struct k_work_q sensor_ppg_work_q; enum sensor_manager_state get_state(); diff --git a/src/SensorManager/Temp.cpp b/src/SensorManager/Temp.cpp index 935d4812..c9be1db3 100644 --- a/src/SensorManager/Temp.cpp +++ b/src/SensorManager/Temp.cpp @@ -14,10 +14,10 @@ MLX90632 Temp::temp; static struct sensor_msg msg_temp; -const SampleRateSetting<8> Temp::sample_rates = { - { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07 }, // reg_vals - { 0.5, 1.0, 2.0, 4.0, 8.0, 16.0, 32.0, 64.0 }, // sample_rates - { 0.5, 1.0, 2.0, 4.0, 8.0, 16.0, 32.0, 64.0 } // true_sample_rates +const SampleRateSetting<7> Temp::sample_rates = { + { 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07 }, // reg_vals + { 1.0, 2.0, 4.0, 8.0, 16.0, 32.0, 64.0 }, // sample_rates + { 1.0, 2.0, 4.0, 8.0, 16.0, 32.0, 64.0 } // true_sample_rates }; bool Temp::init(struct k_msgq * queue) { @@ -50,7 +50,7 @@ bool Temp::init(struct k_msgq * queue) { void Temp::update_sensor(struct k_work *work) { ARG_UNUSED(work); - if (!temp.dataAvailable()) return; + if (!sensor._running || !temp.dataAvailable()) return; MLX90632::status returnError; float temperature = temp.getObjectTemp(returnError); @@ -86,14 +86,15 @@ void Temp::sensor_timer_handler(struct k_timer *dummy) { void Temp::start(int sample_rate_idx) { if (!_active) return; - k_timeout_t t = K_USEC(1000000.0f / sample_rates.true_sample_rates[sample_rate_idx]); + // Poll twice per conversion period so host/sensor clock phase cannot make + // every other completed measurement get overwritten before it is read. + k_timeout_t t = K_USEC(500000.0f / sample_rates.true_sample_rates[sample_rate_idx]); temp.setSampleRateRegVal(sample_rates.reg_vals[sample_rate_idx]); temp.continuousMode(); - k_timer_start(&sensor.sensor_timer, K_NO_WAIT, t); - _running = true; + k_timer_start(&sensor.sensor_timer, K_NO_WAIT, t); } void Temp::stop() { @@ -103,6 +104,8 @@ void Temp::stop() { _running = false; k_timer_stop(&sensor.sensor_timer); + struct k_work_sync sync; + k_work_cancel_sync(&sensor.sensor_work, &sync); temp.sleepMode(); diff --git a/src/SensorManager/Temp.h b/src/SensorManager/Temp.h index 5b3ce0c6..1a7f0630 100644 --- a/src/SensorManager/Temp.h +++ b/src/SensorManager/Temp.h @@ -18,7 +18,7 @@ class Temp : public EdgeMlSensor { void start(int sample_rate_idx) override; void stop() override; - const static SampleRateSetting<8> sample_rates; + const static SampleRateSetting<7> sample_rates; private: static MLX90632 temp; diff --git a/src/audio/CMakeLists.txt b/src/audio/CMakeLists.txt index d16f29d0..6b633d80 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -23,6 +23,10 @@ target_sources_ifdef(CONFIG_EQAULIZER_SOFTWARE app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/Equalizer.cpp) # Fail the link before static buffers can consume the runtime audio budget. -if(CONFIG_NEWLIB_LIBC AND NOT CONFIG_NEWLIB_LIBC_ALIGNED_HEAP_SIZE AND NOT CONFIG_MMU) - zephyr_linker_sources(SECTIONS ${CMAKE_CURRENT_SOURCE_DIR}/audio_heap.ld) +if(CONFIG_AUDIO_MIN_RUNTIME_HEAP_SIZE GREATER 0) + if(CONFIG_COMMON_LIBC_MALLOC AND CONFIG_COMMON_LIBC_MALLOC_ARENA_SIZE EQUAL -1 AND NOT CONFIG_MMU AND NOT CONFIG_USERSPACE) + zephyr_linker_sources(SECTIONS ${CMAKE_CURRENT_SOURCE_DIR}/audio_heap.ld) + else() + message(FATAL_ERROR "The audio heap guard requires the common libc remaining-SRAM arena") + endif() endif() diff --git a/src/audio/Kconfig b/src/audio/Kconfig index 41cfac5d..b9c03095 100644 --- a/src/audio/Kconfig +++ b/src/audio/Kconfig @@ -8,6 +8,13 @@ rsource "Kconfig.defaults" menu "Audio" +config AUDIO_MIN_RUNTIME_HEAP_SIZE + int "Minimum runtime heap for audio" + default 0 + help + Reserve enough remaining SRAM for the seal-check waveform, LC3 allocations, + and other runtime users of malloc. The linker rejects undersized heaps. + choice AUDIO_FRAME_DURATION prompt "Select frame duration - 7.5 ms frame duration is not tested" default AUDIO_FRAME_DURATION_10_MS @@ -32,12 +39,15 @@ config AUDIO_FRAME_DURATION_US config AUDIO_MIN_PRES_DLY_US int "The minimum presentation delay" + default 40000 if BOARD_OPENEARABLE_V2_NRF5340_CPUAPP && AUDIO_DEV = 1 default 5000 if STREAM_BIDIRECTIONAL default 3000 help The minimum allowable presentation delay in microseconds. This needs to allow time for decoding and internal routing. - For 48kHz sampling rate and 96kbps bitrate this is about 4000 us. + OpenEarable headsets reserve four 10 ms frames for controller delivery + jitter. A 5 ms deadline can empty the output FIFO when a late frame + arrives after startup synchronization, causing a jump to and from zero. config AUDIO_MAX_PRES_DLY_US int "The maximum presentation delay" @@ -62,17 +72,17 @@ choice AUDIO_SYSTEM_SAMPLE_RATE config AUDIO_SAMPLE_RATE_16000_HZ bool "16 kHz" help - Sample rate of 16kHz is currently only valid for I2S/line-in. + Sample rate of 16 kHz is currently only valid for I2S/line-in. config AUDIO_SAMPLE_RATE_24000_HZ bool "24 kHz" help - Sample rate of 24kHz is currently only valid for I2S/line-in. + Sample rate of 24 kHz is currently only valid for I2S/line-in. config AUDIO_SAMPLE_RATE_48000_HZ bool "48 kHz" help - Sample rate of 48kHz is valid for both I2S/line-in and USB. + Sample rate of 48 kHz is valid for both I2S/line-in and USB. endchoice config AUDIO_SAMPLE_RATE_HZ @@ -111,21 +121,61 @@ config AUDIO_BIT_DEPTH_OCTETS help Bit depth of one sample in storage given in octets. +config AUDIO_DECODE_CHANNELS_MAX + int "Maximum number of audio channels to decode" + default 1 + help + Maximum number of audio channels that can be decoded. + This number may have a large impact on memory and CPU usage. + +config AUDIO_ENCODE_CHANNELS_MAX + int "Maximum number of audio channels to encode" + default 2 + help + Maximum number of audio channels that can be encoded. + This number may have a large impact on memory and CPU usage. + +config AUDIO_INPUT_CHANNELS + int "Maximum number of simultaneous audio input channels" + default 2 if AUDIO_SOURCE_I2S + default 2 if AUDIO_SOURCE_USB + default 1 + range 1 2 + help + Maximum number of audio channels that can be physically input. + This is dependent on the HW itself. + +config AUDIO_OUTPUT_CHANNELS + int "Maximum number of simultaneous audio output channels" + default 2 if AUDIO_SOURCE_I2S + default 2 if AUDIO_SOURCE_USB + default 1 + range 1 2 + help + Maximum number of audio channels that can be physically output. + This is dependent on the HW itself. + Eg. for the nRF5340 Audio DK, the I2S supports 2 channels. + However, the HW Codec's (CS47L63) DAC only supports 1 channel out. + choice AUDIO_SOURCE_GATEWAY prompt "Audio source for gateway" - default AUDIO_SOURCE_I2S if WALKIE_TALKIE_DEMO + default AUDIO_SOURCE_I2S if AUDIO_DEV = 1 || WALKIE_TALKIE_DEMO default AUDIO_SOURCE_USB help Select audio source for the gateway. config AUDIO_SOURCE_USB - bool "Use USB as audio source" + bool "USB as audio source" + select USB_DEVICE_STACK_NEXT + select USBD_AUDIO2_CLASS help Set USB as audio source. Note that this forces the stream to be unidirectional because of CPU load. config AUDIO_SOURCE_I2S - bool "Use I2S as audio source" + bool "I2S as audio source" + help + Set I2S as audio source. endchoice choice AUDIO_MIC_SOURCE @@ -133,35 +183,66 @@ choice AUDIO_MIC_SOURCE prompt "Audio microphone source for bidirectional stream" default AUDIO_MIC_I2S help - Select microphone source for the bidrectional le audio. + Select the OpenEarable microphone source for bidirectional LE Audio. config AUDIO_MIC_PDM - bool "Use Mic via PDM" + bool "PDM microphone" config AUDIO_MIC_I2S - bool "Use Mic via I2S" + bool "I2S microphone" endchoice choice AUDIO_HEADSET_CHANNEL prompt "Headset audio channel assignment" default AUDIO_HEADSET_CHANNEL_RUNTIME help - Set whether audio channel assignment for the headset - should happen at runtime or compile-time. + Select whether the left/right channel is assigned at runtime or at + compile time. The OpenEarable services still expose this assignment. config AUDIO_HEADSET_CHANNEL_RUNTIME bool "Select at runtime" help - Make channel selection at runtime. Selected value is stored in persistent memory. - Left channel: Hold volume-down button on headset while resetting headset. - Right channel: Hold volume-up button on headset while resetting headset. + Store the selected left/right channel in UICR. config AUDIO_HEADSET_CHANNEL_COMPILE_TIME bool "Set at compile-time" +endchoice + +config AUDIO_HEADSET_CHANNEL + int "Audio channel used by headset" + depends on AUDIO_HEADSET_CHANNEL_COMPILE_TIME + range 0 1 + default 0 + help + Left = 0, right = 1. + +choice AUDIO_DEVICE_LOCATION_WHEN_TO_SET + prompt "Audio device location assignment" + default DEVICE_LOCATION_SET_RUNTIME + help + Set whether audio location assignment + should happen at runtime or compile-time. + +config DEVICE_LOCATION_SET_RUNTIME + bool "Select at runtime" + help + Make location selection at runtime. Selected value is stored in persistent memory. + FRONT_LEFT: Hold volume-down button while resetting headset. + FRONT_RIGHT: Hold volume-up button while resetting headset. + +config DEVICE_LOCATION_SET_COMPILE_TIME + bool "Set at compile-time" help - Set channel selection at compile-time. + Set location at compile-time. endchoice +config DEVICE_LOCATION_AT_COMPILE_TIME + int "Device location bitfield according to bt_audio_location" + depends on DEVICE_LOCATION_SET_COMPILE_TIME + default 1 + help + Sets FRONT_LEFT location as default. + config AUDIO_TEST_TONE bool "Test tone instead of doing user defined action" select TONE @@ -178,19 +259,6 @@ config AUDIO_MUTE Use button 5 to mute audio instead of doing a user defined action. -if AUDIO_HEADSET_CHANNEL_COMPILE_TIME - -config AUDIO_HEADSET_CHANNEL - int "Audio channel used by headset" - range 0 1 - default 0 - help - Audio channel compile-time selection. - Left = 0. - Right = 1. - -endif # AUDIO_HEADSET_CHANNEL_COMPILE_TIME - #----------------------------------------------------------------------------# menu "SW Codec" @@ -384,7 +452,7 @@ config BT_MGMT_MSG_SUB_STACK_SIZE config CONTENT_CONTROL_MSG_SUB_STACK_SIZE int "Stack size for content control subscriber" - default 1024 + default 1200 endmenu # Stack sizes diff --git a/src/audio/Kconfig.defaults b/src/audio/Kconfig.defaults index 809aad94..bede69f2 100644 --- a/src/audio/Kconfig.defaults +++ b/src/audio/Kconfig.defaults @@ -5,9 +5,9 @@ # # Audio sync timer -config NRFX_TIMER1 +config NRFX_TIMER default y # Audio sync timer -config NRFX_DPPI +config NRFX_GPPI default y diff --git a/src/audio/audio_datapath.c b/src/audio/audio_datapath.c index ee944999..4761a9e2 100644 --- a/src/audio/audio_datapath.c +++ b/src/audio/audio_datapath.c @@ -9,36 +9,27 @@ #include #include #include -#include -#include #include #include #include -#include #include #include #include #include "zbus_common.h" #include "macros_common.h" +#include "led_assignments.h" #include "led.h" #include "audio_i2s.h" #include "sw_codec_select.h" #include "audio_system.h" #include "streamctrl.h" #include "sd_card_playback.h" - -#include "Equalizer.h" -#include "sdlogger_wrapper.h" -#include "decimation_filter.h" -#include "../SensorManager/SensorManager.h" -#include "arm_math.h" -#include "hw_codec.h" -//#include "../drivers/ADAU1860.h" - -#include "../SensorManager/SensorManager.h" +#include "audio_clock.h" +#include "audio_startup.h" +#include "bt_mgmt_conn_interval.h" #include "openearable_common.h" -#include "SensorScheme.h" +#include "sdlogger_wrapper.h" #include LOG_MODULE_REGISTER(audio_datapath, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); @@ -51,62 +42,73 @@ LOG_MODULE_REGISTER(audio_datapath, CONFIG_AUDIO_DATAPATH_LOG_LEVEL); * - frame: encoded audio packet exchanged with connectivity */ -#define SDU_REF_DELTA_MAX_ERR_US (int)(CONFIG_AUDIO_FRAME_DURATION_US * 0.001) - -#define BLK_PERIOD_US 1000 +#define BLK_PERIOD_US 500 /* Total sample FIFO period in microseconds */ -#define FIFO_SMPL_PERIOD_US (CONFIG_AUDIO_MAX_PRES_DLY_US * 2) +#define FIFO_CHANNELS_MAX MAX(CONFIG_AUDIO_INPUT_CHANNELS, CONFIG_AUDIO_OUTPUT_CHANNELS) +#define FIFO_SMPL_PERIOD_US (CONFIG_AUDIO_MAX_PRES_DLY_US * FIFO_CHANNELS_MAX) #define FIFO_NUM_BLKS NUM_BLKS(FIFO_SMPL_PERIOD_US) -#define MAX_FIFO_SIZE (FIFO_NUM_BLKS * BLK_SIZE_SAMPLES(CONFIG_AUDIO_SAMPLE_RATE_HZ) * 2) +#define MAX_FIFO_SIZE \ + (FIFO_NUM_BLKS * BLK_SIZE_SAMPLES(CONFIG_AUDIO_SAMPLE_RATE_HZ) * FIFO_CHANNELS_MAX) /* Number of audio blocks given a duration */ #define NUM_BLKS(d) ((d) / BLK_PERIOD_US) -/* Single audio block size in number of samples (stereo) */ +/* Single audio block size in number of samples */ /* clang-format off */ -#define BLK_SIZE_SAMPLES(r) (((r)*BLK_PERIOD_US) / 1000000) +#define BLK_SIZE_SAMPLES(r) (((r)*BLK_PERIOD_US) / USEC_PER_SEC) /* clang-format on */ /* Increment sample FIFO index by one block */ #define NEXT_IDX(i) (((i) < (FIFO_NUM_BLKS - 1)) ? ((i) + 1) : 0) /* Decrement sample FIFO index by one block */ -#define PREV_IDX(i) (((i) > 0) ? ((i)-1) : (FIFO_NUM_BLKS - 1)) +#define PREV_IDX(i) (((i) > 0) ? ((i) - 1) : (FIFO_NUM_BLKS - 1)) + +#define NUM_BLKS_IN_FRAME NUM_BLKS(CONFIG_AUDIO_FRAME_DURATION_US) +#define BLK_MONO_NUM_SAMPS BLK_SIZE_SAMPLES(CONFIG_AUDIO_SAMPLE_RATE_HZ) +#define BLK_MULTI_CHAN_NUM_SAMPS (BLK_MONO_NUM_SAMPS * CONFIG_AUDIO_OUTPUT_CHANNELS) -#define NUM_BLKS_IN_FRAME NUM_BLKS(CONFIG_AUDIO_FRAME_DURATION_US) -#define BLK_MONO_NUM_SAMPS BLK_SIZE_SAMPLES(CONFIG_AUDIO_SAMPLE_RATE_HZ) -#define BLK_STEREO_NUM_SAMPS (BLK_MONO_NUM_SAMPS * 2) /* Number of octets in a single audio block */ -#define BLK_MONO_SIZE_OCTETS (BLK_MONO_NUM_SAMPS * CONFIG_AUDIO_BIT_DEPTH_OCTETS) -#define BLK_STEREO_SIZE_OCTETS (BLK_MONO_SIZE_OCTETS * 2) +#define BLK_MONO_SIZE_OCTETS (BLK_MONO_NUM_SAMPS * CONFIG_AUDIO_BIT_DEPTH_OCTETS) +#define BLK_MULTI_CHAN_SIZE_OCTETS (BLK_MULTI_CHAN_NUM_SAMPS * CONFIG_AUDIO_BIT_DEPTH_OCTETS) + +/* Number of decoder buffers. */ +#define FIFO_NUM_BUFS 2 + /* How many function calls before moving on with drift compensation */ #define DRIFT_COMP_WAITING_CNT (DRIFT_MEAS_PERIOD_US / BLK_PERIOD_US) /* How much data to be collected before moving on with presentation compensation */ #define PRES_COMP_NUM_DATA_PTS (DRIFT_MEAS_PERIOD_US / CONFIG_AUDIO_FRAME_DURATION_US) +/* How many microseconds two timestamps can be apart before they are considered + * non-consecutive + */ +#define CONSECUTIVE_TS_LIMIT_US \ + (CONFIG_AUDIO_FRAME_DURATION_US + (CONFIG_AUDIO_FRAME_DURATION_US / 2)) -/* Audio clock - nRF5340 Analog Phase-Locked Loop (APLL) */ -#define APLL_FREQ_MIN HFCLKAUDIO_12_165_MHZ -#define APLL_FREQ_CENTER HFCLKAUDIO_12_288_MHZ -#define APLL_FREQ_MAX HFCLKAUDIO_12_411_MHZ /* Use nanoseconds to reduce rounding errors */ /* clang-format off */ #define APLL_FREQ_ADJ(t) (-((t)*1000) / 331) /* clang-format on */ -#define DRIFT_MEAS_PERIOD_US 100000 +#define DRIFT_MEAS_PERIOD_US 150000 #define DRIFT_ERR_THRESH_LOCK 16 #define DRIFT_ERR_THRESH_UNLOCK 32 /* To get smaller corrections */ #define DRIFT_REGULATOR_DIV_FACTOR 2 -/* To allow BLE transmission and (host -> HCI -> controller) */ -#define JUST_IN_TIME_TARGET_DLY_US 3000 -#define JUST_IN_TIME_BOUND_US 2500 +/* Allow normal block rounding and locked clock phase error before re-measuring. */ +#define PRES_RELOCK_ERR_US \ + ((BLK_PERIOD_US / 2) + DRIFT_ERR_THRESH_UNLOCK * DRIFT_REGULATOR_DIV_FACTOR) /* How often to print under-run warning */ -#define UNDERRUN_LOG_INTERVAL_BLKS 5000 +#define LOG_INTERVAL_BLKS 5000 + +/* Complete frames: queued input plus one encoding frame and one being filled by I2S. */ +NET_BUF_POOL_FIXED_DEFINE(pool_i2s_rx, CONFIG_FIFO_RX_FRAME_COUNT + 2, + (BLK_MULTI_CHAN_SIZE_OCTETS * CONFIG_FIFO_FRAME_SPLIT_NUM), + sizeof(struct audio_metadata), NULL); +NET_BUF_POOL_FIXED_DEFINE(audio_pcm_pool, FIFO_NUM_BUFS, PCM_NUM_BYTES_MULTI_CHAN, + sizeof(struct audio_metadata), NULL); -/* Smooth the transition from silence to local buffer playback. */ -#define BUFFER_PLAY_FADE_IN_MS 5U -#define BUFFER_PLAY_FADE_IN_SAMPLES ((CONFIG_AUDIO_SAMPLE_RATE_HZ * BUFFER_PLAY_FADE_IN_MS) / 1000U) +static atomic_t drop_next_block; enum drift_comp_state { DRIFT_STATE_INIT, /* Waiting for data to be received */ @@ -136,16 +138,13 @@ static const char *const pres_comp_state_names[] = { "LOCKED", }; -extern struct ring_buf ring_buffer; -extern struct k_mutex write_mutex; - static struct { bool datapath_initialized; bool stream_started; void *decoded_data; struct { - struct data_fifo *fifo; + struct k_msgq *audio_q; } in; struct { @@ -157,6 +156,7 @@ static struct { uint16_t prod_blk_idx; /* Output producer audio block index */ uint16_t cons_blk_idx; /* Output consumer audio block index */ uint32_t prod_blk_ts[FIFO_NUM_BLKS]; + bool prod_blk_valid[FIFO_NUM_BLKS]; /* Statistics */ uint32_t total_blk_underruns; } out; @@ -164,6 +164,7 @@ static struct { uint32_t prev_drift_sdu_ref_us; uint32_t prev_pres_sdu_ref_us; uint32_t current_pres_dly_us; + struct audio_startup startup; struct { enum drift_comp_state state: 8; @@ -178,246 +179,11 @@ static struct { uint16_t ctr; /* Count func calls. Used for collecting data points and waiting */ int32_t sum_err_dly_us; uint32_t pres_delay_us; + uint32_t underruns; bool enabled; } pres_comp; } ctrl_blk; -#include "openearable_common.h" - -#define SENQUEUE_FRAME_SIZE 32 - -static struct k_msgq * sensor_queue; - -//K_MSGQ_DEFINE(rx_queue, sizeof(struct audio_data), 16, 4); -extern struct k_msgq encoder_queue; - -// Definition eines zbus-Kanals -ZBUS_CHAN_DEFINE(audio_channel, struct audio_data, NULL, NULL, ZBUS_OBSERVERS_EMPTY, ZBUS_MSG_INIT(0)); - -// Thread-Stack und Daten -K_THREAD_STACK_DEFINE(data_thread_stack, CONFIG_ENCODER_STACK_SIZE); //CONFIG_DATA_THREAD_STACK_SIZE -static struct k_thread data_thread_data; -static k_tid_t data_thread_id; - -bool _record_to_sd = false; - -// Buffer recording variables -static bool _record_to_buffer = false; -static int16_t *_record_buffer = NULL; -static int _record_num_samples = 0; -static int _record_current_index = 0; -static bool _record_left = false; -static bool _record_right = false; -static void (*_record_callback)(void) = NULL; - -int _count = 0; - -static int16_t *buffer_play_data = NULL; -static uint32_t buffer_play_pos; -static uint32_t buffer_play_fade_pos; -static uint32_t buffer_play_num_samples; -static float buffer_play_amplitude; -static bool buffer_play_loop; -static void (*buffer_play_callback)(void) = NULL; - -extern struct k_poll_signal encoder_sig; -extern struct k_poll_event logger_sig; - -/* - * Decimation output for one interleaved stereo audio block. - * - * The decimator supports factors down to 1, so its worst-case output contains - * every input sample. Seal check currently uses factor 3 (48 kHz -> 16 kHz); - * sizing this buffer for the old fixed factor 4 overflowed it on every block. - */ -static int16_t decimated_audio[BLOCK_SIZE_BYTES / sizeof(int16_t)]; - -// Funktion für den neuen Thread -static void data_thread(void *arg1, void *arg2, void *arg3) -{ - ARG_UNUSED(arg1); - ARG_UNUSED(arg2); - ARG_UNUSED(arg3); - - //struct audio_data audio_item; - void *tmp_pcm_raw_data[CONFIG_FIFO_FRAME_SPLIT_NUM]; - //char pcm_raw_data[FRAME_SIZE_BYTES]; - size_t pcm_block_size; - int ret; - - struct audio_rx_data audio_item; - //memcpy(audio_item.data, pcm_raw_data, FRAME_SIZE_BYTES); - audio_item.size = FRAME_SIZE_BYTES; - - while (1) { - // Daten aus der data_queue lesen - for (int i = 0; i < CONFIG_FIFO_FRAME_SPLIT_NUM; i++) { - // wait for next sample block to be available - ret = data_fifo_pointer_last_filled_get(ctrl_blk.in.fifo, &tmp_pcm_raw_data[i], &pcm_block_size, K_FOREVER); - ERR_CHK(ret); - - uint64_t time_stamp = micros(); - - memcpy(audio_item.data + (i * BLOCK_SIZE_BYTES), tmp_pcm_raw_data[i], pcm_block_size); - - data_fifo_block_free(ctrl_blk.in.fifo, tmp_pcm_raw_data[i]); - - if (_record_to_sd || _record_to_buffer) { - /* Decimate audio data from 48kHz to the desired sampling rate */ - int16_t *audio_block = (int16_t *)(audio_item.data + (i * BLOCK_SIZE_BYTES)); - uint32_t num_frames = BLOCK_SIZE_BYTES / sizeof(int16_t) / 2; /* stereo frames */ - - int decimated_frames = audio_datapath_decimator_process(audio_block, decimated_audio, num_frames); - - // If decimator returns 0 frames (e.g. during cleanup), skip processing - if (decimated_frames <= 0) { - continue; - } - - // Generic buffer recording - if (_record_to_buffer && _record_buffer != NULL) { - for(int frame_index = 0; frame_index < decimated_frames; frame_index++) { - _record_current_index++; - - // Skip samples during initial drop period - if (_record_current_index <= 0) { - continue; - } - - // Calculate actual buffer index (after initial drop) - int buffer_index = _record_current_index - 1; - - if (buffer_index < _record_num_samples) { - if (_record_left && _record_right) { - // Stereo recording - store both channels - if (buffer_index * 2 + 1 < _record_num_samples) { - _record_buffer[buffer_index * 2] = decimated_audio[2 * frame_index]; // Left - _record_buffer[buffer_index * 2 + 1] = decimated_audio[2 * frame_index + 1]; // Right - } - } else if (_record_left) { - // Left channel only - _record_buffer[buffer_index] = decimated_audio[2 * frame_index]; - } else if (_record_right) { - // Right channel only - _record_buffer[buffer_index] = decimated_audio[2 * frame_index + 1]; - } - - } - - // Check if recording is complete - if (buffer_index >= _record_num_samples || - (_record_left && _record_right && buffer_index * 2 >= _record_num_samples)) { - _record_to_buffer = false; - if (_record_callback) { - _record_callback(); - } - break; - } - } - } - - struct sensor_msg audio_msg; - - audio_msg.sd = true; - audio_msg.stream = false; - - audio_msg.data.id = ID_MICRO; - audio_msg.data.time = time_stamp; - - audio_msg.data.size = decimated_frames * 2 * sizeof(int16_t); - - uint32_t data_size[2] = { - sizeof(audio_msg.data.id) + sizeof(audio_msg.data.size) + sizeof(audio_msg.data.time), - audio_msg.data.size - }; - - const void *data_ptrs[2] = { - &audio_msg.data, - decimated_audio - }; - - if ((uint32_t)decimated_frames == num_frames) { - data_ptrs[1] = audio_block; - } - - if (decimated_frames > 0) { - sdlogger_write_data(data_ptrs, data_size, 2); - } - } - - k_yield(); - } - - unsigned int signaled; - k_poll_signal_check(&encoder_sig, &signaled, &ret); - - if (ret == 0 && signaled != 0 && audio_system_encoder_is_started()) { - ret = k_msgq_put(&encoder_queue, &audio_item, K_NO_WAIT); - if (ret) { - LOG_WRN("encoder queue full"); - } - } - } -} - -void set_sensor_queue(struct k_msgq *queue) -{ - sensor_queue = queue; -} - - -void record_to_sd(bool active) { - _record_to_sd = active; -} - -void audio_datapath_stop_recording(void) { - // Stop buffer recording - _record_to_buffer = false; - _record_buffer = NULL; - _record_callback = NULL; - - // Stop SD recording - _record_to_sd = false; - - LOG_DBG("Audio recording stopped safely"); -} - -void record_to_buffer_stop(void) { - _record_to_buffer = false; - _record_buffer = NULL; - _record_callback = NULL; - LOG_DBG("Buffer recording stopped"); -} - -void record_to_buffer(int16_t *buffer, int num_samples, int initial_drop, bool left, bool right, void (*callback)(void)) { - if (buffer == NULL || num_samples <= 0) { - LOG_ERR("Invalid buffer recording parameters"); - return; - } - - _record_buffer = buffer; - _record_num_samples = num_samples; - _record_current_index = -initial_drop; - _record_left = left; - _record_right = right; - _record_callback = callback; - _record_to_buffer = true; - - LOG_INF("Started buffer recording: %d samples, initial_drop=%d, left=%d, right=%d", num_samples, initial_drop, left, right); -} - -// Funktion, um den neuen Thread zu starten -void start_data_thread(void) -{ - if (data_thread_id == NULL) { - data_thread_id = k_thread_create(&data_thread_data, data_thread_stack, CONFIG_ENCODER_STACK_SIZE, - data_thread, NULL, NULL, NULL, - K_PRIO_PREEMPT(5), 0, K_NO_WAIT); //CONFIG_DATA_THREAD_PRIO - k_thread_name_set(&data_thread_data, "data_thread"); - } - -} - /** * @brief Get the current number of blocks in the output buffer. */ @@ -432,16 +198,67 @@ static int filled_blocks_get(void) } } +static struct audio_metadata i2s_meta = {.data_coding = PCM, + .data_len_us = BLK_PERIOD_US, + .sample_rate_hz = CONFIG_AUDIO_SAMPLE_RATE_HZ, + .bits_per_sample = CONFIG_AUDIO_BIT_DEPTH_BITS, + .carried_bits_per_sample = CONFIG_AUDIO_BIT_DEPTH_BITS, + .bytes_per_location = BLK_MONO_SIZE_OCTETS, + .interleaved = true, + .locations = BT_AUDIO_LOCATION_FRONT_LEFT | + BT_AUDIO_LOCATION_FRONT_RIGHT, + .bad_data = 0}; + static bool tone_active; /* Buffer which can hold max 1 period test tone at 100 Hz */ static uint16_t test_tone_buf[CONFIG_AUDIO_SAMPLE_RATE_HZ / 100]; static size_t test_tone_size; +#define BUFFER_PLAY_FADE_IN_MS 5U +#define BUFFER_PLAY_FADE_IN_SAMPLES \ + ((CONFIG_AUDIO_SAMPLE_RATE_HZ * BUFFER_PLAY_FADE_IN_MS) / 1000U) +#define AUDIO_TAP_QUEUE_DEPTH 32 + +struct audio_tap_block { + uint64_t timestamp; + int16_t samples[BLK_MULTI_CHAN_NUM_SAMPS]; +}; + +K_MSGQ_DEFINE(audio_tap_queue, sizeof(struct audio_tap_block), AUDIO_TAP_QUEUE_DEPTH, + sizeof(uint32_t)); +K_MSGQ_DEFINE(auxiliary_audio_q, sizeof(struct net_buf *), 1, sizeof(void *)); +K_THREAD_STACK_DEFINE(audio_tap_stack, CONFIG_ENCODER_STACK_SIZE); +static struct k_thread audio_tap_thread_data; +static k_tid_t audio_tap_thread_id; +static bool record_sd; +static bool record_buffer; +static int16_t *record_buffer_data; +static int record_num_samples; +static int record_current_index; +static bool record_left; +static bool record_right; +static void (*record_callback)(void); +static int16_t decimated_audio[BLK_MULTI_CHAN_NUM_SAMPS]; + +static int16_t *buffer_play_data; +static uint32_t buffer_play_pos; +static uint32_t buffer_play_fade_pos; +static uint32_t buffer_play_num_samples; +static float buffer_play_amplitude; +static bool buffer_play_loop; +static void (*buffer_play_callback)(void); + +static int datapath_acquire_count; +K_MUTEX_DEFINE(datapath_owner_mutex); +/* A partial frame belongs to one encoder session only. */ +static struct net_buf *i2s_current_frame; +static uint32_t i2s_blocks_in_current_frame; + struct auxiliary_audio_state { bool suspended; - bool record_to_sd; - bool record_to_buffer; - int16_t *record_buffer; + bool record_sd; + bool record_buffer; + int16_t *record_buffer_data; int record_num_samples; int record_current_index; bool record_left; @@ -452,7 +269,7 @@ struct auxiliary_audio_state { int16_t *buffer_data; uint32_t buffer_pos; uint32_t buffer_fade_pos; - int buffer_num_samples; + uint32_t buffer_num_samples; float buffer_amplitude; bool buffer_loop; void (*buffer_callback)(void); @@ -460,6 +277,93 @@ struct auxiliary_audio_state { static struct auxiliary_audio_state auxiliary_audio_state; +static void audio_tap_thread(void *arg1, void *arg2, void *arg3) +{ + ARG_UNUSED(arg1); + ARG_UNUSED(arg2); + ARG_UNUSED(arg3); + + struct audio_tap_block block; + + while (true) { + int ret = k_msgq_get(&audio_tap_queue, &block, K_FOREVER); + if (ret) { + continue; + } + + int frames = audio_datapath_decimator_process( + block.samples, decimated_audio, BLK_MULTI_CHAN_NUM_SAMPS / 2U); + if (frames <= 0) { + continue; + } + + if (record_buffer && record_buffer_data != NULL) { + for (int frame = 0; frame < frames && record_buffer; frame++) { + record_current_index++; + if (record_current_index <= 0) { + continue; + } + + int index = record_current_index - 1; + if (record_left && record_right) { + int stereo_index = index * 2; + if ((stereo_index + 1) < record_num_samples) { + record_buffer_data[stereo_index] = decimated_audio[2 * frame]; + record_buffer_data[stereo_index + 1] = + decimated_audio[2 * frame + 1]; + } else { + record_buffer = false; + } + } else if (index < record_num_samples) { + record_buffer_data[index] = record_left + ? decimated_audio[2 * frame] + : decimated_audio[2 * frame + 1]; + } else { + record_buffer = false; + } + } + + if (!record_buffer && record_callback != NULL) { + void (*callback)(void) = record_callback; + record_callback = NULL; + callback(); + } + } + + if (record_sd) { + struct sensor_msg audio_msg = {0}; + audio_msg.sd = true; + audio_msg.data.id = ID_MICRO; + audio_msg.data.time = block.timestamp; + audio_msg.data.size = frames * 2U * sizeof(int16_t); + + uint32_t sizes[] = { + sizeof(audio_msg.data.id) + sizeof(audio_msg.data.size) + + sizeof(audio_msg.data.time), + audio_msg.data.size, + }; + const void *data[] = {&audio_msg.data, decimated_audio}; + sdlogger_write_data(data, sizes, ARRAY_SIZE(data)); + } + } +} + +static void audio_tap_thread_start(void) +{ + if (audio_tap_thread_id == NULL) { + audio_tap_thread_id = k_thread_create( + &audio_tap_thread_data, audio_tap_stack, K_THREAD_STACK_SIZEOF(audio_tap_stack), + audio_tap_thread, NULL, NULL, NULL, K_PRIO_PREEMPT(5), 0, K_NO_WAIT); + (void)k_thread_name_set(audio_tap_thread_id, "Audio tap"); + } +} + +/* Upon first received audio frame, the delta will be invalid (as there is no + * previous value to compare it to). Hence, this function only prints LOG_ERR + * if there are repeated consecutive errors. + */ +static uint32_t consec_invalid_ts_deltas; + /** * @brief Calculate error between sdu_ref and frame_start_ts_us. * @@ -476,7 +380,9 @@ static int32_t err_us_calculate(uint32_t sdu_ref_us, uint32_t frame_start_ts_us) int64_t total_err = ((int64_t)sdu_ref_us - (int64_t)frame_start_ts_us); - /* Store sign for later use, since remainder operation is undefined for negatives */ + /* Store sign for later use, since remainder operation is undefined for + * negatives + */ if (total_err < 0) { err_neg = true; total_err *= -1; @@ -497,16 +403,6 @@ static int32_t err_us_calculate(uint32_t sdu_ref_us, uint32_t frame_start_ts_us) return err_us; } -static void hfclkaudio_set(uint16_t freq_value) -{ - uint16_t freq_val = freq_value; - - freq_val = MIN(freq_val, APLL_FREQ_MAX); - freq_val = MAX(freq_val, APLL_FREQ_MIN); - - nrfx_clock_hfclkaudio_config_set(freq_val); -} - static void drift_comp_state_set(enum drift_comp_state new_state) { if (new_state == ctrl_blk.drift_comp.state) { @@ -525,11 +421,13 @@ static void drift_comp_state_set(enum drift_comp_state new_state) * * @param frame_start_ts_us I2S frame start timestamp. */ -static void audio_datapath_drift_compensation(uint32_t frame_start_ts_us) +static inline void audio_datapath_drift_compensation(uint32_t frame_start_ts_us) { + int ret; + if (CONFIG_AUDIO_DEV == HEADSET) { - /** For headsets we do not use the timestamp gotten from hci_tx_sync_get to adjust - * for drift + /** For headsets we do not use the timestamp gotten from hci_tx_sync_get to + * adjust for drift */ ctrl_blk.prev_drift_sdu_ref_us = ctrl_blk.prev_pres_sdu_ref_us; } @@ -566,7 +464,11 @@ static void audio_datapath_drift_compensation(uint32_t frame_start_ts_us) return; } - hfclkaudio_set(ctrl_blk.drift_comp.center_freq); + ret = audio_clock_set(ctrl_blk.drift_comp.center_freq); + if (ret) { + LOG_ERR("Failed to set audio clock frequency"); + return; + } drift_comp_state_set(DRIFT_STATE_OFFSET); @@ -586,7 +488,11 @@ static void audio_datapath_drift_compensation(uint32_t frame_start_ts_us) err_us /= DRIFT_REGULATOR_DIV_FACTOR; int32_t freq_adj = APLL_FREQ_ADJ(err_us); - hfclkaudio_set(ctrl_blk.drift_comp.center_freq + freq_adj); + ret = audio_clock_set(ctrl_blk.drift_comp.center_freq + freq_adj); + if (ret) { + LOG_ERR("Failed to set audio clock frequency"); + return; + } if ((err_us < DRIFT_ERR_THRESH_LOCK) && (err_us > -DRIFT_ERR_THRESH_LOCK)) { drift_comp_state_set(DRIFT_STATE_LOCKED); @@ -608,7 +514,11 @@ static void audio_datapath_drift_compensation(uint32_t frame_start_ts_us) err_us /= DRIFT_REGULATOR_DIV_FACTOR; int32_t freq_adj = APLL_FREQ_ADJ(err_us); - hfclkaudio_set(ctrl_blk.drift_comp.center_freq + freq_adj); + ret = audio_clock_set(ctrl_blk.drift_comp.center_freq + freq_adj); + if (ret) { + LOG_ERR("Failed to set audio clock frequency"); + return; + } if ((err_us > DRIFT_ERR_THRESH_UNLOCK) || (err_us < -DRIFT_ERR_THRESH_UNLOCK)) { drift_comp_state_set(DRIFT_STATE_INIT); @@ -631,14 +541,13 @@ static void pres_comp_state_set(enum pres_comp_state new_state) ctrl_blk.pres_comp.state = new_state; /* NOTE: The string below is used by the Nordic CI system */ LOG_INF("Pres comp state: %s", pres_comp_state_names[new_state]); - -#if CONFIG_BOARD_NRF5340_AUDIO_DK_NRF5340_CPUAPP +#if DT_NODE_EXISTS(DT_NODELABEL(audioleds)) int ret; if (new_state == PRES_STATE_LOCKED) { - ret = led_on(LED_APP_2_GREEN); + ret = led_on(LED_AUDIO_SYNC_STATUS); } else { - ret = led_off(LED_APP_2_GREEN); + ret = led_off(LED_AUDIO_SYNC_STATUS); } ERR_CHK(ret); #endif @@ -651,22 +560,26 @@ static void pres_comp_state_set(enum pres_comp_state new_state) * * @param recv_frame_ts_us Timestamp of when frame was received. * @param sdu_ref_us ISO timestamp reference from Bluetooth LE controller. - * @param sdu_ref_not_consecutive True if sdu_ref_us and the previous sdu_ref_us - * originate from non-consecutive frames. + * @param sdu_ref_not_consecutive True if sdu_ref_us and the previous sdu_ref_us originate + * from non-consecutive frames. */ static void audio_datapath_presentation_compensation(uint32_t recv_frame_ts_us, uint32_t sdu_ref_us, bool sdu_ref_not_consecutive) { + uint32_t underruns = ctrl_blk.out.total_blk_underruns; + bool underrun = underruns != ctrl_blk.pres_comp.underruns; + ctrl_blk.pres_comp.underruns = underruns; + if (ctrl_blk.drift_comp.state != DRIFT_STATE_LOCKED) { /* Unconditionally reset state machine if drift compensation looses lock */ pres_comp_state_set(PRES_STATE_INIT); return; } - /* Move presentation compensation into PRES_STATE_WAIT if sdu_ref_us and - * the previous sdu_ref_us originate from non-consecutive frames. + /* Let buffered data settle after a timestamp gap or an audible-stream + * underrun. A whole-block delay does not necessarily unlock the clock. */ - if (sdu_ref_not_consecutive) { + if (sdu_ref_not_consecutive || (ctrl_blk.startup.open && underrun)) { ctrl_blk.pres_comp.ctr = 0; pres_comp_state_set(PRES_STATE_WAIT); } @@ -695,6 +608,7 @@ static void audio_datapath_presentation_compensation(uint32_t recv_frame_ts_us, ctrl_blk.pres_comp.ctr = 0; pres_adj_us = ctrl_blk.pres_comp.sum_err_dly_us / PRES_COMP_NUM_DATA_PTS; + ctrl_blk.pres_comp.sum_err_dly_us = 0; if ((pres_adj_us >= (BLK_PERIOD_US / 2)) || (pres_adj_us <= -(BLK_PERIOD_US / 2))) { pres_comp_state_set(PRES_STATE_WAIT); } else { @@ -713,11 +627,21 @@ static void audio_datapath_presentation_compensation(uint32_t recv_frame_ts_us, break; } case PRES_STATE_LOCKED: { - /* - * Presentation delay compensation moves into PRES_STATE_WAIT if sdu_ref_us - * and the previous sdu_ref_us originate from non-consecutive frames, or into - * PRES_STATE_INIT if drift compensation unlocks. + /* Average a full measurement window before requesting a fresh alignment + * measurement. That second window confirms the error before moving PCM. + * Keep the clock locked and leave the already-open startup gate alone. */ + ctrl_blk.pres_comp.sum_err_dly_us += + wanted_pres_dly_us - ctrl_blk.current_pres_dly_us; + if (++ctrl_blk.pres_comp.ctr >= PRES_COMP_NUM_DATA_PTS) { + int32_t error_us = + ctrl_blk.pres_comp.sum_err_dly_us / PRES_COMP_NUM_DATA_PTS; + ctrl_blk.pres_comp.ctr = 0; + ctrl_blk.pres_comp.sum_err_dly_us = 0; + if (error_us > PRES_RELOCK_ERR_US || error_us < -PRES_RELOCK_ERR_US) { + pres_comp_state_set(PRES_STATE_INIT); + } + } break; } @@ -752,14 +676,21 @@ static void audio_datapath_presentation_compensation(uint32_t recv_frame_ts_us, LOG_WRN("Requested presentation delay out of range: pres_adj_us=%d", pres_adj_us); } + /* Preserve the DMA/queued blocks and room for the frame decoded next. */ + int queued = filled_blocks_get(); + pres_adj_blks = CLAMP(pres_adj_blks, -MAX(queued - 2, 0), + MAX(FIFO_NUM_BLKS - queued - NUM_BLKS_IN_FRAME - 2, 0)); + if (pres_adj_blks > 0) { LOG_DBG("Presentation delay inserted: pres_adj_blks=%d", pres_adj_blks); /* Increase presentation delay */ for (int i = 0; i < pres_adj_blks; i++) { + ctrl_blk.out.prod_blk_valid[ctrl_blk.out.prod_blk_idx] = false; /* Mute audio block */ - memset(&ctrl_blk.out.fifo[ctrl_blk.out.prod_blk_idx * BLK_STEREO_NUM_SAMPS], - 0, BLK_STEREO_SIZE_OCTETS); + memset(&ctrl_blk.out + .fifo[ctrl_blk.out.prod_blk_idx * BLK_MULTI_CHAN_NUM_SAMPS], + 0, BLK_MULTI_CHAN_SIZE_OCTETS); /* Record producer block start reference */ ctrl_blk.out.prod_blk_ts[ctrl_blk.out.prod_blk_idx] = @@ -783,112 +714,24 @@ static void tone_stop_worker(struct k_work *work) tone_active = false; memset(test_tone_buf, 0, sizeof(test_tone_buf)); - - LOG_INF("Tone playback stopped"); - - // Call buffer playback callback if set - if (buffer_play_callback) { - buffer_play_callback(); + buffer_play_data = NULL; + if (buffer_play_callback != NULL) { + void (*callback)(void) = buffer_play_callback; buffer_play_callback = NULL; + callback(); } - buffer_play_data = NULL; - LOG_DBG("Tone stopped"); - - struct sensor_config mic = {ID_MICRO, 0, 0}; - config_sensor(&mic); } K_WORK_DEFINE(tone_stop_work, tone_stop_worker); static void tone_stop_timer_handler(struct k_timer *dummy) { - ARG_UNUSED(dummy); - k_work_submit(&tone_stop_work); }; K_TIMER_DEFINE(tone_stop_timer, tone_stop_timer_handler, NULL); -int audio_datapath_auxiliary_suspend(void) -{ - if (auxiliary_audio_state.suspended) { - return -EBUSY; - } - - auxiliary_audio_state = (struct auxiliary_audio_state) { - .suspended = true, - .record_to_sd = _record_to_sd, - .record_to_buffer = _record_to_buffer, - .record_buffer = _record_buffer, - .record_num_samples = _record_num_samples, - .record_current_index = _record_current_index, - .record_left = _record_left, - .record_right = _record_right, - .record_callback = _record_callback, - .tone_active = tone_active, - .tone_remaining_ms = k_timer_remaining_get(&tone_stop_timer), - .buffer_data = buffer_play_data, - .buffer_pos = buffer_play_pos, - .buffer_fade_pos = buffer_play_fade_pos, - .buffer_num_samples = buffer_play_num_samples, - .buffer_amplitude = buffer_play_amplitude, - .buffer_loop = buffer_play_loop, - .buffer_callback = buffer_play_callback, - }; - - k_timer_stop(&tone_stop_timer); - (void)k_work_cancel(&tone_stop_work); - _record_to_sd = false; - _record_to_buffer = false; - _record_buffer = NULL; - _record_callback = NULL; - tone_active = false; - buffer_play_data = NULL; - buffer_play_callback = NULL; - - LOG_INF("Auxiliary audio suspended: tone=%d buffer=%d buffer_recording=%d sd_recording=%d", - auxiliary_audio_state.tone_active, auxiliary_audio_state.buffer_data != NULL, - auxiliary_audio_state.record_to_buffer, auxiliary_audio_state.record_to_sd); - return 0; -} - -int audio_datapath_auxiliary_resume(void) -{ - if (!auxiliary_audio_state.suspended) { - return -EALREADY; - } - - /* A completion queued by measurement playback must not clear restored state. */ - (void)k_work_cancel(&tone_stop_work); - _record_to_sd = auxiliary_audio_state.record_to_sd; - _record_to_buffer = auxiliary_audio_state.record_to_buffer; - _record_buffer = auxiliary_audio_state.record_buffer; - _record_num_samples = auxiliary_audio_state.record_num_samples; - _record_current_index = auxiliary_audio_state.record_current_index; - _record_left = auxiliary_audio_state.record_left; - _record_right = auxiliary_audio_state.record_right; - _record_callback = auxiliary_audio_state.record_callback; - tone_active = auxiliary_audio_state.tone_active; - buffer_play_data = auxiliary_audio_state.buffer_data; - buffer_play_pos = auxiliary_audio_state.buffer_pos; - buffer_play_fade_pos = auxiliary_audio_state.buffer_fade_pos; - buffer_play_num_samples = auxiliary_audio_state.buffer_num_samples; - buffer_play_amplitude = auxiliary_audio_state.buffer_amplitude; - buffer_play_loop = auxiliary_audio_state.buffer_loop; - buffer_play_callback = auxiliary_audio_state.buffer_callback; - - if (tone_active && auxiliary_audio_state.tone_remaining_ms > 0U) { - k_timer_start(&tone_stop_timer, K_MSEC(auxiliary_audio_state.tone_remaining_ms), - K_NO_WAIT); - } - - LOG_INF("Auxiliary audio resumed: tone=%d buffer=%d buffer_recording=%d sd_recording=%d", - tone_active, buffer_play_data != NULL, _record_to_buffer, _record_to_sd); - memset(&auxiliary_audio_state, 0, sizeof(auxiliary_audio_state)); - return 0; -} - int audio_datapath_tone_play(uint16_t freq, uint16_t dur_ms, float amplitude) { int ret; @@ -918,19 +761,19 @@ int audio_datapath_tone_play(uint16_t freq, uint16_t dur_ms, float amplitude) return 0; } -int audio_datapath_buffer_play(int16_t *buffer, int num_samples, bool loop, float amplitude, void (*callback)(void)) +void audio_datapath_tone_stop(void) +{ + k_timer_stop(&tone_stop_timer); + k_work_submit(&tone_stop_work); +} + +int audio_datapath_buffer_play(int16_t *buffer, int num_samples, bool loop, float amplitude, + void (*callback)(void)) { if (buffer_play_data != NULL) { return -EBUSY; } - - if (!IS_ENABLED(CONFIG_AUDIO_TEST_TONE)) { - LOG_WRN("Test tone disabled"); - return -ENOTSUP; - } - - if (buffer == NULL || num_samples <= 0) { - LOG_ERR("Invalid buffer play parameters"); + if (buffer == NULL || num_samples <= 0 || amplitude < 0.0f || amplitude > 1.0f) { return -EINVAL; } @@ -941,26 +784,17 @@ int audio_datapath_buffer_play(int16_t *buffer, int num_samples, bool loop, floa buffer_play_amplitude = amplitude; buffer_play_loop = loop; buffer_play_callback = callback; - - LOG_DBG("Buffer playback started: %d samples, loop=%d, amplitude=%.2f", num_samples, loop, (double)amplitude); return 0; } -void audio_datapath_tone_stop(void) -{ - k_timer_stop(&tone_stop_timer); - k_work_submit(&tone_stop_work); -} - void audio_datapath_buffer_stop(void) { - k_timer_stop(&tone_stop_timer); buffer_play_data = NULL; - if (buffer_play_callback) { - buffer_play_callback(); + if (buffer_play_callback != NULL) { + void (*callback)(void) = buffer_play_callback; buffer_play_callback = NULL; + callback(); } - LOG_DBG("Buffer playback stopped"); } static void tone_mix(uint8_t *tx_buf) @@ -974,42 +808,33 @@ static void tone_mix(uint8_t *tx_buf) ret = contin_array_create(tone_buf_continuous, BLK_MONO_SIZE_OCTETS, test_tone_buf, test_tone_size, &finite_pos); ERR_CHK(ret); - - ret = pcm_mix(tx_buf, BLK_STEREO_SIZE_OCTETS, tone_buf_continuous, BLK_MONO_SIZE_OCTETS, - B_MONO_INTO_A_STEREO_L); + ret = pcm_mix(tx_buf, BLK_MULTI_CHAN_SIZE_OCTETS, tone_buf_continuous, + BLK_MONO_SIZE_OCTETS, B_MONO_INTO_A_STEREO_L); ERR_CHK(ret); } else if (buffer_play_data != NULL) { - int8_t buffer_play_buf[BLK_MONO_SIZE_OCTETS]; - int samples_per_block = BLK_MONO_SIZE_OCTETS / sizeof(int16_t); + int16_t playback[BLK_MONO_NUM_SAMPS]; - /* Copy buffer samples to playback buffer with amplitude scaling */ - for (int i = 0; i < samples_per_block; i++) { + for (size_t i = 0; i < ARRAY_SIZE(playback); i++) { float gain = buffer_play_amplitude; - if (buffer_play_pos >= buffer_play_num_samples) { if (buffer_play_loop) { - buffer_play_pos = 0; /* Loop the buffer */ + buffer_play_pos = 0; } else { - /* Stop after one complete playback */ + memset(&playback[i], 0, + (ARRAY_SIZE(playback) - i) * sizeof(playback[0])); k_work_submit(&tone_stop_work); - memset(&buffer_play_buf[i * 2], 0, (samples_per_block - i) * 2); break; } } - if (buffer_play_fade_pos < BUFFER_PLAY_FADE_IN_SAMPLES) { gain *= (float)buffer_play_fade_pos / BUFFER_PLAY_FADE_IN_SAMPLES; buffer_play_fade_pos++; } - - int16_t sample = (int16_t)(buffer_play_data[buffer_play_pos] * gain); - buffer_play_buf[i * 2] = sample & 0xFF; - buffer_play_buf[i * 2 + 1] = (sample >> 8) & 0xFF; - buffer_play_pos++; + playback[i] = (int16_t)(buffer_play_data[buffer_play_pos++] * gain); } - ret = pcm_mix(tx_buf, BLK_STEREO_SIZE_OCTETS, buffer_play_buf, BLK_MONO_SIZE_OCTETS, - B_MONO_INTO_A_STEREO_L); + ret = pcm_mix(tx_buf, BLK_MULTI_CHAN_SIZE_OCTETS, playback, + BLK_MONO_SIZE_OCTETS, B_MONO_INTO_A_STEREO_L); ERR_CHK(ret); } } @@ -1018,8 +843,8 @@ static void tone_mix(uint8_t *tx_buf) * Used interchangeably by I2S. */ static struct { - uint8_t __aligned(WB_UP(1)) buf_0[BLK_STEREO_SIZE_OCTETS]; - uint8_t __aligned(WB_UP(1)) buf_1[BLK_STEREO_SIZE_OCTETS]; + uint8_t __aligned(WB_UP(1)) buf_0[BLK_MULTI_CHAN_SIZE_OCTETS]; + uint8_t __aligned(WB_UP(1)) buf_1[BLK_MULTI_CHAN_SIZE_OCTETS]; bool buf_0_in_use; bool buf_1_in_use; } alt; @@ -1032,7 +857,7 @@ static struct { * @retval 0 if success. * @retval -ENOMEM No available buffers. */ -static int alt_buffer_get(void **p_buffer) +static inline int alt_buffer_get(void **p_buffer) { if (!alt.buf_0_in_use) { alt.buf_0_in_use = true; @@ -1053,7 +878,7 @@ static int alt_buffer_get(void **p_buffer) * * @param p_buffer Buffer to free. */ -static void alt_buffer_free(void const *const p_buffer) +static inline void alt_buffer_free(void const *const p_buffer) { if (p_buffer == alt.buf_0) { alt.buf_0_in_use = false; @@ -1071,12 +896,38 @@ static void alt_buffer_free_both(void) alt.buf_1_in_use = false; } -__attribute__((weak)) void bt_mgmt_report_audio_underrun(uint32_t count) { - ARG_UNUSED(count); +/* Consume startup PCM normally so timing can settle, but keep it inaudible. + * Local tones and buffer playback are mixed after this gate. + */ +static void audio_datapath_startup_apply(uint8_t *buffer, bool valid_pcm) +{ + if (CONFIG_AUDIO_DEV != HEADSET) { + return; + } - LOG_ERR("Audio underrun reported to bt_mgmt"); -} + const uint32_t fade_frames = CONFIG_AUDIO_SAMPLE_RATE_HZ * 5U / 1000U; + bool synchronized = + (!ctrl_blk.drift_comp.enabled || ctrl_blk.drift_comp.state == DRIFT_STATE_LOCKED) && + (!ctrl_blk.pres_comp.enabled || ctrl_blk.pres_comp.state == PRES_STATE_LOCKED); + bool ready = audio_startup_ready(&ctrl_blk.startup, + stream_state_get() == STATE_STREAMING, valid_pcm && synchronized, BLK_PERIOD_US); + if (!ready) { + memset(buffer, 0, BLK_MULTI_CHAN_SIZE_OCTETS); + } else if (ctrl_blk.startup.fade_frames < fade_frames) { +#if CONFIG_AUDIO_BIT_DEPTH_16 + int16_t *samples = (int16_t *)buffer; +#else + int32_t *samples = (int32_t *)buffer; +#endif + for (uint32_t i = 0; i < BLK_MULTI_CHAN_NUM_SAMPS; i += CONFIG_AUDIO_OUTPUT_CHANNELS) { + uint32_t gain = audio_startup_gain(&ctrl_blk.startup, fade_frames); + for (uint32_t ch = 0; ch < CONFIG_AUDIO_OUTPUT_CHANNELS; ch++) { + samples[i + ch] = audio_startup_scale(samples[i + ch], gain, fade_frames); + } + } + } +} /* * This handler function is called every time I2S needs new buffers for @@ -1091,9 +942,10 @@ __attribute__((weak)) void bt_mgmt_report_audio_underrun(uint32_t count) { static void audio_datapath_i2s_blk_complete(uint32_t frame_start_ts_us, uint32_t *rx_buf_released, uint32_t const *tx_buf_released) { - int ret; - static bool underrun_condition; - static uint32_t released_tx_reuse_count; + int ret = 0; + static uint32_t num_calls; + + num_calls++; alt_buffer_free(tx_buf_released); @@ -1105,109 +957,157 @@ static void audio_datapath_i2s_blk_complete(uint32_t frame_start_ts_us, uint32_t static uint8_t *tx_buf; if (IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || (CONFIG_AUDIO_DEV == HEADSET)) { - if (tx_buf_released != NULL) { - /* Double buffered index */ - uint32_t next_out_blk_idx = NEXT_IDX(ctrl_blk.out.cons_blk_idx); - - if (next_out_blk_idx != ctrl_blk.out.prod_blk_idx) { - /* Only increment if not in under-run condition */ - ctrl_blk.out.cons_blk_idx = next_out_blk_idx; - if (underrun_condition) { - underrun_condition = false; - LOG_WRN("Data received, total under-runs: %d", - ctrl_blk.out.total_blk_underruns); - bt_mgmt_report_audio_underrun(ctrl_blk.out.total_blk_underruns); - } + static bool underrun_condition; + bool valid_pcm = false; - tx_buf = (uint8_t *)&ctrl_blk.out - .fifo[next_out_blk_idx * BLK_STEREO_NUM_SAMPS]; + if (tx_buf_released == NULL) { + ERR_CHK_MSG(-ENOMEM, "No TX data available"); + } - } else { - if (stream_state_get() == STATE_STREAMING) { - underrun_condition = true; - ctrl_blk.out.total_blk_underruns++; + /* Double buffered index */ + uint32_t next_out_blk_idx = NEXT_IDX(ctrl_blk.out.cons_blk_idx); + + if (next_out_blk_idx != ctrl_blk.out.prod_blk_idx) { + /* Only increment if not in under-run condition */ + ctrl_blk.out.cons_blk_idx = next_out_blk_idx; + if (underrun_condition) { + underrun_condition = false; + LOG_WRN("Data received, total under-runs: %d", + ctrl_blk.out.total_blk_underruns); + bt_mgmt_report_audio_underrun(ctrl_blk.out.total_blk_underruns); + } - if ((ctrl_blk.out.total_blk_underruns % - UNDERRUN_LOG_INTERVAL_BLKS) == 0) { - LOG_WRN("In I2S TX under-run condition, total: %d", - ctrl_blk.out.total_blk_underruns); - } - } + tx_buf = (uint8_t *)&ctrl_blk.out + .fifo[next_out_blk_idx * BLK_MULTI_CHAN_NUM_SAMPS]; + valid_pcm = ctrl_blk.out.prod_blk_valid[next_out_blk_idx]; - /* - * No data available in out.fifo - * use alternative buffers - */ - ret = alt_buffer_get((void **)&tx_buf); - if (ret) { - released_tx_reuse_count++; - if (released_tx_reuse_count == 1U || - (released_tx_reuse_count % UNDERRUN_LOG_INTERVAL_BLKS) == 0U) { - LOG_WRN("No alternative I2S TX buffer available; " - "reusing released buffer as silence, total: %u", - (unsigned int)released_tx_reuse_count); - } - /* I2S no longer owns this buffer; recycle it as silence - * instead of leaving tx_buf NULL. - */ - tx_buf = (uint8_t *)tx_buf_released; - } + } else { + if (stream_state_get() == STATE_STREAMING) { + underrun_condition = true; + ctrl_blk.out.total_blk_underruns++; - memset(tx_buf, 0, BLK_STEREO_SIZE_OCTETS); + if ((ctrl_blk.out.total_blk_underruns % LOG_INTERVAL_BLKS) == 0) { + LOG_WRN("In I2S TX under-run condition, total: %d", + ctrl_blk.out.total_blk_underruns); + } } - if (tone_active || buffer_play_data != NULL) { - tone_mix(tx_buf); + /* + * No data available in out.fifo + * use alternative buffers + */ + ret = alt_buffer_get((void **)&tx_buf); + if (ret) { + ERR_CHK(ret); + return; } + + memset(tx_buf, 0, BLK_MULTI_CHAN_SIZE_OCTETS); + } + + audio_datapath_startup_apply(tx_buf, valid_pcm); + + if (tone_active || buffer_play_data != NULL) { + tone_mix(tx_buf); } } /********** I2S RX **********/ - static uint32_t *rx_buf; - static int prev_ret; - - if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || (CONFIG_AUDIO_DEV == GATEWAY)) && IS_ENABLED(CONFIG_AUDIO_MIC_I2S)) { - /* Lock last filled buffer into message queue */ - if (rx_buf_released != NULL) { - ret = data_fifo_block_lock(ctrl_blk.in.fifo, (void **)&rx_buf_released, - BLOCK_SIZE_BYTES); + static uint32_t num_overruns; + static uint32_t num_overruns_last_printed; + + if ((record_sd || record_buffer) && rx_buf_released != NULL) { + struct audio_tap_block block = {.timestamp = micros()}; + memcpy(block.samples, rx_buf_released, sizeof(block.samples)); + if (k_msgq_put(&audio_tap_queue, &block, K_NO_WAIT) != 0) { + LOG_WRN_RATELIMIT("Audio tap queue full"); + } + } - ERR_CHK_MSG(ret, "Unable to lock block RX"); + if (audio_system_encoder_is_started()) { + if (unlikely(rx_buf_released == NULL)) { + ERR_CHK_MSG(-ENOMEM, "No RX data available"); } - /* Get new empty buffer to send to I2S HW */ - ret = data_fifo_pointer_first_vacant_get(ctrl_blk.in.fifo, (void **)&rx_buf, - K_NO_WAIT); - if (ret == 0 && prev_ret == -ENOMEM) { - LOG_WRN("I2S RX continuing stream"); - prev_ret = ret; + if (atomic_get(&drop_next_block)) { + /* Drop this block to align with just-in-time */ + atomic_set(&drop_next_block, false); + goto i2s_rx_done; } - /* If RX FIFO is filled up */ - if (ret == -ENOMEM) { - void *data; - size_t size; + /* Allocate new frame if we don't have one */ + if (i2s_current_frame == NULL) { + /* Check space availability */ + i2s_current_frame = net_buf_alloc(&pool_i2s_rx, K_NO_WAIT); + if (unlikely(i2s_current_frame == NULL)) { + LOG_DBG("Out of I2S RX buffers for frame"); + if (unlikely((++num_overruns % 100) == 1)) { + LOG_WRN("I2S RX overrun count: %d", num_overruns); + } - if (ret != prev_ret) { - LOG_WRN("I2S RX overrun. Single msg"); - prev_ret = ret; + goto i2s_rx_done; } - ret = data_fifo_pointer_last_filled_get(ctrl_blk.in.fifo, &data, &size, - K_NO_WAIT); - ERR_CHK(ret); + /* Initialize metadata for the first block */ + struct audio_metadata *meta = net_buf_user_data(i2s_current_frame); + *meta = i2s_meta; + meta->data_len_us = 0; + meta->bytes_per_location = 0; + i2s_blocks_in_current_frame = 0; + } + + /* Add block data directly to current frame */ + net_buf_add_mem(i2s_current_frame, rx_buf_released, BLK_MULTI_CHAN_SIZE_OCTETS); + + /* Update metadata */ + struct audio_metadata *meta = net_buf_user_data(i2s_current_frame); + + meta->data_len_us += i2s_meta.data_len_us; + meta->bytes_per_location += i2s_meta.bytes_per_location; + i2s_blocks_in_current_frame++; - data_fifo_block_free(ctrl_blk.in.fifo, data); + /* Check if we have a complete 10ms frame */ + if (i2s_blocks_in_current_frame >= CONFIG_FIFO_FRAME_SPLIT_NUM) { + if (unlikely(k_msgq_num_free_get(ctrl_blk.in.audio_q) == 0)) { + LOG_DBG("RX queue full, dropping I2S RX frame"); + if (unlikely((++num_overruns % 100) == 1)) { + LOG_WRN("I2S RX overrun count: %d", num_overruns); + } + + /* Remove latest block to delay frame completion until message + * queue is ready + */ + net_buf_remove_mem(i2s_current_frame, BLK_MULTI_CHAN_SIZE_OCTETS); + meta->data_len_us -= i2s_meta.data_len_us; + meta->bytes_per_location -= i2s_meta.bytes_per_location; + i2s_blocks_in_current_frame--; - ret = data_fifo_pointer_first_vacant_get(ctrl_blk.in.fifo, (void **)&rx_buf, - K_NO_WAIT); + goto i2s_rx_done; + } + + /* Put complete frame into RX queue */ + ret = k_msgq_put(ctrl_blk.in.audio_q, (void *)&i2s_current_frame, + K_NO_WAIT); + if (ret) { + LOG_ERR("Unable to put I2S complete frame into queue"); + net_buf_unref(i2s_current_frame); + } + + /* Reset for next frame */ + i2s_current_frame = NULL; + i2s_blocks_in_current_frame = 0; } - ERR_CHK_MSG(ret, "RX failed to get block"); +i2s_rx_done: + /* Print overrun stats periodically */ + if ((num_calls % LOG_INTERVAL_BLKS == 0) && + (num_overruns != num_overruns_last_printed)) { + num_overruns_last_printed = num_overruns; + } } /*** Data exchange ***/ - audio_i2s_set_next_buf(tx_buf, rx_buf); + audio_i2s_set_next_buf(tx_buf, rx_buf_released); /*** Drift compensation ***/ if (ctrl_blk.drift_comp.enabled) { @@ -1217,50 +1117,28 @@ static void audio_datapath_i2s_blk_complete(uint32_t frame_start_ts_us, uint32_t static void audio_datapath_i2s_start(void) { - int ret; - /* Double buffer I2S */ - uint8_t *tx_buf_one = NULL; - uint8_t *tx_buf_two = NULL; - uint32_t *rx_buf_one = NULL; - uint32_t *rx_buf_two = NULL; + uint8_t *tx_buf_0 = NULL; + uint8_t *tx_buf_1 = NULL; + + /* Buffers used for I2S RX. Used interchangeably by I2S. */ + static uint32_t rx_buf_0[BLK_MULTI_CHAN_SIZE_OCTETS]; + static uint32_t rx_buf_1[BLK_MULTI_CHAN_SIZE_OCTETS]; /* TX */ if (IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || (CONFIG_AUDIO_DEV == HEADSET)) { ctrl_blk.out.cons_blk_idx = PREV_IDX(ctrl_blk.out.cons_blk_idx); - tx_buf_one = (uint8_t *)&ctrl_blk.out - .fifo[ctrl_blk.out.cons_blk_idx * BLK_STEREO_NUM_SAMPS]; + tx_buf_0 = (uint8_t *)&ctrl_blk.out + .fifo[ctrl_blk.out.cons_blk_idx * BLK_MULTI_CHAN_NUM_SAMPS]; ctrl_blk.out.cons_blk_idx = PREV_IDX(ctrl_blk.out.cons_blk_idx); - tx_buf_two = (uint8_t *)&ctrl_blk.out - .fifo[ctrl_blk.out.cons_blk_idx * BLK_STEREO_NUM_SAMPS]; + tx_buf_1 = (uint8_t *)&ctrl_blk.out + .fifo[ctrl_blk.out.cons_blk_idx * BLK_MULTI_CHAN_NUM_SAMPS]; } - /* RX */ - if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || (CONFIG_AUDIO_DEV == GATEWAY)) && IS_ENABLED(CONFIG_AUDIO_MIC_I2S)) { - uint32_t alloced_cnt; - uint32_t locked_cnt; - - ret = data_fifo_num_used_get(ctrl_blk.in.fifo, &alloced_cnt, &locked_cnt); - if (alloced_cnt || locked_cnt || ret) { - ERR_CHK_MSG(-ENOMEM, "FIFO is not empty!"); - } - - ret = data_fifo_pointer_first_vacant_get(ctrl_blk.in.fifo, (void **)&rx_buf_one, - K_NO_WAIT); - ERR_CHK_MSG(ret, "RX failed to get block"); - ret = data_fifo_pointer_first_vacant_get(ctrl_blk.in.fifo, (void **)&rx_buf_two, - K_NO_WAIT); - ERR_CHK_MSG(ret, "RX failed to get block"); - } - -#if CONFIG_EQAULIZER_SOFTWARE - reset_eq(); -#endif - /* Start I2S */ - audio_i2s_start(tx_buf_one, rx_buf_one); - audio_i2s_set_next_buf(tx_buf_two, rx_buf_two); + audio_i2s_start(tx_buf_0, rx_buf_0); + audio_i2s_set_next_buf(tx_buf_1, rx_buf_1); } static void audio_datapath_i2s_stop(void) @@ -1272,8 +1150,8 @@ static void audio_datapath_i2s_stop(void) /** * @brief Adjust timing to make sure audio data is sent just in time for Bluetooth LE event. * - * @note The time from last anchor point is checked and then blocks of 1 ms can be dropped - * to allow the sending of encoded data to be sent just before the connection interval + * @note The time from last anchor point is checked and then blocks of 1 ms can be dropped to + * allow the sending of encoded data to be sent just before the connection interval * opens up. This is done to reduce overall latency. * * @param[in] tx_sync_ts_us The timestamp from get_tx_sync. @@ -1282,16 +1160,16 @@ static void audio_datapath_i2s_stop(void) static void audio_datapath_just_in_time_check_and_adjust(uint32_t tx_sync_ts_us, uint32_t curr_ts_us) { - int ret; static int32_t print_count; int64_t diff; diff = (int64_t)tx_sync_ts_us - curr_ts_us; /* - * The diff should always be positive. If diff is a large negative number, it is likely - * that wrapping has occurred. A small negative value however, may point to the application - * sending data too late, and we need to drop data to get back in sync with the controller. + * The diff should always be positive. If diff is a large negative number, it + * is likely that wrapping has occurred. A small negative value however, may + * point to the application sending data too late, and we need to drop data to + * get back in sync with the controller. */ if (diff < -((int64_t)UINT32_MAX / 2)) { LOG_DBG("Timestamp wrap. diff: %lld", diff); @@ -1303,18 +1181,24 @@ static void audio_datapath_just_in_time_check_and_adjust(uint32_t tx_sync_ts_us, } if (print_count % 100 == 0) { - LOG_DBG("JIT diff: %lld us. Target: %u +/- %u", diff, JUST_IN_TIME_TARGET_DLY_US, - JUST_IN_TIME_BOUND_US); + LOG_DBG("JIT diff: %lld us. Target: %d", diff, + CONFIG_NRF_AUDIO_TX_LEAD_TIME_TGT_US); } + print_count++; - if ((diff < (JUST_IN_TIME_TARGET_DLY_US - JUST_IN_TIME_BOUND_US)) || - (diff > (JUST_IN_TIME_TARGET_DLY_US + JUST_IN_TIME_BOUND_US))) { - ret = audio_system_fifo_rx_block_drop(); - if (ret) { - LOG_WRN("Not able to drop FIFO RX block"); - return; - } + /* If the data is sent too late/too slow, we don't copy in data. Instead, + * blocks are dropped, which in turn will cause the controller to starve and + * send a NULL PDU on air "gaining" an ISO interval of time. This means + * we are again too fast, and drop blocks to come back to sync. + * A factor is chosen * BLK_PERIOD_US to keep latency low, + * whilst allowing for some jitter in the system. + */ + + if (!IN_RANGE(diff, CONFIG_NRF_AUDIO_TX_LEAD_TIME_MIN_US, + CONFIG_NRF_AUDIO_TX_LEAD_TIME_MIN_US + (BLK_PERIOD_US * 6))) { + /* Drop next block to help with just-in-time */ + atomic_set(&drop_next_block, true); LOG_DBG("Dropped block to align with connection interval"); print_count = 0; } @@ -1323,11 +1207,11 @@ static void audio_datapath_just_in_time_check_and_adjust(uint32_t tx_sync_ts_us, /** * @brief Update sdu_ref_us so that drift compensation can work correctly. * - * @note This function is only valid for gateway using I2S as audio source - * and unidirectional audio stream (gateway to one or more headsets). + * @note This function is only valid for gateway using I2S as audio + * source and unidirectional audio stream (gateway to one or more headsets). * - * @param sdu_ref_us ISO timestamp reference from Bluetooth LE controller. - * @param adjust Indicate if the sdu_ref should be used to adjust timing. + * @param sdu_ref_us ISO timestamp reference from Bluetooth LE controller. + * @param adjust Indicate if the sdu_ref should be used to adjust timing. */ static void audio_datapath_sdu_ref_update(const struct zbus_channel *chan) { @@ -1345,7 +1229,7 @@ static void audio_datapath_sdu_ref_update(const struct zbus_channel *chan) if (ctrl_blk.stream_started) { ctrl_blk.prev_drift_sdu_ref_us = tx_sync_ts_us; - if (adjust && tx_sync_ts_us != 0) { + if (adjust) { audio_datapath_just_in_time_check_and_adjust(tx_sync_ts_us, curr_ts_us); } @@ -1377,83 +1261,144 @@ void audio_datapath_pres_delay_us_get(uint32_t *delay_us) *delay_us = ctrl_blk.pres_comp.pres_delay_us; } -void audio_datapath_stream_out(const uint8_t *buf, size_t size, uint32_t sdu_ref_us, bool bad_frame, - uint32_t recv_frame_ts_us) +void audio_datapath_stream_out(struct net_buf *audio_frame_in) { + bool sdu_ref_not_consecutive = false; + if (!ctrl_blk.stream_started) { LOG_WRN("Stream not started"); return; } - /*** Check incoming data ***/ - - if (!buf) { - LOG_ERR("Buffer pointer is NULL"); + if (audio_frame_in == NULL) { + LOG_ERR("Audio frame is NULL"); + return; } - if (sdu_ref_us == ctrl_blk.prev_pres_sdu_ref_us && sdu_ref_us != 0) { - LOG_WRN("Duplicate sdu_ref_us (%d) - Dropping audio frame", sdu_ref_us); + /*** Check incoming data ***/ + struct audio_metadata *meta_in = net_buf_user_data(audio_frame_in); + + if (meta_in->ref_ts_us == ctrl_blk.prev_pres_sdu_ref_us && meta_in->ref_ts_us != 0) { + LOG_WRN("Duplicate sdu_ref_us (%d) - Dropping audio frame", meta_in->ref_ts_us); return; } - bool sdu_ref_not_consecutive = false; + uint32_t sdu_ref_delta_us = meta_in->ref_ts_us - ctrl_blk.prev_pres_sdu_ref_us; - if (ctrl_blk.prev_pres_sdu_ref_us) { - uint32_t sdu_ref_delta_us = sdu_ref_us - ctrl_blk.prev_pres_sdu_ref_us; - - /* Check if the delta is from two consecutive frames */ - if (sdu_ref_delta_us < - (CONFIG_AUDIO_FRAME_DURATION_US + (CONFIG_AUDIO_FRAME_DURATION_US / 2))) { - /* Check for invalid delta */ - if ((sdu_ref_delta_us > - (CONFIG_AUDIO_FRAME_DURATION_US + SDU_REF_DELTA_MAX_ERR_US)) || - (sdu_ref_delta_us < - (CONFIG_AUDIO_FRAME_DURATION_US - SDU_REF_DELTA_MAX_ERR_US))) { - LOG_DBG("Invalid sdu_ref_us delta (%d) - Estimating sdu_ref_us", - sdu_ref_delta_us); - - /* Estimate sdu_ref_us */ - sdu_ref_us = ctrl_blk.prev_pres_sdu_ref_us + - CONFIG_AUDIO_FRAME_DURATION_US; - } + if (meta_in->ref_ts_us == 0 && ctrl_blk.prev_pres_sdu_ref_us == 0) { + /* Timestamp not received yet */ + ctrl_blk.prev_pres_sdu_ref_us = meta_in->ref_ts_us; + sdu_ref_not_consecutive = true; + + } else if (sdu_ref_delta_us > CONSECUTIVE_TS_LIMIT_US) { + /* If the new timestamp is not consecutive wrt. the previous timestamp */ + if (consec_invalid_ts_deltas) { + LOG_ERR("sdu_ref_us not from consecutive frames (delta: %d us)", + sdu_ref_delta_us); } else { - LOG_INF("sdu_ref_us not from consecutive frames (diff: %d us)", + LOG_DBG("sdu_ref_us not from consecutive frames (delta: %d us)", sdu_ref_delta_us); - sdu_ref_not_consecutive = true; } - } - ctrl_blk.prev_pres_sdu_ref_us = sdu_ref_us; + sdu_ref_not_consecutive = true; + ctrl_blk.prev_pres_sdu_ref_us = meta_in->ref_ts_us; + consec_invalid_ts_deltas++; + + } else if (!IN_RANGE(sdu_ref_delta_us, + CONFIG_AUDIO_FRAME_DURATION_US - SDU_REF_CH_DELTA_MAX_US, + CONFIG_AUDIO_FRAME_DURATION_US + SDU_REF_CH_DELTA_MAX_US)) { + /* If timestamp is consecutive but has invalid delta: Estimate the timestamp + */ + if (consec_invalid_ts_deltas) { + LOG_ERR("Invalid sdu_ref_us delta (%d) meta_in->ref_ts_us %d us. " + "Estimating.", + sdu_ref_delta_us, meta_in->ref_ts_us); + } else { + LOG_DBG("Invalid sdu_ref_us delta (%d) meta_in->ref_ts_us %d us. " + "Estimating.", + sdu_ref_delta_us, meta_in->ref_ts_us); + } + + /* Estimate ref_ts_us. + * If the SDU ref was estimated, we don't update the previous reference with + * the estimated value. This is to avoid an infinite loop of estimations. + */ + uint32_t ref_ts_temp_us = meta_in->ref_ts_us; + + meta_in->ref_ts_us = ctrl_blk.prev_pres_sdu_ref_us + CONFIG_AUDIO_FRAME_DURATION_US; + ctrl_blk.prev_pres_sdu_ref_us = ref_ts_temp_us; + + consec_invalid_ts_deltas++; + } else { + /* The new timestamp is valid. It is consecutive and + * within the expected range compared to the last timestamp. + */ + ctrl_blk.prev_pres_sdu_ref_us = meta_in->ref_ts_us; + consec_invalid_ts_deltas = 0; + } /*** Presentation compensation ***/ if (ctrl_blk.pres_comp.enabled) { - audio_datapath_presentation_compensation(recv_frame_ts_us, sdu_ref_us, + audio_datapath_presentation_compensation(meta_in->data_rx_ts_us, meta_in->ref_ts_us, sdu_ref_not_consecutive); } /*** Decode ***/ int ret; - size_t pcm_size; + struct net_buf *audio_frame_out = net_buf_alloc(&audio_pcm_pool, K_NO_WAIT); - ret = sw_codec_decode(buf, size, bad_frame, &ctrl_blk.decoded_data, &pcm_size); + if (audio_frame_out == NULL) { + LOG_ERR("Out of I2S PCM TX buffers."); + return; + } + + /* Output I2S related metadata */ + struct audio_metadata *meta_out = net_buf_user_data(audio_frame_out); + *meta_out = i2s_meta; + meta_out->data_len_us = meta_in->data_len_us; + meta_out->ref_ts_us = meta_in->ref_ts_us; + meta_out->data_rx_ts_us = meta_in->data_rx_ts_us; + meta_out->bad_data = meta_in->bad_data; + + ret = sw_codec_decode(audio_frame_in, audio_frame_out); if (ret) { + net_buf_unref(audio_frame_out); LOG_WRN("SW codec decode error: %d", ret); + return; } if (IS_ENABLED(CONFIG_SD_CARD_PLAYBACK)) { if (sd_card_playback_is_active()) { - sd_card_playback_mix_with_stream(ctrl_blk.decoded_data, pcm_size); + sd_card_playback_mix_with_stream((void *const)audio_frame_out->data, + audio_frame_out->len); } } - if (pcm_size != (BLK_STEREO_SIZE_OCTETS * NUM_BLKS_IN_FRAME)) { - LOG_WRN("Decoded audio has wrong size: %d. Expected: %d", pcm_size, - (BLK_STEREO_SIZE_OCTETS * NUM_BLKS_IN_FRAME)); + if (audio_frame_out->len != PCM_NUM_BYTES_MONO * CONFIG_AUDIO_OUTPUT_CHANNELS) { + LOG_WRN("Decoded audio has wrong size: %d. Expected: %d", audio_frame_out->len, + PCM_NUM_BYTES_MONO * CONFIG_AUDIO_OUTPUT_CHANNELS); /* Discard frame */ + net_buf_unref(audio_frame_out); return; } + /* Both OpenEarable speakers are wired to I2S slot 0. Bluetooth's right + * location describes which music channel this earphone receives, not the + * physical slot used by its mono DAC. The SDK decoder preserves locations. + */ + if (IS_ENABLED(CONFIG_BOARD_OPENEARABLE_V2_NRF5340_CPUAPP) && + meta_in->locations == BT_AUDIO_LOCATION_FRONT_RIGHT) { + for (size_t i = 0; i < audio_frame_out->len; + i += 2U * CONFIG_AUDIO_BIT_DEPTH_OCTETS) { + memcpy(&audio_frame_out->data[i], + &audio_frame_out->data[i + CONFIG_AUDIO_BIT_DEPTH_OCTETS], + CONFIG_AUDIO_BIT_DEPTH_OCTETS); + memset(&audio_frame_out->data[i + CONFIG_AUDIO_BIT_DEPTH_OCTETS], 0, + CONFIG_AUDIO_BIT_DEPTH_OCTETS); + } + } + /*** Add audio data to FIFO buffer ***/ uint32_t num_blks_in_fifo = filled_blocks_get(); @@ -1461,6 +1406,7 @@ void audio_datapath_stream_out(const uint8_t *buf, size_t size, uint32_t sdu_ref LOG_WRN("Output audio stream overrun - Discarding audio frame"); /* Discard frame to allow consumer to catch up */ + net_buf_unref(audio_frame_out); return; } @@ -1468,39 +1414,32 @@ void audio_datapath_stream_out(const uint8_t *buf, size_t size, uint32_t sdu_ref for (uint32_t i = 0; i < NUM_BLKS_IN_FRAME; i++) { if (IS_ENABLED(CONFIG_AUDIO_BIT_DEPTH_16)) { - memcpy(&ctrl_blk.out.fifo[out_blk_idx * BLK_STEREO_NUM_SAMPS], - &((int16_t *)ctrl_blk.decoded_data)[i * BLK_STEREO_NUM_SAMPS], - BLK_STEREO_SIZE_OCTETS); + memcpy(&ctrl_blk.out.fifo[out_blk_idx * BLK_MULTI_CHAN_NUM_SAMPS], + (int16_t *)audio_frame_out->data, BLK_MULTI_CHAN_SIZE_OCTETS); } else if (IS_ENABLED(CONFIG_AUDIO_BIT_DEPTH_32)) { - memcpy(&ctrl_blk.out.fifo[out_blk_idx * BLK_STEREO_NUM_SAMPS], - &((int32_t *)ctrl_blk.decoded_data)[i * BLK_STEREO_NUM_SAMPS], - BLK_STEREO_SIZE_OCTETS); + memcpy(&ctrl_blk.out.fifo[out_blk_idx * BLK_MULTI_CHAN_NUM_SAMPS], + (int32_t *)audio_frame_out->data, BLK_MULTI_CHAN_SIZE_OCTETS); } - //LOG_INF("out_blk_idx: %i", out_blk_idx); - - //uint32_t start = k_cyc_to_us_floor32(k_cycle_get_32()); - -#if CONFIG_EQAULIZER_SOFTWARE - equalize(&ctrl_blk.out.fifo[out_blk_idx * BLK_STEREO_NUM_SAMPS], BLK_STEREO_NUM_SAMPS); -#endif - - /*uint32_t end = k_cyc_to_us_floor32(k_cycle_get_32()); - - LOG_INF("time: %i", end - start);*/ + /* Remove consumed data from net buffer */ + net_buf_pull(audio_frame_out, BLK_MULTI_CHAN_SIZE_OCTETS); /* Record producer block start reference */ - ctrl_blk.out.prod_blk_ts[out_blk_idx] = recv_frame_ts_us + (i * BLK_PERIOD_US); + ctrl_blk.out.prod_blk_ts[out_blk_idx] = + meta_in->data_rx_ts_us + (i * BLK_PERIOD_US); + ctrl_blk.out.prod_blk_valid[out_blk_idx] = !meta_in->bad_data; out_blk_idx = NEXT_IDX(out_blk_idx); } ctrl_blk.out.prod_blk_idx = out_blk_idx; + + net_buf_unref(audio_frame_out); } -int audio_datapath_start(struct data_fifo *fifo_rx) +int audio_datapath_start(struct k_msgq *audio_q_rx) { - __ASSERT_NO_MSG(fifo_rx != NULL); + __ASSERT_NO_MSG(audio_q_rx != NULL); if (!ctrl_blk.datapath_initialized) { LOG_WRN("Audio datapath not initialized"); @@ -1508,73 +1447,195 @@ int audio_datapath_start(struct data_fifo *fifo_rx) } if (!ctrl_blk.stream_started) { - ctrl_blk.in.fifo = fifo_rx; + ctrl_blk.in.audio_q = audio_q_rx; /* Clear counters and mute initial audio */ memset(&ctrl_blk.out, 0, sizeof(ctrl_blk.out)); + ctrl_blk.startup = (struct audio_startup){0}; audio_datapath_i2s_start(); ctrl_blk.stream_started = true; - start_data_thread(); - return 0; } else { return -EALREADY; } } +void audio_datapath_encoder_reset(void) +{ + unsigned int key = irq_lock(); + + if (i2s_current_frame != NULL) { + net_buf_unref(i2s_current_frame); + i2s_current_frame = NULL; + } + i2s_blocks_in_current_frame = 0; + atomic_clear(&drop_next_block); + irq_unlock(key); +} + int audio_datapath_stop(void) { if (ctrl_blk.stream_started) { ctrl_blk.stream_started = false; audio_datapath_i2s_stop(); + audio_datapath_encoder_reset(); ctrl_blk.prev_pres_sdu_ref_us = 0; ctrl_blk.prev_drift_sdu_ref_us = 0; + consec_invalid_ts_deltas = 0; pres_comp_state_set(PRES_STATE_INIT); - data_fifo_empty(ctrl_blk.in.fifo); - - /* Cleanup CascadedDecimator on stop */ - audio_datapath_decimator_cleanup(); - return 0; } else { return -EALREADY; } } -// TODO: not clean with the argument --> move to init? -int audio_datapath_aquire(struct data_fifo *fifo_rx) { - int ret = 0; - if (_count == 0) { - uint32_t alloced_cnt; - uint32_t locked_cnt; - - ret = data_fifo_num_used_get(fifo_rx, &alloced_cnt, &locked_cnt); - if (alloced_cnt || locked_cnt || ret) { - LOG_WRN("FIFO is not empty, alloced: %d, locked: %d, ret: %d", - alloced_cnt, locked_cnt, ret); - data_fifo_empty(fifo_rx); - } - ret = audio_datapath_start(fifo_rx); +void set_sensor_queue(struct k_msgq *queue) +{ + ARG_UNUSED(queue); +} + +void record_to_sd(bool active) +{ + record_sd = active; +} + +void record_to_buffer(int16_t *buffer, int num_samples, int initial_drop, bool left, bool right, + void (*callback)(void)) +{ + if (buffer == NULL || num_samples <= 0 || (!left && !right)) { + LOG_ERR("Invalid buffer recording parameters"); + return; } - _count++; - return ret; + record_buffer_data = buffer; + record_num_samples = num_samples; + record_current_index = -initial_drop; + record_left = left; + record_right = right; + record_callback = callback; + record_buffer = true; +} + +void record_to_buffer_stop(void) +{ + record_buffer = false; + record_buffer_data = NULL; + record_callback = NULL; +} + +void audio_datapath_stop_recording(void) +{ + record_to_buffer_stop(); + record_sd = false; } -int audio_datapath_release(void) { +int audio_datapath_auxiliary_suspend(void) +{ + if (auxiliary_audio_state.suspended) { + return -EBUSY; + } + + auxiliary_audio_state = (struct auxiliary_audio_state) { + .suspended = true, + .record_sd = record_sd, + .record_buffer = record_buffer, + .record_buffer_data = record_buffer_data, + .record_num_samples = record_num_samples, + .record_current_index = record_current_index, + .record_left = record_left, + .record_right = record_right, + .record_callback = record_callback, + .tone_active = tone_active, + .tone_remaining_ms = k_timer_remaining_get(&tone_stop_timer), + .buffer_data = buffer_play_data, + .buffer_pos = buffer_play_pos, + .buffer_fade_pos = buffer_play_fade_pos, + .buffer_num_samples = buffer_play_num_samples, + .buffer_amplitude = buffer_play_amplitude, + .buffer_loop = buffer_play_loop, + .buffer_callback = buffer_play_callback, + }; + + k_timer_stop(&tone_stop_timer); + (void)k_work_cancel(&tone_stop_work); + record_sd = false; + record_buffer = false; + record_buffer_data = NULL; + record_callback = NULL; + tone_active = false; + buffer_play_data = NULL; + buffer_play_callback = NULL; + return 0; +} + +int audio_datapath_auxiliary_resume(void) +{ + if (!auxiliary_audio_state.suspended) { + return -EALREADY; + } + + (void)k_work_cancel(&tone_stop_work); + record_sd = auxiliary_audio_state.record_sd; + record_buffer = auxiliary_audio_state.record_buffer; + record_buffer_data = auxiliary_audio_state.record_buffer_data; + record_num_samples = auxiliary_audio_state.record_num_samples; + record_current_index = auxiliary_audio_state.record_current_index; + record_left = auxiliary_audio_state.record_left; + record_right = auxiliary_audio_state.record_right; + record_callback = auxiliary_audio_state.record_callback; + tone_active = auxiliary_audio_state.tone_active; + buffer_play_data = auxiliary_audio_state.buffer_data; + buffer_play_pos = auxiliary_audio_state.buffer_pos; + buffer_play_fade_pos = auxiliary_audio_state.buffer_fade_pos; + buffer_play_num_samples = auxiliary_audio_state.buffer_num_samples; + buffer_play_amplitude = auxiliary_audio_state.buffer_amplitude; + buffer_play_loop = auxiliary_audio_state.buffer_loop; + buffer_play_callback = auxiliary_audio_state.buffer_callback; + + if (tone_active && auxiliary_audio_state.tone_remaining_ms > 0U) { + k_timer_start(&tone_stop_timer, K_MSEC(auxiliary_audio_state.tone_remaining_ms), + K_NO_WAIT); + } + memset(&auxiliary_audio_state, 0, sizeof(auxiliary_audio_state)); + return 0; +} + +int audio_datapath_aquire(struct k_msgq *queue_rx) +{ int ret = 0; - _count --; + k_mutex_lock(&datapath_owner_mutex, K_FOREVER); + if (datapath_acquire_count == 0) { + ret = audio_datapath_start(queue_rx != NULL ? queue_rx : &auxiliary_audio_q); + } else if (queue_rx != NULL) { + /* Microphone capture may already own I2S when the LE stream starts. */ + unsigned int key = irq_lock(); - if (_count <= 0) { - audio_datapath_stop(); - _count = 0; + ctrl_blk.in.audio_q = queue_rx; + irq_unlock(key); + } + if (ret == 0) { + datapath_acquire_count++; } + k_mutex_unlock(&datapath_owner_mutex); + return ret; +} + +int audio_datapath_release(void) +{ + int ret = 0; + k_mutex_lock(&datapath_owner_mutex, K_FOREVER); + if (datapath_acquire_count == 0) { + ret = -EALREADY; + } else if (--datapath_acquire_count == 0) { + ret = audio_datapath_stop(); + } + k_mutex_unlock(&datapath_owner_mutex); return ret; } @@ -1583,19 +1644,15 @@ int audio_datapath_init(void) memset(&ctrl_blk, 0, sizeof(ctrl_blk)); audio_i2s_blk_comp_cb_register(audio_datapath_i2s_blk_complete); audio_i2s_init(); - /*if (IS_ENABLED(CONFIG_AUDIO_MIC_PDM)) { - pdm_mic_init(); - }*/ ctrl_blk.datapath_initialized = true; + audio_tap_thread_start(); ctrl_blk.drift_comp.enabled = true; ctrl_blk.pres_comp.enabled = true; - _count = 0; - if (IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) && (CONFIG_AUDIO_DEV == GATEWAY)) { - /* Disable presentation compensation feature for microphone return on gateway, - * since there's only one stream output from gateway for now, so no need to - * qhave presentation compensation. + /* Disable presentation compensation feature for microphone return on + * gateway, since there's only one stream output from gateway for now, so no + * need to qhave presentation compensation. */ ctrl_blk.pres_comp.enabled = false; } else { @@ -1615,9 +1672,8 @@ static int cmd_i2s_tone_play(const struct shell *shell, size_t argc, const char float amplitude; if (argc != 4) { - shell_error( - shell, - "3 arguments (freq [Hz], dur [ms], and amplitude [0-1.0] must be provided"); + shell_error(shell, "3 arguments (freq [Hz], dur [ms], and amplitude " + "[0-1.0] must be provided"); return -EINVAL; } diff --git a/src/audio/audio_datapath.h b/src/audio/audio_datapath.h index 211f7be1..9fe9d71d 100644 --- a/src/audio/audio_datapath.h +++ b/src/audio/audio_datapath.h @@ -4,196 +4,130 @@ * SPDX-License-Identifier: LicenseRef-PCFT */ +/** @file + * @defgroup audio_app_datapath Audio Datapath + * @{ + * @brief Audio datapath and synchronization API for Audio applications. + * + * This module implements the audio synchronization functionality required for + * True Wireless Stereo (TWS) operation. + */ + #ifndef _AUDIO_DATAPATH_H_ #define _AUDIO_DATAPATH_H_ #include #include #include -#include -#include +#include #include "sw_codec_select.h" +#include "audio_defines.h" + +#define SDU_REF_CH_DELTA_MAX_US (int)(CONFIG_AUDIO_FRAME_DURATION_US * 0.001) #ifdef __cplusplus extern "C" { #endif /** - * @brief Mixes a tone into the I2S TX stream + * @brief Mixes a tone into the I2S TX stream. * - * @param freq Tone frequency [Hz] - * @param dur_ms Tone duration [ms]. 0 = forever - * @param amplitude Tone amplitude [0, 1] + * @param freq Tone frequency [Hz]. + * @param dur_ms Tone duration [ms]. (0 == forever) + * @param amplitude Tone amplitude [0, 1]. * - * @return 0 if successful, error otherwise + * @return 0 if successful, error otherwise. */ int audio_datapath_tone_play(uint16_t freq, uint16_t dur_ms, float amplitude); /** - * @brief Stops tone playback + * @brief Stop tone playback. */ void audio_datapath_tone_stop(void); - - - - -/** - * @brief Plays a buffer over I2S TX stream - * - * @param buffer Pointer to audio buffer to play - * @param num_samples Number of samples in buffer - * @param loop Whether to loop the buffer continuously - * @param amplitude Playback amplitude [0, 1] - * @param callback Callback function called when playback completes (optional) - * - * @return 0 if successful, error otherwise - */ -int audio_datapath_buffer_play(int16_t *buffer, int num_samples, bool loop, float amplitude, void (*callback)(void)); - -/** - * @brief Stops buffer playback - */ +int audio_datapath_buffer_play(int16_t *buffer, int num_samples, bool loop, float amplitude, + void (*callback)(void)); void audio_datapath_buffer_stop(void); - -/** - * @brief Records audio data to a buffer - * - * @param buffer Pointer to buffer where audio will be stored - * @param num_samples Number of samples to record - * @param initial_drop Number of initial samples to drop before recording - * @param left Whether to record left channel - * @param right Whether to record right channel - * @param callback Callback function called when recording completes (optional) - */ -void record_to_buffer(int16_t *buffer, int num_samples, int initial_drop, bool left, bool right, void (*callback)(void)); - -/** - * @brief Stops buffer recording safely - */ +void record_to_buffer(int16_t *buffer, int num_samples, int initial_drop, bool left, bool right, + void (*callback)(void)); void record_to_buffer_stop(void); - -/** - * @brief Stops all audio recording safely (buffer and SD) - */ +void record_to_sd(bool active); void audio_datapath_stop_recording(void); - -/** - * @brief Suspend local playback and recording without losing their state. - * - * The datapath must be stopped before this function is called. A matching call - * to audio_datapath_auxiliary_resume() restores playback positions, callbacks, - * and recording configuration. - * - * @return 0 on success, or -EBUSY if an auxiliary suspension is already active. - */ int audio_datapath_auxiliary_suspend(void); - -/** - * @brief Restore local playback and recording after an auxiliary suspension. - * - * @return 0 on success, or -EALREADY if no suspension is active. - */ int audio_datapath_auxiliary_resume(void); +void set_sensor_queue(struct k_msgq *queue); +/* Share I2S with microphone capture and local audio; NULL means no encoder input. */ +int audio_datapath_aquire(struct k_msgq *queue_rx); +/* Call after disabling the encoder producer, before starting another session. */ +void audio_datapath_encoder_reset(void); +int audio_datapath_release(void); + +int audio_datapath_decimator_init(uint8_t factor); +int audio_datapath_decimator_process(const int16_t *input, int16_t *output, + uint32_t num_frames); +void audio_datapath_decimator_cleanup(void); +void audio_datapath_decimator_reset(void); +uint8_t audio_datapath_decimator_get_factor(void); /** - * @brief Set the presentation delay + * @brief Set the presentation delay. * - * @param delay_us The presentation delay in µs + * @param delay_us The presentation delay in µs. * - * @return 0 if successful, error otherwise + * @return 0 if successful, error otherwise. */ int audio_datapath_pres_delay_us_set(uint32_t delay_us); /** - * @brief Get the current presentation delay + * @brief Get the current presentation delay. * - * @param delay_us The presentation delay in µs + * @param delay_us The presentation delay in µs. */ void audio_datapath_pres_delay_us_get(uint32_t *delay_us); /** - * @brief Input an audio data frame which is processed and outputted over I2S + * @brief Input an audio data frame which is processed and outputted over I2S. * - * @note A frame of raw encoded audio data is inputted, and this data then is decoded - * and processed before being outputted over I2S. The audio is synchronized - * using sdu_ref_us + * @note A frame of raw encoded audio data is inputted, and this data then is decoded + * and processed before being outputted over I2S. The audio is synchronized + * using sdu_ref_us. * - * @param buf Pointer to audio data frame - * @param size Size of audio data frame in bytes - * @param sdu_ref_us ISO timestamp reference from BLE controller - * @param bad_frame Indicating if the audio frame is bad or not - * @param recv_frame_ts_us Timestamp of when audio frame was received + * @param audio_frame_in Pointer to the coded audio input buffer. */ -void audio_datapath_stream_out(const uint8_t *buf, size_t size, uint32_t sdu_ref_us, bool bad_frame, - uint32_t recv_frame_ts_us); +void audio_datapath_stream_out(struct net_buf *audio_frame_in); /** - * @brief Start the audio datapath module + * @brief Start the audio datapath module. * - * @note The continuously running I2S is started + * @note The continuously running I2S is started. * - * @param fifo_rx Pointer to FIFO structure where I2S RX data is put + * @param queue_rx Pointer to the queue structure where I2S RX data is put. * - * @return 0 if successful, error otherwise + * @return 0 if successful, error otherwise. */ -int audio_datapath_start(struct data_fifo *fifo_rx); +int audio_datapath_start(struct k_msgq *queue_rx); /** - * @brief Stop the audio datapath module + * @brief Stop the audio datapath module. * - * @return 0 if successful, error otherwise + * @return 0 if successful, error otherwise. */ int audio_datapath_stop(void); /** - * @brief Initialize the audio datapath module + * @brief Initialize the audio datapath module. * - * @return 0 if successful, error otherwise + * @return 0 if successful, error otherwise. */ int audio_datapath_init(void); -void start_data_thread(void); - -void record_to_sd(bool active); - -void set_sensor_queue(struct k_msgq *queue); - -int audio_datapath_aquire(struct data_fifo *fifo_rx); -int audio_datapath_release(void); - -//void set_ring_buffer(struct ring_buf *ring_buf); - -/** - * @brief C wrapper for decimator initialization - */ -int audio_datapath_decimator_init(uint8_t factor); - -/** - * @brief C wrapper for decimator processing - */ -int audio_datapath_decimator_process(const int16_t* input, int16_t* output, uint32_t num_frames); - -/** - * @brief C wrapper for decimator cleanup - */ -void audio_datapath_decimator_cleanup(void); - -/** - * @brief C wrapper for decimator reset - */ -void audio_datapath_decimator_reset(void); - -/** - * @brief Get current decimation factor - * @return Current total decimation factor, or 0 if not initialized - */ -uint8_t audio_datapath_decimator_get_factor(void); - - #ifdef __cplusplus } #endif +/** + * @} + */ + #endif /* _AUDIO_DATAPATH_H_ */ diff --git a/src/audio/audio_heap.ld b/src/audio/audio_heap.ld index 784e1b7e..4a98c3da 100644 --- a/src/audio/audio_heap.ld +++ b/src/audio/audio_heap.ld @@ -1,7 +1,7 @@ -/* Newlib uses the RAM between _end and the physical SRAM boundary. The seal - * waveform and LC3 sessions allocate from this space at runtime, so they do - * not appear in the linker's static RAM usage report. +/* Common libc malloc uses the aligned RAM between _end and the SRAM boundary. + * The seal waveform and LC3 sessions allocate from this space at runtime and + * therefore do not appear in the linker's static RAM usage report. */ -ASSERT(_end + CONFIG_NEWLIB_LIBC_MIN_REQUIRED_HEAP_SIZE <= +ASSERT(ALIGN(_end, 8) + CONFIG_AUDIO_MIN_RUNTIME_HEAP_SIZE <= CONFIG_SRAM_BASE_ADDRESS + CONFIG_SRAM_SIZE * 1024, - "Not enough Newlib heap for audio; reduce static RAM usage") + "Not enough libc heap for audio; reduce static RAM usage") diff --git a/src/audio/audio_sdu_timing.h b/src/audio/audio_sdu_timing.h new file mode 100644 index 00000000..5cff1cc3 --- /dev/null +++ b/src/audio/audio_sdu_timing.h @@ -0,0 +1,38 @@ +/* SPDX-License-Identifier: Apache-2.0 */ +#ifndef AUDIO_SDU_TIMING_H_ +#define AUDIO_SDU_TIMING_H_ + +#include +#include + +struct audio_sdu_timing { + uint32_t reference; + uint32_t received_ms; + uint16_t sequence; + bool valid; +}; + +/* A missing ISO timestamp is not zero. Conceal at most ten frames from a + * recent timestamp, without promoting an estimate to a new anchor. Reset + * this state at each stream start/stop. Unsigned differences handle wraps. + */ +static inline bool audio_sdu_timing_resolve(struct audio_sdu_timing *anchor, + bool timestamp_valid, uint16_t sequence, uint32_t reference, + uint32_t now_ms, uint32_t frame_us, uint32_t *resolved) +{ + if (timestamp_valid) { + *anchor = (struct audio_sdu_timing){reference, now_ms, sequence, true}; + *resolved = reference; + return true; + } + + uint16_t frames = (uint16_t)(sequence - anchor->sequence); + if (!anchor->valid || !frame_us || frames == 0 || frames > 10 || + (uint32_t)(now_ms - anchor->received_ms) > 200U) { + return false; + } + *resolved = anchor->reference + frames * frame_us; + return true; +} + +#endif diff --git a/src/audio/audio_startup.h b/src/audio/audio_startup.h new file mode 100644 index 00000000..86c869bc --- /dev/null +++ b/src/audio/audio_startup.h @@ -0,0 +1,48 @@ +/* SPDX-License-Identifier: Apache-2.0 */ +#ifndef AUDIO_STARTUP_H_ +#define AUDIO_STARTUP_H_ + +#include +#include + +struct audio_startup { + uint32_t stable_us; + uint32_t fade_frames; + bool open; +}; + +/* Require 20 ms of consecutive valid, synchronized PCM at startup. Measure + * time rather than calls: the SDK can change the I2S block duration. Once open, + * later loss of lock must not mute music again. + * A stopped Bluetooth stream resets the gate even if recording keeps I2S on. + */ +static inline bool audio_startup_ready(struct audio_startup *startup, + bool streaming, bool stable, uint32_t block_us) +{ + if (!streaming) { + *startup = (struct audio_startup){0}; + return false; + } + if (!startup->open) { + startup->stable_us = stable ? startup->stable_us + block_us : 0U; + startup->open = startup->stable_us >= 20000U; + } + return startup->open; +} + +/* Advance once per stereo sample frame; use the same gain for both channels. */ +static inline uint32_t audio_startup_gain(struct audio_startup *startup, uint32_t fade_frames) +{ + if (startup->fade_frames < fade_frames) { + startup->fade_frames++; + } + return startup->fade_frames; +} + +/* 64-bit multiplication keeps full-scale 16- and 32-bit PCM within range. */ +static inline int32_t audio_startup_scale(int32_t sample, uint32_t gain, uint32_t full_gain) +{ + return (int32_t)(((int64_t)sample * gain) / full_gain); +} + +#endif /* AUDIO_STARTUP_H_ */ diff --git a/src/audio/audio_system.c b/src/audio/audio_system.c index bf399467..26a54459 100644 --- a/src/audio/audio_system.c +++ b/src/audio/audio_system.c @@ -8,8 +8,6 @@ #include #include -#include -#include #include #include #include @@ -17,58 +15,79 @@ #include "macros_common.h" #include "sw_codec_select.h" #include "audio_datapath.h" +#include "audio_clock.h" #include "audio_i2s.h" #include "hw_codec.h" #include "audio_usb.h" #include "streamctrl.h" -#include "openearable_common.h" -//#include "pdm_mic.h" - -#define SENQUEUE_FRAME_SIZE 32 - #include LOG_MODULE_REGISTER(audio_system, CONFIG_AUDIO_SYSTEM_LOG_LEVEL); -#define FIFO_TX_BLOCK_COUNT (CONFIG_FIFO_FRAME_SPLIT_NUM * CONFIG_FIFO_TX_FRAME_COUNT) -#define FIFO_RX_BLOCK_COUNT (CONFIG_FIFO_FRAME_SPLIT_NUM * CONFIG_FIFO_RX_FRAME_COUNT) -/* Twelve 10 ms frames preserve 120 ms of scheduling tolerance while leaving - * room for LC3 and a retained maximum-size audio-response waveform. +#define FIFO_OUT_BLOCK_COUNT (USB_1MS_BLOCKS_NUM_MAX * CONFIG_FIFO_TX_FRAME_COUNT) +#define FIFO_IN_BLOCK_COUNT CONFIG_FIFO_RX_FRAME_COUNT + +/* Size these to fit the use case as part of optimization (e.g. increase decoder pool when it is + * wrapped in a thread). */ -#define ENCODER_QUEUE_FRAME_COUNT 12 +#define FIFO_ENC_POOL_BLK_COUNT 2 +#define FIFO_DEC_POOL_BLK_COUNT 2 #define DEBUG_INTERVAL_NUM 1000 #define TEST_TONE_BASE_FREQ_HZ 1000 K_THREAD_STACK_DEFINE(encoder_thread_stack, CONFIG_ENCODER_STACK_SIZE); -DATA_FIFO_DEFINE(fifo_tx, FIFO_TX_BLOCK_COUNT, WB_UP(BLOCK_SIZE_BYTES)); -DATA_FIFO_DEFINE(fifo_rx, FIFO_RX_BLOCK_COUNT, WB_UP(BLOCK_SIZE_BYTES)); -K_MSGQ_DEFINE(encoder_queue, sizeof(struct audio_rx_data), ENCODER_QUEUE_FRAME_COUNT, 4); +K_MSGQ_DEFINE(audio_q_in, sizeof(struct net_buf *), FIFO_IN_BLOCK_COUNT, sizeof(void *)); -static K_SEM_DEFINE(sem_encoder_start, 0, 1); +NET_BUF_POOL_FIXED_DEFINE(audio_q_enc_pool, FIFO_ENC_POOL_BLK_COUNT, ENC_MULTI_CHAN_MAX_FRAME_SIZE, + sizeof(struct audio_metadata), NULL); +#if CONFIG_AUDIO_SOURCE_USB && (CONFIG_AUDIO_DEV == GATEWAY) +K_MSGQ_DEFINE(audio_q_out, sizeof(struct net_buf *), FIFO_OUT_BLOCK_COUNT, sizeof(void *)); +NET_BUF_POOL_FIXED_DEFINE(audio_q_dec_pool, FIFO_DEC_POOL_BLK_COUNT, PCM_NUM_BYTES_MULTI_CHAN, + sizeof(struct audio_metadata), NULL); +NET_BUF_POOL_FIXED_DEFINE(audio_q_out_pool, FIFO_OUT_BLOCK_COUNT, USB_BLOCK_MULTI_CHAN_1MS_SIZE, + sizeof(struct audio_metadata), NULL); +#endif static struct k_thread encoder_thread_data; static k_tid_t encoder_thread_id; -static atomic_t encoder_started; -static bool audio_system_suspended; -static bool audio_system_resume_requested; -static bool encoder_resume_requested; -K_MUTEX_DEFINE(audio_system_state_mutex); -struct k_poll_signal encoder_sig; +static struct k_poll_signal encoder_sig; +static atomic_t encoder_started; +static atomic_t encoder_generation; static struct k_poll_event encoder_evt = K_POLL_EVENT_INITIALIZER(K_POLL_TYPE_SIGNAL, K_POLL_MODE_NOTIFY_ONLY, &encoder_sig); -static enum audio_channel encoder_channel = AUDIO_CH_L; - static struct sw_codec_config sw_codec_cfg; +K_MUTEX_DEFINE(audio_system_state_mutex); +static bool audio_system_suspended; +static bool audio_system_resume_requested; +static bool encoder_resume_requested; +static uint8_t encoder_channel; /* Buffer which can hold max 1 period test tone at 1000 Hz */ static int16_t test_tone_buf[CONFIG_AUDIO_SAMPLE_RATE_HZ / 1000]; static size_t test_tone_size; -static bool sample_rate_valid(uint32_t sample_rate_hz) +#if CONFIG_AUDIO_SOURCE_USB && (CONFIG_AUDIO_DEV == GATEWAY) +static struct net_buf *usb_out_spillover; + +/* The meta data for the decoder and the expected format of the USB. + * An improvement would be to have the USB convert incoming data to the format it requires. + */ +static struct audio_metadata decoder_meta = {.data_coding = PCM, + .sample_rate_hz = 48000, + .bits_per_sample = CONFIG_AUDIO_BIT_DEPTH_BITS, + .carried_bits_per_sample = CONFIG_AUDIO_BIT_DEPTH_BITS, + .bytes_per_location = PCM_NUM_BYTES_MONO, + .interleaved = false, + .locations = BT_AUDIO_LOCATION_MONO_AUDIO, + .bad_data = 0}; + +#endif + +bool sample_rate_valid(uint32_t sample_rate_hz) { if (sample_rate_hz == 16000 || sample_rate_hz == 24000 || sample_rate_hz == 48000) { return true; @@ -85,20 +104,17 @@ static void audio_gateway_configure(void) ERR_CHK_MSG(-EINVAL, "No codec selected"); } -#if (CONFIG_STREAM_BIDIRECTIONAL) - sw_codec_cfg.decoder.audio_ch = AUDIO_CHANNEL_DEFAULT; - sw_codec_cfg.decoder.num_ch = 1; - sw_codec_cfg.decoder.channel_mode = SW_CODEC_MONO; -#endif /* (CONFIG_STREAM_BIDIRECTIONAL) */ + if (IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL)) { + sw_codec_cfg.decoder.audio_loc = BT_AUDIO_LOCATION_MONO_AUDIO; + sw_codec_cfg.decoder.num_ch = 1; + } if (IS_ENABLED(CONFIG_MONO_TO_ALL_RECEIVERS)) { sw_codec_cfg.encoder.num_ch = 1; - } else { - sw_codec_cfg.encoder.num_ch = 2; + sw_codec_cfg.encoder.audio_loc = BT_AUDIO_LOCATION_MONO_AUDIO; } - sw_codec_cfg.encoder.channel_mode = - (sw_codec_cfg.encoder.num_ch == 1) ? SW_CODEC_MONO : SW_CODEC_STEREO; + LOG_INF("Gateway configured for %d encoder channels", sw_codec_cfg.encoder.num_ch); } static void audio_headset_configure(void) @@ -109,14 +125,19 @@ static void audio_headset_configure(void) ERR_CHK_MSG(-EINVAL, "No codec selected"); } -#if (CONFIG_STREAM_BIDIRECTIONAL) - sw_codec_cfg.encoder.num_ch = 1; - sw_codec_cfg.encoder.channel_mode = SW_CODEC_MONO; - sw_codec_cfg.encoder.audio_ch = encoder_channel; -#endif /* (CONFIG_STREAM_BIDIRECTIONAL) */ + if (IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL)) { + sw_codec_cfg.encoder.audio_loc = encoder_channel == 0U + ? BT_AUDIO_LOCATION_FRONT_LEFT + : BT_AUDIO_LOCATION_FRONT_RIGHT; + sw_codec_cfg.encoder.num_ch = 1; + } - sw_codec_cfg.decoder.num_ch = 1; - sw_codec_cfg.decoder.channel_mode = SW_CODEC_MONO; + device_location_get(&sw_codec_cfg.decoder.audio_loc); + if (sw_codec_cfg.decoder.audio_loc == BT_AUDIO_LOCATION_MONO_AUDIO) { + sw_codec_cfg.decoder.num_ch = 1; + } else { + sw_codec_cfg.decoder.num_ch = POPCOUNT(sw_codec_cfg.decoder.audio_loc); + } if (IS_ENABLED(CONFIG_SD_CARD_PLAYBACK)) { /* Need an extra decoder channel to decode data from SD card */ @@ -126,48 +147,57 @@ static void audio_headset_configure(void) static void encoder_thread(void *arg1, void *arg2, void *arg3) { - ARG_UNUSED(arg1); - ARG_UNUSED(arg2); - ARG_UNUSED(arg3); - int ret; - uint32_t blocks_alloced_num; - uint32_t blocks_locked_num; - - int debug_trans_count = 0; - size_t encoded_data_size = 0; - - /* Receive the complete queue item, including its size field. */ - struct audio_rx_data pcm_frame; - - static uint8_t *encoded_data; static uint32_t test_tone_finite_pos; - static bool encode_failed; + int debug_trans_count = 0; while (1) { /* Don't start encoding until the stream needing it has started */ - (void)k_poll(&encoder_evt, 1, K_FOREVER); + encoder_evt.state = K_POLL_STATE_NOT_READY; + ret = k_poll(&encoder_evt, 1, K_FOREVER); + ERR_CHK_MSG(ret, "Encoder poll failed"); - ret = k_msgq_get(&encoder_queue, &pcm_frame, K_FOREVER); + /* Get complete PCM frame from USB */ + struct net_buf *audio_frame_in; + struct net_buf *audio_frame_out = NULL; + atomic_val_t generation = atomic_get(&encoder_generation); + + ret = k_msgq_get(&audio_q_in, (void *)&audio_frame_in, K_FOREVER); if (ret) { - LOG_WRN("Failed to get message from msgq: %d", ret); continue; } - if (pcm_frame.size != sizeof(pcm_frame.data)) { - LOG_WRN("Dropping incomplete encoder frame: %zu bytes", pcm_frame.size); - continue; - } + /* Stop/uninit must not free LC3 state while this frame is being encoded. */ + k_mutex_lock(&audio_system_state_mutex, K_FOREVER); + if (audio_system_encoder_is_started() && + generation == atomic_get(&encoder_generation)) { + audio_frame_out = net_buf_alloc(&audio_q_enc_pool, K_NO_WAIT); - if (!audio_system_encoder_is_started()) { - LOG_DBG("Dropping stale encoder frame while encoder is stopped"); - continue; - } + if (unlikely(audio_frame_out == NULL)) { + LOG_WRN("Out of encoder buffers"); + goto frame_done; + } + + /* Configure the meta data */ + struct audio_metadata *meta_out = net_buf_user_data(audio_frame_out); + struct audio_metadata *meta_in = net_buf_user_data(audio_frame_in); - if (sw_codec_cfg.encoder.enabled) { - if (test_tone_size) { + /* Bulk copy metadata for speed optimization, needs to be changed if + * user_data changes to contain structs + */ + *meta_out = *meta_in; + + meta_out->data_coding = LC3; + meta_out->sample_rate_hz = sw_codec_cfg.encoder.sample_rate_hz; + meta_out->bitrate_bps = sw_codec_cfg.encoder.bitrate; + meta_out->bytes_per_location = + (meta_out->bitrate_bps * meta_out->data_len_us) / 8000000; + meta_out->interleaved = false; + meta_out->locations = sw_codec_cfg.encoder.audio_loc; + + if (unlikely(test_tone_size)) { /* Test tone takes over audio stream */ - uint32_t num_bytes; + uint32_t num_bytes = 0; char tmp[FRAME_SIZE_BYTES / 2]; ret = contin_array_create(tmp, FRAME_SIZE_BYTES / 2, test_tone_buf, @@ -175,77 +205,73 @@ static void encoder_thread(void *arg1, void *arg2, void *arg3) ERR_CHK(ret); ret = pscm_copy_pad(tmp, FRAME_SIZE_BYTES / 2, - CONFIG_AUDIO_BIT_DEPTH_BITS, pcm_frame.data, - &num_bytes); + CONFIG_AUDIO_BIT_DEPTH_BITS, + audio_frame_in->data, &num_bytes); ERR_CHK(ret); - } - ret = sw_codec_encode(pcm_frame.data, pcm_frame.size, &encoded_data, - &encoded_data_size); - if (ret) { - if (!encode_failed) { - LOG_WRN("Audio encode failed; dropping frames until recovery: %d", ret); + if (unlikely(audio_frame_in->len != num_bytes)) { + LOG_ERR("Audio frame and tone length mismatch: %u != %u", + audio_frame_in->len, num_bytes); } - encode_failed = true; - continue; } - if (encode_failed) { - LOG_INF("Audio encoder recovered"); - encode_failed = false; + ret = sw_codec_encode(audio_frame_in, audio_frame_out); + if (ret) { + LOG_WRN_RATELIMIT("Audio encode failed, dropping frame: %d", ret); + goto frame_done; } + streamctrl_send(audio_frame_out); + } else { + LOG_INF_RATELIMIT("Encoder not initialized or enabled, data dropped"); } - /* Print block usage */ - if (debug_trans_count == DEBUG_INTERVAL_NUM) { - ret = data_fifo_num_used_get(&fifo_rx, &blocks_alloced_num, - &blocks_locked_num); - ERR_CHK(ret); - LOG_DBG(COLOR_CYAN "RX alloced: %d, locked: %d" COLOR_RESET, - blocks_alloced_num, blocks_locked_num); - debug_trans_count = 0; - } else { - debug_trans_count++; +frame_done: + if (audio_frame_out != NULL) { + net_buf_unref(audio_frame_out); } + net_buf_unref(audio_frame_in); + k_mutex_unlock(&audio_system_state_mutex); - if (sw_codec_cfg.encoder.enabled) { - streamctrl_send(encoded_data, encoded_data_size, - sw_codec_cfg.encoder.num_ch); + /* Print block usage - reduced overhead */ + if (unlikely(++debug_trans_count >= DEBUG_INTERVAL_NUM)) { + LOG_DBG(COLOR_CYAN "IN filled: %d" COLOR_RESET, + k_msgq_num_used_get(&audio_q_in)); + debug_trans_count = 0; } + STACK_USAGE_PRINT("encoder_thread", &encoder_thread_data); } } -static void audio_system_encoder_start_internal(void) +static void audio_queue_drain(struct k_msgq *queue) { - if (!sw_codec_cfg.initialized || !sw_codec_cfg.encoder.enabled || !sw_codec_is_initialized()) { - LOG_WRN("Encoder start ignored because codec is not initialized"); - atomic_clear(&encoder_started); - k_poll_signal_reset(&encoder_sig); - k_msgq_purge(&encoder_queue); - return; - } + struct net_buf *frame; - LOG_DBG("Encoder started"); - k_msgq_purge(&encoder_queue); - atomic_set(&encoder_started, true); - k_poll_signal_raise(&encoder_sig, 0); - /*if (IS_ENABLED(CONFIG_AUDIO_MIC_PDM)) { - pdm_mic_start(); - }*/ + while (k_msgq_get(queue, &frame, K_NO_WAIT) == 0) { + net_buf_unref(frame); + } + /* Wake a consumer blocked on a stopped session after releasing all buffers. */ + k_msgq_purge(queue); } static void audio_system_encoder_stop_internal(void) { atomic_clear(&encoder_started); + atomic_inc(&encoder_generation); k_poll_signal_reset(&encoder_sig); - k_msgq_purge(&encoder_queue); + audio_datapath_encoder_reset(); + audio_queue_drain(&audio_q_in); } -bool audio_system_encoder_is_started(void) +static void audio_system_encoder_start_internal(void) { - return atomic_get(&encoder_started) && sw_codec_cfg.initialized && - sw_codec_cfg.encoder.enabled && sw_codec_is_initialized(); + audio_system_encoder_stop_internal(); + if (!sw_codec_cfg.initialized || !sw_codec_cfg.encoder.enabled || !sw_codec_is_initialized()) { + LOG_WRN("Encoder start ignored because codec is not initialized"); + return; + } + atomic_set(&encoder_started, true); + k_poll_signal_raise(&encoder_sig, 0); } void audio_system_encoder_start(void) @@ -270,6 +296,11 @@ void audio_system_encoder_stop(void) k_mutex_unlock(&audio_system_state_mutex); } +bool audio_system_encoder_is_started(void) +{ + return atomic_get(&encoder_started); +} + int audio_system_encode_test_tone_set(uint32_t freq) { int ret; @@ -354,16 +385,13 @@ int audio_system_config_set(uint32_t encoder_sample_rate_hz, uint32_t encoder_bi return 0; } +#if CONFIG_AUDIO_SOURCE_USB && (CONFIG_AUDIO_DEV == GATEWAY) /* This function is only used on gateway using USB as audio source and bidirectional stream */ -int audio_system_decode(void const *const encoded_data, size_t encoded_data_size, bool bad_frame) +int audio_system_decode(struct net_buf *audio_frame_in) { int ret; - uint32_t blocks_alloced_num; - uint32_t blocks_locked_num; static int debug_trans_count; - static void *tmp_pcm_raw_data[CONFIG_FIFO_FRAME_SPLIT_NUM]; - static void *pcm_raw_data; - size_t pcm_block_size; + size_t len; if (!sw_codec_cfg.initialized) { /* Throw away data */ @@ -374,62 +402,138 @@ int audio_system_decode(void const *const encoded_data, size_t encoded_data_size return -EPERM; } - ret = data_fifo_num_used_get(&fifo_tx, &blocks_alloced_num, &blocks_locked_num); - if (ret) { - return ret; + if (!audio_usb_headset_in_enabled()) { + /* Ensure spillover is released and reset if the headset in is disabled */ + if (usb_out_spillover != NULL) { + net_buf_unref(usb_out_spillover); + usb_out_spillover = NULL; + } + + LOG_INF_RATELIMIT("Microphone not enabled, dropping data"); + return 0; } - uint8_t free_blocks_num = FIFO_TX_BLOCK_COUNT - blocks_locked_num; + if (audio_frame_in == NULL) { + LOG_ERR("Buffer pointer is NULL"); + return -EINVAL; + } - /* If not enough space for a full frame, remove oldest samples to make room */ - if (free_blocks_num < CONFIG_FIFO_FRAME_SPLIT_NUM) { - void *old_data; - size_t size; - - for (int i = 0; i < (CONFIG_FIFO_FRAME_SPLIT_NUM - free_blocks_num); i++) { - ret = data_fifo_pointer_last_filled_get(&fifo_tx, &old_data, &size, - K_NO_WAIT); - if (ret == -ENOMSG) { - /* If there are no more blocks in FIFO, break */ - break; - } + struct net_buf *audio_frame_out = net_buf_alloc(&audio_q_dec_pool, K_NO_WAIT); - data_fifo_block_free(&fifo_tx, old_data); - } + if (audio_frame_out == NULL) { + LOG_WRN("Out of PCM buffers"); + return -ENOSPC; } - for (int i = 0; i < CONFIG_FIFO_FRAME_SPLIT_NUM; i++) { - ret = data_fifo_pointer_first_vacant_get(&fifo_tx, &tmp_pcm_raw_data[i], K_FOREVER); - if (ret) { - return ret; - } - } + /* Configure the meta data */ + struct audio_metadata *meta_in = net_buf_user_data(audio_frame_in); + struct audio_metadata *meta_out = net_buf_user_data(audio_frame_out); - ret = sw_codec_decode(encoded_data, encoded_data_size, bad_frame, &pcm_raw_data, - &pcm_block_size); + *meta_out = decoder_meta; + meta_out->data_len_us = meta_in->data_len_us; + meta_out->ref_ts_us = meta_in->ref_ts_us; + meta_out->data_rx_ts_us = meta_in->data_rx_ts_us; + meta_out->bad_data = meta_in->bad_data; + + ret = sw_codec_decode(audio_frame_in, audio_frame_out); if (ret) { LOG_ERR("Failed to decode"); + net_buf_unref(audio_frame_out); return ret; } - /* Split decoded frame into CONFIG_FIFO_FRAME_SPLIT_NUM blocks */ - for (int i = 0; i < CONFIG_FIFO_FRAME_SPLIT_NUM; i++) { - memcpy(tmp_pcm_raw_data[i], (char *)pcm_raw_data + (i * (BLOCK_SIZE_BYTES)), - BLOCK_SIZE_BYTES); + /* If not enough space for a full frame, remove oldest samples to make room */ + while (k_msgq_num_free_get(&audio_q_out) < USB_1MS_BLOCKS_NUM_MAX) { + struct net_buf *stale_buf; + + ret = k_msgq_get(&audio_q_out, (void *)&stale_buf, K_NO_WAIT); + if (ret == -ENOMSG) { + /* If there are no more blocks in FIFO, break */ + LOG_DBG("No more stale buffers"); + break; + } + + net_buf_unref(stale_buf); + } + + size_t usb_out_1ms_frame_size = + USB_BLOCK_1MS_MONO_SIZE * audio_metadata_num_loc_get(meta_out); + + if (usb_out_spillover != NULL) { + len = MIN((usb_out_1ms_frame_size - usb_out_spillover->len), audio_frame_out->len); - ret = data_fifo_block_lock(&fifo_tx, &tmp_pcm_raw_data[i], BLOCK_SIZE_BYTES); + /* Add decoded PCM output to spillover buffer */ + net_buf_add_mem(usb_out_spillover, audio_frame_out->data, len); + net_buf_pull_mem(audio_frame_out, len); + + /* If the working spillover buff is not a full 1ms frame then we return and wait for + * the next audio_frame_out + */ + if (usb_out_spillover->len < usb_out_1ms_frame_size) { + /* Release this input buffer as we have consumed it */ + net_buf_unref(audio_frame_out); + return 0; + } + + struct audio_metadata *usb_spill_meta = net_buf_user_data(usb_out_spillover); + + *usb_spill_meta = *meta_out; + usb_spill_meta->bytes_per_location = USB_BLOCK_1MS_MONO_SIZE; + + ret = k_msgq_put(&audio_q_out, (void *)&usb_out_spillover, K_NO_WAIT); if (ret) { - LOG_ERR("Failed to lock block"); + net_buf_unref(audio_frame_out); + net_buf_unref(usb_out_spillover); + usb_out_spillover = NULL; return ret; } } - if (debug_trans_count == DEBUG_INTERVAL_NUM) { - ret = data_fifo_num_used_get(&fifo_tx, &blocks_alloced_num, &blocks_locked_num); + + /* Split decoded frame into 1ms blocks */ + while (audio_frame_out->len >= usb_out_1ms_frame_size) { + struct net_buf *usb_block = net_buf_alloc(&audio_q_out_pool, K_NO_WAIT); + + if (unlikely(usb_block == NULL)) { + LOG_ERR("Out of USB OUT buffers"); + net_buf_unref(audio_frame_out); + return -ENOMEM; + } + + struct audio_metadata *usb_block_meta = net_buf_user_data(usb_block); + + net_buf_add_mem(usb_block, audio_frame_out->data, usb_out_1ms_frame_size); + *usb_block_meta = *meta_out; + usb_block_meta->bytes_per_location = USB_BLOCK_1MS_MONO_SIZE; + net_buf_pull_mem(audio_frame_out, usb_out_1ms_frame_size); + + ret = k_msgq_put(&audio_q_out, (void *)&usb_block, K_NO_WAIT); if (ret) { + net_buf_unref(audio_frame_out); return ret; } - LOG_DBG(COLOR_MAGENTA "TX alloced: %d, locked: %d" COLOR_RESET, blocks_alloced_num, - blocks_locked_num); + } + + if (audio_frame_out->len != 0) { + struct net_buf *usb_out_spillover = net_buf_alloc(&audio_q_out_pool, K_NO_WAIT); + + if (unlikely(usb_out_spillover == NULL)) { + LOG_ERR("Out of USB OUT buffers"); + net_buf_unref(audio_frame_out); + return -ENOMEM; + } + + net_buf_add_mem(usb_out_spillover, audio_frame_out->data, audio_frame_out->len); + } else { + usb_out_spillover = NULL; + } + + net_buf_unref(audio_frame_out); + + if (debug_trans_count == DEBUG_INTERVAL_NUM) { + uint32_t audio_q_num_used = k_msgq_num_used_get(&audio_q_out); + + LOG_DBG(COLOR_MAGENTA "OUT fill grade: %d/%d" COLOR_RESET, audio_q_num_used, + CONFIG_FIFO_TX_FRAME_COUNT); debug_trans_count = 0; } else { debug_trans_count++; @@ -438,10 +542,9 @@ int audio_system_decode(void const *const encoded_data, size_t encoded_data_size return 0; } -/** - * @brief Initialize the FIFOs and codecs, then start the audio datapath. - * - * @return 0 on success, or a negative error from the failed subsystem. +#endif + +/**@brief Initializes the FIFOs, the codec, and starts the I2S */ static int audio_system_start_internal(void) { @@ -456,24 +559,6 @@ static int audio_system_start_internal(void) return -EINVAL; } - if (!fifo_tx.initialized) { - ret = data_fifo_init(&fifo_tx); - if (ret) { - LOG_ERR("Failed to set up tx FIFO: %d", ret); - return ret; - } - } - - if (!fifo_rx.initialized) { - ret = data_fifo_init(&fifo_rx); - if (ret) { - LOG_ERR("Failed to set up rx FIFO: %d", ret); - return ret; - } - } - - LOG_INF("Microphone channel set to %d", sw_codec_cfg.encoder.audio_ch); - ret = sw_codec_init(sw_codec_cfg); if (ret) { LOG_ERR("Failed to set up codec: %d", ret); @@ -487,57 +572,56 @@ static int audio_system_start_internal(void) &encoder_thread_data, encoder_thread_stack, CONFIG_ENCODER_STACK_SIZE, (k_thread_entry_t)encoder_thread, NULL, NULL, NULL, K_PRIO_PREEMPT(CONFIG_ENCODER_THREAD_PRIO), 0, K_NO_WAIT); - ret = k_thread_name_set(encoder_thread_id, "ENCODER"); + ret = k_thread_name_set(encoder_thread_id, "Encoder"); if (ret) { LOG_WRN("Failed to name encoder thread: %d", ret); } } #if ((CONFIG_AUDIO_SOURCE_USB) && (CONFIG_AUDIO_DEV == GATEWAY)) - ret = audio_usb_start(&fifo_tx, &fifo_rx); + ret = audio_usb_start(&audio_q_out, &audio_q_in); if (ret) { - LOG_ERR("Failed to start USB audio: %d", ret); - goto cleanup_sw_codec; + goto cleanup_codec; } #else + if (IS_ENABLED(CONFIG_BOARD_NRF5340_AUDIO_DK_NRF5340_CPUAPP)) { + ret = hw_codec_default_conf_enable(); + ERR_CHK(ret); + } - ret = audio_datapath_aquire(&fifo_rx); + ret = audio_datapath_aquire(&audio_q_in); if (ret) { - LOG_ERR("Failed to acquire audio datapath: %d", ret); - goto cleanup_sw_codec; + goto cleanup_codec; } - - ret = hw_codec_default_conf_enable(); - if (ret) { - int release_ret; - LOG_ERR("Failed to configure hardware codec: %d", ret); - release_ret = audio_datapath_release(); - if (release_ret) { - LOG_ERR("Failed to release audio datapath after startup error: %d", - release_ret); + if (IS_ENABLED(CONFIG_BOARD_OPENEARABLE_V2_NRF5340_CPUAPP)) { + ret = hw_codec_default_conf_enable(); + if (ret) { + (void)audio_datapath_release(); + goto cleanup_codec; } - goto cleanup_sw_codec; } - - /*if (IS_ENABLED(CONFIG_AUDIO_MIC_PDM)) { - ret = pdm_datapath_start(&fifo_rx); - ERR_CHK(ret); - }*/ - #endif /* ((CONFIG_AUDIO_SOURCE_USB) && (CONFIG_AUDIO_DEV == GATEWAY))) */ return 0; -cleanup_sw_codec: - { - int uninit_ret = sw_codec_uninit(sw_codec_cfg); +cleanup_codec: + (void)sw_codec_uninit(sw_codec_cfg); + sw_codec_cfg.initialized = false; + return ret; +} - sw_codec_cfg.initialized = false; - if (uninit_ret) { - LOG_ERR("Failed to uninitialize codec after startup error: %d", uninit_ret); - } +int audio_system_start(void) +{ + int ret = 0; + + k_mutex_lock(&audio_system_state_mutex, K_FOREVER); + if (audio_system_suspended) { + audio_system_resume_requested = true; + } else if (!sw_codec_cfg.initialized) { + ret = audio_system_start_internal(); } + k_mutex_unlock(&audio_system_state_mutex); return ret; } @@ -545,49 +629,33 @@ static void audio_system_stop_internal(void) { int ret; + audio_system_encoder_stop_internal(); + if (!sw_codec_cfg.initialized) { LOG_WRN("Codec already unitialized"); return; } LOG_DBG("Stopping codec"); - audio_system_encoder_stop_internal(); #if ((CONFIG_AUDIO_DEV == GATEWAY) && CONFIG_AUDIO_SOURCE_USB) audio_usb_stop(); #else - //ret = hw_codec_soft_reset(); - ret = hw_codec_stop_audio(); - ERR_CHK(ret); + if (IS_ENABLED(CONFIG_BOARD_NRF5340_AUDIO_DK_NRF5340_CPUAPP)) { + ret = hw_codec_soft_reset(); + ERR_CHK(ret); + } else if (IS_ENABLED(CONFIG_BOARD_OPENEARABLE_V2_NRF5340_CPUAPP)) { + ret = hw_codec_stop_audio(); + ERR_CHK(ret); + } ret = audio_datapath_release(); ERR_CHK(ret); - - /*if (IS_ENABLED(CONFIG_AUDIO_MIC_PDM)) { - pdm_mic_stop(); - }*/ #endif /* ((CONFIG_AUDIO_DEV == GATEWAY) && CONFIG_AUDIO_SOURCE_USB) */ ret = sw_codec_uninit(sw_codec_cfg); ERR_CHK_MSG(ret, "Failed to uninit codec"); sw_codec_cfg.initialized = false; - - //data_fifo_empty(&fifo_rx); - data_fifo_empty(&fifo_tx); -} - -int audio_system_start(void) -{ - int ret = 0; - - k_mutex_lock(&audio_system_state_mutex, K_FOREVER); - if (audio_system_suspended) { - audio_system_resume_requested = true; - } else { - ret = audio_system_start_internal(); - } - k_mutex_unlock(&audio_system_state_mutex); - return ret; } void audio_system_stop(void) @@ -616,8 +684,6 @@ int audio_system_suspend(void) if (audio_system_resume_requested) { audio_system_stop_internal(); } - LOG_INF("Audio system suspended: resume=%d encoder=%d", audio_system_resume_requested, - encoder_resume_requested); k_mutex_unlock(&audio_system_state_mutex); return 0; } @@ -625,8 +691,6 @@ int audio_system_suspend(void) int audio_system_resume(void) { int ret = 0; - bool resume_requested; - bool encoder_requested; k_mutex_lock(&audio_system_state_mutex, K_FOREVER); if (!audio_system_suspended) { @@ -635,51 +699,75 @@ int audio_system_resume(void) } audio_system_suspended = false; - resume_requested = audio_system_resume_requested; - encoder_requested = encoder_resume_requested; - if (resume_requested) { + if (audio_system_resume_requested) { ret = audio_system_start_internal(); - if (!ret && encoder_requested) { + if (ret == 0 && encoder_resume_requested) { audio_system_encoder_start_internal(); } } - LOG_INF("Audio system suspension released: running=%d encoder=%d result=%d", - resume_requested && !ret, encoder_requested && !ret, ret); audio_system_resume_requested = false; encoder_resume_requested = false; k_mutex_unlock(&audio_system_state_mutex); return ret; } -int audio_system_fifo_rx_block_drop(void) +int audio_system_set_encoder_channel(uint8_t channel) { - int ret; - void *temp; - size_t temp_size; - - ret = data_fifo_pointer_last_filled_get(&fifo_rx, &temp, &temp_size, K_NO_WAIT); - if (ret) { - LOG_WRN("Failed to get last filled block"); - return -ECANCELED; + if (channel > 1U) { + return -EINVAL; } - data_fifo_block_free(&fifo_rx, temp); - - LOG_DBG("Block dropped"); + encoder_channel = channel; + sw_codec_cfg.encoder.num_ch = 1; + sw_codec_cfg.encoder.audio_loc = channel == 0U ? BT_AUDIO_LOCATION_FRONT_LEFT + : BT_AUDIO_LOCATION_FRONT_RIGHT; return 0; } +uint8_t audio_system_get_encoder_channel(void) +{ + return encoder_channel; +} + int audio_system_decoder_num_ch_get(void) { return sw_codec_cfg.decoder.num_ch; } +int audio_system_encoder_num_ch_set(uint32_t locations) +{ + if (IS_ENABLED(CONFIG_MONO_TO_ALL_RECEIVERS)) { + LOG_DBG("Cannot set encoder channels when MONO_TO_ALL_RECEIVERS is enabled"); + return 0; + } + + LOG_DBG("Setting encoder channels to %d based on locations 0x%08x", POPCOUNT(locations), + locations); + + sw_codec_cfg.encoder.num_ch = CONFIG_AUDIO_ENCODE_CHANNELS_MAX; + sw_codec_cfg.encoder.audio_loc = locations; + + return 0; +} + int audio_system_init(void) { int ret; + ret = audio_clock_init(); + if (ret) { + LOG_ERR("Failed to initialize audio clock: %d", ret); + return ret; + } + #if ((CONFIG_AUDIO_DEV == GATEWAY) && (CONFIG_AUDIO_SOURCE_USB)) - ret = audio_usb_init(); + bool host_in = IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL); + /* TODO: OCT-3116 Change when microphone-only support (headset with only an audio source) + * is added + */ + bool host_out = true; + + ret = audio_usb_init(host_in, host_out); if (ret) { LOG_ERR("Failed to initialize USB: %d", ret); return ret; @@ -691,12 +779,16 @@ int audio_system_init(void) return ret; } - ret = hw_codec_init(); - if (ret) { - LOG_ERR("Failed to initialize HW codec: %d", ret); - return ret; + if (IS_ENABLED(CONFIG_BOARD_NRF5340_AUDIO_DK_NRF5340_CPUAPP) || + IS_ENABLED(CONFIG_BOARD_OPENEARABLE_V2_NRF5340_CPUAPP)) { + ret = hw_codec_init(); + if (ret) { + LOG_ERR("Failed to initialize HW codec: %d", ret); + return ret; + } } #endif + k_poll_signal_init(&encoder_sig); return 0; @@ -704,16 +796,10 @@ int audio_system_init(void) static int cmd_audio_system_start(const struct shell *shell, size_t argc, const char **argv) { - int ret; - ARG_UNUSED(argc); ARG_UNUSED(argv); - ret = audio_system_start(); - if (ret) { - shell_error(shell, "Audio system failed to start: %d", ret); - return ret; - } + audio_system_start(); shell_print(shell, "Audio system started"); @@ -740,26 +826,3 @@ SHELL_STATIC_SUBCMD_SET_CREATE(audio_system_cmd, SHELL_SUBCMD_SET_END); SHELL_CMD_REGISTER(audio_system, &audio_system_cmd, "Audio system commands", NULL); - -int audio_system_set_encoder_channel(uint8_t channel) -{ - if (channel >= AUDIO_CH_NUM) { - LOG_ERR("Invalid microphone channel: %d", channel); - return -EINVAL; - } - - encoder_channel = channel; - sw_codec_cfg.encoder.audio_ch = channel; - - if (sw_codec_cfg.initialized && sw_codec_is_initialized()) { - return sw_codec_encoder_channel_set((enum audio_channel)channel); - } - - LOG_INF("Microphone channel set to %d", channel); - return 0; -} - -uint8_t audio_system_get_encoder_channel() -{ - return encoder_channel; -} diff --git a/src/audio/audio_system.h b/src/audio/audio_system.h index c8dc6849..c0b64f63 100644 --- a/src/audio/audio_system.h +++ b/src/audio/audio_system.h @@ -4,19 +4,31 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @defgroup audio_app_system Audio System + * @{ + * @brief Core audio system API for Audio applications. + * + * This module provides the central audio processing functionality for the Audio + * applications, managing both hardware (DAC/ADC) and software codecs (such as LC3). + */ + #ifndef _AUDIO_SYSTEM_H_ #define _AUDIO_SYSTEM_H_ #include #include #include - -#define VALUE_NOT_SET 0 +#include #ifdef __cplusplus extern "C" { #endif +#include "audio_defines.h" + +#define VALUE_NOT_SET 0 + /** * @brief Start the execution of the encoder thread. */ @@ -31,31 +43,12 @@ void audio_system_encoder_start(void); void audio_system_encoder_stop(void); /** - * @brief Check if the encoder is ready to accept PCM frames. + * @brief Check if the encoder thread is currently enabled to execute. * - * @retval true The software codec encoder is initialized and streaming is enabled. - * @retval false The encoder is stopped or the codec has not been initialized. + * @return true if encoder thread is enabled, false otherwise. */ bool audio_system_encoder_is_started(void); -/** - * @brief Exclusively suspend the audio system while preserving requested state. - * - * Start, stop, and encoder requests received during the suspension are deferred - * and applied by audio_system_resume(). - * - * @return 0 on success, or -EBUSY if the system is already suspended. - */ -int audio_system_suspend(void); - -/** - * @brief Release an exclusive suspension and apply the latest audio state. - * - * @return 0 on success, -EALREADY if the system is not suspended, or a - * negative startup error when the preserved audio state cannot resume. - */ -int audio_system_resume(void); - /** * @brief Toggle a test tone on and off. * @@ -93,21 +86,27 @@ int audio_system_encode_test_tone_step(void); int audio_system_config_set(uint32_t encoder_sample_rate_hz, uint32_t encoder_bitrate, uint32_t decoder_sample_rate_hz); +/** + * @brief Set the number of channels for the encoder based on the given audio locations. + * + * @param[in] locations Audio locations bitmask. + * + * @retval -EINVAL Invalid number of channels derived from locations. + * @retval 0 On success. + */ +int audio_system_encoder_num_ch_set(uint32_t locations); + /** * @brief Decode data and then add it to TX FIFO buffer. * - * @param[in] encoded_data Pointer to encoded data. - * @param[in] encoded_data_size Size of encoded data. - * @param[in] bad_frame Indication on missed or incomplete frame. + * @param[in] audio_frame Pointer to the audio data. * * @return 0 on success, error otherwise. */ -int audio_system_decode(void const *const encoded_data, size_t encoded_data_size, bool bad_frame); +int audio_system_decode(struct net_buf *audio_frame); /** - * @brief Initialize and start both HW and SW audio codecs. - * - * @return 0 on success, or a negative error returned by the codec or datapath. + * @brief Initialize and start both HW and SW audio codec. */ int audio_system_start(void); @@ -116,16 +115,6 @@ int audio_system_start(void); */ void audio_system_stop(void); -/** - * @brief Drop oldest block from the fifo_rx buffer. - * - * @note This can be used to reduce latency by adjusting the timing of the completed frame - * that was sampled in relation to the connection interval in Bluetooth LE. - * - * @return 0 on success, -ECANCELED otherwise. - */ -int audio_system_fifo_rx_block_drop(void); - /** * @brief Get number of decoder channels. * @@ -134,24 +123,23 @@ int audio_system_fifo_rx_block_drop(void); int audio_system_decoder_num_ch_get(void); /** - * @brief Initialize the audio_system. + * @brief Initialize the audio system. * * @return 0 on success, error otherwise. */ int audio_system_init(void); -/** - * @brief Set the encoder channel (left or right) - * - * @param[in] channel AUDIO_CH_L for left, AUDIO_CH_R for right - * @return 0 on success, error otherwise - */ +int audio_system_suspend(void); +int audio_system_resume(void); int audio_system_set_encoder_channel(uint8_t channel); - -uint8_t audio_system_get_encoder_channel(); +uint8_t audio_system_get_encoder_channel(void); #ifdef __cplusplus } #endif +/** + * @} + */ + #endif /* _AUDIO_SYSTEM_H_ */ diff --git a/src/audio/decimation_filter.cpp b/src/audio/decimation_filter.cpp index 36af29fa..a7fc7643 100644 --- a/src/audio/decimation_filter.cpp +++ b/src/audio/decimation_filter.cpp @@ -33,6 +33,7 @@ Decimator::Decimator() void Decimator::configure(uint8_t factor) { factor_ = factor; + next_frame_ = 0; initialized_ = false; memset(&biquad_, 0, sizeof(biquad_)); memset(state_, 0, sizeof(state_)); @@ -54,6 +55,7 @@ int Decimator::init() { memset(state_, 0, sizeof(state_)); initialized_ = true; + next_frame_ = 0; LOG_DBG("Decimator initialized with factor %d", factor_); return 0; @@ -81,18 +83,21 @@ int Decimator::process(const int16_t* input, int16_t* output, uint32_t num_frame arm_clip_f32(processing_buffer, processing_buffer, -32768.0f, 32767.0f, num_samples); // Decimate and convert back to int16 - uint32_t out_frames = num_frames / factor_; - uint32_t step = factor_ * 2; - - for (uint32_t i = 0; i < out_frames; i++) { - output[i * 2] = static_cast(processing_buffer[i * step]); - output[i * 2 + 1] = static_cast(processing_buffer[i * step + 1]); + uint32_t out_frames = 0; + uint32_t frame = next_frame_; + for (; frame < num_frames; frame += factor_) { + output[out_frames * 2] = static_cast(processing_buffer[frame * 2]); + output[out_frames * 2 + 1] = static_cast(processing_buffer[frame * 2 + 1]); + out_frames++; } + // Continue the same sample grid when the next block is not factor-aligned. + next_frame_ = frame - num_frames; return out_frames; } void Decimator::reset() { + next_frame_ = 0; if (initialized_) { memset(state_, 0, sizeof(state_)); } @@ -237,6 +242,9 @@ int CascadedDecimator::process(const int16_t* input, int16_t* output, uint32_t n LOG_ERR("Stage %d processing failed: %d", i, frames); return frames; } + if (frames == 0) { + return 0; + } // Next stage input is this stage's output stage_input = stage_output; diff --git a/src/audio/decimation_filter.h b/src/audio/decimation_filter.h index a3f0d5d9..8e5006d0 100644 --- a/src/audio/decimation_filter.h +++ b/src/audio/decimation_filter.h @@ -74,6 +74,7 @@ class Decimator { static constexpr uint32_t NUM_STAGES = 2; uint8_t factor_; + uint8_t next_frame_ = 0; bool initialized_; arm_biquad_cascade_stereo_df2T_instance_f32 biquad_; float32_t state_[4 * NUM_STAGES]; diff --git a/src/audio/le_audio_rx.c b/src/audio/le_audio_rx.c index 9411f230..d15b56d1 100644 --- a/src/audio/le_audio_rx.c +++ b/src/audio/le_audio_rx.c @@ -4,6 +4,9 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +#include "le_audio_rx.h" + +#include #include #include @@ -12,22 +15,15 @@ #include "macros_common.h" #include "audio_system.h" #include "audio_sync_timer.h" +#include "audio_defines.h" +#include "le_audio.h" #include LOG_MODULE_REGISTER(le_audio_rx, CONFIG_LE_AUDIO_RX_LOG_LEVEL); -struct ble_iso_data { - uint8_t data[CONFIG_BT_ISO_RX_MTU]; - size_t data_size; - bool bad_frame; - uint32_t sdu_ref; - uint32_t recv_frame_ts; -} __packed; - struct rx_stats { - uint32_t recv_cnt; - uint32_t bad_frame_cnt; - uint32_t data_size_mismatch_cnt; + uint64_t good_frame_cnt; + uint64_t bad_or_empty_frame_cnt; }; static bool initialized; @@ -35,48 +31,111 @@ static struct k_thread audio_datapath_thread_data; static k_tid_t audio_datapath_thread_id; K_THREAD_STACK_DEFINE(audio_datapath_thread_stack, CONFIG_AUDIO_DATAPATH_STACK_SIZE); -DATA_FIFO_DEFINE(ble_fifo_rx, CONFIG_BUF_BLE_RX_PACKET_NUM, WB_UP(sizeof(struct ble_iso_data))); +K_MSGQ_DEFINE(ble_q_rx, sizeof(struct net_buf *), CONFIG_BUF_BLE_RX_PACKET_NUM, sizeof(void *)); +NET_BUF_POOL_FIXED_DEFINE(ble_rx_pool, CONFIG_BUF_BLE_RX_PACKET_NUM, + (CONFIG_BT_ISO_RX_MTU * CONFIG_BT_AUDIO_CONCURRENT_RX_STREAMS_MAX), + sizeof(struct audio_metadata), NULL); + +static int location_to_net_buf_index(enum bt_audio_location total_loc, + enum bt_audio_location srch_loc, uint8_t *index) +{ + uint8_t idx = 0; + + while (total_loc) { + if (srch_loc & 0x01) { + *index = idx; + return 0; + } + + /* Count the number of active locations */ + if (total_loc & 0x01) { + idx++; + } + + total_loc >>= 1; + srch_loc >>= 1; + } + + return -EINVAL; +} + +static int audio_frame_add(struct net_buf *audio_frame, struct net_buf const *const audio_frame_rx, + struct audio_metadata *meta, enum bt_audio_location stream_locations) +{ + int ret; + uint32_t offset = 0; + + if (audio_frame == NULL || audio_frame_rx == NULL || meta == NULL) { + LOG_ERR("Invalid input parameters"); + return -EINVAL; + } + + if (meta->locations != stream_locations) { + /* Get net_buf_index for given location */ + uint8_t net_buf_index = 0; + + ret = location_to_net_buf_index(stream_locations, meta->locations, &net_buf_index); + if (ret) { + LOG_ERR("Invalid location index, ret: %d", ret); + return ret; + } + + /* Calculate offset in net_buf */ + offset = net_buf_index * meta->bytes_per_location; + + if (offset > audio_frame->size - audio_frame_rx->len) { + LOG_ERR("Calculated offset too large: %d", offset); + return -EINVAL; + } + } + + if (audio_frame_rx->len && !meta->bad_data) { + /* Copy audio data to the correct offset */ + memcpy(&audio_frame->data[offset], audio_frame_rx->data, audio_frame_rx->len); + } + + return 0; +} /* Callback for handling ISO RX */ -void le_audio_rx_data_handler(uint8_t const *const p_data, size_t data_size, bool bad_frame, - uint32_t sdu_ref, enum audio_channel channel_index, - size_t desired_data_size) +void le_audio_rx_data_handler(struct net_buf *audio_frame_rx, struct audio_metadata *meta, + uint8_t location_index) { int ret; - uint32_t blocks_alloced_num, blocks_locked_num; - struct ble_iso_data *iso_received = NULL; - static struct rx_stats rx_stats[AUDIO_CH_NUM]; + static struct net_buf *audio_frame; + static struct rx_stats rx_stats[CONFIG_BT_AUDIO_CONCURRENT_RX_STREAMS_MAX]; static uint32_t num_overruns; static uint32_t num_thrown; + __ASSERT_NO_MSG(audio_frame_rx != NULL); + __ASSERT_NO_MSG(meta != NULL); + if (!initialized) { ERR_CHK_MSG(-EPERM, "Data received but le_audio_rx is not initialized"); } - /* Capture timestamp of when audio frame is received */ - uint32_t recv_frame_ts = audio_sync_timer_capture(); - - rx_stats[channel_index].recv_cnt++; - - if (data_size != desired_data_size) { - /* A valid frame should always be equal to desired_data_size, set bad_frame - * if that is not the case - */ - bad_frame = true; - rx_stats[channel_index].data_size_mismatch_cnt++; + if (meta->bad_data) { + rx_stats[location_index].bad_or_empty_frame_cnt++; + } else { + rx_stats[location_index].good_frame_cnt++; } - if (bad_frame) { - rx_stats[channel_index].bad_frame_cnt++; - } + uint64_t total_frames = rx_stats[location_index].good_frame_cnt + + rx_stats[location_index].bad_or_empty_frame_cnt; - if ((rx_stats[channel_index].recv_cnt % 100) == 0 && rx_stats[channel_index].recv_cnt) { + if ((total_frames % 100) == 0 && (total_frames != 0)) { /* NOTE: The string below is used by the Nordic CI system */ - LOG_DBG("ISO RX SDUs: Ch: %d Total: %d Bad: %d Size mismatch %d", channel_index, - rx_stats[channel_index].recv_cnt, rx_stats[channel_index].bad_frame_cnt, - rx_stats[channel_index].data_size_mismatch_cnt); + LOG_DBG("ISO RX SDUs: Loc: %u Total: %llu Bad: %llu", location_index, total_frames, + rx_stats[location_index].bad_or_empty_frame_cnt); } + LOG_DBG_RATELIMIT_RATE( + 10000, "Bad or 0 SDU: Loc: %u Total: %llu Empty/bad: %llu (%2.3f %%)", + location_index, total_frames, rx_stats[location_index].bad_or_empty_frame_cnt, + total_frames > 0 + ? 100.0 * rx_stats[location_index].bad_or_empty_frame_cnt / total_frames + : 0.0); + if (stream_state_get() != STATE_STREAMING) { /* Throw away data */ num_thrown++; @@ -87,50 +146,114 @@ void le_audio_rx_data_handler(uint8_t const *const p_data, size_t data_size, boo return; } - if (channel_index != AUDIO_CH_L && (CONFIG_AUDIO_DEV == GATEWAY)) { - /* Only left channel RX data in use on gateway */ + if (location_index != 0 && (CONFIG_AUDIO_DEV == GATEWAY)) { + /* Only the first device will be used as mic input on gateway */ return; } - ret = data_fifo_num_used_get(&ble_fifo_rx, &blocks_alloced_num, &blocks_locked_num); - ERR_CHK(ret); - - if (blocks_alloced_num >= CONFIG_BUF_BLE_RX_PACKET_NUM) { + if (k_msgq_num_free_get(&ble_q_rx) == 0) { + struct net_buf *stale_buf = NULL; /* FIFO buffer is full, swap out oldest frame for a new one */ + ret = k_msgq_get(&ble_q_rx, (void *)&stale_buf, K_NO_WAIT); + /* The consumer can drain the queue after the free-space check. */ + if (ret == 0 && stale_buf != NULL) { + num_overruns++; - void *stale_data; - size_t stale_size; - num_overruns++; + if ((num_overruns % 100) == 1) { + LOG_WRN("BLE ISO RX overrun: Num: %d", num_overruns); + } - if ((num_overruns % 100) == 1) { - LOG_WRN("BLE ISO RX overrun: Num: %d", num_overruns); + net_buf_unref(stale_buf); } + } - ret = data_fifo_pointer_last_filled_get(&ble_fifo_rx, &stale_data, &stale_size, - K_NO_WAIT); - ERR_CHK(ret); + uint8_t num_active_streams = 0; + enum bt_audio_location stream_locations = 0; + + ret = le_audio_concurrent_sync_num_get(&num_active_streams, &stream_locations); + if (ret) { + LOG_ERR("Failed to get number of active streams: %d", ret); + return; + } - data_fifo_block_free(&ble_fifo_rx, stale_data); + /* Check if we already have received a frame with the same timestamp */ + if (audio_frame != NULL) { + struct audio_metadata *existing_meta = net_buf_user_data(audio_frame); + uint32_t ref_ts_diff = abs((meta->ref_ts_us - existing_meta->ref_ts_us)); + + if (ref_ts_diff < SDU_REF_CH_DELTA_MAX_US) { + /* We already have a frame with this timestamp, so we will add these + * together if the locations are different + */ + if (existing_meta->locations != meta->locations) { + /* Add location and bad_frame status to the joint audio frame */ + existing_meta->locations |= meta->locations; + existing_meta->bad_data |= meta->bad_data; + + ret = audio_frame_add(audio_frame, audio_frame_rx, meta, + stream_locations); + if (ret) { + LOG_ERR("Failed to add audio frame data"); + net_buf_unref(audio_frame); + audio_frame = NULL; + return; + } + + /* Check if we need to send the frame */ + goto check_send; + } else { + LOG_ERR("Received audio frame with same timestamp and same " + "locations, discarding new data"); + return; + } + } else { + /* We have a new frame with a different timestamp, so we will send the + * old frame and create a new one. This will usually happen if we miss a + * frame or we suddenly go from one to two or more streams. + */ + LOG_WRN("Received audio frame with different timestamp, sending old frame, " + "old ts: %u, new ts: %u", + existing_meta->ref_ts_us, meta->ref_ts_us); + + ret = k_msgq_put(&ble_q_rx, (void *)&audio_frame, K_NO_WAIT); + ERR_CHK_MSG(ret, "Failed to put audio frame into queue"); + audio_frame = NULL; + } } - ret = data_fifo_pointer_first_vacant_get(&ble_fifo_rx, (void *)&iso_received, K_NO_WAIT); - ERR_CHK_MSG(ret, "Unable to get FIFO pointer"); + /* Capture timestamp of when audio frame is received */ + meta->data_rx_ts_us = audio_sync_timer_capture(); - if (data_size > ARRAY_SIZE(iso_received->data)) { - ERR_CHK_MSG(-ENOMEM, "Data size too large for buffer"); + audio_frame = net_buf_alloc(&ble_rx_pool, K_NO_WAIT); + if (audio_frame == NULL) { + LOG_WRN_RATELIMIT_RATE(1000, "Out of RX buffers"); return; } - memcpy(iso_received->data, p_data, data_size); + /* Store metadata from the first frame */ + memcpy(net_buf_user_data(audio_frame), meta, sizeof(struct audio_metadata)); + + /* Set aside memory in net_buf for all locations */ + net_buf_add(audio_frame, meta->bytes_per_location * num_active_streams); - iso_received->bad_frame = bad_frame; - iso_received->data_size = data_size; - iso_received->sdu_ref = sdu_ref; - iso_received->recv_frame_ts = recv_frame_ts; + ret = audio_frame_add(audio_frame, audio_frame_rx, meta, stream_locations); + if (ret) { + LOG_ERR("Failed to add audio frame data"); + net_buf_unref(audio_frame); + audio_frame = NULL; + return; + } - ret = data_fifo_block_lock(&ble_fifo_rx, (void *)&iso_received, - sizeof(struct ble_iso_data)); - ERR_CHK_MSG(ret, "Failed to lock block"); + /* Check if we have received all frames, send if we have */ +check_send: + if (num_active_streams == audio_metadata_num_loc_get(net_buf_user_data(audio_frame))) { + /* We have received all frames we are waiting for, pass data on to + * the next module + */ + ret = k_msgq_put(&ble_q_rx, (void *)&audio_frame, K_NO_WAIT); + ERR_CHK_MSG(ret, "Failed to put audio frame into queue"); + audio_frame = NULL; + } } /** @@ -143,24 +266,20 @@ static void audio_datapath_thread(void *dummy1, void *dummy2, void *dummy3) ARG_UNUSED(dummy3); int ret; - struct ble_iso_data *iso_received = NULL; - size_t iso_received_size; + struct net_buf *audio_frame = NULL; while (1) { - ret = data_fifo_pointer_last_filled_get(&ble_fifo_rx, (void *)&iso_received, - &iso_received_size, K_FOREVER); + ret = k_msgq_get(&ble_q_rx, (void *)&audio_frame, K_FOREVER); ERR_CHK(ret); if (IS_ENABLED(CONFIG_AUDIO_SOURCE_USB) && (CONFIG_AUDIO_DEV == GATEWAY)) { - ret = audio_system_decode(iso_received->data, iso_received->data_size, - iso_received->bad_frame); + ret = audio_system_decode(audio_frame); ERR_CHK(ret); } else { - audio_datapath_stream_out(iso_received->data, iso_received->data_size, - iso_received->sdu_ref, iso_received->bad_frame, - iso_received->recv_frame_ts); + audio_datapath_stream_out(audio_frame); } - data_fifo_block_free(&ble_fifo_rx, (void *)iso_received); + + net_buf_unref(audio_frame); STACK_USAGE_PRINT("audio_datapath_thread", &audio_datapath_thread_data); } @@ -174,7 +293,7 @@ static int audio_datapath_thread_create(void) &audio_datapath_thread_data, audio_datapath_thread_stack, CONFIG_AUDIO_DATAPATH_STACK_SIZE, (k_thread_entry_t)audio_datapath_thread, NULL, NULL, NULL, K_PRIO_PREEMPT(CONFIG_AUDIO_DATAPATH_THREAD_PRIO), 0, K_NO_WAIT); - ret = k_thread_name_set(audio_datapath_thread_id, "AUDIO_DATAPATH"); + ret = k_thread_name_set(audio_datapath_thread_id, "Audio_datapath"); if (ret) { LOG_ERR("Failed to create audio_datapath thread"); return ret; @@ -191,12 +310,6 @@ int le_audio_rx_init(void) return -EALREADY; } - ret = data_fifo_init(&ble_fifo_rx); - if (ret) { - LOG_ERR("Failed to set up ble_rx FIFO"); - return ret; - } - ret = audio_datapath_thread_create(); if (ret) { return ret; diff --git a/src/audio/le_audio_rx.h b/src/audio/le_audio_rx.h index b50e10c7..611c3be6 100644 --- a/src/audio/le_audio_rx.h +++ b/src/audio/le_audio_rx.h @@ -4,28 +4,43 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @defgroup audio_app_le_audio_rx LE Audio RX + * @{ + * @brief LE Audio receive (RX) API for Audio applications. + * + * This module handles the reception and processing of incoming LE Audio streams from + * Bluetooth. It manages the audio data reception pipeline, including + * metadata extraction and audio frame processing for both unicast (CIS) and broadcast + * (BIS) modes. + */ + #ifndef _LE_AUDIO_RX_H_ #define _LE_AUDIO_RX_H_ +#include +#include +#include + /** - * @brief Data handler when ISO data has been received. + * @brief Data handler when ISO data has been received. * - * @param[in] p_data Pointer to the received data. - * @param[in] data_size Size of the received data. - * @param[in] bad_frame Bad frame flag. (I.e. set for missed ISO data). - * @param[in] sdu_ref SDU reference timestamp. - * @param[in] channel_index Which channel is received. - * @param[in] desired_data_size The expected data size. + * @param[in] audio_frame_rx Pointer to the audio buffer. + * @param[in] meta Pointer to the audio metadata. + * @param[in] location_index Which location is received. */ -void le_audio_rx_data_handler(uint8_t const *const p_data, size_t data_size, bool bad_frame, - uint32_t sdu_ref, enum audio_channel channel_index, - size_t desired_data_size); +void le_audio_rx_data_handler(struct net_buf *audio_frame_rx, struct audio_metadata *meta, + uint8_t location_index); /** - * @brief Initialize the receive audio path. + * @brief Initialize the receive audio path. * - * @return 0 if successful, error otherwise. + * @return 0 if successful, error otherwise. */ int le_audio_rx_init(void); +/** + * @} + */ + #endif /* _LE_AUDIO_RX_H_ */ diff --git a/src/audio/streamctrl.c b/src/audio/streamctrl.c index e3dc6df2..44ca2d76 100644 --- a/src/audio/streamctrl.c +++ b/src/audio/streamctrl.c @@ -38,7 +38,10 @@ #include LOG_MODULE_REGISTER(streamctrl, CONFIG_MAIN_LOG_LEVEL); -ZBUS_SUBSCRIBER_DEFINE(button_evt_sub, CONFIG_BUTTON_MSG_SUB_QUEUE_SIZE); +BUILD_ASSERT(CONFIG_BT_AUDIO_CONCURRENT_RX_STREAMS_MAX <= CONFIG_AUDIO_DECODE_CHANNELS_MAX); +BUILD_ASSERT(CONFIG_BT_AUDIO_CONCURRENT_TX_STREAMS_MAX <= CONFIG_AUDIO_ENCODE_CHANNELS_MAX); + +ZBUS_MSG_SUBSCRIBER_DEFINE(button_evt_sub); ZBUS_MSG_SUBSCRIBER_DEFINE(le_audio_evt_sub); @@ -59,6 +62,9 @@ K_THREAD_STACK_DEFINE(button_msg_sub_thread_stack, CONFIG_BUTTON_MSG_SUB_STACK_S K_THREAD_STACK_DEFINE(le_audio_msg_sub_thread_stack, CONFIG_LE_AUDIO_MSG_SUB_STACK_SIZE); static enum stream_state strm_state = STATE_PAUSED; +static const char le_audio_auto_off_token[] = "LEAudio"; + +#define MEDIA_DOUBLE_CLICK_MS 400 /* Function for handling all stream state changes */ static void stream_state_set(enum stream_state stream_state_new) @@ -66,6 +72,16 @@ static void stream_state_set(enum stream_state stream_state_new) strm_state = stream_state_new; } +static void media_play_pause(void) +{ + int ret = bt_content_ctlr_media_state_playing() ? bt_content_ctrl_stop(NULL) : + bt_content_ctrl_start(NULL); + + if (ret) { + LOG_WRN("Could not toggle playback: %d", ret); + } +} + /** * @brief Handle button activity. */ @@ -73,15 +89,24 @@ static void button_msg_sub_thread(void) { int ret; const struct zbus_channel *chan; + int64_t click_deadline = 0; while (1) { - ret = zbus_sub_wait(&button_evt_sub, &chan, K_FOREVER); - ERR_CHK(ret); - struct button_msg msg; + k_timeout_t timeout = click_deadline ? K_TIMEOUT_ABS_MS(click_deadline) : K_FOREVER; - ret = zbus_chan_read(chan, &msg, ZBUS_READ_TIMEOUT_MS); - ERR_CHK(ret); + ret = zbus_sub_wait_msg(&button_evt_sub, &chan, &msg, timeout); + if (ret != -ENOMSG) { + ERR_CHK(ret); + } + + if (click_deadline && k_uptime_get() >= click_deadline) { + click_deadline = 0; + media_play_pause(); + } + if (ret == -ENOMSG) { + continue; + } LOG_DBG("Got btn evt from queue - id = %d, action = %d", msg.button_pin, msg.button_action); @@ -98,20 +123,15 @@ static void button_msg_sub_thread(void) break; } - if (bt_content_ctlr_media_state_playing()) { - ret = bt_content_ctrl_stop(NULL); - if (ret) { - LOG_WRN("Could not stop: %d", ret); - } - - } else if (!bt_content_ctlr_media_state_playing()) { - ret = bt_content_ctrl_start(NULL); + if (click_deadline) { + click_deadline = 0; + ret = bt_content_ctrl_next_track(NULL); if (ret) { - LOG_WRN("Could not start: %d", ret); + LOG_WRN("Could not skip track: %d", ret); } - } else { - LOG_WRN("In invalid state: %d", strm_state); + /* Wait before toggling so a double click sends only next-track. */ + click_deadline = k_uptime_get() + MEDIA_DOUBLE_CLICK_MS; } break; @@ -180,6 +200,7 @@ static void button_msg_sub_thread(void) static void le_audio_msg_sub_thread(void) { int ret; + uint8_t active_audio_dirs = 0; uint32_t pres_delay_us; uint32_t bitrate_bps; uint32_t sampling_rate_hz; @@ -195,6 +216,8 @@ static void le_audio_msg_sub_thread(void) switch (msg.event) { case LE_AUDIO_EVT_STREAMING: + active_audio_dirs |= msg.dir; + auto_off_prohibit(le_audio_auto_off_token); LOG_DBG("LE audio evt streaming"); if (strm_state == STATE_STREAMING) { @@ -222,6 +245,11 @@ static void le_audio_msg_sub_thread(void) break; case LE_AUDIO_EVT_NOT_STREAMING: + /* A stopped microphone must not release the playback veto. */ + active_audio_dirs &= ~msg.dir; + if (active_audio_dirs == 0) { + auto_off_allow(le_audio_auto_off_token); + } LOG_DBG("LE audio evt not streaming"); if (strm_state == STATE_PAUSED) { @@ -390,6 +418,10 @@ static void bt_mgmt_evt_handler(const struct zbus_channel *chan) break; + case BT_MGMT_PAIRING_COMPLETE: + /* Pairing state is handled by the OpenEarable application layer. */ + break; + default: LOG_WRN("Unexpected/unhandled bt_mgmt event: %d", msg->event); @@ -483,6 +515,14 @@ static int ext_adv_populate(struct bt_data *ext_adv_buf, size_t ext_adv_buf_size return ret; } + if (ext_adv_buf_cnt >= ext_adv_buf_size) { + return -ENOMEM; + } + ext_adv_buf[ext_adv_buf_cnt].type = BT_DATA_NAME_COMPLETE; + ext_adv_buf[ext_adv_buf_cnt].data = bt_get_name(); + ext_adv_buf[ext_adv_buf_cnt].data_len = strlen(bt_get_name()); + ext_adv_buf_cnt++; + ret = unicast_server_adv_populate(&ext_adv_buf[ext_adv_buf_cnt], ext_adv_buf_size - ext_adv_buf_cnt); @@ -509,15 +549,13 @@ uint8_t stream_state_get(void) return strm_state; } -void streamctrl_send(void const *const data, size_t size, uint8_t num_ch) +void streamctrl_send(struct net_buf const *const audio_frame) { int ret; static int prev_ret; - struct le_audio_encoded_audio enc_audio = {.data = data, .size = size, .num_ch = num_ch}; - if (strm_state == STATE_STREAMING) { - ret = unicast_server_send(enc_audio); + ret = unicast_server_send(audio_frame); if (ret != 0 && ret != prev_ret) { if (ret == -ECANCELED) { @@ -711,6 +749,11 @@ int streamctrl_start() //streamctrl_start ret = auto_off_init(); ERR_CHK(ret); + ret = auto_off_register_participant(le_audio_auto_off_token, + (power_saving_level_t)CONFIG_POWER_SAVING_LEVEL_LEAUDIO); + ERR_CHK(ret); + auto_off_allow(le_audio_auto_off_token); + ret = audio_system_init(); ERR_CHK(ret); @@ -757,7 +800,9 @@ int streamctrl_start() //streamctrl_start if (ret == 0) sys_reboot(SYS_REBOOT_COLD); else LOG_ERR("UICR writing error: %i", ret); } else { - struct bt_le_scan_param * scan_param = BT_LE_SCAN_PARAM(NRF5340_AUDIO_GATEWAY_SCAN_TYPE, BT_LE_SCAN_OPT_FILTER_DUPLICATE, 32, 32); + struct bt_le_scan_param *scan_param = + BT_LE_SCAN_PARAM(NRF_AUDIO_GATEWAY_SCAN_TYPE, + BT_LE_SCAN_OPT_FILTER_DUPLICATE, 32, 32); int err = bt_le_scan_start(scan_param, device_found); if (err) LOG_ERR("Scanning failed to start (err %d)", err); diff --git a/src/audio/streamctrl.h b/src/audio/streamctrl.h index e7d3f912..d0f2eb8b 100644 --- a/src/audio/streamctrl.h +++ b/src/audio/streamctrl.h @@ -4,13 +4,25 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @defgroup audio_app_streamctrl Audio Stream Control + * @{ + * @brief Stream control API for Audio applications. + * + * This module provides stream state management and control functions for audio streaming + * operations. + */ + #ifndef _STREAMCTRL_H_ #define _STREAMCTRL_H_ #include #include +#include -/* State machine states for peer or stream. */ +/** + * @brief Stream state enumeration for audio streaming operations. + */ enum stream_state { STATE_STREAMING, STATE_PAUSED, @@ -21,20 +33,22 @@ extern "C" { #endif /** - * @brief Get the current streaming state. + * @brief Get the current streaming state. * - * @return strm_state enum value. + * @return strm_state enum value. */ uint8_t stream_state_get(void); /** - * @brief Send audio data over the stream. + * @brief Send audio data over the stream. * - * @param data Data to send. - * @param size Size of data. - * @param num_ch Number of audio channels. + * @param audio_frame Pointer to the audio buffer. + */ +void streamctrl_send(struct net_buf const *const audio_frame); + +/** + * @} */ -void streamctrl_send(void const *const data, size_t size, uint8_t num_ch); int streamctrl_start(); diff --git a/src/audio/sw_codec_select.c b/src/audio/sw_codec_select.c index 8f5de1af..1771a700 100644 --- a/src/audio/sw_codec_select.c +++ b/src/audio/sw_codec_select.c @@ -7,10 +7,13 @@ #include "sw_codec_select.h" #include +#include #include #include #include +#include "macros_common.h" + #if (CONFIG_SW_CODEC_LC3) #include "sw_codec_lc3.h" #endif /* (CONFIG_SW_CODEC_LC3) */ @@ -18,10 +21,13 @@ #include LOG_MODULE_REGISTER(sw_codec_select, CONFIG_SW_CODEC_SELECT_LOG_LEVEL); +/* Used for debugging, inserts 0 instead of packet loss concealment */ +#define SW_CODEC_OVERRIDE_PLC false + static struct sw_codec_config m_config; -static struct sample_rate_converter_ctx encoder_converters[AUDIO_CH_NUM]; -static struct sample_rate_converter_ctx decoder_converters[AUDIO_CH_NUM]; +static struct sample_rate_converter_ctx encoder_converters[CONFIG_AUDIO_ENCODE_CHANNELS_MAX]; +static struct sample_rate_converter_ctx decoder_converters[CONFIG_AUDIO_DECODE_CHANNELS_MAX]; /** * @brief Converts the sample rate of the uncompressed audio stream if needed. @@ -83,288 +89,303 @@ bool sw_codec_is_initialized(void) return m_config.initialized; } -int sw_codec_encoder_channel_set(enum audio_channel channel) +int sw_codec_encode(struct net_buf *audio_frame_in, struct net_buf *audio_frame_out) { - if (channel >= AUDIO_CH_NUM) { - LOG_ERR("Invalid encoder channel: %d", channel); - return -EINVAL; + if (unlikely(!m_config.encoder.enabled)) { + LOG_ERR("Encoder has not been initialized"); + return -ENXIO; } - m_config.encoder.audio_ch = channel; - LOG_INF("Microphone channel set to %d", channel); - - return 0; -} + if (unlikely(audio_frame_in == NULL || audio_frame_out == NULL)) { + LOG_ERR("LC3 encoder input parameter error"); + return -EINVAL; + } -int sw_codec_encode(void *pcm_data, size_t pcm_size, uint8_t **encoded_data, size_t *encoded_size) -{ int ret; - - /* Temp storage for split stereo PCM signal */ - char pcm_data_mono_system_sample_rate[AUDIO_CH_NUM][PCM_NUM_BYTES_MONO] = {0}; - /* Make sure we have enough space for two frames (stereo) */ - static uint8_t m_encoded_data[ENC_MAX_FRAME_SIZE * AUDIO_CH_NUM]; - - char pcm_data_mono_converted_buf[AUDIO_CH_NUM][PCM_NUM_BYTES_MONO] = {0}; - - size_t pcm_block_size_mono_system_sample_rate; - size_t pcm_block_size_mono; - - if (!m_config.encoder.enabled) { - LOG_ERR("Encoder has not been initialized"); - return -ENXIO; - } + struct audio_metadata *meta_in = net_buf_user_data(audio_frame_in); + struct audio_metadata *meta_out = net_buf_user_data(audio_frame_out); switch (m_config.sw_codec) { case SW_CODEC_LC3: { #if (CONFIG_SW_CODEC_LC3) - uint16_t encoded_bytes_written; - char *pcm_data_mono_ptrs[AUDIO_CH_NUM] = {0}; + uint8_t inter_buf[PCM_NUM_BYTES_MONO]; + uint8_t src_buf[PCM_NUM_BYTES_MONO]; + uint8_t chan_in_num, chan_out_num; + uint8_t chan_in = 0; + uint8_t chan_out = 0; + uint8_t *inter_out; + uint8_t *enc_in = audio_frame_in->data; + uint8_t *enc_out = audio_frame_out->data; + size_t enc_in_size = 0; + uint16_t bytes_written = 0; + uint32_t loc_in = 0; + uint32_t loc_out = 0; + + LOG_DBG("LC3 encoder module"); + + if (unlikely((meta_in->data_coding != PCM) || (meta_out->data_coding != LC3))) { + LOG_ERR("LC3 encoder module has incorrect input or output data type: in = " + "%d out = %d", + meta_in->data_coding, meta_out->data_coding); + return -EINVAL; + } - /* Since LC3 is a single channel codec, we must split the - * stereo PCM stream - */ - ret = pscm_two_channel_split(pcm_data, pcm_size, CONFIG_AUDIO_BIT_DEPTH_BITS, - pcm_data_mono_system_sample_rate[AUDIO_CH_L], - pcm_data_mono_system_sample_rate[AUDIO_CH_R], - &pcm_block_size_mono_system_sample_rate); - if (ret) { - return ret; + chan_in_num = audio_metadata_num_loc_get(meta_in); + if (unlikely(audio_frame_in->len < (meta_in->bytes_per_location * chan_in_num))) { + LOG_ERR("Encoder input buffer too small: %d (>=%d)", audio_frame_in->len, + meta_in->bytes_per_location * chan_in_num); + return -EINVAL; } - switch (m_config.encoder.channel_mode) { - case SW_CODEC_MONO: { - enum audio_channel audio_ch = m_config.encoder.audio_ch; + chan_out_num = audio_metadata_num_loc_get(meta_out); + if (unlikely(audio_frame_out->size < + (meta_out->bytes_per_location * chan_out_num))) { + LOG_ERR("Encoder output buffer too small: %d (>=%d)", audio_frame_out->size, + meta_out->bytes_per_location * chan_out_num); + return -EINVAL; + } - if (audio_ch >= AUDIO_CH_NUM) { - LOG_ERR("Invalid encoder channel: %d", audio_ch); - return -EINVAL; - } + if (meta_in->locations == BT_AUDIO_LOCATION_MONO_AUDIO) { + loc_in = 1; + } else { + loc_in = meta_in->locations; + } - ret = sw_codec_sample_rate_convert( - &encoder_converters[audio_ch], CONFIG_AUDIO_SAMPLE_RATE_HZ, - m_config.encoder.sample_rate_hz, - pcm_data_mono_system_sample_rate[audio_ch], - pcm_block_size_mono_system_sample_rate, - pcm_data_mono_converted_buf[audio_ch], &pcm_data_mono_ptrs[audio_ch], - &pcm_block_size_mono); - if (ret) { - LOG_ERR("Sample rate conversion failed for channel %d: %d", audio_ch, ret); - return ret; + if (meta_out->locations == BT_AUDIO_LOCATION_MONO_AUDIO) { + /* Set output to be the lowest set location in meta_in->locations */ + loc_out = 1; + while (loc_out && !(loc_in & loc_out)) { + loc_out <<= 1; } + } else { + loc_out = meta_out->locations; + } - ret = sw_codec_lc3_enc_run(pcm_data_mono_ptrs[audio_ch], - pcm_block_size_mono, LC3_USE_BITRATE_FROM_INIT, - 0, sizeof(m_encoded_data), m_encoded_data, - &encoded_bytes_written); - if (ret) { - return ret; - } - break; + if (unlikely((loc_out & loc_in) == 0)) { + LOG_ERR("No common output location with input"); + LOG_ERR("Input locations: 0x%08x", meta_in->locations); + LOG_ERR("Output locations: 0x%08x", meta_out->locations); + return -EINVAL; } - case SW_CODEC_STEREO: { - /* Stereo encoding consumes both channel pointers below. Reject mismatched - * configurations so the caller can drop the frame safely. - */ - if (m_config.encoder.num_ch != AUDIO_CH_NUM) { - LOG_DBG("Rejecting stereo encode with %u configured channels", - (unsigned int)m_config.encoder.num_ch); - return -EINVAL; - } - for (int i = 0; i < m_config.encoder.num_ch; ++i) { - ret = sw_codec_sample_rate_convert( - &encoder_converters[i], CONFIG_AUDIO_SAMPLE_RATE_HZ, - m_config.encoder.sample_rate_hz, - pcm_data_mono_system_sample_rate[i], - pcm_block_size_mono_system_sample_rate, - pcm_data_mono_converted_buf[i], &pcm_data_mono_ptrs[i], - &pcm_block_size_mono); - if (ret) { - LOG_ERR("Sample rate conversion failed for channel %d: %d", i, ret); - return ret; + /* Encode only the common channel(s) between the input and output locations. */ + while (loc_out && loc_in) { + if (loc_out & loc_in & 0x01) { + if (meta_in->interleaved) { + ret = pscm_deinterleave(audio_frame_in->data, + audio_frame_in->len, chan_in_num, + chan_in, + meta_in->carried_bits_per_sample, + inter_buf, sizeof(inter_buf)); + ERR_CHK_MSG(ret, "Encode: Failed de-interleaving"); + + inter_out = inter_buf; + } else { + inter_out = (uint8_t *)audio_frame_in->data + + (meta_in->bytes_per_location * chan_in); } - } - ret = sw_codec_lc3_enc_run(pcm_data_mono_ptrs[AUDIO_CH_L], - pcm_block_size_mono, LC3_USE_BITRATE_FROM_INIT, - AUDIO_CH_L, sizeof(m_encoded_data), - m_encoded_data, &encoded_bytes_written); - if (ret) { - return ret; - } + ret = sw_codec_sample_rate_convert( + &encoder_converters[chan_out], meta_in->sample_rate_hz, + meta_out->sample_rate_hz, inter_out, + meta_in->bytes_per_location, src_buf, (char **)&enc_in, + &enc_in_size); + ERR_CHK_MSG(ret, "Encode: Sample rate conversion failed"); - ret = sw_codec_lc3_enc_run( - pcm_data_mono_ptrs[AUDIO_CH_R], pcm_block_size_mono, - LC3_USE_BITRATE_FROM_INIT, AUDIO_CH_R, - sizeof(m_encoded_data) - encoded_bytes_written, - m_encoded_data + encoded_bytes_written, &encoded_bytes_written); - if (ret) { - return ret; + ret = sw_codec_lc3_enc_run( + enc_in, enc_in_size, meta_out->bitrate_bps, chan_out, + meta_in->bytes_per_location, enc_out, &bytes_written); + ERR_CHK_MSG(ret, "Encode failed"); + + enc_out += bytes_written; + + LOG_DBG("Completed LC3 encode of ch: %d", chan_out); } - encoded_bytes_written += encoded_bytes_written; - break; + + chan_in += loc_in & 0x01; + chan_out += loc_out & 0x01; + + loc_in >>= 1; + loc_out >>= 1; } - default: - LOG_ERR("Unsupported channel mode for encoder: %d", - m_config.encoder.channel_mode); - return -ENODEV; + + if (IS_ENABLED(CONFIG_MONO_TO_ALL_RECEIVERS) && (m_config.encoder.num_ch > 1)) { + /* Duplicate the mono encoded data to all output locations */ + size_t single_chan_size = bytes_written; + + for (uint8_t ch = 1; ch < m_config.encoder.num_ch; ch++) { + memcpy(audio_frame_out->data + (ch * single_chan_size), + audio_frame_out->data, single_chan_size); + } } - *encoded_data = m_encoded_data; - *encoded_size = encoded_bytes_written; + meta_out->bytes_per_location = bytes_written; + meta_out->locations &= meta_in->locations; + net_buf_add(audio_frame_out, + meta_out->bytes_per_location * audio_metadata_num_loc_get(meta_out)); + return 0; #endif /* (CONFIG_SW_CODEC_LC3) */ break; } default: - LOG_ERR("Unsupported codec: %d", m_config.sw_codec); + LOG_ERR("Unsupported codec: %d", meta_out->data_coding); return -ENODEV; } return 0; } -int sw_codec_decode(uint8_t const *const encoded_data, size_t encoded_size, bool bad_frame, - void **decoded_data, size_t *decoded_size) +int sw_codec_decode(struct net_buf const *const audio_frame_in, + struct net_buf *const audio_frame_out) { + if (audio_frame_in == NULL || audio_frame_out == NULL) { + return -EINVAL; + } + if (!m_config.decoder.enabled) { LOG_ERR("Decoder has not been initialized"); return -ENXIO; } int ret; - - static char pcm_data_stereo[PCM_NUM_BYTES_STEREO]; - - char decoded_data_mono[AUDIO_CH_NUM][PCM_NUM_BYTES_MONO] = {0}; - char decoded_data_mono_system_sample_rate[AUDIO_CH_NUM][PCM_NUM_BYTES_MONO] = {0}; - - size_t pcm_size_stereo = 0; - size_t pcm_size_mono = 0; - size_t decoded_data_size = 0; + struct audio_metadata *meta_in = net_buf_user_data(audio_frame_in); + struct audio_metadata *meta_out = net_buf_user_data(audio_frame_out); switch (m_config.sw_codec) { case SW_CODEC_LC3: { #if (CONFIG_SW_CODEC_LC3) - /* PLC override bypasses sample-rate conversion, so its branches assign these - * fixed channel slots directly to zero-filled PCM buffers. - */ - char *pcm_in_data_ptrs[AUDIO_CH_NUM] = {0}; - - switch (m_config.decoder.channel_mode) { - case SW_CODEC_MONO: { - if (bad_frame && IS_ENABLED(CONFIG_SW_CODEC_OVERRIDE_PLC)) { - memset(decoded_data_mono[AUDIO_CH_L], 0, PCM_NUM_BYTES_MONO); - decoded_data_size = PCM_NUM_BYTES_MONO; - pcm_in_data_ptrs[AUDIO_CH_L] = decoded_data_mono[AUDIO_CH_L]; - pcm_size_mono = decoded_data_size; - LOG_DBG("Replacing bad mono frame with silence"); - } else { - ret = sw_codec_lc3_dec_run( - encoded_data, encoded_size, LC3_PCM_NUM_BYTES_MONO, 0, - decoded_data_mono[AUDIO_CH_L], - (uint16_t *)&decoded_data_size, bad_frame); - if (ret) { - return ret; - } + uint8_t dec_out_buf[PCM_NUM_BYTES_MONO]; + uint8_t src_buf[PCM_NUM_BYTES_MONO]; + uint8_t *data_in; + uint8_t *dec_out = dec_out_buf; + uint8_t *src_out = src_buf; + uint8_t chan_in, chan_out; + uint8_t chans_out_num; + uint8_t *inter_in = dec_out_buf; + uint16_t bytes_written; + uint32_t loc_in, loc_out; + uint32_t bad_data_mask; + size_t inter_in_size = 0; + + if (meta_in->data_coding != LC3 || meta_out->data_coding != PCM) { + LOG_ERR("LC3 decoder module has incorrect input or output data type: in = " + "%d out = %d", + meta_in->data_coding, meta_out->data_coding); + return -EINVAL; + } - ret = sw_codec_sample_rate_convert( - &decoder_converters[AUDIO_CH_L], - m_config.decoder.sample_rate_hz, - CONFIG_AUDIO_SAMPLE_RATE_HZ, decoded_data_mono[AUDIO_CH_L], - decoded_data_size, - decoded_data_mono_system_sample_rate[AUDIO_CH_L], - &pcm_in_data_ptrs[AUDIO_CH_L], &pcm_size_mono); - if (ret) { - LOG_ERR("Sample rate conversion failed for mono: %d", ret); - return ret; - } - } + if (audio_frame_in->len < + meta_in->bytes_per_location * audio_metadata_num_loc_get(meta_in)) { + LOG_ERR("Decoder input frame too small: %d (< %d)", audio_frame_in->len, + (meta_in->bytes_per_location * + audio_metadata_num_loc_get(meta_in))); + return -EINVAL; + } - /* For now, i2s is only stereo, so in order to send - * just one channel, we need to insert 0 for the - * other channel - */ - ret = pscm_zero_pad(pcm_in_data_ptrs[AUDIO_CH_L], pcm_size_mono, - m_config.decoder.audio_ch, CONFIG_AUDIO_BIT_DEPTH_BITS, - pcm_data_stereo, &pcm_size_stereo); - if (ret) { - return ret; - } - break; + chans_out_num = audio_metadata_num_loc_get(meta_out); + if (audio_frame_out->size < meta_out->bytes_per_location * chans_out_num) { + LOG_ERR("Decoder output buffer too small: %d (<%d for %d channel(s))", + audio_frame_out->size, + (meta_out->bytes_per_location * chans_out_num), chans_out_num); + return -EINVAL; } - case SW_CODEC_STEREO: { - if (bad_frame && IS_ENABLED(CONFIG_SW_CODEC_OVERRIDE_PLC)) { - memset(decoded_data_mono[AUDIO_CH_L], 0, PCM_NUM_BYTES_MONO); - memset(decoded_data_mono[AUDIO_CH_R], 0, PCM_NUM_BYTES_MONO); - decoded_data_size = PCM_NUM_BYTES_MONO; - pcm_in_data_ptrs[AUDIO_CH_L] = decoded_data_mono[AUDIO_CH_L]; - pcm_in_data_ptrs[AUDIO_CH_R] = decoded_data_mono[AUDIO_CH_R]; - pcm_size_mono = decoded_data_size; - LOG_DBG("Replacing bad stereo frame with silence"); + + /* If input is only one channel and not mono_audio and the output + * is mono_audio we will set loc_out to 0xFFFF to accept any location + */ + if (meta_in->locations != BT_AUDIO_LOCATION_MONO_AUDIO && + meta_out->locations == BT_AUDIO_LOCATION_MONO_AUDIO && + audio_metadata_num_loc_get(meta_in) == 1) { + loc_out = 0xFFFF; + loc_in = meta_in->locations; + } else if (meta_out->locations == BT_AUDIO_LOCATION_MONO_AUDIO && + meta_in->locations == BT_AUDIO_LOCATION_MONO_AUDIO) { + loc_in = 1; + loc_out = 1; + } else if (meta_out->locations & meta_in->locations) { + loc_in = meta_in->locations; + loc_out = meta_out->locations; + } else { + LOG_ERR("No common output location with input"); + return -EINVAL; + } + + if (!meta_out->interleaved) { + if (IS_ENABLED(CONFIG_SAMPLE_RATE_CONVERTER) && + meta_in->sample_rate_hz != meta_out->sample_rate_hz) { + src_out = (uint8_t *)audio_frame_out->data; } else { - /* Decode left channel */ - ret = sw_codec_lc3_dec_run( - encoded_data, encoded_size / 2, LC3_PCM_NUM_BYTES_MONO, - AUDIO_CH_L, decoded_data_mono[AUDIO_CH_L], - (uint16_t *)&decoded_data_size, bad_frame); - if (ret) { - return ret; - } + dec_out = (uint8_t *)audio_frame_out->data; + } + } - /* Decode right channel */ - ret = sw_codec_lc3_dec_run( - (encoded_data + (encoded_size / 2)), encoded_size / 2, - LC3_PCM_NUM_BYTES_MONO, AUDIO_CH_R, - decoded_data_mono[AUDIO_CH_R], - (uint16_t *)&decoded_data_size, bad_frame); - if (ret) { - return ret; - } + /* Clear all output channels to ensure any unused are zero */ + memset(audio_frame_out->data, 0, audio_frame_out->size); + + chan_in = 0; + chan_out = 0; + bad_data_mask = 0x01; + + /* Decode only the channel(s) common between the input and output locations. + * These will be put in the first channel or channels and the location + * will indicate which channel(s) they are. Prior to playout (I2S or TDM) + * all other channels can be zeroed. + */ + while (loc_in && loc_out) { + if (loc_out & loc_in & 0x01) { + data_in = (uint8_t *)audio_frame_in->data + + (meta_in->bytes_per_location * chan_in); - /* Stereo always initializes both slots consumed by pscm_combine(). */ - for (int i = 0; i < AUDIO_CH_NUM; ++i) { - ret = sw_codec_sample_rate_convert( - &decoder_converters[i], - m_config.decoder.sample_rate_hz, - CONFIG_AUDIO_SAMPLE_RATE_HZ, decoded_data_mono[i], - decoded_data_size, - decoded_data_mono_system_sample_rate[i], - &pcm_in_data_ptrs[i], &pcm_size_mono); - if (ret) { - LOG_ERR("Sample rate conversion failed for channel " - "%d : %d", - i, ret); - return ret; + ret = sw_codec_lc3_dec_run(data_in, meta_in->bytes_per_location, + audio_frame_out->size, chan_in, dec_out, + &bytes_written, + (meta_in->bad_data & bad_data_mask)); + ERR_CHK_MSG(ret, "Decode failed"); + + ret = sw_codec_sample_rate_convert( + &decoder_converters[chan_in], meta_in->sample_rate_hz, + meta_out->sample_rate_hz, dec_out, bytes_written, src_out, + (char **)&inter_in, &inter_in_size); + ERR_CHK_MSG(ret, "Decode: Sample rate converter failed"); + + if (meta_out->interleaved) { + ret = pscm_interleave(inter_in, inter_in_size, chan_out, + meta_out->carried_bits_per_sample, + audio_frame_out->data, + audio_frame_out->size, chans_out_num); + ERR_CHK_MSG(ret, "Decode: Interleave failed"); + } else { + if (IS_ENABLED(CONFIG_SAMPLE_RATE_CONVERTER) && + meta_in->sample_rate_hz != meta_out->sample_rate_hz) { + src_out += inter_in_size; + } else { + dec_out += inter_in_size; } } } - ret = pscm_combine(pcm_in_data_ptrs[AUDIO_CH_L], - pcm_in_data_ptrs[AUDIO_CH_R], pcm_size_mono, - CONFIG_AUDIO_BIT_DEPTH_BITS, pcm_data_stereo, - &pcm_size_stereo); - if (ret) { - return ret; - } - break; - } - default: - LOG_ERR("Unsupported channel mode for decoder: %d", - m_config.decoder.channel_mode); - return -ENODEV; + bad_data_mask <<= 1; + + chan_in += loc_in & 0x01; + chan_out += loc_out & 0x01; + + loc_in >>= 1; + loc_out >>= 1; } - *decoded_size = pcm_size_stereo; - *decoded_data = pcm_data_stereo; + meta_out->bytes_per_location = inter_in_size; + net_buf_add(audio_frame_out, + meta_out->bytes_per_location * audio_metadata_num_loc_get(meta_out)); + #endif /* (CONFIG_SW_CODEC_LC3) */ break; } default: - LOG_ERR("Unsupported codec: %d", m_config.sw_codec); + LOG_ERR("Unsupported codec: %d", meta_in->data_coding); return -ENODEV; } + return 0; } @@ -376,6 +397,7 @@ int sw_codec_uninit(struct sw_codec_config sw_codec_cfg) LOG_ERR("Trying to uninit a codec that is not first initialized"); return -ENODEV; } + switch (m_config.sw_codec) { case SW_CODEC_LC3: #if (CONFIG_SW_CODEC_LC3) @@ -385,10 +407,12 @@ int sw_codec_uninit(struct sw_codec_config sw_codec_cfg) "initialized"); return -EALREADY; } + ret = sw_codec_lc3_enc_uninit_all(); if (ret) { return ret; } + m_config.encoder.enabled = false; } @@ -403,8 +427,14 @@ int sw_codec_uninit(struct sw_codec_config sw_codec_cfg) if (ret) { return ret; } + m_config.decoder.enabled = false; } + + ret = sw_codec_lc3_uninit(); + if (ret) { + return ret; + } #endif /* (CONFIG_SW_CODEC_LC3) */ break; default: @@ -424,9 +454,23 @@ int sw_codec_init(struct sw_codec_config sw_codec_cfg) switch (sw_codec_cfg.sw_codec) { case SW_CODEC_LC3: { #if (CONFIG_SW_CODEC_LC3) - if (m_config.sw_codec != SW_CODEC_LC3) { + if (!m_config.initialized) { /* Check if LC3 is already initialized */ - ret = sw_codec_lc3_init(NULL, NULL, CONFIG_AUDIO_FRAME_DURATION_US); + uint16_t encoder_sample_rate = 0; + uint16_t decoder_sample_rate = 0; + + if (sw_codec_cfg.encoder.enabled) { + encoder_sample_rate = sw_codec_cfg.encoder.sample_rate_hz; + } + + if (sw_codec_cfg.decoder.enabled) { + decoder_sample_rate = sw_codec_cfg.decoder.sample_rate_hz; + } + + ret = sw_codec_lc3_single_rate_init(encoder_sample_rate, + decoder_sample_rate, + NULL, NULL, + CONFIG_AUDIO_FRAME_DURATION_US); if (ret) { return ret; } @@ -437,6 +481,7 @@ int sw_codec_init(struct sw_codec_config sw_codec_cfg) LOG_WRN("The LC3 encoder is already initialized"); return -EALREADY; } + uint16_t pcm_bytes_req_enc; LOG_DBG("Encode: %dHz %dbits %dus %dbps %d channel(s)", @@ -448,7 +493,6 @@ int sw_codec_init(struct sw_codec_config sw_codec_cfg) sw_codec_cfg.encoder.sample_rate_hz, CONFIG_AUDIO_BIT_DEPTH_BITS, CONFIG_AUDIO_FRAME_DURATION_US, sw_codec_cfg.encoder.bitrate, sw_codec_cfg.encoder.num_ch, &pcm_bytes_req_enc); - if (ret) { return ret; } @@ -467,7 +511,6 @@ int sw_codec_init(struct sw_codec_config sw_codec_cfg) ret = sw_codec_lc3_dec_init( sw_codec_cfg.decoder.sample_rate_hz, CONFIG_AUDIO_BIT_DEPTH_BITS, CONFIG_AUDIO_FRAME_DURATION_US, sw_codec_cfg.decoder.num_ch); - if (ret) { return ret; } @@ -485,12 +528,14 @@ int sw_codec_init(struct sw_codec_config sw_codec_cfg) return false; } - if (sw_codec_cfg.encoder.enabled && IS_ENABLED(SAMPLE_RATE_CONVERTER)) { - int converter_count = (sw_codec_cfg.encoder.channel_mode == SW_CODEC_MONO) ? - AUDIO_CH_NUM : - sw_codec_cfg.encoder.channel_mode; + if (sw_codec_cfg.encoder.num_ch > CONFIG_AUDIO_ENCODE_CHANNELS_MAX) { + LOG_ERR("Number of encoder channels %d exceeds max %d", sw_codec_cfg.encoder.num_ch, + CONFIG_AUDIO_ENCODE_CHANNELS_MAX); + return -EINVAL; + } - for (int i = 0; i < converter_count; i++) { + if (sw_codec_cfg.encoder.enabled && IS_ENABLED(CONFIG_SAMPLE_RATE_CONVERTER)) { + for (int i = 0; i < sw_codec_cfg.encoder.num_ch; i++) { ret = sample_rate_converter_open(&encoder_converters[i]); if (ret) { LOG_ERR("Failed to initialize the sample rate converter for " @@ -501,8 +546,14 @@ int sw_codec_init(struct sw_codec_config sw_codec_cfg) } } - if (sw_codec_cfg.decoder.enabled && IS_ENABLED(SAMPLE_RATE_CONVERTER)) { - for (int i = 0; i < sw_codec_cfg.decoder.channel_mode; i++) { + if (sw_codec_cfg.decoder.num_ch > CONFIG_AUDIO_DECODE_CHANNELS_MAX) { + LOG_ERR("Number of decoder channels %d exceeds max %d", sw_codec_cfg.decoder.num_ch, + CONFIG_AUDIO_DECODE_CHANNELS_MAX); + return -EINVAL; + } + + if (sw_codec_cfg.decoder.enabled && IS_ENABLED(CONFIG_SAMPLE_RATE_CONVERTER)) { + for (int i = 0; i < sw_codec_cfg.decoder.num_ch; i++) { ret = sample_rate_converter_open(&decoder_converters[i]); if (ret) { LOG_ERR("Failed to initialize the sample rate converter for " @@ -516,7 +567,5 @@ int sw_codec_init(struct sw_codec_config sw_codec_cfg) m_config = sw_codec_cfg; m_config.initialized = true; - LOG_INF("Microphone channel set to %d", m_config.encoder.audio_ch); - return 0; } diff --git a/src/audio/sw_codec_select.h b/src/audio/sw_codec_select.h index 6240a7f7..eba13d11 100644 --- a/src/audio/sw_codec_select.h +++ b/src/audio/sw_codec_select.h @@ -4,18 +4,33 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @defgroup audio_app_sw_codec_select Audio Software Codec Selection + * @{ + * @brief Software codec selection and configuration API for Audio applications. + * + * This module provides the software codec interface for audio encoding and decoding operations. + * It supports the LC3 (Low Complexity Communication Codec) as specified in the + * Bluetooth LE Audio standard. The module handles codec initialization and codec configuration. + * It provides encoding/decoding functions with support for different sample rates, + * bitrates, and channel modes (mono/stereo). It integrates with @ref audio_app_system to + * process audio data for both unicast (CIS) and broadcast (BIS) streaming modes. + */ + #ifndef _SW_CODEC_SELECT_H_ #define _SW_CODEC_SELECT_H_ #include -#include "channel_assignment.h" +#include +#include "device_location.h" #if (CONFIG_SW_CODEC_LC3) -#define LC3_MAX_FRAME_SIZE_MS 10 -#define LC3_ENC_MONO_FRAME_SIZE (CONFIG_LC3_BITRATE_MAX * LC3_MAX_FRAME_SIZE_MS / (8 * 1000)) - +#define LC3_MAX_FRAME_SIZE_US 10000 +#define LC3_ENC_MONO_FRAME_SIZE \ + (CONFIG_LC3_BITRATE_MAX * LC3_MAX_FRAME_SIZE_US / (8 * USEC_PER_SEC)) #define LC3_PCM_NUM_BYTES_MONO \ - (CONFIG_AUDIO_SAMPLE_RATE_HZ * CONFIG_AUDIO_BIT_DEPTH_OCTETS * LC3_MAX_FRAME_SIZE_MS / 1000) + (CONFIG_AUDIO_SAMPLE_RATE_HZ * CONFIG_AUDIO_BIT_DEPTH_OCTETS * LC3_MAX_FRAME_SIZE_US / \ + USEC_PER_SEC) #define LC3_ENC_TIME_US 3000 #define LC3_DEC_TIME_US 1500 #else @@ -26,47 +41,69 @@ #endif /* CONFIG_SW_CODEC_LC3 */ /* Max will be used when multiple codecs are supported */ -#define ENC_MAX_FRAME_SIZE MAX(LC3_ENC_MONO_FRAME_SIZE, 0) -#define ENC_TIME_US MAX(LC3_ENC_TIME_US, 0) -#define DEC_TIME_US MAX(LC3_DEC_TIME_US, 0) -#define PCM_NUM_BYTES_MONO MAX(LC3_PCM_NUM_BYTES_MONO, 0) -#define PCM_NUM_BYTES_STEREO (PCM_NUM_BYTES_MONO * 2) +#define ENC_MAX_FRAME_SIZE MAX(LC3_ENC_MONO_FRAME_SIZE, 0) +#define ENC_MULTI_CHAN_MAX_FRAME_SIZE (LC3_ENC_MONO_FRAME_SIZE * CONFIG_AUDIO_ENCODE_CHANNELS_MAX) +#define ENC_TIME_US MAX(LC3_ENC_TIME_US, 0) +#define DEC_TIME_US MAX(LC3_DEC_TIME_US, 0) +#define PCM_NUM_BYTES_MONO MAX(LC3_PCM_NUM_BYTES_MONO, 0) +#define PCM_NUM_BYTES_MULTI_CHAN \ + (PCM_NUM_BYTES_MONO * MAX(CONFIG_AUDIO_DECODE_CHANNELS_MAX, CONFIG_AUDIO_OUTPUT_CHANNELS)) +/** + * @brief Software codec selection enumeration. + */ enum sw_codec_select { - SW_CODEC_NONE, - SW_CODEC_LC3, /* Low Complexity Communication Codec */ + SW_CODEC_NONE, /**< No codec selected */ + SW_CODEC_LC3, /**< Low Complexity Communication Codec */ }; -enum sw_codec_channel_mode { - SW_CODEC_MONO = 1, - SW_CODEC_STEREO, +/** + * @brief Private encoder context. + */ +struct lc3_encoder_context { + /* Array of encoder channel handles. */ + struct lc3_encoder_handle *lc3_enc_channel[CONFIG_AUDIO_ENCODE_CHANNELS_MAX]; +}; + +/** + * @brief Private decoder context. + */ +struct lc3_decoder_context { + /* Array of decoder channel handles. */ + struct lc3_decoder_handle *lc3_dec_channel[CONFIG_AUDIO_DECODE_CHANNELS_MAX]; }; +/** + * @brief Software codec encoder configuration structure. + */ struct sw_codec_encoder { bool enabled; int bitrate; - enum sw_codec_channel_mode channel_mode; uint8_t num_ch; - enum audio_channel audio_ch; + enum bt_audio_location audio_loc; uint32_t sample_rate_hz; + struct lc3_encoder_context lc3_ctx; }; +/** + * @brief Software codec decoder configuration structure. + */ struct sw_codec_decoder { bool enabled; - enum sw_codec_channel_mode channel_mode; /* Mono or stereo. */ - uint8_t num_ch; /* Number of decoder channels. */ - enum audio_channel audio_ch; /* Used to choose which channel to use. */ + uint8_t num_ch; + enum bt_audio_location audio_loc; uint32_t sample_rate_hz; + struct lc3_decoder_context lc3_ctx; }; /** * @brief Sw_codec configuration structure. */ struct sw_codec_config { - enum sw_codec_select sw_codec; /* sw_codec to be used, e.g. LC3, etc. */ - struct sw_codec_decoder decoder; /* Struct containing settings for decoder. */ - struct sw_codec_encoder encoder; /* Struct containing settings for encoder. */ - bool initialized; /* Status of codec. */ + enum sw_codec_select sw_codec; + struct sw_codec_decoder decoder; + struct sw_codec_encoder encoder; + bool initialized; }; /** @@ -77,43 +114,32 @@ struct sw_codec_config { */ bool sw_codec_is_initialized(void); -/** - * @brief Set the encoder input channel used for mono encoding. - * - * @param[in] channel Audio channel to encode. - * - * @return 0 on success, error otherwise. - */ -int sw_codec_encoder_channel_set(enum audio_channel channel); - /** * @brief Encode PCM data and output encoded data. * - * @note Takes in stereo PCM stream, will encode either one or two - * channels, based on channel_mode set during init. + * @note Takes in a PCM stream, will encode either one or multiple + * channels, based on num_ch set during init. * - * @param[in] pcm_data Pointer to PCM data. - * @param[in] pcm_size Size of PCM data. - * @param[out] encoded_data Pointer to buffer to store encoded data. - * @param[out] encoded_size Size of encoded data. + * @param[in] audio_frame_in Pointer to the audio PCM buffer. + * @param[out] audio_frame_out Pointer to the audio encoded buffer. * * @return 0 if success, error codes depends on sw_codec selected. */ -int sw_codec_encode(void *pcm_data, size_t pcm_size, uint8_t **encoded_data, size_t *encoded_size); +int sw_codec_encode(struct net_buf *audio_frame_in, struct net_buf *audio_frame_out); /** * @brief Decode encoded data and output PCM data. * - * @param[in] encoded_data Pointer to encoded data. - * @param[in] encoded_size Size of encoded data. - * @param[in] bad_frame Flag to indicate a missing/bad frame (only LC3). - * @param[out] pcm_data Pointer to buffer to store decoded PCM data. - * @param[out] pcm_size Size of decoded data. + * @note Takes in a coded bitstream, will decode either one or multiple + * channels, based on num_ch set during init. + * + * @param[in] audio_frame_in Pointer to the audio input buffer. + * @param[out] audio_frame_out Pointer to the audio output buffer. * * @return 0 if success, error codes depends on sw_codec selected. */ -int sw_codec_decode(uint8_t const *const encoded_data, size_t encoded_size, bool bad_frame, - void **pcm_data, size_t *pcm_size); +int sw_codec_decode(struct net_buf const *const audio_frame_in, + struct net_buf *const audio_frame_out); /** * @brief Uninitialize the software codec and free the allocated space. @@ -137,4 +163,8 @@ int sw_codec_uninit(struct sw_codec_config sw_codec_cfg); */ int sw_codec_init(struct sw_codec_config sw_codec_cfg); +/** + * @} + */ + #endif /* _SW_CODEC_SELECT_H_ */ diff --git a/src/bluetooth/Kconfig b/src/bluetooth/Kconfig index 0bafa549..9f7d43f6 100644 --- a/src/bluetooth/Kconfig +++ b/src/bluetooth/Kconfig @@ -38,47 +38,98 @@ if TRANSPORT_CIS rsource "bt_stream/unicast/Kconfig" endif # TRANSPORT_CIS +rsource "bt_stream/bt_le_audio_tx/Kconfig" + config BT_AUDIO_PACKING_INTERLEAVED bool "Interleaved packing" default n help ISO channels can either be interleaved or sequentially packed; sequential is the default one. -config BT_AUDIO_PREF_SAMPLE_RATE_VALUE +config BT_AUDIO_CONCURRENT_RX_STREAMS_MAX + int "Max number of concurrent streams" + range 1 32 + default 1 + help + The maximum number of concurrent streams supported in the RX direction. + This is the maximum number of streams that can be active at the same time. + +config BT_AUDIO_CONCURRENT_TX_STREAMS_MAX + int "Max number of concurrent streams" + range 1 32 + default 1 + help + The maximum number of concurrent streams supported in the TX direction. + This is the maximum number of streams that can be active at the same time. + +config BT_AUDIO_PREF_SINK_SAMPLE_RATE_VALUE + hex + default 0x03 if BT_AUDIO_PREF_SINK_SAMPLE_RATE_16KHZ + default 0x05 if BT_AUDIO_PREF_SINK_SAMPLE_RATE_24KHZ + default 0x08 if BT_AUDIO_PREF_SINK_SAMPLE_RATE_48KHZ + +choice BT_AUDIO_PREF_SINK_SAMPLE_RATE + prompt "Preferred BT audio sink sample rate" + default BT_AUDIO_PREF_SINK_SAMPLE_RATE_16KHZ if BT_BAP_BROADCAST_16_2_1 + default BT_AUDIO_PREF_SINK_SAMPLE_RATE_16KHZ if BT_BAP_BROADCAST_16_2_2 + default BT_AUDIO_PREF_SINK_SAMPLE_RATE_16KHZ if BT_BAP_UNICAST_16_2_1 + default BT_AUDIO_PREF_SINK_SAMPLE_RATE_24KHZ if STREAM_BIDIRECTIONAL + default BT_AUDIO_PREF_SINK_SAMPLE_RATE_24KHZ if BT_BAP_BROADCAST_24_2_1 + default BT_AUDIO_PREF_SINK_SAMPLE_RATE_24KHZ if BT_BAP_BROADCAST_24_2_2 + default BT_AUDIO_PREF_SINK_SAMPLE_RATE_24KHZ if BT_BAP_UNICAST_24_2_1 + default BT_AUDIO_PREF_SINK_SAMPLE_RATE_48KHZ + help + Select the preferred sample rate to stream if there are more than one to choose from. + Only valid when used by unicast_client if CONFIG_SAMPLE_RATE_CONVERTER=y and + CONFIG_AUDIO_SAMPLE_RATE_48000_HZ=y, meaning 16, 24, and 48kHz are supported. + +config BT_AUDIO_PREF_SINK_SAMPLE_RATE_48KHZ + bool "48 kHz" + help + Select 48000 Hz as the preferred sink sample rate. + +config BT_AUDIO_PREF_SINK_SAMPLE_RATE_24KHZ + bool "24 kHz" + help + Select 24000 Hz as the preferred sink sample rate. + +config BT_AUDIO_PREF_SINK_SAMPLE_RATE_16KHZ + bool "16 kHz" + help + Select 16000 Hz as the preferred sink sample rate. +endchoice + +config BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_VALUE hex - default 0x03 if BT_AUDIO_PREF_SAMPLE_RATE_16KHZ - default 0x05 if BT_AUDIO_PREF_SAMPLE_RATE_24KHZ - default 0x08 if BT_AUDIO_PREF_SAMPLE_RATE_48KHZ - -choice BT_AUDIO_PREF_SAMPLE_RATE - prompt "Preferred BT audio sample rate" - default BT_AUDIO_PREF_SAMPLE_RATE_16KHZ if BT_BAP_BROADCAST_16_2_1 - default BT_AUDIO_PREF_SAMPLE_RATE_16KHZ if BT_BAP_BROADCAST_16_2_2 - default BT_AUDIO_PREF_SAMPLE_RATE_16KHZ if BT_BAP_UNICAST_16_2_1 - default BT_AUDIO_PREF_SAMPLE_RATE_24KHZ if STREAM_BIDIRECTIONAL - default BT_AUDIO_PREF_SAMPLE_RATE_24KHZ if BT_BAP_BROADCAST_24_2_1 - default BT_AUDIO_PREF_SAMPLE_RATE_24KHZ if BT_BAP_BROADCAST_24_2_2 - default BT_AUDIO_PREF_SAMPLE_RATE_24KHZ if BT_BAP_UNICAST_24_2_1 - default BT_AUDIO_PREF_SAMPLE_RATE_48KHZ + default 0x03 if BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_16KHZ + default 0x05 if BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_24KHZ + default 0x08 if BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_48KHZ + +choice BT_AUDIO_PREF_SOURCE_SAMPLE_RATE + prompt "Preferred BT audio source sample rate" + default BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_16KHZ if BT_BAP_UNICAST_16_2_1 + default BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_24KHZ if STREAM_BIDIRECTIONAL + default BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_24KHZ if BT_BAP_UNICAST_24_2_1 + default BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_48KHZ help Select the preferred sample rate to stream if there are more than one to choose from. Only valid when used by unicast_client if CONFIG_SAMPLE_RATE_CONVERTER=y and CONFIG_AUDIO_SAMPLE_RATE_48000_HZ=y, meaning 16, 24, and 48kHz are supported. -config BT_AUDIO_PREF_SAMPLE_RATE_48KHZ +config BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_48KHZ bool "48 kHz" help - Select 48000 Hz as the preferred sample rate. + Select 48000 Hz as the preferred source sample rate. -config BT_AUDIO_PREF_SAMPLE_RATE_24KHZ +config BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_24KHZ bool "24 kHz" help - Select 24000 Hz as the preferred sample rate. + Select 24000 Hz as the preferred source sample rate. -config BT_AUDIO_PREF_SAMPLE_RATE_16KHZ +config BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_16KHZ bool "16 kHz" help - Select 16000 Hz as the preferred sample rate. + Select 16000 Hz as the preferred source sample rate. endchoice #----------------------------------------------------------------------------# diff --git a/src/bluetooth/Kconfig.defaults b/src/bluetooth/Kconfig.defaults index 73a48281..f1af856b 100644 --- a/src/bluetooth/Kconfig.defaults +++ b/src/bluetooth/Kconfig.defaults @@ -9,7 +9,7 @@ config BT_AUDIO config BT_DEVICE_NAME default BT_AUDIO_BROADCAST_NAME if TRANSPORT_BIS - default "NRF5340_AUDIO" + default "NRF_AUDIO" config BT_DEVICE_NAME_DYNAMIC default y diff --git a/src/bluetooth/bt_content_control/CMakeLists.txt b/src/bluetooth/bt_content_control/CMakeLists.txt index 2d5d99d5..5fc052af 100644 --- a/src/bluetooth/bt_content_control/CMakeLists.txt +++ b/src/bluetooth/bt_content_control/CMakeLists.txt @@ -5,13 +5,11 @@ # zephyr_library_include_directories( - media + media ) -target_sources(app PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/bt_content_ctrl.c) +target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/bt_content_ctrl.c) -if (CONFIG_BT_MCC OR CONFIG_BT_MCS) -target_sources(app PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/media/bt_content_ctrl_media.c) +if(CONFIG_BT_MCC OR CONFIG_BT_MCS) + target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/media/bt_content_ctrl_media.c) endif() diff --git a/src/bluetooth/bt_content_control/bt_content_ctrl.c b/src/bluetooth/bt_content_control/bt_content_ctrl.c index 858d0a1d..b70cea94 100644 --- a/src/bluetooth/bt_content_control/bt_content_ctrl.c +++ b/src/bluetooth/bt_content_control/bt_content_ctrl.c @@ -31,7 +31,9 @@ static void media_control_cb(bool play) } ret = zbus_chan_pub(&cont_media_chan, &msg, K_NO_WAIT); - ERR_CHK_MSG(ret, "zbus publication failed"); + if (ret) { + LOG_ERR("zbus publication failed: %d", ret); + } } int bt_content_ctrl_start(struct bt_conn *conn) @@ -52,7 +54,9 @@ int bt_content_ctrl_start(struct bt_conn *conn) msg.event = MEDIA_START; ret = zbus_chan_pub(&cont_media_chan, &msg, K_NO_WAIT); - ERR_CHK_MSG(ret, "zbus publication failed"); + if (ret) { + LOG_ERR("zbus publication failed: %d", ret); + } return 0; } @@ -75,11 +79,22 @@ int bt_content_ctrl_stop(struct bt_conn *conn) msg.event = MEDIA_STOP; ret = zbus_chan_pub(&cont_media_chan, &msg, K_NO_WAIT); - ERR_CHK_MSG(ret, "zbus publication failed"); + if (ret) { + LOG_ERR("zbus publication failed: %d", ret); + } return 0; } +int bt_content_ctrl_next_track(struct bt_conn *conn) +{ + if (!IS_ENABLED(CONFIG_BT_MCC) && !IS_ENABLED(CONFIG_BT_MCS)) { + return -ENOTSUP; + } + + return bt_content_ctrl_media_next_track(conn); +} + int bt_content_ctrl_conn_disconnected(struct bt_conn *conn) { int ret; diff --git a/src/bluetooth/bt_content_control/bt_content_ctrl.h b/src/bluetooth/bt_content_control/bt_content_ctrl.h index 805dedc8..9ac0385f 100644 --- a/src/bluetooth/bt_content_control/bt_content_ctrl.h +++ b/src/bluetooth/bt_content_control/bt_content_ctrl.h @@ -4,6 +4,12 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @defgroup audio_app_bt_content_ctrl Functions for content control. + * @{ + * @brief Helper functions to manage content control. + */ + #ifndef _BT_CONTENT_CTRL_H_ #define _BT_CONTENT_CTRL_H_ @@ -27,6 +33,9 @@ int bt_content_ctrl_start(struct bt_conn *conn); */ int bt_content_ctrl_stop(struct bt_conn *conn); +/** Send next-track to the media player; NULL targets all discovered peers. */ +int bt_content_ctrl_next_track(struct bt_conn *conn); + /** * @brief Handle disconnected connection for the content control services. * @@ -71,4 +80,8 @@ bool bt_content_ctlr_media_state_playing(void); */ int bt_content_ctrl_init(void); +/** + * @} + */ + #endif /* _BT_CONTENT_CTRL_H_ */ diff --git a/src/bluetooth/bt_content_control/media/Kconfig b/src/bluetooth/bt_content_control/media/Kconfig index eb8881f2..bc28592b 100644 --- a/src/bluetooth/bt_content_control/media/Kconfig +++ b/src/bluetooth/bt_content_control/media/Kconfig @@ -13,5 +13,18 @@ module = BT_CONTENT_CTRL_MEDIA module-str = bt_content_ctrl_media source "subsys/logging/Kconfig.template.log_config" +config BT_CONTENT_CTRL_MEDIA + bool "Bluetooth content control media" + select BT_MCC if BT_BAP_UNICAST_SERVER + select BT_MCC_READ_MEDIA_STATE if BT_BAP_UNICAST_SERVER + select BT_MCC_SET_MEDIA_CONTROL_POINT if BT_BAP_UNICAST_SERVER + select BT_MCS if BT_BAP_UNICAST_CLIENT + select BT_MPL if BT_BAP_UNICAST_CLIENT + select MCTL if BT_BAP_UNICAST_CLIENT + select MCTL_LOCAL_PLAYER_CONTROL if BT_BAP_UNICAST_CLIENT + select MCTL_LOCAL_PLAYER_REMOTE_CONTROL if BT_BAP_UNICAST_CLIENT + help + Bluetooth content control media module. + endmenu # Log level endmenu # Media diff --git a/src/bluetooth/bt_content_control/media/bt_content_ctrl_media.c b/src/bluetooth/bt_content_control/media/bt_content_ctrl_media.c index f1b5e3e3..9e3db1f8 100644 --- a/src/bluetooth/bt_content_control/media/bt_content_ctrl_media.c +++ b/src/bluetooth/bt_content_control/media/bt_content_ctrl_media.c @@ -454,6 +454,16 @@ int bt_content_ctrl_media_pause(struct bt_conn *conn) return 0; } +int bt_content_ctrl_media_next_track(struct bt_conn *conn) +{ + struct mpl_cmd cmd = { + .opcode = BT_MCS_OPC_NEXT_TRACK, + .use_param = false, + }; + + return mpl_cmd_send(conn, &cmd); +} + bool bt_content_ctlr_media_state_playing(void) { if (media_player_state == BT_MCS_MEDIA_STATE_PLAYING) { diff --git a/src/bluetooth/bt_content_control/media/bt_content_ctrl_media_internal.h b/src/bluetooth/bt_content_control/media/bt_content_ctrl_media_internal.h index fdda2621..730eba4f 100644 --- a/src/bluetooth/bt_content_control/media/bt_content_ctrl_media_internal.h +++ b/src/bluetooth/bt_content_control/media/bt_content_ctrl_media_internal.h @@ -62,6 +62,9 @@ int bt_content_ctrl_media_play(struct bt_conn *conn); */ int bt_content_ctrl_media_pause(struct bt_conn *conn); +/** Send next-track; NULL targets all discovered media-control peers. */ +int bt_content_ctrl_media_next_track(struct bt_conn *conn); + /** * @brief Reset the media control peer's discovered state * diff --git a/src/bluetooth/bt_management/CMakeLists.txt b/src/bluetooth/bt_management/CMakeLists.txt index 995b7118..3eb5da5f 100644 --- a/src/bluetooth/bt_management/CMakeLists.txt +++ b/src/bluetooth/bt_management/CMakeLists.txt @@ -14,12 +14,17 @@ zephyr_library_include_directories( target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/bt_mgmt.c - ${CMAKE_CURRENT_SOURCE_DIR}/bt_mgmt_conn_interval.cpp ${CMAKE_CURRENT_SOURCE_DIR}/controller_config/bt_mgmt_ctlr_cfg.c + ${CMAKE_CURRENT_SOURCE_DIR}/bt_mgmt_conn_interval.cpp ${CMAKE_CURRENT_SOURCE_DIR}/conn_interval/conn_intvl_exp_backoff_lin_inc.cpp ${CMAKE_CURRENT_SOURCE_DIR}/conn_interval/conn_intvl_linear.cpp ) +if (CONFIG_BT_SETTINGS) + target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/bt_mgmt_bond_storage.c) + zephyr_ld_options(-Wl,--wrap=bt_settings_store_keys) +endif() + if (CONFIG_BT_CENTRAL) target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/scanning/bt_mgmt_scan_for_conn.c) diff --git a/src/bluetooth/bt_management/Kconfig b/src/bluetooth/bt_management/Kconfig index b2313dd1..fa299800 100644 --- a/src/bluetooth/bt_management/Kconfig +++ b/src/bluetooth/bt_management/Kconfig @@ -8,26 +8,21 @@ menu "BT management" rsource "controller_config/Kconfig" -#----------------------------------------------------------------------------# config WDT_CTLR bool "Enable watchdog for controller" default y + select WATCHDOG + select TASK_WDT help - When true, the controller will be polled at regular intervals to check that it is alive. - Turn off to reduce overhead, or HCI traffic. - The watchdog will be deactivated automatically for DFU procedures. - -menu "Thread priorities" + Poll the controller every two seconds to detect an IPC or controller stall. + The watchdog is stopped before shutdown or entering standalone DFU mode. config CTLR_POLL_WORK_Q_PRIO int "Work queue priority for controller poll" default 2 - help - This is a preemptible work queue. - This work queue will poll the controller to check it is alive. - -endmenu # Thread priorities + depends on WDT_CTLR +#----------------------------------------------------------------------------# rsource "dfu/Kconfig" rsource "advertising/Kconfig" rsource "scanning/Kconfig" diff --git a/src/bluetooth/bt_management/advertising/Kconfig b/src/bluetooth/bt_management/advertising/Kconfig index 8f895f3c..65e2b2ef 100644 --- a/src/bluetooth/bt_management/advertising/Kconfig +++ b/src/bluetooth/bt_management/advertising/Kconfig @@ -67,6 +67,18 @@ config BLE_ACL_ADV_SID range 0x00 0x0F default 0x00 +config BT_MGMT_ADV_STACK_SIZE + int "Stack size" + default 1024 + help + Stack size for the Bluetooth management advertising thread. + +config BT_MGMT_ADV_WQ_PRIO + int "Work queue priority for bt_mgmt advertising" + default 5 + help + This work queue will subscribe to BT management advertising work items. + #----------------------------------------------------------------------------# menu "Log level" diff --git a/src/bluetooth/bt_management/advertising/bt_mgmt_adv.c b/src/bluetooth/bt_management/advertising/bt_mgmt_adv.c index d43317b4..22fac72d 100644 --- a/src/bluetooth/bt_management/advertising/bt_mgmt_adv.c +++ b/src/bluetooth/bt_management/advertising/bt_mgmt_adv.c @@ -4,479 +4,481 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ - #include "bt_mgmt.h" - - #include - #include - #include - - #include "macros_common.h" - #include "zbus_common.h" - - #include - LOG_MODULE_REGISTER(bt_mgmt_adv, CONFIG_BT_MGMT_ADV_LOG_LEVEL); - - ZBUS_CHAN_DECLARE(bt_mgmt_chan); - - #ifndef CONFIG_BT_MAX_PAIRED - #define BONDS_QUEUE_SIZE 0 - #else - #define BONDS_QUEUE_SIZE CONFIG_BT_MAX_PAIRED - #endif - - static struct k_work adv_work; - static bool dir_adv_timed_out; - static struct bt_le_ext_adv *ext_adv[CONFIG_BT_EXT_ADV_MAX_ADV_SET]; - - static const struct bt_data *adv_local[CONFIG_BT_EXT_ADV_MAX_ADV_SET]; - static size_t adv_local_size[CONFIG_BT_EXT_ADV_MAX_ADV_SET]; - static const struct bt_data *per_adv_local[CONFIG_BT_EXT_ADV_MAX_ADV_SET]; - static size_t per_adv_local_size[CONFIG_BT_EXT_ADV_MAX_ADV_SET]; - - /* Bonded address queue */ - K_MSGQ_DEFINE(bonds_queue, sizeof(bt_addr_le_t), BONDS_QUEUE_SIZE, 4); - K_MSGQ_DEFINE(adv_queue, sizeof(uint8_t), CONFIG_BT_EXT_ADV_MAX_ADV_SET, 4); - - static struct bt_le_adv_param ext_adv_param = { - .id = BT_ID_DEFAULT, - .sid = CONFIG_BLE_ACL_ADV_SID, - .secondary_max_skip = 0, - .options = BT_LE_ADV_OPT_EXT_ADV | BT_LE_ADV_OPT_USE_NAME, - .interval_min = CONFIG_BLE_ACL_EXT_ADV_INT_MIN, - .interval_max = CONFIG_BLE_ACL_EXT_ADV_INT_MAX, - .peer = NULL, - }; - +#include "bt_mgmt.h" + +#include +#include +#include + +#include "macros_common.h" +#include "zbus_common.h" + +#include +LOG_MODULE_REGISTER(bt_mgmt_adv, CONFIG_BT_MGMT_ADV_LOG_LEVEL); + +struct k_work_q adv_work_q; +K_THREAD_STACK_DEFINE(adv_work_q_stack_area, CONFIG_BT_MGMT_ADV_STACK_SIZE); + +ZBUS_CHAN_DECLARE(bt_mgmt_chan); + +#ifndef CONFIG_BT_MAX_PAIRED +#define BONDS_QUEUE_SIZE 0 +#else +#define BONDS_QUEUE_SIZE CONFIG_BT_MAX_PAIRED +#endif + +static struct k_work adv_work; +static bool dir_adv_timed_out; +static struct bt_le_ext_adv *ext_adv[CONFIG_BT_EXT_ADV_MAX_ADV_SET]; + +static const struct bt_data *adv_local[CONFIG_BT_EXT_ADV_MAX_ADV_SET]; +static size_t adv_local_size[CONFIG_BT_EXT_ADV_MAX_ADV_SET]; +static const struct bt_data *per_adv_local[CONFIG_BT_EXT_ADV_MAX_ADV_SET]; +static size_t per_adv_local_size[CONFIG_BT_EXT_ADV_MAX_ADV_SET]; + +/* Bonded address queue */ +K_MSGQ_DEFINE(bonds_q, sizeof(bt_addr_le_t), BONDS_QUEUE_SIZE, sizeof(void *)); +K_MSGQ_DEFINE(adv_q, sizeof(uint8_t), CONFIG_BT_EXT_ADV_MAX_ADV_SET, sizeof(void *)); + +static struct bt_le_adv_param ext_adv_param = { + .id = BT_ID_DEFAULT, + .sid = CONFIG_BLE_ACL_ADV_SID, + .secondary_max_skip = 0, + .options = BT_LE_ADV_OPT_EXT_ADV, + .interval_min = CONFIG_BLE_ACL_EXT_ADV_INT_MIN, + .interval_max = CONFIG_BLE_ACL_EXT_ADV_INT_MAX, + .peer = NULL, +}; + static void bond_find(const struct bt_bond_info *info, void *user_data) { - ARG_UNUSED(user_data); - - int ret; - struct bt_conn *conn; - - if (!IS_ENABLED(CONFIG_BT_BONDABLE)) { - return; - } - - /* Filter already connected peers. */ - conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, &info->addr); - if (conn) { - struct bt_conn_info conn_info; - - ret = bt_conn_get_info(conn, &conn_info); - if (ret) { - LOG_WRN("Could not get conn info"); - bt_conn_unref(conn); - return; - } - - if (conn_info.state == BT_CONN_STATE_CONNECTED) { - LOG_WRN("Already connected"); - bt_conn_unref(conn); - return; - } - - bt_conn_unref(conn); - } - - ret = k_msgq_put(&bonds_queue, (void *)&info->addr, K_NO_WAIT); - if (ret) { - LOG_WRN("No space in the queue for the bond"); - } - } - + int ret; + struct bt_conn *conn; + + if (!IS_ENABLED(CONFIG_BT_BONDABLE)) { + return; + } + + /* Filter already connected peers. */ + conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, &info->addr); + if (conn) { + struct bt_conn_info conn_info; + + ret = bt_conn_get_info(conn, &conn_info); + if (ret) { + LOG_WRN("Could not get conn info"); + bt_conn_unref(conn); + return; + } + + if (conn_info.state == BT_CONN_STATE_CONNECTED) { + LOG_WRN("Already connected"); + bt_conn_unref(conn); + return; + } + + bt_conn_unref(conn); + } + + ret = k_msgq_put(&bonds_q, (void *)&info->addr, K_NO_WAIT); + if (ret) { + LOG_WRN("No space in the queue for the bond"); + } +} + static void filter_accept_list_add(const struct bt_bond_info *info, void *user_data) { - ARG_UNUSED(user_data); - - int ret; - - ret = bt_le_filter_accept_list_add(&info->addr); - if (ret) { - LOG_WRN("Could not add peer to Filter Accept List: %d", ret); - return; - } - } - - /** - * @brief Prints the address of the local device and the remote device. - * - * @note The address of the remote device is only printed if directed advertisement is active. - */ - static int addr_print(bt_addr_le_t const *const local_addr, bt_addr_le_t const *const dir_adv_addr) - { - char local_addr_str[BT_ADDR_LE_STR_LEN] = {'\0'}; - char directed_to_addr_str[BT_ADDR_LE_STR_LEN] = {'\0'}; - - if (local_addr == NULL) { - return -EINVAL; - } - - (void)bt_addr_le_to_str(local_addr, local_addr_str, BT_ADDR_LE_STR_LEN); - LOG_INF("Local addr: %s", local_addr_str); - - if (dir_adv_addr != NULL) { - (void)bt_addr_le_to_str(dir_adv_addr, directed_to_addr_str, BT_ADDR_LE_STR_LEN); - LOG_INF("Adv directed to: %s.", directed_to_addr_str); - } - - return 0; - } - - #if defined(CONFIG_BT_PRIVACY) - static bool adv_rpa_expired_cb(struct bt_le_ext_adv *adv) - { - int ret; - struct bt_le_ext_adv_info ext_adv_info; - - LOG_INF("RPA (Resolvable Private Address) expired."); - - ret = bt_le_ext_adv_get_info(adv, &ext_adv_info); - ERR_CHK_MSG(ret, "bt_le_ext_adv_get_info failed"); - - ret = addr_print(ext_adv_info.addr, NULL); - if (ret) { - LOG_ERR("addr_print failed"); - } - - return true; - } - #endif /* CONFIG_BT_PRIVACY */ - - static const struct bt_le_ext_adv_cb adv_cb = { - #if defined(CONFIG_BT_PRIVACY) - .rpa_expired = adv_rpa_expired_cb, - #endif /* CONFIG_BT_PRIVACY */ - }; - - static int direct_adv_create(uint8_t ext_adv_index, bt_addr_le_t addr) - { - int ret; - struct bt_le_ext_adv_info ext_adv_info; - - ext_adv_param = *BT_LE_ADV_CONN_DIR(&addr); - ext_adv_param.id = BT_ID_DEFAULT; - ext_adv_param.options |= BT_LE_ADV_OPT_DIR_ADDR_RPA; - - /* Clear ADV data set before update to direct advertising */ - ret = bt_le_ext_adv_set_data(ext_adv[ext_adv_index], NULL, 0, NULL, 0); - if (ret) { - LOG_ERR("Failed to clear advertising data for set %d. Err: %d", ext_adv_index, ret); - return ret; - } - - ret = bt_le_ext_adv_update_param(ext_adv[ext_adv_index], &ext_adv_param); - if (ret) { - LOG_ERR("Failed to update ext_adv set %d to directed advertising. Err = %d", - ext_adv_index, ret); - return ret; - } - - ret = bt_le_ext_adv_get_info(ext_adv[ext_adv_index], &ext_adv_info); - if (ret) { - return ret; - } - - ret = addr_print(ext_adv_info.addr, &addr); - if (ret) { - return ret; - } - - return 0; - } - - static int extended_adv_create(uint8_t ext_adv_index) - { - int ret; - struct bt_le_ext_adv_info ext_adv_info; - - if (adv_local[ext_adv_index] == NULL) { - LOG_ERR("Adv_local not set"); - return -ENXIO; - } - - ret = bt_le_ext_adv_set_data(ext_adv[ext_adv_index], adv_local[ext_adv_index], - adv_local_size[ext_adv_index], NULL, 0); - if (ret) { - LOG_ERR("Failed to set advertising data: %d", ret); - return ret; - } - - if (per_adv_local[ext_adv_index] != NULL && IS_ENABLED(CONFIG_BT_PER_ADV)) { - /* Set periodic advertising parameters */ - ret = bt_le_per_adv_set_param(ext_adv[ext_adv_index], LE_AUDIO_PERIODIC_ADV); - if (ret) { - LOG_ERR("Failed to set periodic advertising parameters: %d", ret); - return ret; - } - - ret = bt_le_per_adv_set_data(ext_adv[ext_adv_index], per_adv_local[ext_adv_index], - per_adv_local_size[ext_adv_index]); - if (ret) { - LOG_ERR("Failed to set periodic advertising data: %d", ret); - return ret; - } - } - - ret = bt_le_ext_adv_get_info(ext_adv[ext_adv_index], &ext_adv_info); - if (ret) { - return ret; - } - - ret = addr_print(ext_adv_info.addr, NULL); - if (ret) { - return ret; - } - - return 0; - } - + int ret; + + ret = bt_le_filter_accept_list_add(&info->addr); + if (ret) { + LOG_WRN("Could not add peer to Filter Accept List: %d", ret); + return; + } +} + +/** + * @brief Prints the address of the local device and the remote device. + * + * @note The address of the remote device is only printed if directed advertisement is active. + */ +static int addr_print(bt_addr_le_t const *const local_addr, bt_addr_le_t const *const dir_adv_addr) +{ + char local_addr_str[BT_ADDR_LE_STR_LEN] = {'\0'}; + char directed_to_addr_str[BT_ADDR_LE_STR_LEN] = {'\0'}; + + if (local_addr == NULL) { + return -EINVAL; + } + + (void)bt_addr_le_to_str(local_addr, local_addr_str, BT_ADDR_LE_STR_LEN); + LOG_INF("Local addr: %s", local_addr_str); + + if (dir_adv_addr != NULL) { + (void)bt_addr_le_to_str(dir_adv_addr, directed_to_addr_str, BT_ADDR_LE_STR_LEN); + LOG_INF("Adv directed to: %s.", directed_to_addr_str); + } + + return 0; +} + +#if defined(CONFIG_BT_PRIVACY) +static bool adv_rpa_expired_cb(struct bt_le_ext_adv *adv) +{ + int ret; + struct bt_le_ext_adv_info ext_adv_info; + + LOG_INF("RPA (Resolvable Private Address) expired."); + + ret = bt_le_ext_adv_get_info(adv, &ext_adv_info); + ERR_CHK_MSG(ret, "bt_le_ext_adv_get_info failed"); + + ret = addr_print(ext_adv_info.addr, NULL); + if (ret) { + LOG_ERR("addr_print failed"); + } + + return true; +} +#endif /* CONFIG_BT_PRIVACY */ + +static const struct bt_le_ext_adv_cb adv_cb = { +#if defined(CONFIG_BT_PRIVACY) + .rpa_expired = adv_rpa_expired_cb, +#endif /* CONFIG_BT_PRIVACY */ +}; + +static int direct_adv_create(uint8_t ext_adv_index, bt_addr_le_t addr) +{ + int ret; + struct bt_le_ext_adv_info ext_adv_info; + + ext_adv_param = *BT_LE_ADV_CONN_DIR(&addr); + ext_adv_param.id = BT_ID_DEFAULT; + ext_adv_param.options |= BT_LE_ADV_OPT_DIR_ADDR_RPA; + + /* Clear ADV data set before update to direct advertising */ + ret = bt_le_ext_adv_set_data(ext_adv[ext_adv_index], NULL, 0, NULL, 0); + if (ret) { + LOG_ERR("Failed to clear advertising data for set %d. Err: %d", ext_adv_index, ret); + return ret; + } + + ret = bt_le_ext_adv_update_param(ext_adv[ext_adv_index], &ext_adv_param); + if (ret) { + LOG_ERR("Failed to update ext_adv set %d to directed advertising. Err = %d", + ext_adv_index, ret); + return ret; + } + + ret = bt_le_ext_adv_get_info(ext_adv[ext_adv_index], &ext_adv_info); + if (ret) { + return ret; + } + + ret = addr_print(ext_adv_info.addr, &addr); + if (ret) { + return ret; + } + + return 0; +} + +static int extended_adv_create(uint8_t ext_adv_index) +{ + int ret; + struct bt_le_ext_adv_info ext_adv_info; + + if (adv_local[ext_adv_index] == NULL) { + LOG_ERR("Adv_local not set"); + return -ENXIO; + } + + ret = bt_le_ext_adv_set_data(ext_adv[ext_adv_index], adv_local[ext_adv_index], + adv_local_size[ext_adv_index], NULL, 0); + if (ret) { + LOG_ERR("Failed to set advertising data: %d", ret); + return ret; + } + + if (per_adv_local[ext_adv_index] != NULL && IS_ENABLED(CONFIG_BT_PER_ADV)) { + /* Set periodic advertising parameters */ + ret = bt_le_per_adv_set_param(ext_adv[ext_adv_index], LE_AUDIO_PERIODIC_ADV); + if (ret) { + LOG_ERR("Failed to set periodic advertising parameters: %d", ret); + return ret; + } + + ret = bt_le_per_adv_set_data(ext_adv[ext_adv_index], per_adv_local[ext_adv_index], + per_adv_local_size[ext_adv_index]); + if (ret) { + LOG_ERR("Failed to set periodic advertising data: %d", ret); + return ret; + } + } + + ret = bt_le_ext_adv_get_info(ext_adv[ext_adv_index], &ext_adv_info); + if (ret) { + return ret; + } + + ret = addr_print(ext_adv_info.addr, NULL); + if (ret) { + return ret; + } + + return 0; +} + static void advertising_process(struct k_work *work) { - ARG_UNUSED(work); - - int ret; - struct bt_mgmt_msg msg; - uint8_t ext_adv_index; - - ret = k_msgq_get(&adv_queue, &ext_adv_index, K_NO_WAIT); - if (ret) { - LOG_ERR("No ext_adv_index found"); - return; - } - - k_msgq_purge(&bonds_queue); - - if (IS_ENABLED(CONFIG_BT_BONDABLE)) { - bt_foreach_bond(BT_ID_DEFAULT, bond_find, NULL); - /* Populate Filter Accept List */ - if (IS_ENABLED(CONFIG_BT_FILTER_ACCEPT_LIST)) { - ret = bt_le_filter_accept_list_clear(); - if (ret) { - LOG_ERR("Failed to clear filter accept list"); - return; - } - - bt_foreach_bond(BT_ID_DEFAULT, filter_accept_list_add, NULL); - } - } - - bt_addr_le_t addr; - - if (!k_msgq_get(&bonds_queue, &addr, K_NO_WAIT) && !dir_adv_timed_out) { - ret = direct_adv_create(ext_adv_index, addr); - if (ret) { - LOG_WRN("Failed to create direct advertisement: %d", ret); - return; - } - - ret = bt_le_ext_adv_start( - ext_adv[ext_adv_index], - BT_LE_EXT_ADV_START_PARAM(BT_GAP_ADV_HIGH_DUTY_CYCLE_MAX_TIMEOUT, 0)); - } else { - ret = extended_adv_create(ext_adv_index); - if (ret) { - LOG_WRN("Failed to create extended advertisement: %d", ret); - return; - } - - dir_adv_timed_out = false; - ret = bt_le_ext_adv_start(ext_adv[ext_adv_index], BT_LE_EXT_ADV_START_DEFAULT); - } - - if (ret) { - LOG_ERR("Failed to start advertising set. Err: %d", ret); - return; - } - - if (per_adv_local[ext_adv_index] != NULL && IS_ENABLED(CONFIG_BT_PER_ADV)) { - /* Enable Periodic Advertising */ - ret = bt_le_per_adv_start(ext_adv[ext_adv_index]); - if (ret) { - LOG_ERR("Failed to enable periodic advertising: %d", ret); - return; - } - - msg.event = BT_MGMT_EXT_ADV_WITH_PA_READY; - msg.ext_adv = ext_adv[ext_adv_index]; - msg.index = ext_adv_index; - - ret = zbus_chan_pub(&bt_mgmt_chan, &msg, K_NO_WAIT); - ERR_CHK(ret); - } - - /* NOTE: The string below is used by the Nordic CI system */ - LOG_INF("Advertising successfully started"); - } - - void bt_mgmt_dir_adv_timed_out(uint8_t ext_adv_index) - { - int ret; - - dir_adv_timed_out = true; - - LOG_DBG("Clearing ext_adv"); - - ret = bt_le_ext_adv_delete(ext_adv[ext_adv_index]); - if (ret) { - LOG_ERR("Failed to clear ext_adv"); - } - - if (IS_ENABLED(CONFIG_BT_FILTER_ACCEPT_LIST)) { - ret = bt_le_ext_adv_create(LE_AUDIO_EXTENDED_ADV_CONN_NAME_FILTER, &adv_cb, - &ext_adv[ext_adv_index]); - } else { - ret = bt_le_ext_adv_create(LE_AUDIO_EXTENDED_ADV_CONN_NAME, &adv_cb, - &ext_adv[ext_adv_index]); - } - - if (ret) { - LOG_ERR("Unable to create a connectable extended advertising set: %d", ret); - return; - } - - /* Restart normal advertising */ - ret = bt_mgmt_adv_start(ext_adv_index, NULL, 0, NULL, 0, true); - if (ret) { - LOG_ERR("Unable start advertising: %d", ret); - return; - } - } - - int bt_mgmt_manufacturer_uuid_populate(struct net_buf_simple *uuid_buf, uint16_t company_id) - { - if (net_buf_simple_tailroom(uuid_buf) >= BT_UUID_SIZE_16) { - net_buf_simple_add_le16(uuid_buf, company_id); - } else { - return -ENOMEM; - } - - return 0; - } - - int bt_mgmt_adv_buffer_put(struct bt_data *const adv_buf, uint32_t *index, size_t adv_buf_vacant, - size_t data_len, uint8_t type, void *data) - { - if (adv_buf == NULL || index == NULL || data_len == 0) { - return -EINVAL; - } - - /* Check that we have space for data */ - if (adv_buf_vacant <= *index) { - return -ENOMEM; - } - - adv_buf[*index].type = type; - adv_buf[*index].data_len = data_len; - adv_buf[*index].data = data; - (*index)++; - - return 0; - } - - int bt_mgmt_per_adv_stop(uint8_t ext_adv_index) - { - int ret; - - ret = bt_le_per_adv_stop(ext_adv[ext_adv_index]); - if (ret) { - LOG_ERR("Failed to top periodic advertising: %d", ret); - return ret; - } - - return 0; - } - - int bt_mgmt_ext_adv_stop(uint8_t ext_adv_index) - { - int ret; - - ret = bt_le_ext_adv_stop(ext_adv[ext_adv_index]); - if (ret) { - LOG_ERR("Failed to stop advertising set: %d", ret); - return ret; - } - - ret = bt_le_ext_adv_delete(ext_adv[ext_adv_index]); - if (ret) { - LOG_ERR("Failed to delete advertising set: %d", ret); - return ret; - } - - return 0; - } - - int bt_mgmt_adv_start(uint8_t ext_adv_index, const struct bt_data *adv, size_t adv_size, - const struct bt_data *per_adv, size_t per_adv_size, bool connectable) - { - int ret; - - /* Special case for restarting advertising */ - if (adv == NULL && adv_size == 0 && per_adv == NULL && per_adv_size == 0) { - if (adv_local[ext_adv_index] == NULL) { - LOG_ERR("No valid advertising data stored"); - return -ENOENT; - } - - ret = k_msgq_put(&adv_queue, &ext_adv_index, K_NO_WAIT); - if (ret) { - LOG_ERR("No space in the queue for adv_index"); - return -ENOMEM; - } - k_work_submit(&adv_work); - - return 0; - } - - if (adv == NULL) { - LOG_ERR("No adv struct received"); - return -EINVAL; - } - - if (adv_size == 0) { - LOG_ERR("Invalid size of adv struct"); - return -EINVAL; - } - - adv_local[ext_adv_index] = adv; - adv_local_size[ext_adv_index] = adv_size; - per_adv_local[ext_adv_index] = per_adv; - per_adv_local_size[ext_adv_index] = per_adv_size; - - /* Only use fixed address if no privacy and it is the first ext adv set */ - if (!IS_ENABLED(CONFIG_BT_PRIVACY) && ext_adv_index == 0) { - ext_adv_param.options |= BT_LE_ADV_OPT_USE_IDENTITY; - } else { - /* If privacy is enabled, use RPA */ - ext_adv_param.options &= ~BT_LE_ADV_OPT_USE_IDENTITY; - } - - if (connectable) { - ret = bt_le_ext_adv_create(LE_AUDIO_EXTENDED_ADV_CONN_NAME, &adv_cb, - &ext_adv[ext_adv_index]); - if (ret) { - LOG_ERR("Unable to create a connectable extended advertising set: %d", ret); - return ret; - } - } else { - ret = bt_le_ext_adv_create(&ext_adv_param, &adv_cb, &ext_adv[ext_adv_index]); - if (ret) { - LOG_ERR("Unable to create extended advertising set: %d", ret); - return ret; - } - } - - ret = k_msgq_put(&adv_queue, &ext_adv_index, K_NO_WAIT); - if (ret) { - LOG_ERR("No space in the queue for adv_index"); - return -ENOMEM; - } - k_work_submit(&adv_work); - - return 0; - } - + int ret; + struct bt_mgmt_msg msg; + uint8_t ext_adv_index; + + ret = k_msgq_get(&adv_q, &ext_adv_index, K_NO_WAIT); + if (ret) { + LOG_ERR("No ext_adv_index found"); + return; + } + + k_msgq_purge(&bonds_q); + + if (IS_ENABLED(CONFIG_BT_BONDABLE)) { + bt_foreach_bond(BT_ID_DEFAULT, bond_find, NULL); + /* Populate Filter Accept List */ + if (IS_ENABLED(CONFIG_BT_FILTER_ACCEPT_LIST)) { + ret = bt_le_filter_accept_list_clear(); + if (ret) { + LOG_ERR("Failed to clear filter accept list"); + return; + } + + bt_foreach_bond(BT_ID_DEFAULT, filter_accept_list_add, NULL); + } + } + + bt_addr_le_t addr; + + if (!k_msgq_get(&bonds_q, &addr, K_NO_WAIT) && !dir_adv_timed_out) { + ret = direct_adv_create(ext_adv_index, addr); + if (ret) { + LOG_WRN("Failed to create direct advertisement: %d", ret); + return; + } + + ret = bt_le_ext_adv_start( + ext_adv[ext_adv_index], + BT_LE_EXT_ADV_START_PARAM(BT_GAP_ADV_HIGH_DUTY_CYCLE_MAX_TIMEOUT, 0)); + } else { + ret = extended_adv_create(ext_adv_index); + if (ret) { + LOG_WRN("Failed to create extended advertisement: %d", ret); + return; + } + + dir_adv_timed_out = false; + ret = bt_le_ext_adv_start(ext_adv[ext_adv_index], BT_LE_EXT_ADV_START_DEFAULT); + } + + if (ret) { + LOG_ERR("Failed to start advertising set. Err: %d", ret); + return; + } + + if (per_adv_local[ext_adv_index] != NULL && IS_ENABLED(CONFIG_BT_PER_ADV)) { + /* Enable Periodic Advertising */ + ret = bt_le_per_adv_start(ext_adv[ext_adv_index]); + if (ret) { + LOG_ERR("Failed to enable periodic advertising: %d", ret); + return; + } + + msg.event = BT_MGMT_EXT_ADV_WITH_PA_READY; + msg.ext_adv = ext_adv[ext_adv_index]; + msg.index = ext_adv_index; + + ret = zbus_chan_pub(&bt_mgmt_chan, &msg, K_NO_WAIT); + ERR_CHK(ret); + } + + /* NOTE: The string below is used by the Nordic CI system */ + LOG_INF("Advertising successfully started"); +} + +void bt_mgmt_dir_adv_timed_out(uint8_t ext_adv_index) +{ + int ret; + + dir_adv_timed_out = true; + + LOG_DBG("Clearing ext_adv"); + + ret = bt_le_ext_adv_delete(ext_adv[ext_adv_index]); + if (ret) { + LOG_ERR("Failed to clear ext_adv"); + } + + if (IS_ENABLED(CONFIG_BT_FILTER_ACCEPT_LIST)) { + ret = bt_le_ext_adv_create(LE_AUDIO_EXTENDED_ADV_CONN_FILTER, &adv_cb, + &ext_adv[ext_adv_index]); + } else { + ret = bt_le_ext_adv_create(LE_AUDIO_EXTENDED_ADV_CONN, &adv_cb, + &ext_adv[ext_adv_index]); + } + + if (ret) { + LOG_ERR("Unable to create a connectable extended advertising set: %d", ret); + return; + } + + /* Restart normal advertising */ + ret = bt_mgmt_adv_start(ext_adv_index, NULL, 0, NULL, 0, true); + if (ret) { + LOG_ERR("Unable start advertising: %d", ret); + return; + } +} + +int bt_mgmt_manufacturer_uuid_populate(struct net_buf_simple *uuid_buf, uint16_t company_id) +{ + if (net_buf_simple_tailroom(uuid_buf) >= BT_UUID_SIZE_16) { + net_buf_simple_add_le16(uuid_buf, company_id); + } else { + return -ENOMEM; + } + + return 0; +} + +int bt_mgmt_adv_buffer_put(struct bt_data *const adv_buf, uint32_t *index, size_t adv_buf_vacant, + size_t data_len, uint8_t type, void *data) +{ + if (adv_buf == NULL || index == NULL || data_len == 0) { + return -EINVAL; + } + + /* Check that we have space for data */ + if (adv_buf_vacant <= *index) { + return -ENOMEM; + } + + adv_buf[*index].type = type; + adv_buf[*index].data_len = data_len; + adv_buf[*index].data = data; + (*index)++; + + return 0; +} + +int bt_mgmt_per_adv_stop(uint8_t ext_adv_index) +{ + int ret; + + ret = bt_le_per_adv_stop(ext_adv[ext_adv_index]); + if (ret) { + LOG_ERR("Failed to top periodic advertising: %d", ret); + return ret; + } + + return 0; +} + +int bt_mgmt_ext_adv_stop(uint8_t ext_adv_index) +{ + int ret; + + ret = bt_le_ext_adv_stop(ext_adv[ext_adv_index]); + if (ret) { + LOG_ERR("Failed to stop advertising set: %d", ret); + return ret; + } + + ret = bt_le_ext_adv_delete(ext_adv[ext_adv_index]); + if (ret) { + LOG_ERR("Failed to delete advertising set: %d", ret); + return ret; + } + + return 0; +} + +int bt_mgmt_adv_start(uint8_t ext_adv_index, const struct bt_data *adv, size_t adv_size, + const struct bt_data *per_adv, size_t per_adv_size, bool connectable) +{ + int ret; + + /* Special case for restarting advertising */ + if (adv == NULL && adv_size == 0 && per_adv == NULL && per_adv_size == 0) { + if (adv_local[ext_adv_index] == NULL) { + LOG_ERR("No valid advertising data stored"); + return -ENOENT; + } + + ret = k_msgq_put(&adv_q, &ext_adv_index, K_NO_WAIT); + if (ret) { + LOG_ERR("No space in the queue for adv_index"); + return -ENOMEM; + } + k_work_submit_to_queue(&adv_work_q, &adv_work); + + return 0; + } + + if (adv == NULL) { + LOG_ERR("No adv struct received"); + return -EINVAL; + } + + if (adv_size == 0) { + LOG_ERR("Invalid size of adv struct"); + return -EINVAL; + } + + adv_local[ext_adv_index] = adv; + adv_local_size[ext_adv_index] = adv_size; + per_adv_local[ext_adv_index] = per_adv; + per_adv_local_size[ext_adv_index] = per_adv_size; + + /* Only use fixed address if no privacy and it is the first ext adv set */ + if (!IS_ENABLED(CONFIG_BT_PRIVACY) && ext_adv_index == 0) { + ext_adv_param.options |= BT_LE_ADV_OPT_USE_IDENTITY; + } else { + /* If privacy is enabled, use RPA */ + ext_adv_param.options &= ~BT_LE_ADV_OPT_USE_IDENTITY; + } + + if (connectable) { + ret = bt_le_ext_adv_create(LE_AUDIO_EXTENDED_ADV_CONN, &adv_cb, + &ext_adv[ext_adv_index]); + if (ret) { + LOG_ERR("Unable to create a connectable extended advertising set: %d", ret); + return ret; + } + } else { + ret = bt_le_ext_adv_create(&ext_adv_param, &adv_cb, &ext_adv[ext_adv_index]); + if (ret) { + LOG_ERR("Unable to create extended advertising set: %d", ret); + return ret; + } + } + + ret = k_msgq_put(&adv_q, &ext_adv_index, K_NO_WAIT); + if (ret) { + LOG_ERR("No space in the queue for adv_index"); + return -ENOMEM; + } + k_work_submit_to_queue(&adv_work_q, &adv_work); + + return 0; +} + void bt_mgmt_adv_init(void) { k_work_init(&adv_work, advertising_process); + k_work_queue_init(&adv_work_q); + k_work_queue_start(&adv_work_q, adv_work_q_stack_area, + K_THREAD_STACK_SIZEOF(adv_work_q_stack_area), CONFIG_BT_MGMT_ADV_WQ_PRIO, + NULL); + k_thread_name_set(&adv_work_q.thread, "BT_adv_WQ"); } diff --git a/src/bluetooth/bt_management/bt_mgmt.c b/src/bluetooth/bt_management/bt_mgmt.c index 4d0a1818..07c3d7bb 100644 --- a/src/bluetooth/bt_management/bt_mgmt.c +++ b/src/bluetooth/bt_management/bt_mgmt.c @@ -6,29 +6,23 @@ #include "bt_mgmt.h" -#include -#include "channel_assignment.h" - #include #include #include #include +#include #include #include #include #include "macros_common.h" #include "zbus_common.h" -#include "button_assignments.h" -#include "../../buttons/button_manager.h" -#include "uicr.h" - -#include "bt_mgmt_ctlr_cfg_internal.h" #include "bt_mgmt_adv_internal.h" -#include "bt_mgmt_dfu_internal.h" +#include "bt_mgmt_ctlr_cfg_internal.h" #include "bt_mgmt_conn_interval.h" - +#include "bt_mgmt_bond_storage.h" #include "BootState.h" +#include "uicr.h" #include LOG_MODULE_REGISTER(bt_mgmt, CONFIG_BT_MGMT_LOG_LEVEL); @@ -68,37 +62,10 @@ static void conn_state_connected_check(struct bt_conn *conn, void *data) (*num_conn)++; } -void mtu_updated(struct bt_conn *conn, uint16_t tx, uint16_t rx) +static void conn_params_updated(struct bt_conn *conn, uint16_t interval, + uint16_t latency, uint16_t timeout) { - ARG_UNUSED(conn); - - LOG_INF("Updated MTU: TX: %d RX: %d bytes", tx, rx); -} - -static void le_data_length_updated(struct bt_conn *conn, - struct bt_conn_le_data_len_info *info) -{ - ARG_UNUSED(conn); - - LOG_INF("LE data len updated: TX (len: %d time: %d)" - " RX (len: %d time: %d)", info->tx_max_len, - info->tx_max_time, info->rx_max_len, info->rx_max_time); -} - -static struct bt_le_conn_param *conn_param = BT_LE_CONN_PARAM(CONFIG_BLE_ACL_CONN_INTERVAL, CONFIG_BLE_ACL_CONN_INTERVAL, CONFIG_BLE_ACL_SLAVE_LATENCY, CONFIG_BLE_ACL_SUP_TIMEOUT); - -//callback -static void conn_params_updated(struct bt_conn *conn, uint16_t interval, uint16_t latency, uint16_t timeout) -{ - LOG_INF("Conn params updated: interval %d unit, latency %d, timeout: %d0 ms",interval, latency, timeout); - bt_mgmt_ci_on_conn_param_updated(conn, interval, latency, timeout); - - /*msg.event = BT_MGMT_CONNECTED; - msg.conn = conn; - - ret = zbus_chan_pub(&bt_mgmt_chan, &msg, K_NO_WAIT); - ERR_CHK(ret);*/ } static void connected_cb(struct bt_conn *conn, uint8_t err) @@ -147,6 +114,7 @@ static void connected_cb(struct bt_conn *conn, uint8_t err) /* ACL connection established */ /* NOTE: The string below is used by the Nordic CI system */ LOG_INF("Connected: %s", addr); + bt_mgmt_ci_on_connected(conn); msg.event = BT_MGMT_CONNECTED; msg.conn = conn; @@ -154,33 +122,19 @@ static void connected_cb(struct bt_conn *conn, uint8_t err) ret = zbus_chan_pub(&bt_mgmt_chan, &msg, K_NO_WAIT); ERR_CHK(ret); - err = bt_conn_le_phy_update(conn, BT_CONN_LE_PHY_PARAM_2M); - if (err) { - LOG_ERR("Phy update request failed: %d", err); - } - - err = bt_conn_le_data_len_update(conn, BT_LE_DATA_LEN_PARAM_MAX); - if (err) { - LOG_ERR("LE data length update request failed: %d", err); + ret = bt_conn_le_param_update(conn, + BT_LE_CONN_PARAM(CONFIG_BLE_ACL_CONN_INTERVAL, CONFIG_BLE_ACL_CONN_INTERVAL, + CONFIG_BLE_ACL_SLAVE_LATENCY, CONFIG_BLE_ACL_SUP_TIMEOUT)); + if (ret && ret != -EALREADY) { + LOG_WRN("Connection parameter update request failed: %d", ret); } - err = bt_conn_le_param_update(conn, conn_param); - if (err) { - LOG_ERR("Cannot update conneciton parameter (err: %d)", err); - return; - } - LOG_INF("Connection parameters update requested: interval_min %d interval_max %d latency %d timeout %d", - conn_param->interval_min, conn_param->interval_max, - conn_param->latency, conn_param->timeout); - bt_mgmt_ci_on_connected(conn); - if (IS_ENABLED(CONFIG_BT_CENTRAL)) { ret = bt_conn_set_security(conn, BT_SECURITY_L2); if (ret) { LOG_ERR("Failed to set security to L2: %d", ret); } } - } K_MUTEX_DEFINE(mtx_duplicate_scan); @@ -195,6 +149,7 @@ static void disconnected_cb(struct bt_conn *conn, uint8_t reason) /* NOTE: The string below is used by the Nordic CI system */ LOG_INF("Disconnected: %s, reason 0x%02x %s", addr, reason, bt_hci_err_to_str(reason)); + bt_mgmt_ci_on_disconnected(conn, reason); if (IS_ENABLED(CONFIG_BT_CENTRAL)) { bt_conn_unref(conn); @@ -229,38 +184,126 @@ static void disconnected_cb(struct bt_conn *conn, uint8_t reason) #if defined(CONFIG_BT_SMP) static void security_changed_cb(struct bt_conn *conn, bt_security_t level, enum bt_security_err err) { + /* The address may not be resolved at this point */ int ret; struct bt_mgmt_msg msg; if (err) { - LOG_WRN("Security failed: level %d err %d %s", level, err, - bt_security_err_to_str(err)); + if (err == BT_SECURITY_ERR_UNSPECIFIED) { + LOG_WRN("Security failed: level %d err %d Clear bond on peer?", level, err); + } else { + LOG_WRN("Security failed: level %d err %d %s", level, err, + bt_security_err_to_str(err)); + } ret = bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL); - if (ret) { + if (ret == -ENOTCONN) { + LOG_DBG("Not connected"); + } else if (ret) { LOG_WRN("Failed to disconnect %d", ret); } + + } else if (level < BT_SECURITY_L2) { + LOG_WRN("Security changed: level %d too low, disconnecting", level); + ret = bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL); + if (ret == -ENOTCONN) { + LOG_DBG("Not connected"); + } else if (ret) { + LOG_ERR("Failed to disconnect %d", ret); + } } else { - LOG_DBG("Security changed: level %d", level); + const bt_addr_le_t *peer_addr = bt_conn_get_dst(conn); + char peer_str[BT_ADDR_LE_STR_LEN]; + + bt_addr_le_to_str(peer_addr, peer_str, BT_ADDR_LE_STR_LEN); + + LOG_INF("Security changed: level %d %s", level, peer_str); + /* Publish connected */ msg.event = BT_MGMT_SECURITY_CHANGED; msg.conn = conn; + msg.addr = *peer_addr; ret = zbus_chan_pub(&bt_mgmt_chan, &msg, K_NO_WAIT); ERR_CHK(ret); } } + +void identity_resolved_cb(struct bt_conn *conn, const bt_addr_le_t *rpa, + const bt_addr_le_t *identity) +{ + char rpa_str[BT_ADDR_LE_STR_LEN]; + char identity_str[BT_ADDR_LE_STR_LEN]; + (void)bt_addr_le_to_str(rpa, rpa_str, BT_ADDR_LE_STR_LEN); + (void)bt_addr_le_to_str(identity, identity_str, BT_ADDR_LE_STR_LEN); + LOG_INF("ID is resolved. RPA: %s, Identity: %s", rpa_str, identity_str); +}; + #endif /* defined(CONFIG_BT_SMP) */ static struct bt_conn_cb conn_callbacks = { .connected = connected_cb, .disconnected = disconnected_cb, .le_param_updated = conn_params_updated, - .le_data_len_updated = le_data_length_updated, #if defined(CONFIG_BT_SMP) + .identity_resolved = identity_resolved_cb, .security_changed = security_changed_cb, #endif /* defined(CONFIG_BT_SMP) */ }; +void bond_deleted_cb(uint8_t id, const bt_addr_le_t *peer) +{ + int ret; + char str[BT_ADDR_LE_STR_LEN]; + struct bt_mgmt_msg msg; + + (void)bt_addr_le_to_str(peer, str, BT_ADDR_LE_STR_LEN); + LOG_INF("Bond deleted: id %d, peer %s", id, str); + + msg.event = BT_MGMT_BOND_DELETED; + msg.addr = *peer; + + ret = zbus_chan_pub(&bt_mgmt_chan, &msg, K_NO_WAIT); + ERR_CHK(ret); +} + +void pairing_complete_cb(struct bt_conn *conn, bool bonded) +{ + LOG_INF("Pairing complete. Bonded: %d", bonded); + int ret; + struct bt_mgmt_msg msg; + const bt_addr_le_t *peer_addr = bt_conn_get_dst(conn); + char str[BT_ADDR_LE_STR_LEN]; + + (void)bt_addr_le_to_str(peer_addr, str, BT_ADDR_LE_STR_LEN); + + msg.event = BT_MGMT_PAIRING_COMPLETE; + msg.addr = *peer_addr; + msg.conn = conn; + + ret = zbus_chan_pub(&bt_mgmt_chan, &msg, K_NO_WAIT); + ERR_CHK(ret); +} + +void pairing_failed_cb(struct bt_conn *conn, enum bt_security_err reason) +{ + int ret; + + LOG_WRN("Pairing failed: %d %s", reason, bt_security_err_to_str(reason)); + + ret = bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN); + if (ret == -ENOTCONN) { + LOG_DBG("Not connected"); + } else if (ret) { + LOG_ERR("Failed to disconnect %d", ret); + } +} + +static struct bt_conn_auth_info_cb conn_auth_info_callbacks = { + .bond_deleted = bond_deleted_cb, + .pairing_complete = pairing_complete_cb, + .pairing_failed = pairing_failed_cb, +}; + static void bt_enabled_cb(int err) { if (err) { @@ -275,13 +318,11 @@ static void bt_enabled_cb(int err) static int bonding_clear_check(void) { - int ret; - + /* The power button may still be held during an ordinary startup. + * Only the PMIC timer reset should clear the stored phone bonds. + */ if (oe_boot_state.timer_reset) { - LOG_INF("Device Count: %i", bonded_device_count); - - ret = bt_mgmt_bonding_clear(); - return ret; + return bt_mgmt_bonding_clear(); } return 0; @@ -323,7 +364,7 @@ static int local_identity_addr_print(void) bt_id_get(addrs, &num_ids); - for (size_t i = 0; i < num_ids; i++) { + for (int i = 0; i < num_ids; i++) { (void)bt_addr_le_to_str(&(addrs[i]), addr_str, BT_ADDR_LE_STR_LEN); LOG_INF("Local identity addr: %s", addr_str); } @@ -331,15 +372,34 @@ static int local_identity_addr_print(void) return 0; } + void bt_mgmt_num_conn_get(uint8_t *num_conn) { bt_conn_foreach(BT_CONN_TYPE_LE, conn_state_connected_check, (void *)num_conn); } +static void bond_count_cb(const struct bt_bond_info *info, void *user_data) +{ + ARG_UNUSED(info); + uint8_t *count = user_data; + if (*count < UINT8_MAX) { + (*count)++; + } +} + +void bt_mgmt_num_bonds_get(uint8_t *num_bonds) +{ + *num_bonds = 0; + bt_foreach_bond(BT_ID_DEFAULT, bond_count_cb, num_bonds); +} + int bt_mgmt_bonding_clear(void) { int ret; + /* TODO: Delay. Awaiting fix in NCSDK-35186 */ + k_sleep(K_MSEC(100)); + if (IS_ENABLED(CONFIG_SETTINGS)) { LOG_INF("Clearing all bonds"); @@ -389,44 +449,40 @@ int bt_mgmt_conn_disconnect(struct bt_conn *conn, uint8_t reason) return 0; } -int bonded_device_count = 0; - -void count_bonds(const struct bt_bond_info *info, void *user_data) { - ARG_UNUSED(info); - ARG_UNUSED(user_data); - - bonded_device_count++; -} - -static struct bt_gatt_cb gatt_callbacks = { - .att_mtu_updated = mtu_updated, -}; - int bt_mgmt_init(void) { int ret; - static char name[CONFIG_BT_DEVICE_NAME_MAX]; ret = bt_enable(bt_enabled_cb); if (ret) { return ret; } - bt_gatt_cb_register(&gatt_callbacks); + ret = k_sem_take(&sem_bt_enabled, K_MSEC(BT_ENABLE_TIMEOUT_MS)); + if (ret) { + LOG_ERR("bt_enable timed out"); + return ret; + } + +#if defined(CONFIG_BT_USER_PHY_UPDATE) && defined(CONFIG_BT_AUTO_PHY_PERIPHERAL_2M) + /* The host skips its automatic PHY request when a connection starts on 2M. + * Set the controller preference too, so Android's LE Audio 1M uplink request + * does not reduce sensor bandwidth after reconnecting. + */ + ret = bt_conn_le_set_default_phy(BT_GAP_LE_PHY_2M, BT_GAP_LE_PHY_2M); + if (ret) { + LOG_WRN("Failed to set default 2M PHY preference: %d", ret); + } +#endif + /* Keep the pair-specific name used by the phone and OpenEarable app. */ + char name[CONFIG_BT_DEVICE_NAME_MAX]; uint32_t sirk = uicr_sirk_get(); - snprintf(name, CONFIG_BT_DEVICE_NAME_MAX, "%s-%04X", CONFIG_BT_DEVICE_NAME, - (unsigned int)((sirk != 0xFFFFFFFFU ? sirk : oe_boot_state.device_id) & 0xFFFF)); + snprintf(name, sizeof(name), "%s-%04X", CONFIG_BT_DEVICE_NAME, + (unsigned int)((sirk != 0xFFFFFFFFU ? sirk : oe_boot_state.device_id) & 0xFFFF)); ret = bt_set_name(name); - if (ret) { - LOG_ERR("bt_enable timed out"); - return ret; - } - - ret = k_sem_take(&sem_bt_enabled, K_MSEC(BT_ENABLE_TIMEOUT_MS)); if (ret) { - LOG_ERR("bt_enable timed out"); return ret; } @@ -443,10 +499,6 @@ int bt_mgmt_init(void) return ret; } - bonded_device_count = 0; - - bt_foreach_bond(BT_ID_DEFAULT, count_bonds, NULL); - ret = bonding_clear_check(); if (ret) { return ret; @@ -458,27 +510,25 @@ int bt_mgmt_init(void) return ret; } } - } -#if defined(CONFIG_AUDIO_BT_MGMT_DFU) - bool pressed; + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + ret = bt_mgmt_bond_storage_compat(); + if (ret) { + LOG_ERR("Failed to preserve downgrade-compatible bonds: %d", ret); + return ret; + } + } + } - ret = button_pressed(BUTTON_4, &pressed); +#if defined(CONFIG_MCUMGR_TRANSPORT_BT_DYNAMIC_SVC_REGISTRATION) && \ + !defined(CONFIG_AUDIO_BT_MGMT_DFU) + /* Unregister SMP (Simple Management Protocol) service if DFU is not enabled */ + ret = smp_bt_unregister(); if (ret) { + LOG_ERR("Failed to unregister SMP service: %d", ret); return ret; } - - if (pressed) { - ret = bt_mgmt_ctlr_cfg_init(false); - if (ret) { - return ret; - } - /* This call will not return */ - bt_mgmt_dfu_start(); - } - -#endif /* CONFIG_AUDIO_BT_MGMT_DFU */ - +#endif ret = bt_mgmt_ctlr_cfg_init(IS_ENABLED(CONFIG_WDT_CTLR)); if (ret) { return ret; @@ -490,7 +540,17 @@ int bt_mgmt_init(void) } if (IS_ENABLED(CONFIG_BT_CONN)) { - bt_conn_cb_register(&conn_callbacks); + ret = bt_conn_cb_register(&conn_callbacks); + if (ret) { + LOG_ERR("Failed to register conn callbacks: %d", ret); + return ret; + } + + ret = bt_conn_auth_info_cb_register(&conn_auth_info_callbacks); + if (ret) { + LOG_ERR("Failed to register conn auth info callbacks: %d", ret); + return ret; + } } if (IS_ENABLED(CONFIG_BT_PERIPHERAL) || IS_ENABLED(CONFIG_BT_BROADCASTER)) { diff --git a/src/bluetooth/bt_management/bt_mgmt.h b/src/bluetooth/bt_management/bt_mgmt.h index e5b889a0..9d181ce6 100644 --- a/src/bluetooth/bt_management/bt_mgmt.h +++ b/src/bluetooth/bt_management/bt_mgmt.h @@ -4,221 +4,232 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ - #ifndef _BT_MGMT_H_ - #define _BT_MGMT_H_ - - #include - #include - #include - - #define LE_AUDIO_EXTENDED_ADV_NAME \ - BT_LE_ADV_PARAM(BT_LE_ADV_OPT_EXT_ADV | BT_LE_ADV_OPT_USE_NAME, \ - CONFIG_BLE_ACL_EXT_ADV_INT_MIN, CONFIG_BLE_ACL_EXT_ADV_INT_MAX, NULL) - - #define LE_AUDIO_EXTENDED_ADV_CONN_NAME \ - BT_LE_ADV_PARAM(BT_LE_ADV_OPT_EXT_ADV | BT_LE_ADV_OPT_CONN | BT_LE_ADV_OPT_USE_NAME, \ - CONFIG_BLE_ACL_EXT_ADV_INT_MIN, CONFIG_BLE_ACL_EXT_ADV_INT_MAX, NULL) - - #define LE_AUDIO_EXTENDED_ADV_CONN_NAME_FILTER \ - BT_LE_ADV_PARAM(BT_LE_ADV_OPT_EXT_ADV | BT_LE_ADV_OPT_CONN | BT_LE_ADV_OPT_USE_NAME | \ - BT_LE_ADV_OPT_FILTER_CONN, \ - CONFIG_BLE_ACL_EXT_ADV_INT_MIN, CONFIG_BLE_ACL_EXT_ADV_INT_MAX, NULL) - - #define LE_AUDIO_PERIODIC_ADV \ - BT_LE_PER_ADV_PARAM(CONFIG_BLE_ACL_PER_ADV_INT_MIN, CONFIG_BLE_ACL_PER_ADV_INT_MAX, \ - BT_LE_PER_ADV_OPT_NONE) - - #define BT_LE_ADV_FAST_CONN \ - BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONN, BT_GAP_ADV_FAST_INT_MIN_1, BT_GAP_ADV_FAST_INT_MAX_1, \ - NULL) - - /* Broadcast name can be max 32 bytes long, so this will be the limit for both. - * Add one for '\0' at the end. - */ - #define BLE_SEARCH_NAME_MAX_LEN 33 - - #if (CONFIG_SCAN_MODE_ACTIVE) - #define NRF5340_AUDIO_GATEWAY_SCAN_TYPE BT_LE_SCAN_TYPE_ACTIVE - #define NRF5340_AUDIO_GATEWAY_SCAN_PARAMS BT_LE_SCAN_ACTIVE - #elif (CONFIG_SCAN_MODE_PASSIVE) - #define NRF5340_AUDIO_GATEWAY_SCAN_TYPE BT_LE_SCAN_TYPE_PASSIVE - #define NRF5340_AUDIO_GATEWAY_SCAN_PARAMS BT_LE_SCAN_PASSIVE - #else - #error "Select either CONFIG_SCAN_MODE_ACTIVE or CONFIG_SCAN_MODE_PASSIVE" - #endif - - enum bt_mgmt_scan_type { - BT_MGMT_SCAN_TYPE_CONN = 1, - BT_MGMT_SCAN_TYPE_BROADCAST = 2, - }; - - extern int bonded_device_count; - - #define BRDCAST_ID_NOT_USED (BT_AUDIO_BROADCAST_ID_MAX + 1) - - #ifdef __cplusplus - extern "C" { - #endif - - /** - * @brief Get the numbers of connected members of a given 'Set Identity Resolving Key' (SIRK). - * The SIRK shall be set through bt_mgmt_scan_sirk_set() before calling this function. - * - * @param[out] num_filled The number of connected set members. - */ - void bt_mgmt_set_size_filled_get(uint8_t *num_filled); - - /** - * @brief Set 'Set Identity Resolving Key' (SIRK). - * Used for searching for other member of the same set. - * - * @param[in] sirk Pointer to the Set Identity Resolving Key to store. - */ - void bt_mgmt_scan_sirk_set(uint8_t const *const sirk); - - /** - * @brief Load advertising data into an advertising buffer. - * - * @param[out] adv_buf Pointer to the advertising buffer to load. - * @param[in,out] index Next free index in the advertising buffer. - * @param[in] adv_buf_vacant Number of free advertising buffers. - * @param[in] data_len Length of the data. - * @param[in] type Type of the data. - * @param[in] data Data to store in the buffer, can be a pointer or value. - * - * @return 0 if success, error otherwise. - */ - int bt_mgmt_adv_buffer_put(struct bt_data *const adv_buf, uint32_t *index, size_t adv_buf_vacant, - size_t data_len, uint8_t type, void *data); - - /** - * @brief Start scanning for advertisements. - * - * @param[in] scan_intvl Scan interval in units of 0.625ms. - * Valid range: 0x4 - 0xFFFF; can be 0. - * @param[in] scan_win Scan window in units of 0.625ms. - * Valid range: 0x4 - 0xFFFF; can be 0. - * @param[in] type Type to scan for: ACL connection or broadcaster. - * @param[in] name Name to search for. Depending on @p type of search, - * device name or broadcast name. Can be max - * BLE_SEARCH_NAME_MAX_LEN long; everything beyond that value - * will be cropped. Can be NULL. Shall be '\0' terminated. - * @param[in] brdcast_id Broadcast ID to search for. Only valid if @p type is - * BT_MGMT_SCAN_TYPE_BROADCAST. If both @p name and @p brdcast_id are - * provided, then brdcast_id will be used. - * Set to BRDCAST_ID_NOT_USED if not in use. - * - * @note To restart scanning, call this function with all 0s and NULL, except for @p type. - * The same scanning parameters as when bt_mgmt_scan_start was last called will then - * be used. - * - * @return 0 if success, error otherwise. - */ - int bt_mgmt_scan_start(uint16_t scan_intvl, uint16_t scan_win, enum bt_mgmt_scan_type type, - char const *const name, uint32_t brdcast_id); - - /** - * @brief Add manufacturer ID UUID to the advertisement packet. - * - * @param[out] uuid_buf Buffer being populated with UUIDs. - * @param[in] company_id 16 bit UUID specific to the company. - * - * @return 0 for success, error otherwise. - */ - int bt_mgmt_manufacturer_uuid_populate(struct net_buf_simple *uuid_buf, uint16_t company_id); - - /** - * @brief Stop periodic advertising. - * - * @note Must be called before bt_mgmt_ext_adv_stop. - * - * @param[in] ext_adv_index Index of the advertising set to stop. - * - * @return 0 if success, error otherwise. - */ - int bt_mgmt_per_adv_stop(uint8_t ext_adv_index); - - /** - * @brief Stop extended advertising. - * - * @return 0 if success, error otherwise. - */ - int bt_mgmt_ext_adv_stop(uint8_t ext_adv_index); - - /** - * @brief Create and start advertising for an ACL connection. - * - * @param[in] ext_adv_index Index of the advertising set to start. - * @param[in] ext_adv The data to be put in the extended advertisement. - * @param[in] ext_adv_size Size of @p ext_adv. - * @param[in] per_adv The data for the periodic advertisement; can be NULL. - * @param[in] per_adv_size Size of @p per_adv. - * @param[in] connectable Specify if advertisement should be connectable or not. - * - * @note To restart advertising, call this function with all 0s and NULL, except for - * connectable. The same advertising parameters as when bt_mgmt_adv_start was last - * called will then be used. - * - * @return 0 if success, error otherwise. - */ - int bt_mgmt_adv_start(uint8_t ext_adv_index, const struct bt_data *ext_adv, size_t ext_adv_size, - const struct bt_data *per_adv, size_t per_adv_size, bool connectable); - - /** - * @brief Get the number of active connections. - * - * @param[out] num_conn The number of active connections. - */ - void bt_mgmt_num_conn_get(uint8_t *num_conn); - - /** - * @brief Clear all bonded devices. - * - * @return 0 if success, error otherwise. - */ - int bt_mgmt_bonding_clear(void); - - /** - * @brief Scan delegator feature initialization. - */ - void bt_mgmt_scan_delegator_init(void); - - /** - * @brief Get the pointer to broadcast code. - * - * @param[out] broadcast_code_ptr Pointer to the broadcast code. - */ - void bt_mgmt_broadcast_code_ptr_get(uint8_t **broadcast_code_ptr); - - /** - * @brief Delete a periodic advertisement sync. - * - * @param[in] pa_sync Pointer to the periodic advertisement sync to delete. - * - * @return 0 if success, error otherwise. - */ - int bt_mgmt_pa_sync_delete(struct bt_le_per_adv_sync *pa_sync); - - /** - * @brief Disconnect from a remote device or cancel the pending connection. - * - * @param[in] conn Connection to disconnect. - * @param[in] reason Reason code for the disconnection, as specified in - * HCI Error Codes, BT Core Spec [Vol 1, Part F]. - * - * @return 0 if success, error otherwise. - */ - int bt_mgmt_conn_disconnect(struct bt_conn *conn, uint8_t reason); - - /** - * @brief Initialize the Bluetooth management module. - * - * @return 0 if success, error otherwise. - */ - int bt_mgmt_init(void); - - #ifdef __cplusplus - } - #endif - - #endif /* _BT_MGMT_H_ */ \ No newline at end of file +/** @file + * @defgroup audio_app_bt_mgmt Audio Bluetooth Management + * @{ + * @brief Bluetooth management API for Audio applications. + * + * This module provides comprehensive Bluetooth management functionality including device + * initialization, connection handling, scanning, advertising, and security management. + */ + +#ifndef _BT_MGMT_H_ +#define _BT_MGMT_H_ + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define LE_AUDIO_EXTENDED_ADV \ + BT_LE_ADV_PARAM(BT_LE_ADV_OPT_EXT_ADV, CONFIG_BLE_ACL_EXT_ADV_INT_MIN, \ + CONFIG_BLE_ACL_EXT_ADV_INT_MAX, NULL) + +#define LE_AUDIO_EXTENDED_ADV_CONN \ + BT_LE_ADV_PARAM(BT_LE_ADV_OPT_EXT_ADV | BT_LE_ADV_OPT_CONN, \ + CONFIG_BLE_ACL_EXT_ADV_INT_MIN, CONFIG_BLE_ACL_EXT_ADV_INT_MAX, NULL) + +#define LE_AUDIO_EXTENDED_ADV_CONN_FILTER \ + BT_LE_ADV_PARAM(BT_LE_ADV_OPT_EXT_ADV | BT_LE_ADV_OPT_CONN | BT_LE_ADV_OPT_FILTER_CONN, \ + CONFIG_BLE_ACL_EXT_ADV_INT_MIN, CONFIG_BLE_ACL_EXT_ADV_INT_MAX, NULL) + +#define LE_AUDIO_PERIODIC_ADV \ + BT_LE_PER_ADV_PARAM(CONFIG_BLE_ACL_PER_ADV_INT_MIN, CONFIG_BLE_ACL_PER_ADV_INT_MAX, \ + BT_LE_PER_ADV_OPT_NONE) + +#define BT_LE_ADV_FAST_CONN \ + BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONN, BT_GAP_ADV_FAST_INT_MIN_1, BT_GAP_ADV_FAST_INT_MAX_1, \ + NULL) + +#define BLE_SEARCH_NAME_MAX_LEN 33 + +#if (CONFIG_SCAN_MODE_ACTIVE) +#define NRF_AUDIO_GATEWAY_SCAN_TYPE BT_LE_SCAN_TYPE_ACTIVE +#define NRF_AUDIO_GATEWAY_SCAN_PARAMS BT_LE_SCAN_ACTIVE +#elif (CONFIG_SCAN_MODE_PASSIVE) +#define NRF_AUDIO_GATEWAY_SCAN_TYPE BT_LE_SCAN_TYPE_PASSIVE +#define NRF_AUDIO_GATEWAY_SCAN_PARAMS BT_LE_SCAN_PASSIVE +#else +#error "Select either CONFIG_SCAN_MODE_ACTIVE or CONFIG_SCAN_MODE_PASSIVE" +#endif + +/** + * @brief Bluetooth management scan type enumeration. + */ +enum bt_mgmt_scan_type { + BT_MGMT_SCAN_TYPE_CONN = 1, /**< Scan for connection (unicast) devices */ + BT_MGMT_SCAN_TYPE_BROADCAST = 2, /**< Scan for broadcast devices */ +}; + +#define BRDCAST_ID_NOT_USED (BT_AUDIO_BROADCAST_ID_MAX + 1) + +/** + * @brief Get the numbers of connected members of a given 'Set Identity Resolving Key' (SIRK). + * The SIRK shall be set through bt_mgmt_scan_sirk_set() before calling this function. + * + * @param[out] num_filled The number of connected set members. + */ +void bt_mgmt_set_size_filled_get(uint8_t *num_filled); + +/** + * @brief Set 'Set Identity Resolving Key' (SIRK). + * Used for searching for other member of the same set. + * + * @param[in] sirk Pointer to the Set Identity Resolving Key to store. + */ +void bt_mgmt_scan_sirk_set(uint8_t const *const sirk); + +/** + * @brief Load advertising data into an advertising buffer. + * + * @param[out] adv_buf Pointer to the advertising buffer to load. + * @param[in,out] index Next free index in the advertising buffer. + * @param[in] adv_buf_vacant Number of free advertising buffers. + * @param[in] data_len Length of the data. + * @param[in] type Type of the data. + * @param[in] data Data to store in the buffer, can be a pointer or value. + * + * @return 0 if success, error otherwise. + */ +int bt_mgmt_adv_buffer_put(struct bt_data *const adv_buf, uint32_t *index, size_t adv_buf_vacant, + size_t data_len, uint8_t type, void *data); + +/** + * @brief Start scanning for advertisements. + * + * @param[in] scan_intvl Scan interval in units of 0.625ms. + * Valid range: 0x4 - 0xFFFF; can be 0. + * @param[in] scan_win Scan window in units of 0.625ms. + * Valid range: 0x4 - 0xFFFF; can be 0. + * @param[in] type Type to scan for: ACL connection or broadcaster. + * @param[in] name Name to search for. Depending on @p type of search, + * device name or broadcast name. Can be max + * BLE_SEARCH_NAME_MAX_LEN long; everything beyond that value + * will be cropped. Can be NULL. Shall be '\0' terminated. + * @param[in] brdcast_id Broadcast ID to search for. Only valid if @p type is + * BT_MGMT_SCAN_TYPE_BROADCAST. If both @p name and @p brdcast_id are + * provided, then brdcast_id will be used. + * Set to BRDCAST_ID_NOT_USED if not in use. + * + * @note To restart scanning, call this function with all 0s and NULL, except for @p type. + * The same scanning parameters as when bt_mgmt_scan_start was last called will then + * be used. + * + * @return 0 if success, error otherwise. + */ +int bt_mgmt_scan_start(uint16_t scan_intvl, uint16_t scan_win, enum bt_mgmt_scan_type type, + char const *const name, uint32_t brdcast_id); + +/** + * @brief Add manufacturer ID UUID to the advertisement packet. + * + * @param[out] uuid_buf Buffer being populated with UUIDs. + * @param[in] company_id 16 bit UUID specific to the company. + * + * @return 0 for success, error otherwise. + */ +int bt_mgmt_manufacturer_uuid_populate(struct net_buf_simple *uuid_buf, uint16_t company_id); + +/** + * @brief Stop periodic advertising. + * + * @note Must be called before bt_mgmt_ext_adv_stop. + * + * @param[in] ext_adv_index Index of the advertising set to stop. + * + * @return 0 if success, error otherwise. + */ +int bt_mgmt_per_adv_stop(uint8_t ext_adv_index); + +/** + * @brief Stop extended advertising. + * + * @return 0 if success, error otherwise. + */ +int bt_mgmt_ext_adv_stop(uint8_t ext_adv_index); + +/** + * @brief Create and start advertising for an ACL connection. + * + * @param[in] ext_adv_index Index of the advertising set to start. + * @param[in] ext_adv The data to be put in the extended advertisement. + * @param[in] ext_adv_size Size of @p ext_adv. + * @param[in] per_adv The data for the periodic advertisement; can be NULL. + * @param[in] per_adv_size Size of @p per_adv. + * @param[in] connectable Specify if advertisement should be connectable or not. + * + * @note To restart advertising, call this function with all 0s and NULL, except for + * connectable. The same advertising parameters as when bt_mgmt_adv_start was last + * called will then be used. + * + * @return 0 if success, error otherwise. + */ +int bt_mgmt_adv_start(uint8_t ext_adv_index, const struct bt_data *ext_adv, size_t ext_adv_size, + const struct bt_data *per_adv, size_t per_adv_size, bool connectable); + +/** + * @brief Get the number of active connections. + * + * @param[out] num_conn The number of active connections. + */ +void bt_mgmt_num_conn_get(uint8_t *num_conn); +void bt_mgmt_num_bonds_get(uint8_t *num_bonds); + +/** + * @brief Clear all bonded devices. + * + * @return 0 if success, error otherwise. + */ +int bt_mgmt_bonding_clear(void); + +/** + * @brief Scan delegator feature initialization. + */ +void bt_mgmt_scan_delegator_init(void); + +/** + * @brief Get the pointer to broadcast code. + * + * @param[out] broadcast_code_ptr Pointer to the broadcast code. + */ +void bt_mgmt_broadcast_code_ptr_get(uint8_t **broadcast_code_ptr); + +/** + * @brief Delete a periodic advertisement sync. + * + * @param[in] pa_sync Pointer to the periodic advertisement sync to delete. + * + * @return 0 if success, error otherwise. + */ +int bt_mgmt_pa_sync_delete(struct bt_le_per_adv_sync *pa_sync); + +/** + * @brief Disconnect from a remote device or cancel the pending connection. + * + * @param[in] conn Connection to disconnect. + * @param[in] reason Reason code for the disconnection, as specified in + * HCI Error Codes, BT Core Spec [Vol 1, Part F]. + * + * @return 0 if success, error otherwise. + */ +int bt_mgmt_conn_disconnect(struct bt_conn *conn, uint8_t reason); + +/** + * @brief Initialize the Bluetooth management module. + * + * @return 0 if success, error otherwise. + */ +int bt_mgmt_init(void); + +#ifdef __cplusplus +} +#endif + +/** + * @} + */ + +#endif /* _BT_MGMT_H_ */ diff --git a/src/bluetooth/bt_management/bt_mgmt_bond_storage.c b/src/bluetooth/bt_management/bt_mgmt_bond_storage.c new file mode 100644 index 00000000..3cfe3645 --- /dev/null +++ b/src/bluetooth/bt_management/bt_mgmt_bond_storage.c @@ -0,0 +1,58 @@ +/* SPDX-License-Identifier: Apache-2.0 */ + +#include +#include +#include + +#include "keys.h" +#include "settings.h" +#include "bt_mgmt_bond_storage.h" + +/* NCS 3.4.1 prepends four configuration bytes to the NCS 3.0.1 bond record. + * Keep the flash representation readable by firmware 2.2.9. The new SDK already + * accepts this legacy representation and adds its header when loading it. + * Reject build/configuration changes until their storage layout is reviewed. + */ +#define BOND_CONFIG_SIZE (offsetof(struct bt_keys, enc_size) - \ + offsetof(struct bt_keys, storage_start)) +BUILD_ASSERT(BOND_CONFIG_SIZE == 4 && BT_KEYS_STORAGE_LEN == 56); +BUILD_ASSERT(IS_ENABLED(CONFIG_BT_SMP_SC_PAIR_ONLY) && + !IS_ENABLED(CONFIG_BT_SIGNING) && !IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST)); + +int __real_bt_settings_store_keys(uint8_t id, const bt_addr_le_t *addr, + const void *value, size_t val_len); + +/* Wrap all SDK bond writes, including newly paired phones and key updates. */ +int __wrap_bt_settings_store_keys(uint8_t id, const bt_addr_le_t *addr, + const void *value, size_t val_len) +{ + const uint8_t *record = value; + + if (val_len != BT_KEYS_STORAGE_LEN || record[0] != 17 || + sys_get_le24(&record[1]) != BT_KEYS_CFG_SC_PAIR_ONLY) { + return -EINVAL; + } + + return __real_bt_settings_store_keys(id, addr, record + BOND_CONFIG_SIZE, + val_len - BOND_CONFIG_SIZE); +} + +static void store_legacy_bond(struct bt_keys *keys, void *data) +{ + int *err = data; + + if (*err == 0) { + *err = bt_keys_store(keys); + } +} + +int bt_mgmt_bond_storage_compat(void) +{ + int err = 0; + + /* Also convert bonds written by an earlier 2.2.10 build, before advertising. + * NVS skips writes when the stored value is already identical. + */ + bt_keys_foreach_type(BT_KEYS_ALL, store_legacy_bond, &err); + return err; +} diff --git a/src/bluetooth/bt_management/bt_mgmt_bond_storage.h b/src/bluetooth/bt_management/bt_mgmt_bond_storage.h new file mode 100644 index 00000000..4e19fdf9 --- /dev/null +++ b/src/bluetooth/bt_management/bt_mgmt_bond_storage.h @@ -0,0 +1,13 @@ +/* SPDX-License-Identifier: Apache-2.0 */ + +#ifndef BT_MGMT_BOND_STORAGE_H_ +#define BT_MGMT_BOND_STORAGE_H_ + +/** + * Persist loaded bonds in the format shared by firmware 2.2.9 and 2.2.10. + * Call after settings_load(), before advertising. Does not change in-memory + * keys or Bluetooth identities. Returns zero on success, or a storage error. + */ +int bt_mgmt_bond_storage_compat(void); + +#endif diff --git a/src/bluetooth/bt_management/controller_config/bt_mgmt_ctlr_cfg.c b/src/bluetooth/bt_management/controller_config/bt_mgmt_ctlr_cfg.c index 8bd09f19..73a462b1 100644 --- a/src/bluetooth/bt_management/controller_config/bt_mgmt_ctlr_cfg.c +++ b/src/bluetooth/bt_management/controller_config/bt_mgmt_ctlr_cfg.c @@ -11,6 +11,7 @@ #include #include #include +#include #include "macros_common.h" @@ -19,6 +20,8 @@ LOG_MODULE_REGISTER(bt_mgmt_ctlr_cfg, CONFIG_BT_MGMT_CTLR_CFG_LOG_LEVEL); #define COMPANY_ID_NORDIC 0x0059 +#if defined(CONFIG_WDT_CTLR) + #define WDT_TIMEOUT_MS 3000 #define CTLR_POLL_INTERVAL_MS (WDT_TIMEOUT_MS - 1000) @@ -36,7 +39,8 @@ struct k_work_queue_config ctrl_poll_work_q_config = { }; static void ctlr_poll_timer_handler(struct k_timer *timer_id); -static int wdt_ch_id; +static int wdt_ch_id = -1; +static atomic_t watchdog_active; K_TIMER_DEFINE(ctlr_poll_timer, ctlr_poll_timer_handler, NULL); @@ -46,7 +50,14 @@ static void work_ctlr_poll_handler(struct k_work *work) int ret; uint16_t manufacturer = 0; + if (!atomic_get(&watchdog_active)) { + return; + } + ret = bt_mgmt_ctlr_cfg_manufacturer_get(false, &manufacturer); + if (!atomic_get(&watchdog_active)) { + return; + } ERR_CHK_MSG(ret, "Failed to contact net core"); ret = task_wdt_feed(wdt_ch_id); @@ -58,6 +69,10 @@ static void ctlr_poll_timer_handler(struct k_timer *timer_id) ARG_UNUSED(timer_id); int ret; + if (!atomic_get(&watchdog_active)) { + return; + } + ret = k_work_submit_to_queue(&ctrl_poll_work_q, &work_ctlr_poll); if (ret < 0) { LOG_ERR("Work q submit failed: %d", ret); @@ -68,9 +83,13 @@ static void wdt_timeout_cb(int channel_id, void *user_data) { ARG_UNUSED(channel_id); ARG_UNUSED(user_data); - ERR_CHK_MSG(-ETIMEDOUT, "No response from IPC or controller"); + if (atomic_get(&watchdog_active)) { + ERR_CHK_MSG(-ETIMEDOUT, "No response from IPC or controller"); + } } +#endif /* CONFIG_WDT_CTLR */ + int bt_mgmt_ctlr_cfg_manufacturer_get(bool print_version, uint16_t *manufacturer) { int ret; @@ -84,13 +103,14 @@ int bt_mgmt_ctlr_cfg_manufacturer_get(bool print_version, uint16_t *manufacturer struct bt_hci_rp_read_local_version_info *rp = (void *)rsp->data; if (print_version) { - if (rp->manufacturer == COMPANY_ID_NORDIC) { + if (sys_le16_to_cpu(rp->manufacturer) == COMPANY_ID_NORDIC) { /* NOTE: The string below is used by the Nordic CI system */ LOG_INF("Controller: SoftDevice: Version %s (0x%02x), Revision %d", bt_hci_get_ver_str(rp->hci_version), rp->hci_version, rp->hci_revision); } else { LOG_ERR("Unsupported controller"); + net_buf_unref(rsp); return -EPERM; } } @@ -112,6 +132,8 @@ int bt_mgmt_ctlr_cfg_init(bool watchdog_enable) return ret; } + +#if defined(CONFIG_WDT_CTLR) if (watchdog_enable) { ret = task_wdt_init(NULL); if (ret != 0) { @@ -131,16 +153,33 @@ int bt_mgmt_ctlr_cfg_init(bool watchdog_enable) &ctrl_poll_work_q_config); k_work_init(&work_ctlr_poll, work_ctlr_poll_handler); + atomic_set(&watchdog_active, 1); k_timer_start(&ctlr_poll_timer, K_MSEC(CTLR_POLL_INTERVAL_MS), K_MSEC(CTLR_POLL_INTERVAL_MS)); } +#else + ARG_UNUSED(watchdog_enable); +#endif return 0; } int bt_mgmt_stop_watchdog(void) { +#if defined(CONFIG_WDT_CTLR) + struct k_work_sync sync; + + if (!atomic_cas(&watchdog_active, 1, 0)) { + return 0; + } + k_timer_stop(&ctlr_poll_timer); + /* Drain a pending poll before deleting its watchdog channel. */ + k_work_cancel_sync(&work_ctlr_poll, &sync); int ret = task_wdt_delete(wdt_ch_id); + wdt_ch_id = -1; return ret; +#else + return 0; +#endif } diff --git a/src/bluetooth/bt_management/controller_config/bt_mgmt_ctlr_cfg_internal.h b/src/bluetooth/bt_management/controller_config/bt_mgmt_ctlr_cfg_internal.h index 2530bae1..c4b39ca4 100644 --- a/src/bluetooth/bt_management/controller_config/bt_mgmt_ctlr_cfg_internal.h +++ b/src/bluetooth/bt_management/controller_config/bt_mgmt_ctlr_cfg_internal.h @@ -34,7 +34,7 @@ int bt_mgmt_ctlr_cfg_manufacturer_get(bool print_version, uint16_t *manufacturer */ int bt_mgmt_ctlr_cfg_init(bool watchdog_enable); -int bt_mgmt_stop_watchdog(); +int bt_mgmt_stop_watchdog(void); #ifdef __cplusplus } diff --git a/src/bluetooth/bt_management/dfu/bt_mgmt_dfu.c b/src/bluetooth/bt_management/dfu/bt_mgmt_dfu.c index d1560b9a..2203a04a 100644 --- a/src/bluetooth/bt_management/dfu/bt_mgmt_dfu.c +++ b/src/bluetooth/bt_management/dfu/bt_mgmt_dfu.c @@ -7,6 +7,7 @@ /* Override compiler definition to use size-bounded string copying and concatenation function */ #define _BSD_SOURCE #include "bt_mgmt_dfu_internal.h" +#include "bt_mgmt_ctlr_cfg_internal.h" #include #include @@ -89,7 +90,7 @@ static void dfu_set_bt_name(void) } #if (CONFIG_AUDIO_DEV == GATEWAY) - ret = strlcat(name, GW_TAG, CONFIG_BT_DEVICE_NAME_MAX); + ret = strlcat(name, CHANNEL_GW_TAG, CONFIG_BT_DEVICE_NAME_MAX); if (ret >= CONFIG_BT_DEVICE_NAME_MAX) { LOG_ERR("Failed to set full BT name, will truncate"); } @@ -124,6 +125,7 @@ static void dfu_set_bt_name(void) void bt_mgmt_dfu_start(void) { LOG_INF("Entering SMP server mode"); + (void)bt_mgmt_stop_watchdog(); bt_conn_cb_register(&dfu_conn_callbacks); dfu_set_bt_name(); diff --git a/src/bluetooth/bt_management/scanning/Kconfig b/src/bluetooth/bt_management/scanning/Kconfig index ec8c2301..8010a47d 100644 --- a/src/bluetooth/bt_management/scanning/Kconfig +++ b/src/bluetooth/bt_management/scanning/Kconfig @@ -8,7 +8,7 @@ rsource "Kconfig.defaults" menu "Scanning" -choice NRF5340_AUDIO_GATEWAY_SCAN_MODE +choice NRF_AUDIO_GATEWAY_SCAN_MODE prompt "Select Scan Mode to find Unicast Headset" default SCAN_MODE_PASSIVE @@ -20,6 +20,37 @@ config SCAN_MODE_ACTIVE endchoice +choice BT_MGMT_CONNECT_BY + prompt "Scan Filter" + default BT_MGMT_CONNECT_BY_NAME + +config BT_MGMT_CONNECT_BY_NAME + bool "Name" + help + If enabled, the scanner will look for a specific name instead of just connecting + to the first device it finds. The name to look for is CONFIG_BT_DEVICE_NAME. + This is useful for testing with a known device. + Note: This is only used if no CSIP Set Identity Resolving Key (SIRK) is available + in the bond information. + +config BT_MGMT_CONNECT_BY_ADDR + bool "Address" + help + If enabled, the scanner will look for a specific address instead of a name. + The address to look for is set in bt_mgmt_scan_for_conn.c in the addr_check() + function. This is useful for testing with a known device. + Note: This is only used if no CSIP Set Identity Resolving Key (SIRK) is available + in the bond information. + +endchoice + +config CONNECT_TO_BT_ADDR_LE_STR + string "Bluetooth LE Address to connect to" + default "00:00:00:00:00:00" + help + The Bluetooth LE address to connect to. This is useful for testing with a + specific device. + #----------------------------------------------------------------------------# menu "Connection" diff --git a/src/bluetooth/bt_management/scanning/bt_mgmt_scan.c b/src/bluetooth/bt_management/scanning/bt_mgmt_scan.c index 94cd2567..6e5f8559 100644 --- a/src/bluetooth/bt_management/scanning/bt_mgmt_scan.c +++ b/src/bluetooth/bt_management/scanning/bt_mgmt_scan.c @@ -55,13 +55,11 @@ int bt_mgmt_scan_start(uint16_t scan_intvl, uint16_t scan_win, enum bt_mgmt_scan scan_window = scan_win; } - struct bt_le_scan_param *scan_param = NULL; - -#if defined(CONFIG_BT_CENTRAL) || defined(CONFIG_BT_BAP_BROADCAST_SINK) - scan_param = - BT_LE_SCAN_PARAM(NRF5340_AUDIO_GATEWAY_SCAN_TYPE, BT_LE_SCAN_OPT_FILTER_DUPLICATE, + struct bt_le_scan_param *scan_param = + BT_LE_SCAN_PARAM(NRF_AUDIO_GATEWAY_SCAN_TYPE, BT_LE_SCAN_OPT_FILTER_DUPLICATE, scan_interval, scan_window); -#endif + /* Peripheral-only builds have neither scanning role enabled. */ + ARG_UNUSED(scan_param); if (type == BT_MGMT_SCAN_TYPE_CONN && IS_ENABLED(CONFIG_BT_CENTRAL)) { ret = bt_mgmt_scan_for_conn_start(scan_param, srch_name); diff --git a/src/bluetooth/bt_management/scanning/bt_mgmt_scan_for_broadcast_internal.h b/src/bluetooth/bt_management/scanning/bt_mgmt_scan_for_broadcast_internal.h index f4c89eac..d02a962a 100644 --- a/src/bluetooth/bt_management/scanning/bt_mgmt_scan_for_broadcast_internal.h +++ b/src/bluetooth/bt_management/scanning/bt_mgmt_scan_for_broadcast_internal.h @@ -4,6 +4,14 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @addtogroup audio_app_bt_mgmt + * @{ + * @defgroup bt_mgmt_scan_for_broadcast_internal Functions for scanning for a broadcast. + * @{ + * @brief Helper functions to manage scanning for a broadcast. + */ + #ifndef _BT_MGMT_SCAN_FOR_BROADCAST_INTERNAL_H_ #define _BT_MGMT_SCAN_FOR_BROADCAST_INTERNAL_H_ @@ -21,4 +29,9 @@ int bt_mgmt_scan_for_broadcast_start(struct bt_le_scan_param *scan_param, char const *const name, uint32_t brdcast_id); +/** + * @} + * @} + */ + #endif /* _BT_MGMT_SCAN_FOR_BROADCAST_INTERNAL_H_ */ diff --git a/src/bluetooth/bt_management/scanning/bt_mgmt_scan_for_conn.c b/src/bluetooth/bt_management/scanning/bt_mgmt_scan_for_conn.c index 425e0730..3454f460 100644 --- a/src/bluetooth/bt_management/scanning/bt_mgmt_scan_for_conn.c +++ b/src/bluetooth/bt_management/scanning/bt_mgmt_scan_for_conn.c @@ -19,6 +19,7 @@ #include LOG_MODULE_DECLARE(bt_mgmt_scan); +#define BT_ADDR_LE_STR CONFIG_CONNECT_TO_BT_ADDR_LE_STR /* Replace with desired address */ #define CONNECTION_PARAMETERS \ BT_LE_CONN_PARAM(CONFIG_BLE_ACL_CONN_INTERVAL, CONFIG_BLE_ACL_CONN_INTERVAL, \ CONFIG_BLE_ACL_SLAVE_LATENCY, CONFIG_BLE_ACL_SUP_TIMEOUT) @@ -184,6 +185,73 @@ static bool device_name_check(struct bt_data *data, void *user_data) return true; } +/** + * @brief Check the advertising data for the matching address. + * + * @param[in] data The advertising data to be checked. + * @param[in] user_data Pointer to the address. + * + * @retval false Stop going through adv data. + * @retval true Continue checking the data. + */ +static bool addr_check(struct bt_data *data, void *user_data) +{ + int ret; + char addr_string[BT_ADDR_LE_STR_LEN]; + bt_addr_le_t *addr = user_data; + struct bt_conn *conn = NULL; + + bt_addr_le_to_str(addr, addr_string, BT_ADDR_LE_STR_LEN); + + bt_addr_le_t cmp_addr; + /* BT_ADDR_LE_STR defined at the start of this file */ + + ret = bt_addr_le_from_str(BT_ADDR_LE_STR, "public", &cmp_addr); + if (ret) { + LOG_ERR("Failed to create bt_addr_le from string: %d", ret); + return true; + } + + bt_addr_le_to_str(&cmp_addr, addr_string, BT_ADDR_LE_STR_LEN); + + if (!bt_addr_le_cmp(addr, &cmp_addr)) { + /* Check if the device is still connected due to waiting for ACL timeout */ + if (conn_exist_check(addr)) { + /* Device is already connected, stop parsing the adv data */ + return false; + } + + LOG_INF("Device found: %s", srch_name); + + bt_le_scan_cb_unregister(&scan_callback); + cb_registered = false; + + ret = bt_le_scan_stop(); + if (ret) { + LOG_ERR("Stop scan failed: %d", ret); + } + + bt_addr_le_to_str(addr, addr_string, BT_ADDR_LE_STR_LEN); + + LOG_INF("Creating connection to device: %s", addr_string); + + ret = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, CONNECTION_PARAMETERS, &conn); + if (ret) { + LOG_ERR("Could not init connection: %d", ret); + + ret = bt_mgmt_scan_start(0, 0, BT_MGMT_SCAN_TYPE_CONN, NULL, + BRDCAST_ID_NOT_USED); + if (ret) { + LOG_ERR("Failed to restart scanning: %d", ret); + } + } + + return false; + } + + return true; +} + /** * @brief Check the advertising data for the matching 'Set Identity Resolving Key' (SIRK). * @@ -267,7 +335,11 @@ static void scan_recv_cb(const struct bt_le_scan_recv_info *info, struct net_buf /* Note: May lead to connection creation */ if (bonded_num < CONFIG_BT_MAX_PAIRED) { if (server_sirk == NULL) { - bt_data_parse(ad, device_name_check, (void *)info->addr); + if (IS_ENABLED(CONFIG_BT_MGMT_CONNECT_BY_ADDR)) { + bt_data_parse(ad, addr_check, (void *)info->addr); + } else { + bt_data_parse(ad, device_name_check, (void *)info->addr); + } } else { bt_data_parse(ad, csip_found, (void *)info->addr); } diff --git a/src/bluetooth/bt_management/scanning/bt_mgmt_scan_for_conn_internal.h b/src/bluetooth/bt_management/scanning/bt_mgmt_scan_for_conn_internal.h index f3d03ba7..1e1497f8 100644 --- a/src/bluetooth/bt_management/scanning/bt_mgmt_scan_for_conn_internal.h +++ b/src/bluetooth/bt_management/scanning/bt_mgmt_scan_for_conn_internal.h @@ -4,6 +4,14 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @addtogroup audio_app_bt_mgmt + * @{ + * @defgroup bt_mgmt_scan_for_conn_internal Functions for scanning for a connection. + * @{ + * @brief Helper functions to manage scanning for a connection. + */ + #ifndef _BT_MGMT_SCAN_FOR_CONN_INTERNAL_H_ #define _BT_MGMT_SCAN_FOR_CONN_INTERNAL_H_ @@ -19,4 +27,9 @@ */ int bt_mgmt_scan_for_conn_start(struct bt_le_scan_param *scan_param, char const *const name); +/** + * @} + * @} + */ + #endif /* _BT_MGMT_SCAN_FOR_CONN_INTERNAL_H_ */ diff --git a/src/bluetooth/bt_rendering_and_capture/CMakeLists.txt b/src/bluetooth/bt_rendering_and_capture/CMakeLists.txt index b7f7ac8c..b6f744b7 100644 --- a/src/bluetooth/bt_rendering_and_capture/CMakeLists.txt +++ b/src/bluetooth/bt_rendering_and_capture/CMakeLists.txt @@ -5,22 +5,19 @@ # zephyr_library_include_directories( - volume + volume ) -target_sources(app PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/bt_rendering_and_capture.c) +target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/bt_rendering_and_capture.c) -if (CONFIG_BT_VCP_VOL_CTLR AND CONFIG_BT_VCP_VOL_REND) - message(FATAL_ERROR "No support for vol controller and renderer on same device") +if(CONFIG_BT_VCP_VOL_CTLR AND CONFIG_BT_VCP_VOL_REND) + message(FATAL_ERROR "No support for vol controller and renderer on same device") endif() -if (CONFIG_BT_VCP_VOL_REND) -target_sources(app PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/volume/bt_vol_rend.c) +if(CONFIG_BT_VCP_VOL_REND) + target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/volume/bt_vol_rend.c) endif() -if (CONFIG_BT_VCP_VOL_CTLR) - target_sources(app PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/volume/bt_vol_ctlr.c) +if(CONFIG_BT_VCP_VOL_CTLR) + target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/volume/bt_vol_ctlr.c) endif() diff --git a/src/bluetooth/bt_rendering_and_capture/bt_rendering_and_capture.h b/src/bluetooth/bt_rendering_and_capture/bt_rendering_and_capture.h index 870d0e0f..af5cfb71 100644 --- a/src/bluetooth/bt_rendering_and_capture/bt_rendering_and_capture.h +++ b/src/bluetooth/bt_rendering_and_capture/bt_rendering_and_capture.h @@ -4,6 +4,15 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @defgroup audio_app_bt_rendering_and_capture Bluetooth Audio Rendering and Capture + * @{ + * @brief Bluetooth audio rendering and capture API for Audio applications. + * + * This module provides volume control and audio rendering functionality for LE Audio + * streams. + */ + #ifndef _BT_REND_H_ #define _BT_REND_H_ @@ -78,4 +87,8 @@ int bt_r_and_c_uuid_populate(struct net_buf_simple *uuid_buf); */ int bt_r_and_c_init(void); +/** + * @} + */ + #endif /* _BT_REND_H_ */ diff --git a/src/bluetooth/bt_stream/CMakeLists.txt b/src/bluetooth/bt_stream/CMakeLists.txt index 389ec2d4..967a9e80 100644 --- a/src/bluetooth/bt_stream/CMakeLists.txt +++ b/src/bluetooth/bt_stream/CMakeLists.txt @@ -5,32 +5,30 @@ # zephyr_library_include_directories( - broadcast - unicast - bt_le_audio_tx + broadcast + unicast + bt_le_audio_tx ) add_subdirectory(bt_le_audio_tx) -target_sources(app PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/le_audio.c) +target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/le_audio.c) -if (CONFIG_BT_BAP_BROADCAST_SINK) - target_sources(app PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/broadcast/broadcast_sink.c) +if(CONFIG_BT_BAP_BROADCAST_SINK) + target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/broadcast/broadcast_sink.c) endif() -if (CONFIG_BT_BAP_BROADCAST_SOURCE) - target_sources(app PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/broadcast/broadcast_source.c) +if(CONFIG_BT_BAP_BROADCAST_SOURCE) + target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/broadcast/broadcast_source.c) endif() -if (CONFIG_BT_BAP_UNICAST_CLIENT) - target_sources(app PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/unicast/unicast_client.c) +if(CONFIG_BT_BAP_UNICAST_CLIENT) + target_sources(app PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/unicast/unicast_client.c + ${CMAKE_CURRENT_SOURCE_DIR}/unicast/server_store.c + ) endif() -if (CONFIG_BT_BAP_UNICAST_SERVER) - target_sources(app PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/unicast/unicast_server.c) +if(CONFIG_BT_BAP_UNICAST_SERVER) + target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/unicast/unicast_server.c) endif() diff --git a/src/bluetooth/bt_stream/broadcast/Kconfig b/src/bluetooth/bt_stream/broadcast/Kconfig index 84646ff6..bbe9da70 100644 --- a/src/bluetooth/bt_stream/broadcast/Kconfig +++ b/src/bluetooth/bt_stream/broadcast/Kconfig @@ -132,19 +132,40 @@ config BT_AUDIO_BROADCAST_ENCRYPTION_KEY characters. Encryption keys larger than BT_ISO_BROADCAST_CODE_SIZE will be truncated to BT_ISO_BROADCAST_CODE_SIZE. +choice BT_AUDIO_BROADCAST_ID_TYPE + prompt "Broadcast ID" + depends on TRANSPORT_BIS + default BT_AUDIO_USE_BROADCAST_ID_FICR + help + Select the broadcast ID to use for the broadcast. + +config BT_AUDIO_USE_BROADCAST_ID_FICR + bool "Use the FICR broadcast ID" + depends on TRANSPORT_BIS + help + Use the FICR broadcast ID. This is a fixed broadcast ID that is unique to each device. + It is stored in the FICR NFC.TAGHEADER0 register. + config BT_AUDIO_USE_BROADCAST_ID_RANDOM bool "Use a random broadcast ID" depends on TRANSPORT_BIS - default y help Use a randomly generated broadcast ID. +config BT_AUDIO_USE_BROADCAST_ID_FIXED + bool "Use a fixed broadcast ID" + depends on TRANSPORT_BIS + help + Use a fixed broadcast ID. This is useful for debugging purposes. + +endchoice + config BT_AUDIO_BROADCAST_ID_FIXED hex "Fixed broadcast ID" depends on TRANSPORT_BIS default 0x123456 help - Fixed broadcast ID; 3 octets. Will only be used if BT_AUDIO_USE_BROADCAST_ID_RANDOM=n. + Fixed broadcast ID; 3 octets. Will only be used if BT_AUDIO_USE_BROADCAST_ID_FIXED=y. Only use for debugging. config BT_AUDIO_BROADCAST_PBA_METADATA_SIZE @@ -239,6 +260,11 @@ config BT_AUDIO_BROADCAST_ZBUS_EVT_STREAM_SENT be prepared. As this event will trigger once for each frame it will cause significant overhead, even if the event is not used. +config BT_AUDIO_BROADCAST_BASE_PRINT + bool "Print BASE information" + help + Print the contents of the BASE when received. This is useful for debugging purposes. + #----------------------------------------------------------------------------# menu "Log levels" diff --git a/src/bluetooth/bt_stream/broadcast/Kconfig.defaults b/src/bluetooth/bt_stream/broadcast/Kconfig.defaults index 63eccfa4..fbe83485 100644 --- a/src/bluetooth/bt_stream/broadcast/Kconfig.defaults +++ b/src/bluetooth/bt_stream/broadcast/Kconfig.defaults @@ -4,8 +4,5 @@ # SPDX-License-Identifier: LicenseRef-Nordic-5-Clause # -config BT_BUF_ACL_RX_SIZE - default 502 if (AUDIO_DFU > 0) - config BT_AUDIO_CODEC_CFG_MAX_METADATA_SIZE default 80 diff --git a/src/bluetooth/bt_stream/broadcast/broadcast_sink.c b/src/bluetooth/bt_stream/broadcast/broadcast_sink.c index ff1a05f5..dca8ee36 100644 --- a/src/bluetooth/bt_stream/broadcast/broadcast_sink.c +++ b/src/bluetooth/bt_stream/broadcast/broadcast_sink.c @@ -20,7 +20,8 @@ #include "bt_mgmt.h" #include "macros_common.h" #include "zbus_common.h" -#include "channel_assignment.h" +#include "device_location.h" +#include "audio_defines.h" #include LOG_MODULE_REGISTER(broadcast_sink, CONFIG_BROADCAST_SINK_LOG_LEVEL); @@ -43,37 +44,40 @@ struct audio_codec_info { int octets_per_sdu; int bitrate; int blocks_per_sdu; -}; -struct active_audio_stream { - struct bt_bap_stream *stream; - struct audio_codec_info *codec; uint32_t pd; }; +struct stream_info { + struct bt_bap_base_codec_id codec_id; + uint8_t subgroup_index; + uint8_t bis_index; +}; + static struct bt_bap_broadcast_sink *broadcast_sink; static struct bt_bap_stream audio_streams[CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT]; +static struct bt_bap_stream *audio_streams_p[ARRAY_SIZE(audio_streams)]; static struct audio_codec_info audio_codec_info[CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT]; -static uint32_t bis_index_bitfields[CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT]; +static uint32_t bis_index_bitfield; static struct bt_le_per_adv_sync *pa_sync_stored; -static struct active_audio_stream active_stream; /* The values of sync_stream_cnt and active_stream_index must never become larger * than the sizes of the arrays above (audio_streams etc.) */ static uint8_t sync_stream_cnt; -static uint8_t active_stream_index; static struct bt_audio_codec_cap codec_cap = BT_AUDIO_CODEC_CAP_LC3( BT_AUDIO_CODEC_CAPABILIY_FREQ, (BT_AUDIO_CODEC_CAP_DURATION_10 | BT_AUDIO_CODEC_CAP_DURATION_PREFER_10), - BT_AUDIO_CODEC_CAP_CHAN_COUNT_SUPPORT(1), LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MIN), - LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX), 1u, BT_AUDIO_CONTEXT_TYPE_ANY); + BT_AUDIO_CODEC_CAP_CHAN_COUNT_SUPPORT(1), + LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MIN, 10000), + LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX, 10000), 1u, BT_AUDIO_CONTEXT_TYPE_ANY); static struct bt_pacs_cap capabilities = { .codec_cap = &codec_cap, }; #define AVAILABLE_SINK_CONTEXT (BT_AUDIO_CONTEXT_TYPE_ANY) +#define PROGRAM_INFO_MAX_LEN 256 static le_audio_receive_cb receive_cb; @@ -93,6 +97,17 @@ enum csip_set_rank { CSIP_HR_RANK = 2 }; +static int stream_index_get(const struct bt_bap_stream *stream) +{ + for (int i = 0; i < ARRAY_SIZE(audio_streams); i++) { + if (stream == &audio_streams[i]) { + return i; + } + } + + return -ENODEV; +} + /* Callback for locking state change from server side */ static void csip_lock_changed_cb(struct bt_conn *conn, struct bt_csip_set_member_svc_inst *csip, bool locked) @@ -118,76 +133,12 @@ struct bt_csip_set_member_register_param csip_param = { .cb = &csip_callbacks, }; -int broadcast_sink_uuid_populate(struct net_buf_simple *uuid_buf) -{ - if (net_buf_simple_tailroom(uuid_buf) >= (BT_UUID_SIZE_16 * 3)) { - net_buf_simple_add_le16(uuid_buf, BT_UUID_BASS_VAL); - net_buf_simple_add_le16(uuid_buf, BT_UUID_PACS_VAL); - } else { - LOG_ERR("Not enough space for UUIDS"); - return -ENOMEM; - } - - return 0; -} - -int broadcast_sink_adv_populate(struct bt_data *adv_buf, uint8_t adv_buf_vacant) -{ - int ret; - uint32_t adv_buf_cnt = 0; - - if (IS_ENABLED(CONFIG_BT_CSIP_SET_MEMBER)) { - ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, - sizeof(csip_rsi_adv_data), BT_DATA_CSIS_RSI, - (void *)csip_rsi_adv_data); - if (ret) { - return ret; - } - } - - /* - * AD format required for broadcast sink with scan delegator. - * Details can be found in Basic Audio Profile Section 3.9.2. - */ - sys_put_le16(BT_UUID_BASS_VAL, &bass_service_uuid[0]); - - ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, - sizeof(bass_service_uuid), BT_DATA_SVC_DATA16, - (void *)bass_service_uuid); - if (ret) { - return ret; - } - - sys_put_le16(CONFIG_BT_DEVICE_APPEARANCE, &gap_appear_adv_data[0]); - - ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, - sizeof(gap_appear_adv_data), BT_DATA_GAP_APPEARANCE, - (void *)gap_appear_adv_data); - if (ret) { - return ret; - } - - flags_adv_data = BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR; - - ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, sizeof(uint8_t), - BT_DATA_FLAGS, (void *)&flags_adv_data); - if (ret) { - return ret; - } - - return adv_buf_cnt; -} - static int broadcast_sink_cleanup(void) { int ret; init_routine_completed = false; - active_stream.pd = 0; - active_stream.stream = NULL; - active_stream.codec = NULL; - if (broadcast_sink != NULL) { ret = bt_bap_broadcast_sink_delete(broadcast_sink); if (ret && ret != -EALREADY) { @@ -228,18 +179,6 @@ static void le_audio_event_publish(enum le_audio_evt_type event) ERR_CHK(ret); } -static void print_codec(const struct audio_codec_info *codec) -{ - LOG_INF("Codec config for LC3:"); - LOG_INF("\tFrequency: %d Hz", codec->frequency); - LOG_INF("\tFrame Duration: %d us", codec->frame_duration_us); - LOG_INF("\tOctets per frame: %d (%d kbps)", codec->octets_per_sdu, codec->bitrate); - LOG_INF("\tFrames per SDU: %d", codec->blocks_per_sdu); - if (codec->chan_allocation >= 0) { - LOG_INF("\tChannel allocation: 0x%x", codec->chan_allocation); - } -} - static void get_codec_info(const struct bt_audio_codec_cfg *codec, struct audio_codec_info *codec_info) { @@ -282,14 +221,20 @@ static void get_codec_info(const struct bt_audio_codec_cfg *codec, static void stream_started_cb(struct bt_bap_stream *stream) { le_audio_event_publish(LE_AUDIO_EVT_STREAMING); + sync_stream_cnt++; /* NOTE: The string below is used by the Nordic CI system */ - LOG_INF("Stream index %d started", active_stream_index); - print_codec(&audio_codec_info[active_stream_index]); + LOG_INF("Stream index %d started", stream_index_get(stream)); + le_audio_print_codec(stream->codec_cfg, BT_AUDIO_DIR_SINK); } static void stream_stopped_cb(struct bt_bap_stream *stream, uint8_t reason) { + if (sync_stream_cnt == 0) { + LOG_WRN("Stream stopped, but no streams are currently synced"); + return; + } + sync_stream_cnt--; switch (reason) { case BT_HCI_ERR_LOCALHOST_TERM_CONN: @@ -325,25 +270,27 @@ static void stream_stopped_cb(struct bt_bap_stream *stream, uint8_t reason) } /* NOTE: The string below is used by the Nordic CI system */ - LOG_INF("Stream index %d stopped. Reason: %d", active_stream_index, reason); + LOG_INF("Stream index %d stopped. Reason: %d", stream_index_get(stream), reason); } static void stream_recv_cb(struct bt_bap_stream *stream, const struct bt_iso_recv_info *info, - struct net_buf *buf) + struct net_buf *audio_frame) { - bool bad_frame = false; + int ret; + struct audio_metadata meta; if (receive_cb == NULL) { LOG_ERR("The RX callback has not been set"); return; } - if (!(info->flags & BT_ISO_FLAGS_VALID)) { - bad_frame = true; + ret = le_audio_metadata_populate(&meta, stream, info, audio_frame); + if (ret) { + LOG_ERR("Failed to populate meta data: %d", ret); + return; } - receive_cb(buf->data, buf->len, bad_frame, info->ts, active_stream_index, - active_stream.codec->octets_per_sdu); + receive_cb(audio_frame, &meta, stream_index_get(stream)); } static struct bt_bap_stream_ops stream_ops = { @@ -352,7 +299,15 @@ static struct bt_bap_stream_ops stream_ops = { .recv = stream_recv_cb, }; -static bool base_subgroup_bis_cb(const struct bt_bap_base_subgroup_bis *bis, void *user_data) +/** + * @brief Parse a BIS in the context of a base subgroup. + * + * @param bis Pointer to the BIS to parse. + * @param user_data Pointer to user data (not used). + * + * @return true if parsing should continue, false otherwise. + */ +static bool bis_per_subgroup_parse(const struct bt_bap_base_subgroup_bis *bis, void *user_data) { int ret; struct bt_audio_codec_cfg codec_cfg = {0}; @@ -371,17 +326,25 @@ static bool base_subgroup_bis_cb(const struct bt_bap_base_subgroup_bis *bis, voi LOG_DBG("Channel allocation: 0x%x for BIS index %d", audio_codec_info[bis->index - 1].chan_allocation, bis->index); + enum bt_audio_location device_location_temp; - uint32_t chan_bitfield = audio_codec_info[bis->index - 1].chan_allocation; - bool single_bit = (chan_bitfield & (chan_bitfield - 1)) == 0; + device_location_get(&device_location_temp); - if (single_bit) { - bis_index_bitfields[bis->index - 1] = BIT(bis->index - 1); - } else { - LOG_WRN("More than one bit set in channel location, we only support 1 channel per " - "BIS"); + if (!(audio_codec_info[bis->index - 1].chan_allocation & device_location_temp)) { + LOG_DBG("BIS idx %d channel alloc. 0x%x does not match this device' location 0x%x", + bis->index, audio_codec_info[bis->index - 1].chan_allocation, + device_location_temp); + return true; } + if (POPCOUNT(bis_index_bitfield) >= CONFIG_BT_AUDIO_CONCURRENT_RX_STREAMS_MAX) { + LOG_DBG("Maximum number of BISes reached, ignoring BIS index %d", bis->index); + return true; + } + + bis_index_bitfield |= BIT(bis->index - 1); + LOG_DBG("BIS index %d added to bitfield 0x%08x", bis->index, bis_index_bitfield); + return true; } @@ -421,21 +384,223 @@ static bool base_subgroup_cb(const struct bt_bap_base_subgroup *subgroup, void * LOG_DBG("Subgroup %p has %d BISes", (void *)subgroup, bis_num); if (bis_num > 0) { *suitable_stream_found = true; - sync_stream_cnt = bis_num; + for (int i = 0; i < bis_num; i++) { get_codec_info(&codec_cfg, &audio_codec_info[i]); + audio_codec_info[i].id = codec_id.id; + audio_codec_info[i].cid = codec_id.cid; + audio_codec_info[i].vid = codec_id.vid; } - ret = bt_bap_base_subgroup_foreach_bis(subgroup, base_subgroup_bis_cb, NULL); + ret = bt_bap_base_subgroup_foreach_bis(subgroup, bis_per_subgroup_parse, NULL); if (ret < 0) { LOG_WRN("Could not get BIS for subgroup %p: %d", (void *)subgroup, ret); } + + return false; + } + + return true; +} + +static bool base_print_subgroup_per_bis_cb(const struct bt_bap_base_subgroup_bis *bis, + void *user_data) +{ + int ret; + struct stream_info *info = user_data; + enum bt_audio_location chan_allocation; + + struct bt_audio_codec_cfg codec_cfg = { + .id = info->codec_id.id, + .cid = info->codec_id.cid, + .vid = info->codec_id.vid, + }; + + ret = bt_bap_base_subgroup_bis_codec_to_codec_cfg(bis, &codec_cfg); + if (ret < 0) { + LOG_WRN("Failed to convert codec to codec_cfg: %d", ret); return false; } + ret = bt_audio_codec_cfg_get_chan_allocation(&codec_cfg, &chan_allocation, false); + if (ret < 0) { + LOG_WRN("Failed to get channel allocation: %d", ret); + return false; + } + + if (chan_allocation == BT_AUDIO_LOCATION_MONO_AUDIO) { + LOG_INF("\t\t\tBIS %d: Mono", info->bis_index); + } else { + for (size_t i = 0; i < 32; i++) { + uint32_t bit_val = BIT(i); + + if (chan_allocation & bit_val) { + LOG_INF("\t\t\tBIS %d: %s", info->bis_index, + bt_audio_location_bit_to_str(bit_val)); + } + } + } + + info->bis_index += 1; + return true; } +static bool base_print_per_subgroup_cb(const struct bt_bap_base_subgroup *subgroup, void *user_data) +{ + int ret; + struct bt_bap_base_codec_id codec_id; + struct bt_audio_codec_cfg codec_cfg; + uint8_t *subgroup_index = user_data; + + LOG_INF("Subgroup: %d", *subgroup_index); + + ret = bt_bap_base_get_subgroup_codec_id(subgroup, &codec_id); + if (ret < 0) { + return false; + } + + struct stream_info info = { + .codec_id = codec_id, + .subgroup_index = *subgroup_index, + .bis_index = 0, + }; + + ret = bt_bap_base_subgroup_codec_to_codec_cfg(subgroup, &codec_cfg); + if (ret != 0) { + LOG_WRN("Failed to convert codec to codec_cfg: %d", ret); + return false; + } + + LOG_INF("\tCodec specific configuration:"); + + if (codec_cfg.data_len == 0U) { + LOG_INF("\t\tNone"); + } else if (codec_cfg.id == BT_HCI_CODING_FORMAT_LC3) { + int ret; + + ret = bt_audio_codec_cfg_get_freq(&codec_cfg); + if (ret >= 0) { + LOG_INF("\t\tSampling rate: %u Hz", + bt_audio_codec_cfg_freq_to_freq_hz(ret)); + } else { + LOG_WRN("Failed to get frequency: %d", ret); + } + + ret = bt_audio_codec_cfg_get_frame_dur(&codec_cfg); + if (ret >= 0) { + LOG_INF("\t\tFrame duration: %u us", + bt_audio_codec_cfg_frame_dur_to_frame_dur_us(ret)); + } else { + LOG_WRN("Failed to get frame duration: %d", ret); + } + + ret = bt_audio_codec_cfg_get_octets_per_frame(&codec_cfg); + if (ret >= 0) { + LOG_INF("\t\tOctets per frame: %u", (uint16_t)ret); + } else { + LOG_WRN("Failed to get octets per frame: %d", ret); + } + + ret = bt_bap_base_get_subgroup_bis_count(subgroup); + if (ret >= 0) { + LOG_INF("\t\tNumber of BISes: %u", (uint8_t)ret); + } else { + LOG_WRN("Failed to get number of BISes: %d", ret); + } + + LOG_INF("\t\tLocation:"); + ret = bt_bap_base_subgroup_foreach_bis(subgroup, base_print_subgroup_per_bis_cb, + &info); + if (ret < 0) { + return false; + } + + } else { + LOG_INF("\t\tUnsupported codec"); + } + + LOG_INF("\tCodec specific metadata:"); + + if (codec_cfg.meta_len == 0U) { + LOG_INF("\tNone"); + } else { + const uint8_t *data; + + ret = bt_audio_codec_cfg_meta_get_stream_context(&codec_cfg); + if (ret >= 0) { + /* Iterate through the context bits */ + for (size_t i = 0U; i < 16; i++) { + uint16_t bit_val = BIT(i); + + if (ret & bit_val) { + LOG_INF("\t\tContext: %s", + bt_audio_context_bit_to_str(bit_val)); + } + } + } + + ret = bt_audio_codec_cfg_meta_get_program_info(&codec_cfg, &data); + if (ret >= 0) { + char program_info[PROGRAM_INFO_MAX_LEN] = {'\0'}; + int meta_len = ret; + + if (meta_len > PROGRAM_INFO_MAX_LEN) { + meta_len = PROGRAM_INFO_MAX_LEN - 1; + LOG_WRN("Program info length %d exceeds max, truncating. Note that " + "this is non-spec compliant behavior: %d. Program info can " + "be up to 255 bytes long according to the spec, but only " + "%d bytes will be printed", + ret, meta_len, PROGRAM_INFO_MAX_LEN - 1); + } + + memcpy(program_info, data, meta_len); + LOG_INF("\t\tProgram info: %s", program_info); + } + + const uint8_t *lang; + + ret = bt_audio_codec_cfg_meta_get_lang(&codec_cfg, &lang); + if (ret == 0) { + LOG_INF("\t\tLanguage: %c%c%c", (char)lang[0], (char)lang[1], + (char)lang[2]); + } + + ret = bt_audio_codec_cfg_meta_get_parental_rating(&codec_cfg); + if (ret >= 0) { + LOG_INF("\t\tParental rating: %s", bt_audio_parental_rating_to_str(ret)); + } + + ret = bt_audio_codec_cfg_meta_get_audio_active_state(&codec_cfg); + if (ret >= 0) { + LOG_INF("\t\tAudio active state: %s", + ret == BT_AUDIO_ACTIVE_STATE_ENABLED ? "enabled" : "disabled"); + } + + ret = bt_audio_codec_cfg_meta_get_bcast_audio_immediate_rend_flag(&codec_cfg); + if (ret >= 0) { + LOG_INF("\t\tImmediate rendering flag: %s", + ret == 0 ? "enabled" : "disabled"); + } + } + + *subgroup_index += 1; + + return true; +} + +static void base_print(struct bt_bap_broadcast_sink *sink, const struct bt_bap_base *base) +{ + int ret; + + uint8_t subgroup_index = 0; + + ret = bt_bap_base_foreach_subgroup(base, base_print_per_subgroup_cb, &subgroup_index); + if (ret < 0) { + LOG_DBG("Invalid BASE: %d", ret); + } +} + static void base_recv_cb(struct bt_bap_broadcast_sink *sink, const struct bt_bap_base *base, size_t base_size) { @@ -459,39 +624,22 @@ static void base_recv_cb(struct bt_bap_broadcast_sink *sink, const struct bt_bap } if (suitable_stream_found) { - /* Set the initial active stream based on the defined channel of the device */ - enum audio_channel audio_channel_temp; - - channel_assignment_get(&audio_channel_temp); - if (audio_channel_temp > AUDIO_CH_NUM) { - LOG_ERR("Invalid channel assignment"); - return; - } - - active_stream_index = (uint8_t)audio_channel_temp; - - /** If the stream matching channel is not present, revert back to first BIS, e.g. - * mono stream but channel assignment is RIGHT - */ - if ((active_stream_index + 1) > sync_stream_cnt) { - LOG_WRN("BIS index: %d not found, reverting to first BIS", - (active_stream_index + 1)); - active_stream_index = 0; - } - - active_stream.stream = &audio_streams[active_stream_index]; - active_stream.codec = &audio_codec_info[active_stream_index]; ret = bt_bap_base_get_pres_delay(base); if (ret == -EINVAL) { LOG_WRN("Failed to get pres_delay: %d", ret); - active_stream.pd = 0; + /* Since all BISes in a subgroup share the same codec info, + * we can use index 0. + */ + audio_codec_info[0].pd = 0; } else { - active_stream.pd = ret; + audio_codec_info[0].pd = ret; } le_audio_event_publish(LE_AUDIO_EVT_CONFIG_RECEIVED); - LOG_DBG("Channel %s active", - ((active_stream_index == AUDIO_CH_L) ? CH_L_TAG : CH_R_TAG)); + if (IS_ENABLED(CONFIG_BT_AUDIO_BROADCAST_BASE_PRINT)) { + base_print(sink, base); + } + LOG_DBG("Waiting for syncable"); } else { LOG_DBG("Found no suitable stream"); @@ -502,34 +650,28 @@ static void base_recv_cb(struct bt_bap_broadcast_sink *sink, const struct bt_bap static void syncable_cb(struct bt_bap_broadcast_sink *sink, const struct bt_iso_biginfo *biginfo) { int ret; - struct bt_bap_stream *audio_streams_p[] = {&audio_streams[active_stream_index]}; static uint32_t prev_broadcast_id; LOG_DBG("Broadcast sink is syncable"); - if (active_stream.stream != NULL && active_stream.stream->ep != NULL) { - if (active_stream.stream->ep->status.state == BT_BAP_EP_STATE_STREAMING) { - LOG_WRN("Syncable received, but already in a stream"); - return; - } - } - if (paused) { LOG_DBG("Syncable received, but in paused state"); return; } - if (bis_index_bitfields[active_stream_index] == 0) { + if (bis_index_bitfield == 0) { LOG_ERR("No bits set in bitfield"); return; - } else if (!IS_POWER_OF_TWO(bis_index_bitfields[active_stream_index])) { - /* Check that only one bit is set */ - LOG_ERR("Application syncs to only one stream"); - return; + } + + if (biginfo->encryption == true) { + LOG_INF("BIG is encrypted"); + } else { + LOG_INF("BIG is not encrypted"); } /* NOTE: The string below is used by the Nordic CI system */ - LOG_INF("Syncing to broadcast stream index %d", active_stream_index); + LOG_INF("Syncing to broadcast stream index 0x%04x (bitfield)", bis_index_bitfield); if (IS_ENABLED(CONFIG_BT_AUDIO_BROADCAST_ENCRYPTED)) { memcpy(bis_encryption_key, CONFIG_BT_AUDIO_BROADCAST_ENCRYPTION_KEY, @@ -542,15 +684,15 @@ static void syncable_cb(struct bt_bap_broadcast_sink *sink, const struct bt_iso_ */ if (!broadcast_code_received && biginfo->encryption == true && sink->broadcast_id != prev_broadcast_id) { - LOG_WRN("Stream is encrypted, but haven not received broadcast code"); + LOG_WRN("Stream is encrypted, but have not received broadcast code"); return; } broadcast_code_received = false; } - ret = bt_bap_broadcast_sink_sync(broadcast_sink, bis_index_bitfields[active_stream_index], - audio_streams_p, bis_encryption_key); + ret = bt_bap_broadcast_sink_sync(broadcast_sink, bis_index_bitfield, audio_streams_p, + bis_encryption_key); if (ret) { LOG_WRN("Unable to sync to broadcast source, ret: %d", ret); @@ -559,75 +701,124 @@ static void syncable_cb(struct bt_bap_broadcast_sink *sink, const struct bt_iso_ prev_broadcast_id = sink->broadcast_id; - /* Only a single stream used for now */ - active_stream.stream = &audio_streams[active_stream_index]; - init_routine_completed = true; } +static bool is_any_active_streams(void) +{ + for (int i = 0; i < ARRAY_SIZE(audio_streams); i++) { + if (audio_streams[i].ep != NULL && + audio_streams[i].ep->state == BT_BAP_EP_STATE_STREAMING) { + return true; + } + } + + return false; +} + static struct bt_bap_broadcast_sink_cb broadcast_sink_cbs = { .base_recv = base_recv_cb, .syncable = syncable_cb, }; -int broadcast_sink_change_active_audio_stream(void) +int broadcast_sink_uuid_populate(struct net_buf_simple *uuid_buf) +{ + if (net_buf_simple_tailroom(uuid_buf) >= (BT_UUID_SIZE_16 * 3)) { + net_buf_simple_add_le16(uuid_buf, BT_UUID_BASS_VAL); + net_buf_simple_add_le16(uuid_buf, BT_UUID_PACS_VAL); + } else { + LOG_ERR("Not enough space for UUIDS"); + return -ENOMEM; + } + + return 0; +} + +int broadcast_sink_adv_populate(struct bt_data *adv_buf, uint8_t adv_buf_vacant) { int ret; + uint32_t adv_buf_cnt = 0; - if (broadcast_sink == NULL) { - LOG_WRN("No broadcast sink"); - return -ECANCELED; + if (IS_ENABLED(CONFIG_BT_CSIP_SET_MEMBER)) { + ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, + sizeof(csip_rsi_adv_data), BT_DATA_CSIS_RSI, + (void *)csip_rsi_adv_data); + if (ret) { + return ret; + } } - if (active_stream.stream != NULL && active_stream.stream->ep != NULL) { - if (active_stream.stream->ep->status.state == BT_BAP_EP_STATE_STREAMING) { - ret = bt_bap_broadcast_sink_stop(broadcast_sink); - if (ret) { - LOG_ERR("Failed to stop sink"); - } - } + /* + * AD format required for broadcast sink with scan delegator. + * Details can be found in Basic Audio Profile Section 3.9.2. + */ + sys_put_le16(BT_UUID_BASS_VAL, &bass_service_uuid[0]); + + ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, + sizeof(bass_service_uuid), BT_DATA_SVC_DATA16, + (void *)bass_service_uuid); + if (ret) { + return ret; } - /* Wrap streams */ - if (++active_stream_index >= sync_stream_cnt) { - active_stream_index = 0; + sys_put_le16(CONFIG_BT_DEVICE_APPEARANCE, &gap_appear_adv_data[0]); + + ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, + sizeof(gap_appear_adv_data), BT_DATA_GAP_APPEARANCE, + (void *)gap_appear_adv_data); + if (ret) { + return ret; } - active_stream.stream = &audio_streams[active_stream_index]; - active_stream.codec = &audio_codec_info[active_stream_index]; + flags_adv_data = BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR; - LOG_INF("Changed to stream %d", active_stream_index); + ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, sizeof(uint8_t), + BT_DATA_FLAGS, (void *)&flags_adv_data); + if (ret) { + return ret; + } - return 0; + return adv_buf_cnt; } -int broadcast_sink_config_get(uint32_t *bitrate, uint32_t *sampling_rate, uint32_t *pres_delay) +int le_audio_concurrent_sync_num_get(uint8_t *num_streams, enum bt_audio_location *locations) { - if (active_stream.codec == NULL) { - LOG_WRN("No active stream to get config from"); - return -ENXIO; + if (num_streams == NULL || locations == NULL) { + LOG_ERR("Invalid input parameters"); + return -EINVAL; } + *num_streams = sync_stream_cnt; + *locations = 0; + + for (int i = 0; i < ARRAY_SIZE(audio_codec_info); i++) { + if (bis_index_bitfield & BIT(i)) { + *locations |= audio_codec_info[i].chan_allocation; + } + } + + return 0; +} + +int broadcast_sink_config_get(uint32_t *bitrate, uint32_t *sampling_rate, uint32_t *pres_delay) +{ if (bitrate == NULL && sampling_rate == NULL && pres_delay == NULL) { LOG_ERR("No valid pointers received"); return -ENXIO; } + /* Since all BISes in a subgroup share the same codec info, we can use index 0 */ if (sampling_rate != NULL) { - *sampling_rate = active_stream.codec->frequency; + *sampling_rate = audio_codec_info[0].frequency; } if (bitrate != NULL) { - *bitrate = active_stream.codec->bitrate; + *bitrate = audio_codec_info[0].bitrate; } if (pres_delay != NULL) { - if (active_stream.stream == NULL) { - LOG_WRN("No active stream"); - return -ENXIO; - } - *pres_delay = active_stream.pd; + *pres_delay = audio_codec_info[0].pd; } return 0; @@ -644,16 +835,16 @@ int broadcast_sink_pa_sync_set(struct bt_le_per_adv_sync *pa_sync, uint32_t broa LOG_DBG("Trying to set PA sync with ID: %d", broadcast_id); - if (active_stream.stream != NULL && active_stream.stream->ep != NULL) { - if (active_stream.stream->ep->status.state == BT_BAP_EP_STATE_STREAMING) { - ret = bt_bap_broadcast_sink_stop(broadcast_sink); - if (ret) { - LOG_ERR("Failed to stop broadcast sink: %d", ret); - return ret; - } + if (is_any_active_streams()) { + LOG_DBG("There are active streams, stopping them before setting new PA sync"); - broadcast_sink_cleanup(); + ret = bt_bap_broadcast_sink_stop(broadcast_sink); + if (ret) { + LOG_ERR("Failed to stop broadcast sink: %d", ret); + return ret; } + + broadcast_sink_cleanup(); } /* If broadcast_sink was not in an active stream we still need to clean it up */ @@ -705,21 +896,16 @@ int broadcast_sink_stop(void) return -EALREADY; } - if (active_stream.stream == NULL || active_stream.stream->ep == NULL) { - LOG_WRN("Stream or endpoint not set"); - return -EPERM; + if (is_any_active_streams() == false) { + LOG_WRN("No active streams to stop"); + return -EALREADY; } - if (active_stream.stream->ep->status.state == BT_BAP_EP_STATE_STREAMING) { - paused = true; - ret = bt_bap_broadcast_sink_stop(broadcast_sink); - if (ret) { - LOG_ERR("Failed to stop broadcast sink: %d", ret); - return ret; - } - } else { - LOG_WRN("Current stream not in streaming state"); - return -EALREADY; + paused = true; + ret = bt_bap_broadcast_sink_stop(broadcast_sink); + if (ret) { + LOG_ERR("Failed to stop broadcast sink: %d", ret); + return ret; } return 0; @@ -729,12 +915,10 @@ int broadcast_sink_disable(void) { int ret; - if (active_stream.stream != NULL && active_stream.stream->ep != NULL) { - if (active_stream.stream->ep->status.state == BT_BAP_EP_STATE_STREAMING) { - ret = bt_bap_broadcast_sink_stop(broadcast_sink); - if (ret) { - LOG_ERR("Failed to stop sink"); - } + if (is_any_active_streams()) { + ret = bt_bap_broadcast_sink_stop(broadcast_sink); + if (ret) { + LOG_ERR("Failed to stop sink"); } } @@ -761,7 +945,11 @@ int broadcast_sink_enable(le_audio_receive_cb recv_cb) { int ret; static bool initialized; - enum audio_channel channel; + enum bt_audio_location device_location; + const struct bt_pacs_register_param pacs_param = { + .snk_pac = true, + .snk_loc = true, + }; if (initialized) { LOG_WRN("Already initialized"); @@ -775,21 +963,32 @@ int broadcast_sink_enable(le_audio_receive_cb recv_cb) receive_cb = recv_cb; - channel_assignment_get(&channel); + device_location_get(&device_location); - if (channel == AUDIO_CH_L) { - ret = bt_pacs_set_location(BT_AUDIO_DIR_SINK, BT_AUDIO_LOCATION_FRONT_LEFT); - csip_param.rank = CSIP_HL_RANK; - } else { - ret = bt_pacs_set_location(BT_AUDIO_DIR_SINK, BT_AUDIO_LOCATION_FRONT_RIGHT); - csip_param.rank = CSIP_HR_RANK; + ret = bt_pacs_register(&pacs_param); + if (ret) { + LOG_ERR("Could not register PACS (err %d)\n", ret); + return ret; } + ret = bt_pacs_set_location(BT_AUDIO_DIR_SINK, device_location); if (ret) { LOG_ERR("Location set failed"); return ret; } + /* + * Set RANK. Use 1 for left, 2 for right, 1 otherwise, + * as rank will have to be considered depending on the application. + */ + if (device_location == BT_AUDIO_LOCATION_FRONT_LEFT) { + csip_param.rank = CSIP_HL_RANK; + } else if (device_location == BT_AUDIO_LOCATION_FRONT_RIGHT) { + csip_param.rank = CSIP_HR_RANK; + } else { + csip_param.rank = CSIP_HL_RANK; + } + ret = bt_pacs_set_supported_contexts(BT_AUDIO_DIR_SINK, AVAILABLE_SINK_CONTEXT); if (ret) { LOG_ERR("Supported context set failed. Err: %d", ret); @@ -839,6 +1038,7 @@ int broadcast_sink_enable(le_audio_receive_cb recv_cb) for (int i = 0; i < ARRAY_SIZE(audio_streams); i++) { audio_streams[i].ops = &stream_ops; + audio_streams_p[i] = &audio_streams[i]; } initialized = true; diff --git a/src/bluetooth/bt_stream/broadcast/broadcast_sink.h b/src/bluetooth/bt_stream/broadcast/broadcast_sink.h index 29129df5..8420f376 100644 --- a/src/bluetooth/bt_stream/broadcast/broadcast_sink.h +++ b/src/bluetooth/bt_stream/broadcast/broadcast_sink.h @@ -4,6 +4,14 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @addtogroup audio_app_bt_stream + * @{ + * @defgroup broadcast_sink Functions for broadcast sink functionality. + * @{ + * @brief Helper functions to manage broadcast sink functionality. + */ + #ifndef _BROADCAST_SINK_H_ #define _BROADCAST_SINK_H_ @@ -32,18 +40,6 @@ int broadcast_sink_uuid_populate(struct net_buf_simple *uuid_buf); */ int broadcast_sink_adv_populate(struct bt_data *adv_buf, uint8_t adv_buf_vacant); -/** - * @brief Change the active audio stream if the broadcast isochronous group (BIG) contains - * more than one broadcast isochronous stream (BIS). - * - * @note Only streams within the same broadcast source are relevant, meaning - * that the broadcast source is not changed. - * The active stream will iterate every time this function is called. - * - * @return 0 for success, error otherwise. - */ -int broadcast_sink_change_active_audio_stream(void); - /** * @brief Get configuration for the audio stream. * @@ -107,4 +103,9 @@ int broadcast_sink_disable(void); */ int broadcast_sink_enable(le_audio_receive_cb recv_cb); +/** + * @} + * @} + */ + #endif /* _BROADCAST_SINK_H_ */ diff --git a/src/bluetooth/bt_stream/broadcast/broadcast_source.c b/src/bluetooth/bt_stream/broadcast/broadcast_source.c index 1dd9d7d5..0bdbc586 100644 --- a/src/bluetooth/bt_stream/broadcast/broadcast_source.c +++ b/src/bluetooth/bt_stream/broadcast/broadcast_source.c @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -41,17 +42,55 @@ ZBUS_CHAN_DEFINE(le_audio_chan, struct le_audio_msg, NULL, NULL, ZBUS_OBSERVERS_ ZBUS_MSG_INIT(0)); static struct bt_cap_broadcast_source *broadcast_sources[CONFIG_BT_ISO_MAX_BIG]; -struct bt_cap_initiator_broadcast_create_param create_param[CONFIG_BT_ISO_MAX_BIG]; +static struct bt_cap_initiator_broadcast_create_param create_param[CONFIG_BT_ISO_MAX_BIG]; /* Make sure we have statically allocated streams for all potential BISes */ static struct bt_cap_stream cap_streams[CONFIG_BT_ISO_MAX_BIG] [CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT] [CONFIG_BT_BAP_BROADCAST_SRC_STREAM_COUNT]; -static struct bt_bap_lc3_preset lc3_preset = BT_BAP_LC3_BROADCAST_PRESET_NRF5340_AUDIO; +static struct bt_bap_lc3_preset lc3_preset = BT_BAP_LC3_BROADCAST_PRESET_NRF_AUDIO; -static bool initialized; +static bool initialized[CONFIG_BT_ISO_MAX_BIG]; static bool delete_broadcast_src[CONFIG_BT_ISO_MAX_BIG]; +/* Define one TX context per subgroup per BIG. + * Note: Nested LISTIFY is not supported by the C preprocessor due to + * blue-painting rules, so we use separate macros per BIG index. + */ +#define DEFINE_SG_CTX_BIG0(sg_idx, _) BT_LE_AUDIO_TX_DEFINE_INDEXED(tx_ctx, 0, sg_idx) +LISTIFY(CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT, DEFINE_SG_CTX_BIG0, (;), ~); + +#if CONFIG_BT_ISO_MAX_BIG == 2 +#define DEFINE_SG_CTX_BIG1(sg_idx, _) BT_LE_AUDIO_TX_DEFINE_INDEXED(tx_ctx, 1, sg_idx) +LISTIFY(CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT, DEFINE_SG_CTX_BIG1, (;), ~); +#elif CONFIG_BT_ISO_MAX_BIG > 2 +#error Only up to 2 BIGs supported for static TX context allocation +#endif + +#define SG_CTX_PTR_BIG0(sg_idx, _) (&tx_ctx_0_##sg_idx##_ctx) +#define SG_CTX_PTR_BIG1(sg_idx, _) (&tx_ctx_1_##sg_idx##_ctx) + +static struct bt_le_audio_tx_ctx + *tx_ctx_arr[CONFIG_BT_ISO_MAX_BIG][CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT] = { + /* clang-format off */ + {LISTIFY(CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT, SG_CTX_PTR_BIG0, (,), ~)}, +#if CONFIG_BT_ISO_MAX_BIG > 1 + {LISTIFY(CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT, SG_CTX_PTR_BIG1, (,), ~)}, +#endif + /* clang-format on */ +}; + +/* Helper to get context for a given BIG and subgroup */ +static struct bt_le_audio_tx_ctx *tx_ctx_get(uint8_t big_idx, uint8_t sg_idx) +{ + if (big_idx >= CONFIG_BT_ISO_MAX_BIG || + sg_idx >= CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT) { + return NULL; + } + + return tx_ctx_arr[big_idx][sg_idx]; +} + static int metadata_u8_add(uint8_t buffer[], uint8_t *index, uint8_t type, uint8_t value) { if (buffer == NULL || index == NULL) { @@ -112,7 +151,9 @@ static void stream_sent_cb(struct bt_bap_stream *stream) le_audio_event_publish(LE_AUDIO_EVT_STREAM_SENT, &idx); } - ERR_CHK(bt_le_audio_tx_stream_sent(idx)); + struct bt_le_audio_tx_ctx *ctx = tx_ctx_get(idx.lvl1, idx.lvl2); + + ERR_CHK(bt_le_audio_tx_stream_sent(ctx, idx)); } static void stream_started_cb(struct bt_bap_stream *stream) @@ -125,12 +166,14 @@ static void stream_started_cb(struct bt_bap_stream *stream) return; } - ERR_CHK(bt_le_audio_tx_stream_started(idx)); + struct bt_le_audio_tx_ctx *ctx = tx_ctx_get(idx.lvl1, idx.lvl2); + + ERR_CHK(bt_le_audio_tx_stream_started(ctx, idx)); le_audio_event_publish(LE_AUDIO_EVT_STREAMING, &idx); /* NOTE: The string below is used by the Nordic CI system */ - LOG_INF("Broadcast source %p started", (void *)stream); + LOG_INF("Broadcast stream %p started", (void *)stream); le_audio_print_codec(stream->codec_cfg, BT_AUDIO_DIR_SOURCE); } @@ -168,7 +211,7 @@ static void stream_stopped_cb(struct bt_bap_stream *stream, uint8_t reason) le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, &idx); - LOG_INF("Broadcast source %p stopped. Reason: %d", (void *)stream, reason); + LOG_INF("Broadcast stream %p stopped. Reason: %d", (void *)stream, reason); if (delete_broadcast_src[idx.lvl1] && broadcast_sources[idx.lvl1] != NULL && !source_has_streaming_streams(idx.lvl1)) { @@ -623,8 +666,8 @@ int broadcast_source_id_get(uint8_t big_index, uint32_t *broadcast_id) return 0; } -int broadcast_source_send(uint8_t big_index, uint8_t subgroup_index, - struct le_audio_encoded_audio enc_audio) +int broadcast_source_send(struct net_buf const *const audio_frame, uint8_t big_index, + uint8_t subgroup_index) { int ret; uint8_t num_active_streams = 0; @@ -657,8 +700,8 @@ int broadcast_source_send(uint8_t big_index, uint8_t subgroup_index, /* TODO: Use the function below once * https://github.com/zephyrproject-rtos/zephyr/pull/72908 is merged */ - /* tx[num_active_streams].audio_channel = audio_map_location_get(stream);*/ - tx[num_active_streams].audio_channel = i; + /* tx[num_active_streams].audio_location = audio_map_location_get(stream);*/ + tx[num_active_streams].audio_location = i; num_active_streams++; } @@ -668,7 +711,9 @@ int broadcast_source_send(uint8_t big_index, uint8_t subgroup_index, return -ECANCELED; } - ret = bt_le_audio_tx_send(tx, num_active_streams, enc_audio); + struct bt_le_audio_tx_ctx *ctx = tx_ctx_get(big_index, subgroup_index); + + ret = bt_le_audio_tx_send(ctx, audio_frame, tx, num_active_streams); if (ret) { return ret; } @@ -708,7 +753,7 @@ int broadcast_source_disable(uint8_t big_index) broadcast_sources[big_index] = NULL; } - initialized = false; + initialized[big_index] = false; LOG_DBG("Broadcast source disabled"); @@ -754,8 +799,62 @@ void broadcast_source_default_create(struct broadcast_source_big *broadcast_para } bt_audio_codec_cfg_meta_set_lang(&subgroups.group_lc3_preset.codec_cfg, "eng"); + + memcpy(broadcast_param->broadcast_name, CONFIG_BT_AUDIO_BROADCAST_NAME, + sizeof(CONFIG_BT_AUDIO_BROADCAST_NAME)); +} + +void broadcast_started_cb(struct bt_cap_broadcast_source *source) +{ + if (!IS_ENABLED(CONFIG_DEBUG)) { + return; + } + + int ret; + int idx = -EINVAL; + + for (int i = 0; i < CONFIG_BT_ISO_MAX_BIG; i++) { + if (broadcast_sources[i] == source) { + idx = i; + break; + } + } + + if (idx < 0) { + LOG_ERR("Broadcaster not found"); + return; + } + + uint32_t broadcast_id; + + ret = broadcast_source_id_get(idx, &broadcast_id); + if (ret) { + LOG_ERR("Failed to get broadcast ID: %d", ret); + return; + } + + LOG_INF("Source %p started", (void *)source); + LOG_INF("\tIndex: %u", idx); + LOG_INF("\tID: %u", broadcast_id); + LOG_INF("\tNum subgroups: %u", create_param[idx].subgroup_count); + + if (create_param[idx].encryption) { + LOG_INF("\tEncrypted with key: %s", create_param[idx].broadcast_code); + } else { + LOG_INF("\tNot encrypted"); + } } +void broadcast_stopped_cb(struct bt_cap_broadcast_source *source, uint8_t reason) +{ + LOG_INF("Broadcast source %p stopped. Reason: %d", (void *)source, reason); +} + +static struct bt_cap_initiator_cb cap_cbs = { + .broadcast_started = broadcast_started_cb, + .broadcast_stopped = broadcast_stopped_cb, +}; + int broadcast_source_enable(struct broadcast_source_big const *const broadcast_param, uint8_t big_index) { @@ -767,11 +866,14 @@ int broadcast_source_enable(struct broadcast_source_big const *const broadcast_p return -EINVAL; } - if (!initialized) { - bt_le_audio_tx_init(); - } + ret = bt_cap_initiator_register_cb(&cap_cbs); - LOG_INF("Enabling broadcast_source %d", big_index); + if (ret == -EALREADY) { + LOG_DBG("Callbacks already registered"); + } else if (ret) { + LOG_ERR("Failed to register callbacks: %d", ret); + return ret; + } ret = create_param_produce(big_index, broadcast_param, &create_param[big_index]); if (ret) { @@ -779,12 +881,28 @@ int broadcast_source_enable(struct broadcast_source_big const *const broadcast_p return ret; } - /* Register callbacks per stream */ - for (size_t j = 0U; j < create_param[big_index].subgroup_count; j++) { - for (size_t k = 0; k < create_param[big_index].subgroup_params[j].stream_count; - k++) { + for (size_t i = 0; i < create_param[big_index].subgroup_count; i++) { + if (!initialized[big_index]) { + struct bt_le_audio_tx_ctx *ctx = tx_ctx_get(big_index, i); + + if (ctx == NULL) { + LOG_ERR("Failed to get context for BIG %d subgroup %d", big_index, + i); + return -EINVAL; + } + + ret = bt_le_audio_tx_init(ctx, true); + if (ret != 0) { + LOG_ERR("Failed to initialize LE Audio TX: %d", ret); + return ret; + } + } + + /* Register callbacks per stream */ + for (size_t j = 0; j < create_param[big_index].subgroup_params[i].stream_count; + j++) { bt_cap_stream_ops_register( - create_param[big_index].subgroup_params[j].stream_params[k].stream, + create_param[big_index].subgroup_params[i].stream_params[j].stream, &stream_ops); } } @@ -796,9 +914,9 @@ int broadcast_source_enable(struct broadcast_source_big const *const broadcast_p return ret; } - initialized = true; - - LOG_DBG("Broadcast source enabled"); + initialized[big_index] = true; + LOG_INF("Created: %p %s", (void *)broadcast_sources[big_index], + broadcast_param->broadcast_name); return 0; } diff --git a/src/bluetooth/bt_stream/broadcast/broadcast_source.h b/src/bluetooth/bt_stream/broadcast/broadcast_source.h index d13a04fc..00dd84c1 100644 --- a/src/bluetooth/bt_stream/broadcast/broadcast_source.h +++ b/src/bluetooth/bt_stream/broadcast/broadcast_source.h @@ -4,6 +4,14 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @addtogroup audio_app_bt_stream + * @{ + * @defgroup broadcast_source Functions for broadcast source functionality. + * @{ + * @brief Helper functions to manage broadcast source functionality. + */ + #ifndef _BROADCAST_SOURCE_H_ #define _BROADCAST_SOURCE_H_ @@ -12,66 +20,66 @@ #include "bt_le_audio_tx.h" #if CONFIG_BT_AUDIO_BROADCAST_CONFIGURABLE -#define BT_BAP_LC3_BROADCAST_PRESET_NRF5340_AUDIO \ +#define BT_BAP_LC3_BROADCAST_PRESET_NRF_AUDIO \ BT_BAP_LC3_PRESET_CONFIGURABLE( \ BT_AUDIO_LOCATION_FRONT_LEFT | BT_AUDIO_LOCATION_FRONT_RIGHT, \ BT_AUDIO_CONTEXT_TYPE_MEDIA, CONFIG_BT_AUDIO_BITRATE_BROADCAST_SRC) #elif CONFIG_BT_BAP_BROADCAST_16_2_1 -#define BT_BAP_LC3_BROADCAST_PRESET_NRF5340_AUDIO \ +#define BT_BAP_LC3_BROADCAST_PRESET_NRF_AUDIO \ BT_BAP_LC3_BROADCAST_PRESET_16_2_1(BT_AUDIO_LOCATION_FRONT_LEFT | \ BT_AUDIO_LOCATION_FRONT_RIGHT, \ BT_AUDIO_CONTEXT_TYPE_MEDIA) #elif CONFIG_BT_BAP_BROADCAST_16_2_2 -#define BT_BAP_LC3_BROADCAST_PRESET_NRF5340_AUDIO \ +#define BT_BAP_LC3_BROADCAST_PRESET_NRF_AUDIO \ BT_BAP_LC3_BROADCAST_PRESET_16_2_2(BT_AUDIO_LOCATION_FRONT_LEFT | \ BT_AUDIO_LOCATION_FRONT_RIGHT, \ BT_AUDIO_CONTEXT_TYPE_MEDIA) #elif CONFIG_BT_BAP_BROADCAST_24_2_1 -#define BT_BAP_LC3_BROADCAST_PRESET_NRF5340_AUDIO \ +#define BT_BAP_LC3_BROADCAST_PRESET_NRF_AUDIO \ BT_BAP_LC3_BROADCAST_PRESET_24_2_1(BT_AUDIO_LOCATION_FRONT_LEFT | \ BT_AUDIO_LOCATION_FRONT_RIGHT, \ BT_AUDIO_CONTEXT_TYPE_MEDIA) #elif CONFIG_BT_BAP_BROADCAST_24_2_2 -#define BT_BAP_LC3_BROADCAST_PRESET_NRF5340_AUDIO \ +#define BT_BAP_LC3_BROADCAST_PRESET_NRF_AUDIO \ BT_BAP_LC3_BROADCAST_PRESET_24_2_2(BT_AUDIO_LOCATION_FRONT_LEFT | \ BT_AUDIO_LOCATION_FRONT_RIGHT, \ BT_AUDIO_CONTEXT_TYPE_MEDIA) #elif CONFIG_BT_BAP_BROADCAST_48_2_1 -#define BT_BAP_LC3_BROADCAST_PRESET_NRF5340_AUDIO \ +#define BT_BAP_LC3_BROADCAST_PRESET_NRF_AUDIO \ BT_BAP_LC3_BROADCAST_PRESET_48_2_1(BT_AUDIO_LOCATION_FRONT_LEFT | \ BT_AUDIO_LOCATION_FRONT_RIGHT, \ BT_AUDIO_CONTEXT_TYPE_MEDIA) #elif CONFIG_BT_BAP_BROADCAST_48_2_2 -#define BT_BAP_LC3_BROADCAST_PRESET_NRF5340_AUDIO \ +#define BT_BAP_LC3_BROADCAST_PRESET_NRF_AUDIO \ BT_BAP_LC3_BROADCAST_PRESET_48_2_2(BT_AUDIO_LOCATION_FRONT_LEFT | \ BT_AUDIO_LOCATION_FRONT_RIGHT, \ BT_AUDIO_CONTEXT_TYPE_MEDIA) #elif CONFIG_BT_BAP_BROADCAST_48_4_1 -#define BT_BAP_LC3_BROADCAST_PRESET_NRF5340_AUDIO \ +#define BT_BAP_LC3_BROADCAST_PRESET_NRF_AUDIO \ BT_BAP_LC3_BROADCAST_PRESET_48_4_1(BT_AUDIO_LOCATION_FRONT_LEFT | \ BT_AUDIO_LOCATION_FRONT_RIGHT, \ BT_AUDIO_CONTEXT_TYPE_MEDIA) #elif CONFIG_BT_BAP_BROADCAST_48_4_2 -#define BT_BAP_LC3_BROADCAST_PRESET_NRF5340_AUDIO \ +#define BT_BAP_LC3_BROADCAST_PRESET_NRF_AUDIO \ BT_BAP_LC3_BROADCAST_PRESET_48_4_2(BT_AUDIO_LOCATION_FRONT_LEFT | \ BT_AUDIO_LOCATION_FRONT_RIGHT, \ BT_AUDIO_CONTEXT_TYPE_MEDIA) #elif CONFIG_BT_BAP_BROADCAST_48_6_1 -#define BT_BAP_LC3_BROADCAST_PRESET_NRF5340_AUDIO \ +#define BT_BAP_LC3_BROADCAST_PRESET_NRF_AUDIO \ BT_BAP_LC3_BROADCAST_PRESET_48_6_1(BT_AUDIO_LOCATION_FRONT_LEFT | \ BT_AUDIO_LOCATION_FRONT_RIGHT, \ BT_AUDIO_CONTEXT_TYPE_MEDIA) #elif CONFIG_BT_BAP_BROADCAST_48_6_2 -#define BT_BAP_LC3_BROADCAST_PRESET_NRF5340_AUDIO \ +#define BT_BAP_LC3_BROADCAST_PRESET_NRF_AUDIO \ BT_BAP_LC3_BROADCAST_PRESET_48_6_2(BT_AUDIO_LOCATION_FRONT_LEFT | \ BT_AUDIO_LOCATION_FRONT_RIGHT, \ BT_AUDIO_CONTEXT_TYPE_MEDIA) @@ -222,14 +230,14 @@ int broadcast_source_id_get(uint8_t big_index, uint32_t *broadcast_id); /** * @brief Broadcast the Bluetooth LE Audio data. * + * @param[in] audio_frame Pointer to the audio buffer. * @param[in] big_index Index of the Broadcast Isochronous Group (BIG) to broadcast. * @param[in] subgroup_index Index of the subgroup to broadcast. - * @param[in] enc_audio Encoded audio struct. * * @return 0 for success, error otherwise. */ -int broadcast_source_send(uint8_t big_index, uint8_t subgroup_index, - struct le_audio_encoded_audio enc_audio); +int broadcast_source_send(struct net_buf const *const audio_frame, uint8_t big_index, + uint8_t subgroup_index); /** * @brief Disable the LE Audio broadcast (BIS) source. @@ -263,4 +271,9 @@ void broadcast_source_default_create(struct broadcast_source_big *broadcast_para int broadcast_source_enable(struct broadcast_source_big const *const broadcast_param, uint8_t big_index); +/** + * @} + * @} + */ + #endif /* _BROADCAST_SOURCE_H_ */ diff --git a/src/bluetooth/bt_stream/bt_le_audio_tx/CMakeLists.txt b/src/bluetooth/bt_stream/bt_le_audio_tx/CMakeLists.txt index e22ad9be..ac3887c9 100644 --- a/src/bluetooth/bt_stream/bt_le_audio_tx/CMakeLists.txt +++ b/src/bluetooth/bt_stream/bt_le_audio_tx/CMakeLists.txt @@ -4,7 +4,6 @@ # SPDX-License-Identifier: LicenseRef-Nordic-5-Clause # -if (CONFIG_BT_AUDIO_TX) - target_sources(app PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/bt_le_audio_tx.c) +if(CONFIG_BT_AUDIO_TX) + target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/bt_le_audio_tx.c) endif() diff --git a/src/bluetooth/bt_stream/bt_le_audio_tx/Kconfig b/src/bluetooth/bt_stream/bt_le_audio_tx/Kconfig new file mode 100644 index 00000000..3847b970 --- /dev/null +++ b/src/bluetooth/bt_stream/bt_le_audio_tx/Kconfig @@ -0,0 +1,37 @@ +menu "BT LE Audio TX" + +config NRF_AUDIO_TX_LEAD_TIME_TGT_US + int + default 2500 + range NRF_AUDIO_TX_LEAD_TIME_MIN_US 10000 + help + Do not change this value unless you know what you are doing. + Target lead time to send data to controller before it sends the next SDU on air. + +config NRF_AUDIO_TX_LEAD_TIME_MIN_US + int + default 2000 + help + Do not change this value unless you know what you are doing. + Min allowed lead time to send data to controller before it sends the next SDU on air. + Used to make sure that the data is sent to the controller just in time for the next SDU, + which can help with reducing the overall latency and power consumption. + Setting this too low may lead to flushed data. + Must be higher than HCI_ISO_TX_SDU_ARRIVAL_MARGIN_US plus a margin depending on SoC. + This value may take into account audio processing jitter, + which is caused by e.g. variable LC3 encoding time. + Modules may use this value in different ways. + +config NRF_AUDIO_TX_LEAD_TIME_BASE_US + int + default 4700 + help + NOTE: The actual value is the one above + an SDU interval. + Do not change this value unless you know what you are doing. + Max allowed lead time to send data to the controller before it sends the next SDU on air. + This means data is sent too early/fast, and will flush one SDU. + This value may take into account audio processing jitter, + which is caused by e.g. variable LC3 encoding time. + Modules may use this value in different ways. + +endmenu # BT LE Audio TX diff --git a/src/bluetooth/bt_stream/bt_le_audio_tx/bt_le_audio_tx.c b/src/bluetooth/bt_stream/bt_le_audio_tx/bt_le_audio_tx.c index 81550475..1c04467a 100644 --- a/src/bluetooth/bt_stream/bt_le_audio_tx/bt_le_audio_tx.c +++ b/src/bluetooth/bt_stream/bt_le_audio_tx/bt_le_audio_tx.c @@ -11,6 +11,7 @@ #include #include <../subsys/bluetooth/audio/bap_stream.h> #include +#include #include "zbus_common.h" #include "audio_sync_timer.h" @@ -23,69 +24,81 @@ ZBUS_CHAN_DEFINE(sdu_ref_chan, struct sdu_ref_msg, NULL, NULL, ZBUS_OBSERVERS_EM #define HANDLE_INVALID 0xFFFF -#define HCI_ISO_BUF_PER_CHAN 2 +BUILD_ASSERT(CONFIG_NRF_AUDIO_TX_LEAD_TIME_MIN_US > HCI_ISO_TX_SDU_ARRIVAL_MARGIN_US, + "CONFIG_NRF_AUDIO_TX_LEAD_TIME_MIN_US <= HCI_ISO_TX_SDU_ARRIVAL_MARGIN_US"); -#if defined(CONFIG_BT_ISO_MAX_CIG) && defined(CONFIG_BT_ISO_MAX_BIG) -#define GROUP_MAX (CONFIG_BT_ISO_MAX_BIG + CONFIG_BT_ISO_MAX_CIG) -#elif defined(CONFIG_BT_ISO_MAX_CIG) -#define GROUP_MAX CONFIG_BT_ISO_MAX_CIG -#elif defined(CONFIG_BT_ISO_MAX_BIG) -#define GROUP_MAX CONFIG_BT_ISO_MAX_BIG -#else -#error Neither CIG nor BIG defined -#endif +/* Maximum allowed time deviation for calling tx_send. + * E.g. 0.2 = 20 % deviation from the common interval + * So, if the common interval is 10 ms, the function can be called with an interval between 8 ms and + * 12 ms without considering it a gap and resyncing with the controller. + */ +#define INVOKE_TIME_LIMIT_DEVIATION_WINDOW 0.25f +#define INVOKE_TIME_LIM_MAX (1.0f + INVOKE_TIME_LIMIT_DEVIATION_WINDOW) +#define INVOKE_TIME_LIM_MIN (1.0f - INVOKE_TIME_LIMIT_DEVIATION_WINDOW) + +/* Time between performing a timestamp re-sync with the Bluetooth controller. + * For gateway/central (unicast client/broadcast source) where this device is the master + * Bluetooth LE clocking device, the wall clock and BLE clock should be in sync. + * Hence, there is no strict need for regular re-sync after the first timestamp is obtained. + * For simplicity and risk mitigation, a periodic re-sync is still performed. + * However, for the headset/peripheral (unicast server/broadcast sink) + * regular re-sync is needed to compensate for clock drift. + */ +#define TX_TS_RESYNC_US 1000000U + +/* When starting a stream, I2S or USB feeding the TX function with data, will usually need some time + * to stabilize and potentially drop data to meet just-in-time requirements. + */ +#define SUBSEQUENT_RAPID_CORRECTIONS_LIMIT 20U -#if (defined(CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT) && defined(CONFIG_BT_BAP_UNICAST)) -/* 1 since CIGs doesn't have the concept of subgroups */ -#define SUBGROUP_MAX (1 + CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT) +#define ISO_FLUSH_Q_STACK_SIZE 512 -#elif (defined(CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT) && !defined(CONFIG_BT_BAP_UNICAST)) -#define SUBGROUP_MAX CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT +BUILD_ASSERT(IS_ENABLED(CONFIG_BT_TX_PROCESSOR_THREAD), "BT_TX_PROCESSOR_THREAD must be enabled"); -#else -/* 1 since CIGs doesn't have the concept of subgroups */ -#define SUBGROUP_MAX 1 +#if CONFIG_BT_TX_PROCESSOR_THREAD_PRIO == K_LOWEST_APPLICATION_THREAD_PRIO +#error "CONFIG_BT_TX_PROCESSOR_THREAD_PRIO must be set higher than K_LOWEST_APPLICATION_THREAD_PRIO" #endif +#define ISO_FLUSH_Q_PRIORITY 0 -/* Since we can't assume that the number of streams are equally distributed on the subgroups ,we - * need to allocate the max number per subgroup - */ -#define STREAMS_MAX (CONFIG_BT_ISO_MAX_CHAN) +BUILD_ASSERT(ISO_FLUSH_Q_PRIORITY > CONFIG_BT_TX_PROCESSOR_THREAD_PRIO, + "ISO_FLUSH_Q_PRIORITY must have a lower or equal priority than " + "CONFIG_BT_TX_PROCESSOR_THREAD_PRIO"); -#define NET_BUF_POOL_MAX ((GROUP_MAX) * (SUBGROUP_MAX) * (STREAMS_MAX) * (HCI_ISO_BUF_PER_CHAN)) +static const int prio_bt_tx = CONFIG_BT_TX_PROCESSOR_THREAD_PRIO; -NET_BUF_POOL_FIXED_DEFINE(iso_tx_pool, NET_BUF_POOL_MAX, BT_ISO_SDU_BUF_SIZE(CONFIG_BT_ISO_TX_MTU), - CONFIG_BT_CONN_TX_USER_DATA_SIZE, NULL); +K_THREAD_STACK_DEFINE(iso_flush_work_q_stack_area, ISO_FLUSH_Q_STACK_SIZE); -struct tx_inf { - uint16_t iso_conn_handle; - struct bt_iso_tx_info iso_tx; - struct bt_iso_tx_info iso_tx_readback; - atomic_t iso_tx_pool_alloc; - bool hci_wrn_printed; -}; +static struct k_work_q iso_flush_work_q; +static struct k_work iso_flush_work; -static bool initialized; -static struct tx_inf tx_info_arr[GROUP_MAX][SUBGROUP_MAX][STREAMS_MAX]; +static void iso_flush_work_handler(struct k_work *work) +{ + (void)work; +} /** * @brief Sends audio data over a single BAP stream. * * @param data Audio data to send. * @param size Size of data. - * @param bap_stream Pointer to BAP stream to use. + * @param cap_stream Pointer to CAP stream to use. + * @param ctx Pointer to TX context. * @param tx_info Pointer to tx_info struct. * @param ts_tx Timestamp to send. Note that for some controllers, BT_ISO_TIMESTAMP_NONE * is used. This timestamp is used to ensure that SDUs are sent in the same * connection interval. + * @param send_ts_tx Whether to send the ts_tx timestamp or not. + * * @return 0 if successful, error otherwise. */ static int iso_stream_send(uint8_t const *const data, size_t size, struct bt_cap_stream *cap_stream, - struct tx_inf *tx_info, uint32_t ts_tx) + struct bt_le_audio_tx_ctx *ctx, struct tx_inf *tx_info) { int ret; struct net_buf *buf; + bool send_ts_tx = ctx->ts_ctlr_esti_us_valid; + /* net_buf_alloc allocates buffers for APP->NET transfer over HCI RPMsg, * but when these buffers are released it is not guaranteed that the * data has actually been sent. The data might be queued on the NET core, @@ -96,16 +109,22 @@ static int iso_stream_send(uint8_t const *const data, size_t size, struct bt_cap */ if (atomic_get(&tx_info->iso_tx_pool_alloc) >= HCI_ISO_BUF_PER_CHAN) { if (!tx_info->hci_wrn_printed) { + struct bt_iso_chan *iso_chan; + + iso_chan = bt_bap_stream_iso_chan_get(&cap_stream->bap_stream); + ARG_UNUSED(iso_chan); + LOG_WRN("HCI ISO TX overrun on stream %p - Single print", (void *)&cap_stream->bap_stream); tx_info->hci_wrn_printed = true; } + return -ENOMEM; } tx_info->hci_wrn_printed = false; - buf = net_buf_alloc(&iso_tx_pool, K_NO_WAIT); + buf = net_buf_alloc(ctx->iso_tx_pool, K_NO_WAIT); if (buf == NULL) { /* This should never occur because of the iso_tx_pool_alloc check above */ LOG_WRN("Out of TX buffers"); @@ -117,10 +136,10 @@ static int iso_stream_send(uint8_t const *const data, size_t size, struct bt_cap atomic_inc(&tx_info->iso_tx_pool_alloc); - if (ts_tx == 0) { - ret = bt_cap_stream_send(cap_stream, buf, tx_info->iso_tx.seq_num); + if (send_ts_tx) { + ret = bt_cap_stream_send_ts(cap_stream, buf, 0U, ctx->ts_ctlr_esti_us); } else { - ret = bt_cap_stream_send_ts(cap_stream, buf, tx_info->iso_tx.seq_num, ts_tx); + ret = bt_cap_stream_send(cap_stream, buf, 0U); } if (ret < 0) { @@ -129,10 +148,14 @@ static int iso_stream_send(uint8_t const *const data, size_t size, struct bt_cap (void *)&cap_stream->bap_stream); } net_buf_unref(buf); - atomic_dec(&tx_info->iso_tx_pool_alloc); + (void)atomic_dec(&tx_info->iso_tx_pool_alloc); return ret; } else { - tx_info->iso_tx.seq_num++; + if (ctx->last_data_status == STATUS_NOT_SET && send_ts_tx) { + ctx->last_data_status = STATUS_SENT_WITH_TS; + } else if (ctx->last_data_status == STATUS_NOT_SET && !send_ts_tx) { + ctx->last_data_status = STATUS_SENT_WITHOUT_TS; + } } return 0; @@ -141,24 +164,24 @@ static int iso_stream_send(uint8_t const *const data, size_t size, struct bt_cap static int get_tx_sync_sdc(uint16_t iso_conn_handle, struct bt_iso_tx_info *info) { int ret; - sdc_hci_cmd_vs_iso_read_tx_timestamp_t cmd_read_tx_timestamp; + sdc_hci_cmd_vs_iso_read_tx_timestamp_t cmd_read_tx_ts; sdc_hci_cmd_vs_iso_read_tx_timestamp_return_t rsp_params; - cmd_read_tx_timestamp.conn_handle = iso_conn_handle; + cmd_read_tx_ts.conn_handle = iso_conn_handle; - ret = hci_vs_sdc_iso_read_tx_timestamp(&cmd_read_tx_timestamp, &rsp_params); - if (ret) { + ret = hci_vs_sdc_iso_read_tx_timestamp(&cmd_read_tx_ts, &rsp_params); + if (ret != 0) { return ret; } info->ts = rsp_params.tx_time_stamp; info->seq_num = rsp_params.packet_sequence_number; - info->offset = 0; + info->offset = 0U; return 0; } -static int iso_conn_handle_set(struct bt_bap_stream *bap_stream, uint16_t *iso_conn_handle) +static int iso_conn_handle_set(const struct bt_bap_stream *bap_stream, uint16_t *iso_conn_handle) { int ret; @@ -166,13 +189,13 @@ static int iso_conn_handle_set(struct bt_bap_stream *bap_stream, uint16_t *iso_c struct bt_bap_ep_info ep_info; ret = bt_bap_ep_get_info(bap_stream->ep, &ep_info); - if (ret) { + if (ret != 0) { LOG_WRN("Unable to get info for ep"); return -EACCES; } ret = bt_hci_get_conn_handle(ep_info.iso_chan->iso, iso_conn_handle); - if (ret) { + if (ret != 0) { LOG_ERR("Failed obtaining conn_handle (ret:%d)", ret); return ret; } @@ -183,182 +206,610 @@ static int iso_conn_handle_set(struct bt_bap_stream *bap_stream, uint16_t *iso_c return 0; } -int bt_le_audio_tx_send(struct le_audio_tx_info *tx, uint8_t num_tx, - struct le_audio_encoded_audio enc_audio) +static int verify_stream_idx(struct stream_index idx) { - int ret; - size_t data_size_pr_stream = 0; - - if (!initialized) { - return -EACCES; - } - - if (tx == NULL) { - return -EINVAL; + if (idx.lvl1 >= GROUP_MAX || idx.lvl2 >= SUBGROUP_MAX || idx.lvl3 >= TX_STREAMS_MAX) { + return -ESPIPE; } - data_size_pr_stream = enc_audio.size / enc_audio.num_ch; - - /* When sending ISO data, we always send ts = 0 to the first active transmitting channel. - * The controller will populate with a ts which is fetched using bt_iso_chan_get_tx_sync. - * This timestamp will be submitted to all the other channels in order to place data on all - * channels in the same ISO interval. - */ - - uint32_t common_tx_sync_ts_us = 0; - uint32_t curr_ts_us = 0; - bool ts_common_acquired = false; - uint32_t common_interval = 0; + return 0; +} +static int tx_entries_validate(struct le_audio_tx_info const *const tx, uint8_t num_tx) +{ for (int i = 0; i < num_tx; i++) { - struct tx_inf *tx_info = - &tx_info_arr[tx[i].idx.lvl1][tx[i].idx.lvl2][tx[i].idx.lvl3]; + int ret = verify_stream_idx(tx[i].idx); - if (tx_info->iso_tx.seq_num == 0) { - /* Temporary fix until /zephyr/pull/68745/ is available - */ -#if defined(CONFIG_BT_BAP_DEBUG_STREAM_SEQ_NUM) - tx[i].cap_stream->bap_stream._prev_seq_num = 0; -#endif /* CONFIG_BT_BAP_DEBUG_STREAM_SEQ_NUM */ + if (ret != 0) { + LOG_ERR("Invalid stream index for tx[%d]", i); + return -EINVAL; } - if (!le_audio_ep_state_check(tx[i].cap_stream->bap_stream.ep, - BT_BAP_EP_STATE_STREAMING)) { - /* This bap_stream is not streaming*/ - continue; + if (tx[i].cap_stream == NULL || tx[i].cap_stream->bap_stream.ep == NULL || + tx[i].cap_stream->bap_stream.qos == NULL || + tx[i].cap_stream->bap_stream.codec_cfg == NULL) { + LOG_ERR("Invalid tx stream configuration for tx[%d]", i); + return -EINVAL; } + } + + return 0; +} - if (tx[i].audio_channel > enc_audio.num_ch) { - LOG_WRN("Unsupported audio_channel: %d", tx[i].audio_channel); +/** + * @brief Validate all TX streams before sending audio data. + * + * Iterates over all streams in @p tx and, for each stream in the STREAMING state: + * - Increments @p num_streaming_state. + * - Verifies that @p data_size_pr_stream matches the SDU size derived from the + * stream's codec configuration (bitrate and frame duration). + * - Verifies that the streams share the same ISO interval, and + * writes the common interval to @p common_interval_us. + * + * Streams not in the STREAMING state are silently skipped. + */ +static int tx_precheck(struct le_audio_tx_info const *const tx, uint8_t num_tx, + size_t data_size_pr_stream, uint32_t *common_interval_us, + uint8_t *num_streaming_state) +{ + int ret; + + for (int i = 0; i < num_tx; i++) { + const struct bt_bap_stream *bap_stream = &tx[i].cap_stream->bap_stream; + + if (unlikely(!le_audio_ep_state_check(bap_stream->ep, BT_BAP_EP_STATE_STREAMING))) { + /* This bap_stream is not streaming */ continue; } + (*num_streaming_state)++; + uint32_t bitrate; + int dur_us; - ret = le_audio_bitrate_get(tx[i].cap_stream->bap_stream.codec_cfg, &bitrate); - if (ret) { + ret = le_audio_bitrate_get(bap_stream->codec_cfg, &bitrate); + if (ret != 0) { LOG_ERR("Failed to calculate bitrate: %d", ret); return ret; } - if (data_size_pr_stream != LE_AUDIO_SDU_SIZE_OCTETS(bitrate)) { - LOG_ERR("The encoded data size does not match the SDU size"); + ret = le_audio_duration_us_get(bap_stream->codec_cfg, &dur_us); + if (ret != 0) { + LOG_ERR("Error retrieving frame duration: %d", ret); + return ret; + } + + if (unlikely(data_size_pr_stream != LE_AUDIO_SDU_SIZE_OCTETS(bitrate, dur_us))) { + LOG_ERR("The encoded data size (%zu) does not match the SDU size (%d)", + data_size_pr_stream, LE_AUDIO_SDU_SIZE_OCTETS(bitrate, dur_us)); return -EINVAL; } - if (common_interval != 0 && - (common_interval != tx[i].cap_stream->bap_stream.qos->interval)) { + if (unlikely(*common_interval_us != 0U && + (*common_interval_us != bap_stream->qos->interval))) { LOG_ERR("Not all channels have the same ISO interval"); return -EINVAL; } - common_interval = tx[i].cap_stream->bap_stream.qos->interval; - /* Check if same audio is sent to all channels */ - if (enc_audio.num_ch == 1) { - ret = iso_stream_send(enc_audio.data, data_size_pr_stream, tx[i].cap_stream, - tx_info, common_tx_sync_ts_us); - } else { - ret = iso_stream_send( - &enc_audio.data[(data_size_pr_stream * tx[i].audio_channel)], - data_size_pr_stream, tx[i].cap_stream, tx_info, - common_tx_sync_ts_us); + *common_interval_us = bap_stream->qos->interval; + } + + return 0; +} + +static int controller_ts_tx_get(const struct bt_bap_stream *bap_stream, uint32_t *ts_tx_us, + struct tx_inf *tx_info) +{ + int ret; + + ret = iso_conn_handle_set(bap_stream, &tx_info->iso_conn_handle); + if (ret != 0) { + LOG_ERR("Failed to set iso_conn_handle: %d", ret); + *ts_tx_us = 0U; + return ret; + } + + /* The host may re-order ISO and command data. Hence, we flush the ISO data by executing + * a workqueue which executes with lower priority than the BT TX processor. + * See https://github.com/zephyrproject-rtos/zephyr/issues/110939. + */ + int prio_this = k_thread_priority_get(k_current_get()); + + LOG_DBG("Tx sync. Current prio: %d, BT TX processor prio: %d", prio_this, prio_bt_tx); + + if (prio_this <= prio_bt_tx || prio_this < 0) { + ret = k_work_submit_to_queue(&iso_flush_work_q, &iso_flush_work); + if (ret < 0) { + LOG_WRN("Failed to submit ISO flush work: %d", ret); + return ret; } - if (ret) { - /* DBG used here as prints are handled within iso_stream_send */ - LOG_DBG("Failed to send to idx: %d stream: %p, ret: %d ", i, - (void *)&tx[i].cap_stream->bap_stream, ret); - continue; + ret = k_work_queue_drain(&iso_flush_work_q, false); + if (ret < 0) { + LOG_ERR("Failed to drain ISO flush work return value: %d", ret); + return -EFAULT; } + } - ret = iso_conn_handle_set(&tx[i].cap_stream->bap_stream, &tx_info->iso_conn_handle); - if (ret) { - continue; + ret = get_tx_sync_sdc(tx_info->iso_conn_handle, &tx_info->iso_tx_readback); + if (ret != 0) { + LOG_ERR("Unable to get tx sync. ret: %d", ret); + *ts_tx_us = 0U; + return ret; + } + + *ts_tx_us = tx_info->iso_tx_readback.ts; + return 0; +} + +static int num_loc_get(struct net_buf const *const audio_frame, uint8_t *const num_loc) +{ + struct audio_metadata *meta = net_buf_user_data(audio_frame); + + if (meta == NULL) { + LOG_ERR("Audio metadata is missing"); + return -EINVAL; + } + + *num_loc = audio_metadata_num_loc_get(meta); + + if (*num_loc == 0U || (audio_frame->len % *num_loc != 0U)) { + LOG_ERR("Invalid number (%u) of locations in audio frame (%u)", *num_loc, + audio_frame->len); + return -EINVAL; + } + + return 0; +} + +static void tx_call_interval_check(struct bt_le_audio_tx_ctx *ctx, uint32_t ts_now_us, + uint32_t common_interval_us) +{ + uint32_t time_since_last_call_us = ts_now_us - ctx->ts_last_us; + uint32_t upper_lim_us = (uint32_t)(common_interval_us * INVOKE_TIME_LIM_MAX); + uint32_t lower_lim_us = (uint32_t)(common_interval_us * INVOKE_TIME_LIM_MIN); + const uint32_t PRINT_WRN_LIMIT_US = 200000U; + + if (!IN_RANGE(time_since_last_call_us, lower_lim_us, upper_lim_us) && + ctx->ts_last_us_valid) { + /* This will happen if a stream is paused/restarted */ + ctx->ts_ctlr_esti_us_valid = false; + + if (time_since_last_call_us > PRINT_WRN_LIMIT_US) { + /* Likely a result of a stream being paused/restarted */ + LOG_INF("Audio tx called with interval of %u us, (min %u us, max %u us) " + "expected " + "around %u us. Resync with controller", + time_since_last_call_us, lower_lim_us, upper_lim_us, + common_interval_us); + } else { + LOG_WRN("Audio tx called with interval of %u us, (min %u us, max %u us) " + "expected " + "around %u us. Resync with controller", + time_since_last_call_us, lower_lim_us, upper_lim_us, + common_interval_us); } + } +} - /* Strictly, it is only required to call get_tx_sync_sdc on the first streaming - * channel to get the timestamp which is sent to all other channels. - * However, to be able to detect errors, this is called on each TX. - */ - ret = get_tx_sync_sdc(tx_info->iso_conn_handle, &tx_info->iso_tx_readback); - if (ret) { - if (ret != -ENOTCONN) { - LOG_WRN("Unable to get tx sync. ret: %d stream: %p", ret, - (void *)&tx[i].cap_stream->bap_stream); +static uint8_t tx_streams_send(struct bt_le_audio_tx_ctx *ctx, + struct net_buf const *const audio_frame, struct le_audio_tx_info *tx, + uint8_t num_tx, uint8_t num_loc, size_t data_size_pr_stream, + const struct bt_bap_stream **last_successful_bap_stream, + struct tx_inf **last_successful_tx_info) +{ + int ret; + uint8_t sent_streams = 0U; + + for (int i = 0; i < num_tx; i++) { + const struct bt_bap_stream *bap_stream = &tx[i].cap_stream->bap_stream; + struct tx_inf *tx_info = &ctx->tx_info_arr[tx[i].idx.lvl3]; + + /* Check if same audio is sent to all locations */ + if (num_loc == 1U) { + ret = iso_stream_send(audio_frame->data, data_size_pr_stream, + tx[i].cap_stream, ctx, tx_info); + } else { + if (unlikely(tx[i].audio_location >= num_loc)) { + LOG_ERR("audio_location (%u) out of bounds (num_loc: %u) for " + "stream " + "%d", + tx[i].audio_location, num_loc, i); + continue; } - continue; + + ret = iso_stream_send(audio_frame->data + + (tx[i].audio_location * data_size_pr_stream), + data_size_pr_stream, tx[i].cap_stream, ctx, tx_info); } - if (!ts_common_acquired) { - curr_ts_us = audio_sync_timer_capture(); - common_tx_sync_ts_us = tx_info->iso_tx_readback.ts; - ts_common_acquired = true; + if (ret != 0) { + /* May happen if we try to send to a device which has disconnected */ + LOG_INF_RATELIMIT_RATE(1000, + "Failed to send to idx: %d stream: %p, ret: %d ", i, + (void *)&tx[i].cap_stream->bap_stream, ret); + } else { + *last_successful_bap_stream = bap_stream; + *last_successful_tx_info = tx_info; + sent_streams++; } } - if (ts_common_acquired) { - struct sdu_ref_msg msg; + return sent_streams; +} + +static int tx_controller_sync_and_correct(struct bt_le_audio_tx_ctx *ctx, + const struct bt_bap_stream *last_successful_bap_stream, + uint32_t common_interval_us, uint32_t ts_now_us, + struct tx_inf *tx_info) +{ + int ret; + uint32_t ts_ctlr_real_us = 0U; + bool sync_required = false; + + int64_t corr_diff_us = 0U; + + /* Timestamp is invalid: send with no timestamp and re-sync */ + if (!ctx->ts_ctlr_esti_us_valid) { + sync_required = true; + } + + /* Time since last correction over limit: send with no timestamp and re-sync. + * This is required on the headset/peripheral/broadcast sink side to compensate + * for clock drift compared to the gateway/central/broadcast source. + */ + if ((uint32_t)(ctx->ts_ctlr_esti_us - ctx->ts_ctlr_real_us_last) >= TX_TS_RESYNC_US) { + sync_required = true; + } + + /* Same as above. Checks against current time. */ + if ((uint32_t)(ts_now_us - ctx->ts_last_us) >= TX_TS_RESYNC_US) { + sync_required = true; + } - msg.tx_sync_ts_us = common_tx_sync_ts_us; - msg.curr_ts_us = curr_ts_us; - msg.adjust = true; + if (!sync_required) { + return 0; + } - ret = zbus_chan_pub(&sdu_ref_chan, &msg, K_NO_WAIT); - if (ret) { - LOG_WRN("Failed to publish timestamp: %d", ret); + ret = controller_ts_tx_get(last_successful_bap_stream, &ts_ctlr_real_us, tx_info); + if (ret != 0) { + LOG_ERR("Failed to get timestamp after sending: %d", ret); + return ret; + } + + corr_diff_us = (int64_t)ts_ctlr_real_us - (int64_t)ctx->ts_ctlr_esti_us; + LOG_DBG("Controller timestamp sync required. Estimated: %u us, Real: %u us, Diff: %lld us", + ctx->ts_ctlr_esti_us, ts_ctlr_real_us, corr_diff_us); + + if ((uint32_t)(ts_now_us - ctx->ts_last_correction) < TX_TS_RESYNC_US) { + ctx->subsequent_rapid_corrections++; + } else { + ctx->subsequent_rapid_corrections = 0U; + } + + if (ctx->subsequent_rapid_corrections > SUBSEQUENT_RAPID_CORRECTIONS_LIMIT) { + LOG_ERR_RATELIMIT_RATE(1000U, "%d subsequent time corrections", + ctx->subsequent_rapid_corrections); + } + + /* There are several options which can occur at this point: + * + * - corr_diff_us == 0: The estimate is correct, no correction needed. + * + * - corr_diff_us >> 0: Data was given to this module too slow/late. + * Hence, the controller schedules sending for the next ISO + * interval. An empty packet will be sent on air. + * + * - corr_diff_us << 0: Data was given to this module too fast/early. + * + * - corr_diff_us > or < 0: This means that the estimate has gotten a small + * correction. As a central/gateway/unicast client/broadcast source, the + * clocks shall be running in lockstep and this should never happen. As a + * peripheral/headset/unicast server/broadcast sink, this will occur to + * correct for drift. + */ + + const int64_t half_common_interval_us = (int64_t)common_interval_us / 2; + + if (corr_diff_us == 0) { + /* No correction needed, the estimate is correct. */ + LOG_DBG_RATELIMIT("TX clock no adj (%lld us)", corr_diff_us); + ctx->ts_ctlr_esti_us_valid = true; + + } else if (corr_diff_us > half_common_interval_us) { + LOG_DBG("TX clock corrected by (%lld us). Late: Empty packet(s) on air", + corr_diff_us); + + ctx->ts_ctlr_esti_us_valid = true; + } else if (corr_diff_us < -half_common_interval_us) { + LOG_DBG("TX clock corrected by (%lld us). Early", corr_diff_us); + ctx->ts_ctlr_esti_us_valid = true; + } else { + /* Ts corrected by some small factor*/ + if (ctx->is_ble_clock_master) { + /* The gateway is the Bluetooth central. This shall not happen */ + LOG_ERR("TX clock has been drift adjusted by (%lld us)", corr_diff_us); + ctx->ts_ctlr_esti_us_valid = false; + return -EFAULT; + } else { + LOG_DBG("TX clock has been drift adjusted by (%lld us)", corr_diff_us); + ctx->ts_ctlr_esti_us_valid = true; + } + } + + ctx->corr_diff_us = corr_diff_us; + ctx->ts_ctlr_real_us_last = ts_ctlr_real_us; + /* Update the estimate to the real (most current) value */ + ctx->ts_ctlr_esti_us = ts_ctlr_real_us; + ctx->ts_last_correction = ts_now_us; + + return 0; +} + +static int tx_publish_sdu_ref_and_finalize(struct bt_le_audio_tx_ctx *ctx, uint32_t ts_now_us, + struct le_audio_tx_info *tx, uint8_t num_tx) +{ + int ret; + struct sdu_ref_msg msg; + + msg.tx_sync_ts_us = ctx->ts_ctlr_esti_us; + msg.tx_sync_ts_us_valid = ctx->ts_ctlr_esti_us_valid; + msg.curr_ts_us = audio_sync_timer_capture(); + msg.adjust = true; + msg.status = ctx->last_data_status; + + /* Add stream indices */ + msg.num_idx = num_tx; + for (uint8_t i = 0; i < num_tx; i++) { + msg.idx[i] = tx[i].idx; + } + + ret = zbus_chan_pub(&sdu_ref_chan, &msg, K_NO_WAIT); + if (ret != 0) { + LOG_ERR("Failed to publish timestamp: %d", ret); + return ret; + } + + return 0; +} + +int bt_le_audio_tx_send(struct bt_le_audio_tx_ctx *ctx, struct net_buf const *const audio_frame, + struct le_audio_tx_info *tx, uint8_t num_tx) +{ + int ret; + size_t data_size_pr_stream = 0U; + + if (unlikely(ctx == NULL || tx == NULL || num_tx == 0U || audio_frame == NULL)) { + return -EINVAL; + } + + ctx->last_data_status = STATUS_NOT_SET; + + ret = tx_entries_validate(tx, num_tx); + if (ret != 0) { + return ret; + } + + uint8_t num_loc; + + ret = num_loc_get(audio_frame, &num_loc); + if (ret != 0) { + return ret; + } + + if (num_loc > num_tx) { + /* May happen if we are trying to send N streams, but N-X locations are available */ + LOG_DBG("Number of locations (%u) more than number of streams (%u)", num_loc, + num_tx); + } else if (num_loc < num_tx) { + if (num_loc != 1U) { + LOG_ERR("If number of locations (%u) is less than number of streams (%u) " + "it must be 1. Single loc. sent to all streams.", + num_loc, num_tx); + return -EINVAL; + } + + if (!IS_ENABLED(CONFIG_MONO_TO_ALL_RECEIVERS)) { + LOG_ERR("Num locations (%u) sent to all streams (%u)." + "CONFIG_MONO_TO_ALL_RECEIVERS must be enabled", + num_loc, num_tx); + return -EINVAL; + } + } + + uint32_t common_interval_us = 0U; + uint8_t num_streaming_state = 0U; + + data_size_pr_stream = audio_frame->len / num_loc; + if (unlikely(data_size_pr_stream == 0U)) { + LOG_ERR("Data size per stream is 0"); + return -EINVAL; + } + + ret = tx_precheck(tx, num_tx, data_size_pr_stream, &common_interval_us, + &num_streaming_state); + if (ret != 0) { + return ret; + } + + if (common_interval_us == 0U) { + LOG_ERR("common_interval is zero"); + return -EINVAL; + } + + uint32_t ts_now_us = audio_sync_timer_capture(); + + if (num_streaming_state == 0U) { + LOG_DBG("No streams are in streaming state"); + goto finalize; + } + + uint8_t sent_streams = 0U; + const struct bt_bap_stream *last_successful_bap_stream = NULL; + struct tx_inf *tx_info = NULL; + + tx_call_interval_check(ctx, ts_now_us, common_interval_us); + + ctx->ts_last_us = ts_now_us; + ctx->ts_last_us_valid = true; + ctx->ts_ctlr_esti_us += common_interval_us; + + if (ctx->ts_ctlr_esti_us_valid) { + + /* A large positive time diff: + * Too fast/too much data -> Throw + * A small positive time diff or negative: + * Too slow/not enough data -> Skip SDU + */ + int32_t time_diff_us = (int32_t)(ctx->ts_ctlr_esti_us - ts_now_us); + + LOG_DBG_RATELIMIT_RATE(1000, "Time diff to controller timestamp: %d us", + time_diff_us); + + /* The time diffs below will be too large at times if the audio source is not + * synced to the BLE master clock. This will be the case for syncronous USB, where + * the USB host (often a PC) will provide data slightly faster or slower than the + * BLE master clock. I2S or asynchronous USB will not have this issue. + */ + + if (time_diff_us < CONFIG_NRF_AUDIO_TX_LEAD_TIME_MIN_US) { + /* If the diff is below the margin: + * Data provided too slow. Increase TS by + * an extra interval, get one empty SDU on air per location. + */ + LOG_INF("Data provided too slow. May happen for sync USB. time_diff_us:" + " %d(not in range : %u - %u) ", + time_diff_us, + (uint32_t)(CONFIG_NRF_AUDIO_TX_LEAD_TIME_BASE_US + + common_interval_us), + (uint32_t) CONFIG_NRF_AUDIO_TX_LEAD_TIME_MIN_US); + + ctx->ts_ctlr_esti_us += common_interval_us; + ctx->last_data_status = STATUS_UNDERRUN_EMPTY_SDU_ON_AIR; + + } else if (time_diff_us > + (CONFIG_NRF_AUDIO_TX_LEAD_TIME_BASE_US + common_interval_us)) { + + /* Data provided too fast. Will need to flush this data */ + LOG_INF("Data provided too fast. May happen for sync USB. time_diff_us: " + " %d (not in range: %u - %u)", + time_diff_us, + (uint32_t)(CONFIG_NRF_AUDIO_TX_LEAD_TIME_BASE_US + + common_interval_us), + (uint32_t) CONFIG_NRF_AUDIO_TX_LEAD_TIME_MIN_US); + ctx->ts_ctlr_esti_us -= common_interval_us; + ctx->last_data_status = STATUS_OVERRUN_FLUSHED; + goto finalize; + } else { + /* Do nothing. */ } + + /* If ts_ctlr_esti_us_valid is false, this means we shall send with no timestamp. + * This can lead to tx_sync_get returning a timestamp which is +1 or +2 intervals. + */ + } + + sent_streams = tx_streams_send(ctx, audio_frame, tx, num_tx, num_loc, data_size_pr_stream, + &last_successful_bap_stream, &tx_info); + + if (sent_streams == 0U) { + /* If nothing was sent, do not continue with timestamp handling + * Info is printed in tx_streams_send(). + */ + LOG_DBG("No streams were sent"); + return -EIO; + } + + if (last_successful_bap_stream == NULL || tx_info == NULL) { + LOG_ERR("No successful stream available for timestamp query"); + return -EIO; + } + + ret = tx_controller_sync_and_correct(ctx, last_successful_bap_stream, common_interval_us, + ts_now_us, tx_info); + + if (ret != 0) { + LOG_WRN("Failed to sync and correct with controller after sending: %d", ret); + return ret; + } + +finalize: + ret = tx_publish_sdu_ref_and_finalize(ctx, ts_now_us, tx, num_tx); + if (ret != 0) { + LOG_ERR("Failed to publish SDU reference and finalize: %d", ret); + return ret; } return 0; } -int bt_le_audio_tx_stream_started(struct stream_index idx) +int bt_le_audio_tx_stream_started(struct bt_le_audio_tx_ctx *ctx, struct stream_index idx) { - if (!initialized) { - return -EACCES; + int ret; + + if (ctx == NULL) { + return -EINVAL; + } + + ret = verify_stream_idx(idx); + if (ret != 0) { + return ret; } - atomic_clear(&tx_info_arr[idx.lvl1][idx.lvl2][idx.lvl3].iso_tx_pool_alloc); + (void)atomic_clear(&ctx->tx_info_arr[idx.lvl3].iso_tx_pool_alloc); - tx_info_arr[idx.lvl1][idx.lvl2][idx.lvl3].hci_wrn_printed = false; - tx_info_arr[idx.lvl1][idx.lvl2][idx.lvl3].iso_conn_handle = HANDLE_INVALID; - tx_info_arr[idx.lvl1][idx.lvl2][idx.lvl3].iso_tx.seq_num = 0; - tx_info_arr[idx.lvl1][idx.lvl2][idx.lvl3].iso_tx_readback.seq_num = 0; + ctx->tx_info_arr[idx.lvl3].hci_wrn_printed = false; + ctx->tx_info_arr[idx.lvl3].iso_conn_handle = HANDLE_INVALID; + ctx->tx_info_arr[idx.lvl3].iso_tx_readback.seq_num = 0U; return 0; } -int bt_le_audio_tx_stream_sent(struct stream_index stream_idx) +int bt_le_audio_tx_stream_sent(struct bt_le_audio_tx_ctx *ctx, struct stream_index idx) { - if (!initialized) { - return -EACCES; + int ret; + + if (ctx == NULL) { + return -EINVAL; + } + + ret = verify_stream_idx(idx); + if (ret != 0) { + return ret; } - atomic_dec( - &tx_info_arr[stream_idx.lvl1][stream_idx.lvl2][stream_idx.lvl3].iso_tx_pool_alloc); + (void)atomic_dec(&ctx->tx_info_arr[idx.lvl3].iso_tx_pool_alloc); return 0; } -void bt_le_audio_tx_init(void) +int bt_le_audio_tx_init(struct bt_le_audio_tx_ctx *ctx, bool is_clock_master) { - if (initialized) { - /* If TX is disabled and enabled again this should be called to reset the state */ - LOG_DBG("Already initialized"); - } - - for (int i = 0; i < GROUP_MAX; i++) { - for (int j = 0; j < SUBGROUP_MAX; j++) { - for (int k = 0; k < STREAMS_MAX; k++) { - tx_info_arr[i][j][k].hci_wrn_printed = false; - tx_info_arr[i][j][k].iso_conn_handle = HANDLE_INVALID; - tx_info_arr[i][j][k].iso_tx.ts = 0; - tx_info_arr[i][j][k].iso_tx.offset = 0; - tx_info_arr[i][j][k].iso_tx.seq_num = 0; - tx_info_arr[i][j][k].iso_tx_readback.ts = 0; - tx_info_arr[i][j][k].iso_tx_readback.offset = 0; - tx_info_arr[i][j][k].iso_tx_readback.seq_num = 0; - } - } + if (ctx == NULL || ctx->iso_tx_pool == NULL) { + return -EINVAL; + } + + static bool init_done; + + struct net_buf_pool *tmp = ctx->iso_tx_pool; + + (void)memset(ctx, 0, sizeof(*ctx)); + + ctx->iso_tx_pool = tmp; + ctx->is_ble_clock_master = is_clock_master; + + for (int i = 0; i < TX_STREAMS_MAX; i++) { + ctx->tx_info_arr[i].iso_conn_handle = HANDLE_INVALID; } - initialized = true; + if (!init_done) { + k_work_queue_init(&iso_flush_work_q); + k_work_init(&iso_flush_work, iso_flush_work_handler); + + k_work_queue_start(&iso_flush_work_q, iso_flush_work_q_stack_area, + K_THREAD_STACK_SIZEOF(iso_flush_work_q_stack_area), + ISO_FLUSH_Q_PRIORITY, NULL); + } + + init_done = true; + + return 0; } diff --git a/src/bluetooth/bt_stream/bt_le_audio_tx/bt_le_audio_tx.h b/src/bluetooth/bt_stream/bt_le_audio_tx/bt_le_audio_tx.h index 51de67a0..b9da3aa0 100644 --- a/src/bluetooth/bt_stream/bt_le_audio_tx/bt_le_audio_tx.h +++ b/src/bluetooth/bt_stream/bt_le_audio_tx/bt_le_audio_tx.h @@ -4,59 +4,199 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @addtogroup audio_app_bt_stream + * @{ + * @defgroup le_audio_tx Functions for LE Audio TX functionality. + * @{ + * @brief Helper functions to manage LE Audio TX functionality. + */ + #ifndef _LE_AUDIO_TX_H_ #define _LE_AUDIO_TX_H_ +#include +#include #include #include #include +#include "zbus_common.h" #include "le_audio.h" +#include "audio_defines.h" struct le_audio_tx_info { struct stream_index idx; struct bt_cap_stream *cap_stream; - uint8_t audio_channel; + uint8_t audio_location; }; +struct bt_le_audio_tx_ctx; + +#define HCI_ISO_BUF_PER_CHAN 2 + +#if defined(CONFIG_BT_ISO_MAX_CIG) && defined(CONFIG_BT_ISO_MAX_BIG) +#define GROUP_MAX (CONFIG_BT_ISO_MAX_BIG + CONFIG_BT_ISO_MAX_CIG) +#elif defined(CONFIG_BT_ISO_MAX_CIG) +#define GROUP_MAX CONFIG_BT_ISO_MAX_CIG +#elif defined(CONFIG_BT_ISO_MAX_BIG) +#define GROUP_MAX CONFIG_BT_ISO_MAX_BIG +#else +#error Neither CIG nor BIG defined +#endif + +#if (defined(CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT) && defined(CONFIG_BT_BAP_UNICAST)) +#define SUBGROUP_MAX (1 + CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT) +#elif (defined(CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT) && !defined(CONFIG_BT_BAP_UNICAST)) +#define SUBGROUP_MAX CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT +#else +#define SUBGROUP_MAX 1 +#endif + +#define TX_STREAMS_MAX CONFIG_BT_ISO_MAX_CHAN + +#define TX_BUF_NUM (TX_STREAMS_MAX * HCI_ISO_BUF_PER_CHAN) + +struct tx_inf { + uint16_t iso_conn_handle; + struct bt_iso_tx_info iso_tx; + struct bt_iso_tx_info iso_tx_readback; + atomic_t iso_tx_pool_alloc; + bool hci_wrn_printed; +}; + +struct bt_le_audio_tx_ctx { + struct net_buf_pool *iso_tx_pool; + struct tx_inf tx_info_arr[TX_STREAMS_MAX]; + /* Estimated timestamp from the controller and validity */ + uint32_t ts_ctlr_esti_us; + bool ts_ctlr_esti_us_valid; + /* Real timestamp from the controller */ + uint32_t ts_ctlr_real_us_last; + /* Timestamp - when the last correction was applied */ + uint32_t ts_last_correction; + /* Counter to track subsequent rapid corrections. Indicates an error state */ + uint32_t subsequent_rapid_corrections; + /* Correction between estimated and real timestamps */ + int64_t corr_diff_us; + /* The previous "now" timestamp and validity */ + uint32_t ts_last_us; + bool ts_last_us_valid; + /* This device is Bluetooth clock master (central/unicast client or broadcast source) */ + bool is_ble_clock_master; + /* Status of the last submitted data */ + enum tx_data_status last_data_status; +}; + +#define BT_LE_AUDIO_TX_DEFINE(name) \ + NET_BUF_POOL_FIXED_DEFINE(name##_cfg_buf, TX_BUF_NUM, \ + BT_ISO_SDU_BUF_SIZE(CONFIG_BT_ISO_TX_MTU), \ + CONFIG_BT_CONN_TX_USER_DATA_SIZE, NULL); \ + static struct bt_le_audio_tx_ctx name##_ctx = { \ + .iso_tx_pool = &name##_cfg_buf, \ + }; \ + static struct bt_le_audio_tx_ctx *name = &name##_ctx + +#define BT_LE_AUDIO_TX_DEFINE_INDEXED(name, group_idx, subgroup_idx) \ + NET_BUF_POOL_FIXED_DEFINE(name##_##group_idx##_##subgroup_idx##_buf, TX_BUF_NUM, \ + BT_ISO_SDU_BUF_SIZE(CONFIG_BT_ISO_TX_MTU), \ + CONFIG_BT_CONN_TX_USER_DATA_SIZE, NULL); \ + static struct bt_le_audio_tx_ctx name##_##group_idx##_##subgroup_idx##_ctx = { \ + .iso_tx_pool = &name##_##group_idx##_##subgroup_idx##_buf, \ + }; \ + __unused static struct bt_le_audio_tx_ctx *name##_##group_idx##_##subgroup_idx = \ + &name##_##group_idx##_##subgroup_idx##_ctx + /** - * @brief Allocates buffers and sends data to the controller. + * @brief Sends N channels of synchronized audio data to the controller. + * + * @details + * This function is the main LE Audio TX scheduling point. One call corresponds to one + * frame interval for the configured streams. All streams which shall be sent together + * (synchronized) are sent at the same time. + * This implies calling once for every subgroup when doing broadcast and once for each + * CIG in unicast. If 0 is returned, it publishes a reference message on @c sdu_ref_chan. + * This ZBUS message contains the current time, and when the SDU(s) are expected on air. + * Calling too late/slow will cause empty data on air, whilst calling too early/fast will + * trigger this module to flush reduce latency and avoid buffer overruns. * - * @note Send all available channels in a single call. - * Do not call this for each channel. + * Summary: + * - Supply data which shall be sent synchronized in a CIS/subgroup. (E.g, + * 2 streams every 10 ms). + * - Call @ref bt_le_audio_tx_stream_sent for each stream after TX completion to keep + * pool accounting correct. * - * @param[in] tx Pointer to an array of le_audio_tx_info elements. + * @note Not tested for ISO interval not equal to frame length. + * + * @param[in] ctx Pointer to an initialized TX context pr CIG for unicast and pr + * subgroup for broadcast. + * @param[in] audio_frame Pointer to encoded audio data and metadata. + * @param[in] tx Pointer to an array of @ref le_audio_tx_info elements. * @param[in] num_tx Number of elements in @p tx. - * @param[in] enc_audio Encoded audio data. * - * @return 0 if successful, error otherwise. + * @retval 0 Success (including controlled pacing cases where no SDU is sent, + * but finalize/publish succeeds). + * @retval -EINVAL Invalid arguments, invalid stream index tuple, invalid frame + * metadata, or inconsistent stream/audio configuration. + * @retval -EIO No stream was sent successfully. + * + * @return Other negative error codes may be propagated from lower layers (for example + * timestamp synchronization/HCI readback or SDU reference publish failures). */ -int bt_le_audio_tx_send(struct le_audio_tx_info *tx, uint8_t num_tx, - struct le_audio_encoded_audio enc_audio); +int bt_le_audio_tx_send(struct bt_le_audio_tx_ctx *ctx, struct net_buf const *const audio_frame, + struct le_audio_tx_info *tx, uint8_t num_tx); /** - * @brief Initializes a stream. Must be called when a TX stream is started. + * @brief Initializes per-stream TX bookkeeping when a stream enters streaming state. * + * @param[in] ctx Pointer to TX context. * @param[in] stream_idx Stream index. * - * @retval -EACCES The module has not been initialized. * @retval 0 Success. + * @retval -EINVAL ctx is NULL or the module has not been initialized. + * @retval -ESPIPE stream_idx is out of bounds. */ -int bt_le_audio_tx_stream_started(struct stream_index stream_idx); +int bt_le_audio_tx_stream_started(struct bt_le_audio_tx_ctx *ctx, struct stream_index stream_idx); /** - * @brief Frees a TX buffer. Must be called when a TX stream has been sent. + * @brief Releases one outstanding TX pool allocation for the provided stream. * + * @details + * Call this after TX completion notification for each stream that consumed one TX buffer. + * This keeps the module's per-stream pool accounting aligned with actual controller + * completions. + * + * @param[in] ctx Pointer to TX context. * @param[in] stream_idx Stream index. * - * @retval -EACCES The module has not been initialized. * @retval 0 Success. + * @retval -EINVAL ctx is NULL or the module has not been initialized. + * @retval -ESPIPE stream_idx is out of bounds. */ -int bt_le_audio_tx_stream_sent(struct stream_index stream_idx); +int bt_le_audio_tx_stream_sent(struct bt_le_audio_tx_ctx *ctx, struct stream_index stream_idx); /** - * @brief Initializes the TX path for ISO transmission. + * @brief Initializes the TX path context and shared ISO flush worker. + * + * @param[in] ctx Pointer to TX context. + * @param[in] is_clock_master Set to true if this device is a Bluetooth central, + * (Unicast client) or Broadcast source, + * false = peripheral (unicast server) or Broadcast sink. + * + * @details + * The context memory is cleared and re-initialized while preserving @p ctx->iso_tx_pool. + * A shared internal work queue used for deferred ISO flush handling is started once on the + * first successful call. + * + * @retval 0 Success. + * @retval -EINVAL @p ctx is NULL or required configuration is missing. + */ +int bt_le_audio_tx_init(struct bt_le_audio_tx_ctx *ctx, bool is_clock_master); + +/** + * @} + * @} */ -void bt_le_audio_tx_init(void); + #endif /* _LE_AUDIO_TX_H_ */ diff --git a/src/bluetooth/bt_stream/le_audio.c b/src/bluetooth/bt_stream/le_audio.c index d4f1c50a..92daba75 100644 --- a/src/bluetooth/bt_stream/le_audio.c +++ b/src/bluetooth/bt_stream/le_audio.c @@ -6,70 +6,66 @@ #include "le_audio.h" -#include - #include +#include #include -#include -#include - -#include "AutoOffManager.h" -#include "zbus_common.h" +#include <../subsys/bluetooth/audio/bap_endpoint.h> +/* TODO: Can be removed when access is granted to internal structures */ +#include <../subsys/bluetooth/audio/cap_internal.h> #include LOG_MODULE_REGISTER(le_audio, CONFIG_BLE_LOG_LEVEL); -static const char le_audio_auto_off_token[] = "LEAudio"; - -ZBUS_CHAN_DECLARE(le_audio_chan); - -static void le_audio_auto_off_event_handler(const struct zbus_channel *chan); -ZBUS_LISTENER_DEFINE(le_audio_auto_off_listener, le_audio_auto_off_event_handler); - -static int le_audio_auto_off_init(void) +int le_audio_metadata_populate(struct audio_metadata *meta, const struct bt_bap_stream *stream, + const struct bt_iso_recv_info *info, + const struct net_buf *audio_frame) { int ret; - ret = auto_off_register_participant(le_audio_auto_off_token, - (power_saving_level_t)CONFIG_POWER_SAVING_LEVEL_LEAUDIO); - if (ret && ret != -EALREADY) { - LOG_WRN("Failed to register LE Audio with auto-off: %d", ret); + /* Populate the audio frame structure */ + meta->ref_ts_us = info->ts; + meta->bad_data = 0; + le_audio_freq_hz_get(stream->codec_cfg, &meta->sample_rate_hz); + le_audio_duration_us_get(stream->codec_cfg, &meta->data_len_us); + + ret = bt_audio_codec_cfg_get_chan_allocation(stream->codec_cfg, &meta->locations, true); + if (ret < 0) { + LOG_ERR("Failed to get channel allocation: %d", ret); return ret; } - auto_off_allow(le_audio_auto_off_token); + uint32_t octets_per_frame = 0; - ret = zbus_chan_add_obs(&le_audio_chan, &le_audio_auto_off_listener, - ZBUS_ADD_OBS_TIMEOUT_MS); - if (ret) { - LOG_WRN("Failed to add LE Audio auto-off listener: %d", ret); + ret = le_audio_octets_per_frame_get(stream->codec_cfg, &octets_per_frame); + if (ret < 0) { + LOG_ERR("Failed to get octets per frame: %d", ret); return ret; } - return 0; -} + meta->bytes_per_location = octets_per_frame / audio_metadata_num_loc_get(meta); + if (meta->bytes_per_location == 0) { + LOG_ERR("Failed to get bytes per location"); + return -EINVAL; + } -static void le_audio_auto_off_event_handler(const struct zbus_channel *chan) -{ - const struct le_audio_msg *msg = zbus_chan_const_msg(chan); - - switch (msg->event) { - case LE_AUDIO_EVT_STREAMING: - auto_off_prohibit(le_audio_auto_off_token); - break; - case LE_AUDIO_EVT_NOT_STREAMING: - case LE_AUDIO_EVT_SYNC_LOST: - case LE_AUDIO_EVT_NO_VALID_CFG: - auto_off_allow(le_audio_auto_off_token); - break; - default: - break; + if (stream->codec_cfg->id == BT_HCI_CODING_FORMAT_LC3) { + meta->data_coding = LC3; + } else { + LOG_ERR("Unsupported codec ID: %d", stream->codec_cfg->id); + return -EINVAL; } -} -SYS_INIT(le_audio_auto_off_init, APPLICATION, CONFIG_APPLICATION_INIT_PRIORITY); + /* A valid packet will always have a valid timestamp from the Softdevice Controller*/ + + if (!(info->flags & BT_ISO_FLAGS_VALID) || + audio_frame->len != bt_audio_codec_cfg_get_octets_per_frame(stream->codec_cfg)) { + meta->bad_data = meta->locations; + } -int le_audio_ep_state_get(struct bt_bap_ep *ep, uint8_t *state) + return 0; +} + +int le_audio_ep_state_get(struct bt_bap_ep const *const ep, uint8_t *state) { int ret; struct bt_bap_ep_info ep_info; @@ -91,7 +87,7 @@ int le_audio_ep_state_get(struct bt_bap_ep *ep, uint8_t *state) } /*TODO: Create helper function in host to perform this action. */ -bool le_audio_ep_state_check(struct bt_bap_ep *ep, enum bt_bap_ep_state state) +bool le_audio_ep_state_check(struct bt_bap_ep const *const ep, enum bt_bap_ep_state state) { int ret; uint8_t ep_state; @@ -212,8 +208,8 @@ int le_audio_bitrate_get(const struct bt_audio_codec_cfg *const codec, uint32_t return ret; } - int frames_per_sec = 1000000 / dur_us; - int octets_per_sdu; + float frames_per_sec = (float)USEC_PER_SEC / dur_us; + uint32_t octets_per_sdu; ret = le_audio_octets_per_frame_get(codec, &octets_per_sdu); if (ret) { @@ -230,10 +226,14 @@ int le_audio_stream_dir_get(struct bt_bap_stream const *const stream) int ret; struct bt_bap_ep_info ep_info; - ret = bt_bap_ep_get_info(stream->ep, &ep_info); + if (unlikely(stream->ep == NULL)) { + LOG_ERR("Stream has no endpoint"); + return -EINVAL; + } + ret = bt_bap_ep_get_info(stream->ep, &ep_info); if (ret) { - LOG_WRN("Failed to get ep_info"); + LOG_ERR("Failed to get ep_info %d", ret); return ret; } @@ -244,6 +244,7 @@ bool le_audio_bitrate_check(const struct bt_audio_codec_cfg *codec) { int ret; uint32_t octets_per_sdu; + int dur_us; ret = le_audio_octets_per_frame_get(codec, &octets_per_sdu); if (ret) { @@ -251,10 +252,16 @@ bool le_audio_bitrate_check(const struct bt_audio_codec_cfg *codec) return false; } - if (octets_per_sdu < LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MIN)) { + ret = le_audio_duration_us_get(codec, &dur_us); + if (ret) { + LOG_ERR("Error retrieving frame duration: %d", ret); + return false; + } + + if (octets_per_sdu < LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MIN, dur_us)) { LOG_WRN("Bitrate too low"); return false; - } else if (octets_per_sdu > LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX)) { + } else if (octets_per_sdu > LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX, dur_us)) { LOG_WRN("Bitrate too high"); return false; } @@ -361,15 +368,109 @@ void le_audio_print_codec(const struct bt_audio_codec_cfg *codec, enum bt_audio_ } else if (dir == BT_AUDIO_DIR_SOURCE) { LOG_INF("LC3 codec config for source:"); } else { - LOG_INF("LC3 codec config for :"); + LOG_ERR("LC3 codec config for :"); } - LOG_INF("\tFrequency: %d Hz", freq_hz); - LOG_INF("\tDuration: %d us", dur_us); - LOG_INF("\tChannel allocation: 0x%x", chan_allocation); - LOG_INF("\tOctets per frame: %d (%d bps)", octets_per_sdu, bitrate); - LOG_INF("\tFrames per SDU: %d", frame_blks_per_sdu); + LOG_INF("\tFrequency:\t %d Hz", freq_hz); + LOG_INF("\tDuration:\t %d us", dur_us); + LOG_INF("\tChan alloc:\t 0x%x", chan_allocation); + LOG_INF("\tOct pr frame:\t %d (%d bps)", octets_per_sdu, bitrate); + LOG_INF("\tFrames pr SDU:\t %d", frame_blks_per_sdu); } else { LOG_WRN("Codec is not LC3, codec_id: 0x%2x", codec->id); } } + +int le_audio_print_qos_from_stream(struct bt_bap_stream const *const stream) +{ + if (stream == NULL || stream->qos == NULL) { + LOG_WRN("Invalid parameters to print QoS"); + return -EINVAL; + } + + LOG_INF("BAP stream (%p) QoS:", (void *)stream); + LOG_INF("\tPres dly:\t %d us", stream->qos->pd); + if (stream->qos->framing == BT_BAP_QOS_CFG_FRAMING_UNFRAMED) { + LOG_INF("\tFraming:\t Unframed"); + } else if (stream->qos->framing == BT_BAP_QOS_CFG_FRAMING_FRAMED) { + LOG_INF("\tFraming:\t Framed"); + } else { + LOG_ERR("\tFraming:\t Unknown (%d)", stream->qos->framing); + } + + if (stream->qos->phy == BT_BAP_QOS_CFG_1M) { + LOG_INF("\tRadio PHY:\t 1M"); + } else if (stream->qos->phy == BT_BAP_QOS_CFG_2M) { + LOG_INF("\tRadio PHY:\t 2M"); + } else if (stream->qos->phy == BT_BAP_QOS_CFG_CODED) { + LOG_INF("\tRadio PHY:\t Coded"); + } else { + LOG_ERR("\tRadio PHY:\t Unknown (%d)", stream->qos->phy); + } + + LOG_INF("\tRecmd. ret.:\t %d", stream->qos->rtn); + LOG_INF("\tMax SDU size:\t %d", stream->qos->sdu); + +#if defined(CONFIG_BT_BAP_BROADCAST_SOURCE) || defined(CONFIG_BT_BAP_UNICAST) + LOG_INF("\tMax trans lat:\t %d ms", stream->qos->latency); +#endif /* CONFIG_BT_BAP_BROADCAST_SOURCE || CONFIG_BT_BAP_UNICAST */ + LOG_INF("\tSDU interval:\t %d us", stream->qos->interval); + + return 0; +} + +int le_audio_print_unicast_group(struct bt_cap_unicast_group const *const unicast_group) +{ + if (unicast_group == NULL) { + LOG_WRN("Invalid parameters to print unicast group"); + return -EINVAL; + } + + LOG_INF("Unicast Group Index: %d", unicast_group->bap_unicast_group->index); + + if (unicast_group->bap_unicast_group->sink_pd != BT_BAP_PD_UNSET) { + LOG_INF("\tSink pres dly:\t %u us", unicast_group->bap_unicast_group->sink_pd); + } else { + LOG_INF("\tSink pres dly:\t Not set"); + } + + if (unicast_group->bap_unicast_group->source_pd != BT_BAP_PD_UNSET) { + LOG_INF("\t Src pres dly:\t %u us", unicast_group->bap_unicast_group->source_pd); + } else { + LOG_INF("\t Src pres dly:\t Not set"); + } + + if (!unicast_group->bap_unicast_group->cig_param.c_to_p_interval) { + LOG_INF("\tC->P interval:\t Not set (%d)", + unicast_group->bap_unicast_group->cig_param.c_to_p_interval); + } else { + LOG_INF("\tC->P interval:\t %d us", + unicast_group->bap_unicast_group->cig_param.c_to_p_interval); + } + + if (!unicast_group->bap_unicast_group->cig_param.c_to_p_latency) { + LOG_INF("\tC->P latency:\t Not set (%d)", + unicast_group->bap_unicast_group->cig_param.c_to_p_latency); + } else { + LOG_INF("\tC->P latency:\t %d ms", + unicast_group->bap_unicast_group->cig_param.c_to_p_latency); + } + + if (!unicast_group->bap_unicast_group->cig_param.p_to_c_interval) { + LOG_INF("\tP->C interval:\t Not set (%d)", + unicast_group->bap_unicast_group->cig_param.p_to_c_interval); + } else { + LOG_INF("\tP->C interval:\t %d us", + unicast_group->bap_unicast_group->cig_param.p_to_c_interval); + } + + if (!unicast_group->bap_unicast_group->cig_param.p_to_c_latency) { + LOG_INF("\tP->C latency:\t Not set (%d)", + unicast_group->bap_unicast_group->cig_param.p_to_c_latency); + } else { + LOG_INF("\tP->C latency:\t %d ms", + unicast_group->bap_unicast_group->cig_param.p_to_c_latency); + } + + return 0; +} diff --git a/src/bluetooth/bt_stream/le_audio.h b/src/bluetooth/bt_stream/le_audio.h index 658a947a..3ddbdff8 100644 --- a/src/bluetooth/bt_stream/le_audio.h +++ b/src/bluetooth/bt_stream/le_audio.h @@ -4,14 +4,25 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @defgroup audio_app_bt_stream LE Audio helper functions + * @{ + * @brief LE Audio helper functions for Audio applications. + * + * This module provides the core LE Audio helper functions for both unicast (CIS) + * and broadcast (BIS) modes. + */ + #ifndef _LE_AUDIO_H_ #define _LE_AUDIO_H_ #include +#include +#include #include #define LE_AUDIO_ZBUS_EVENT_WAIT_TIME K_MSEC(5) -#define LE_AUDIO_SDU_SIZE_OCTETS(bitrate) (bitrate / (1000000 / CONFIG_AUDIO_FRAME_DURATION_US) / 8) +#define LE_AUDIO_SDU_SIZE_OCTETS(bitrate, dur_us) (bitrate / (USEC_PER_SEC / dur_us) / 8) #if CONFIG_SAMPLE_RATE_CONVERTER && CONFIG_AUDIO_SAMPLE_RATE_48000_HZ #define BT_AUDIO_CODEC_CAPABILIY_FREQ \ @@ -31,75 +42,101 @@ #error No sample rate supported #endif /* CONFIG_SAMPLE_RATE_CONVERTER */ +/** Configure LC3 codec preset with customizable parameters for LE Audio streams + * using location, stream context, and bitrate parameters. + */ #define BT_BAP_LC3_PRESET_CONFIGURABLE(_loc, _stream_context, _bitrate) \ - BT_BAP_LC3_PRESET(BT_AUDIO_CODEC_LC3_CONFIG(CONFIG_BT_AUDIO_PREF_SAMPLE_RATE_VALUE, \ + BT_BAP_LC3_PRESET(BT_AUDIO_CODEC_LC3_CONFIG(CONFIG_BT_AUDIO_PREF_SINK_SAMPLE_RATE_VALUE, \ BT_AUDIO_CODEC_CFG_DURATION_10, _loc, \ - LE_AUDIO_SDU_SIZE_OCTETS(_bitrate), 1, \ + LE_AUDIO_SDU_SIZE_OCTETS(_bitrate, 10000), 1, \ _stream_context), \ - BT_AUDIO_CODEC_QOS_UNFRAMED(10000u, LE_AUDIO_SDU_SIZE_OCTETS(_bitrate), \ - CONFIG_BT_AUDIO_RETRANSMITS, \ - CONFIG_BT_AUDIO_MAX_TRANSPORT_LATENCY_MS, \ - CONFIG_BT_AUDIO_PRESENTATION_DELAY_US)) + BT_BAP_QOS_CFG_UNFRAMED(10000u, \ + LE_AUDIO_SDU_SIZE_OCTETS(_bitrate, 10000), \ + CONFIG_BT_AUDIO_RETRANSMITS, \ + CONFIG_BT_AUDIO_MAX_TRANSPORT_LATENCY_MS, \ + CONFIG_BT_AUDIO_PRESENTATION_DELAY_US)) /** * @brief Callback for receiving Bluetooth LE Audio data. * - * @param data Pointer to received data. - * @param size Size of received data. - * @param bad_frame Indicating if the frame is a bad frame or not. - * @param sdu_ref ISO timestamp. - * @param channel_index Audio channel index. - * @param desired_size The expected data size. + * @param audio_frame Pointer to audio buffer. + * @param meta Pointer to audio metadata. + * @param location_index Audio location index. */ -typedef void (*le_audio_receive_cb)(const uint8_t *const data, size_t size, bool bad_frame, - uint32_t sdu_ref, enum audio_channel channel_index, - size_t desired_size); +typedef void (*le_audio_receive_cb)(struct net_buf *audio_frame, struct audio_metadata *meta, + uint8_t location_index); /** - * @brief Encoded audio data and information. + * @brief Stream index structure for identifying audio streams. * - * @note Container for SW codec (typically LC3) compressed audio data. + * This structure provides a hierarchical index for identifying audio streams + * in both unicast (CIS) and broadcast (BIS) modes. */ -struct le_audio_encoded_audio { - uint8_t const *const data; - size_t size; - uint8_t num_ch; -}; - struct stream_index { - uint8_t lvl1; /* BIG / CIG */ - uint8_t lvl2; /* Subgroups (only applicable to Broadcast) */ - uint8_t lvl3; /* BIS / CIS */ + /** Level 1: BIG (Broadcast Isochronous Group) or CIG (Connected Isochronous Group) */ + uint8_t lvl1; + /** Level 2: Subgroups (only applicable to Broadcast) */ + uint8_t lvl2; + /** Level 3: BIS (Broadcast Isochronous Stream) or CIS (Connected Isochronous Stream) */ + uint8_t lvl3; }; +/** + * @brief Get the number of active streams. + * + * @note This function returns the number of streams and locations that are currently active. + * An active stream is one that has been set up and is ready to receive data. + * This function is used by broadcast_sink, unicast_server and unicast_client. + * + * @param[out] num_streams Pointer to the number of active streams. + * @param[out] locations Pointer to the bitmask of active locations. + * + * @return 0 for success, error otherwise. + */ +int le_audio_concurrent_sync_num_get(uint8_t *num_streams, enum bt_audio_location *locations); + +/** + * @brief Function to populate the audio metadata. + * + * @param[in] meta Pointer to the audio metadata. + * @param[in] stream Pointer to the stream. + * @param[in] info Pointer to the ISO information. + * @param[in] audio_frame Pointer to the buffer. + * + * @return 0 if successful, error otherwise. + */ +int le_audio_metadata_populate(struct audio_metadata *meta, const struct bt_bap_stream *stream, + const struct bt_iso_recv_info *info, + const struct net_buf *audio_frame); + /** * @brief Get the current state of an endpoint. * - * @param[in] ep The endpoint to check. - * @param[out] state The state of the endpoint. + * @param[in] ep The endpoint to check. + * @param[out] state The state of the endpoint. * * @return 0 for success, error otherwise. */ -int le_audio_ep_state_get(struct bt_bap_ep *ep, uint8_t *state); +int le_audio_ep_state_get(struct bt_bap_ep const *const ep, uint8_t *state); /** * @brief Check if an endpoint is in the given state. * If the endpoint is NULL, it is not in the * given state, and this function returns false. * - * @param[in] ep The endpoint to check. - * @param[in] state The state to check for. + * @param[in] ep The endpoint to check. + * @param[in] state The state to check for. * * @retval true The endpoint is in the given state. * @retval false Otherwise. */ -bool le_audio_ep_state_check(struct bt_bap_ep *ep, enum bt_bap_ep_state state); +bool le_audio_ep_state_check(struct bt_bap_ep const *const ep, enum bt_bap_ep_state state); /** * @brief Check if an endpoint has had the QoS configured. * If the endpoint is NULL, it is not in the state, and this function returns false. * - * @param[in] ep The endpoint to check. + * @param[in] ep The endpoint to check. * * @retval true The endpoint QoS is configured. * @retval false Otherwise. @@ -198,4 +235,26 @@ bool le_audio_freq_check(const struct bt_audio_codec_cfg *codec); */ void le_audio_print_codec(const struct bt_audio_codec_cfg *codec, enum bt_audio_dir dir); +/** + * @brief Print the QoS configuration for a stream. + * + * @param[in] stream Pointer to the BAP stream to print QoS information from. + * + * @return 0 for success, error otherwise. + */ +int le_audio_print_qos_from_stream(struct bt_bap_stream const *const stream); + +/** + * @brief Print the unicast group information. + * + * @param[in] unicast_group Pointer to the CAP unicast group to print information from. + * + * @return 0 for success, error otherwise. + */ +int le_audio_print_unicast_group(struct bt_cap_unicast_group const *const unicast_group); + +/** + * @} + */ + #endif /* _LE_AUDIO_H_ */ diff --git a/src/bluetooth/bt_stream/unicast/Kconfig b/src/bluetooth/bt_stream/unicast/Kconfig index 4500db6b..4cd70630 100644 --- a/src/bluetooth/bt_stream/unicast/Kconfig +++ b/src/bluetooth/bt_stream/unicast/Kconfig @@ -81,16 +81,15 @@ config BT_AUDIO_PRES_DELAY_SRCH_SOURCE then it will revert to the closest supported value. endchoice -config BT_AUDIO_EP_PRINT - bool "Print discovered endpoint capabilities" - default n +config BT_AUDIO_PAC_REC_PRINT + bool "Print discovered Published Audio Capabilities" help - Print the supported capabilities of an endpoint when it is discovered. + Print the supported capabilities of a unicast server when it is discovered. config CODEC_CAP_COUNT_MAX int "Max storage of codec capabilities" - default 5 + default 10 help Max number of codec capabilities to store per stream. @@ -157,5 +156,9 @@ module = UNICAST_CLIENT module-str = unicast_client source "subsys/logging/Kconfig.template.log_config" +module = SERVER_STORE +module-str = server_store +source "subsys/logging/Kconfig.template.log_config" + endmenu # Log levels endmenu # Unicast diff --git a/src/bluetooth/bt_stream/unicast/Kconfig.defaults b/src/bluetooth/bt_stream/unicast/Kconfig.defaults index fd607265..70dbd52f 100644 --- a/src/bluetooth/bt_stream/unicast/Kconfig.defaults +++ b/src/bluetooth/bt_stream/unicast/Kconfig.defaults @@ -32,11 +32,10 @@ config BT_L2CAP_TX_BUF_COUNT default 12 config BT_BUF_ACL_RX_SIZE - default 502 if (AUDIO_DFU > 0) default 259 -config BT_BUF_ACL_TX_COUNT - default 12 +config BT_BUF_EVT_RX_COUNT + default 14 config SETTINGS default y diff --git a/src/bluetooth/bt_stream/unicast/server_store.c b/src/bluetooth/bt_stream/unicast/server_store.c new file mode 100644 index 00000000..cdf6b306 --- /dev/null +++ b/src/bluetooth/bt_stream/unicast/server_store.c @@ -0,0 +1,1588 @@ +/* + * Copyright (c) 2025 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause + */ + +#include +#include +#include +#include +#include +#include +#include +#include <../subsys/bluetooth/audio/bap_endpoint.h> + +#include "server_store.h" +#include "macros_common.h" +#include "le_audio.h" + +#include +LOG_MODULE_REGISTER(server_store, CONFIG_SERVER_STORE_LOG_LEVEL); + +static struct bt_bap_lc3_preset lc3_preset_48_4_1 = BT_BAP_LC3_UNICAST_PRESET_48_4_1( + BT_AUDIO_LOCATION_ANY, (BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED)); +static struct bt_bap_lc3_preset lc3_preset_24_2_1 = BT_BAP_LC3_UNICAST_PRESET_24_2_1( + BT_AUDIO_LOCATION_ANY, (BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED)); +static struct bt_bap_lc3_preset lc3_preset_16_2_1 = BT_BAP_LC3_UNICAST_PRESET_16_2_1( + BT_AUDIO_LOCATION_ANY, (BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED)); + +K_SEM_DEFINE(sem, 1, 1); +static atomic_ptr_t lock_owner; + +#if CONFIG_DEBUG +static char owner_file[50]; +static int owner_line; +#endif + +static void valid_entry_check(char const *const str) +{ + __ASSERT(k_sem_count_get(&sem) == 0, "%s: Semaphore not taken", str); + __ASSERT(atomic_ptr_get(&lock_owner) == k_current_get(), "%s: Thread mismatch", str); +} + +#define MAX_SERVERS (CONFIG_BT_MAX_CONN + CONFIG_BT_MAX_PAIRED) +#define NO_ADDR BT_ADDR_LE_ANY + +static struct server_store servers[MAX_SERVERS]; + +/* NOTE: This is set to static as one instance of server_store is very large, we do not + * want to pull this from whatever stack has called this function. Used when adding new servers + */ +static struct server_store add_server; + +struct pd { + uint32_t min; + uint32_t pref_min; + uint32_t pref_max; + uint32_t max; +}; + +/* Add a new server */ +static int server_add(struct server_store *server) +{ + if (server == NULL) { + return -EINVAL; + } + + for (int i = 0; i < MAX_SERVERS; i++) { + char peer_str[BT_ADDR_LE_STR_LEN]; + + bt_addr_le_to_str(&server->addr, peer_str, BT_ADDR_LE_STR_LEN); + + if (bt_addr_le_eq(&servers[i].addr, NO_ADDR)) { + memcpy(&servers[i], server, sizeof(struct server_store)); + LOG_DBG("Added server %s to index %d", peer_str, i); + return 0; + } + } + + return -ENOMEM; +} + +/* Fully remove a server */ +static int server_remove(struct server_store *server, bool force) +{ + if (server == NULL) { + LOG_ERR("Server is NULL"); + return -ENODEV; + } + + if (server->conn != NULL && !force) { + LOG_ERR("Cannot remove server with active connection"); + return -EACCES; + } + + memset(server, 0, sizeof(struct server_store)); + return 0; +} + +/* Find the smallest commonly acceptable set of presentation delays */ +static int pres_delay_compute(struct bt_bap_qos_cfg_pref *common, + struct bt_bap_qos_cfg_pref const *const in) +{ + if (in->pd_min) { + common->pd_min = MAX(in->pd_min, common->pd_min); + } else { + LOG_ERR("pd_min is zero."); + return -EINVAL; + } + + if (in->pref_pd_min) { + common->pref_pd_min = MAX(in->pref_pd_min, common->pref_pd_min); + } + + if (in->pref_pd_max) { + common->pref_pd_max = MIN(in->pref_pd_max, common->pref_pd_max); + } + + if (in->pd_max) { + common->pd_max = MIN(in->pd_max, common->pd_max); + } else { + LOG_ERR("No max presentation delay required"); + return -EINVAL; + } + + return 0; +} + +/* Populate a preset based on preferred sample rate */ +static int sample_rate_check(uint16_t lc3_freq_bit, struct bt_bap_lc3_preset *preset, + uint8_t pref_sample_rate) +{ + /* Try with preferred first */ + switch (pref_sample_rate) { + case BT_AUDIO_CODEC_CFG_FREQ_48KHZ: + if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_48KHZ) { + memcpy(preset, &lc3_preset_48_4_1, sizeof(struct bt_bap_lc3_preset)); + return 0; + } + + break; + + case BT_AUDIO_CODEC_CFG_FREQ_24KHZ: + if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_24KHZ) { + memcpy(preset, &lc3_preset_24_2_1, sizeof(struct bt_bap_lc3_preset)); + return 0; + } + + break; + + case BT_AUDIO_CODEC_CFG_FREQ_16KHZ: + if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_16KHZ) { + memcpy(preset, &lc3_preset_16_2_1, sizeof(struct bt_bap_lc3_preset)); + return 0; + } + + break; + + default: + break; + } + + /* If no match with the preferred, find highest sample rate */ + if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_48KHZ) { + memcpy(preset, &lc3_preset_48_4_1, sizeof(struct bt_bap_lc3_preset)); + return 0; + } else if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_24KHZ) { + memcpy(preset, &lc3_preset_24_2_1, sizeof(struct bt_bap_lc3_preset)); + return 0; + } else if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_16KHZ) { + memcpy(preset, &lc3_preset_16_2_1, sizeof(struct bt_bap_lc3_preset)); + return 0; + } + + LOG_DBG("No supported sample rate found"); + return -ENOTSUP; +} + +/* Generic parser function for sink and source */ +static bool pac_parse(struct bt_data *data, struct bt_bap_lc3_preset *preset, + uint8_t pref_sample_rate) +{ + int ret; + + switch (data->type) { + case BT_AUDIO_CODEC_CAP_TYPE_FREQ: + uint16_t lc3_freq_bit = sys_get_le16(data->data); + + ret = sample_rate_check(lc3_freq_bit, preset, pref_sample_rate); + if (ret) { + /* This PAC record from the server is not supported by the client. + * Stop parsing this record. + */ + return false; + } + break; + + case BT_AUDIO_CODEC_CAP_TYPE_FRAME_LEN: + /* Make sure the presets octets per frame is within the range of the codec + * capabilities. + */ + uint16_t lc3_min_frame_length = sys_get_le16(data->data); + uint16_t lc3_max_frame_length = sys_get_le16(data->data + sizeof(uint16_t)); + int frame_dur_us = 0; + + ret = le_audio_duration_us_get(&preset->codec_cfg, &frame_dur_us); + if (ret) { + LOG_WRN("Failed to get frame duration: %d", ret); + memset(preset, 0, sizeof(struct bt_bap_lc3_preset)); + return false; + } + + if (lc3_min_frame_length > + LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX, frame_dur_us) || + lc3_max_frame_length < + LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MIN, frame_dur_us)) { + LOG_WRN("LC3 frame length %d - %d not in range [%d, %d], not possible to " + "find a preset that meets these capabilities", + lc3_min_frame_length, lc3_max_frame_length, + LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MIN, frame_dur_us), + LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX, frame_dur_us)); + memset(preset, 0, sizeof(struct bt_bap_lc3_preset)); + return false; + } + + lc3_min_frame_length = + MAX(lc3_min_frame_length, + LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MIN, frame_dur_us)); + lc3_max_frame_length = + MIN(lc3_max_frame_length, + LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX, frame_dur_us)); + + int preset_octets_per_frame = 0; + + ret = le_audio_octets_per_frame_get(&preset->codec_cfg, &preset_octets_per_frame); + if (ret) { + LOG_WRN("Failed to get preset octets per frame: %d", ret); + memset(preset, 0, sizeof(struct bt_bap_lc3_preset)); + return false; + } + + if (!IN_RANGE(preset_octets_per_frame, lc3_min_frame_length, + lc3_max_frame_length)) { + LOG_DBG("Preset octets/frame %d not in range [%d, %d]", + preset_octets_per_frame, lc3_min_frame_length, + lc3_max_frame_length); + ret = bt_audio_codec_cfg_set_octets_per_frame(&preset->codec_cfg, + lc3_max_frame_length); + if (ret < 0) { + LOG_ERR("Failed to set preset octets per frame: %d", ret); + memset(preset, 0, sizeof(struct bt_bap_lc3_preset)); + return false; + } + } + break; + + default: + break; + } + + /* Did not find what we were looking for, continue parsing LTV */ + return true; +} + +/* Parse published audio capabilities (PAC) for sink direction (client->server)/(gateway->headset) + */ +static bool sink_pac_parse(struct bt_data *data, void *user_data) +{ + struct bt_bap_lc3_preset *preset = (struct bt_bap_lc3_preset *)user_data; + + return pac_parse(data, preset, CONFIG_BT_AUDIO_PREF_SINK_SAMPLE_RATE_VALUE); +} + +/* Parse published audio capabilities (PAC) for source direction (server->client)/(headset->gateway) + */ +static bool source_pac_parse(struct bt_data *data, void *user_data) +{ + struct bt_bap_lc3_preset *preset = (struct bt_bap_lc3_preset *)user_data; + + return pac_parse(data, preset, CONFIG_BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_VALUE); +} + +static void set_color_if_supported(char *str, uint16_t bitfield, uint16_t mask) +{ + if (bitfield & mask) { + strcat(str, COLOR_GREEN); + } else { + strcat(str, COLOR_RED); + strcat(str, "!"); + } +} + +/* Print all published audio capabilities records */ +static bool pac_record_print(struct bt_data *data, void *user_data) +{ + if (data->type == BT_AUDIO_CODEC_CAP_TYPE_FREQ) { + uint16_t freq_bit = sys_get_le16(data->data); + char supported_freq[320] = ""; + + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_8KHZ); + strcat(supported_freq, "8, "); + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_11KHZ); + strcat(supported_freq, "11.025, "); + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_16KHZ); + strcat(supported_freq, "16, "); + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_22KHZ); + strcat(supported_freq, "22.05, "); + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_24KHZ); + strcat(supported_freq, "24, "); + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_32KHZ); + strcat(supported_freq, "32, "); + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_44KHZ); + strcat(supported_freq, "44.1, "); + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_48KHZ); + strcat(supported_freq, "48, "); + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_88KHZ); + strcat(supported_freq, "88.2, "); + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_96KHZ); + strcat(supported_freq, "96, "); + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_176KHZ); + strcat(supported_freq, "176, "); + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_192KHZ); + strcat(supported_freq, "192, "); + set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_384KHZ); + strcat(supported_freq, "384"); + + LOG_INF("\tFreq kHz: %s", supported_freq); + } + + if (data->type == BT_AUDIO_CODEC_CAP_TYPE_DURATION) { + uint16_t dur_bit = sys_get_le16(data->data); + char supported_dur[80] = ""; + + set_color_if_supported(supported_dur, dur_bit, BT_AUDIO_CODEC_CAP_DURATION_7_5); + strcat(supported_dur, "7.5, "); + set_color_if_supported(supported_dur, dur_bit, BT_AUDIO_CODEC_CAP_DURATION_10); + strcat(supported_dur, "10"); + + LOG_INF("\tFrame duration ms: %s", supported_dur); + } + + if (data->type == BT_AUDIO_CODEC_CAP_TYPE_CHAN_COUNT) { + uint16_t chan_bit = sys_get_le16(data->data); + char supported_chan[140] = ""; + + set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_1); + strcat(supported_chan, "1, "); + set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_2); + strcat(supported_chan, "2, "); + set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_3); + strcat(supported_chan, "3, "); + set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_4); + strcat(supported_chan, "4, "); + set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_5); + strcat(supported_chan, "5, "); + set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_6); + strcat(supported_chan, "6, "); + set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_7); + strcat(supported_chan, "7, "); + set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_8); + strcat(supported_chan, "8"); + + LOG_INF("\tNum channels supported: %s", supported_chan); + } + + if (data->type == BT_AUDIO_CODEC_CAP_TYPE_FRAME_LEN) { + uint16_t lc3_min_frame_length = sys_get_le16(data->data); + uint16_t lc3_max_frame_length = sys_get_le16(data->data + sizeof(uint16_t)); + + LOG_INF("\tFrame length bytes: %d - %d", lc3_min_frame_length, + lc3_max_frame_length); + } + + if (data->type == BT_AUDIO_CODEC_CAP_TYPE_FRAME_COUNT) { + uint16_t lc3_frame_per_sdu = sys_get_le16(data->data); + + LOG_INF("\tMax frames per SDU: %d", lc3_frame_per_sdu); + } + + return true; +} + +static void stream_print(struct bt_bap_qos_cfg_pref const *const qos, bool evaluated, + const char *usr_string) +{ + char buf[10] = {0}; + + if (evaluated) { + strcpy(buf, "(Eval)"); + } else { + strcpy(buf, "(N/A) "); + } + + LOG_DBG("%s%s\t abs min: %05u pref min: %05u pref max: %05u abs max: " + "%05u ", + usr_string, buf, qos->pd_min, qos->pref_pd_min, qos->pref_pd_max, qos->pd_max); +} + +static void done_print(uint8_t existing_streams_checked, + struct bt_bap_qos_cfg_pref const *const common_qos, + uint32_t computed_pres_dly_us, uint32_t existing_pres_dly_us) +{ + char buf[20] = {0}; + + if (computed_pres_dly_us != BT_BAP_PD_UNSET) { + snprintf(buf, sizeof(buf), "%u", computed_pres_dly_us); + } else { + strcpy(buf, "No common value"); + } + + LOG_DBG("Outcome \t\t abs min: %05u pref min: %05u pref max: %05u abs max: %05u\r\n" + "\t selected: %s us, existing: %u us, 1 incoming + %d existing stream(s) " + "compared.", + common_qos->pd_min, common_qos->pref_pd_min, common_qos->pref_pd_max, + common_qos->pd_max, buf, existing_pres_dly_us, existing_streams_checked); +} + +/* Check to see if the existing stream can be ignored when we try to find a valid presentation delay + * when a new stream is added. + */ +static bool pres_dly_stream_ignore(struct bt_bap_stream const *const existing_stream, + struct bt_bap_stream const *const stream_in) +{ + if (existing_stream == NULL || stream_in == NULL) { + LOG_ERR("NULL parameter"); + return true; + } + + if (existing_stream->group == NULL) { + return true; + } + + if (existing_stream == stream_in) { + /* The existing stream is the same as the incoming stream */ + return true; + } + + if (existing_stream->ep == NULL) { + return true; + } + + int existing_dir = le_audio_stream_dir_get(existing_stream); + + if (existing_dir <= 0) { + LOG_ERR("Failed to get dir of existing stream %p", existing_stream); + return true; + } + + int incoming_dir = le_audio_stream_dir_get(stream_in); + + if (incoming_dir <= 0) { + LOG_ERR("Failed to get dir of incoming stream %p", stream_in); + return true; + } + + if (existing_dir != incoming_dir) { + /* The existing stream is not in the same direction as the incoming stream */ + LOG_DBG("Existing stream dir: (%d) not in same dir as incoming stream (%d)", + existing_dir, incoming_dir); + return true; + } + + /* Check if the existing stream has gotten into a QoS configured, + * enabling or streaming state. + */ + if (!le_audio_ep_state_check(existing_stream->ep, BT_BAP_EP_STATE_QOS_CONFIGURED) && + !le_audio_ep_state_check(existing_stream->ep, BT_BAP_EP_STATE_ENABLING) && + !le_audio_ep_state_check(existing_stream->ep, BT_BAP_EP_STATE_STREAMING)) { + LOG_DBG("Existing stream not in QoS configured, enabling or " + "streaming state"); + return true; + } + + stream_print(&existing_stream->ep->qos_pref, true, "Existing"); + return false; +} + +int srv_store_foreach_server(srv_store_foreach_func_t func, void *user_data) +{ + valid_entry_check(__func__); + + if (func == NULL) { + return -EINVAL; + } + + /* Call the given function for each server with a valid conn pointer */ + for (int i = 0; i < MAX_SERVERS; i++) { + struct server_store *server = &servers[i]; + + if (server->conn == NULL) { + continue; + } + + const bool keep_iterating = func(server, user_data); + + if (!keep_iterating) { + return -ECANCELED; + } + } + + return 0; +} + +bool srv_store_preset_validated(struct bt_audio_codec_cfg const *const new, + struct bt_audio_codec_cfg const *const existing, + uint8_t pref_sample_rate_value) +{ + valid_entry_check(__func__); + + int ret; + + if (new == NULL || existing == NULL) { + return false; + } + + int new_freq_hz = 0; + int existing_freq_hz = 0; + int pref_freq_hz = 0; + + ret = le_audio_freq_hz_get(new, &new_freq_hz); + if (ret) { + LOG_ERR("Failed to get new freq hz: %d", ret); + return false; + } + + const struct bt_audio_codec_cfg zero_dummy = {0}; + + if (memcmp(existing, &zero_dummy, sizeof(struct bt_audio_codec_cfg)) == 0) { + /* No existing preset, will use new one */ + return true; + } + + ret = le_audio_freq_hz_get(existing, &existing_freq_hz); + if (ret) { + LOG_ERR("Failed to get existing freq hz: %d", ret); + return false; + } + + switch (pref_sample_rate_value) { + case BT_AUDIO_CODEC_CFG_FREQ_48KHZ: + pref_freq_hz = 48000; + break; + case BT_AUDIO_CODEC_CFG_FREQ_24KHZ: + pref_freq_hz = 24000; + break; + case BT_AUDIO_CODEC_CFG_FREQ_16KHZ: + pref_freq_hz = 16000; + break; + default: + pref_freq_hz = -1; + break; + } + + if ((new_freq_hz >= existing_freq_hz && existing_freq_hz != pref_freq_hz) || + new_freq_hz == pref_freq_hz) { + LOG_DBG("New preset has higher frequency, or pref freq met: %d > %d", new_freq_hz, + existing_freq_hz); + + if (new_freq_hz == pref_freq_hz && existing_freq_hz != pref_freq_hz) { + LOG_DBG("New preset has preferred frequency: %d == %d", new_freq_hz, + pref_freq_hz); + return true; + } + + int new_octets = 0; + int existing_octets = 0; + + ret = le_audio_octets_per_frame_get(new, &new_octets); + if (ret) { + LOG_ERR("Failed to get new octets/frame: %d", ret); + return false; + } + + ret = le_audio_octets_per_frame_get(existing, &existing_octets); + if (ret) { + LOG_ERR("Failed to get existing octets/frame: %d", ret); + return false; + } + + if (new_octets >= existing_octets) { + LOG_DBG("New preset has higher or equal octets/frame: %d >= %d", new_octets, + existing_octets); + return true; + } + } + + return false; +} + +struct foreach_stream_data { + int ret; + struct bt_bap_ep_info *incoming_ep_info; + struct bt_bap_qos_cfg_pref *common_qos; + struct bt_bap_qos_cfg_pref const *incoming_qos; + struct bt_bap_stream *incoming_stream; + uint8_t *streams_checked; + uint32_t *existing_pres_dly_us; + uint32_t *computed_pres_dly_us; + uint32_t *existing_pres_dly_us_check; + bool *group_reconfig_needed; + bool existing_pres_dly_already_in_range; +}; + +static bool stream_check_pd(struct bt_cap_stream *existing_stream, void *user_data) +{ + struct foreach_stream_data *ctx = (struct foreach_stream_data *)user_data; + + if (existing_stream->bap_stream.group != ctx->incoming_stream->group) { + /* The existing stream is not in the same group as the incoming stream */ + LOG_ERR("Existing stream not in same group as incoming stream"); + ctx->ret = -EINVAL; + return false; + } + + if (pres_dly_stream_ignore(&(existing_stream->bap_stream), ctx->incoming_stream)) { + /* Do not consider this stream. Continue parsing */ + LOG_DBG("Ignoring existing stream"); + return true; + } + + /* All already running streams in the same direction and in the + * same group shall have the same presentation delay. + */ + + if (*ctx->existing_pres_dly_us_check == BT_BAP_PD_UNSET) { + *ctx->existing_pres_dly_us_check = *ctx->existing_pres_dly_us; + } else if (*ctx->existing_pres_dly_us_check != *ctx->existing_pres_dly_us) { + LOG_ERR("Illegal value. Pres delays do not match: %u != %u", + *ctx->existing_pres_dly_us_check, *ctx->existing_pres_dly_us); + ctx->ret = -EINVAL; + return false; + } + + if (*ctx->existing_pres_dly_us == 0) { + LOG_ERR("Existing presentation delay is zero"); + ctx->ret = -EINVAL; + return false; + } + + if (IN_RANGE(*ctx->existing_pres_dly_us, ctx->incoming_qos->pd_min, + ctx->incoming_qos->pd_max)) { + *ctx->computed_pres_dly_us = *ctx->existing_pres_dly_us; + LOG_INF("Existing pres delay is within the incoming stream QoS range"); + ctx->existing_pres_dly_already_in_range = true; + return false; + } + + *ctx->group_reconfig_needed = true; + (*ctx->streams_checked)++; + + ctx->ret = pres_delay_compute(ctx->common_qos, &existing_stream->bap_stream.ep->qos_pref); + if (ctx->ret) { + LOG_ERR("Failed to compute common presentation delay: %d", ctx->ret); + return false; + } + + /* Continue iteration */ + return true; +} + +/* Find the presentation delay. Needs to be the same within a CIG for a given direction */ +int srv_store_pres_dly_find(struct bt_bap_stream *stream, uint32_t *computed_pres_dly_us, + uint32_t *existing_pres_dly_us, + struct bt_bap_qos_cfg_pref const *server_qos_pref, + bool *group_reconfig_needed, struct bt_cap_unicast_group *unicast_group) +{ + valid_entry_check(__func__); + + int ret; + + if (stream == NULL || computed_pres_dly_us == NULL || existing_pres_dly_us == NULL || + server_qos_pref == NULL || group_reconfig_needed == NULL) { + LOG_ERR("NULL parameter"); + + return -EINVAL; + } + + *existing_pres_dly_us = BT_BAP_PD_UNSET; + *group_reconfig_needed = false; + *computed_pres_dly_us = BT_BAP_PD_UNSET; + uint32_t existing_pres_dly_us_check = BT_BAP_PD_UNSET; + uint8_t existing_streams_checked = 0; + + if (stream->group == NULL) { + LOG_ERR("The incoming stream %p has no group", stream); + return -EINVAL; + } + + if (server_qos_pref->pd_min == 0 || server_qos_pref->pd_max == 0) { + LOG_ERR("Incoming pd_min or pd_max is zero"); + return -EINVAL; + } + + stream_print(server_qos_pref, true, "Incoming"); + + /* Common QoS with most permissive values */ + struct bt_bap_qos_cfg_pref common_qos = { + .pd_min = 0, + .pref_pd_min = 0, + .pref_pd_max = UINT32_MAX, + .pd_max = UINT32_MAX, + }; + + ret = pres_delay_compute(&common_qos, server_qos_pref); + if (ret) { + LOG_ERR("Failed to find initial common presentation delay: %d", ret); + return ret; + } + + struct bt_bap_ep_info ep_info; + + ret = bt_bap_ep_get_info(stream->ep, &ep_info); + if (ret) { + LOG_ERR("Failed to get ep info: %d", ret); + return ret; + } + + struct bt_cap_unicast_group_info cap_info; + struct bt_bap_unicast_group_info bap_info; + + ret = bt_cap_unicast_group_get_info(unicast_group, &cap_info); + if (ret != 0) { + LOG_ERR("Failed to get CAP unicast group info: %d", ret); + return ret; + } + + ret = bt_bap_unicast_group_get_info(cap_info.unicast_group, &bap_info); + + if (ret != 0) { + LOG_ERR("Failed to get BAP unicast group info: %d", ret); + return ret; + } + + int dir = le_audio_stream_dir_get(stream); + + if (dir <= 0) { + LOG_ERR("Failed to get dir of incoming stream %p", stream); + return -EINVAL; + } + + if (dir == BT_AUDIO_DIR_SINK) { + LOG_DBG("Incoming stream is a sink. Group sink PD: %u", bap_info.sink_pd); + *existing_pres_dly_us = bap_info.sink_pd; + } else if (dir == BT_AUDIO_DIR_SOURCE) { + LOG_DBG("Incoming stream is a source. Group source PD: %u", bap_info.source_pd); + *existing_pres_dly_us = bap_info.source_pd; + } else { + LOG_ERR("Incoming stream has unknown direction: %d", dir); + return -EINVAL; + } + + struct foreach_stream_data foreach_data = { + .ret = 0, + .incoming_ep_info = &ep_info, + .incoming_qos = server_qos_pref, + .common_qos = &common_qos, + .incoming_stream = stream, + .streams_checked = &existing_streams_checked, + .group_reconfig_needed = group_reconfig_needed, + .existing_pres_dly_us = existing_pres_dly_us, + .existing_pres_dly_us_check = &existing_pres_dly_us_check, + .computed_pres_dly_us = computed_pres_dly_us, + .existing_pres_dly_already_in_range = false, + }; + + + ret = bt_cap_unicast_group_foreach_stream(unicast_group, stream_check_pd, + (void *)&foreach_data); + if (foreach_data.ret) { + LOG_ERR("Failed to compute presentation delay"); + *computed_pres_dly_us = BT_BAP_PD_UNSET; + return foreach_data.ret; + } + + if (ret != 0 && ret != -ECANCELED) { + LOG_ERR("Failed to iterate streams in group: %d", ret); + *computed_pres_dly_us = BT_BAP_PD_UNSET; + return ret; + } + + LOG_DBG("Checked %d stream(s) in the same group", existing_streams_checked); + + if (foreach_data.existing_pres_dly_already_in_range) { + LOG_DBG("Existing pres delay already in range, no reconfig needed"); + + ret = 0; + goto print_and_return; + } + + if (existing_streams_checked == 0) { + LOG_DBG("No other streams in the same group found"); + + /* No other streams in the same group found */ + if (server_qos_pref->pref_pd_min == 0) { + *computed_pres_dly_us = common_qos.pd_min; + } else { + *computed_pres_dly_us = common_qos.pref_pd_min; + } + + ret = 0; + goto print_and_return; + } + + if (!group_reconfig_needed) { + ret = 0; + goto print_and_return; + } + + if (common_qos.pd_min > common_qos.pd_max) { + LOG_ERR("No common ground for pd_min %u and pd_max %u", common_qos.pd_min, + common_qos.pd_max); + *computed_pres_dly_us = BT_BAP_PD_UNSET; + + ret = -ESPIPE; + goto print_and_return; + } + + /* No streams have a preferred min */ + if (common_qos.pref_pd_min == 0) { + *computed_pres_dly_us = common_qos.pd_min; + + ret = 0; + goto print_and_return; + } + + if (common_qos.pref_pd_min < common_qos.pd_min) { + /* Preferred min is lower than min, use min */ + LOG_ERR("pref PD min is lower than min. Using min"); + *computed_pres_dly_us = common_qos.pd_min; + } else if (common_qos.pref_pd_min > common_qos.pd_min && + common_qos.pref_pd_min <= common_qos.pd_max) { + /* Preferred min is in range, use pref min */ + *computed_pres_dly_us = common_qos.pref_pd_min; + } else { + *computed_pres_dly_us = common_qos.pd_min; + } + +print_and_return: + done_print(existing_streams_checked, &common_qos, *computed_pres_dly_us, + *existing_pres_dly_us); + return ret; +} + +int srv_store_location_set(struct bt_conn const *const conn, enum bt_audio_dir dir, + enum bt_audio_location loc) +{ + valid_entry_check(__func__); + + int ret; + + if (conn == NULL) { + LOG_ERR("No valid connection pointer received"); + return -EINVAL; + } + + if (dir != BT_AUDIO_DIR_SINK && dir != BT_AUDIO_DIR_SOURCE) { + LOG_ERR("Invalid direction: %d", dir); + return -EINVAL; + } + + struct server_store *server = NULL; + + ret = srv_store_from_conn_get(conn, &server); + if (ret) { + return ret; + } + + switch (dir) { + case BT_AUDIO_DIR_SINK: + server->snk.locations = loc; + break; + + case BT_AUDIO_DIR_SOURCE: + server->src.locations = loc; + break; + + default: + LOG_ERR("Unknown direction: %d", dir); + return -EINVAL; + } + + return 0; +} + +int srv_store_valid_codec_cap_check(struct bt_conn const *const conn, enum bt_audio_dir dir, + uint32_t *valid_codec_caps, + struct client_supp_configs const **const client_supp_cfgs, + uint8_t num_client_supp_cfgs) +{ + valid_entry_check(__func__); + /* Ref: OCT-3480. These parameters can be checked to determine a full match of + * client versus server(s) capabilities. + */ + ARG_UNUSED(client_supp_cfgs); + ARG_UNUSED(num_client_supp_cfgs); + + int ret; + struct bt_bap_lc3_preset zero_preset = {0}; + + if (conn == NULL || valid_codec_caps == NULL) { + LOG_ERR("Invalid parameters: conn or valid_codec_caps is NULL"); + return -EINVAL; + } + + *valid_codec_caps = 0; + struct server_store *server = NULL; + + ret = srv_store_from_conn_get(conn, &server); + if (ret) { + LOG_ERR("Unknown connection, should not reach here"); + return ret; + } + + /* Only the sampling frequency is checked */ + if (dir == BT_AUDIO_DIR_SINK) { + LOG_DBG("Discovered %d sink endpoint(s) for server", server->snk.num_eps); + + for (int i = 0; i < server->snk.num_codec_caps; i++) { + struct bt_bap_lc3_preset preset = {0}; + + if (IS_ENABLED(CONFIG_BT_AUDIO_PAC_REC_PRINT)) { + LOG_INF("Sink PAC %d:", i); + ret = bt_audio_data_parse(server->snk.codec_caps[i].data, + server->snk.codec_caps[i].data_len, + pac_record_print, NULL); + if (ret) { + LOG_ERR("Failed data parse %d:", ret); + } + LOG_INF("__________________________"); + } + + ret = bt_audio_data_parse(server->snk.codec_caps[i].data, + server->snk.codec_caps[i].data_len, + sink_pac_parse, &preset); + if (ret && ret != -ECANCELED) { + LOG_ERR("PAC record sink parse failed %d:", ret); + continue; + } + + if (memcmp(&preset, &zero_preset, sizeof(struct bt_bap_lc3_preset)) == 0) { + continue; + } + + LOG_DBG("Valid codec capabilities found for server, sink EP %d", i); + *valid_codec_caps |= 1 << i; + + if (srv_store_preset_validated( + &(preset.codec_cfg), &(server->snk.lc3_preset[0].codec_cfg), + CONFIG_BT_AUDIO_PREF_SINK_SAMPLE_RATE_VALUE)) { + memcpy(&server->snk.lc3_preset[0], &preset, + sizeof(struct bt_bap_lc3_preset)); + } + } + } else if (dir == BT_AUDIO_DIR_SOURCE) { + LOG_DBG("Discovered %d source endpoint(s) for server", server->src.num_eps); + + for (int i = 0; i < server->src.num_codec_caps; i++) { + struct bt_bap_lc3_preset preset = {0}; + + if (IS_ENABLED(CONFIG_BT_AUDIO_PAC_REC_PRINT)) { + LOG_INF("Source PAC %d:", i); + ret = bt_audio_data_parse(server->src.codec_caps[i].data, + server->src.codec_caps[i].data_len, + pac_record_print, NULL); + if (ret) { + LOG_ERR("Failed data parse %d:", ret); + } + LOG_INF("__________________________"); + } + + ret = bt_audio_data_parse(server->src.codec_caps[i].data, + server->src.codec_caps[i].data_len, + source_pac_parse, &preset); + + if (ret && ret != -ECANCELED) { + LOG_ERR("PAC record source parse failed %d:", ret); + continue; + } + + if (memcmp(&preset, &zero_preset, sizeof(struct bt_bap_lc3_preset)) == 0) { + continue; + } + + LOG_DBG("Valid codec capabilities found for server, source EP %d", i); + *valid_codec_caps |= 1 << i; + + if (srv_store_preset_validated( + &(preset.codec_cfg), &(server->src.lc3_preset[0].codec_cfg), + CONFIG_BT_AUDIO_PREF_SOURCE_SAMPLE_RATE_VALUE)) { + memcpy(&server->src.lc3_preset[0], &preset, + sizeof(struct bt_bap_lc3_preset)); + } + } + } else { + LOG_ERR("Unknown direction"); + } + + return 0; +} + +int srv_store_from_stream_get(struct bt_bap_stream const *const stream, + struct server_store **server) +{ + valid_entry_check(__func__); + + struct server_store *tmp_server = NULL; + uint32_t matches = 0; + + if (stream == NULL || server == NULL) { + LOG_ERR("Invalid parameters: stream or server is NULL"); + return -EINVAL; + } + + *server = NULL; + + for (int srv_idx = 0; srv_idx < MAX_SERVERS; srv_idx++) { + tmp_server = &servers[srv_idx]; + if (bt_addr_le_eq(&tmp_server->addr, NO_ADDR)) { + continue; + } + for (int i = 0; i < CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT; i++) { + + LOG_DBG("Checking server %d, sink stream %d %p %p", srv_idx, i, + &tmp_server->snk.cap_streams[i].bap_stream, stream); + + if (&tmp_server->snk.cap_streams[i].bap_stream == stream) { + *server = tmp_server; + LOG_DBG("Found server for sink stream %p at index %d", stream, + srv_idx); + matches++; + } + } + + for (int i = 0; i < CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT; i++) { + LOG_DBG("Checking server %d, source stream %d", srv_idx, i); + if (&tmp_server->src.cap_streams[i].bap_stream == stream) { + *server = tmp_server; + LOG_DBG("Found server for source stream %p at index %d", stream, + srv_idx); + matches++; + } + } + } + + if (matches == 0) { + LOG_ERR("No server found for the given stream"); + *server = NULL; + + return -ENOENT; + } else if (matches > 1) { + LOG_ERR("Multiple servers found for the same stream. Should not happen"); + *server = NULL; + + return -ESPIPE; + } + + return 0; +} + +static int srv_store_ep_state_count(struct bt_conn const *const conn, enum bt_bap_ep_state state, + enum bt_audio_dir dir) +{ + valid_entry_check(__func__); + + int ret; + int count = 0; + struct server_store *server = NULL; + + if (conn == NULL) { + LOG_ERR("NULL parameter"); + return -EINVAL; + } + + ret = srv_store_from_conn_get(conn, &server); + if (ret) { + return ret; + } + + switch (dir) { + case BT_AUDIO_DIR_SINK: + for (int i = 0; i < CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT; i++) { + if (server->snk.cap_streams[i].bap_stream.ep == NULL) { + break; + } + + struct bt_bap_ep_info ep_info; + + ret = bt_bap_ep_get_info(server->snk.cap_streams[i].bap_stream.ep, + &ep_info); + if (ret) { + return ret; + } + + if (ep_info.state == state) { + count++; + } + } + break; + + case BT_AUDIO_DIR_SOURCE: + for (int i = 0; i < CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT; i++) { + if (server->src.cap_streams[i].bap_stream.ep != NULL) { + struct bt_bap_ep_info ep_info; + + ret = bt_bap_ep_get_info(server->src.cap_streams[i].bap_stream.ep, + &ep_info); + if (ret) { + return ret; + } + + if (ep_info.state == state) { + count++; + } + } else { + break; + } + } + break; + + default: + LOG_ERR("Unknown direction: %d", dir); + return -EINVAL; + } + + return count; +} + +int srv_store_all_ep_state_count(enum bt_bap_ep_state state, enum bt_audio_dir dir) +{ + valid_entry_check(__func__); + + int count = 0; + int count_total = 0; + struct server_store *server = NULL; + + for (int srv_idx = 0; srv_idx < MAX_SERVERS; srv_idx++) { + server = &servers[srv_idx]; + if (bt_addr_le_eq(&server->addr, NO_ADDR)) { + /* Empty entry */ + continue; + } + + if (server->conn == NULL) { + continue; + } + + count = srv_store_ep_state_count(server->conn, state, dir); + if (count < 0) { + LOG_ERR("Failed to get ep state count for server " + "%d: %d", + srv_idx, count); + return count; + } + count_total += count; + } + + return count_total; +} + +int srv_store_avail_context_set(struct bt_conn *conn, enum bt_audio_context snk_ctx, + enum bt_audio_context src_ctx) +{ + valid_entry_check(__func__); + int ret; + + if (conn == NULL) { + LOG_ERR("No valid connection pointer received"); + return -EINVAL; + } + + struct server_store *server = NULL; + + ret = srv_store_from_conn_get(conn, &server); + if (ret) { + return ret; + } + + server->snk.available_ctx = snk_ctx; + server->src.available_ctx = src_ctx; + + return 0; +} + +int srv_store_codec_cap_set(struct bt_conn const *const conn, enum bt_audio_dir dir, + struct bt_audio_codec_cap const *const codec) +{ + valid_entry_check(__func__); + int ret; + + if (conn == NULL || codec == NULL) { + LOG_ERR("NULL parameter"); + return -EINVAL; + } + + if (codec->data_len > CONFIG_BT_AUDIO_CODEC_CAP_MAX_DATA_SIZE) { + LOG_ERR("Codec data length exceeds maximum size: %d", codec->data_len); + return -ENOMEM; + } + + struct server_store *server = NULL; + + ret = srv_store_from_conn_get(conn, &server); + if (ret) { + return ret; + } + + if (codec->data_len > CONFIG_BT_AUDIO_CODEC_CAP_MAX_DATA_SIZE) { + LOG_ERR("Codec data length exceeds maximum size: %d", codec->data_len); + return -EINVAL; + } + + if (dir == BT_AUDIO_DIR_SINK) { + /* num_codec_caps is an increasing index that starts at 0 */ + if (server->snk.num_codec_caps >= ARRAY_SIZE(server->snk.codec_caps)) { + LOG_WRN("No more space (%d) for sink codec capabilities, increase " + "CONFIG_CODEC_CAP_COUNT_MAX(%d)", + server->snk.num_codec_caps, ARRAY_SIZE(server->snk.codec_caps)); + return -ENOMEM; + } + + memcpy(&server->snk.codec_caps[server->snk.num_codec_caps], codec, + sizeof(struct bt_audio_codec_cap)); + server->snk.num_codec_caps++; + } else if (dir == BT_AUDIO_DIR_SOURCE) { + /* num_codec_caps is an increasing index that starts at 0 */ + if (server->src.num_codec_caps >= ARRAY_SIZE(server->src.codec_caps)) { + LOG_WRN("No more space for source codec capabilities, increase " + "CONFIG_CODEC_CAP_COUNT_MAX"); + return -ENOMEM; + } + + memcpy(&server->src.codec_caps[server->src.num_codec_caps], codec, + sizeof(struct bt_audio_codec_cap)); + server->src.num_codec_caps++; + } else { + LOG_ERR("PAC record direction not recognized: %d", dir); + return -EINVAL; + } + + return 0; +} + +int srv_store_from_addr_get(bt_addr_le_t const *const addr, struct server_store **server) +{ + valid_entry_check(__func__); + + if (addr == NULL || server == NULL) { + LOG_ERR("NULL parameter"); + return -EINVAL; + } + + for (int i = 0; i < MAX_SERVERS; i++) { + if (bt_addr_le_eq(&servers[i].addr, addr)) { + *server = &servers[i]; + return 0; + } + } + + *server = NULL; + return -ENOENT; +} + +bool srv_store_server_exists(bt_addr_le_t const *const addr) +{ + valid_entry_check(__func__); + int ret; + + if (addr == NULL) { + LOG_ERR("No valid address pointer received"); + return false; + } + + struct server_store *temp_server = NULL; + + ret = srv_store_from_addr_get(addr, &temp_server); + if (ret == 0) { + return true; + } else if (ret == -ENOENT) { + return false; + } + + LOG_ERR("Error checking if server exists: %d", ret); + return false; +} + +int srv_store_from_conn_get(struct bt_conn const *const conn, struct server_store **server) +{ + valid_entry_check(__func__); + int ret; + + if (conn == NULL || server == NULL) { + LOG_ERR("NULL parameter"); + return -EINVAL; + } + + ret = srv_store_from_addr_get(bt_conn_get_dst(conn), server); + if (ret) { + return ret; + } + + if ((*server)->conn != conn) { + LOG_DBG("Conn %p does not match stored %p", conn, (*server)->conn); + return -ENOTCONN; + } + + return 0; +} + +int srv_store_num_get(void) +{ + valid_entry_check(__func__); + int num_servers = 0; + + for (int i = 0; i < MAX_SERVERS; i++) { + if (!bt_addr_le_eq(&servers[i].addr, NO_ADDR)) { + num_servers++; + } + } + + return num_servers; +} + +int srv_store_add_by_conn(struct bt_conn *conn) +{ + valid_entry_check(__func__); + + int ret; + struct server_store *temp_server = NULL; + + if (conn == NULL) { + LOG_ERR("NULL parameter"); + return -EINVAL; + } + + const bt_addr_le_t *peer_addr = bt_conn_get_dst(conn); + char peer_str[BT_ADDR_LE_STR_LEN]; + + bt_addr_le_to_str(peer_addr, peer_str, BT_ADDR_LE_STR_LEN); + + LOG_DBG("Adding server by conn for peer: %s", peer_str); + + /* Check if server already exists */ + ret = srv_store_from_conn_get(conn, &temp_server); + if (ret == 0) { + /* Server already exists, no need to add again, but we update the conn pointer */ + temp_server->conn = conn; + LOG_DBG("Server already exists for conn: %p", conn); + return -EALREADY; + } + + /* Clear the temporary add_server before populating it */ + add_server.conn = NULL; + + ret = server_remove(&add_server, false); + if (ret) { + return ret; + } + + add_server.conn = conn; + bt_addr_le_copy(&add_server.addr, peer_addr); + + return server_add(&add_server); +} + +int srv_store_add_by_addr(const bt_addr_le_t *addr) +{ + valid_entry_check(__func__); + + int ret; + char peer_str[BT_ADDR_LE_STR_LEN]; + struct server_store *temp_server = NULL; + + if (addr == NULL) { + LOG_ERR("NULL parameter"); + return -EINVAL; + } + + ret = srv_store_from_addr_get(addr, &temp_server); + if (ret == 0) { + bt_addr_le_to_str(addr, peer_str, BT_ADDR_LE_STR_LEN); + LOG_DBG("Server already exists for addr: %s", peer_str); + return -EALREADY; + } + + /* Clear the temporary add_server before populating it */ + add_server.conn = NULL; + + ret = server_remove(&add_server, false); + if (ret) { + return ret; + } + + bt_addr_le_copy(&add_server.addr, addr); + + bt_addr_le_to_str(addr, peer_str, BT_ADDR_LE_STR_LEN); + LOG_DBG("Adding server for addr: %s", peer_str); + + return server_add(&add_server); +} + +int srv_store_conn_update(struct bt_conn *conn, bt_addr_le_t const *const addr) +{ + valid_entry_check(__func__); + int ret; + + struct server_store *server = NULL; + + if (conn == NULL || addr == NULL) { + LOG_ERR("NULL parameter"); + return -EINVAL; + } + + ret = srv_store_from_addr_get(addr, &server); + if (ret) { + return ret; + } + + const bt_addr_le_t *peer_addr = bt_conn_get_dst(conn); + + if (!bt_addr_le_eq(&server->addr, peer_addr)) { + LOG_ERR("Address does not match the connection's peer address"); + return -EPERM; + } + + if (server->conn != NULL) { + LOG_ERR("Server already has a different conn assigned"); + return -EACCES; + } else if (server->conn == conn) { + LOG_WRN("Server is already assigned to the same conn"); + return -EALREADY; + } + + server->conn = conn; + + return 0; +} + +int srv_store_clear_by_conn(struct bt_conn const *const conn) +{ + valid_entry_check(__func__); + + int ret; + + if (conn == NULL) { + LOG_ERR("NULL parameter"); + return -EINVAL; + } + + struct server_store *server = NULL; + + ret = srv_store_from_conn_get(conn, &server); + + if (ret == -ENOENT) { + return 0; + } else if (ret == -ENOTCONN) { + LOG_DBG("Server exists different conn pointer, clearing anyway"); + } else if (ret) { + return ret; + } + + /* Address is not cleared */ + server->name = "NOT_SET"; + server->conn = NULL; + server->member = NULL; + + server->snk.waiting_for_disc = false; + server->src.waiting_for_disc = false; + server->snk.locations = 0; + server->src.locations = 0; + memset(&server->snk.lc3_preset, 0, sizeof(server->snk.lc3_preset)); + memset(&server->src.lc3_preset, 0, sizeof(server->src.lc3_preset)); + memset(&server->snk.codec_caps, 0, sizeof(server->snk.codec_caps)); + memset(&server->src.codec_caps, 0, sizeof(server->src.codec_caps)); + server->snk.num_codec_caps = 0; + server->src.num_codec_caps = 0; + memset(&server->snk.eps, 0, sizeof(server->snk.eps)); + memset(&server->src.eps, 0, sizeof(server->src.eps)); + server->snk.num_eps = 0; + server->src.num_eps = 0; + server->snk.supported_ctx = 0; + server->src.supported_ctx = 0; + server->snk.available_ctx = 0; + server->src.available_ctx = 0; + /* Streams are not touched as these are handled by the stack. + * Clearing them here may lead to faults as they are needed beyond ACL disconnection + */ + + return 0; +} + +int srv_store_remove_by_addr(bt_addr_le_t const *const addr) +{ + valid_entry_check(__func__); + int ret; + + struct server_store *server = NULL; + + ret = srv_store_from_addr_get(addr, &server); + if (ret) { + return ret; + } + + return server_remove(server, false); +} + +int srv_store_remove_by_conn(struct bt_conn const *const conn) +{ + valid_entry_check(__func__); + int ret; + + if (conn == NULL) { + LOG_ERR("NULL parameter"); + return -EINVAL; + } + + struct server_store *server = NULL; + + ret = srv_store_from_conn_get(conn, &server); + if (ret == -ENOENT) { + LOG_WRN("Server does not exist"); + return ret; + } else if (ret) { + return ret; + } + + return server_remove(server, true); +} + +int srv_store_remove_all(bool force) +{ + valid_entry_check(__func__); + int ret; + + for (int i = 0; i < MAX_SERVERS; i++) { + ret = server_remove(&servers[i], force); + if (ret) { + LOG_ERR("Failed to remove server at index %d, error: %d", i, ret); + return ret; + } + } + return 0; +} + +int _srv_store_lock(k_timeout_t timeout, const char *file, int line) +{ + int ret; + + ret = k_sem_take(&sem, timeout); + if (ret) { + +#if CONFIG_DEBUG + LOG_ERR("Sem take error: %d. Owner: %s Line: %d", ret, owner_file, owner_line); +#else + LOG_ERR("Sem take error: %d", ret); +#endif + return ret; + } + + atomic_ptr_set(&lock_owner, k_current_get()); +#if CONFIG_DEBUG + if (strlen(file) > ARRAY_SIZE(owner_file) - 1) { + strncpy(owner_file, file, ARRAY_SIZE(owner_file) - 1); + } else { + strcpy(owner_file, file); + } + owner_line = line; +#endif + return 0; +} + +void srv_store_unlock(void) +{ + valid_entry_check(__func__); + + atomic_ptr_set(&lock_owner, NULL); +#if CONFIG_DEBUG + owner_line = -1; + owner_file[0] = '\0'; +#endif + k_sem_give(&sem); +} + +int srv_store_init(void) +{ + valid_entry_check(__func__); + + return srv_store_remove_all(false); +} diff --git a/src/bluetooth/bt_stream/unicast/server_store.h b/src/bluetooth/bt_stream/unicast/server_store.h new file mode 100644 index 00000000..224ecd57 --- /dev/null +++ b/src/bluetooth/bt_stream/unicast/server_store.h @@ -0,0 +1,448 @@ +/* + * Copyright (c) 2025 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause + */ + +/** @file + * @addtogroup audio_app_bt_stream + * @{ + * @defgroup server_store Storage on unicast client of remote unicast servers. + * @{ + * @brief Helper functions to manage how a unicast client (gateway side) stores the + * unicast server(s) (headset side) it is connected to. + */ + +#ifndef _SERVER_STORE_H_ +#define _SERVER_STORE_H_ + +#include +#include +#include +#include +#include +#include +#include + +struct unicast_server_snk_vars { + bool waiting_for_disc; + uint32_t locations; + struct bt_bap_lc3_preset lc3_preset[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT]; + struct bt_audio_codec_cap codec_caps[CONFIG_CODEC_CAP_COUNT_MAX]; + size_t num_codec_caps; + struct bt_bap_ep *eps[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT]; + size_t num_eps; + enum bt_audio_context supported_ctx; + enum bt_audio_context available_ctx; + struct bt_cap_stream cap_streams[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT]; +}; + +struct unicast_server_src_vars { + bool waiting_for_disc; + uint32_t locations; + struct bt_bap_lc3_preset lc3_preset[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT]; + struct bt_audio_codec_cap codec_caps[CONFIG_CODEC_CAP_COUNT_MAX]; + size_t num_codec_caps; + struct bt_bap_ep *eps[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT]; + size_t num_eps; + enum bt_audio_context supported_ctx; + enum bt_audio_context available_ctx; + struct bt_cap_stream cap_streams[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT]; +}; + +/* This struct holds the parameters for a given unicast server/headset device */ +struct server_store { + char *name; + bt_addr_le_t addr; + struct bt_conn *conn; + const struct bt_csip_set_coordinator_set_member *member; + struct unicast_server_snk_vars snk; + struct unicast_server_src_vars src; +}; + +struct client_supp_configs { + enum bt_audio_codec_cap_freq freq; + enum bt_audio_codec_cap_frame_dur dur; + enum bt_audio_codec_cap_chan_count chan_count; + struct bt_audio_codec_octets_per_codec_frame oct_per_codec_frame; +}; + +/** + * @brief Function for bt_bap_unicast_group_foreach_server(). + * + * @param[in] server The audio server. + * @param[in] user_data User data. + * + * @retval true Continue iterating. + * @retval false Stop iterating. + */ +typedef bool (*srv_store_foreach_func_t)(struct server_store *server, void *user_data); + +/** + * @brief Iterate through all stored servers and call the given function for each. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] func Function to call for each server. + * @param[in] user_data User data to pass to the function. + * + * @retval 0 Success. + * @retval -EINVAL Function pointer is NULL. + * @retval -ECANCELED The iteration was stopped by the callback function. + */ +int srv_store_foreach_server(srv_store_foreach_func_t func, void *user_data); + +/** + * @brief Validate the codec configuration preset. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] new Pointer to the new codec configuration. + * @param[in] existing Pointer to the existing codec configuration. + * @param[in] pref_sample_rate_value Preferred sample rate value. + * + * @retval true The new preset should overwrite the existing one. + * @retval false Otherwise. + */ +bool srv_store_preset_validated(struct bt_audio_codec_cfg const *const new, + struct bt_audio_codec_cfg const *const existing, + uint8_t pref_sample_rate_value); + +/** + * @brief Search for a common presentation delay across all server Audio Stream Endpoints (ASEs) in + * a given @p unicast_group for the given direction. + * + * This function will try to satisfy the preferred presentation delay for all + * ASEs. If that is not possible, it will try to satisfy the max and min values. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] stream Pointer to a new stream to be started + * @param[out] computed_pres_dly_us Pointer to store the computed presentation delay in + * microseconds. + * @param[out] existing_pres_dly_us Pointer to store the existing presentation delay in + * microseconds. + * @param[in] server_qos_pref Pointer to the preferred QoS configuration. + * @param[out] group_reconfig_needed True if a group reconfiguration is needed. + * @param[in] unicast_group Pointer to the unicast group to search within. + * + * @retval 0 Success, negative error code on failure. + * @retval -ESPIPE There is no common presentation delay found. + * @retval -EINVAL Illegal argument(s), or submitted streams are in different groups. + */ +int srv_store_pres_dly_find(struct bt_bap_stream *stream, uint32_t *computed_pres_dly_us, + uint32_t *existing_pres_dly_us, + struct bt_bap_qos_cfg_pref const *server_qos_pref, + bool *group_reconfig_needed, + struct bt_cap_unicast_group *unicast_group); + +/** + * @brief Set the valid locations of a unicast server. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] conn Pointer to the connection. + * @param[in] dir Direction to store. + * @param[in] loc Location to store. + * + * @retval 0 Success. + * @retval -ENOENT Server not found. + * @retval -ENOTCONN Server conn is NULL. + */ +int srv_store_location_set(struct bt_conn const *const conn, enum bt_audio_dir dir, + enum bt_audio_location loc); + +/** + * @brief Check which codec capabilities are valid. + * + * @note Will go through each of the codec_caps for a given direction for a given server. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] conn Pointer to the connection. + * @param[in] dir Direction to check for (sink or source). + * @param[out] valid_codec_caps Bit field to be populated with valid codec capabilities. + * @param[in] client_supp_cfgs (Reserved) Supported configurations of the unicast client. + * @param[in] num_client_supp_cfgs (Reserved) Number of supported configurations in the array. + * + * @retval 0 Success. + * @retval -ENOENT Server not found. + * @retval -ENOTCONN Server conn is NULL. + */ +int srv_store_valid_codec_cap_check(struct bt_conn const *const conn, enum bt_audio_dir dir, + uint32_t *valid_codec_caps, + struct client_supp_configs const **const client_supp_cfgs, + uint8_t num_client_supp_cfgs); + +/** + * @brief Get a server based on stream pointer. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] stream Pointer to the stream. + * @param[out] server Pointer to the server. NULL if not found. + * + * @retval 0 Success. + * @retval -ENOENT Server not found. + */ +int srv_store_from_stream_get(struct bt_bap_stream const *const stream, + struct server_store **server); + +/** + * @brief Get the number of endpoints in a given state across all stored servers. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] state State to search for. + * @param[in] dir Direction to filter on. + * + * @return Number of endpoints in the given state and direction. + */ +int srv_store_all_ep_state_count(enum bt_bap_ep_state state, enum bt_audio_dir dir); + +/** + * @brief Store the available audio context for a server based on conn. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] conn Pointer to the connection. + * @param[in] snk_ctx Sink context. + * @param[out] src_ctx Source context. + * + * @retval 0 Success. + * @retval -EINVAL Illegal argument(s) + * @retval -ENOENT Server not found. + */ +int srv_store_avail_context_set(struct bt_conn *conn, enum bt_audio_context snk_ctx, + enum bt_audio_context src_ctx); + +/** + * @brief Store the codec capabilities for a given server based on conn. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] conn Pointer to the connection. + * @param[in] dir Direction of the capability. + * @param[out] codec Codec capability to store. + * + * @retval 0 Success. + * @retval -EINVAL Illegal argument(s) + * @retval -ENOMEM Out of memory. + * @retval -ENOTCONN Server conn is NULL. + */ +int srv_store_codec_cap_set(struct bt_conn const *const conn, enum bt_audio_dir dir, + struct bt_audio_codec_cap const *const codec); + +/** + * @brief Get a server from the address. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] addr Pointer to the address of the server. + * @param[out] server Pointer to the server. NULL if not found. + * + * @retval 0 Success. + * @retval -ENOENT Server not found in storage. + */ +int srv_store_from_addr_get(bt_addr_le_t const *const addr, struct server_store **server); + +/** + * @brief Check if there is a server with the given address in the server store. + * + * @param[in] addr Pointer to the address of the server. + * + * @retval true The server exists. + * @retval false The server does not exist. + */ +bool srv_store_server_exists(bt_addr_le_t const *const addr); + +/** + * @brief Get a server from the conn pointer. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] conn Pointer to the connection to find the server for. + * @param[out] server Pointer to the server. NULL if not found. + * + * @retval 0 Success. + * @retval -ENOENT Server not found in storage. + * @retval -ENOTCONN Server conn is NULL. + */ +int srv_store_from_conn_get(struct bt_conn const *const conn, struct server_store **server); + +/** + * @brief Get the number of stored servers. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @note Do not iterate through the servers, as they may not be stored consecutively in + * memory. Use the srv_store_foreach_func_t function. + * + * @retval Number of servers. + * @retval -EINVAL Non-consecutive server storage detected. + * @retval -ENOENT Server not found in storage. + */ +int srv_store_num_get(void); + +/** + * @brief Add a server to the storage based on conn. + * + * @note This function should not be used if the peer uses a resolvable address + * which has not yet been resolved, or any other changing address type. + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] conn Pointer to the connection associated with the server to add, based on + * address. + * + * @retval 0 Success. + * @retval -EALREADY The server already exists. + * @retval -ENOMEM Out of memory. + * @retval Other negative. Errors from underlying functions. + */ +int srv_store_add_by_conn(struct bt_conn *conn); + +/** + * @brief Add a server to the storage based on address. + * + * @note This function can be used when getting addresses from bonding information. + * At this point, there will only be an address associated with the server, no conn + * pointer, and so on. Hence, you must call srv_store_conn_update() when a connection + * is established. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] addr Pointer to the address associated with the server to add. + * + * @retval 0 Success. + * @retval -EALREADY The server already exists. + * @retval -ENOMEM Out of memory. + * @retval Other negative. Errors from underlying functions. + */ +int srv_store_add_by_addr(const bt_addr_le_t *addr); + +/** + * @brief Update the conn pointer of an existing server, based on address. + * + * @note This function can be used when an earlier pairing has been done to this server. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] conn Pointer to the connection to write. + * @param[in] addr Address to write. + * + * @retval 0 Success. + * @retval -ENOENT Server not found. + * @retval -EPERM Address does not match the connection's peer address. + * @retval -EACCES Server already has a different conn assigned. + * @retval -EALREADY Server is already assigned to the same conn. + * @retval Other negative. Errors from underlying functions. + */ +int srv_store_conn_update(struct bt_conn *conn, bt_addr_le_t const *const addr); + +/** + * @brief Clear the contents of a server based on conn pointer. + * + * @note This function can be used in a disconnect callback. + * Will not clear the address or streams. + * Must only be called when there are no active connections. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] conn Connection to the server to be cleared. + * + * @retval 0 Success, negative error code on failure. + * @retval Other negative. Errors from underlying functions. + */ +int srv_store_clear_by_conn(struct bt_conn const *const conn); + +/** + * @brief Remove a server by conn pointer. + * + * @note Depending on the application, it is strongly recommended to call this function + * when an unbonded (untrusted) connection is terminated or the bond for that + * connection is cleared. If not, the address will still be stored, and other or new + * connections that maliciously present the same address will be recognized as a valid, + * previously stored server. + * + * @note This function assumes that this conn will no longer be valid. + * Hence, it will remove a server with a (previously) active conn. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] conn Connection to the server to be removed. + * + * @retval 0 Success. + * @retval -EACCES Server has active conn. + * @retval Other negative. Errors from underlying functions. + */ +int srv_store_remove_by_conn(struct bt_conn const *const conn); + +/** + * @brief Remove a single stored server based on address. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] addr Pointer to the address associated with the server to remove. + * + * @retval 0 Success. + * @retval Negative error code on failure. + */ +int srv_store_remove_by_addr(bt_addr_le_t const *const addr); + +/** + * @brief Remove all servers. + * + * @note Must only be called when there are no active connections. + * + * @note srv_store_lock() must be called before accessing this function. + * + * @param[in] force Set true to remove servers even if they have active connection. + * Use false for normal operation, use true with caution. + * + * @retval 0 Success. + * @retval Negative error code on failure. + */ +int srv_store_remove_all(bool force); + +/** + * @brief Take the server store semaphore to lock down server store. + * This is to prevent race conditions. + * + * @note Do not use directly. Use the supplied defines. + * + * @param timeout[in] Waiting period to take the semaphore, or one of the special values + * K_NO_WAIT and K_FOREVER. + * @param file[in] String to log the lock source. + * @param line[in] String to log the lock line number. + * + * @retval 0 Lock taken. One can now operate on the server store. + * @retval -EBUSY Returned without waiting. + * @retval -EAGAIN Waiting period timed out, or the semaphore reset during the waiting period. + */ +int _srv_store_lock(k_timeout_t timeout, const char *file, int line); + +#ifdef CONFIG_DEBUG +#define srv_store_lock(timeout) _srv_store_lock(timeout, __func__, __LINE__) +#else +#define srv_store_lock(timeout) _srv_store_lock(timeout, NULL, 0) +#endif + +/** + * @brief Give the server store semaphore to unlock server store. + */ +void srv_store_unlock(void); + +/** + * @brief Initialize the server store and clear all contents. + * + * @retval 0 Success. + * @retval Negative error code on failure. + */ +int srv_store_init(void); + +/** + * @} + * @} + */ + +#endif /* _SERVER_STORE_H_ */ diff --git a/src/bluetooth/bt_stream/unicast/unicast_client.c b/src/bluetooth/bt_stream/unicast/unicast_client.c index 26aabe4b..94854af6 100644 --- a/src/bluetooth/bt_stream/unicast/unicast_client.c +++ b/src/bluetooth/bt_stream/unicast/unicast_client.c @@ -6,7 +6,6 @@ #include "unicast_client.h" -#include #include #include #include @@ -22,10 +21,14 @@ /* https://github.com/zephyrproject-rtos/zephyr/issues/72359 */ #include <../subsys/bluetooth/audio/bap_endpoint.h> +/* TODO: Remove when group configuration is properly addressed: OCT-3787 */ +#include <../subsys/bluetooth/audio/cap_internal.h> + #include "macros_common.h" #include "zbus_common.h" #include "bt_le_audio_tx.h" #include "le_audio.h" +#include "server_store.h" #include LOG_MODULE_REGISTER(unicast_client, CONFIG_UNICAST_CLIENT_LOG_LEVEL); @@ -33,90 +36,47 @@ LOG_MODULE_REGISTER(unicast_client, CONFIG_UNICAST_CLIENT_LOG_LEVEL); ZBUS_CHAN_DEFINE(le_audio_chan, struct le_audio_msg, NULL, NULL, ZBUS_OBSERVERS_EMPTY, ZBUS_MSG_INIT(0)); -#define CAP_PROCED_MUTEX_WAIT_TIME_MS K_MSEC(500) - -struct le_audio_unicast_server { - char *ch_name; - struct bt_conn *device_conn; - enum bt_audio_location location; - bool qos_reconfigure; - uint32_t reconfigure_pd; - /* Sink variables */ - bool waiting_for_sink_disc; - struct bt_audio_codec_cap sink_codec_cap[CONFIG_CODEC_CAP_COUNT_MAX]; - uint8_t num_sink_eps; - struct bt_bap_ep *sink_ep; - struct bt_cap_stream cap_sink_stream; - /* Source variables */ - bool waiting_for_source_disc; - struct bt_audio_codec_cap source_codec_cap[CONFIG_CODEC_CAP_COUNT_MAX]; - uint8_t num_source_eps; - struct bt_bap_ep *source_ep; - struct bt_cap_stream cap_source_stream; - const struct bt_csip_set_coordinator_set_member *member; +#define CAP_PROCED_SEM_WAIT_TIME_MS K_MSEC(500) +K_SEM_DEFINE(sem_cap_procedure_proceed, 1, 1); + +enum cap_procedure_type { + CAP_PROCEDURE_START = 1, + CAP_PROCEDURE_UPDATE, + CAP_PROCEDURE_STOP, }; +K_MSGQ_DEFINE(cap_proc_q, sizeof(enum cap_procedure_type), CONFIG_BT_ISO_MAX_CHAN, sizeof(void *)); + +/* For unicast (as opposed to broadcast) level 2/subgroup is not defined in the specification */ +#define LVL2 0 +/* Will return 1 if x == 0, due to how locations are defined in LE Audio */ +#define POPCOUNT_ZERO(x) ((x) == 0 ? 1 : POPCOUNT(x)) + struct discover_dir { struct bt_conn *conn; bool sink; bool source; }; -struct temp_cap_storage { - struct bt_conn *conn; - uint8_t num_caps; - /* Must be the same size as sink_codec_cap and source_codec_cap */ - struct bt_audio_codec_cap codec[CONFIG_CODEC_CAP_COUNT_MAX]; -}; - -/* Since there is no subgroups for CIG we will use 1 as a hard coded value */ -static struct le_audio_unicast_server unicast_servers[CONFIG_BT_ISO_MAX_CIG][1][CONFIG_BT_MAX_CONN]; - -K_MSGQ_DEFINE(cap_start_msgq, sizeof(struct stream_index), CONFIG_BT_ISO_MAX_CHAN, - sizeof(uint32_t)); - -static struct temp_cap_storage temp_cap[CONFIG_BT_ISO_MAX_CHAN]; - -/* Make sure that we have at least one unicast_server per CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK */ -BUILD_ASSERT(ARRAY_SIZE(unicast_servers[0][0]) >= CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT, - "We need to have at least one unicast_server per ASE SINK"); - -/* Make sure that we have at least one unicast_server per CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC */ -BUILD_ASSERT(ARRAY_SIZE(unicast_servers[0][0]) >= CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT, - "We need to have at least one unicast_server per ASE SOURCE"); - BUILD_ASSERT(CONFIG_BT_ISO_MAX_CIG == 1, "Only one CIG is supported"); static le_audio_receive_cb receive_cb; -static struct bt_bap_unicast_group *unicast_group; -static bool unicast_group_created; +BT_LE_AUDIO_TX_DEFINE(bt_le_audio_tx); -static struct bt_bap_lc3_preset lc3_preset_sink = BT_BAP_LC3_UNICAST_PRESET_NRF5340_AUDIO_SINK; -static struct bt_bap_lc3_preset lc3_preset_sink_48_4_1 = BT_BAP_LC3_UNICAST_PRESET_48_4_1( - BT_AUDIO_LOCATION_ANY, (BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED)); -static struct bt_bap_lc3_preset lc3_preset_sink_24_2_1 = BT_BAP_LC3_UNICAST_PRESET_24_2_1( - BT_AUDIO_LOCATION_ANY, (BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED)); -static struct bt_bap_lc3_preset lc3_preset_sink_16_2_1 = BT_BAP_LC3_UNICAST_PRESET_16_2_1( - BT_AUDIO_LOCATION_ANY, (BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED)); - -static struct bt_bap_lc3_preset lc3_preset_source = BT_BAP_LC3_UNICAST_PRESET_NRF5340_AUDIO_SOURCE; -static struct bt_bap_lc3_preset lc3_preset_source_48_4_1 = - BT_BAP_LC3_UNICAST_PRESET_48_4_1(BT_AUDIO_LOCATION_ANY, BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED); -static struct bt_bap_lc3_preset lc3_preset_source_24_2_1 = - BT_BAP_LC3_UNICAST_PRESET_24_2_1(BT_AUDIO_LOCATION_ANY, BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED); -static struct bt_bap_lc3_preset lc3_preset_source_16_2_1 = - BT_BAP_LC3_UNICAST_PRESET_16_2_1(BT_AUDIO_LOCATION_ANY, BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED); +static struct bt_cap_unicast_group *unicast_group; +static bool unicast_group_created; static bool playing_state = true; static void le_audio_event_publish(enum le_audio_evt_type event, struct bt_conn *conn, - enum bt_audio_dir dir) + struct bt_bap_stream *stream, enum bt_audio_dir dir) { int ret; struct le_audio_msg msg; msg.event = event; + msg.stream = stream; msg.conn = conn; msg.dir = dir; @@ -124,938 +84,850 @@ static void le_audio_event_publish(enum le_audio_evt_type event, struct bt_conn ERR_CHK(ret); } -K_MUTEX_DEFINE(mtx_cap_procedure_proceed); - -static void cap_start_worker(struct k_work *work) +static int stream_idx_get(struct bt_bap_stream *stream, struct stream_index *idx) { int ret; - struct stream_index idx; - int device_iterator = 0; - int stream_iterator = 0; - - /* Check msgq for a pending start procedure */ - ret = k_msgq_get(&cap_start_msgq, &idx, K_NO_WAIT); - if (ret) { - LOG_ERR("Failed to get device index for pending cap start procedure: %d", ret); - return; - } - - struct le_audio_unicast_server *unicast_server = - &unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3]; + struct bt_iso_info info; - if (unicast_server->sink_ep == NULL && unicast_server->source_ep == NULL) { - LOG_ERR("No sink or source endpoint found for device"); - return; + ret = bt_iso_chan_get_info(stream->iso, &info); + if (ret < 0) { + LOG_ERR("Failed to get ISO channel info: %d", ret); + return ret; } - if (unicast_group_created == false) { - uint8_t cig_index = idx.lvl1; - struct bt_bap_unicast_group_stream_pair_param - pair_params[ARRAY_SIZE(unicast_servers[cig_index][0])]; - /* 2 streams (one sink and one source stream) for each unicast_server */ - struct bt_bap_unicast_group_stream_param - group_stream_params[(ARRAY_SIZE(unicast_servers[cig_index][0]) * 2)]; - struct bt_bap_unicast_group_param group_param; - - for (int i = 0; i < ARRAY_SIZE(group_stream_params); i++) { - /* Every other stream should be sink or source */ - if ((i % 2) == 0) { - group_stream_params[i].qos = &lc3_preset_sink.qos; - group_stream_params[i].stream = - &unicast_servers[cig_index][0][device_iterator] - .cap_sink_stream.bap_stream; - } else { - group_stream_params[i].qos = &lc3_preset_source.qos; - group_stream_params[i].stream = - &unicast_servers[cig_index][0][device_iterator] - .cap_source_stream.bap_stream; - device_iterator++; - } - } - - for (int i = 0; i < ARRAY_SIZE(pair_params); i++) { - if (unicast_server->sink_ep) { - pair_params[i].tx_param = &group_stream_params[stream_iterator]; - } else { - pair_params[i].tx_param = NULL; - } - stream_iterator++; - - if (unicast_server->source_ep) { - pair_params[i].rx_param = &group_stream_params[stream_iterator]; - } else { - pair_params[i].rx_param = NULL; - } - - stream_iterator++; - } + idx->lvl1 = info.unicast.cig_id; + idx->lvl2 = LVL2; + idx->lvl3 = info.unicast.cis_id; - group_param.params = pair_params; - group_param.params_count = ARRAY_SIZE(pair_params); + return 0; +} - if (IS_ENABLED(CONFIG_BT_AUDIO_PACKING_INTERLEAVED)) { - group_param.packing = BT_ISO_PACKING_INTERLEAVED; - } else { - group_param.packing = BT_ISO_PACKING_SEQUENTIAL; - } +/** + * @brief Check if there is any CAP procedure waiting, execute it if so. + */ +static void cap_proc_waiting_check(void) +{ + int ret; + enum cap_procedure_type proc; - ret = bt_bap_unicast_group_create(&group_param, &unicast_group); - if (ret) { - LOG_ERR("Failed to create unicast group: %d", ret); - } else { - unicast_group_created = true; - } + ret = k_msgq_get(&cap_proc_q, &proc, K_NO_WAIT); + if (ret == -ENOMSG) { + /* No procedure waiting */ + return; + } else if (ret) { + LOG_ERR("Failed to get message from cap_proc_q: %d", ret); + return; } - ret = k_mutex_lock(&mtx_cap_procedure_proceed, CAP_PROCED_MUTEX_WAIT_TIME_MS); - if (ret == -EAGAIN) { - LOG_ERR("CAP procedure lock timeout"); + switch (proc) { + case CAP_PROCEDURE_START: + unicast_client_start(0); + break; + case CAP_PROCEDURE_UPDATE: + LOG_ERR("Update procedure not implemented"); + break; + case CAP_PROCEDURE_STOP: + unicast_client_stop(0); + break; + default: + LOG_ERR("Unknown procedure: %d", proc); + break; } +} - struct bt_cap_unicast_audio_start_stream_param - cap_stream_params[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT + - CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT]; - - struct bt_cap_unicast_audio_start_param param; - - param.stream_params = cap_stream_params; - param.count = 0; - param.type = BT_CAP_SET_TYPE_AD_HOC; +struct num_eps_total { + uint16_t sink; + uint16_t source; +}; - if (unicast_server->sink_ep) { - cap_stream_params[param.count].member.member = unicast_server->device_conn; - cap_stream_params[param.count].stream = &unicast_server->cap_sink_stream; - cap_stream_params[param.count].ep = unicast_server->sink_ep; - cap_stream_params[param.count].codec_cfg = &lc3_preset_sink.codec_cfg; - param.count++; - } +static bool num_eps_count(struct server_store *server, void *user_data) +{ + struct num_eps_total *num_eps = (struct num_eps_total *)user_data; - if (unicast_server->source_ep) { - cap_stream_params[param.count].member.member = unicast_server->device_conn; - cap_stream_params[param.count].stream = &unicast_server->cap_source_stream; - cap_stream_params[param.count].ep = unicast_server->source_ep; - cap_stream_params[param.count].codec_cfg = &lc3_preset_source.codec_cfg; - param.count++; - } + num_eps->sink += server->snk.num_eps; + num_eps->source += server->src.num_eps; - ret = bt_cap_initiator_unicast_audio_start(¶m); - if (ret == -EBUSY) { - /* Try again once the ongoing start procedure is completed */ - ret = k_msgq_put(&cap_start_msgq, &idx, K_NO_WAIT); - if (ret) { - LOG_ERR("Failed to put device_index on the queue: %d", ret); - } - } else if (ret) { - LOG_ERR("Failed to start unicast sink audio: %d", ret); - } + return true; } -K_WORK_DEFINE(cap_start_work, cap_start_worker); -/** - * @brief Get the common presentation delay for all unicast_servers. - * - * @param[in] index The index of the device to test against. - * @param[out] pres_dly_us Pointer to where the presentation delay will be stored. - * - * @retval 0 Operation successful. - * @retval -EINVAL Any error. - */ -static int device_pres_delay_find(struct stream_index idx, uint32_t *pres_dly_us) -{ - struct le_audio_unicast_server *unicast_server = - &unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3]; - - uint32_t pd_min = unicast_server->sink_ep->qos_pref.pd_min; - uint32_t pd_max = unicast_server->sink_ep->qos_pref.pd_max; - uint32_t pref_pd_min = unicast_server->sink_ep->qos_pref.pref_pd_min; - uint32_t pref_pd_max = unicast_server->sink_ep->qos_pref.pref_pd_max; - - LOG_DBG("Index: %d, Pref min: %d, pref max: %d, pres_min: %d, pres_max: %d", idx.lvl3, - pref_pd_min, pref_pd_max, pd_min, pd_max); - - *pres_dly_us = 0; - - for (int i = 0; i < ARRAY_SIZE(unicast_servers[idx.lvl1][idx.lvl2]); i++) { - if (le_audio_ep_qos_configured(unicast_servers[idx.lvl1][idx.lvl2][i].sink_ep)) { - LOG_DBG("i: %d, Pref min: %d, pref max: %d, pres_min: %d, pres_max: %d", i, - unicast_servers[idx.lvl1][idx.lvl2][i] - .sink_ep->qos_pref.pref_pd_min, - unicast_servers[idx.lvl1][idx.lvl2][i] - .sink_ep->qos_pref.pref_pd_max, - unicast_servers[idx.lvl1][idx.lvl2][i].sink_ep->qos_pref.pd_min, - unicast_servers[idx.lvl1][idx.lvl2][i].sink_ep->qos_pref.pd_max); - - pd_min = MAX( - pd_min, - unicast_servers[idx.lvl1][idx.lvl2][i].sink_ep->qos_pref.pd_min); - pref_pd_min = MAX(pref_pd_min, unicast_servers[idx.lvl1][idx.lvl2][i] - .sink_ep->qos_pref.pref_pd_min); - - if (unicast_servers[idx.lvl1][idx.lvl2][i].sink_ep->qos_pref.pd_max) { - pd_max = MIN(pd_max, unicast_servers[idx.lvl1][idx.lvl2][i] - .sink_ep->qos_pref.pd_max); - } - - if (unicast_servers[idx.lvl1][idx.lvl2][i].sink_ep->qos_pref.pref_pd_max) { - pref_pd_max = - MIN(pref_pd_max, unicast_servers[idx.lvl1][idx.lvl2][i] - .sink_ep->qos_pref.pref_pd_max); - } - } - } +struct group_streams_populate_data { + uint8_t sink_iterator; + uint8_t source_iterator; + struct bt_cap_unicast_group_stream_param + sink_stream_params[CONFIG_BT_BAP_UNICAST_CLIENT_GROUP_STREAM_COUNT]; + struct bt_cap_unicast_group_stream_param + source_stream_params[CONFIG_BT_BAP_UNICAST_CLIENT_GROUP_STREAM_COUNT]; +}; - if (IS_ENABLED(CONFIG_BT_AUDIO_PRES_DELAY_SRCH_MIN)) { - *pres_dly_us = pd_min; +static bool unicast_group_populate(struct server_store *server, void *user_data) +{ + struct group_streams_populate_data *data = (struct group_streams_populate_data *)user_data; - return 0; + if (server->snk.num_eps == 0 && server->src.num_eps == 0) { + LOG_DBG("Server %s has no valid sink or source EPs, skipping", server->name); + return true; } - if (IS_ENABLED(CONFIG_BT_AUDIO_PRES_DELAY_SRCH_MAX)) { - *pres_dly_us = pd_max; + /* Add only the streams that has a valid preset set */ + for (int j = 0; j < MIN(server->snk.num_eps, POPCOUNT_ZERO(server->snk.locations)); j++) { + if (server->snk.lc3_preset[j].qos.pd == 0) { + LOG_DBG("Sink EP %d has no valid preset, skipping", j); + return true; + } - return 0; + data->sink_stream_params[data->sink_iterator].qos_cfg = + &server->snk.lc3_preset[j].qos; + data->sink_stream_params[data->sink_iterator].stream = &server->snk.cap_streams[j]; + data->sink_iterator++; } - if (IS_ENABLED(CONFIG_BT_AUDIO_PRES_DELAY_SRCH_PREF_MIN)) { - if (pref_pd_min == 0) { - *pres_dly_us = pd_min; - } else if (pref_pd_min < pd_min) { - *pres_dly_us = pd_min; - LOG_WRN("pref_pd_min < pd_min (%d < %d), pres delay set to %d", pref_pd_min, - pd_min, *pres_dly_us); - } else if (pref_pd_min <= pd_max) { - *pres_dly_us = pref_pd_min; - } else { - *pres_dly_us = pd_max; - LOG_WRN("pref_pd_min > pd_max (%d > %d), pres delay set to %d", pref_pd_min, - pd_max, *pres_dly_us); - } - - return 0; - } - - if (IS_ENABLED(CONFIG_BT_AUDIO_PRES_DELAY_SRCH_PREF_MAX)) { - if (pref_pd_max == 0) { - *pres_dly_us = pd_max; - } else if (pref_pd_max > pd_max) { - *pres_dly_us = pd_max; - LOG_WRN("pref_pd_max > pd_max (%d > %d), pres delay set to %d", pref_pd_max, - pd_max, *pres_dly_us); - } else if (pref_pd_max >= pd_min) { - *pres_dly_us = pref_pd_max; - } else { - *pres_dly_us = pd_min; - LOG_WRN("pref_pd_max < pd_min (%d < %d), pres delay set to %d", pref_pd_max, - pd_min, *pres_dly_us); + /* Add only the streams that has a valid preset set */ + for (int j = 0; j < MIN(server->src.num_eps, POPCOUNT_ZERO(server->src.locations)); j++) { + if (server->src.lc3_preset[j].qos.pd == 0) { + LOG_DBG("Source EP %d has no valid preset, skipping", j); + return true; } - return 0; + data->source_stream_params[data->source_iterator].qos_cfg = + &server->src.lc3_preset[j].qos; + data->source_stream_params[data->source_iterator].stream = + &server->src.cap_streams[j]; + data->source_iterator++; } - LOG_ERR("Trying to use unrecognized search mode"); - - return -EINVAL; + return true; } /** - * @brief Get device index based on connection. - * - * @param[in] conn The connection to search for. - * @param[out] index The device index. - * - * @retval 0 Operation successful. - * @retval -EINVAL There is no match. + * @brief Create a unicast group with all connected servers and their valid EPs. + * Pair sink/source streams from the same server together to make sure they are + * part of the same CIS. */ -static int device_index_get(const struct bt_conn *conn, struct stream_index *idx) +static void unicast_group_create(void) { - if (conn == NULL) { - LOG_ERR("No connection provided"); - return -EINVAL; - } + int ret; - for (int i = 0; i < CONFIG_BT_ISO_MAX_CIG; i++) { - for (int j = 0; j < ARRAY_SIZE(unicast_servers[i][0]); j++) { - if (unicast_servers[i][0][j].device_conn == conn) { - idx->lvl1 = i; - idx->lvl2 = 0; - idx->lvl3 = j; - return 0; - } - } + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return; } - return -EINVAL; -} - -static int device_index_vacant_get(const struct bt_conn *conn, struct stream_index *idx) -{ + /* Find out how many valid sink EPs we have */ + struct num_eps_total num_eps = {0, 0}; - if (device_index_get(conn, idx) == 0) { - LOG_WRN("Device has already been discovered"); - return -EALREADY; + ret = srv_store_foreach_server(num_eps_count, &num_eps); + if (ret < 0) { + LOG_ERR("Failed to count valid EPs: %d", ret); + srv_store_unlock(); + return; } - for (int i = 0; i < CONFIG_BT_ISO_MAX_CIG; i++) { - for (int j = 0; j < ARRAY_SIZE(unicast_servers[i][0]); j++) { - if (unicast_servers[i][0][j].device_conn == NULL) { - idx->lvl1 = i; - idx->lvl2 = 0; - idx->lvl3 = j; - return 0; - } - } + if (num_eps.sink == 0 && num_eps.source == 0) { + LOG_ERR("No valid sink or source EPs found, cannot create unicast " + "group"); + srv_store_unlock(); + return; } - LOG_WRN("No more room in device list"); - return -ENOSPC; -} - -static void supported_sample_rates_print(uint16_t supported_sample_rates, enum bt_audio_dir dir) -{ - char supported_str[20] = ""; - - if (supported_sample_rates & BT_AUDIO_CODEC_CAP_FREQ_48KHZ) { - strcat(supported_str, "48, "); - } + /* Populate the stream params arrays */ + struct group_streams_populate_data data = {0}; - if (supported_sample_rates & BT_AUDIO_CODEC_CAP_FREQ_24KHZ) { - strcat(supported_str, "24, "); + ret = srv_store_foreach_server(unicast_group_populate, &data); + if (ret < 0) { + LOG_ERR("Failed to populate unicast group stream params: %d", ret); + srv_store_unlock(); + return; } - if (supported_sample_rates & BT_AUDIO_CODEC_CAP_FREQ_32KHZ) { - strcat(supported_str, "32, "); - } + struct bt_cap_unicast_group_stream_pair_param + pair_params[CONFIG_BT_BAP_UNICAST_CLIENT_GROUP_STREAM_COUNT]; + struct bt_cap_unicast_group_param group_param; + int stream_iterator = 0; - if (supported_sample_rates & BT_AUDIO_CODEC_CAP_FREQ_16KHZ) { - strcat(supported_str, "16, "); - } + /* Pair TX and RX from same server. + * We pair in the order of sink to source because the sink stream will + * always be created before the source stream + */ + for (int i = 0; i < data.sink_iterator; i++) { + pair_params[i].tx_param = &data.sink_stream_params[i]; + + /* Search streams for a matching source */ + bool source_found = false; + struct server_store *sink_server = NULL; + + ret = srv_store_from_stream_get(&data.sink_stream_params[i].stream->bap_stream, + &sink_server); + if (ret < 0) { + LOG_ERR("Failed to get server from sink stream %p: %d", + (void *)&data.sink_stream_params[i].stream->bap_stream, ret); + srv_store_unlock(); + return; + } - if (dir == BT_AUDIO_DIR_SINK) { - LOG_DBG("Unicast_server supports: %s kHz in sink direction", supported_str); - } else if (dir == BT_AUDIO_DIR_SOURCE) { - LOG_DBG("Unicast_server supports: %s kHz in source direction", supported_str); - } -} + for (int j = stream_iterator; j < data.source_iterator; j++) { + /* Check if the source stream belongs to the same device */ + struct server_store *source_server = NULL; -static bool sink_parse_cb(struct bt_data *data, void *user_data) -{ - if (data->type == BT_AUDIO_CODEC_CAP_TYPE_FREQ) { - uint16_t lc3_freq_bit = sys_get_le16(data->data); - - supported_sample_rates_print(lc3_freq_bit, BT_AUDIO_DIR_SINK); - - /* Try with the preferred sample rate first */ - switch (CONFIG_BT_AUDIO_PREF_SAMPLE_RATE_VALUE) { - case BT_AUDIO_CODEC_CFG_FREQ_48KHZ: - if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_48KHZ) { - lc3_preset_sink = lc3_preset_sink_48_4_1; - *(bool *)user_data = true; - /* Found what we were looking for, stop parsing LTV */ - return false; + ret = srv_store_from_stream_get( + &data.source_stream_params[j].stream->bap_stream, &source_server); + if (ret < 0) { + LOG_ERR("Failed to get server from source stream %p: %d", + (void *)&data.source_stream_params[j].stream->bap_stream, + ret); + srv_store_unlock(); + return; } - break; - - case BT_AUDIO_CODEC_CFG_FREQ_24KHZ: - if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_24KHZ) { - lc3_preset_sink = lc3_preset_sink_24_2_1; - *(bool *)user_data = true; - /* Found what we were looking for, stop parsing LTV */ - return false; + if (sink_server == source_server) { + pair_params[i].rx_param = &data.source_stream_params[j]; + source_found = true; + stream_iterator++; + break; } + } + + if (!source_found) { + LOG_DBG("Setting RX param for sink EP %d to NULL", i); + pair_params[i].rx_param = NULL; + stream_iterator++; + } + } - break; + /* Check if there are unpaired source streams, if so; add them */ + for (int i = 0; i < data.source_iterator; i++) { + /* Check if the source has already been added */ + bool source_already_added = false; - case BT_AUDIO_CODEC_CFG_FREQ_16KHZ: - if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_16KHZ) { - lc3_preset_sink = lc3_preset_sink_16_2_1; - *(bool *)user_data = true; - /* Found what we were looking for, stop parsing LTV */ - return false; + for (int j = 0; j < stream_iterator; j++) { + if (pair_params[j].rx_param == &data.source_stream_params[i]) { + source_already_added = true; + break; } - - break; } - /* If no match with the preferred, revert to trying highest first */ - if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_48KHZ) { - lc3_preset_sink = lc3_preset_sink_48_4_1; - *(bool *)user_data = true; - } else if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_24KHZ) { - lc3_preset_sink = lc3_preset_sink_24_2_1; - *(bool *)user_data = true; - } else if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_16KHZ) { - lc3_preset_sink = lc3_preset_sink_16_2_1; - *(bool *)user_data = true; + if (source_already_added) { + LOG_DBG("Source EP %d already added, skipping", i); + continue; } - /* Found what we were looking for, stop parsing LTV */ - return false; + LOG_DBG("Adding unpaired source EP %d", i); + pair_params[stream_iterator].tx_param = NULL; + pair_params[stream_iterator].rx_param = &data.source_stream_params[i]; + stream_iterator++; } - /* Did not find what we were looking for, continue parsing LTV */ - return true; -} + group_param.params = pair_params; + group_param.params_count = stream_iterator; -static void set_color_if_supported(char *str, uint16_t bitfield, uint16_t mask) -{ - if (bitfield & mask) { - strcat(str, COLOR_GREEN); + if (group_param.params_count == 0) { + LOG_ERR("No valid streams to create a unicast group"); + srv_store_unlock(); + return; + } + + if (IS_ENABLED(CONFIG_BT_AUDIO_PACKING_INTERLEAVED)) { + group_param.packing = BT_ISO_PACKING_INTERLEAVED; } else { - strcat(str, COLOR_RED); + group_param.packing = BT_ISO_PACKING_SEQUENTIAL; } -} -static bool caps_print_cb(struct bt_data *data, void *user_data) -{ - if (data->type == BT_AUDIO_CODEC_CAP_TYPE_FREQ) { - uint16_t freq_bit = sys_get_le16(data->data); - char supported_freq[320] = ""; - - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_8KHZ); - strcat(supported_freq, "8, "); - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_11KHZ); - strcat(supported_freq, "11.025, "); - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_16KHZ); - strcat(supported_freq, "16, "); - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_22KHZ); - strcat(supported_freq, "22.05, "); - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_24KHZ); - strcat(supported_freq, "24, "); - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_32KHZ); - strcat(supported_freq, "32, "); - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_44KHZ); - strcat(supported_freq, "44.1, "); - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_48KHZ); - strcat(supported_freq, "48, "); - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_88KHZ); - strcat(supported_freq, "88.2, "); - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_96KHZ); - strcat(supported_freq, "96, "); - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_176KHZ); - strcat(supported_freq, "176, "); - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_192KHZ); - strcat(supported_freq, "192, "); - set_color_if_supported(supported_freq, freq_bit, BT_AUDIO_CODEC_CAP_FREQ_384KHZ); - strcat(supported_freq, "384"); - - LOG_INF("\tFrequencies kHz: %s", supported_freq); - } - - if (data->type == BT_AUDIO_CODEC_CAP_TYPE_DURATION) { - uint16_t dur_bit = sys_get_le16(data->data); - char supported_dur[30] = ""; - - set_color_if_supported(supported_dur, dur_bit, BT_AUDIO_CODEC_CAP_DURATION_7_5); - strcat(supported_dur, "7.5, "); - set_color_if_supported(supported_dur, dur_bit, BT_AUDIO_CODEC_CAP_DURATION_10); - strcat(supported_dur, "10"); - - LOG_INF("\tFrame duration ms: %s", supported_dur); - } - - if (data->type == BT_AUDIO_CODEC_CAP_TYPE_CHAN_COUNT) { - uint16_t chan_bit = sys_get_le16(data->data); - char supported_chan[120] = ""; - - set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_1); - strcat(supported_chan, "1, "); - set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_2); - strcat(supported_chan, "2, "); - set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_3); - strcat(supported_chan, "3, "); - set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_4); - strcat(supported_chan, "4, "); - set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_5); - strcat(supported_chan, "5, "); - set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_6); - strcat(supported_chan, "6, "); - set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_7); - strcat(supported_chan, "7, "); - set_color_if_supported(supported_chan, chan_bit, BT_AUDIO_CODEC_CAP_CHAN_COUNT_8); - strcat(supported_chan, "8"); - - LOG_INF("\tChannels supported: %s", supported_chan); - } - - if (data->type == BT_AUDIO_CODEC_CAP_TYPE_FRAME_LEN) { - uint16_t lc3_min_frame_length = sys_get_le16(data->data); - uint16_t lc3_max_frame_length = sys_get_le16(data->data + sizeof(uint16_t)); - - LOG_INF("\tFrame length bytes: %d - %d", lc3_min_frame_length, - lc3_max_frame_length); - } - - if (data->type == BT_AUDIO_CODEC_CAP_TYPE_FRAME_COUNT) { - uint16_t lc3_frame_per_sdu = sys_get_le16(data->data); - - LOG_INF("\tMax frames per SDU: %d", lc3_frame_per_sdu); + ret = bt_cap_unicast_group_create(&group_param, &unicast_group); + if (ret) { + LOG_ERR("Failed to create unicast group: %d", ret); + } else { + LOG_INF("Created unicast group"); + unicast_group_created = true; } - return true; + srv_store_unlock(); } -static bool source_parse_cb(struct bt_data *data, void *user_data) +/** + * @brief Function to check if a stream is already in the unicast group. + * + * @param[in] stream Stream to check. + * @param[in] user_data User data, in this case a pointer to the server_store to + * check against. + * + * @retval False The stream is in the group. + * @retval True The stream is not already in the group. (stop iterating) + */ +static bool stream_in_group_check(struct bt_cap_stream *stream, void *user_data) { - if (data->type == BT_AUDIO_CODEC_CAP_TYPE_FREQ) { - uint16_t lc3_freq_bit = sys_get_le16(data->data); - - supported_sample_rates_print(lc3_freq_bit, BT_AUDIO_DIR_SOURCE); - - /* Try with the preferred sample rate first */ - switch (CONFIG_BT_AUDIO_PREF_SAMPLE_RATE_VALUE) { - case BT_AUDIO_CODEC_CFG_FREQ_48KHZ: - if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_48KHZ) { - lc3_preset_source = lc3_preset_source_48_4_1; - *(bool *)user_data = true; - /* Found what we were looking for, stop parsing LTV */ - return false; - } - - break; - - case BT_AUDIO_CODEC_CFG_FREQ_24KHZ: - if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_24KHZ) { - lc3_preset_source = lc3_preset_source_24_2_1; - *(bool *)user_data = true; - /* Found what we were looking for, stop parsing LTV */ - return false; - } - - break; - - case BT_AUDIO_CODEC_CFG_FREQ_16KHZ: - if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_16KHZ) { - lc3_preset_source = lc3_preset_source_16_2_1; - *(bool *)user_data = true; - /* Found what we were looking for, stop parsing LTV */ - return false; - } - - break; - } - - /* If no match with the preferred, revert to trying highest first */ - if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_48KHZ) { - lc3_preset_source = lc3_preset_source_48_4_1; - *(bool *)user_data = true; - } else if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_24KHZ) { - lc3_preset_source = lc3_preset_source_24_2_1; - *(bool *)user_data = true; - } else if (lc3_freq_bit & BT_AUDIO_CODEC_CAP_FREQ_16KHZ) { - lc3_preset_source = lc3_preset_source_16_2_1; - *(bool *)user_data = true; - } + struct bt_cap_stream *server_stream = (struct bt_cap_stream *)user_data; - /* Found what we were looking for, stop parsing LTV */ + if (stream == server_stream) { + /* Found the stream in the group, stop iterating */ return false; } - /* Did not find what we were looking for, continue parsing LTV */ return true; } /** - * @brief Check if the gateway can support the codec capabilities. + * @brief Function to check if a server has any streams in the unicast group. * - * @note Currently only the sampling frequency is checked. + * @param[in] server Server to check. + * @param[in] user_data Unused. * - * @param[in] cap_array The array of pointers to codec capabilities. - * @param[in] num_caps The size of cap_array. - * @param[in] dir Direction of the capabilities to check. - * @param[in] index Device index. - * - * @return True if valid codec capability found, false otherwise. + * @retval True All streams from the server are in the group. + * @retval False At least one stream is missing from the group. */ -static bool valid_codec_cap_check(struct bt_audio_codec_cap cap_array[], uint8_t num_caps, - enum bt_audio_dir dir, uint8_t index) +static bool server_stream_in_unicast_group_check(struct server_store *server, void *user_data) { - bool valid_result = false; + int ret; - /* Only the sampling frequency is checked */ - if (dir == BT_AUDIO_DIR_SINK) { - LOG_INF("Discovered %d sink endpoint(s) for device %d", num_caps, index); - for (int i = 0; i < num_caps; i++) { - if (IS_ENABLED(CONFIG_BT_AUDIO_EP_PRINT)) { - LOG_INF(""); - LOG_INF("Dev: %d Sink EP %d", index, i); - (void)bt_audio_data_parse(cap_array[i].data, cap_array[i].data_len, - caps_print_cb, NULL); - LOG_INF("__________________________"); - } + ARG_UNUSED(user_data); - (void)bt_audio_data_parse(cap_array[i].data, cap_array[i].data_len, - sink_parse_cb, &valid_result); - } - } else if (dir == BT_AUDIO_DIR_SOURCE) { - LOG_INF("Discovered %d source endpoint(s) for device %d", num_caps, index); - for (int i = 0; i < num_caps; i++) { - if (IS_ENABLED(CONFIG_BT_AUDIO_EP_PRINT)) { - LOG_INF(""); - LOG_INF("Dev: %d Source EP %d", index, i); - (void)bt_audio_data_parse(cap_array[i].data, cap_array[i].data_len, - caps_print_cb, NULL); - LOG_INF("__________________________"); - } + /* Check that the server is connected */ + struct bt_conn_info info; - (void)bt_audio_data_parse(cap_array[i].data, cap_array[i].data_len, - source_parse_cb, &valid_result); - } + ret = bt_conn_get_info(server->conn, &info); + if (ret) { + LOG_ERR("Failed to get connection info for conn: %p", (void *)server->conn); + return true; } - return valid_result; -} - -/** - * @brief Set the allocation to a preset codec configuration. - * - * @param codec The preset codec configuration. - * @param loc Location bitmask setting. - * - */ -static void bt_audio_codec_allocation_set(struct bt_audio_codec_cfg *codec_cfg, - enum bt_audio_location loc) -{ - for (size_t i = 0U; i < codec_cfg->data_len;) { - const uint8_t len = codec_cfg->data[i++]; - const uint8_t type = codec_cfg->data[i++]; - uint8_t *value = &codec_cfg->data[i]; - const uint8_t value_len = len - sizeof(type); - - if (type == BT_AUDIO_CODEC_CFG_CHAN_ALLOC) { - const uint32_t loc_32 = loc; + if (info.state != BT_CONN_STATE_CONNECTED) { + LOG_DBG("Connection %p is not connected, skipping", (void *)server->conn); + return true; + } - sys_put_le32(loc_32, value); + /* Check if the server has at least one valid preset set */ + if (server->snk.lc3_preset[0].qos.pd == 0 && server->src.lc3_preset[0].qos.pd == 0) { + LOG_DBG("Server %s has no valid preset, skipping", server->name); + return true; + } - return; + /* Check each of the streams in the unicast_group against all of the + * streams in the server + */ + for (int i = 0; i < MIN(server->snk.num_eps, POPCOUNT_ZERO(server->snk.locations)); i++) { + ret = bt_cap_unicast_group_foreach_stream(unicast_group, stream_in_group_check, + &server->snk.cap_streams[i]); + if (ret == 0) { + LOG_INF("Server %s sink stream %d (%p) not found in unicast group", + server->name, i, (void *)&server->snk.cap_streams[i]); + /* A stream is missing from the group, stop iterating */ + return false; } - i += value_len; } + + return true; } -static int update_cap_sink_stream_qos(struct le_audio_unicast_server *unicast_server, - uint32_t pres_delay_us) +/** + * @brief Worker to start unicast streams. If a unicast group doesn't exist, it will + * be created. If it does exist, it will check if there is room for more streams, and + * if so, check if there are any connected servers that are not part of the group that + * can be added. If the group is full, or there are no more servers to add, it will + * start the streams in the unicast group. + */ +static void cap_start_worker(struct k_work *work) { int ret; - if (unicast_server->cap_sink_stream.bap_stream.ep == NULL) { - return -ESRCH; + /* Create a unicast group if it doesn't already exist */ + if (unicast_group_created == false) { + LOG_DBG("Unicast group not created, creating unicast group"); + unicast_group_create(); + goto start_streams; } - if (unicast_server->cap_sink_stream.bap_stream.qos == NULL) { - LOG_WRN("No QoS found for %p", (void *)&unicast_server->cap_sink_stream.bap_stream); - return -ENXIO; - } + struct bt_cap_unicast_group_info info; + uint8_t group_length = 0; - if (unicast_server->cap_sink_stream.bap_stream.qos->pd != pres_delay_us) { - struct bt_cap_unicast_audio_stop_param param; - struct bt_cap_stream *streams[2]; + ret = bt_cap_unicast_group_get_info(unicast_group, &info); + if (ret) { + LOG_ERR("Failed to get unicast group info: %d", ret); + return; + } - LOG_DBG("Current preset PD = %d us, target PD = %d us", - unicast_server->cap_sink_stream.bap_stream.qos->pd, pres_delay_us); + group_length = sys_slist_len(&info.unicast_group->streams); + if (group_length >= CONFIG_BT_BAP_UNICAST_CLIENT_GROUP_STREAM_COUNT) { + /* The group is as full as it can get, start the relevant streams */ + LOG_DBG("Unicast group is full, cannot add more streams"); + goto start_streams; + } - param.streams = streams; - param.count = 0; - param.type = BT_CAP_SET_TYPE_AD_HOC; - param.release = true; + /* Group is created, but there is still room for more devices, check if + * there are any waiting to join. + */ - if (playing_state && - le_audio_ep_state_check(unicast_server->cap_sink_stream.bap_stream.ep, - BT_BAP_EP_STATE_STREAMING)) { - LOG_DBG("Update streaming %s unicast_server, connection %p, stream %p", - unicast_server->ch_name, (void *)&unicast_server->device_conn, - (void *)&unicast_server->cap_sink_stream.bap_stream); + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return; + } - unicast_server->qos_reconfigure = true; - unicast_server->reconfigure_pd = pres_delay_us; + /* Check if each of the connected servers in srv_store are in the unicast_group */ + ret = srv_store_foreach_server(server_stream_in_unicast_group_check, NULL); + srv_store_unlock(); - streams[param.count] = &unicast_server->cap_sink_stream; - param.count++; - } else { - LOG_DBG("Reset %s unicast_server, connection %p, stream %p", - unicast_server->ch_name, (void *)&unicast_server->device_conn, - (void *)&unicast_server->cap_sink_stream.bap_stream); - unicast_server->cap_sink_stream.bap_stream.qos->pd = pres_delay_us; + if (ret == -ECANCELED) { + /* A new group will be created after the released_cb has been called */ + ret = unicast_client_stop(0); + if (ret == -EAGAIN) { + unicast_group_create(); + goto start_streams; } - if (playing_state && - le_audio_ep_state_check(unicast_server->cap_source_stream.bap_stream.ep, - BT_BAP_EP_STATE_STREAMING)) { - unicast_server->qos_reconfigure = true; - unicast_server->reconfigure_pd = pres_delay_us; + unicast_group_created = false; - streams[param.count] = &unicast_server->cap_source_stream; - param.count++; - } - - if (param.count > 0) { - ret = bt_cap_initiator_unicast_audio_stop(¶m); - if (ret) { - LOG_WRN("Failed to stop streams: %d, use default PD in preset", - ret); - return ret; - } - } + return; } - return 0; +start_streams: + + ret = unicast_client_start(0); + if (ret < 0) { + LOG_ERR("Failed to start unicast client: %d", ret); + return; + } } +K_WORK_DEFINE(cap_start_work, cap_start_worker); + +/** + * @brief Callback for when a location is discovered. This function will store the location + * in the server_store struct for the corresponding connection and direction. + * + * @note If both front left and front right channel locations are set, the device is + * considered a stereo device, and the location will be set to front left and + * front right. If only one of the channels are set, the location will be set to + * front left or front right accordingly. If the location is not supported, a + * warning will be logged and the LE_AUDIO_EVT_NO_VALID_CFG event will be + * published. + * + * @param[in] conn Connection pointer to the unicast_server where the location was discovered. + * @param[in] dir Direction of the location (sink or source). + * @param[in] loc Discovered location. + */ static void unicast_client_location_cb(struct bt_conn *conn, enum bt_audio_dir dir, enum bt_audio_location loc) { int ret; - struct stream_index idx; + struct server_store *server = NULL; + + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return; + } - ret = device_index_get(conn, &idx); + ret = srv_store_from_conn_get(conn, &server); if (ret) { - LOG_ERR("Device index not found"); + LOG_ERR("%s: Unknown connection, should not reach here", __func__); + srv_store_unlock(); return; } - struct le_audio_unicast_server *unicast_server = - &unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3]; + if (loc & BT_AUDIO_LOCATION_FRONT_LEFT && loc & BT_AUDIO_LOCATION_FRONT_RIGHT && + dir == BT_AUDIO_DIR_SINK) { + LOG_INF("Both front left and right channel locations are set, stereo device found"); + + ret = srv_store_location_set( + conn, dir, BT_AUDIO_LOCATION_FRONT_LEFT | BT_AUDIO_LOCATION_FRONT_RIGHT); + if (ret) { + LOG_ERR("Failed to set location for conn %p, dir %d, loc %d: %d", + (void *)conn, dir, loc, ret); + srv_store_unlock(); + return; + } + + server->name = "STEREO"; + srv_store_unlock(); + return; + } - if ((loc & BT_AUDIO_LOCATION_FRONT_LEFT) || (loc & BT_AUDIO_LOCATION_SIDE_LEFT) || + if ((loc & BT_AUDIO_LOCATION_FRONT_LEFT) || (loc & BT_AUDIO_LOCATION_BACK_LEFT) || + (loc & BT_AUDIO_LOCATION_FRONT_LEFT_OF_CENTER) || (loc & BT_AUDIO_LOCATION_SIDE_LEFT) || + (loc & BT_AUDIO_LOCATION_TOP_FRONT_LEFT) || (loc & BT_AUDIO_LOCATION_TOP_BACK_LEFT) || + (loc & BT_AUDIO_LOCATION_TOP_SIDE_LEFT) || + (loc & BT_AUDIO_LOCATION_BOTTOM_FRONT_LEFT) || + (loc & BT_AUDIO_LOCATION_FRONT_LEFT_WIDE) || (loc & BT_AUDIO_LOCATION_LEFT_SURROUND) || (loc == BT_AUDIO_LOCATION_MONO_AUDIO)) { - unicast_server->location = BT_AUDIO_LOCATION_FRONT_LEFT; - unicast_server->ch_name = "LEFT"; + ret = srv_store_location_set(conn, dir, BT_AUDIO_LOCATION_FRONT_LEFT); + if (ret) { + LOG_ERR("Failed to set location for conn %p, dir %d, loc %d: %d", + (void *)conn, dir, loc, ret); + srv_store_unlock(); + return; + } + + server->name = "LEFT"; + + } else if ((loc & BT_AUDIO_LOCATION_FRONT_RIGHT) || (loc & BT_AUDIO_LOCATION_BACK_RIGHT) || + (loc & BT_AUDIO_LOCATION_FRONT_RIGHT_OF_CENTER) || + (loc & BT_AUDIO_LOCATION_SIDE_RIGHT) || + (loc & BT_AUDIO_LOCATION_TOP_FRONT_RIGHT) || + (loc & BT_AUDIO_LOCATION_TOP_BACK_RIGHT) || + (loc & BT_AUDIO_LOCATION_TOP_SIDE_RIGHT) || + (loc & BT_AUDIO_LOCATION_BOTTOM_FRONT_RIGHT) || + (loc & BT_AUDIO_LOCATION_FRONT_RIGHT_WIDE) || + (loc & BT_AUDIO_LOCATION_RIGHT_SURROUND)) { + ret = srv_store_location_set(conn, dir, BT_AUDIO_LOCATION_FRONT_RIGHT); + if (ret) { + LOG_ERR("Failed to set location for conn %p, dir %d, loc %d: %d", + (void *)conn, dir, loc, ret); + srv_store_unlock(); + return; + } + + server->name = "RIGHT"; - } else if ((loc & BT_AUDIO_LOCATION_FRONT_RIGHT) || (loc & BT_AUDIO_LOCATION_SIDE_RIGHT)) { - unicast_server->location = BT_AUDIO_LOCATION_FRONT_RIGHT; - unicast_server->ch_name = "RIGHT"; } else { LOG_WRN("Channel location not supported: %d", loc); - le_audio_event_publish(LE_AUDIO_EVT_NO_VALID_CFG, conn, dir); + le_audio_event_publish(LE_AUDIO_EVT_NO_VALID_CFG, conn, NULL, dir); } + + srv_store_unlock(); } +/** + * @brief Callback for when available contexts are discovered. This function will store the + * available contexts in the server_store struct for the corresponding connection. + * + * @param[in] conn Connection pointer to the unicast_server where the available contexts + * was discovered. + * @param[in] snk_ctx Available sink contexts. + * @param[in] src_ctx Available source contexts. + */ static void available_contexts_cb(struct bt_conn *conn, enum bt_audio_context snk_ctx, enum bt_audio_context src_ctx) { - char addr[BT_ADDR_LE_STR_LEN]; - - (void)bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); - - LOG_DBG("conn: %s, snk ctx %d src ctx %d", addr, snk_ctx, src_ctx); -} + int ret; -static int temp_cap_index_get(struct bt_conn *conn, uint8_t *index) -{ - if (conn == NULL) { - LOG_ERR("No conn provided"); - return -EINVAL; - } + LOG_DBG("conn: %p, snk ctx %d src ctx %d", (void *)conn, snk_ctx, src_ctx); - for (int i = 0; i < ARRAY_SIZE(temp_cap); i++) { - if (temp_cap[i].conn == conn) { - *index = i; - return 0; - } + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return; } - /* Connection not found in temp_cap, searching for empty slot */ - for (int i = 0; i < ARRAY_SIZE(temp_cap); i++) { - if (temp_cap[i].conn == NULL) { - temp_cap[i].conn = conn; - *index = i; - return 0; - } + ret = srv_store_avail_context_set(conn, snk_ctx, src_ctx); + if (ret) { + LOG_ERR("Failed to set available contexts for conn %p, snk ctx %d src ctx %d: %d", + (void *)conn, snk_ctx, src_ctx, ret); } - LOG_WRN("No more space in temp_cap"); - - return -ECANCELED; + srv_store_unlock(); } +/** + * @brief Callback for when a codec capability is discovered. This function will store the + * codec capability in the server_store struct for the corresponding connection and + * direction. + * + * @param[in] conn Connection pointer to the unicast_server where the codec capability was + * discovered. + * @param[in] dir Direction of the codec capability (sink or source). + * @param[in] codec Pointer to the discovered codec capability. + */ static void pac_record_cb(struct bt_conn *conn, enum bt_audio_dir dir, const struct bt_audio_codec_cap *codec) { int ret; - uint8_t temp_cap_index; - - ret = temp_cap_index_get(conn, &temp_cap_index); - if (ret) { - LOG_ERR("Could not get temporary CAP storage index"); - return; - } if (codec->id != BT_HCI_CODING_FORMAT_LC3) { LOG_DBG("Only the LC3 codec is supported"); return; } - /* num_caps is an increasing index that starts at 0 */ - if (temp_cap[temp_cap_index].num_caps < ARRAY_SIZE(temp_cap[temp_cap_index].codec)) { - struct bt_audio_codec_cap *codec_loc = - &temp_cap[temp_cap_index].codec[temp_cap[temp_cap_index].num_caps]; - - memcpy(codec_loc, codec, sizeof(struct bt_audio_codec_cap)); + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return; + } - temp_cap[temp_cap_index].num_caps++; - } else { - LOG_WRN("No more space. Increase CONFIG_CODEC_CAP_COUNT_MAX"); + ret = srv_store_codec_cap_set(conn, dir, codec); + if (ret) { + LOG_ERR("Failed to set codec capability: %d", ret); } + + srv_store_unlock(); } +/** + * @brief Callback for when an endpoint is discovered. This function will store the endpoint + * in the server_store struct for the corresponding connection and direction. + * + * @param[in] conn Connection pointer to the unicast_server where the endpoint was discovered. + * @param[in] dir Direction of the endpoint (sink or source). + * @param[in] ep Pointer to the discovered endpoint. NULL if the discovery is complete. + */ static void endpoint_cb(struct bt_conn *conn, enum bt_audio_dir dir, struct bt_bap_ep *ep) { int ret; - struct stream_index idx; - ret = device_index_get(conn, &idx); - if (ret) { - LOG_ERR("Unknown connection, should not reach here"); + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); return; } - struct le_audio_unicast_server *unicast_server = - &unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3]; + struct server_store *server = NULL; - if (dir == BT_AUDIO_DIR_SINK) { - if (ep != NULL) { - if (unicast_server->num_sink_eps > 0) { - LOG_WRN("More than one sink endpoint found, idx 0 is used " - "by default"); - return; - } + ret = srv_store_from_conn_get(conn, &server); + if (ret) { + LOG_ERR("%s: Unknown connection, should not reach here", __func__); + srv_store_unlock(); + return; + } - unicast_server->sink_ep = ep; - unicast_server->num_sink_eps++; - return; + if (ep == NULL) { + if (dir == BT_AUDIO_DIR_SINK && server->snk.eps[0] == NULL) { + LOG_WRN("No sink endpoints found"); } - if (unicast_server->sink_ep == NULL) { - LOG_WRN("No sink endpoints found"); + if (dir == BT_AUDIO_DIR_SOURCE && server->src.eps[0] == NULL) { + LOG_WRN("No source endpoints found"); } + srv_store_unlock(); return; - } else if (dir == BT_AUDIO_DIR_SOURCE) { - if (ep != NULL) { - if (unicast_server->num_source_eps > 0) { - LOG_WRN("More than one source endpoint found, idx 0 is " - "used by default"); - return; - } + } - unicast_server->source_ep = ep; - unicast_server->num_source_eps++; + if (dir == BT_AUDIO_DIR_SINK) { + if (server->snk.num_eps >= ARRAY_SIZE(server->snk.eps)) { + LOG_WRN("No more space (%d) for sink endpoints, increase " + "CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT (%d)", + server->snk.num_eps, ARRAY_SIZE(server->snk.eps)); + srv_store_unlock(); return; } - if (unicast_server->source_ep == NULL) { - LOG_WRN("No source endpoints found"); + server->snk.eps[server->snk.num_eps] = ep; + server->snk.num_eps++; + + } else if (dir == BT_AUDIO_DIR_SOURCE) { + if (server->src.num_eps >= ARRAY_SIZE(server->src.eps)) { + LOG_WRN("No more space for source endpoints, increase " + "CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT"); + srv_store_unlock(); + return; } - return; + server->src.eps[server->src.num_eps] = ep; + server->src.num_eps++; + } else { LOG_WRN("Endpoint direction not recognized: %d", dir); } + + srv_store_unlock(); } -static void discover_cb(struct bt_conn *conn, int err, enum bt_audio_dir dir) +/** + * @brief Callback when sink discovery has finished. This function will also set the codec + * configuration to be used in the preset(s) based on the discovered codec capabilities + * and channel allocation. + * + * @note If a stereo sink is found, the preset for both left and right channel will be set to + * the same preset, only differing in channel allocation. + * + * @param[in] conn Connection pointer to the unicast_server where the discovery was done. + * @param[in] err Error code, 0 if successful. + * @param[in] server Server pointer to the server_store struct where the discovered data will be + * stored. + */ +static void discover_cb_sink(struct bt_conn *conn, int err, struct server_store *server) { int ret; - uint8_t temp_cap_index; - struct stream_index idx; + if (err == BT_ATT_ERR_ATTRIBUTE_NOT_FOUND) { + LOG_WRN("No sinks found"); + return; + } + + if (err) { + LOG_ERR("Discovery failed: %d", err); + return; + } + + uint32_t valid_sink_caps = 0; - ret = device_index_get(conn, &idx); + ret = srv_store_valid_codec_cap_check(conn, BT_AUDIO_DIR_SINK, &valid_sink_caps, NULL, 0); if (ret) { - LOG_ERR("Unknown connection, should not reach here"); + LOG_ERR("Failed to check for valid codec capabilities: %d", ret); return; } - struct le_audio_unicast_server *unicast_server = - &unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3]; + if (!valid_sink_caps) { + /* NOTE: The string below is used by the Nordic CI system */ + LOG_WRN("No valid codec capability found for %s sink", server->name); + return; + } - if (err == BT_ATT_ERR_ATTRIBUTE_NOT_FOUND) { - if (dir == BT_AUDIO_DIR_SINK) { - LOG_WRN("No sinks found"); - unicast_server->waiting_for_sink_disc = false; - } else if (dir == BT_AUDIO_DIR_SOURCE) { - LOG_WRN("No sources found"); - unicast_server->waiting_for_source_disc = false; + /* Get the valid configuration to set for this stream and put that in the corresponding + * preset + */ + if (POPCOUNT_ZERO(server->snk.locations) == 1 && server->snk.num_eps >= 1) { + ret = bt_audio_codec_cfg_set_val( + &server->snk.lc3_preset[0].codec_cfg, BT_AUDIO_CODEC_CFG_CHAN_ALLOC, + (uint8_t *)&server->snk.locations, sizeof(server->snk.locations)); + if (ret < 0) { + LOG_ERR("Failed to set codec channel allocation: %d", ret); + return; + } + + } else if (POPCOUNT_ZERO(server->snk.locations) == 2 && server->snk.num_eps >= 2 && + (server->snk.locations & + (BT_AUDIO_LOCATION_FRONT_LEFT | BT_AUDIO_LOCATION_FRONT_RIGHT)) == + (BT_AUDIO_LOCATION_FRONT_LEFT | BT_AUDIO_LOCATION_FRONT_RIGHT)) { + + uint32_t left = BT_AUDIO_LOCATION_FRONT_LEFT; + uint32_t right = BT_AUDIO_LOCATION_FRONT_RIGHT; + + LOG_INF("STEREO sink found, setting up stereo codec capabilities"); + ret = bt_audio_codec_cfg_set_val(&server->snk.lc3_preset[0].codec_cfg, + BT_AUDIO_CODEC_CFG_CHAN_ALLOC, (uint8_t *)&left, + sizeof(server->snk.locations)); + if (ret < 0) { + LOG_ERR("Failed to set codec channel allocation: %d", ret); + return; + } + + /* Use the same preset for both channels */ + memcpy(&server->snk.lc3_preset[1], &server->snk.lc3_preset[0], + sizeof(struct bt_bap_lc3_preset)); + + ret = bt_audio_codec_cfg_set_val(&server->snk.lc3_preset[1].codec_cfg, + BT_AUDIO_CODEC_CFG_CHAN_ALLOC, (uint8_t *)&right, + sizeof(server->snk.locations)); + if (ret < 0) { + LOG_ERR("Failed to set codec channel allocation: %d", ret); + return; } - } else if (err) { + } else { + LOG_WRN("Unsupported unicast server/headset configuration"); + LOG_WRN("Number of sink locations: %d, number of sink endpoints: %d", + POPCOUNT_ZERO(server->snk.locations), server->snk.num_eps); + + le_audio_event_publish(LE_AUDIO_EVT_NO_VALID_CFG, conn, NULL, BT_AUDIO_DIR_SINK); + + return; + } +} + +/** + * @brief Callback when source discovery has finished. This function will also set the codec + * configuration to be used in the preset(s) based on the discovered codec capabilities + * and channel allocation. + * + * @param[in] conn Connection pointer to the unicast_server where the discovery was done. + * @param[in] err Error code, 0 if successful. + * @param[in] server Server pointer to the server_store struct where the discovered data will be + * stored. + */ +static void discover_cb_source(struct bt_conn *conn, int err, struct server_store *server) +{ + int ret; + + if (err == BT_ATT_ERR_ATTRIBUTE_NOT_FOUND) { + LOG_WRN("No sources found"); + return; + } + + if (err) { LOG_ERR("Discovery failed: %d", err); return; } - ret = temp_cap_index_get(conn, &temp_cap_index); - if (ret) { - LOG_ERR("Could not get temporary CAP storage index"); + uint32_t valid_source_caps = 0; + + ret = srv_store_valid_codec_cap_check(conn, BT_AUDIO_DIR_SOURCE, &valid_source_caps, NULL, + 0); + if (!valid_source_caps) { + LOG_WRN("No valid codec capability found for %s source", server->name); return; } - for (int i = 0; i < CONFIG_CODEC_CAP_COUNT_MAX; i++) { - if (dir == BT_AUDIO_DIR_SINK && !err) { - memcpy(&unicast_server->sink_codec_cap[i], - &temp_cap[temp_cap_index].codec[i], - sizeof(struct bt_audio_codec_cap)); - } else if (dir == BT_AUDIO_DIR_SOURCE && !err) { - memcpy(&unicast_server->source_codec_cap[i], - &temp_cap[temp_cap_index].codec[i], - sizeof(struct bt_audio_codec_cap)); - } + ret = bt_audio_codec_cfg_set_val( + &server->src.lc3_preset[0].codec_cfg, BT_AUDIO_CODEC_CFG_CHAN_ALLOC, + (uint8_t *)&server->src.locations, sizeof(server->src.locations)); + if (ret < 0) { + LOG_ERR("Failed to set codec channel allocation: %d", ret); + return; } +} - if (dir == BT_AUDIO_DIR_SINK && !err) { - if (valid_codec_cap_check(unicast_server->sink_codec_cap, - temp_cap[temp_cap_index].num_caps, BT_AUDIO_DIR_SINK, - idx.lvl3)) { - bt_audio_codec_allocation_set(&lc3_preset_sink.codec_cfg, - unicast_server->location); - } else { - /* NOTE: The string below is used by the Nordic CI system */ - LOG_WRN("No valid codec capability found for %s sink", - unicast_server->ch_name); - unicast_server->sink_ep = NULL; - } - } else if (dir == BT_AUDIO_DIR_SOURCE && !err) { - if (valid_codec_cap_check(unicast_server->source_codec_cap, - temp_cap[temp_cap_index].num_caps, BT_AUDIO_DIR_SOURCE, - idx.lvl3)) { - bt_audio_codec_allocation_set(&lc3_preset_source.codec_cfg, - unicast_server->location); - } else { - LOG_WRN("No valid codec capability found for %s source", - unicast_server->ch_name); - unicast_server->source_ep = NULL; - } +static bool server_is_not_waiting_for_disc_check(struct server_store *server, void *user_data) +{ + ARG_UNUSED(user_data); + + if (server->snk.waiting_for_disc || server->src.waiting_for_disc) { + LOG_DBG("Server %s is still waiting for discovery to complete", server->name); + /* Stop iterating */ + return false; } - /* Free up the slot in temp_cap */ - memset(temp_cap[temp_cap_index].codec, 0, sizeof(temp_cap[temp_cap_index].codec)); - temp_cap[temp_cap_index].conn = NULL; - temp_cap[temp_cap_index].num_caps = 0; + /* Continue iterating */ + return true; +} - if (dir == BT_AUDIO_DIR_SINK) { - unicast_server->waiting_for_sink_disc = false; +static void discover_cb(struct bt_conn *conn, int err, enum bt_audio_dir dir) +{ + int ret; - if (unicast_server->waiting_for_source_disc) { - ret = bt_bap_unicast_client_discover(conn, BT_AUDIO_DIR_SOURCE); - if (ret) { - LOG_WRN("Failed to discover source: %d", ret); - } + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return; + } - return; - } + struct server_store *server = NULL; + + ret = srv_store_from_conn_get(conn, &server); + if (ret) { + LOG_ERR("%s: Unknown connection, should not reach here", __func__); + srv_store_unlock(); + return; + } + + if (dir == BT_AUDIO_DIR_SINK) { + server->snk.waiting_for_disc = false; + discover_cb_sink(conn, err, server); } else if (dir == BT_AUDIO_DIR_SOURCE) { - unicast_server->waiting_for_source_disc = false; + server->src.waiting_for_disc = false; + discover_cb_source(conn, err, server); + } else { + LOG_ERR("%s: Unknown direction: %d", __func__, dir); + srv_store_unlock(); + return; + } + + if (server->src.waiting_for_disc) { + ret = bt_bap_unicast_client_discover(conn, BT_AUDIO_DIR_SOURCE); + if (ret) { + LOG_WRN("Failed to start source discovery: %d", ret); + } + + srv_store_unlock(); + return; } if (!playing_state) { /* Since we are not in a playing state we return before starting the new streams */ + srv_store_unlock(); return; } - ret = k_msgq_put(&cap_start_msgq, &idx, K_NO_WAIT); - if (ret) { - LOG_ERR("Failed to put device_index on the queue: %d", ret); + ret = srv_store_foreach_server(server_is_not_waiting_for_disc_check, NULL); + + if (ret == -ECANCELED) { + /* If any of the servers are still waiting for discovery to complete, we should not + * start the streams yet + */ + srv_store_unlock(); return; } + le_audio_event_publish(LE_AUDIO_EVT_DISCOVERY_COMPLETE, conn, NULL, dir); + + srv_store_unlock(); k_work_submit(&cap_start_work); } @@ -1068,141 +940,113 @@ static void stream_sent_cb(struct bt_bap_stream *stream) ret = le_audio_ep_state_get(stream->ep, &state); if (ret) { + LOG_ERR("Failed to get endpoint state: %d", ret); return; } if (state == BT_BAP_EP_STATE_STREAMING) { - - ret = device_index_get(stream->conn, &idx); + ret = stream_idx_get(stream, &idx); if (ret) { - LOG_ERR("Device index not found"); - } else { - ERR_CHK(bt_le_audio_tx_stream_sent(idx)); + LOG_ERR("%s: Failed to get stream index: %d", __func__, ret); + return; } + ERR_CHK(bt_le_audio_tx_stream_sent(bt_le_audio_tx, idx)); } else { LOG_WRN("Not in streaming state: %d", state); } } #endif /* CONFIG_BT_AUDIO_TX */ -static void check_and_update_pd_in_group(struct stream_index idx, uint32_t new_pres_dly_us) -{ - int ret; - - for (int i = 0; i < ARRAY_SIZE(unicast_servers[idx.lvl1][idx.lvl2]); i++) { - if (i != idx.lvl3 && unicast_servers[idx.lvl1][idx.lvl2][i].device_conn != NULL) { - ret = update_cap_sink_stream_qos(&unicast_servers[idx.lvl1][idx.lvl2][i], - new_pres_dly_us); - if (ret && ret != -ESRCH) { - /* TODO: Fix OCT-3111 and then turn the WRN to ERR */ - LOG_WRN("Presentation delay not set for %s " - "device: %d", - unicast_servers[idx.lvl1][idx.lvl2][i].ch_name, ret); - } - } - } - - LOG_DBG("Set %s, connection %p, stream %p", - unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3].ch_name, - (void *)&unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3].device_conn, - (void *)&unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3].cap_sink_stream.bap_stream); - - unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3].cap_sink_stream.bap_stream.qos->pd = - new_pres_dly_us; -} static void stream_configured_cb(struct bt_bap_stream *stream, - const struct bt_audio_codec_qos_pref *pref) + const struct bt_bap_qos_cfg_pref *server_pref) { int ret; - uint32_t new_pres_dly_us; enum bt_audio_dir dir; - struct stream_index idx; + uint32_t new_pres_dly_us = 0; - ret = device_index_get(stream->conn, &idx); - if (ret) { - LOG_ERR("Unknown connection, should not reach here"); + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); return; } - struct le_audio_unicast_server *unicast_server = - &unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3]; + struct server_store *server = NULL; + + ret = srv_store_from_stream_get(stream, &server); + if (ret) { + LOG_ERR("Unknown stream, should not reach here"); + srv_store_unlock(); + return; + } dir = le_audio_stream_dir_get(stream); if (dir <= 0) { LOG_ERR("Failed to get dir of stream %p", (void *)stream); + srv_store_unlock(); return; } - if (dir == BT_AUDIO_DIR_SINK) { - /* NOTE: The string below is used by the Nordic CI system */ - LOG_INF("%s sink stream configured", unicast_server->ch_name); - le_audio_print_codec(unicast_server->cap_sink_stream.bap_stream.codec_cfg, dir); - } else if (dir == BT_AUDIO_DIR_SOURCE) { - LOG_INF("%s source stream configured", unicast_server->ch_name); - le_audio_print_codec(unicast_server->cap_source_stream.bap_stream.codec_cfg, dir); - } else { + /* Sanity check */ + if ((dir != BT_AUDIO_DIR_SINK && dir != BT_AUDIO_DIR_SOURCE)) { LOG_ERR("Endpoint direction not recognized: %d", dir); + srv_store_unlock(); return; } - LOG_DBG("Configured Stream info: %s, %p, dir %d", unicast_server->ch_name, (void *)stream, - dir); - ret = device_pres_delay_find(idx, &new_pres_dly_us); + /* NOTE: The string below is used by the Nordic CI system */ + LOG_INF("%s %s stream configured", server->name, + (dir == BT_AUDIO_DIR_SINK) ? "sink" : "source"); + le_audio_print_codec(stream->codec_cfg, dir); + + LOG_DBG("Configured Stream info: %s, %p, dir %d", server->name, (void *)stream, dir); + + bool group_reconfigure_needed = false; + uint32_t existing_pres_dly_us = 0; + + ret = srv_store_pres_dly_find(stream, &new_pres_dly_us, &existing_pres_dly_us, server_pref, + &group_reconfigure_needed, unicast_group); if (ret) { LOG_ERR("Cannot get a valid presentation delay"); + srv_store_unlock(); return; } - if (unicast_server->waiting_for_source_disc) { + if (server->src.waiting_for_disc) { + srv_store_unlock(); return; } - if (le_audio_ep_state_check(unicast_server->cap_sink_stream.bap_stream.ep, - BT_BAP_EP_STATE_CODEC_CONFIGURED)) { - check_and_update_pd_in_group(idx, new_pres_dly_us); - } + srv_store_unlock(); - le_audio_event_publish(LE_AUDIO_EVT_CONFIG_RECEIVED, stream->conn, dir); + LOG_INF("Group PD sink: %u, source PD: %u", unicast_group->bap_unicast_group->sink_pd, + unicast_group->bap_unicast_group->source_pd); - /* Make sure both sink and source ep (if both are discovered) are configured before - * QoS + /* TODO: This part is temporary, see OCT-3787. It only works for PD as this is not part of + * the CIG, and relies on the internal headers. */ - if ((unicast_server->sink_ep != NULL && - !le_audio_ep_state_check(unicast_server->cap_sink_stream.bap_stream.ep, - BT_BAP_EP_STATE_CODEC_CONFIGURED)) || - (unicast_server->source_ep != NULL && - !le_audio_ep_state_check(unicast_server->cap_source_stream.bap_stream.ep, - BT_BAP_EP_STATE_CODEC_CONFIGURED))) { - return; + uint32_t group_pres_dly_us = (dir == BT_AUDIO_DIR_SINK) ? + unicast_group->bap_unicast_group->sink_pd : + unicast_group->bap_unicast_group->source_pd; + + if ((new_pres_dly_us != group_pres_dly_us) || group_reconfigure_needed) { + LOG_INF("Stream QoS PD: %d, prev group PD: %d, new PD %d", stream->qos->pd, + existing_pres_dly_us, new_pres_dly_us); + if (dir == BT_AUDIO_DIR_SINK) { + unicast_group->bap_unicast_group->sink_pd = new_pres_dly_us; + } + + if (dir == BT_AUDIO_DIR_SOURCE) { + unicast_group->bap_unicast_group->source_pd = new_pres_dly_us; + } } + + le_audio_event_publish(LE_AUDIO_EVT_CONFIG_RECEIVED, stream->conn, stream, dir); } static void stream_qos_set_cb(struct bt_bap_stream *stream) { - int ret; - struct stream_index idx; - LOG_DBG("QoS set cb"); - - ret = device_index_get(stream->conn, &idx); - if (ret) { - LOG_ERR("Unknown connection, should not reach here"); - return; - } - - struct le_audio_unicast_server *unicast_server = - &unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3]; - - if (unicast_server->qos_reconfigure) { - LOG_DBG("Reconfiguring: %s to PD: %d", unicast_server->ch_name, - unicast_server->reconfigure_pd); - - unicast_server->qos_reconfigure = false; - unicast_server->cap_sink_stream.bap_stream.qos->pd = unicast_server->reconfigure_pd; - } else { - LOG_DBG("Set %s to PD: %d", unicast_server->ch_name, stream->qos->pd); - } } static void stream_enabled_cb(struct bt_bap_stream *stream) @@ -1221,21 +1065,22 @@ static void stream_started_cb(struct bt_bap_stream *stream) return; } - if (IS_ENABLED(CONFIG_BT_AUDIO_TX)) { - struct stream_index idx; + struct stream_index idx = {0}; - ret = device_index_get(stream->conn, &idx); - if (ret) { - LOG_ERR("Device index not found"); - } else { - ERR_CHK(bt_le_audio_tx_stream_started(idx)); - } + ret = stream_idx_get(stream, &idx); + if (ret) { + LOG_ERR("%s: Failed to get stream index: %d", __func__, ret); + return; + } + + if (IS_ENABLED(CONFIG_BT_AUDIO_TX)) { + ERR_CHK(bt_le_audio_tx_stream_started(bt_le_audio_tx, idx)); } /* NOTE: The string below is used by the Nordic CI system */ - LOG_INF("Stream %p started", (void *)stream); + LOG_INF("Stream %p started, idx: %d %d %d", (void *)stream, idx.lvl1, idx.lvl2, idx.lvl3); - le_audio_event_publish(LE_AUDIO_EVT_STREAMING, stream->conn, dir); + le_audio_event_publish(LE_AUDIO_EVT_STREAMING, stream->conn, stream, dir); } static void stream_metadata_updated_cb(struct bt_bap_stream *stream) @@ -1250,72 +1095,112 @@ static void stream_disabled_cb(struct bt_bap_stream *stream) static void stream_stopped_cb(struct bt_bap_stream *stream, uint8_t reason) { + int ret; + /* NOTE: The string below is used by the Nordic CI system */ LOG_INF("Stream %p stopped. Reason %d", (void *)stream, reason); + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return; + } + /* Check if the other streams are streaming, send event if not */ - for (int i = 0; i < CONFIG_BT_ISO_MAX_CIG; i++) { - for (int j = 0; j < ARRAY_SIZE(unicast_servers[i][0]); j++) { - if (le_audio_ep_state_check( - unicast_servers[i][0][j].cap_sink_stream.bap_stream.ep, - BT_BAP_EP_STATE_STREAMING) || - le_audio_ep_state_check( - unicast_servers[i][0][j].cap_source_stream.bap_stream.ep, - BT_BAP_EP_STATE_STREAMING)) { - return; - } - } + if (srv_store_all_ep_state_count(BT_BAP_EP_STATE_STREAMING, BT_AUDIO_DIR_SINK) || + srv_store_all_ep_state_count(BT_BAP_EP_STATE_STREAMING, BT_AUDIO_DIR_SOURCE)) { + LOG_DBG("Other streams are still streaming, not publishing NOT_STREAMING event"); + srv_store_unlock(); + return; + } + + srv_store_unlock(); + + enum bt_audio_dir dir = 0; + + dir = le_audio_stream_dir_get(stream); + if (dir <= 0) { + LOG_ERR("Failed to get dir of stream %p", (void *)stream); + } + + le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, stream->conn, stream, dir); +} + +/** + * @brief Function to check if all streams in the unicast group have been released. + * + * @param[in] stream Stream to check. + * @param[in] user_data User data, not used. + * + * @retval true A stream is found that is not yet released. + * @retval false All streams are released. + */ +static bool all_streams_released_check(struct bt_cap_stream *stream, void *user_data) +{ + ARG_UNUSED(user_data); + + if (stream->bap_stream.ep != NULL) { + LOG_DBG("stream %p is not released", stream); + /* Found a stream that is not released, will stop iterating */ + return false; } - le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, stream->conn, BT_AUDIO_DIR_SINK); + return true; } static void stream_released_cb(struct bt_bap_stream *stream) { + int ret; + LOG_DBG("Audio Stream %p released", (void *)stream); - /* Check if the other streams are streaming, send event if not */ - for (int i = 0; i < CONFIG_BT_ISO_MAX_CIG; i++) { - for (int j = 0; j < ARRAY_SIZE(unicast_servers[i][0]); j++) { - if (le_audio_ep_state_check( - unicast_servers[i][0][j].cap_sink_stream.bap_stream.ep, - BT_BAP_EP_STATE_STREAMING) || - le_audio_ep_state_check( - unicast_servers[i][0][j].cap_source_stream.bap_stream.ep, - BT_BAP_EP_STATE_STREAMING)) { - return; - } + /* Check if unicast_group_recreate has been requested */ + if (unicast_group_created == false) { + /* Check if all streams have been released, only delete group if they all are */ + ret = bt_cap_unicast_group_foreach_stream(unicast_group, all_streams_released_check, + NULL); + if (ret == -ECANCELED) { + LOG_DBG("Not all streams have been released, not deleting group"); + return; + } + + ret = bt_cap_unicast_group_delete(unicast_group); + if (ret) { + LOG_ERR("Failed to delete unicast group: %d", ret); } - } - le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, stream->conn, BT_AUDIO_DIR_SINK); + /* Create a new unicast group */ + k_work_submit(&cap_start_work); + } } #if (CONFIG_BT_AUDIO_RX) static void stream_recv_cb(struct bt_bap_stream *stream, const struct bt_iso_recv_info *info, - struct net_buf *buf) + struct net_buf *audio_frame) { int ret; - bool bad_frame = false; - struct stream_index idx; + struct audio_metadata meta; if (receive_cb == NULL) { LOG_ERR("The RX callback has not been set"); return; } - if (!(info->flags & BT_ISO_FLAGS_VALID)) { - bad_frame = true; + ret = le_audio_metadata_populate(&meta, stream, info, audio_frame); + if (ret) { + LOG_ERR("Failed to populate meta data: %d", ret); + return; } - ret = device_index_get(stream->conn, &idx); + struct stream_index idx; + + ret = stream_idx_get(stream, &idx); if (ret) { - LOG_ERR("Device index not found"); + LOG_ERR("%s: Failed to get stream index: %d", __func__, ret); return; } - receive_cb(buf->data, buf->len, bad_frame, info->ts, idx.lvl3, - bt_audio_codec_cfg_get_octets_per_frame(stream->codec_cfg)); + receive_cb(audio_frame, &meta, idx.lvl3); } #endif /* (CONFIG_BT_AUDIO_RX) */ @@ -1344,44 +1229,40 @@ static struct bt_bap_unicast_client_cb unicast_client_cbs = { .discover = discover_cb, }; -static void disconnected_cleanup(struct stream_index idx) -{ - struct le_audio_unicast_server *unicast_server = - &unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3]; - - unicast_server->device_conn = NULL; - unicast_server->sink_ep = NULL; - memset(unicast_server->sink_codec_cap, 0, sizeof(unicast_server->sink_codec_cap)); - unicast_server->source_ep = NULL; - memset(unicast_server->source_codec_cap, 0, sizeof(unicast_server->source_codec_cap)); - - unicast_server->num_sink_eps = 0; - unicast_server->num_source_eps = 0; -} - static void unicast_discovery_complete_cb(struct bt_conn *conn, int err, const struct bt_csip_set_coordinator_set_member *member, const struct bt_csip_set_coordinator_csis_inst *csis_inst) { int ret; struct le_audio_msg msg; - struct stream_index idx; + struct server_store *server = NULL; + + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return; + } - ret = device_index_get(conn, &idx); + ret = srv_store_from_conn_get(conn, &server); if (ret) { - LOG_ERR("Device not found"); + LOG_ERR("%s: Unknown connection, should not reach here", __func__); + srv_store_unlock(); return; } - if (err || csis_inst == NULL) { + if (err) { LOG_WRN("Got err: %d from conn: %p", err, (void *)conn); msg.set_size = 0; msg.sirk = NULL; + } else if (csis_inst == NULL) { + LOG_WRN("csis_inst is NULL from conn: %p", (void *)conn); + msg.set_size = 0; + msg.sirk = NULL; } else { LOG_DBG("\tErr: %d, set_size: %d", err, csis_inst->info.set_size); LOG_HEXDUMP_DBG(csis_inst->info.sirk, BT_CSIP_SIRK_SIZE, "\tSIRK:"); - unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3].member = member; + server->member = member; msg.set_size = csis_inst->info.set_size; msg.sirk = csis_inst->info.sirk; } @@ -1393,31 +1274,28 @@ static void unicast_discovery_complete_cb(struct bt_conn *conn, int err, ret = zbus_chan_pub(&le_audio_chan, &msg, LE_AUDIO_ZBUS_EVENT_WAIT_TIME); ERR_CHK(ret); + + srv_store_unlock(); } static void unicast_start_complete_cb(int err, struct bt_conn *conn) { - int ret; - struct stream_index idx; - - k_mutex_unlock(&mtx_cap_procedure_proceed); + k_sem_give(&sem_cap_procedure_proceed); if (err) { LOG_WRN("Failed start_complete for conn: %p, err: %d", (void *)conn, err); } LOG_DBG("Unicast start complete cb"); - ret = k_msgq_peek(&cap_start_msgq, &idx); - if (ret == 0) { - /* Pending start procedure found, call k_work */ - k_work_submit(&cap_start_work); - } else { - LOG_DBG("No outstanding work"); - } + playing_state = true; + + cap_proc_waiting_check(); } static void unicast_update_complete_cb(int err, struct bt_conn *conn) { + k_sem_give(&sem_cap_procedure_proceed); + if (err) { LOG_WRN("Failed update_complete for conn: %p, err: %d", (void *)conn, err); } @@ -1427,32 +1305,16 @@ static void unicast_update_complete_cb(int err, struct bt_conn *conn) static void unicast_stop_complete_cb(int err, struct bt_conn *conn) { - int ret; + k_sem_give(&sem_cap_procedure_proceed); if (err) { LOG_WRN("Failed stop_complete for conn: %p, err: %d", (void *)conn, err); } LOG_DBG("Unicast stop complete cb"); + playing_state = false; - /* Check for reconfigurable sink streams */ - for (int i = 0; i < CONFIG_BT_ISO_MAX_CIG; i++) { - for (int j = 0; j < ARRAY_SIZE(unicast_servers[i][0]); j++) { - if (unicast_servers[i][0][j].qos_reconfigure && playing_state) { - struct stream_index idx = { - .lvl1 = i, - .lvl2 = 0, - .lvl3 = j, - }; - ret = k_msgq_put(&cap_start_msgq, &idx, K_NO_WAIT); - if (ret) { - LOG_ERR("Failed to put device_index %d on the queue: %d", j, - ret); - } - k_work_submit(&cap_start_work); - } - } - } + cap_proc_waiting_check(); } static struct bt_cap_initiator_cb cap_cbs = { @@ -1462,308 +1324,622 @@ static struct bt_cap_initiator_cb cap_cbs = { .unicast_stop_complete = unicast_stop_complete_cb, }; -int unicast_client_config_get(struct bt_conn *conn, enum bt_audio_dir dir, uint32_t *bitrate, - uint32_t *sampling_rate_hz) +static bool first_source_location_get(struct bt_cap_stream *stream, void *user_data) { int ret; + enum bt_audio_dir dir; struct stream_index idx; - if (conn == NULL) { - LOG_ERR("No valid connection pointer received"); - return -EINVAL; + if (stream == NULL || user_data == NULL) { + LOG_ERR("Invalid parameters"); + return true; } - if (bitrate == NULL && sampling_rate_hz == NULL) { - LOG_ERR("No valid pointers received"); - return -ENXIO; - } + enum bt_audio_location *locations = (enum bt_audio_location *)user_data; + + dir = le_audio_stream_dir_get(&stream->bap_stream); - ret = device_index_get(conn, &idx); + ret = stream_idx_get(&stream->bap_stream, &idx); if (ret) { - LOG_WRN("No configured streams found"); + LOG_ERR("Failed to get stream index: %d", ret); return ret; } - struct le_audio_unicast_server *unicast_server = - &unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3]; + if ((dir != BT_AUDIO_DIR_SOURCE) || (idx.lvl1 != 0) || (idx.lvl2 != 0) || (idx.lvl3 != 0)) { + /* Not the first source stream, continue searching */ + return true; + } - if (dir == BT_AUDIO_DIR_SINK) { - if (unicast_server->cap_sink_stream.bap_stream.codec_cfg == NULL) { - LOG_ERR("No codec found for the stream"); + ret = bt_audio_codec_cfg_get_chan_allocation(stream->bap_stream.codec_cfg, locations, + false); + if (ret) { + LOG_WRN("Failed to get channel allocation"); + return ret; + } - return -ENXIO; - } + /* Found the first source stream, stop iterating */ + return false; +} - if (sampling_rate_hz != NULL) { - ret = le_audio_freq_hz_get( - unicast_server->cap_sink_stream.bap_stream.codec_cfg, - sampling_rate_hz); - if (ret) { - LOG_ERR("Invalid sampling frequency: %d", ret); - return -ENXIO; - } - } +int le_audio_concurrent_sync_num_get(uint8_t *num_streams, enum bt_audio_location *locations) +{ + int ret; - if (bitrate != NULL) { - ret = le_audio_bitrate_get( - unicast_server->cap_sink_stream.bap_stream.codec_cfg, bitrate); - if (ret) { - LOG_ERR("Unable to calculate bitrate: %d", ret); - return -ENXIO; - } - } - } else if (dir == BT_AUDIO_DIR_SOURCE) { - if (unicast_server->cap_source_stream.bap_stream.codec_cfg == NULL) { - LOG_ERR("No codec found for the stream"); + if (num_streams == NULL || locations == NULL) { + return -EINVAL; + } + + /* Only one stream supported at the moment */ + *num_streams = 1; + /* Get location of source stream with idx 0.0.0 */ + ret = bt_cap_unicast_group_foreach_stream(unicast_group, first_source_location_get, + locations); + if (ret != -ECANCELED) { + LOG_ERR("Failed to get source location: %d", ret); + return ret; + } + + return 0; +} + +int unicast_client_config_get(struct bt_bap_stream *stream, uint32_t *bitrate, + uint32_t *sampling_rate_hz) +{ + int ret; + + if (stream == NULL) { + LOG_ERR("No valid stream pointer received"); + return -EINVAL; + } + + if (bitrate == NULL && sampling_rate_hz == NULL) { + LOG_ERR("No valid pointers received"); + return -ENXIO; + } + + if (stream->codec_cfg == NULL) { + LOG_ERR("No codec found for the stream"); + return -ENXIO; + } + + if (sampling_rate_hz != NULL) { + ret = le_audio_freq_hz_get(stream->codec_cfg, sampling_rate_hz); + if (ret) { + LOG_ERR("Invalid sampling frequency: %d", ret); return -ENXIO; } + } - if (sampling_rate_hz != NULL) { - ret = le_audio_freq_hz_get( - unicast_server->cap_source_stream.bap_stream.codec_cfg, - sampling_rate_hz); - if (ret) { - LOG_ERR("Invalid sampling frequency: %d", ret); - return -ENXIO; - } + if (bitrate != NULL) { + ret = le_audio_bitrate_get(stream->codec_cfg, bitrate); + if (ret) { + LOG_ERR("Unable to calculate bitrate: %d", ret); + return -ENXIO; } + } - if (bitrate != NULL) { - ret = le_audio_bitrate_get( - unicast_server->cap_source_stream.bap_stream.codec_cfg, bitrate); - if (ret) { - LOG_ERR("Unable to calculate bitrate: %d", ret); - return -ENXIO; - } + return 0; +} + +/* Get the supported sink locations from all connected unicast servers, called once per server */ +static bool sink_locations_get(struct server_store *server, void *user_data) +{ + uint32_t *locations = (uint32_t *)user_data; + + *locations |= server->snk.locations; + + return true; +} + +/* Get the supported source locations from all connected unicast servers, called once per server */ +static bool source_locations_get(struct server_store *server, void *user_data) +{ + uint32_t *locations = (uint32_t *)user_data; + + *locations |= server->src.locations; + + return true; +} + +int unicast_client_locations_get(uint32_t *locations, enum bt_audio_dir dir) +{ + int ret; + + if (locations == NULL) { + return -EINVAL; + } + + *locations = 0; + + if (dir != BT_AUDIO_DIR_SINK && dir != BT_AUDIO_DIR_SOURCE) { + return -EINVAL; + } + + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return ret; + } + + if (dir == BT_AUDIO_DIR_SINK) { + ret = srv_store_foreach_server(sink_locations_get, locations); + if (ret) { + LOG_ERR("Failed to get locations: %d", ret); + srv_store_unlock(); + return ret; + } + } else if (dir == BT_AUDIO_DIR_SOURCE) { + ret = srv_store_foreach_server(source_locations_get, locations); + if (ret) { + LOG_ERR("Failed to get locations: %d", ret); + srv_store_unlock(); + return ret; } } + srv_store_unlock(); + return 0; } void unicast_client_conn_disconnected(struct bt_conn *conn) { int ret; - struct stream_index idx; - ret = device_index_get(conn, &idx); + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return; + } + + ret = srv_store_clear_by_conn(conn); if (ret) { - LOG_WRN("Unknown connection disconnected"); - } else { - disconnected_cleanup(idx); + LOG_ERR("Failed to clear server store for conn %p: %d", (void *)conn, ret); } + + srv_store_unlock(); } int unicast_client_discover(struct bt_conn *conn, enum unicast_discover_dir dir) { int ret; - struct stream_index idx; - ret = device_index_vacant_get(conn, &idx); + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return ret; + } + + struct server_store *server = NULL; + + ret = srv_store_from_conn_get(conn, &server); if (ret) { + LOG_ERR("%s: Unknown connection, should not reach here", __func__); + srv_store_unlock(); return ret; } - unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3].device_conn = conn; + /* Register ops */ + for (int i = 0; i < ARRAY_SIZE(server->snk.cap_streams); i++) { + bt_cap_stream_ops_register(&server->snk.cap_streams[i], &stream_ops); + } + + for (int i = 0; i < ARRAY_SIZE(server->src.cap_streams); i++) { + bt_cap_stream_ops_register(&server->src.cap_streams[i], &stream_ops); + } ret = bt_cap_initiator_unicast_discover(conn); if (ret) { LOG_WRN("Failed to start cap discover: %d", ret); + srv_store_unlock(); return ret; } if (dir & BT_AUDIO_DIR_SOURCE) { - unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3].waiting_for_source_disc = true; + server->src.waiting_for_disc = true; } if (dir & BT_AUDIO_DIR_SINK) { - unicast_servers[idx.lvl1][idx.lvl2][idx.lvl3].waiting_for_sink_disc = true; + server->snk.waiting_for_disc = true; } if (dir == UNICAST_SERVER_BIDIR) { /* If we need to discover both source and sink, do sink first */ ret = bt_bap_unicast_client_discover(conn, BT_AUDIO_DIR_SINK); + srv_store_unlock(); return ret; } ret = bt_bap_unicast_client_discover(conn, dir); - return ret; + if (ret) { + LOG_WRN("Failed to discover %d", ret); + srv_store_unlock(); + return ret; + } + + srv_store_unlock(); + + return 0; } -int unicast_client_start(uint8_t cig_index) +static bool is_connected(struct bt_conn const *const conn) { int ret; - struct bt_cap_unicast_audio_start_stream_param - cap_stream_params[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT + - CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT]; - static struct bt_cap_unicast_audio_start_param param; - if (cig_index >= CONFIG_BT_ISO_MAX_CIG) { - LOG_ERR("Trying to start CIG %d out of %d", cig_index, CONFIG_BT_ISO_MAX_CIG); - return -EINVAL; + /* Check if the connection is valid */ + struct bt_conn_info info; + + if (conn == NULL) { + LOG_DBG("Connection is NULL, not connected"); + return false; } - param.stream_params = cap_stream_params; - param.count = 0; - param.type = BT_CAP_SET_TYPE_AD_HOC; + ret = bt_conn_get_info(conn, &info); + if (ret) { + LOG_ERR("Failed to get connection info for conn %p: %d", (void *)conn, ret); + return false; + } + + if (info.state == BT_CONN_STATE_CONNECTED) { + LOG_DBG("Connection %p is connected", (void *)conn); + return true; + } + + return false; +} + +static bool add_to_start_params(struct server_store *server, void *user_data) +{ + int ret; + struct bt_cap_unicast_audio_start_param *param = user_data; + + if (!is_connected(server->conn)) { + LOG_DBG("Server %s is not connected, skipping", server->name); + return true; + } - for (int i = 0; i < ARRAY_SIZE(unicast_servers[cig_index][0]); i++) { + for (int j = 0; j < MIN(server->snk.num_eps, POPCOUNT_ZERO(server->snk.locations)); j++) { uint8_t state; - ret = le_audio_ep_state_get(unicast_servers[cig_index][0][i].sink_ep, &state); - if (ret) { + ret = le_audio_ep_state_get(server->snk.eps[j], &state); + if (state == BT_BAP_EP_STATE_STREAMING || ret) { + LOG_DBG("Sink endpoint is already streaming, skipping start"); continue; } - if (state == BT_BAP_EP_STATE_IDLE) { - /* Start all streams in the configured state */ - cap_stream_params[param.count].member.member = - unicast_servers[cig_index][0][i].device_conn; - cap_stream_params[param.count].stream = - &unicast_servers[cig_index][0][i].cap_sink_stream; - cap_stream_params[param.count].ep = - unicast_servers[cig_index][0][i].sink_ep; - cap_stream_params[param.count].codec_cfg = &lc3_preset_sink.codec_cfg; - param.count++; - } else { - LOG_WRN("Found unicast_server[%d] with an endpoint not in IDLE state: %d", - i, state); - } + param->stream_params[param->count].member.member = server->conn; + param->stream_params[param->count].stream = &server->snk.cap_streams[j]; + param->stream_params[param->count].ep = server->snk.eps[j]; + param->stream_params[param->count].codec_cfg = &server->snk.lc3_preset[j].codec_cfg; + param->count++; + } - ret = le_audio_ep_state_get(unicast_servers[cig_index][0][i].source_ep, &state); - if (ret) { + /* Check if we have valid source endpoints to start */ + for (int j = 0; j < MIN(server->src.num_eps, POPCOUNT_ZERO(server->src.locations)); j++) { + uint8_t state; + + ret = le_audio_ep_state_get(server->src.eps[j], &state); + if (state == BT_BAP_EP_STATE_STREAMING || ret) { + LOG_DBG("Source endpoint is already streaming, skipping start"); continue; } - if (state == BT_BAP_EP_STATE_IDLE) { - /* Start all streams in the configured state */ - cap_stream_params[param.count].member.member = - unicast_servers[cig_index][0][i].device_conn; - cap_stream_params[param.count].stream = - &unicast_servers[cig_index][0][i].cap_source_stream; - cap_stream_params[param.count].ep = - unicast_servers[cig_index][0][i].source_ep; - cap_stream_params[param.count].codec_cfg = &lc3_preset_source.codec_cfg; - param.count++; - } else { - LOG_WRN("Found unicast_server[%d] with an endpoint not in IDLE state: %d", - i, state); - } + param->stream_params[param->count].member.member = server->conn; + param->stream_params[param->count].stream = &server->src.cap_streams[j]; + param->stream_params[param->count].ep = server->src.eps[j]; + param->stream_params[param->count].codec_cfg = &server->src.lc3_preset[j].codec_cfg; + param->count++; } - if (param.count > 0) { - ret = bt_cap_initiator_unicast_audio_start(¶m); + return true; +} + +int unicast_client_start(uint8_t cig_index) +{ + int ret; + + ret = k_sem_take(&sem_cap_procedure_proceed, K_NO_WAIT); + if (ret == -EBUSY) { + /* Ongoing procedure, try again later */ + enum cap_procedure_type proc_type; + + proc_type = CAP_PROCEDURE_START; + + ret = k_msgq_put(&cap_proc_q, &proc_type, K_NO_WAIT); if (ret) { - LOG_ERR("Failed to start unicast audio: %d", ret); - return ret; + LOG_WRN("Failed to put start procedure in queue: %d", ret); } - playing_state = true; - } else { + return ret; + } else if (ret) { + LOG_ERR("Failed to take sem_cap_procedure_proceed: %d", ret); + return ret; + } + + if (unicast_group == NULL) { + LOG_WRN("No unicast group to start"); + k_sem_give(&sem_cap_procedure_proceed); + return -EIO; + } + + /* Start all unicast_servers with valid endpoints */ + struct bt_cap_unicast_audio_start_stream_param + cap_stream_params[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT + + CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT]; + + struct bt_cap_unicast_audio_start_param param; + + param.stream_params = cap_stream_params; + param.count = 0; + param.type = BT_CAP_SET_TYPE_AD_HOC; + + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return ret; + } + + ret = srv_store_foreach_server(add_to_start_params, ¶m); + if (ret) { + LOG_ERR("Failed to add streams to start params: %d", ret); + k_sem_give(&sem_cap_procedure_proceed); + srv_store_unlock(); + return ret; + } + + if (param.count == 0) { + LOG_DBG("No streams to start"); + k_sem_give(&sem_cap_procedure_proceed); + srv_store_unlock(); return -EIO; } + ret = bt_cap_initiator_unicast_audio_start(¶m); + if (ret) { + LOG_ERR("Failed to start unicast sink audio: %d", ret); + k_sem_give(&sem_cap_procedure_proceed); + srv_store_unlock(); + return ret; + } + + srv_store_unlock(); + return 0; } +static bool add_to_stop_params(struct bt_cap_stream *stream, void *user_data) +{ + struct bt_cap_unicast_audio_stop_param *param = user_data; + + if (stream->bap_stream.ep == NULL) { + /* Stream already released */ + return true; + } + + param->streams[param->count++] = stream; + + return true; +} + +static bool server_connected_check(struct bt_cap_stream *stream, void *user_data) +{ + int ret; + struct server_store *server = NULL; + bool *connected_server_found = (bool *)user_data; + + ret = srv_store_from_stream_get(&stream->bap_stream, &server); + if (ret) { + LOG_ERR("Failed to get server from stream: %d", ret); + return true; + } + + if (server && is_connected(server->conn)) { + *connected_server_found = true; + /* Found a connected server, will stop iterating */ + return false; + } + + return true; +} + int unicast_client_stop(uint8_t cig_index) { int ret; - struct bt_cap_stream *streams[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT + - CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT]; + struct bt_cap_stream *streams[(CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT + + CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT) * + CONFIG_BT_MAX_CONN]; static struct bt_cap_unicast_audio_stop_param param; + ret = k_sem_take(&sem_cap_procedure_proceed, K_NO_WAIT); + if (ret == -EBUSY) { + enum cap_procedure_type proc_type; + + proc_type = CAP_PROCEDURE_STOP; + + ret = k_msgq_put(&cap_proc_q, &proc_type, K_NO_WAIT); + if (ret) { + LOG_WRN("Failed to put stop procedure in queue: %d", ret); + } + + return ret; + + } else if (ret) { + LOG_ERR("Failed to take sem_cap_procedure_proceed: %d", ret); + return ret; + } + if (cig_index >= CONFIG_BT_ISO_MAX_CIG) { LOG_ERR("Trying to stop CIG %d out of %d", cig_index, CONFIG_BT_ISO_MAX_CIG); return -EINVAL; } + if (unicast_group == NULL) { + LOG_WRN("No unicast group to stop"); + k_sem_give(&sem_cap_procedure_proceed); + return -EIO; + } + param.streams = streams; param.count = 0; param.type = BT_CAP_SET_TYPE_AD_HOC; param.release = true; - le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, NULL, 0); + ret = bt_cap_unicast_group_foreach_stream(unicast_group, add_to_stop_params, ¶m); + if (ret) { + LOG_ERR("Failed to add streams to stop params: %d", ret); + k_sem_give(&sem_cap_procedure_proceed); + return ret; + } - for (int i = 0; i < ARRAY_SIZE(unicast_servers[cig_index][0]); i++) { - if (le_audio_ep_state_check(unicast_servers[cig_index][0][i].sink_ep, - BT_BAP_EP_STATE_STREAMING)) { - /* Stop all sink streams currently in a streaming state */ - streams[param.count++] = &unicast_servers[cig_index][0][i].cap_sink_stream; - } + if (param.count == 0) { + LOG_DBG("No streams to stop"); + k_sem_give(&sem_cap_procedure_proceed); - if (le_audio_ep_state_check(unicast_servers[cig_index][0][i].source_ep, - BT_BAP_EP_STATE_STREAMING)) { - /* Stop all source streams currently in a streaming state */ - streams[param.count++] = - &unicast_servers[cig_index][0][i].cap_source_stream; + /* No streams found. Check if devices are connected, if no, delete the group */ + bool connected_server_found = false; + + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return ret; } - } - if (param.count > 0) { - ret = bt_cap_initiator_unicast_audio_stop(¶m); - if (ret) { - LOG_ERR("Failed to stop unicast audio: %d", ret); + ret = bt_cap_unicast_group_foreach_stream(unicast_group, server_connected_check, + &connected_server_found); + + if (ret == -ECANCELED) { + /* If cancelled a connected server has been found */ + srv_store_unlock(); + return ret; + } else if (ret) { + LOG_ERR("Failed to check if servers are connected: %d", ret); + srv_store_unlock(); return ret; } - playing_state = false; - } else { + srv_store_unlock(); + + if (!connected_server_found) { + LOG_DBG("No connected servers found, deleting unicast group"); + ret = bt_cap_unicast_group_delete(unicast_group); + if (ret) { + LOG_ERR("Failed to delete unicast group: %d", ret); + } + + unicast_group = NULL; + unicast_group_created = false; + + return -EAGAIN; + } + return -EIO; } + ret = bt_cap_initiator_unicast_audio_stop(¶m); + if (ret) { + LOG_ERR("Failed to stop unicast audio: %d", ret); + k_sem_give(&sem_cap_procedure_proceed); + return ret; + } + return 0; } -int unicast_client_send(uint8_t cig_index, struct le_audio_encoded_audio enc_audio) +struct unicast_send_info { + struct le_audio_tx_info *tx; + uint8_t num_active_streams; +}; + +static bool unicast_send_info_populate(struct server_store *server, void *user_data) { -#if (CONFIG_BT_AUDIO_TX) int ret; - uint8_t num_active_streams = 0; - - if (cig_index >= CONFIG_BT_ISO_MAX_CIG) { - LOG_ERR("Trying to send to CIG %d out of %d", cig_index, CONFIG_BT_ISO_MAX_CIG); - return -EINVAL; - } - - struct le_audio_tx_info tx[ARRAY_SIZE(unicast_servers[cig_index][0])]; + struct unicast_send_info *info = (struct unicast_send_info *)user_data; - for (int i = 0; i < ARRAY_SIZE(unicast_servers[cig_index][0]); i++) { + for (int i = 0; i < server->snk.num_eps; i++) { /* Skip unicast_servers not in a streaming state */ - if (!le_audio_ep_state_check( - unicast_servers[cig_index][0][i].cap_sink_stream.bap_stream.ep, - BT_BAP_EP_STATE_STREAMING)) { + if (!le_audio_ep_state_check(server->snk.cap_streams[i].bap_stream.ep, + BT_BAP_EP_STATE_STREAMING)) { continue; } /* Set cap stream pointer */ - tx[num_active_streams].cap_stream = - &unicast_servers[cig_index][0][i].cap_sink_stream; + info->tx[info->num_active_streams].cap_stream = &server->snk.cap_streams[i]; /* Set index */ - tx[num_active_streams].idx.lvl1 = cig_index; - tx[num_active_streams].idx.lvl2 = 0; - tx[num_active_streams].idx.lvl3 = i; + ret = stream_idx_get(&server->snk.cap_streams[i].bap_stream, + &info->tx[info->num_active_streams].idx); + if (ret) { + LOG_ERR("%s: Failed to get stream index: %d", __func__, ret); + return false; + } + + const uint8_t *loc; + + ret = bt_audio_codec_cfg_get_val(server->snk.cap_streams[i].bap_stream.codec_cfg, + BT_AUDIO_CODEC_CFG_CHAN_ALLOC, &loc); + if (ret < 0) { + LOG_ERR("Failed to get channel allocation: %d", ret); + return false; + } /* Set channel location */ /* Both mono and left unicast_servers will receive left channel */ - tx[num_active_streams].audio_channel = - (unicast_servers[cig_index][0][i].location == BT_AUDIO_LOCATION_FRONT_RIGHT) - ? AUDIO_CH_R - : AUDIO_CH_L; + info->tx[info->num_active_streams].audio_location = + *loc == BT_AUDIO_LOCATION_FRONT_RIGHT ? AUDIO_CH_R : AUDIO_CH_L; - num_active_streams++; + info->num_active_streams++; } - if (num_active_streams == 0) { - LOG_WRN("No active streams"); + return true; +} + +int unicast_client_send(struct net_buf const *const audio_frame, uint8_t cig_index) +{ +#if (CONFIG_BT_AUDIO_TX) + int ret; + + if (cig_index >= CONFIG_BT_ISO_MAX_CIG) { + LOG_ERR("Trying to send to CIG %d out of %d", cig_index, CONFIG_BT_ISO_MAX_CIG); + return -EINVAL; + } + + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return ret; + } + + uint8_t num_streaming = + srv_store_all_ep_state_count(BT_BAP_EP_STATE_STREAMING, BT_AUDIO_DIR_SINK); + struct le_audio_tx_info tx[num_streaming]; + + struct unicast_send_info info = { + .tx = tx, + .num_active_streams = 0, + }; + + /* Populate tx struct */ + ret = srv_store_foreach_server(unicast_send_info_populate, &info); + if (ret) { + LOG_ERR("Failed to populate send info: %d", ret); + srv_store_unlock(); + return ret; + } + + if (info.num_active_streams == 0) { + /* This could happen if we had a sink + source and just the sink disconnected */ + LOG_DBG("No active streams"); + srv_store_unlock(); return -ECANCELED; } - ret = bt_le_audio_tx_send(tx, num_active_streams, enc_audio); + ret = bt_le_audio_tx_send(bt_le_audio_tx, audio_frame, info.tx, info.num_active_streams); if (ret) { + srv_store_unlock(); return ret; } + + srv_store_unlock(); #endif /* (CONFIG_BT_AUDIO_TX) */ return 0; @@ -1781,47 +1957,58 @@ int unicast_client_enable(uint8_t cig_index, le_audio_receive_cb recv_cb) int ret; static bool initialized; - if (cig_index >= CONFIG_BT_ISO_MAX_CIG) { - LOG_ERR("Trying to enable CIG %d out of %d", cig_index, CONFIG_BT_ISO_MAX_CIG); - return -EINVAL; - } - if (initialized) { LOG_WRN("Already initialized"); return -EALREADY; } + ret = srv_store_lock(CAP_PROCED_SEM_WAIT_TIME_MS); + if (ret < 0) { + LOG_ERR("%s: Failed to lock server store: %d", __func__, ret); + return ret; + } + + ret = srv_store_init(); + if (ret) { + srv_store_unlock(); + return ret; + } + if (recv_cb == NULL) { LOG_ERR("Receive callback is NULL"); + srv_store_unlock(); return -EINVAL; } receive_cb = recv_cb; - for (int i = 0; i < ARRAY_SIZE(unicast_servers[cig_index][0]); i++) { - bt_cap_stream_ops_register(&unicast_servers[cig_index][0][i].cap_sink_stream, - &stream_ops); - bt_cap_stream_ops_register(&unicast_servers[cig_index][0][i].cap_source_stream, - &stream_ops); - } - ret = bt_bap_unicast_client_register_cb(&unicast_client_cbs); if (ret) { LOG_ERR("Failed to register client callbacks: %d", ret); + srv_store_unlock(); return ret; } ret = bt_cap_initiator_register_cb(&cap_cbs); if (ret) { LOG_ERR("Failed to register cap callbacks: %d", ret); + srv_store_unlock(); return ret; } if (IS_ENABLED(CONFIG_BT_AUDIO_TX)) { - bt_le_audio_tx_init(); + /* A unicast client is the Bluetooth central device */ + ret = bt_le_audio_tx_init(bt_le_audio_tx, true); + if (ret) { + LOG_ERR("Failed to initialize LE Audio TX: %d", ret); + srv_store_unlock(); + return ret; + } } initialized = true; + srv_store_unlock(); + return 0; } diff --git a/src/bluetooth/bt_stream/unicast/unicast_client.h b/src/bluetooth/bt_stream/unicast/unicast_client.h index 834d19cd..587fc140 100644 --- a/src/bluetooth/bt_stream/unicast/unicast_client.h +++ b/src/bluetooth/bt_stream/unicast/unicast_client.h @@ -4,6 +4,14 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @addtogroup audio_app_bt_stream + * @{ + * @defgroup unicast_client Functions for unicast client functionality. + * @{ + * @brief Helper functions to manage unicast client (gateway side) functionality. + */ + #ifndef _UNICAST_CLIENT_H_ #define _UNICAST_CLIENT_H_ @@ -12,6 +20,9 @@ #include #include +/** + * @brief Unicast discovery direction enumeration. + */ enum unicast_discover_dir { UNICAST_SERVER_SINK = BT_AUDIO_DIR_SINK, UNICAST_SERVER_SOURCE = BT_AUDIO_DIR_SOURCE, @@ -19,38 +30,38 @@ enum unicast_discover_dir { }; #if CONFIG_BT_BAP_UNICAST_CONFIGURABLE -#define BT_BAP_LC3_UNICAST_PRESET_NRF5340_AUDIO_SINK \ +#define BT_BAP_LC3_UNICAST_PRESET_NRF_AUDIO_SINK \ BT_BAP_LC3_PRESET_CONFIGURABLE(BT_AUDIO_LOCATION_FRONT_LEFT, \ BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED, \ CONFIG_BT_AUDIO_BITRATE_UNICAST_SINK) -#define BT_BAP_LC3_UNICAST_PRESET_NRF5340_AUDIO_SOURCE \ +#define BT_BAP_LC3_UNICAST_PRESET_NRF_AUDIO_SOURCE \ BT_BAP_LC3_PRESET_CONFIGURABLE(BT_AUDIO_LOCATION_FRONT_LEFT, \ BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED, \ CONFIG_BT_AUDIO_BITRATE_UNICAST_SRC) #elif CONFIG_BT_BAP_UNICAST_16_2_1 -#define BT_BAP_LC3_UNICAST_PRESET_NRF5340_AUDIO_SINK \ +#define BT_BAP_LC3_UNICAST_PRESET_NRF_AUDIO_SINK \ BT_BAP_LC3_UNICAST_PRESET_16_2_1(BT_AUDIO_LOCATION_FRONT_LEFT, \ BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED) -#define BT_BAP_LC3_UNICAST_PRESET_NRF5340_AUDIO_SOURCE \ +#define BT_BAP_LC3_UNICAST_PRESET_NRF_AUDIO_SOURCE \ BT_BAP_LC3_UNICAST_PRESET_16_2_1(BT_AUDIO_LOCATION_FRONT_LEFT, \ BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED) #elif CONFIG_BT_BAP_UNICAST_24_2_1 -#define BT_BAP_LC3_UNICAST_PRESET_NRF5340_AUDIO_SINK \ +#define BT_BAP_LC3_UNICAST_PRESET_NRF_AUDIO_SINK \ BT_BAP_LC3_UNICAST_PRESET_24_2_1(BT_AUDIO_LOCATION_FRONT_LEFT, \ BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED) -#define BT_BAP_LC3_UNICAST_PRESET_NRF5340_AUDIO_SOURCE \ +#define BT_BAP_LC3_UNICAST_PRESET_NRF_AUDIO_SOURCE \ BT_BAP_LC3_UNICAST_PRESET_24_2_1(BT_AUDIO_LOCATION_FRONT_LEFT, \ BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED) #elif CONFIG_BT_BAP_UNICAST_48_4_1 -#define BT_BAP_LC3_UNICAST_PRESET_NRF5340_AUDIO_SINK \ +#define BT_BAP_LC3_UNICAST_PRESET_NRF_AUDIO_SINK \ BT_BAP_LC3_UNICAST_PRESET_48_4_1(BT_AUDIO_LOCATION_ANY, BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED) -#define BT_BAP_LC3_UNICAST_PRESET_NRF5340_AUDIO_SOURCE \ +#define BT_BAP_LC3_UNICAST_PRESET_NRF_AUDIO_SOURCE \ BT_BAP_LC3_UNICAST_PRESET_48_4_1(BT_AUDIO_LOCATION_ANY, BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED) #else #error Unsupported LC3 codec preset for unicast @@ -59,16 +70,25 @@ enum unicast_discover_dir { /** * @brief Get configuration for the audio stream. * - * @param[in] conn Pointer to the connection to get the configuration for. - * @param[in] dir Direction to get the configuration from. + * @param[in] stream Pointer to the stream to get the configuration for. * @param[out] bitrate Pointer to the bit rate used; can be NULL. * @param[out] sampling_rate_hz Pointer to the sampling rate used; can be NULL. * * @return 0 for success, error otherwise. */ -int unicast_client_config_get(struct bt_conn *conn, enum bt_audio_dir dir, uint32_t *bitrate, +int unicast_client_config_get(struct bt_bap_stream *stream, uint32_t *bitrate, uint32_t *sampling_rate_hz); +/** + * @brief Get the current audio locations used by the unicast client. + * + * @param[out] locations Pointer to store the audio locations bitmask. + * @param[in] dir Direction of the streams in question. + * + * @return 0 for success, error otherwise. + */ +int unicast_client_locations_get(uint32_t *locations, enum bt_audio_dir dir); + /** * @brief Start service discovery for a Bluetooth LE Audio unicast (CIS) server. * @@ -113,12 +133,12 @@ int unicast_client_stop(uint8_t cig_index); /** * @brief Send encoded audio using the Bluetooth LE Audio unicast. * + * @param[in] audio_frame Pointer to the audio to send. * @param[in] cig_index Index of the Connected Isochronous Group (CIG) to send to. - * @param[in] enc_audio Encoded audio struct. * * @return 0 for success, error otherwise. */ -int unicast_client_send(uint8_t cig_index, struct le_audio_encoded_audio enc_audio); +int unicast_client_send(struct net_buf const *const audio_frame, uint8_t cig_index); /** * @brief Disable the Bluetooth LE Audio unicast (CIS) client. @@ -139,4 +159,9 @@ int unicast_client_disable(uint8_t cig_index); */ int unicast_client_enable(uint8_t cig_index, le_audio_receive_cb recv_cb); +/** + * @} + * @} + */ + #endif /* _UNICAST_CLIENT_H_ */ diff --git a/src/bluetooth/bt_stream/unicast/unicast_server.c b/src/bluetooth/bt_stream/unicast/unicast_server.c index cd01b88c..2ed30f3a 100644 --- a/src/bluetooth/bt_stream/unicast/unicast_server.c +++ b/src/bluetooth/bt_stream/unicast/unicast_server.c @@ -4,828 +4,904 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ - #include "unicast_server.h" - - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - - #include "macros_common.h" - #include "zbus_common.h" - #include "bt_mgmt.h" - #include "bt_le_audio_tx.h" - #include "channel_assignment.h" - #include "le_audio.h" - #include "uicr.h" - -#include "BootState.h" - - #include - LOG_MODULE_REGISTER(unicast_server, CONFIG_UNICAST_SERVER_LOG_LEVEL); - - ZBUS_CHAN_DEFINE(le_audio_chan, struct le_audio_msg, NULL, NULL, ZBUS_OBSERVERS_EMPTY, - ZBUS_MSG_INIT(0)); - - #define BLE_ISO_LATENCY_MS 10 - #define CSIP_SET_SIZE 2 - enum csip_set_rank { - CSIP_HL_RANK = 1, - CSIP_HR_RANK = 2 - }; - - static le_audio_receive_cb receive_cb; - static struct bt_csip_set_member_svc_inst *csip; - - /* Advertising data for peer connection */ - static uint8_t csip_rsi_adv_data[BT_CSIP_RSI_SIZE]; - - static uint8_t flags_adv_data; - - static uint8_t gap_appear_adv_data[BT_UUID_SIZE_16]; - - static const uint8_t cap_adv_data[] = { - BT_UUID_16_ENCODE(BT_UUID_CAS_VAL), - BT_AUDIO_UNICAST_ANNOUNCEMENT_TARGETED, - }; - - #if defined(CONFIG_BT_AUDIO_RX) - #define AVAILABLE_SINK_CONTEXT (BT_AUDIO_CONTEXT_TYPE_ANY) - #else - #define AVAILABLE_SINK_CONTEXT BT_AUDIO_CONTEXT_TYPE_PROHIBITED - #endif /* CONFIG_BT_AUDIO_RX */ - - static struct bt_cap_stream *cap_tx_streams[CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT]; - - #if defined(CONFIG_BT_AUDIO_TX) - #define AVAILABLE_SOURCE_CONTEXT (BT_AUDIO_CONTEXT_TYPE_ANY) - #else - #define AVAILABLE_SOURCE_CONTEXT BT_AUDIO_CONTEXT_TYPE_PROHIBITED - #endif /* CONFIG_BT_AUDIO_TX */ - - static struct bt_bap_unicast_server_register_param unicast_server_params = { - CONFIG_BT_ASCS_MAX_ASE_SNK_COUNT, CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT}; - - static uint8_t unicast_server_adv_data[] = { - BT_UUID_16_ENCODE(BT_UUID_ASCS_VAL), - BT_AUDIO_UNICAST_ANNOUNCEMENT_TARGETED, - BT_BYTES_LIST_LE16(AVAILABLE_SINK_CONTEXT), - BT_BYTES_LIST_LE16(AVAILABLE_SOURCE_CONTEXT), - 0x00, /* Metadata length */ - }; - -static uint8_t device_identifier[] = { - BT_BYTES_LIST_LE64(0x0), - BT_BYTES_LIST_LE16(0x0), +#include "unicast_server.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "macros_common.h" +#include "zbus_common.h" +#include "bt_mgmt.h" +#include "bt_le_audio_tx.h" +#include "le_audio.h" +#include "audio_sdu_timing.h" + +#include +LOG_MODULE_REGISTER(unicast_server, CONFIG_UNICAST_SERVER_LOG_LEVEL); + +BT_LE_AUDIO_TX_DEFINE(bt_le_audio_tx); + +ZBUS_CHAN_DEFINE(le_audio_chan, struct le_audio_msg, NULL, NULL, ZBUS_OBSERVERS_EMPTY, + ZBUS_MSG_INIT(0)); + +#define BLE_ISO_LATENCY_MS 10 +#define CSIP_SET_SIZE 2 +enum csip_set_rank { + CSIP_HL_RANK = 1, + CSIP_HR_RANK = 2 +}; + +static le_audio_receive_cb receive_cb; +static struct bt_csip_set_member_svc_inst *csip; +static uint8_t concurrent_sink_streams_num; + +/* Advertising data for peer connection */ +static uint8_t csip_rsi_adv_data[BT_CSIP_RSI_SIZE]; + +static uint8_t flags_adv_data; + +static uint8_t gap_appear_adv_data[BT_UUID_SIZE_16]; + +static const uint8_t cap_adv_data[] = { + BT_UUID_16_ENCODE(BT_UUID_CAS_VAL), + BT_AUDIO_UNICAST_ANNOUNCEMENT_TARGETED, +}; + +#if defined(CONFIG_BT_AUDIO_RX) +#define AVAILABLE_SINK_CONTEXT (BT_AUDIO_CONTEXT_TYPE_ANY) +#else +#define AVAILABLE_SINK_CONTEXT BT_AUDIO_CONTEXT_TYPE_NONE +#endif /* CONFIG_BT_AUDIO_RX */ + +static struct bt_cap_stream *cap_tx_streams[CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT]; + +#if defined(CONFIG_BT_AUDIO_TX) +#define AVAILABLE_SOURCE_CONTEXT (BT_AUDIO_CONTEXT_TYPE_ANY) +#else +#define AVAILABLE_SOURCE_CONTEXT BT_AUDIO_CONTEXT_TYPE_NONE +#endif /* CONFIG_BT_AUDIO_TX */ + +static struct bt_bap_unicast_server_register_param unicast_server_params = {0, 0}; + +static uint8_t unicast_server_adv_data[] = { + BT_UUID_16_ENCODE(BT_UUID_ASCS_VAL), + BT_AUDIO_UNICAST_ANNOUNCEMENT_TARGETED, + BT_BYTES_LIST_LE16(AVAILABLE_SINK_CONTEXT), + BT_BYTES_LIST_LE16(AVAILABLE_SOURCE_CONTEXT), 0x00, /* Metadata length */ }; - - static void le_audio_event_publish(enum le_audio_evt_type event, struct bt_conn *conn, - enum bt_audio_dir dir) - { - int ret; - struct le_audio_msg msg; - - msg.event = event; - msg.conn = conn; - msg.dir = dir; - - ret = zbus_chan_pub(&le_audio_chan, &msg, LE_AUDIO_ZBUS_EVENT_WAIT_TIME); - ERR_CHK(ret); - } - - /* Callback for locking state change from server side */ -static void csip_lock_changed_cb(struct bt_conn *conn, struct bt_csip_set_member_svc_inst *csip_instance, - bool locked) + +static void le_audio_event_publish(enum le_audio_evt_type event, struct bt_conn *conn, + enum bt_audio_dir dir) { - ARG_UNUSED(csip_instance); - LOG_DBG("Client %p %s the lock", (void *)conn, locked ? "locked" : "released"); - } - - /* Callback for SIRK read request from peer side */ -static uint8_t sirk_read_req_cb(struct bt_conn *conn, struct bt_csip_set_member_svc_inst *csip_instance) + int ret; + struct le_audio_msg msg; + + msg.event = event; + msg.conn = conn; + msg.dir = dir; + + ret = zbus_chan_pub(&le_audio_chan, &msg, LE_AUDIO_ZBUS_EVENT_WAIT_TIME); + ERR_CHK(ret); +} + +/* Callback for locking state change from server side */ +static void csip_lock_changed_cb(struct bt_conn *conn, struct bt_csip_set_member_svc_inst *csip, + bool locked) { - ARG_UNUSED(conn); - ARG_UNUSED(csip_instance); - /* Accept the request to read the SIRK, but return encrypted SIRK instead of plaintext */ - return BT_CSIP_READ_SIRK_REQ_RSP_ACCEPT_ENC; - } - - static struct bt_csip_set_member_cb csip_callbacks = { - .lock_changed = csip_lock_changed_cb, - .sirk_read_req = sirk_read_req_cb, - }; - struct bt_csip_set_member_register_param csip_param = { - .set_size = CSIP_SET_SIZE, - .lockable = true, - .cb = &csip_callbacks, - }; - - #if defined(CONFIG_BT_AUDIO_RX) - static struct bt_audio_codec_cap lc3_codec_sink = BT_AUDIO_CODEC_CAP_LC3( - BT_AUDIO_CODEC_CAPABILIY_FREQ, - (BT_AUDIO_CODEC_CAP_DURATION_10 | BT_AUDIO_CODEC_CAP_DURATION_PREFER_10), - BT_AUDIO_CODEC_CAP_CHAN_COUNT_SUPPORT(1), LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MIN), - LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX), 1u, AVAILABLE_SINK_CONTEXT); - #endif /* (CONFIG_BT_AUDIO_RX) */ - - #if defined(CONFIG_BT_AUDIO_TX) - static struct bt_audio_codec_cap lc3_codec_source = BT_AUDIO_CODEC_CAP_LC3( - BT_AUDIO_CODEC_CAPABILIY_FREQ, - (BT_AUDIO_CODEC_CAP_DURATION_10 | BT_AUDIO_CODEC_CAP_DURATION_PREFER_10), - BT_AUDIO_CODEC_CAP_CHAN_COUNT_SUPPORT(1), LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MIN), - LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX), 1u, AVAILABLE_SOURCE_CONTEXT); - #endif /* (CONFIG_BT_AUDIO_TX) */ - - static enum bt_audio_dir caps_dirs[] = { - #if defined(CONFIG_BT_AUDIO_RX) - BT_AUDIO_DIR_SINK, - #endif /* CONFIG_BT_AUDIO_RX */ - #if defined(CONFIG_BT_AUDIO_TX) - BT_AUDIO_DIR_SOURCE, - #endif /* (CONFIG_BT_AUDIO_TX) */ - }; - - static const struct bt_bap_qos_cfg_pref qos_pref = BT_BAP_QOS_CFG_PREF( - true, BT_GAP_LE_PHY_2M, CONFIG_BT_AUDIO_RETRANSMITS, BLE_ISO_LATENCY_MS, - CONFIG_AUDIO_MIN_PRES_DLY_US, CONFIG_AUDIO_MAX_PRES_DLY_US, - CONFIG_BT_AUDIO_PREFERRED_MIN_PRES_DLY_US, CONFIG_BT_AUDIO_PREFERRED_MAX_PRES_DLY_US); - - /* clang-format off */ - static struct bt_pacs_cap caps[] = { - #if (CONFIG_BT_AUDIO_RX) - { - .codec_cap = &lc3_codec_sink, - }, - #endif - #if (CONFIG_BT_AUDIO_TX) - { - .codec_cap = &lc3_codec_source, - } - #endif /* (CONFIG_BT_AUDIO_TX) */ - }; - /* clang-format on */ - - static struct bt_cap_stream - cap_audio_streams[CONFIG_BT_ASCS_MAX_ASE_SNK_COUNT + CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT]; - - #if (CONFIG_BT_AUDIO_TX) - BUILD_ASSERT(CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT <= 1, - "CIS headset only supports one source stream for now"); - #endif /* (CONFIG_BT_AUDIO_TX) */ - - static int lc3_config_cb(struct bt_conn *conn, const struct bt_bap_ep *ep, enum bt_audio_dir dir, - const struct bt_audio_codec_cfg *codec, struct bt_bap_stream **stream, - struct bt_bap_qos_cfg_pref *const pref, struct bt_bap_ascs_rsp *rsp) + LOG_DBG("Client %p %s the lock", (void *)conn, locked ? "locked" : "released"); +} + +/* Callback for SIRK read request from peer side */ +static uint8_t sirk_read_req_cb(struct bt_conn *conn, struct bt_csip_set_member_svc_inst *csip) { - int ret; - ARG_UNUSED(ep); - ARG_UNUSED(rsp); - LOG_DBG("LC3 config callback"); - - for (size_t i = 0; i < ARRAY_SIZE(cap_audio_streams); i++) { - struct bt_cap_stream *cap_audio_stream = &cap_audio_streams[i]; - - if (!cap_audio_stream->bap_stream.conn) { - LOG_DBG("ASE Codec Config stream %p", (void *)cap_audio_stream); - - ret = le_audio_bitrate_check(codec); - if (!ret) { - LOG_WRN("Bitrate check failed"); - return -EINVAL; - } - - ret = le_audio_freq_check(codec); - if (!ret) { - LOG_WRN("Sample rate not supported"); - return -EINVAL; - } - - if (dir == BT_AUDIO_DIR_SINK) { - LOG_DBG("BT_AUDIO_DIR_SINK"); - le_audio_print_codec(codec, dir); - le_audio_event_publish(LE_AUDIO_EVT_CONFIG_RECEIVED, conn, dir); - } - #if (CONFIG_BT_AUDIO_TX) - else if (dir == BT_AUDIO_DIR_SOURCE) { - LOG_DBG("BT_AUDIO_DIR_SOURCE"); - le_audio_print_codec(codec, dir); - le_audio_event_publish(LE_AUDIO_EVT_CONFIG_RECEIVED, conn, dir); - - /* CIS headset only supports one source stream for now */ - cap_tx_streams[0] = cap_audio_stream; - } - #endif /* (CONFIG_BT_AUDIO_TX) */ - else { - LOG_ERR("UNKNOWN DIR"); - return -EINVAL; - } - - *stream = &cap_audio_stream->bap_stream; - *pref = qos_pref; - - return 0; - } - } - - LOG_WRN("No audio_stream available"); - return -ENOMEM; - } - - static int lc3_reconfig_cb(struct bt_bap_stream *stream, enum bt_audio_dir dir, - const struct bt_audio_codec_cfg *codec, - struct bt_bap_qos_cfg_pref *const pref, struct bt_bap_ascs_rsp *rsp) + /* Accept the request to read the SIRK, but return encrypted SIRK instead of plaintext */ + return BT_CSIP_READ_SIRK_REQ_RSP_ACCEPT_ENC; +} + +static struct bt_csip_set_member_cb csip_callbacks = { + .lock_changed = csip_lock_changed_cb, + .sirk_read_req = sirk_read_req_cb, +}; +struct bt_csip_set_member_register_param csip_param = { + .set_size = CSIP_SET_SIZE, + .lockable = true, + .cb = &csip_callbacks, +}; + +#if defined(CONFIG_BT_AUDIO_RX) +BUILD_ASSERT(CONFIG_BT_ISO_RX_MTU >= LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX, 10000), + "ISO RX buffers must fit the advertised LC3 maximum"); +static struct bt_audio_codec_cap lc3_codec_sink = BT_AUDIO_CODEC_CAP_LC3( + BT_AUDIO_CODEC_CAPABILIY_FREQ, + (BT_AUDIO_CODEC_CAP_DURATION_10 | BT_AUDIO_CODEC_CAP_DURATION_PREFER_10), + BT_AUDIO_CODEC_CAP_CHAN_COUNT_SUPPORT(1), + LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MIN, 10000), + LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX, 10000), 1u, AVAILABLE_SINK_CONTEXT); +#endif /* (CONFIG_BT_AUDIO_RX) */ + +#if defined(CONFIG_BT_AUDIO_TX) +BUILD_ASSERT(CONFIG_BT_ISO_TX_MTU >= LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX, 10000), + "ISO TX buffers must fit the advertised LC3 maximum"); +static struct bt_audio_codec_cap lc3_codec_source = BT_AUDIO_CODEC_CAP_LC3( + BT_AUDIO_CODEC_CAPABILIY_FREQ, + (BT_AUDIO_CODEC_CAP_DURATION_10 | BT_AUDIO_CODEC_CAP_DURATION_PREFER_10), + BT_AUDIO_CODEC_CAP_CHAN_COUNT_SUPPORT(1), + LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MIN, 10000), + LE_AUDIO_SDU_SIZE_OCTETS(CONFIG_LC3_BITRATE_MAX, 10000), 1u, AVAILABLE_SOURCE_CONTEXT); +#endif /* (CONFIG_BT_AUDIO_TX) */ + +static enum bt_audio_dir caps_dirs[] = { +#if defined(CONFIG_BT_AUDIO_RX) + BT_AUDIO_DIR_SINK, +#endif /* CONFIG_BT_AUDIO_RX */ +#if defined(CONFIG_BT_AUDIO_TX) + BT_AUDIO_DIR_SOURCE, +#endif /* (CONFIG_BT_AUDIO_TX) */ +}; + +static struct bt_bap_qos_cfg_pref qos_pref = BT_BAP_QOS_CFG_PREF( + true, BT_GAP_LE_PHY_2M, CONFIG_BT_AUDIO_RETRANSMITS, BLE_ISO_LATENCY_MS, + CONFIG_AUDIO_MIN_PRES_DLY_US, CONFIG_AUDIO_MAX_PRES_DLY_US, + CONFIG_BT_AUDIO_PREFERRED_MIN_PRES_DLY_US, CONFIG_BT_AUDIO_PREFERRED_MAX_PRES_DLY_US); + +/* clang-format off */ +static struct bt_pacs_cap caps[] = { +#if (CONFIG_BT_AUDIO_RX) + { + .codec_cap = &lc3_codec_sink, + }, +#endif +#if (CONFIG_BT_AUDIO_TX) + { + .codec_cap = &lc3_codec_source, + } +#endif /* (CONFIG_BT_AUDIO_TX) */ +}; +/* clang-format on */ + +static struct bt_cap_stream + cap_audio_streams[CONFIG_BT_ASCS_MAX_ASE_SNK_COUNT + CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT]; + +static struct audio_sdu_timing rx_timing[ARRAY_SIZE(cap_audio_streams)]; + +static struct audio_sdu_timing *rx_timing_get(struct bt_bap_stream *stream) +{ + struct bt_cap_stream *cap = CONTAINER_OF(stream, struct bt_cap_stream, bap_stream); + + return &rx_timing[cap - cap_audio_streams]; +} + +#if (CONFIG_BT_AUDIO_TX) +BUILD_ASSERT(CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT <= 1, + "CIS headset only supports one source stream for now"); +#endif /* (CONFIG_BT_AUDIO_TX) */ + +static int lc3_config_cb(struct bt_conn *conn, const struct bt_bap_ep *ep, enum bt_audio_dir dir, + const struct bt_audio_codec_cfg *codec, struct bt_bap_stream **stream, + struct bt_bap_qos_cfg_pref *const pref, struct bt_bap_ascs_rsp *rsp) +{ + int ret; + LOG_DBG("LC3 config callback"); + + for (int i = 0; i < ARRAY_SIZE(cap_audio_streams); i++) { + struct bt_cap_stream *cap_audio_stream = &cap_audio_streams[i]; + + if (!cap_audio_stream->bap_stream.conn) { + LOG_DBG("ASE Codec Config stream %p", (void *)cap_audio_stream); + + ret = le_audio_bitrate_check(codec); + if (!ret) { + LOG_WRN("Bitrate check failed"); + return -EINVAL; + } + + ret = le_audio_freq_check(codec); + if (!ret) { + LOG_WRN("Sample rate not supported"); + return -EINVAL; + } + + if (dir == BT_AUDIO_DIR_SINK) { + LOG_DBG("BT_AUDIO_DIR_SINK"); + le_audio_print_codec(codec, dir); + le_audio_event_publish(LE_AUDIO_EVT_CONFIG_RECEIVED, conn, dir); + } +#if (CONFIG_BT_AUDIO_TX) + else if (dir == BT_AUDIO_DIR_SOURCE) { + LOG_DBG("BT_AUDIO_DIR_SOURCE"); + le_audio_print_codec(codec, dir); + le_audio_event_publish(LE_AUDIO_EVT_CONFIG_RECEIVED, conn, dir); + + /* CIS headset only supports one source stream for now */ + cap_tx_streams[0] = cap_audio_stream; + } +#endif /* (CONFIG_BT_AUDIO_TX) */ + else { + LOG_ERR("UNKNOWN DIR"); + return -EINVAL; + } + + *stream = &cap_audio_stream->bap_stream; + *pref = qos_pref; + + return 0; + } + } + + LOG_WRN("No audio_stream available"); + return -ENOMEM; +} + +static int lc3_reconfig_cb(struct bt_bap_stream *stream, enum bt_audio_dir dir, + const struct bt_audio_codec_cfg *codec, + struct bt_bap_qos_cfg_pref *const pref, struct bt_bap_ascs_rsp *rsp) { - ARG_UNUSED(dir); - ARG_UNUSED(codec); - ARG_UNUSED(pref); - ARG_UNUSED(rsp); - LOG_DBG("ASE Codec Reconfig: stream %p", (void *)stream); - - return 0; - } - - static int lc3_qos_cb(struct bt_bap_stream *stream, const struct bt_bap_qos_cfg *qos, + LOG_DBG("ASE Codec Reconfig: stream %p", (void *)stream); + + return 0; +} + +static int lc3_qos_cb(struct bt_bap_stream *stream, const struct bt_bap_qos_cfg *qos, + struct bt_bap_ascs_rsp *rsp) +{ + enum bt_audio_dir dir; + + dir = le_audio_stream_dir_get(stream); + if (dir <= 0) { + LOG_ERR("Failed to get dir of stream %p", stream); + return -EIO; + } + + le_audio_event_publish(LE_AUDIO_EVT_PRES_DELAY_SET, stream->conn, dir); + + LOG_DBG("QoS: stream %p qos %p", (void *)stream, (void *)qos); + + return 0; +} + +static int lc3_enable_cb(struct bt_bap_stream *stream, const uint8_t *meta, size_t meta_len, + struct bt_bap_ascs_rsp *rsp) +{ + LOG_DBG("Enable: stream %p meta_len %d", (void *)stream, meta_len); + + return 0; +} + +static int lc3_start_cb(struct bt_bap_stream *stream, struct bt_bap_ascs_rsp *rsp) +{ + LOG_DBG("Start stream %p", (void *)stream); + return 0; +} + +static int lc3_metadata_cb(struct bt_bap_stream *stream, const uint8_t *meta, size_t meta_len, struct bt_bap_ascs_rsp *rsp) { - ARG_UNUSED(rsp); - enum bt_audio_dir dir; - - dir = le_audio_stream_dir_get(stream); - if (dir <= 0) { - LOG_ERR("Failed to get dir of stream %p", stream); - return -EIO; - } - - le_audio_event_publish(LE_AUDIO_EVT_PRES_DELAY_SET, stream->conn, dir); - - LOG_DBG("QoS: stream %p qos %p", (void *)stream, (void *)qos); - - return 0; - } - - static int lc3_enable_cb(struct bt_bap_stream *stream, const uint8_t *meta, size_t meta_len, - struct bt_bap_ascs_rsp *rsp) + LOG_DBG("Metadata: stream %p meta_len %d", (void *)stream, meta_len); + return 0; +} + +static int lc3_disable_cb(struct bt_bap_stream *stream, struct bt_bap_ascs_rsp *rsp) { - ARG_UNUSED(meta); - ARG_UNUSED(rsp); - LOG_DBG("Enable: stream %p meta_len %d", (void *)stream, meta_len); - - return 0; - } - - static int lc3_start_cb(struct bt_bap_stream *stream, struct bt_bap_ascs_rsp *rsp) + enum bt_audio_dir dir; + + dir = le_audio_stream_dir_get(stream); + if (dir <= 0) { + LOG_ERR("Failed to get dir of stream %p", stream); + return -EIO; + } + + LOG_DBG("Disable: stream %p", (void *)stream); + + le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, stream->conn, dir); + + return 0; +} + +static int lc3_stop_cb(struct bt_bap_stream *stream, struct bt_bap_ascs_rsp *rsp) { - ARG_UNUSED(rsp); - LOG_DBG("Start stream %p", (void *)stream); - return 0; - } - - static int lc3_metadata_cb(struct bt_bap_stream *stream, const uint8_t *meta, size_t meta_len, - struct bt_bap_ascs_rsp *rsp) + enum bt_audio_dir dir; + + dir = le_audio_stream_dir_get(stream); + if (dir <= 0) { + LOG_ERR("Failed to get dir of stream %p", stream); + return -EIO; + } + + LOG_DBG("Stop: stream %p", (void *)stream); + + le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, stream->conn, dir); + + return 0; +} + +static int lc3_release_cb(struct bt_bap_stream *stream, struct bt_bap_ascs_rsp *rsp) { - ARG_UNUSED(meta); - ARG_UNUSED(rsp); - LOG_DBG("Metadata: stream %p meta_len %d", (void *)stream, meta_len); - return 0; - } - - static int lc3_disable_cb(struct bt_bap_stream *stream, struct bt_bap_ascs_rsp *rsp) + enum bt_audio_dir dir; + + dir = le_audio_stream_dir_get(stream); + if (dir <= 0) { + LOG_ERR("Failed to get dir of stream %p", stream); + return -EIO; + } + + LOG_DBG("Release: stream %p", (void *)stream); + + le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, stream->conn, dir); + + return 0; +} + +static const struct bt_bap_unicast_server_cb unicast_server_cb = { + .config = lc3_config_cb, + .reconfig = lc3_reconfig_cb, + .qos = lc3_qos_cb, + .enable = lc3_enable_cb, + .start = lc3_start_cb, + .metadata = lc3_metadata_cb, + .disable = lc3_disable_cb, + .stop = lc3_stop_cb, + .release = lc3_release_cb, +}; + +#if (CONFIG_BT_AUDIO_RX) +static void stream_recv_cb(struct bt_bap_stream *stream, const struct bt_iso_recv_info *info, + struct net_buf *audio_frame) { - ARG_UNUSED(rsp); - enum bt_audio_dir dir; - - dir = le_audio_stream_dir_get(stream); - if (dir <= 0) { - LOG_ERR("Failed to get dir of stream %p", stream); - return -EIO; - } - - LOG_DBG("Disable: stream %p", (void *)stream); - - le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, stream->conn, dir); - - return 0; - } - - static int lc3_stop_cb(struct bt_bap_stream *stream, struct bt_bap_ascs_rsp *rsp) + int ret; + struct audio_metadata meta; + + if (receive_cb == NULL) { + LOG_ERR("The RX callback has not been set"); + return; + } + + ret = le_audio_metadata_populate(&meta, stream, info, audio_frame); + if (ret) { + LOG_ERR("Failed to populate meta data: %d", ret); + return; + } + + /* Preserve concealment without feeding absent timestamps into clock sync. */ + if (!audio_sdu_timing_resolve(rx_timing_get(stream), info->flags & BT_ISO_FLAGS_TS, + info->seq_num, info->ts, k_uptime_get_32(), meta.data_len_us, + &meta.ref_ts_us)) { + return; + } + + receive_cb(audio_frame, &meta, 0); +} +#endif /* (CONFIG_BT_AUDIO_RX) */ + +#if (CONFIG_BT_AUDIO_TX) +static void stream_sent_cb(struct bt_bap_stream *stream) { - ARG_UNUSED(rsp); - enum bt_audio_dir dir; - - dir = le_audio_stream_dir_get(stream); - if (dir <= 0) { - LOG_ERR("Failed to get dir of stream %p", stream); - return -EIO; - } - - LOG_DBG("Stop: stream %p", (void *)stream); - - le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, stream->conn, dir); - - return 0; - } - - static int lc3_release_cb(struct bt_bap_stream *stream, struct bt_bap_ascs_rsp *rsp) + /* Unicast server/CIS headset only supports one source stream for now */ + struct stream_index idx = { + .lvl1 = 0, + .lvl2 = 0, + .lvl3 = 0, + }; + ERR_CHK(bt_le_audio_tx_stream_sent(bt_le_audio_tx, idx)); +} +#endif /* (CONFIG_BT_AUDIO_TX) */ + +static void stream_enabled_cb(struct bt_bap_stream *stream) { - ARG_UNUSED(rsp); - enum bt_audio_dir dir; - - dir = le_audio_stream_dir_get(stream); - if (dir <= 0) { - LOG_ERR("Failed to get dir of stream %p", stream); - return -EIO; - } - - LOG_DBG("Release: stream %p", (void *)stream); - - le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, stream->conn, dir); - - return 0; - } - - static const struct bt_bap_unicast_server_cb unicast_server_cb = { - .config = lc3_config_cb, - .reconfig = lc3_reconfig_cb, - .qos = lc3_qos_cb, - .enable = lc3_enable_cb, - .start = lc3_start_cb, - .metadata = lc3_metadata_cb, - .disable = lc3_disable_cb, - .stop = lc3_stop_cb, - .release = lc3_release_cb, - }; - - #if (CONFIG_BT_AUDIO_RX) - static void stream_recv_cb(struct bt_bap_stream *stream, const struct bt_iso_recv_info *info, - struct net_buf *buf) - { - bool bad_frame = false; - - if (receive_cb == NULL) { - LOG_ERR("The RX callback has not been set"); - return; - } - - if (!(info->flags & BT_ISO_FLAGS_VALID)) { - bad_frame = true; - } - - receive_cb(buf->data, buf->len, bad_frame, info->ts, 0, - bt_audio_codec_cfg_get_octets_per_frame(stream->codec_cfg)); - } - #endif /* (CONFIG_BT_AUDIO_RX) */ - - #if (CONFIG_BT_AUDIO_TX) - static void stream_sent_cb(struct bt_bap_stream *stream) - { - ARG_UNUSED(stream); - /* Unicast server/CIS headset only supports one source stream for now */ - struct stream_index idx = { - .lvl1 = 0, - .lvl2 = 0, - .lvl3 = 0, - }; - ERR_CHK(bt_le_audio_tx_stream_sent(idx)); - } - #endif /* (CONFIG_BT_AUDIO_TX) */ - - static void stream_enabled_cb(struct bt_bap_stream *stream) - { - int ret; - enum bt_audio_dir dir; - - dir = le_audio_stream_dir_get(stream); - if (dir <= 0) { - LOG_ERR("Failed to get dir of stream %p", stream); - return; - } - - LOG_DBG("Stream %p enabled", stream); - - if (dir == BT_AUDIO_DIR_SINK) { - /* Automatically do the receiver start ready operation */ - ret = bt_bap_stream_start(stream); - if (ret != 0) { - LOG_ERR("Failed to start stream: %d", ret); - return; - } - } - } - + int ret; + enum bt_audio_dir dir; + + dir = le_audio_stream_dir_get(stream); + if (dir <= 0) { + LOG_ERR("Failed to get dir of stream %p", stream); + return; + } + + LOG_DBG("Stream %p enabled", stream); + + if (dir == BT_AUDIO_DIR_SINK) { + /* Automatically do the receiver start ready operation */ + ret = bt_bap_stream_start(stream); + if (ret != 0) { + LOG_ERR("Failed to start stream: %d", ret); + return; + } + } +} + static void stream_disabled_cb(struct bt_bap_stream *stream) { - ARG_UNUSED(stream); - LOG_INF("Stream %p disabled", stream); - } - - static void stream_started_cb(struct bt_bap_stream *stream) - { - enum bt_audio_dir dir; - - dir = le_audio_stream_dir_get(stream); - if (dir <= 0) { - LOG_ERR("Failed to get dir of stream %p", stream); - return; - } - - LOG_INF("Stream %p started", stream); - - if (dir == BT_AUDIO_DIR_SOURCE && IS_ENABLED(CONFIG_BT_AUDIO_TX)) { - struct stream_index idx = { - .lvl1 = 0, - .lvl2 = 0, - .lvl3 = 0, - }; - ERR_CHK(bt_le_audio_tx_stream_started(idx)); - } - - le_audio_event_publish(LE_AUDIO_EVT_STREAMING, stream->conn, dir); - } - - static void stream_stopped_cb(struct bt_bap_stream *stream, uint8_t reason) - { - enum bt_audio_dir dir; - - dir = le_audio_stream_dir_get(stream); - if (dir <= 0) { - LOG_ERR("Failed to get dir of stream %p", stream); - return; - } - - LOG_DBG("Stream %p stopped. Reason: %d", stream, reason); - - le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, stream->conn, dir); - } - + LOG_INF("Stream %p disabled", stream); +} + +static void stream_started_cb(struct bt_bap_stream *stream) +{ + enum bt_audio_dir dir; + + *rx_timing_get(stream) = (struct audio_sdu_timing){0}; + + dir = le_audio_stream_dir_get(stream); + if (dir <= 0) { + LOG_ERR("Failed to get dir of stream %p", stream); + return; + } + + LOG_INF("Stream %p started", stream); + + if (dir == BT_AUDIO_DIR_SINK) { + concurrent_sink_streams_num++; + } + + if (dir == BT_AUDIO_DIR_SOURCE && IS_ENABLED(CONFIG_BT_AUDIO_TX)) { + struct stream_index idx = { + .lvl1 = 0, + .lvl2 = 0, + .lvl3 = 0, + }; + ERR_CHK(bt_le_audio_tx_stream_started(bt_le_audio_tx, idx)); + } + + le_audio_event_publish(LE_AUDIO_EVT_STREAMING, stream->conn, dir); +} + +static void stream_stopped_cb(struct bt_bap_stream *stream, uint8_t reason) +{ + enum bt_audio_dir dir; + + *rx_timing_get(stream) = (struct audio_sdu_timing){0}; + + dir = le_audio_stream_dir_get(stream); + if (dir <= 0) { + LOG_ERR("Failed to get dir of stream %p", stream); + return; + } + + LOG_DBG("Stream %p stopped. Reason: %d", stream, reason); + + if (dir == BT_AUDIO_DIR_SINK) { + concurrent_sink_streams_num--; + } + + le_audio_event_publish(LE_AUDIO_EVT_NOT_STREAMING, stream->conn, dir); +} + static void stream_released_cb(struct bt_bap_stream *stream) { - ARG_UNUSED(stream); - /* NOTE: The string below is used by the Nordic CI system */ - LOG_INF("Stream %p released", stream); - } - - static struct bt_bap_stream_ops stream_ops = { - #if (CONFIG_BT_AUDIO_RX) - .recv = stream_recv_cb, - #endif /* (CONFIG_BT_AUDIO_RX) */ - #if (CONFIG_BT_AUDIO_TX) - .sent = stream_sent_cb, - #endif /* (CONFIG_BT_AUDIO_TX) */ - .enabled = stream_enabled_cb, - .disabled = stream_disabled_cb, - .started = stream_started_cb, - .stopped = stream_stopped_cb, - .released = stream_released_cb, - }; - - int unicast_server_config_get(struct bt_conn *conn, enum bt_audio_dir dir, uint32_t *bitrate, - uint32_t *sampling_rate_hz, uint32_t *pres_delay_us) - { - ARG_UNUSED(conn); - int ret; - - if (bitrate == NULL && sampling_rate_hz == NULL && pres_delay_us == NULL) { - LOG_ERR("No valid pointers received"); - return -ENXIO; - } - - if (dir == BT_AUDIO_DIR_SINK) { - /* If multiple sink streams exists, they should have the same configurations, - * hence we only check the first one. - */ - if (cap_audio_streams[0].bap_stream.codec_cfg == NULL) { - LOG_ERR("No codec found for the stream"); - - return -ENXIO; - } - - if (sampling_rate_hz != NULL) { - ret = le_audio_freq_hz_get(cap_audio_streams[0].bap_stream.codec_cfg, - sampling_rate_hz); - if (ret) { - LOG_ERR("Invalid sampling frequency: %d", ret); - return -ENXIO; - } - } - - if (bitrate != NULL) { - ret = le_audio_bitrate_get(cap_audio_streams[0].bap_stream.codec_cfg, - bitrate); - if (ret) { - LOG_ERR("Unable to calculate bitrate: %d", ret); - return -ENXIO; - } - } - - if (pres_delay_us != NULL) { - if (cap_audio_streams[0].bap_stream.qos == NULL) { - LOG_ERR("No QoS found for the stream"); - return -ENXIO; - } - - *pres_delay_us = cap_audio_streams[0].bap_stream.qos->pd; - } - } else if (dir == BT_AUDIO_DIR_SOURCE && IS_ENABLED(CONFIG_BT_AUDIO_TX)) { - /* If multiple source streams exists, they should have the same configurations, - * hence we only check the first one. - */ - if (cap_tx_streams[0]->bap_stream.codec_cfg == NULL) { - LOG_ERR("No codec found for the stream"); - return -ENXIO; - } - - if (sampling_rate_hz != NULL) { - ret = le_audio_freq_hz_get(cap_tx_streams[0]->bap_stream.codec_cfg, - sampling_rate_hz); - if (ret) { - LOG_ERR("Invalid sampling frequency: %d", ret); - return -ENXIO; - } - } - - if (bitrate != NULL) { - ret = le_audio_bitrate_get(cap_tx_streams[0]->bap_stream.codec_cfg, - bitrate); - if (ret) { - LOG_ERR("Unable to calculate bitrate: %d", ret); - return -ENXIO; - } - } - - if (pres_delay_us != NULL) { - if (cap_tx_streams[0]->bap_stream.qos == NULL) { - LOG_ERR("No QoS found for the stream"); - return -ENXIO; - } - - *pres_delay_us = cap_tx_streams[0]->bap_stream.qos->pd; - LOG_ERR("pres_delay_us: %d", *pres_delay_us); - } - } - - return 0; - } - - int unicast_server_uuid_populate(struct net_buf_simple *uuid_buf) - { - if (net_buf_simple_tailroom(uuid_buf) >= (BT_UUID_SIZE_16 * 2)) { - net_buf_simple_add_le16(uuid_buf, BT_UUID_ASCS_VAL); - net_buf_simple_add_le16(uuid_buf, BT_UUID_PACS_VAL); - } else { - LOG_ERR("Not enough space for UUIDS"); - return -ENOMEM; - } - - return 0; - } - - int unicast_server_adv_populate(struct bt_data *adv_buf, uint8_t adv_buf_vacant) - { - int ret; - uint32_t adv_buf_cnt = 0; - - ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, - sizeof(unicast_server_adv_data), BT_DATA_SVC_DATA16, - unicast_server_adv_data); - if (ret) { - return ret; - } - - if (IS_ENABLED(CONFIG_BT_CSIP_SET_MEMBER)) { - ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, - sizeof(csip_rsi_adv_data), BT_DATA_CSIS_RSI, - (void *)csip_rsi_adv_data); - if (ret) { - return ret; - } - } - - sys_put_le16(CONFIG_BT_DEVICE_APPEARANCE, &gap_appear_adv_data[0]); - - ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, - sizeof(gap_appear_adv_data), BT_DATA_GAP_APPEARANCE, - (void *)gap_appear_adv_data); - if (ret) { - return ret; - } - - flags_adv_data = BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR; - - ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, sizeof(uint8_t), - BT_DATA_FLAGS, (void *)&flags_adv_data); - if (ret) { - return ret; - } - - ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, - ARRAY_SIZE(cap_adv_data), BT_DATA_SVC_DATA16, - (void *)cap_adv_data); - if (ret) { - return ret; - } - - enum audio_channel channel; - channel_assignment_get(&channel); - - memcpy(device_identifier, &oe_boot_state.device_id, sizeof(oe_boot_state.device_id)); - memcpy(device_identifier + sizeof(uint64_t), &channel, sizeof(uint8_t)); + /* NOTE: The string below is used by the Nordic CI system */ + LOG_INF("Stream %p released", stream); +} + +static struct bt_bap_stream_ops stream_ops = { +#if (CONFIG_BT_AUDIO_RX) + .recv = stream_recv_cb, +#endif /* (CONFIG_BT_AUDIO_RX) */ +#if (CONFIG_BT_AUDIO_TX) + .sent = stream_sent_cb, +#endif /* (CONFIG_BT_AUDIO_TX) */ + .enabled = stream_enabled_cb, + .disabled = stream_disabled_cb, + .started = stream_started_cb, + .stopped = stream_stopped_cb, + .released = stream_released_cb, +}; + +int unicast_server_pd_min_set(uint32_t dly_min_in_us) +{ + if (dly_min_in_us > qos_pref.pd_max) { + LOG_WRN("Min pres delay (%u) cannot be higher than max (%u), keeping old min (%u)", + dly_min_in_us, qos_pref.pd_max, qos_pref.pd_min); + return -EINVAL; + } + + if (dly_min_in_us < CONFIG_AUDIO_MIN_PRES_DLY_US) { + LOG_WRN("Min pres delay (%u) too low, setting to %u", dly_min_in_us, + CONFIG_AUDIO_MIN_PRES_DLY_US); + return -EINVAL; + } + + qos_pref.pd_min = dly_min_in_us; + qos_pref.pref_pd_min = dly_min_in_us; + + return 0; +} + +int le_audio_concurrent_sync_num_get(uint8_t *num_streams, enum bt_audio_location *locations) +{ + int ret; + + if (num_streams == NULL || locations == NULL) { + LOG_ERR("Invalid input parameters"); + return -EINVAL; + } + + *num_streams = concurrent_sink_streams_num; + *locations = 0; + + for (int i = 0; i < ARRAY_SIZE(cap_audio_streams); i++) { + if (!cap_audio_streams[i].bap_stream.conn || + !cap_audio_streams[i].bap_stream.codec_cfg) { + continue; + } + + enum bt_audio_location chan_allocation; + + ret = bt_audio_codec_cfg_get_chan_allocation( + cap_audio_streams[i].bap_stream.codec_cfg, &chan_allocation, false); + if (ret) { + LOG_WRN("Failed to get channel allocation"); + return ret; + } + + *locations |= chan_allocation; + } + + return 0; +} + +int unicast_server_config_get(struct bt_conn *conn, enum bt_audio_dir dir, uint32_t *bitrate, + uint32_t *sampling_rate_hz, uint32_t *pres_delay_us) +{ + int ret; + + if (bitrate == NULL && sampling_rate_hz == NULL && pres_delay_us == NULL) { + LOG_ERR("No valid pointers received"); + return -ENXIO; + } + + if (dir == BT_AUDIO_DIR_SINK) { + /* If multiple sink streams exists, they should have the same configurations, + * hence we only check the first one. + */ + if (cap_audio_streams[0].bap_stream.codec_cfg == NULL) { + LOG_ERR("No codec found for the stream"); + + return -ENXIO; + } + + if (sampling_rate_hz != NULL) { + ret = le_audio_freq_hz_get(cap_audio_streams[0].bap_stream.codec_cfg, + sampling_rate_hz); + if (ret) { + LOG_ERR("Invalid sampling frequency: %d", ret); + return -ENXIO; + } + } + + if (bitrate != NULL) { + ret = le_audio_bitrate_get(cap_audio_streams[0].bap_stream.codec_cfg, + bitrate); + if (ret) { + LOG_ERR("Unable to calculate bitrate: %d", ret); + return -ENXIO; + } + } + + if (pres_delay_us != NULL) { + if (cap_audio_streams[0].bap_stream.qos == NULL) { + LOG_ERR("No QoS found for the stream"); + return -ENXIO; + } + + *pres_delay_us = cap_audio_streams[0].bap_stream.qos->pd; + } + } else if (dir == BT_AUDIO_DIR_SOURCE && IS_ENABLED(CONFIG_BT_AUDIO_TX)) { + /* If multiple source streams exists, they should have the same configurations, + * hence we only check the first one. + */ + if (cap_tx_streams[0]->bap_stream.codec_cfg == NULL) { + LOG_ERR("No codec found for the stream"); + return -ENXIO; + } + + if (sampling_rate_hz != NULL) { + ret = le_audio_freq_hz_get(cap_tx_streams[0]->bap_stream.codec_cfg, + sampling_rate_hz); + if (ret) { + LOG_ERR("Invalid sampling frequency: %d", ret); + return -ENXIO; + } + } + + if (bitrate != NULL) { + ret = le_audio_bitrate_get(cap_tx_streams[0]->bap_stream.codec_cfg, + bitrate); + if (ret) { + LOG_ERR("Unable to calculate bitrate: %d", ret); + return -ENXIO; + } + } + + if (pres_delay_us != NULL) { + if (cap_tx_streams[0]->bap_stream.qos == NULL) { + LOG_ERR("No QoS found for the stream"); + return -ENXIO; + } + + *pres_delay_us = cap_tx_streams[0]->bap_stream.qos->pd; + LOG_ERR("pres_delay_us: %d", *pres_delay_us); + } + } + + return 0; +} + +int unicast_server_uuid_populate(struct net_buf_simple *uuid_buf) +{ + if (net_buf_simple_tailroom(uuid_buf) >= (BT_UUID_SIZE_16 * 2)) { + net_buf_simple_add_le16(uuid_buf, BT_UUID_ASCS_VAL); + net_buf_simple_add_le16(uuid_buf, BT_UUID_PACS_VAL); + } else { + LOG_ERR("Not enough space for UUIDS"); + return -ENOMEM; + } + + return 0; +} + +int unicast_server_adv_populate(struct bt_data *adv_buf, uint8_t adv_buf_vacant) +{ + int ret; + uint32_t adv_buf_cnt = 0; + + ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, + sizeof(unicast_server_adv_data), BT_DATA_SVC_DATA16, + unicast_server_adv_data); + if (ret) { + return ret; + } + + if (IS_ENABLED(CONFIG_BT_CSIP_SET_MEMBER)) { + ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, + sizeof(csip_rsi_adv_data), BT_DATA_CSIS_RSI, + (void *)csip_rsi_adv_data); + if (ret) { + return ret; + } + } + + sys_put_le16(CONFIG_BT_DEVICE_APPEARANCE, &gap_appear_adv_data[0]); + + ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, + sizeof(gap_appear_adv_data), BT_DATA_GAP_APPEARANCE, + (void *)gap_appear_adv_data); + if (ret) { + return ret; + } + + flags_adv_data = BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR; + + ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, sizeof(uint8_t), + BT_DATA_FLAGS, (void *)&flags_adv_data); + if (ret) { + return ret; + } ret = bt_mgmt_adv_buffer_put(adv_buf, &adv_buf_cnt, adv_buf_vacant, - ARRAY_SIZE(device_identifier), BT_DATA_MANUFACTURER_DATA, (void *)device_identifier); + ARRAY_SIZE(cap_adv_data), BT_DATA_SVC_DATA16, + (void *)cap_adv_data); + if (ret) { + return ret; + } + + return adv_buf_cnt; +} + +int unicast_server_send(struct net_buf const *const audio_frame) +{ +#if (CONFIG_BT_AUDIO_TX) + int ret; + uint8_t num_active_streams = 0; + + struct le_audio_tx_info tx[CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT]; + + for (int i = 0; i < ARRAY_SIZE(cap_tx_streams); i++) { + if (!le_audio_ep_state_check(cap_tx_streams[i]->bap_stream.ep, + BT_BAP_EP_STATE_STREAMING)) { + continue; + } + + /* Set cap stream pointer */ + tx[num_active_streams].cap_stream = cap_tx_streams[i]; + + /* Set index */ + tx[num_active_streams].idx.lvl1 = 0; + tx[num_active_streams].idx.lvl2 = 0; + tx[num_active_streams].idx.lvl3 = i; + tx[num_active_streams].audio_location = 0; + + num_active_streams++; + } + + ret = bt_le_audio_tx_send(bt_le_audio_tx, audio_frame, tx, num_active_streams); + if (ret) { + return ret; + } + + return 0; +#else + return -ENOTSUP; +#endif /* (CONFIG_BT_AUDIO_TX) */ +} + +int unicast_server_disable(void) +{ + return -ENOTSUP; +} + +int unicast_server_enable(le_audio_receive_cb recv_cb, enum bt_audio_location location) +{ + int ret; + static bool initialized; + /* clang-format off */ + const struct bt_pacs_register_param pacs_param = { + IF_ENABLED(CONFIG_BT_AUDIO_RX, (.snk_pac = true,)) + IF_ENABLED(CONFIG_BT_AUDIO_RX, (.snk_loc = true,)) + IF_ENABLED(CONFIG_BT_AUDIO_TX, (.src_pac = true,)) + IF_ENABLED(CONFIG_BT_AUDIO_TX, (.src_loc = true,))}; + /* clang-format on */ + + __ASSERT(strlen(CONFIG_BT_SET_IDENTITY_RESOLVING_KEY) == BT_CSIP_SIRK_SIZE, + "SIRK incorrect size, must be 16 bytes"); + + if (initialized) { + LOG_WRN("Already initialized"); + return -EALREADY; + } + + if (recv_cb == NULL && IS_ENABLED(CONFIG_BT_AUDIO_RX)) { + LOG_ERR("Receive callback is NULL"); + return -EINVAL; + } + + receive_cb = recv_cb; + + /* For this application, we create one sink endpoint for each location */ + unicast_server_params.snk_cnt = POPCOUNT(location); + if (unicast_server_params.snk_cnt == 0) { + LOG_ERR("No sink endpoint requested"); + return -EINVAL; + } + + if (unicast_server_params.snk_cnt > CONFIG_BT_ASCS_MAX_ASE_SNK_COUNT) { + LOG_WRN("Too many sink endpoints requested (%d), max %d, will just use first " + "location", + unicast_server_params.snk_cnt, CONFIG_BT_ASCS_MAX_ASE_SNK_COUNT); + unicast_server_params.snk_cnt = CONFIG_BT_ASCS_MAX_ASE_SNK_COUNT; + /* Use lowest valid set location */ + location = location & ~(location - 1); + } + + unicast_server_params.src_cnt = CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT; + + ret = bt_bap_unicast_server_register(&unicast_server_params); + if (ret) { + LOG_ERR("Could not register unicast server (err %d)", ret); + return ret; + } + + ret = bt_bap_unicast_server_register_cb(&unicast_server_cb); + if (ret) { + LOG_ERR("Could not register unicast server callbacks (err %d)", ret); + return ret; + } + + if (IS_ENABLED(CONFIG_BT_CSIP_SET_MEMBER_TEST_SAMPLE_DATA)) { + LOG_WRN("CSIP test sample data is used, must be changed " + "before production"); + } else { + if (strcmp(CONFIG_BT_SET_IDENTITY_RESOLVING_KEY_DEFAULT, + CONFIG_BT_SET_IDENTITY_RESOLVING_KEY) == 0) { + LOG_WRN("CSIP using the default SIRK, must be changed " + "before production"); + } + + memcpy(csip_param.sirk, CONFIG_BT_SET_IDENTITY_RESOLVING_KEY, BT_CSIP_SIRK_SIZE); + } + ret = bt_pacs_register(&pacs_param); if (ret) { + LOG_ERR("Could not register PACS (err %d)\n", ret); return ret; } - - return adv_buf_cnt; - } - - int unicast_server_send(struct le_audio_encoded_audio enc_audio) - { - #if (CONFIG_BT_AUDIO_TX) - int ret; - uint8_t num_active_streams = 0; - - struct le_audio_tx_info tx[CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT]; - - for (size_t i = 0; i < ARRAY_SIZE(cap_tx_streams); i++) { - if (!le_audio_ep_state_check(cap_tx_streams[i]->bap_stream.ep, - BT_BAP_EP_STATE_STREAMING)) { - continue; - } - - /* Set cap stream pointer */ - tx[num_active_streams].cap_stream = cap_tx_streams[i]; - - /* Set index */ - tx[num_active_streams].idx.lvl1 = 0; - tx[num_active_streams].idx.lvl2 = 0; - tx[num_active_streams].idx.lvl3 = i; - - /* Set channel location */ - tx[num_active_streams].audio_channel = AUDIO_MIC; - - num_active_streams++; - } - - ret = bt_le_audio_tx_send(tx, num_active_streams, enc_audio); - if (ret) { - return ret; - } - - return 0; - #else - return -ENOTSUP; - #endif /* (CONFIG_BT_AUDIO_TX) */ - } - - int unicast_server_disable(void) - { - return -ENOTSUP; - } - - int unicast_server_enable(le_audio_receive_cb recv_cb, enum bt_audio_location location) - { - int ret; - static bool initialized; - - __ASSERT(strlen(CONFIG_BT_SET_IDENTITY_RESOLVING_KEY) == BT_CSIP_SIRK_SIZE, - "SIRK incorrect size, must be 16 bytes"); - - if (initialized) { - LOG_WRN("Already initialized"); - return -EALREADY; - } - - if (recv_cb == NULL && IS_ENABLED(CONFIG_BT_AUDIO_RX)) { - LOG_ERR("Receive callback is NULL"); - return -EINVAL; - } - - receive_cb = recv_cb; - - bt_bap_unicast_server_register(&unicast_server_params); - bt_bap_unicast_server_register_cb(&unicast_server_cb); - - if (IS_ENABLED(CONFIG_BT_CSIP_SET_MEMBER_TEST_SAMPLE_DATA)) { - LOG_WRN("CSIP test sample data is used, must be changed " - "before production"); - } else { - if (strcmp(CONFIG_BT_SET_IDENTITY_RESOLVING_KEY_DEFAULT, - CONFIG_BT_SET_IDENTITY_RESOLVING_KEY) == 0) { - LOG_WRN("CSIP using the default SIRK, must be changed " - "before production"); - } - - uint8_t sirk_string[17]; - - uint32_t sirk = uicr_sirk_get(); - - memset(sirk_string, 0, sizeof(sirk_string)); - - if (sirk != 0xFFFFFFFF) { - snprintf(sirk_string, 16, "%08X", sirk); //"%016llX" - } else { - snprintf(sirk_string, 16, "%08X", - (unsigned int)oe_boot_state.device_id); //"%016llX" + + for (int i = 0; i < ARRAY_SIZE(caps); i++) { + ret = bt_pacs_cap_register(caps_dirs[i], &caps[i]); + if (ret) { + LOG_ERR("Capability register failed. Err: %d (%d)", ret, i); + return ret; + } + } + + if (location == BT_AUDIO_LOCATION_FRONT_LEFT) { + csip_param.rank = CSIP_HL_RANK; + } else if (location == BT_AUDIO_LOCATION_FRONT_RIGHT) { + csip_param.rank = CSIP_HR_RANK; + } else if (location == (BT_AUDIO_LOCATION_FRONT_LEFT | BT_AUDIO_LOCATION_FRONT_RIGHT)) { + csip_param.rank = CSIP_HL_RANK; + csip_param.set_size = 1; + } else { + LOG_ERR("Location not supported"); + return -ECANCELED; + } + + if (IS_ENABLED(CONFIG_BT_AUDIO_RX)) { + ret = bt_pacs_set_location(BT_AUDIO_DIR_SINK, location); + if (ret) { + LOG_ERR("Location set failed. Err: %d", ret); + return ret; + } + } + + if (IS_ENABLED(CONFIG_BT_AUDIO_TX)) { + /* A unicast server is the Bluetooth peripheral device */ + ret = bt_le_audio_tx_init(bt_le_audio_tx, false); + if (ret) { + LOG_ERR("Failed to initialize LE Audio TX: %d", ret); + return ret; + } + + ret = bt_pacs_set_location(BT_AUDIO_DIR_SOURCE, location); + if (ret) { + LOG_ERR("Location set failed. Err: %d", ret); + return ret; + } + } + + if (IS_ENABLED(CONFIG_BT_AUDIO_RX)) { + ret = bt_pacs_set_supported_contexts(BT_AUDIO_DIR_SINK, AVAILABLE_SINK_CONTEXT); + + if (ret) { + LOG_ERR("Supported context set failed (sink). Err: %d", ret); + return ret; + } + + ret = bt_pacs_set_available_contexts(BT_AUDIO_DIR_SINK, AVAILABLE_SINK_CONTEXT); + if (ret) { + LOG_ERR("Available context set failed (sink). Err: %d", ret); + return ret; + } + } + + if (IS_ENABLED(CONFIG_BT_AUDIO_TX)) { + ret = bt_pacs_set_supported_contexts(BT_AUDIO_DIR_SOURCE, AVAILABLE_SOURCE_CONTEXT); + if (ret) { + LOG_ERR("Supported context set failed (source). Err: %d", ret); + return ret; + } + + ret = bt_pacs_set_available_contexts(BT_AUDIO_DIR_SOURCE, AVAILABLE_SOURCE_CONTEXT); + if (ret) { + LOG_ERR("Available context set failed (source). Err: %d", ret); + return ret; } + } + + for (int i = 0; i < ARRAY_SIZE(cap_audio_streams); i++) { + bt_cap_stream_ops_register(&cap_audio_streams[i], &stream_ops); + } + + if (IS_ENABLED(CONFIG_BT_CSIP_SET_MEMBER)) { + ret = bt_cap_acceptor_register(&csip_param, &csip); + if (ret) { + LOG_ERR("Failed to register CAP acceptor. Err: %d", ret); + return ret; + } + + ret = bt_csip_set_member_generate_rsi(csip, csip_rsi_adv_data); + if (ret) { + LOG_ERR("Failed to generate RSI. Err: %d", ret); + return ret; + } + } + + initialized = true; - // LOG_INF("SIRK as String: %s", sirk_string); - - memcpy(csip_param.sirk, sirk_string, - BT_CSIP_SIRK_SIZE); - - // memcpy(csip_param.sirk, CONFIG_BT_SET_IDENTITY_RESOLVING_KEY, BT_CSIP_SIRK_SIZE); - } - - for (size_t i = 0; i < ARRAY_SIZE(caps); i++) { - ret = bt_pacs_cap_register(caps_dirs[i], &caps[i]); - if (ret) { - LOG_ERR("Capability register failed. Err: %d", ret); - return ret; - } - } - - if (IS_ENABLED(CONFIG_BT_AUDIO_RX)) { - if (location == BT_AUDIO_LOCATION_FRONT_LEFT) { - csip_param.rank = CSIP_HL_RANK; - } else if (location == BT_AUDIO_LOCATION_FRONT_RIGHT) { - csip_param.rank = CSIP_HR_RANK; - } else { - LOG_ERR("Channel not supported"); - return -ECANCELED; - } - - ret = bt_pacs_set_location(BT_AUDIO_DIR_SINK, location); - if (ret) { - LOG_ERR("Location set failed. Err: %d", ret); - return ret; - } - } - - if (IS_ENABLED(CONFIG_BT_AUDIO_TX)) { - bt_le_audio_tx_init(); - - ret = bt_pacs_set_location(BT_AUDIO_DIR_SOURCE, location); - if (ret) { - LOG_ERR("Location set failed. Err: %d", ret); - return ret; - } - } - - ret = bt_pacs_set_supported_contexts(BT_AUDIO_DIR_SINK, AVAILABLE_SINK_CONTEXT); - - if (ret) { - LOG_ERR("Supported context set failed. Err: %d", ret); - return ret; - } - - ret = bt_pacs_set_available_contexts(BT_AUDIO_DIR_SINK, AVAILABLE_SINK_CONTEXT); - if (ret) { - LOG_ERR("Available context set failed. Err: %d", ret); - return ret; - } - - ret = bt_pacs_set_supported_contexts(BT_AUDIO_DIR_SOURCE, AVAILABLE_SOURCE_CONTEXT); - - if (ret) { - LOG_ERR("Supported context set failed. Err: %d", ret); - return ret; - } - - ret = bt_pacs_set_available_contexts(BT_AUDIO_DIR_SOURCE, AVAILABLE_SOURCE_CONTEXT); - if (ret) { - LOG_ERR("Available context set failed. Err: %d", ret); - return ret; - } - - for (size_t i = 0; i < ARRAY_SIZE(cap_audio_streams); i++) { - bt_cap_stream_ops_register(&cap_audio_streams[i], &stream_ops); - } - - if (IS_ENABLED(CONFIG_BT_CSIP_SET_MEMBER)) { - ret = bt_cap_acceptor_register(&csip_param, &csip); - if (ret) { - LOG_ERR("Failed to register CAP acceptor. Err: %d", ret); - return ret; - } - - ret = bt_csip_set_member_generate_rsi(csip, csip_rsi_adv_data); - if (ret) { - LOG_ERR("Failed to generate RSI. Err: %d", ret); - return ret; - } - } - - initialized = true; - - return 0; - } + return 0; +} diff --git a/src/bluetooth/bt_stream/unicast/unicast_server.h b/src/bluetooth/bt_stream/unicast/unicast_server.h index a6f633c5..7546b44b 100644 --- a/src/bluetooth/bt_stream/unicast/unicast_server.h +++ b/src/bluetooth/bt_stream/unicast/unicast_server.h @@ -4,6 +4,14 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @addtogroup audio_app_bt_stream + * @{ + * @defgroup unicast_server Functions for unicast server functionality. + * @{ + * @brief Helper functions to manage unicast server (headset side) functionality. + */ + #ifndef _UNICAST_SERVER_H_ #define _UNICAST_SERVER_H_ @@ -12,6 +20,15 @@ #include +/** + * @brief Set the minimum presentation delay for audio streams. + * + * @param[in] dly_min_in_us Minimum presentation delay in microseconds. + * + * @return 0 for success, error otherwise. + */ +int unicast_server_pd_min_set(uint32_t dly_min_in_us); + /** * @brief Get configuration for audio stream. * @@ -54,11 +71,11 @@ int unicast_server_adv_populate(struct bt_data *adv_buf, uint8_t adv_buf_vacant) /** * @brief Send data from the LE Audio unicast (CIS) server, if configured as a source. * - * @param[in] enc_audio Encoded audio struct. + * @param[in] audio_frame Pointer to the audio data. * * @return 0 for success, error otherwise. */ -int unicast_server_send(struct le_audio_encoded_audio enc_audio); +int unicast_server_send(struct net_buf const *const audio_frame); /** * @brief Disable the Bluetooth LE Audio unicast (CIS) server. @@ -77,4 +94,9 @@ int unicast_server_disable(void); */ int unicast_server_enable(le_audio_receive_cb rx_cb, enum bt_audio_location location); +/** + * @} + * @} + */ + #endif /* _UNICAST_SERVER_H_ */ diff --git a/src/bluetooth/gatt_services/CMakeLists.txt b/src/bluetooth/gatt_services/CMakeLists.txt index d74ba9b3..201de6e2 100644 --- a/src/bluetooth/gatt_services/CMakeLists.txt +++ b/src/bluetooth/gatt_services/CMakeLists.txt @@ -3,6 +3,7 @@ target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/battery_service.cpp ${CMAKE_CURRENT_SOURCE_DIR}/button_service.c ${CMAKE_CURRENT_SOURCE_DIR}/sensor_service.c + ${CMAKE_CURRENT_SOURCE_DIR}/sensor_transport.c ${CMAKE_CURRENT_SOURCE_DIR}/audio_config_service.c ${CMAKE_CURRENT_SOURCE_DIR}/power_saving_service.c ${CMAKE_CURRENT_SOURCE_DIR}/led_service.cpp diff --git a/src/bluetooth/gatt_services/audio_response_service.c b/src/bluetooth/gatt_services/audio_response_service.c index 66f0e352..761d5647 100644 --- a/src/bluetooth/gatt_services/audio_response_service.c +++ b/src/bluetooth/gatt_services/audio_response_service.c @@ -98,7 +98,6 @@ static struct k_work_delayable transfer_timeout_work; static bool transfer_owner_disconnected; K_MUTEX_DEFINE(service_mutex); -extern struct data_fifo fifo_rx; extern const struct bt_gatt_service_static audio_response_svc; static int notify_transfer_status(enum audio_response_transfer_status status, uint16_t credits); @@ -815,16 +814,6 @@ static void measurement_work_handler(struct k_work *work) goto fail; } - if (!fifo_rx.initialized) { - LOG_DBG("Initializing RX FIFO for audio response measurement"); - ret = data_fifo_init(&fifo_rx); - if (ret != 0) { - LOG_ERR("Failed to initialize RX FIFO: %d", ret); - goto fail; - } - } - LOG_DBG("RX FIFO ready for audio response measurement: id=%u", pending_config.id); - ret = audio_datapath_decimator_init(CONFIG_AUDIO_SAMPLE_RATE_HZ / AUDIO_RESPONSE_CAPTURE_SAMPLE_RATE); if (ret != 0) { @@ -833,7 +822,7 @@ static void measurement_work_handler(struct k_work *work) } LOG_DBG("Audio response decimator initialized: input_rate=%u output_rate=%u", CONFIG_AUDIO_SAMPLE_RATE_HZ, AUDIO_RESPONSE_CAPTURE_SAMPLE_RATE); - ret = audio_datapath_aquire(&fifo_rx); + ret = audio_datapath_aquire(NULL); if (ret != 0) { LOG_ERR("Failed to acquire audio datapath: %d", ret); goto fail; diff --git a/src/bluetooth/gatt_services/device_info.c b/src/bluetooth/gatt_services/device_info.c index 13c4438b..78a04c80 100644 --- a/src/bluetooth/gatt_services/device_info.c +++ b/src/bluetooth/gatt_services/device_info.c @@ -45,7 +45,7 @@ static ssize_t read_firmware(struct bt_conn *conn, uint16_t offset) { return bt_gatt_attr_read(conn, attr, buf, len, offset, firmware, - sizeof(firmware)); + strlen(firmware)); } BT_GATT_SERVICE_DEFINE(device_svc, diff --git a/src/bluetooth/gatt_services/led_service.cpp b/src/bluetooth/gatt_services/led_service.cpp index f12a82f1..aea3a8e1 100644 --- a/src/bluetooth/gatt_services/led_service.cpp +++ b/src/bluetooth/gatt_services/led_service.cpp @@ -5,13 +5,27 @@ #include LOG_MODULE_REGISTER(led_service, CONFIG_BLE_LOG_LEVEL); +static ssize_t read_led(struct bt_conn *conn, const struct bt_gatt_attr *attr, + void *buf, uint16_t len, uint16_t offset) +{ + RGBColor color; + state_indicator.get_custom_color(color); + return bt_gatt_attr_read(conn, attr, buf, len, offset, color, sizeof(color)); +} + +static ssize_t read_state(struct bt_conn *conn, const struct bt_gatt_attr *attr, + void *buf, uint16_t len, uint16_t offset) +{ + uint8_t mode = state_indicator.get_indication_mode(); + return bt_gatt_attr_read(conn, attr, buf, len, offset, &mode, sizeof(mode)); +} + static ssize_t write_led(struct bt_conn *conn, const struct bt_gatt_attr *attr, const void *buf, uint16_t len, uint16_t offset, uint8_t flags) { ARG_UNUSED(conn); - ARG_UNUSED(attr); ARG_UNUSED(flags); if (len != 3U) { LOG_INF("Write led: Incorrect data length"); @@ -24,6 +38,7 @@ static ssize_t write_led(struct bt_conn *conn, } state_indicator.set_custom_color(*((const RGBColor*)(buf))); + (void)bt_gatt_notify(NULL, attr, buf, len); return len; } @@ -34,7 +49,6 @@ static ssize_t write_state(struct bt_conn *conn, uint16_t len, uint16_t offset, uint8_t flags) { ARG_UNUSED(conn); - ARG_UNUSED(attr); ARG_UNUSED(flags); if (len != 1U) { LOG_INF("Write led: Incorrect data length"); @@ -46,7 +60,12 @@ static ssize_t write_state(struct bt_conn *conn, return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); } - state_indicator.set_indication_mode((led_mode) *((uint8_t*)buf)); + uint8_t mode = *((const uint8_t*)buf); + if (mode != STATE_INDICATION && mode != CUSTOM) { + return BT_GATT_ERR(BT_ATT_ERR_VALUE_NOT_ALLOWED); + } + state_indicator.set_indication_mode((led_mode)mode); + (void)bt_gatt_notify(NULL, attr, buf, len); return len; } @@ -54,13 +73,15 @@ static ssize_t write_state(struct bt_conn *conn, BT_GATT_SERVICE_DEFINE(rgb_led_svc, BT_GATT_PRIMARY_SERVICE(BT_UUID_LED), BT_GATT_CHARACTERISTIC(BT_UUID_LED_RGB, - BT_GATT_CHRC_WRITE, - BT_GATT_PERM_WRITE, - NULL, write_led, NULL), + BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE | BT_GATT_CHRC_NOTIFY, + BT_GATT_PERM_READ | BT_GATT_PERM_WRITE, + read_led, write_led, NULL), + BT_GATT_CCC(NULL, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE), BT_GATT_CHARACTERISTIC(BT_UUID_LED_STATE, - BT_GATT_CHRC_WRITE, - BT_GATT_PERM_WRITE, - NULL, write_state, NULL), + BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE | BT_GATT_CHRC_NOTIFY, + BT_GATT_PERM_READ | BT_GATT_PERM_WRITE, + read_state, write_state, NULL), + BT_GATT_CCC(NULL, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE), ); int init_led_service() { diff --git a/src/bluetooth/gatt_services/sensor_service.c b/src/bluetooth/gatt_services/sensor_service.c index c358ceff..9820c5a7 100644 --- a/src/bluetooth/gatt_services/sensor_service.c +++ b/src/bluetooth/gatt_services/sensor_service.c @@ -1,4 +1,7 @@ #include "sensor_service.h" +#include "sensor_transport.h" +#include +#include #include #include #include "../SensorManager/SensorManager.h" @@ -11,6 +14,11 @@ LOG_MODULE_REGISTER(sensor_manager, CONFIG_MODULE_BUTTON_HANDLER_LOG_LEVEL); #define MAX_SENSOR_REC_NAME_LENGTH 64 #define MAX_NOTIFIES_IN_FLIGHT 4 +#define BATCH_LATENCY_MS 20 +BUILD_ASSERT(MAX_NOTIFIES_IN_FLIGHT <= CONFIG_BT_ATT_TX_COUNT); +static atomic_t stream_epoch; +static struct bt_conn *sensor_conn; +struct queued_sensor { struct sensor_data data; uint32_t epoch; }; static struct k_thread thread_data_notify; @@ -23,7 +31,7 @@ ZBUS_CHAN_DECLARE(bt_mgmt_chan); static K_THREAD_STACK_DEFINE(thread_stack_notify, CONFIG_SENSOR_GATT_NOTIFY_STACK_SIZE); -K_MSGQ_DEFINE(gatt_queue, sizeof(struct sensor_data), CONFIG_SENSOR_GATT_SUB_QUEUE_SIZE, 4); +K_MSGQ_DEFINE(gatt_queue, sizeof(struct queued_sensor), CONFIG_SENSOR_GATT_SUB_QUEUE_SIZE, 4); static struct sensor_data sensor_data_value; static struct sensor_config config; @@ -61,7 +69,7 @@ enum sensor_notify_context_state { struct sensor_notify_context { struct bt_gatt_notify_params params; - struct sensor_data payload; + struct oe_sensor_batch payload; enum sensor_notify_context_state state; uint32_t generation; }; @@ -83,8 +91,12 @@ static void reset_sensor_notification_state(void) notify_enabled = false; sensor_config_status_ntfy_enabled = false; connection_complete = false; + struct bt_conn *old = sensor_conn; + sensor_conn = NULL; + atomic_inc(&stream_epoch); k_spin_unlock(¬ify_state_lock, key); + if (old) bt_conn_unref(old); k_msgq_purge(&gatt_queue); } @@ -95,8 +107,8 @@ static void reset_sensor_notification_state(void) * @retval Pointer to a free notification context. * @retval NULL No context is currently available. */ -static struct sensor_notify_context *acquire_notify_context(const struct sensor_data *data, - uint32_t *generation); +static struct sensor_notify_context *acquire_notify_context(const struct oe_sensor_batch *data, + uint32_t *generation, uint32_t epoch); /** * @brief Mark a reserved context as queued in the Bluetooth stack. @@ -162,6 +174,7 @@ static void connect_evt_handler(const struct zbus_channel *chan) case BT_MGMT_CONNECTED: { k_spinlock_key_t key = k_spin_lock(¬ify_state_lock); + sensor_conn = bt_conn_ref(msg->conn); connection_complete = true; k_spin_unlock(¬ify_state_lock, key); break; @@ -184,6 +197,7 @@ static void sensor_ccc_cfg_changed(const struct bt_gatt_attr *attr, k_spinlock_key_t key = k_spin_lock(¬ify_state_lock); notify_enabled = (value == BT_GATT_CCC_NOTIFY); + atomic_inc(&stream_epoch); k_spin_unlock(¬ify_state_lock, key); LOG_INF("Sensor data notifications %s", notify_enabled ? "enabled" : "disabled"); @@ -314,12 +328,12 @@ BT_GATT_CHARACTERISTIC(BT_UUID_SENSOR_RECORDING_NAME, read_sensor_rec_name, write_sensor_rec_name, NULL), ); -static struct sensor_notify_context *acquire_notify_context(const struct sensor_data *data, - uint32_t *generation) +static struct sensor_notify_context *acquire_notify_context(const struct oe_sensor_batch *data, + uint32_t *generation, uint32_t epoch) { k_spinlock_key_t key = k_spin_lock(¬ify_state_lock); - if (!connection_complete || !notify_enabled || notify_count >= MAX_NOTIFIES_IN_FLIGHT) { + if (!connection_complete || !notify_enabled || epoch != (uint32_t)atomic_get(&stream_epoch) || notify_count >= MAX_NOTIFIES_IN_FLIGHT) { k_spin_unlock(¬ify_state_lock, key); return NULL; } @@ -330,7 +344,7 @@ static struct sensor_notify_context *acquire_notify_context(const struct sensor_ notify_contexts[i].generation++; notify_contexts[i].payload = *data; notify_contexts[i].params.attr = &sensor_service.attrs[4]; - notify_contexts[i].params.data = ¬ify_contexts[i].payload; + notify_contexts[i].params.data = notify_contexts[i].payload.data; notify_contexts[i].params.func = notify_complete; notify_contexts[i].params.user_data = ¬ify_contexts[i]; notify_count++; @@ -362,71 +376,122 @@ static void notify_complete(struct bt_conn *conn, void *user_data) release_notify_context(context, generation); } -static void notification_task(void) { - int ret; - struct sensor_data sensor_data; - - while (1) { - ret = k_msgq_get(&gatt_queue, &sensor_data, K_FOREVER); - - if (ret != 0) { - LOG_WRN("No data to process"); - continue; - } - - while (1) { - const uint16_t size = sizeof(sensor_data.id) + sizeof(sensor_data.size) + - sizeof(sensor_data.time) + sensor_data.size; - struct sensor_notify_context *context; - uint32_t generation; - - context = acquire_notify_context(&sensor_data, &generation); - if (context == NULL) { - k_spinlock_key_t key = k_spin_lock(¬ify_state_lock); - bool can_retry = connection_complete && notify_enabled; - bool saturated = notify_count >= MAX_NOTIFIES_IN_FLIGHT; - k_spin_unlock(¬ify_state_lock, key); - - if (!can_retry) { - break; - } - - if (saturated) { - k_sleep(K_MSEC(1)); - continue; - } - - LOG_WRN("No free GATT notify context available"); - break; - } - - context->params.len = size; - ret = bt_gatt_notify_cb(NULL, &context->params); - if (ret != 0) { - LOG_WRN("Failed to send data: %d.\n", ret); - release_notify_context(context, generation); - } else { - mark_notify_context_in_flight(context, generation); - } - break; - } - } +/* A connection reference and epoch keep queued data out of a later session. */ +static struct bt_conn *stream_connection(uint32_t epoch) +{ + k_spinlock_key_t key = k_spin_lock(¬ify_state_lock); + struct bt_conn *conn = NULL; + if (connection_complete && notify_enabled && sensor_conn && + epoch == (uint32_t)atomic_get(&stream_epoch)) { + conn = bt_conn_ref(sensor_conn); + } + k_spin_unlock(¬ify_state_lock, key); + return conn; } -void sensor_queue_listener_cb(const struct zbus_channel *chan) { - ARG_UNUSED(chan); - int ret; - const struct sensor_msg * msg; - - msg = (struct sensor_msg *)zbus_chan_const_msg(&sensor_chan); +static void send_batch(struct oe_sensor_batch *batch, uint32_t epoch) +{ + if (!batch->count) return; + while (1) { + struct bt_conn *conn = stream_connection(epoch); + if (!conn) break; + uint32_t generation; + struct sensor_notify_context *context = acquire_notify_context(batch, &generation, epoch); + if (!context) { + bt_conn_unref(conn); + k_sleep(K_MSEC(1)); + continue; + } + context->params.len = batch->len; + int ret = bt_gatt_notify_cb(conn, &context->params); + bt_conn_unref(conn); + if (ret) { + LOG_WRN("Failed to send data: %d.\n", ret); + release_notify_context(context, generation); + } else { + mark_notify_context_in_flight(context, generation); + } + break; + } + batch->count = 0; + batch->len = 0; +} - if (msg->stream) { - ret = k_msgq_put(&gatt_queue, &msg->data, K_NO_WAIT); +static void notification_task(void) +{ + static struct oe_sensor_batch batches[8]; + int64_t started[8] = {0}; + uint32_t epoch = atomic_get(&stream_epoch); + unsigned next = 0; + while (1) { + if (epoch != (uint32_t)atomic_get(&stream_epoch)) { + for (unsigned i = 0; i < 8; ++i) send_batch(&batches[i], epoch); + epoch = atomic_get(&stream_epoch); + } + int64_t now = k_uptime_get(); + int64_t wait_ms = BATCH_LATENCY_MS; + /* Rotate the first sensor serviced so a busy FIFO cannot monopolize TX. */ + for (unsigned n = 0; n < 8; ++n) { + unsigned i = (next + n) % 8; + if (!batches[i].count) continue; + if (now - started[i] >= BATCH_LATENCY_MS) send_batch(&batches[i], epoch); + else wait_ms = MIN(wait_ms, BATCH_LATENCY_MS - (now - started[i])); + } + next = (next + 1) % 8; + struct queued_sensor item; + if (k_msgq_get(&gatt_queue, &item, K_MSEC(wait_ms))) continue; + const struct sensor_data *data = &item.data; + unsigned id = data->id; + if (id >= 8) continue; + unsigned count = oe_sensor_sample_count(id, data->size); + if (data->size > sizeof(data->data) || !count) { + + continue; + } + struct bt_conn *conn = stream_connection(item.epoch); + if (!conn) { + + continue; + } + if (item.epoch != epoch) { + for (unsigned n = 0; n < 8; ++n) send_batch(&batches[n], epoch); + epoch = item.epoch; + } + unsigned limit = MIN(OE_SENSOR_PACKET_MAX, bt_gatt_get_mtu(conn) - 3); + bt_conn_unref(conn); + unsigned width = oe_sensor_sample_size(id); + unsigned period = count > 1 ? sys_get_le16(data->data + data->size - 2) : 0; + for (unsigned i = 0; i < count; ++i) { + const uint8_t *sample = data->data + i * width; + uint64_t time = data->time + (uint64_t)i * period; + struct oe_sensor_batch *batch = &batches[id]; + if (!oe_sensor_batch_append(batch, id, sample, time, limit)) { + send_batch(batch, epoch); + if (!oe_sensor_batch_append(batch, id, sample, time, limit)) { + + continue; + } + } + if (batch->count == 1) started[id] = k_uptime_get(); + } + } +} - if (ret) { - LOG_WRN("ble sensor stream queue full"); - } - } +void sensor_queue_listener_cb(const struct zbus_channel *chan) +{ + const struct sensor_msg *msg = zbus_chan_const_msg(chan); + if (!msg->stream || msg->data.id >= 8) return; + struct queued_sensor item = { .data = msg->data, .epoch = atomic_get(&stream_epoch) }; + struct bt_conn *conn = stream_connection(item.epoch); + if (!conn) return; + bt_conn_unref(conn); + if (k_msgq_put(&gatt_queue, &item, K_NO_WAIT)) { + /* Keep live sensor data fresh when the link cannot drain the queue. */ + struct queued_sensor discarded; + (void)k_msgq_get(&gatt_queue, &discarded, K_NO_WAIT); + (void)k_msgq_put(&gatt_queue, &item, K_NO_WAIT); + LOG_WRN_RATELIMIT_RATE(1000, "ble sensor stream queue full"); + } } int init_sensor_config_status() { @@ -457,6 +522,17 @@ int init_sensor_config_status() { return 0; } +int get_sensor_config_status(uint8_t sensor_id, struct sensor_config *config) { + if (!active_sensor_configs || !config) return -EINVAL; + for (size_t i = 0; i < active_sensor_configs_size; i++) { + if (active_sensor_configs[i].sensorId == sensor_id) { + *config = active_sensor_configs[i]; + return 0; + } + } + return -ENOENT; +} + int set_sensor_config_status(struct sensor_config sensor_configuration) { LOG_DBG("Setting sensor config status for sensorId: %i", sensor_configuration.sensorId); @@ -484,6 +560,10 @@ int set_sensor_config_status(struct sensor_config sensor_configuration) { active_sensor_configs[active_sensor_configs_size - 1] = sensor_configuration; } + return notify_sensor_config_status(); +} + +int notify_sensor_config_status(void) { if (sensor_config_status_ntfy_enabled) { LOG_DBG("Sensor config status notification, notifying %zu active sensor configs", active_sensor_configs_size); struct bt_gatt_notify_params params = { diff --git a/src/bluetooth/gatt_services/sensor_service.h b/src/bluetooth/gatt_services/sensor_service.h index 239062fe..4b94602b 100644 --- a/src/bluetooth/gatt_services/sensor_service.h +++ b/src/bluetooth/gatt_services/sensor_service.h @@ -36,6 +36,8 @@ const char *get_sensor_recording_name(); //int send_sensor_data(); //struct sensor_data * data); int set_sensor_config_status(struct sensor_config config); +int get_sensor_config_status(uint8_t sensor_id, struct sensor_config *config); +int notify_sensor_config_status(void); void temp_disable_notifies(bool disable); @@ -43,4 +45,4 @@ void temp_disable_notifies(bool disable); } #endif -#endif \ No newline at end of file +#endif diff --git a/src/bluetooth/gatt_services/sensor_transport.c b/src/bluetooth/gatt_services/sensor_transport.c new file mode 100644 index 00000000..d5a33052 --- /dev/null +++ b/src/bluetooth/gatt_services/sensor_transport.c @@ -0,0 +1,120 @@ +#include "sensor_transport.h" +#include "ppg_protocol.h" +#include "imu_protocol.h" +#include +#include + +#define MAX_BATCH_TIME_ERROR_US 32 + +/* BLE-only: invert the BMX160's fixed scales without changing SD samples. */ +static bool compact_imu(const uint8_t *sample, uint8_t packed[24]) +{ + const float scales[2] = {(2.0f * 9.80665f) / 32768.0f, 2000.0f / 32768.0f}; + float values[9]; + int16_t raw[6]; + memcpy(values, sample, sizeof(values)); + for (unsigned i = 0; i < 6; ++i) { + const float count = roundf(values[i] / scales[i / 3]); + if (!(count >= INT16_MIN && count <= INT16_MAX)) return false; + raw[i] = (int16_t)count; + const float restored = raw[i] * scales[i / 3]; + if (memcmp(&restored, &values[i], sizeof(restored)) != 0) return false; + } + const imu_compact_sample_t encoded = { + raw[0], raw[1], raw[2], raw[3], raw[4], raw[5], + values[6], values[7], values[8], + }; + size_t written; + return imu_compact_sample_encode(&encoded, packed, 24, &written) == PROTOCOL_OK; +} + +/* BLE-only encoding. Acquisition records and SD/.oe samples remain 16 bytes. */ +static bool compact_ppg(const uint8_t *sample, uint8_t packed[10]) +{ + uint32_t v[4]; + for (unsigned i = 0; i < 4; ++i) { + const uint8_t *p = sample + 4 * i; + v[i] = (uint32_t)p[0] | (uint32_t)p[1] << 8 | + (uint32_t)p[2] << 16 | (uint32_t)p[3] << 24; + if (v[i] > 0x7ffff) return false; + } + const ppg_compact_sample_t encoded = { + .bits_0_31 = v[0] | (v[1] << 19), + .bits_32_63 = (v[1] >> 13) | (v[2] << 6) | (v[3] << 25), + .bits_64_79 = v[3] >> 7, + }; + size_t written; + return ppg_compact_sample_encode(&encoded, packed, 10, &written) == PROTOCOL_OK; +} + +unsigned oe_sensor_sample_size(uint8_t id) +{ + switch (id) { + case 0: return 36; + case 1: return 8; + case 4: return 16; + case 6: return 4; + case 7: return 6; + default: return 0; + } +} + +unsigned oe_sensor_sample_count(uint8_t id, unsigned size) +{ + unsigned width = oe_sensor_sample_size(id); + if (!width) return 0; + if (size == width) return 1; + return size >= width + 2 && (size - 2) % width == 0 ? (size - 2) / width : 0; +} + +bool oe_sensor_batch_append(struct oe_sensor_batch *b, uint8_t id, + const uint8_t *sample, uint64_t time, unsigned limit) +{ + unsigned width = oe_sensor_sample_size(id); + if (!width || !sample || limit > OE_SENSOR_PACKET_MAX) return false; + uint8_t packed[24]; + if (id == 0) { + if (!compact_imu(sample, packed)) return false; + sample = packed; + width = 24; + } else if (id == 4) { + if (!compact_ppg(sample, packed)) return false; + sample = packed; + width = 10; + } + unsigned len = 10 + (b->count + 1) * width + (b->count ? 2 : 0); + if (len > limit) return false; + if (b->count) { + if (b->data[0] != id || time <= b->last_time) return false; + uint64_t delta = time - b->last_time; + if (delta > UINT16_MAX) return false; + if (b->count == 1) { + b->period = (uint16_t)delta; + } else { + /* Keep the period fixed and bound each reconstructed timestamp's + * error against the packet anchor, so errors cannot accumulate. */ + uint64_t first = 0; + for (unsigned i = 0; i < 8; ++i) + first |= (uint64_t)b->data[2 + i] << (8 * i); + uint64_t span = (uint64_t)b->count * b->period; + if (first > UINT64_MAX - span) return false; + uint64_t expected = first + span; + uint64_t error = time > expected ? time - expected : expected - time; + if (error > MAX_BATCH_TIME_ERROR_US) return false; + } + } else { + b->data[0] = id; + for (unsigned i = 0; i < 8; ++i) b->data[2 + i] = time >> (8 * i); + b->period = 0; + } + memcpy(b->data + 10 + b->count * width, sample, width); + b->count++; + b->len = len; + b->data[1] = len - 10; + b->last_time = time; + if (b->count > 1) { + b->data[len - 2] = b->period; + b->data[len - 1] = b->period >> 8; + } + return true; +} diff --git a/src/bluetooth/gatt_services/sensor_transport.h b/src/bluetooth/gatt_services/sensor_transport.h new file mode 100644 index 00000000..fb45eff3 --- /dev/null +++ b/src/bluetooth/gatt_services/sensor_transport.h @@ -0,0 +1,32 @@ +#ifndef OE_SENSOR_TRANSPORT_H +#define OE_SENSOR_TRANSPORT_H + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* One 251-byte link-layer payload, less L2CAP and ATT headers. */ +#define OE_SENSOR_PACKET_MAX 244 + +struct oe_sensor_batch { + uint8_t data[OE_SENSOR_PACKET_MAX]; + uint16_t len; + uint16_t count; + uint16_t period; + uint64_t last_time; +}; + +unsigned oe_sensor_sample_size(uint8_t id); +unsigned oe_sensor_sample_count(uint8_t id, unsigned size); +/* False means flush the previous batch and try this sample again. */ +bool oe_sensor_batch_append(struct oe_sensor_batch *batch, uint8_t id, + const uint8_t *sample, uint64_t time, unsigned limit); + +#ifdef __cplusplus +} +#endif +#endif diff --git a/src/drivers/ADAU1860.cpp b/src/drivers/ADAU1860.cpp index 4b662914..f4af293b 100644 --- a/src/drivers/ADAU1860.cpp +++ b/src/drivers/ADAU1860.cpp @@ -377,12 +377,30 @@ int ADAU1860::end() { } int ADAU1860::setup() { - -#if !CONFIG_FDSP - // Unmute DAC (no need to wait for the ASCRs to lock) - uint8_t dac_ctrl2 = 0x0; - dac.writeReg(registers::DAC_CTRL2, &dac_ctrl2, sizeof(dac_ctrl2)); -#endif + // UG-2257 STATUS2: power-up complete, SPT0 lock, ASRC input lock, + // and frequency-multiplier ready. PLL_LOCK is not used in FM mode. + constexpr uint8_t required = (1 << 7) | (1 << 4) | (1 << 2) | (1 << 1); + const int64_t deadline = k_uptime_get() + 1000; + int64_t stable_since = -1; + + while (true) { + uint8_t status = 0; + if (!readReg(registers::STATUS2, &status, sizeof(status))) { + return -EIO; + } + const int64_t now = k_uptime_get(); + if ((status & required) == required) { + if (stable_since < 0) stable_since = now; + if (now - stable_since >= 100) break; + } else { + stable_since = -1; + } + if (now >= deadline) { + LOG_ERR("Codec clocks did not settle (STATUS2: 0x%02x)", status); + return -ETIMEDOUT; + } + k_msleep(5); + } return 0; } diff --git a/src/drivers/CMakeLists.txt b/src/drivers/CMakeLists.txt index 37d0f0dc..80af5689 100644 --- a/src/drivers/CMakeLists.txt +++ b/src/drivers/CMakeLists.txt @@ -4,7 +4,10 @@ # SPDX-License-Identifier: LicenseRef-Nordic-5-Clause # -target_sources_ifdef(CONFIG_NRF5340_AUDIO_ADAU1860_DRIVER +target_sources_ifdef(CONFIG_NRF_AUDIO_CS47L63_DRIVER app PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/cs47l63_comm.c) + +target_sources_ifdef(CONFIG_NRF_AUDIO_ADAU1860_DRIVER app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/ADAU1860.cpp ) diff --git a/src/drivers/Kconfig b/src/drivers/Kconfig index 327b92d6..eb58ce59 100644 --- a/src/drivers/Kconfig +++ b/src/drivers/Kconfig @@ -6,14 +6,22 @@ menu "Drivers" -menuconfig NRF5340_AUDIO_CS47L63_DRIVER +config OPENEARABLE_SD_SPI_FAST_WRITE + bool "Use 32 MHz SD writes on nRF5340" + depends on SOC_NRF5340_CPUAPP && SPI_SDHC + default y + help + Negotiate SD high-speed mode and send write blocks at 32 MHz when + supported. Commands, responses and reads stay at 16 MHz or below. + Cards that do not support high-speed mode keep their original speed. + +menuconfig NRF_AUDIO_CS47L63_DRIVER bool "CS47L63 HW codec driver" - depends on BOARD_NRF5340_AUDIO_DK_NRF5340_CPUAPP select HW_CODEC_CIRRUS_LOGIC help Include the driver for the Cirrus Logic CS47L63 hardware codec chip -if NRF5340_AUDIO_CS47L63_DRIVER +if NRF_AUDIO_CS47L63_DRIVER config CS47L63_THREAD_PRIO int "Priority for CS47L63 thread" @@ -29,16 +37,16 @@ module = CS47L63 module-str = cs47l63 source "subsys/logging/Kconfig.template.log_config" -endif # NRF5340_AUDIO_CS47L63_DRIVER +endif # NRF_AUDIO_CS47L63_DRIVER -menuconfig NRF5340_AUDIO_ADAU1860_DRIVER +menuconfig NRF_AUDIO_ADAU1860_DRIVER bool "ADAU1860 HW codec driver" depends on BOARD_OPENEARABLE_V2_NRF5340_CPUAPP || BOARD_OPENEARABLE_V2_NRF5340_CPUAPP_NS default y if BOARD_OPENEARABLE_V2_NRF5340_CPUAPP || BOARD_OPENEARABLE_V2_NRF5340_CPUAPP_NS help Include the driver for the Analog Devices ADAU1860 hardware codec chip -if NRF5340_AUDIO_ADAU1860_DRIVER +if NRF_AUDIO_ADAU1860_DRIVER config ADAU1860_THREAD_PRIO int "Priority for ADAU1860 thread" @@ -71,5 +79,5 @@ module = ADAU1860 module-str = adau1860 source "subsys/logging/Kconfig.template.log_config" -endif # NRF5340_AUDIO_ADAU1860_DRIVER +endif # NRF_AUDIO_ADAU1860_DRIVER endmenu # Drivers diff --git a/src/modules/CMakeLists.txt b/src/modules/CMakeLists.txt index 45cf0745..b6ddeabf 100644 --- a/src/modules/CMakeLists.txt +++ b/src/modules/CMakeLists.txt @@ -4,21 +4,26 @@ # SPDX-License-Identifier: LicenseRef-Nordic-5-Clause # -target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/audio_i2s.c) -# target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/audio_usb.c) -target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/audio_sync_timer.c) +target_sources(app PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/audio_i2s.c + ${CMAKE_CURRENT_SOURCE_DIR}/audio_sync_timer.c + ${CMAKE_CURRENT_SOURCE_DIR}/audio_clock.c) -target_sources_ifdef(CONFIG_NRF5340_AUDIO_CS47L63_DRIVER app PRIVATE +target_sources_ifdef(CONFIG_AUDIO_SOURCE_USB app PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/audio_usb.c + ${CMAKE_CURRENT_SOURCE_DIR}/audio_usb_init.c) + +target_sources_ifdef(CONFIG_NRF_AUDIO_CS47L63_DRIVER app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/hw_codec.c) -target_sources_ifdef(CONFIG_NRF5340_AUDIO_ADAU1860_DRIVER app PRIVATE +target_sources_ifdef(CONFIG_NRF_AUDIO_ADAU1860_DRIVER app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/hw_codec_adau1860.cpp) target_sources_ifdef(CONFIG_NRF5340_AUDIO_POWER_MEASUREMENT app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/power_meas.c) -target_sources_ifdef(CONFIG_NRF5340_AUDIO_SD_CARD_MODULE app PRIVATE +target_sources_ifdef(CONFIG_NRF_AUDIO_SD_CARD_MODULE app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/sd_card.c) -target_sources_ifdef(CONFIG_NRF5340_AUDIO_SD_CARD_LC3_FILE app PRIVATE +target_sources_ifdef(CONFIG_NRF_AUDIO_SD_CARD_LC3_FILE app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/lc3_file.c) target_sources_ifdef(CONFIG_SD_CARD_PLAYBACK app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/sd_card_playback.c) -target_sources_ifdef(CONFIG_NRF5340_AUDIO_SD_CARD_LC3_STREAMER app PRIVATE +target_sources_ifdef(CONFIG_NRF_AUDIO_SD_CARD_LC3_STREAMER app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/lc3_streamer.c) diff --git a/src/modules/Kconfig b/src/modules/Kconfig index 434f13e5..1c78450a 100644 --- a/src/modules/Kconfig +++ b/src/modules/Kconfig @@ -15,7 +15,42 @@ config BUTTON_DEBOUNCE_MS config AUDIO_SYNC_TIMER_USES_RTC bool default y - select NRFX_RTC0 + select NRFX_RTC + +config USBD_PID + hex + default 0x530A + +config USBD_VID + hex + default 0x1915 + +config USBD_PRODUCT + string + default "nRF5340 USB Audio" + +config USBD_MANUFACTURER + string + default "Nordic Semiconductor AS" + +config USBD_SELF_POWERED + bool "USB device Self-powered attribute" + default y + help + Set the Self-powered attribute in the configuration. + +config USBD_MAX_POWER + int + default 50 if USBD_SELF_POWERED + default 250 # if !USBD_SELF_POWERED + help + The maximum power consumption of the USB device in units of 2 mA + (i.e., the value 50 represents 100 mA, the value 250 represents 500 mA). + +config USBD_REMOTE_WAKEUP + bool "USB device Remote Wakeup attribute" + help + Set the Remote Wakeup attribute in the configuration. #----------------------------------------------------------------------------# menuconfig NRF5340_AUDIO_POWER_MEASUREMENT @@ -41,7 +76,7 @@ config POWER_MEAS_START_ON_BOOT the voltage, current consumption, and power usage for the following rails: VBAT, VDD1_CODEC, VDD2_CODEC, and VDD2_NRF -endif # NRF5340_AUDIO_POWER_MEASUREMENT +endif # NRF_AUDIO_POWER_MEASUREMENT #----------------------------------------------------------------------------# menu "I2S" @@ -144,37 +179,37 @@ config VOLUME_MSG_SUB_QUEUE_SIZE endmenu # Zbus #----------------------------------------------------------------------------# -config NRF5340_AUDIO_SD_CARD_MODULE +config NRF_AUDIO_SD_CARD_MODULE bool "Audio SD Card Module" help Include the audio SD card module to support streaming audio files from an SD card. -if NRF5340_AUDIO_SD_CARD_MODULE +if NRF_AUDIO_SD_CARD_MODULE module = MODULE_SD_CARD module-str = module-sd-card source "subsys/logging/Kconfig.template.log_config" -endif # NRF5340_AUDIO_SD_CARD_MODULE +endif # NRF_AUDIO_SD_CARD_MODULE -config NRF5340_AUDIO_SD_CARD_LC3_FILE +config NRF_AUDIO_SD_CARD_LC3_FILE bool "SD card LC3 file support" - depends on NRF5340_AUDIO_SD_CARD_MODULE + depends on NRF_AUDIO_SD_CARD_MODULE default n help Include support for reading and parsing LC3 files from an SD card. -if NRF5340_AUDIO_SD_CARD_LC3_FILE +if NRF_AUDIO_SD_CARD_LC3_FILE module = MODULE_SD_CARD_LC3_FILE module-str = module-sd-card-lc3-file source "subsys/logging/Kconfig.template.log_config" -endif # NRF5340_AUDIO_SD_CARD_LC3_FILE +endif # NRF_AUDIO_SD_CARD_LC3_FILE menuconfig SD_CARD_PLAYBACK bool "Enable playback from SD card" - depends on NRF5340_AUDIO_SD_CARD_MODULE + depends on NRF_AUDIO_SD_CARD_MODULE select EXPERIMENTAL default n select RING_BUFFER @@ -200,12 +235,12 @@ source "subsys/logging/Kconfig.template.log_config" endif # SD_CARD_PLAYBACK -menuconfig NRF5340_AUDIO_SD_CARD_LC3_STREAMER +menuconfig NRF_AUDIO_SD_CARD_LC3_STREAMER bool "Audio SD Card LC3 Streamer" - depends on NRF5340_AUDIO_SD_CARD_LC3_FILE + depends on NRF_AUDIO_SD_CARD_LC3_FILE select EXPERIMENTAL -if NRF5340_AUDIO_SD_CARD_LC3_STREAMER +if NRF_AUDIO_SD_CARD_LC3_STREAMER config SD_CARD_LC3_STREAMER_STACK_SIZE int "Stack size for the SD card LC3 streamer thread" @@ -228,6 +263,6 @@ module = MODULE_SD_CARD_LC3_STREAMER module-str = module-sd-card-lc3-streamer source "subsys/logging/Kconfig.template.log_config" -endif # NRF5340_AUDIO_SD_CARD_LC3_STREAMER +endif # NRF_AUDIO_SD_CARD_LC3_STREAMER endmenu # Modules diff --git a/src/modules/Kconfig.defaults b/src/modules/Kconfig.defaults index 18a7a83d..b43b3664 100644 --- a/src/modules/Kconfig.defaults +++ b/src/modules/Kconfig.defaults @@ -9,36 +9,32 @@ if AUDIO_DEV = 2 if AUDIO_SOURCE_USB -config USB_DEVICE_STACK - default y +config UDC_NRF_THREAD_STACK_SIZE + default 1024 -# Net buf options needed for USB stack -config NET_BUF - default y +config USBD_THREAD_STACK_SIZE + default 4096 -config USB_DEVICE_AUDIO - default y +config UDC_BUF_POOL_SIZE + default 2048 -## TODO: Nordic VID, change accordingly -config USB_DEVICE_VID - default 0x1915 +config NRF_USBD_COMMON_LOG_LEVEL + default 2 -## TODO: Change for final product -config USB_DEVICE_PID - default 0x530A +config USBC_LOG_LEVEL + default 2 -config USB_DEVICE_PRODUCT - default "nRF5340 USB Audio" +config USBD_LOG_LEVEL + default 2 -config USB_DEVICE_MANUFACTURER - default "Nordic Semiconductor AS" +config USBD_UAC2_LOG_LEVEL + default 2 -## Avoid redundant warnings for endpoint setting in USB stack -config USB_DRIVER_LOG_LEVEL - default 1 +config UDC_DRIVER_LOG_LEVEL + default 2 -config USB_DEVICE_LOG_LEVEL - default 1 +config USBD_MAX_SPEED + default 0 # FULL endif # AUDIO_SOURCE_USB diff --git a/src/modules/audio_clock.c b/src/modules/audio_clock.c new file mode 100644 index 00000000..3425abcc --- /dev/null +++ b/src/modules/audio_clock.c @@ -0,0 +1,53 @@ +/* + * Copyright (c) 2026 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause + */ + +#include "audio_clock.h" + +#include +#include +#include + +int audio_clock_set(uint16_t freq_value) +{ + freq_value = CLAMP(freq_value, APLL_FREQ_MIN, APLL_FREQ_MAX); + +#if NRF_CLOCK_HAS_HFCLKAUDIO + nrfx_clock_hfclkaudio_config_set(freq_value); + +#else + return -ENOTSUP; +#endif /* NRF_CLOCK_HAS_HFCLKAUDIO */ + + return 0; +} + +int audio_clock_init(void) +{ + int ret; + +#if NRF_CLOCK_HAS_HFCLKAUDIO + ret = nrfx_clock_divider_set(NRF_CLOCK_DOMAIN_HFCLK, NRF_CLOCK_HFCLK_DIV_1); + if (ret) { + return ret; + } + + ret = audio_clock_set(APLL_FREQ_CENTER); + if (ret) { + return ret; + } + + NRF_CLOCK->TASKS_HFCLKAUDIOSTART = 1; + + /* Wait for ACLK to start */ + while (!NRF_CLOCK_EVENT_HFCLKAUDIOSTARTED) { + k_sleep(K_MSEC(1)); + } +#else + return -ENOTSUP; +#endif /* NRF_CLOCK_HAS_HFCLKAUDIO */ + + return 0; +} diff --git a/src/modules/audio_clock.h b/src/modules/audio_clock.h new file mode 100644 index 00000000..ed8caa73 --- /dev/null +++ b/src/modules/audio_clock.h @@ -0,0 +1,67 @@ +/* + * Copyright (c) 2026 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause + */ + +/** @file + * @defgroup audio_clock Audio Clock + * @{ + * @brief Audio clock management for nRF Audio application. + * + * This module provides functions to manage the audio clock. On the nRF5340 SoC, the audio clock is + * typically derived from the Analog Phase-Locked Loop (APLL), which is used to generate the + * required frequencies for transmission, for instance I2S or TDM. + */ + +#ifndef _AUDIO_CLOCK_H_ +#define _AUDIO_CLOCK_H_ + +#include +#include + +/** @brief HFCLKAUDIO frequency value for 12.288-MHz output. + * + * This macro defines the frequency value for the High Frequency Clock Audio (HFCLKAUDIO) + * to generate a 12.288-MHz output frequency. The value is calculated according to the formula + * in the nRF5340 SoC documentation. + */ +#define HFCLKAUDIO_12_288_MHZ 0x9BA6 + +#define HFCLKAUDIO_12_165_MHZ 0x8FD8 + +#define HFCLKAUDIO_12_411_MHZ 0xA774 + +/* Audio clock - nRF5340 Analog Phase-Locked Loop (APLL) */ +#define APLL_FREQ_MIN HFCLKAUDIO_12_165_MHZ +#define APLL_FREQ_CENTER HFCLKAUDIO_12_288_MHZ +#define APLL_FREQ_MAX HFCLKAUDIO_12_411_MHZ + +/** + * @brief Set the audio clock frequency. + * This function configures the audio clock to the specified frequency value. The frequency value + * must be within the range defined by APLL_FREQ_MIN and APLL_FREQ_MAX. The function will adjust the + * frequency to fit within this range if necessary. + * + * @param[in] freq_value The desired frequency value for the audio clock, calculated + * according to the formula in the nRF5340 SoC documentation. + * + * @return 0 on success, or a negative error code on failure. + */ +int audio_clock_set(uint16_t freq_value); + +/** + * @brief Initialize the audio clock. + * This function initializes the audio clock system, which may involve configuring the necessary + * hardware components and ensuring that the clock is ready for use. This should be called before + * any other audio clock functions are used. + * + * @return 0 on success, or a negative error code on failure. + */ +int audio_clock_init(void); + +/** + * @} + */ + +#endif /* _AUDIO_CLOCK_H_ */ diff --git a/src/modules/audio_i2s.c b/src/modules/audio_i2s.c index 6cbee8d2..90920d8a 100644 --- a/src/modules/audio_i2s.c +++ b/src/modules/audio_i2s.c @@ -10,7 +10,6 @@ #include #include #include -#include #include "audio_sync_timer.h" @@ -27,16 +26,16 @@ static enum audio_i2s_state state = AUDIO_I2S_STATE_UNINIT; PINCTRL_DT_DEFINE(I2S_NL); #if CONFIG_AUDIO_SAMPLE_RATE_16000_HZ -#define CONFIG_AUDIO_RATIO NRF_I2S_RATIO_384X +#define I2S_RATIO NRF_I2S_RATIO_384X #elif CONFIG_AUDIO_SAMPLE_RATE_24000_HZ -#define CONFIG_AUDIO_RATIO NRF_I2S_RATIO_256X +#define I2S_RATIO NRF_I2S_RATIO_256X #elif CONFIG_AUDIO_SAMPLE_RATE_48000_HZ -#define CONFIG_AUDIO_RATIO NRF_I2S_RATIO_128X +#define I2S_RATIO NRF_I2S_RATIO_128X #else #error "Current AUDIO_SAMPLE_RATE_HZ setting not supported" #endif -static nrfx_i2s_t i2s_inst = NRFX_I2S_INSTANCE(0); +static nrfx_i2s_t i2s_inst = NRFX_I2S_INSTANCE(NRF_I2S0); static nrfx_i2s_config_t cfg = { /* Pins are configured by pinctrl. */ @@ -46,8 +45,11 @@ static nrfx_i2s_config_t cfg = { .mode = NRF_I2S_MODE_MASTER, .format = NRF_I2S_FORMAT_I2S, .alignment = NRF_I2S_ALIGN_LEFT, - .ratio = CONFIG_AUDIO_RATIO, - .mck_setup = 0x66666000, + .prescalers = { + .ratio = I2S_RATIO, + .mck_setup = 0x66666000, + .enable_bypass = false, + }, #if (CONFIG_AUDIO_BIT_DEPTH_16) .sample_width = NRF_I2S_SWIDTH_16BIT, //.sample_width = NRF_I2S_SWIDTH_16BIT_IN32BIT, @@ -58,7 +60,6 @@ static nrfx_i2s_config_t cfg = { #endif /* (CONFIG_AUDIO_BIT_DEPTH_16) */ .channels = NRF_I2S_CHANNELS_STEREO, .clksrc = NRF_I2S_CLKSRC_ACLK, - .enable_bypass = false, }; static i2s_blk_comp_callback_t i2s_blk_comp_callback; @@ -67,12 +68,13 @@ static void i2s_comp_handler(nrfx_i2s_buffers_t const *released_bufs, uint32_t s { if ((status == NRFX_I2S_STATUS_NEXT_BUFFERS_NEEDED) && released_bufs && i2s_blk_comp_callback && (released_bufs->p_rx_buffer || released_bufs->p_tx_buffer)) { - i2s_blk_comp_callback(audio_sync_timer_capture_get(), released_bufs->p_rx_buffer, - released_bufs->p_tx_buffer); + i2s_blk_comp_callback(audio_sync_timer_frame_start_capture_get(), + released_bufs->p_rx_buffer, released_bufs->p_tx_buffer); } } -void audio_i2s_set_next_buf(const uint8_t *tx_buf, uint32_t *rx_buf) +/* Prevent function from being inlined when LTO is enabled */ +void __noinline audio_i2s_set_next_buf(const uint8_t *tx_buf, uint32_t *rx_buf) { __ASSERT_NO_MSG(state == AUDIO_I2S_STATE_STARTED); if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || (CONFIG_AUDIO_DEV == GATEWAY)) && IS_ENABLED(CONFIG_AUDIO_MIC_I2S)) { @@ -87,13 +89,14 @@ void audio_i2s_set_next_buf(const uint8_t *tx_buf, uint32_t *rx_buf) .p_tx_buffer = (uint32_t *)tx_buf, .buffer_size = I2S_SAMPLES_NUM}; - nrfx_err_t ret; + int ret; ret = nrfx_i2s_next_buffers_set(&i2s_inst, &i2s_buf); - __ASSERT_NO_MSG(ret == NRFX_SUCCESS); + __ASSERT_NO_MSG(ret == 0); } -void audio_i2s_start(const uint8_t *tx_buf, uint32_t *rx_buf) +/* Prevent function from being inlined when LTO is enabled */ +void __noinline audio_i2s_start(const uint8_t *tx_buf, uint32_t *rx_buf) { __ASSERT_NO_MSG(state == AUDIO_I2S_STATE_IDLE); if ((IS_ENABLED(CONFIG_STREAM_BIDIRECTIONAL) || (CONFIG_AUDIO_DEV == GATEWAY)) && IS_ENABLED(CONFIG_AUDIO_MIC_I2S)) { @@ -108,11 +111,11 @@ void audio_i2s_start(const uint8_t *tx_buf, uint32_t *rx_buf) .p_tx_buffer = (uint32_t *)tx_buf, .buffer_size = I2S_SAMPLES_NUM}; - nrfx_err_t ret; + int ret; /* Buffer size in 32-bit words */ ret = nrfx_i2s_start(&i2s_inst, &i2s_buf, 0); - __ASSERT_NO_MSG(ret == NRFX_SUCCESS); + __ASSERT_NO_MSG(ret == 0); state = AUDIO_I2S_STATE_STARTED; } @@ -135,25 +138,16 @@ void audio_i2s_init(void) { __ASSERT_NO_MSG(state == AUDIO_I2S_STATE_UNINIT); - nrfx_err_t ret; - - nrfx_clock_hfclkaudio_config_set(HFCLKAUDIO_12_288_MHZ); - - NRF_CLOCK->TASKS_HFCLKAUDIOSTART = 1; - - /* Wait for ACLK to start */ - while (!NRF_CLOCK_EVENT_HFCLKAUDIOSTARTED) { - k_sleep(K_MSEC(1)); - } + int ret; ret = pinctrl_apply_state(PINCTRL_DT_DEV_CONFIG_GET(I2S_NL), PINCTRL_STATE_DEFAULT); __ASSERT_NO_MSG(ret == 0); - IRQ_CONNECT(DT_IRQN(I2S_NL), DT_IRQ(I2S_NL, priority), nrfx_isr, nrfx_i2s_0_irq_handler, 0); + IRQ_CONNECT(DT_IRQN(I2S_NL), DT_IRQ(I2S_NL, priority), nrfx_i2s_irq_handler, &i2s_inst, 0); irq_enable(DT_IRQN(I2S_NL)); ret = nrfx_i2s_init(&i2s_inst, &cfg, i2s_comp_handler); - __ASSERT_NO_MSG(ret == NRFX_SUCCESS); + __ASSERT_NO_MSG(ret == 0); state = AUDIO_I2S_STATE_IDLE; } diff --git a/src/modules/audio_i2s.h b/src/modules/audio_i2s.h index ea217ee3..ccd8c077 100644 --- a/src/modules/audio_i2s.h +++ b/src/modules/audio_i2s.h @@ -4,6 +4,15 @@ * SPDX-License-Identifier: LicenseRef-PCFT */ +/** @file + * @defgroup audio_app_i2s Audio I2S + * @{ + * @brief Audio I2S interface API for Audio applications. + * + * This module provides the I2S (Inter-IC Sound) interface for audio data transfer + * between the application and external audio hardware. + */ + #ifndef _AUDIO_I2S_H_ #define _AUDIO_I2S_H_ @@ -11,20 +20,11 @@ #include /** - * Calculation: - * FREQ_VALUE = 2^16 * ((12 * f_out / 32M) - 4) - * f_out == 12.288 - * 39845.888 = 2^16 * ((12 * 12.288 / 32M) - 4) - * 39846 = 0x9BA6 - */ -#define HFCLKAUDIO_12_288_MHZ 0x9BA6 -//#define HFCLKAUDIO_12_288_MHZ 0x9B9D -#define HFCLKAUDIO_12_165_MHZ 0x8FD8 -#define HFCLKAUDIO_12_411_MHZ 0xA774 - -/* - * Calculate the number of bytes of one frame, as per now, this frame can either - * be 10 or 7.5 ms. Since we can't have floats in a define we use 15/2 instead + * @brief Calculate frame size in bytes based on audio configuration. + * + * This macro calculates the number of bytes in one audio frame based on the + * configured I2S sample rate, number of channels, and bit depth. The frame + * duration can be either 10 ms or 7.5 ms. */ #if ((CONFIG_AUDIO_FRAME_DURATION_US == 7500) && CONFIG_SW_CODEC_LC3) @@ -36,11 +36,16 @@ ((CONFIG_I2S_LRCK_FREQ_HZ / 1000 * 10) * CONFIG_I2S_CH_NUM * CONFIG_AUDIO_BIT_DEPTH_OCTETS) #endif /* ((CONFIG_AUDIO_FRAME_DURATION_US == 7500) && CONFIG_SW_CODEC_LC3) */ -#define BLOCK_SIZE_BYTES (FRAME_SIZE_BYTES / CONFIG_FIFO_FRAME_SPLIT_NUM) +#define FRAME_SIZE_MONO_BYTES (FRAME_SIZE_BYTES / CONFIG_I2S_CH_NUM) +#define BLOCK_SIZE_BYTES (FRAME_SIZE_BYTES / CONFIG_FIFO_FRAME_SPLIT_NUM) -/* - * Calculate the number of samples in a block, divided by the number of samples - * that will fit within a 32-bit word +/** + * @brief Calculate number of samples per I2S block. + * + * This macro calculates the number of audio samples in each I2S block, + * accounting for the bit depth and 32-bit word alignment. The calculation + * determines how many samples fit within a 32-bit word based on the + * configured audio bit depth. */ #define I2S_SAMPLES_NUM \ (BLOCK_SIZE_BYTES / (CONFIG_AUDIO_BIT_DEPTH_OCTETS) / (32 / CONFIG_AUDIO_BIT_DEPTH_BITS)) @@ -88,4 +93,8 @@ void audio_i2s_blk_comp_cb_register(i2s_blk_comp_callback_t blk_comp_callback); */ void audio_i2s_init(void); +/** + * @} + */ + #endif /* _AUDIO_I2S_H_ */ diff --git a/src/modules/audio_sync_timer.c b/src/modules/audio_sync_timer.c index 2cf94eb9..cda1d310 100644 --- a/src/modules/audio_sync_timer.c +++ b/src/modules/audio_sync_timer.c @@ -4,286 +4,267 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ - #include "audio_sync_timer.h" - - #include - #include - #include - #include - #include - #include - #include - #include - - #include - LOG_MODULE_REGISTER(audio_sync_timer, CONFIG_AUDIO_SYNC_TIMER_LOG_LEVEL); - - #define AUDIO_SYNC_TIMER_NET_APP_IPC_EVT_CHANNEL 4 - #define AUDIO_SYNC_TIMER_NET_APP_IPC_EVT NRF_IPC_EVENT_RECEIVE_4 - - #define AUDIO_SYNC_HF_TIMER_INSTANCE_NUMBER 1 - - #define AUDIO_SYNC_HF_TIMER_I2S_FRAME_START_EVT_CAPTURE_CHANNEL 0 - #define AUDIO_SYNC_HF_TIMER_I2S_FRAME_START_EVT_CAPTURE NRF_TIMER_TASK_CAPTURE0 - #define AUDIO_SYNC_HF_TIMER_CURR_TIME_CAPTURE_CHANNEL 1 - #define AUDIO_SYNC_HF_TIMER_CURR_TIME_CAPTURE NRF_TIMER_TASK_CAPTURE1 - - static const nrfx_timer_t audio_sync_hf_timer_instance = - NRFX_TIMER_INSTANCE(AUDIO_SYNC_HF_TIMER_INSTANCE_NUMBER); - - static uint8_t dppi_channel_i2s_frame_start; - - #define AUDIO_SYNC_LF_TIMER_INSTANCE_NUMBER 0 - - #define AUDIO_SYNC_LF_TIMER_I2S_FRAME_START_EVT_CAPTURE_CHANNEL 0 - #define AUDIO_SYNC_LF_TIMER_I2S_FRAME_START_EVT_CAPTURE NRF_RTC_TASK_CAPTURE_0 - #define AUDIO_SYNC_LF_TIMER_CURR_TIME_CAPTURE_CHANNEL 1 - #define AUDIO_SYNC_LF_TIMER_CURR_TIME_CAPTURE NRF_RTC_TASK_CAPTURE_1 - #define CC_GET_CALLS_MAX 30 - - static uint8_t dppi_channel_curr_time_capture; - - static const nrfx_rtc_config_t rtc_cfg = NRFX_RTC_DEFAULT_CONFIG; - - static const nrfx_rtc_t audio_sync_lf_timer_instance = - NRFX_RTC_INSTANCE(AUDIO_SYNC_LF_TIMER_INSTANCE_NUMBER); - - static uint8_t dppi_channel_timer_sync_with_rtc; - static uint8_t dppi_channel_rtc_start; - static volatile uint32_t num_rtc_overflows; - - static nrfx_timer_config_t cfg = {.frequency = NRFX_MHZ_TO_HZ(1UL), - .mode = NRF_TIMER_MODE_TIMER, - .bit_width = NRF_TIMER_BIT_WIDTH_32, - .interrupt_priority = NRFX_TIMER_DEFAULT_CONFIG_IRQ_PRIORITY, - .p_context = NULL}; - - static uint32_t timestamp_from_rtc_and_timer_get(uint32_t ticks, uint32_t remainder_us) - { - const uint64_t rtc_ticks_in_femto_units = 30517578125UL; - const uint32_t rtc_overflow_time_us = 512000000UL; - - return ((ticks * rtc_ticks_in_femto_units) / 1000000000UL) + - (num_rtc_overflows * rtc_overflow_time_us) + remainder_us; - } - - uint32_t audio_sync_timer_capture(void) - { - /* Ensure that the follow product specification statement is handled: - * - * There is a delay of 6 PCLK16M periods from when the TASKS_CAPTURE[n] is triggered - * until the corresponding CC[n] register is updated. - * - * Lets have a stale value in the CC[n] register and compare that it is different when - * we capture using DPPI. - * - * We ensure it is stale by setting it as the previous tick relative to current - * counter value. - */ - uint32_t tick_stale = nrf_rtc_counter_get(audio_sync_lf_timer_instance.p_reg); - - /* Set a stale value in the CC[n] register */ - tick_stale--; - nrf_rtc_cc_set(audio_sync_lf_timer_instance.p_reg, - AUDIO_SYNC_LF_TIMER_CURR_TIME_CAPTURE_CHANNEL, tick_stale); - - /* Trigger EGU task to capture RTC and TIMER value */ - nrf_egu_task_trigger(NRF_EGU0, NRF_EGU_TASK_TRIGGER0); - - /* Read captured RTC value */ - uint32_t tick = nrf_rtc_cc_get(audio_sync_lf_timer_instance.p_reg, - AUDIO_SYNC_LF_TIMER_CURR_TIME_CAPTURE_CHANNEL); - - /* If required, wait until CC[n] register is updated */ - while (tick == tick_stale) { - tick = nrf_rtc_cc_get(audio_sync_lf_timer_instance.p_reg, - AUDIO_SYNC_LF_TIMER_CURR_TIME_CAPTURE_CHANNEL); - } - - /* Read captured TIMER value */ - uint32_t remainder_us = - nrf_timer_cc_get(NRF_TIMER1, AUDIO_SYNC_HF_TIMER_CURR_TIME_CAPTURE_CHANNEL); - - return timestamp_from_rtc_and_timer_get(tick, remainder_us); - } - - uint32_t audio_sync_timer_capture_get(void) - { - uint32_t cc_get_calls = 0; - uint32_t tick = 0; - static uint32_t prev_tick; - uint32_t remainder_us = 0; - static uint32_t prev_remainder_us; - - /* This function is called too soon after I2S frame start may - * result in values not yet being updated in the *_cc_get calls. - * Ref: OCT-2585. To ensure new values are fetched, they are - * read in a while-loop with a timeout. - */ - - do { - tick = nrf_rtc_cc_get(audio_sync_lf_timer_instance.p_reg, - AUDIO_SYNC_LF_TIMER_I2S_FRAME_START_EVT_CAPTURE_CHANNEL); - cc_get_calls++; - if (cc_get_calls > CC_GET_CALLS_MAX) { - LOG_WRN("Unable to get new CC value"); - break; - } - } while (tick == prev_tick); - - cc_get_calls = 0; - - do { - remainder_us = nrf_timer_cc_get( - NRF_TIMER1, AUDIO_SYNC_HF_TIMER_I2S_FRAME_START_EVT_CAPTURE_CHANNEL); - cc_get_calls++; - if (cc_get_calls > CC_GET_CALLS_MAX) { - LOG_WRN("Unable to get new CC value"); - break; - } - } while (remainder_us == prev_remainder_us); - - prev_tick = tick; - prev_remainder_us = remainder_us; - - return timestamp_from_rtc_and_timer_get(tick, remainder_us); - } - - static void unused_timer_isr_handler(nrf_timer_event_t event_type, void *ctx) - { - ARG_UNUSED(event_type); - ARG_UNUSED(ctx); - } - - static void rtc_isr_handler(nrfx_rtc_int_type_t int_type) - { - if (int_type == NRFX_RTC_INT_OVERFLOW) { - num_rtc_overflows++; - } - } - - /** - * @brief Initialize audio sync timer - * - * @note The audio sync timers is replicating the controller's clock. - * The controller starts or clears the sync timer using a PPI signal - * sent from the controller. This makes the two clocks synchronized. - * - * @return 0 if successful, error otherwise - */ - static int audio_sync_timer_init(void) - { - nrfx_err_t ret; - nrfx_dppi_t dppi = NRFX_DPPI_INSTANCE(0); - - ret = nrfx_timer_init(&audio_sync_hf_timer_instance, &cfg, unused_timer_isr_handler); - if (ret - NRFX_ERROR_BASE_NUM) { - LOG_ERR("nrfx timer init error: %d", ret); - return -ENODEV; - } - - ret = nrfx_rtc_init(&audio_sync_lf_timer_instance, &rtc_cfg, rtc_isr_handler); - if (ret - NRFX_ERROR_BASE_NUM) { - LOG_ERR("nrfx rtc init error: %d", ret); - return -ENODEV; - } - - IRQ_CONNECT(RTC0_IRQn, IRQ_PRIO_LOWEST, nrfx_isr, nrfx_rtc_0_irq_handler, 0); - nrfx_rtc_overflow_enable(&audio_sync_lf_timer_instance, true); - - /* Initialize capturing of I2S frame start event timestamps */ - ret = nrfx_dppi_channel_alloc(&dppi, &dppi_channel_i2s_frame_start); - if (ret - NRFX_ERROR_BASE_NUM) { - LOG_ERR("nrfx DPPI channel alloc error (I2S frame start): %d", ret); - return -ENOMEM; - } - - nrf_timer_subscribe_set(audio_sync_hf_timer_instance.p_reg, - AUDIO_SYNC_HF_TIMER_I2S_FRAME_START_EVT_CAPTURE, - dppi_channel_i2s_frame_start); - - /* Initialize capturing of I2S frame start event timestamps at the RTC as well. */ - nrf_rtc_subscribe_set(audio_sync_lf_timer_instance.p_reg, - AUDIO_SYNC_LF_TIMER_I2S_FRAME_START_EVT_CAPTURE, - dppi_channel_i2s_frame_start); - - nrf_i2s_publish_set(NRF_I2S0, NRF_I2S_EVENT_FRAMESTART, dppi_channel_i2s_frame_start); - ret = nrfx_dppi_channel_enable(&dppi, dppi_channel_i2s_frame_start); - if (ret - NRFX_ERROR_BASE_NUM) { - LOG_ERR("nrfx DPPI channel enable error (I2S frame start): %d", ret); - return -EIO; - } - - /* Initialize capturing of current timestamps */ - ret = nrfx_dppi_channel_alloc(&dppi, &dppi_channel_curr_time_capture); - if (ret - NRFX_ERROR_BASE_NUM) { - LOG_ERR("nrfx DPPI channel alloc error (I2S frame start) - Return value: %d", ret); - return -ENOMEM; - } - - nrf_rtc_subscribe_set(audio_sync_lf_timer_instance.p_reg, - AUDIO_SYNC_LF_TIMER_CURR_TIME_CAPTURE, - dppi_channel_curr_time_capture); - - nrf_timer_subscribe_set(audio_sync_hf_timer_instance.p_reg, - AUDIO_SYNC_HF_TIMER_CURR_TIME_CAPTURE, - dppi_channel_curr_time_capture); - - nrf_egu_publish_set(NRF_EGU0, NRF_EGU_EVENT_TRIGGERED0, dppi_channel_curr_time_capture); - - ret = nrfx_dppi_channel_enable(&dppi, dppi_channel_curr_time_capture); - if (ret - NRFX_ERROR_BASE_NUM) { - LOG_ERR("nrfx DPPI channel enable error (I2S frame start) - Return value: %d", ret); - return -EIO; - } - - /* Initialize functionality for synchronization between APP and NET core */ - ret = nrfx_dppi_channel_alloc(&dppi, &dppi_channel_rtc_start); - if (ret - NRFX_ERROR_BASE_NUM) { - LOG_ERR("nrfx DPPI channel alloc error (timer clear): %d", ret); - return -ENOMEM; - } - - nrf_rtc_subscribe_set(audio_sync_lf_timer_instance.p_reg, NRF_RTC_TASK_CLEAR, - dppi_channel_rtc_start); - nrf_timer_subscribe_set(audio_sync_hf_timer_instance.p_reg, NRF_TIMER_TASK_START, - dppi_channel_rtc_start); - - nrf_ipc_receive_config_set(NRF_IPC, AUDIO_SYNC_TIMER_NET_APP_IPC_EVT_CHANNEL, - NRF_IPC_CHANNEL_4); - nrf_ipc_publish_set(NRF_IPC, AUDIO_SYNC_TIMER_NET_APP_IPC_EVT, dppi_channel_rtc_start); - - ret = nrfx_dppi_channel_enable(&dppi, dppi_channel_rtc_start); - if (ret - NRFX_ERROR_BASE_NUM) { - LOG_ERR("nrfx DPPI channel enable error (timer clear): %d", ret); - return -EIO; - } - - /* Initialize functionality for synchronization between RTC and TIMER */ - ret = nrfx_dppi_channel_alloc(&dppi, &dppi_channel_timer_sync_with_rtc); - if (ret - NRFX_ERROR_BASE_NUM) { - LOG_ERR("nrfx DPPI channel alloc error (timer clear): %d", ret); - return -ENOMEM; - } - - nrf_rtc_publish_set(audio_sync_lf_timer_instance.p_reg, NRF_RTC_EVENT_TICK, - dppi_channel_timer_sync_with_rtc); - nrf_timer_subscribe_set(audio_sync_hf_timer_instance.p_reg, NRF_TIMER_TASK_CLEAR, - dppi_channel_timer_sync_with_rtc); - - nrfx_rtc_tick_enable(&audio_sync_lf_timer_instance, false); - - ret = nrfx_dppi_channel_enable(&dppi, dppi_channel_timer_sync_with_rtc); - if (ret - NRFX_ERROR_BASE_NUM) { - LOG_ERR("nrfx DPPI channel enable error (timer clear): %d", ret); - return -EIO; - } - - nrfx_rtc_enable(&audio_sync_lf_timer_instance); - - LOG_DBG("Audio sync timer initialized"); - - return 0; - } - - SYS_INIT(audio_sync_timer_init, POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT); \ No newline at end of file +#include "audio_sync_timer.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +LOG_MODULE_REGISTER(audio_sync_timer, CONFIG_AUDIO_SYNC_TIMER_LOG_LEVEL); + +#define AUDIO_SYNC_TIMER_NET_APP_IPC_EVT_CHANNEL 4 +#define AUDIO_SYNC_TIMER_NET_APP_IPC_EVT NRF_IPC_EVENT_RECEIVE_4 + +#define AUDIO_SYNC_HF_TIMER_INSTANCE_NUMBER 1 + +#define AUDIO_SYNC_HF_TIMER_I2S_FRAME_START_EVT_CAPTURE_CHANNEL 0 +#define AUDIO_SYNC_HF_TIMER_I2S_FRAME_START_EVT_CAPTURE NRF_TIMER_TASK_CAPTURE0 +#define AUDIO_SYNC_HF_TIMER_CURR_TIME_CAPTURE_CHANNEL 1 +#define AUDIO_SYNC_HF_TIMER_CURR_TIME_CAPTURE NRF_TIMER_TASK_CAPTURE1 + +static nrfx_timer_t audio_sync_hf_timer_instance = + NRFX_TIMER_INSTANCE(NRF_TIMER_INST_GET(AUDIO_SYNC_HF_TIMER_INSTANCE_NUMBER)); + +static nrfx_gppi_handle_t dppi_handle_i2s_frame_start; + +#define AUDIO_SYNC_LF_TIMER_INSTANCE NRF_RTC0 + +#define AUDIO_SYNC_LF_TIMER_I2S_FRAME_START_EVT_CAPTURE_CHANNEL 0 +#define AUDIO_SYNC_LF_TIMER_I2S_FRAME_START_EVT_CAPTURE NRF_RTC_TASK_CAPTURE_0 +#define AUDIO_SYNC_LF_TIMER_CURR_TIME_CAPTURE_CHANNEL 1 +#define AUDIO_SYNC_LF_TIMER_CURR_TIME_CAPTURE NRF_RTC_TASK_CAPTURE_1 +#define CC_GET_CALLS_MAX 20 + +static nrfx_gppi_handle_t dppi_handle_curr_time_capture; + +static const nrfx_rtc_config_t rtc_cfg = NRFX_RTC_DEFAULT_CONFIG; + +static nrfx_rtc_t audio_sync_lf_timer_instance = NRFX_RTC_INSTANCE(AUDIO_SYNC_LF_TIMER_INSTANCE); + +static nrfx_gppi_handle_t dppi_handle_timer_sync_with_rtc; +static nrfx_gppi_handle_t dppi_handle_rtc_start; +static volatile uint32_t num_rtc_overflows; + +static nrfx_timer_config_t cfg = {.frequency = NRFX_MHZ_TO_HZ(1UL), + .mode = NRF_TIMER_MODE_TIMER, + .bit_width = NRF_TIMER_BIT_WIDTH_32, + .interrupt_priority = NRFX_TIMER_DEFAULT_CONFIG_IRQ_PRIORITY, + .p_context = NULL}; + +static uint32_t timestamp_from_rtc_and_timer_get(uint32_t ticks, uint32_t remainder_us) +{ + const uint64_t rtc_ticks_in_femto_units = 30517578125UL; + const uint32_t rtc_overflow_time_us = 512000000UL; + + return ((ticks * rtc_ticks_in_femto_units) / 1000000000UL) + + (num_rtc_overflows * rtc_overflow_time_us) + remainder_us; +} + +uint32_t audio_sync_timer_capture(void) +{ + /* Ensure that the follow product specification statement is handled: + * + * There is a delay of 6 PCLK16M periods from when the TASKS_CAPTURE[n] is triggered + * until the corresponding CC[n] register is updated. + * + * Lets have a stale value in the CC[n] register and compare that it is different when + * we capture using DPPI. + * + * We ensure it is stale by setting it as the previous tick relative to current + * counter value. + */ + uint32_t tick_stale = nrf_rtc_counter_get(audio_sync_lf_timer_instance.p_reg); + + /* Set a stale value in the CC[n] register */ + tick_stale--; + nrf_rtc_cc_set(audio_sync_lf_timer_instance.p_reg, + AUDIO_SYNC_LF_TIMER_CURR_TIME_CAPTURE_CHANNEL, tick_stale); + + /* Trigger EGU task to capture RTC and TIMER value */ + nrf_egu_task_trigger(NRF_EGU0, NRF_EGU_TASK_TRIGGER0); + + /* Read captured RTC value */ + uint32_t tick = nrf_rtc_cc_get(audio_sync_lf_timer_instance.p_reg, + AUDIO_SYNC_LF_TIMER_CURR_TIME_CAPTURE_CHANNEL); + + /* If required, wait until CC[n] register is updated */ + while (tick == tick_stale) { + tick = nrf_rtc_cc_get(audio_sync_lf_timer_instance.p_reg, + AUDIO_SYNC_LF_TIMER_CURR_TIME_CAPTURE_CHANNEL); + } + + /* Read captured TIMER value */ + uint32_t remainder_us = + nrf_timer_cc_get(NRF_TIMER1, AUDIO_SYNC_HF_TIMER_CURR_TIME_CAPTURE_CHANNEL); + + return timestamp_from_rtc_and_timer_get(tick, remainder_us); +} + +uint32_t audio_sync_timer_frame_start_capture_get(void) +{ + uint32_t cc_get_calls = 0; + uint32_t tick = 0; + static uint32_t prev_tick; + uint32_t remainder_us = 0; + + /* This ISR may be called before the I2S FRAMESTART event which does timer capture, + * resulting in values not yet being updated in the *_cc_get calls. + * To ensure new values are fetched, they are read in a while-loop with a timeout. + * Depending on variables such as sample frequency, this delay varies. + * For 16 kHz sampling rate is about 20 us. Less for 24 kHz and 48 kHz. + * Ref: OCT-2585 and OCT-3368. + */ + + tick = nrf_rtc_cc_get(audio_sync_lf_timer_instance.p_reg, + AUDIO_SYNC_LF_TIMER_I2S_FRAME_START_EVT_CAPTURE_CHANNEL); + + while (tick == prev_tick) { + k_busy_wait(1); + tick = nrf_rtc_cc_get(audio_sync_lf_timer_instance.p_reg, + AUDIO_SYNC_LF_TIMER_I2S_FRAME_START_EVT_CAPTURE_CHANNEL); + cc_get_calls++; + if (cc_get_calls > CC_GET_CALLS_MAX) { + LOG_ERR("Unable to get new tick from nrf_rtc_cc_get"); + break; + } + }; + + /* The HF timer is cleared on every I2S frame. Hence, this value can be + * the same many times in a row. + */ + remainder_us = nrf_timer_cc_get(NRF_TIMER1, + AUDIO_SYNC_HF_TIMER_I2S_FRAME_START_EVT_CAPTURE_CHANNEL); + + prev_tick = tick; + + return timestamp_from_rtc_and_timer_get(tick, remainder_us); +} + +static void unused_timer_isr_handler(nrf_timer_event_t event_type, void *ctx) +{ + ARG_UNUSED(event_type); + ARG_UNUSED(ctx); +} + +static void rtc_isr_handler(nrf_rtc_event_t event_type, void *ctx) +{ + ARG_UNUSED(ctx); + + if (event_type == NRF_RTC_EVENT_OVERFLOW) { + num_rtc_overflows++; + } +} + +/** + * @brief Initialize audio sync timer + * + * @note The audio sync timers is replicating the controller's clock. + * The controller starts or clears the sync timer using a PPI signal + * sent from the controller. This makes the two clocks synchronized. + * + * @return 0 if successful, error otherwise + */ +static int audio_sync_timer_init(void) +{ + int ret; + uint32_t eep0, tep0, tep1; + + ret = nrfx_timer_init(&audio_sync_hf_timer_instance, &cfg, unused_timer_isr_handler); + if (ret < 0) { + LOG_ERR("nrfx timer init error: %d", ret); + return -ENODEV; + } + + ret = nrfx_rtc_init(&audio_sync_lf_timer_instance, &rtc_cfg, rtc_isr_handler); + if (ret < 0) { + LOG_ERR("nrfx rtc init error: %d", ret); + return -ENODEV; + } + + IRQ_CONNECT(RTC0_IRQn, IRQ_PRIO_LOWEST, nrfx_rtc_irq_handler, + &audio_sync_lf_timer_instance, 0); + nrfx_rtc_overflow_enable(&audio_sync_lf_timer_instance, true); + + /* Initialize capturing of I2S frame start event timestamps */ + eep0 = nrf_i2s_event_address_get(NRF_I2S0, NRF_I2S_EVENT_FRAMESTART); + tep0 = nrfx_rtc_task_address_get(&audio_sync_lf_timer_instance, + AUDIO_SYNC_LF_TIMER_I2S_FRAME_START_EVT_CAPTURE); + tep1 = nrfx_timer_task_address_get(&audio_sync_hf_timer_instance, + AUDIO_SYNC_HF_TIMER_I2S_FRAME_START_EVT_CAPTURE); + + /* A firmware handoff can retain the previous image's DPPI endpoints. */ + nrfx_gppi_ep_clear(eep0); + nrfx_gppi_ep_clear(tep0); + nrfx_gppi_ep_clear(tep1); + ret = nrfx_gppi_conn_alloc(eep0, tep0, &dppi_handle_i2s_frame_start); + if (ret < 0) { + LOG_ERR("nrfx DPPI channel alloc error (I2S frame start): %d", ret); + return ret; + } + + nrfx_gppi_ep_attach(tep1, dppi_handle_i2s_frame_start); + nrfx_gppi_conn_enable(dppi_handle_i2s_frame_start); + + /* Initialize capturing of current timestamps */ + eep0 = nrf_egu_event_address_get(NRF_EGU0, NRF_EGU_EVENT_TRIGGERED0); + tep0 = nrfx_rtc_task_address_get(&audio_sync_lf_timer_instance, + AUDIO_SYNC_LF_TIMER_CURR_TIME_CAPTURE); + tep1 = nrfx_timer_task_address_get(&audio_sync_hf_timer_instance, + AUDIO_SYNC_HF_TIMER_CURR_TIME_CAPTURE); + + nrfx_gppi_ep_clear(eep0); + nrfx_gppi_ep_clear(tep0); + nrfx_gppi_ep_clear(tep1); + ret = nrfx_gppi_conn_alloc(eep0, tep0, &dppi_handle_curr_time_capture); + if (ret < 0) { + LOG_ERR("nrfx DPPI channel alloc error (I2S frame start): %d", ret); + return ret; + } + nrfx_gppi_ep_attach(tep1, dppi_handle_curr_time_capture); + nrfx_gppi_conn_enable(dppi_handle_curr_time_capture); + + /* Initialize functionality for synchronization between APP and NET core */ + eep0 = nrf_ipc_event_address_get(NRF_IPC, AUDIO_SYNC_TIMER_NET_APP_IPC_EVT); + tep0 = nrfx_rtc_task_address_get(&audio_sync_lf_timer_instance, NRF_RTC_TASK_CLEAR); + tep1 = nrfx_timer_task_address_get(&audio_sync_hf_timer_instance, NRF_TIMER_TASK_START); + nrfx_gppi_ep_clear(eep0); + nrfx_gppi_ep_clear(tep0); + nrfx_gppi_ep_clear(tep1); + ret = nrfx_gppi_conn_alloc(eep0, tep0, &dppi_handle_rtc_start); + if (ret < 0) { + LOG_ERR("nrfx DPPI channel alloc error (I2S frame start): %d", ret); + return ret; + } + nrfx_gppi_ep_attach(tep1, dppi_handle_rtc_start); + nrf_ipc_receive_config_set(NRF_IPC, AUDIO_SYNC_TIMER_NET_APP_IPC_EVT_CHANNEL, + NRF_IPC_CHANNEL_4); + nrfx_gppi_conn_enable(dppi_handle_rtc_start); + + /* Initialize functionality for synchronization between RTC and TIMER */ + eep0 = nrfx_rtc_event_address_get(&audio_sync_lf_timer_instance, NRF_RTC_EVENT_TICK); + tep0 = nrfx_timer_task_address_get(&audio_sync_hf_timer_instance, NRF_TIMER_TASK_CLEAR); + nrfx_gppi_ep_clear(eep0); + nrfx_gppi_ep_clear(tep0); + ret = nrfx_gppi_conn_alloc(eep0, tep0, &dppi_handle_timer_sync_with_rtc); + if (ret < 0) { + LOG_ERR("nrfx DPPI channel alloc error (I2S frame start): %d", ret); + return ret; + } + + nrfx_rtc_tick_enable(&audio_sync_lf_timer_instance, false); + nrfx_gppi_conn_enable(dppi_handle_timer_sync_with_rtc); + + nrfx_rtc_enable(&audio_sync_lf_timer_instance); + + LOG_DBG("Audio sync timer initialized"); + + return 0; +} + +SYS_INIT(audio_sync_timer_init, POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT); diff --git a/src/modules/audio_sync_timer.h b/src/modules/audio_sync_timer.h index 56dccf6d..8e9ac70b 100644 --- a/src/modules/audio_sync_timer.h +++ b/src/modules/audio_sync_timer.h @@ -4,34 +4,47 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ - #ifndef _AUDIO_SYNC_TIMER_H_ - #define _AUDIO_SYNC_TIMER_H_ - - #include - #include - - /** - * @brief Capture a timestamp on the sync timer. - * - * @retval The current timestamp of the audio sync timer. - */ - uint32_t audio_sync_timer_capture(void); - - /** - * @brief Returns the last captured value of the sync timer. - * - * The captured time is corresponding to the I2S frame start. - * NOTE: This function is not reentrant and must only be called - * once in the I2S ISR. There may be a delay in the capturing of - * the clock value. Hence, there is a retry-loop with a timeout. - * Should we not get the new capture value before the timeout, - * a warning will be printed and calculations based on the old - * timer capture values. - * - * See @ref audio_sync_timer_capture(). - * - * @retval The last captured timestamp of the audio sync timer. - */ - uint32_t audio_sync_timer_capture_get(void); - - #endif /* _AUDIO_SYNC_TIMER_H_ */ \ No newline at end of file +/** @file + * @defgroup audio_app_sync_timer Audio Sync Timer + * @{ + * @brief Audio synchronization timer API for Audio applications. + * + * This module provides precise timing functionality for audio synchronization across + * multiple devices. + */ + +#ifndef _AUDIO_SYNC_TIMER_H_ +#define _AUDIO_SYNC_TIMER_H_ + +#include +#include + +/** + * @brief Capture a timestamp on the sync timer. + * + * @retval The current timestamp of the audio sync timer. + */ +uint32_t audio_sync_timer_capture(void); + +/** + * @brief Returns the last captured value of the sync timer. + * + * The captured time is corresponding to the I2S frame start. + * NOTE: This function is not reentrant and must only be called + * once in the I2S ISR. There may be a delay in the capturing of + * the clock value. Hence, there is a retry-loop with a timeout. + * Should we not get the new capture value before the timeout, + * a warning will be printed and calculations based on the old + * timer capture values. + * + * See @ref audio_sync_timer_capture(). + * + * @retval The last captured timestamp of the audio sync timer. + */ +uint32_t audio_sync_timer_frame_start_capture_get(void); + +/** + * @} + */ + +#endif /* _AUDIO_SYNC_TIMER_H_ */ diff --git a/src/modules/audio_usb.c b/src/modules/audio_usb.c index 2b8b330f..898a5d2a 100644 --- a/src/modules/audio_usb.c +++ b/src/modules/audio_usb.c @@ -7,208 +7,449 @@ #include "audio_usb.h" #include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include "macros_common.h" +#include "device_location.h" +#include #include LOG_MODULE_REGISTER(audio_usb, CONFIG_MODULE_AUDIO_USB_LOG_LEVEL); -#define USB_FRAME_SIZE_STEREO \ - (((CONFIG_AUDIO_SAMPLE_RATE_HZ * CONFIG_AUDIO_BIT_DEPTH_OCTETS) / 1000) * 2) +#define TERMINAL_ID_HEADSET_OUT UAC2_ENTITY_ID(DT_NODELABEL(out_terminal)) +#define TERMINAL_ID_HEADPHONES_OUT UAC2_ENTITY_ID(DT_NODELABEL(hp_out_terminal)) +#define TERMINAL_ID_HEADSET_IN UAC2_ENTITY_ID(DT_NODELABEL(in_terminal)) -static struct data_fifo *fifo_tx; -static struct data_fifo *fifo_rx; +/* Absolute minimum is 2 OUT buffers but add 2 additional buffers to prevent out of memory + * errors when USB host decides to perform rapid terminal enable/disable cycles. + */ +#define USB_BLOCKS 4 -NET_BUF_POOL_FIXED_DEFINE(pool_out, CONFIG_FIFO_FRAME_SPLIT_NUM, USB_FRAME_SIZE_STEREO, 8, - net_buf_destroy); +/* See udc_buf.h for more information about UDC_BUF_GRANULARITY and UDC_BUF_ALIGN */ +K_MEM_SLAB_DEFINE_STATIC(usb_out_slab, ROUND_UP(USB_BLOCK_MULTI_CHAN_1MS_SIZE, UDC_BUF_GRANULARITY), + USB_BLOCKS, UDC_BUF_ALIGN); +K_MEM_SLAB_DEFINE_STATIC(usb_in_slab, ROUND_UP(USB_BLOCK_MULTI_CHAN_1MS_SIZE, UDC_BUF_GRANULARITY), + USB_BLOCKS, UDC_BUF_ALIGN); -static uint32_t rx_num_overruns; -static bool rx_first_data; -static bool tx_first_data; +static struct k_msgq *audio_q_in; +static struct k_msgq *audio_q_out; -#if (CONFIG_STREAM_BIDIRECTIONAL) -static uint32_t tx_num_underruns; +/* USB blocks are 1 ms each, but we split them into 0.5 ms blocks for processing, + * hence the dividing by two + */ +NET_BUF_POOL_FIXED_DEFINE(pool_in, USB_BLOCKS, + (USB_BLOCK_MULTI_CHAN_1MS_SIZE * CONFIG_FIFO_FRAME_SPLIT_NUM / 2), + sizeof(struct audio_metadata), NULL); + +static uint32_t in_num_overruns; +static uint32_t out_num_underruns; +static bool terminal_headset_out_enabled; +static bool terminal_headphones_out_enabled; +static bool terminal_headset_in_enabled; + +static bool playing_state; +static bool local_host_in; +static bool local_host_out; +static bool usb_sof_synchronized; + +/* The meta data for the USB and that required for the following audio system. */ +struct audio_metadata usb_in_meta = {.data_coding = PCM, + .data_len_us = 1000, + .sample_rate_hz = CONFIG_AUDIO_SAMPLE_RATE_HZ, + .bits_per_sample = CONFIG_AUDIO_BIT_DEPTH_BITS, + .carried_bits_per_sample = CONFIG_AUDIO_BIT_DEPTH_BITS, + .bytes_per_location = USB_BLOCK_MULTI_CHAN_1MS_SIZE / 2, + .interleaved = true, + .locations = BT_AUDIO_LOCATION_FRONT_LEFT | + BT_AUDIO_LOCATION_FRONT_RIGHT, + .bad_data = 0}; + +static void terminal_update_cb(const struct device *dev, uint8_t terminal, bool enabled, + bool microframes, void *user_data) +{ + ARG_UNUSED(dev); + ARG_UNUSED(microframes); + ARG_UNUSED(user_data); + + if (terminal == TERMINAL_ID_HEADSET_OUT) { + terminal_headset_out_enabled = enabled; + } else if (terminal == TERMINAL_ID_HEADPHONES_OUT) { + terminal_headphones_out_enabled = enabled; + } else if (terminal == TERMINAL_ID_HEADSET_IN) { + terminal_headset_in_enabled = enabled; + out_num_underruns = 0; + } else { + LOG_WRN("Unknown terminal ID: %d", terminal); + return; + } + + LOG_DBG("Terminal %d %s", terminal, enabled ? "enabled" : "disabled"); +} -static void data_write(const struct device *dev) +static void usb_send_cb(const struct device *dev, void *user_data) { int ret; + void *pcm_buf; + struct net_buf *usb_data_out; + + /* Fast path exit - cache combined condition */ + bool out_ready = (audio_q_in != NULL && terminal_headset_in_enabled && playing_state && + local_host_in); - if (fifo_tx == NULL) { + if (unlikely(!out_ready)) { return; } - void *data_out; - size_t data_out_size; - struct net_buf *buf_out; - - buf_out = net_buf_alloc(&pool_out, K_NO_WAIT); + ret = k_mem_slab_alloc(&usb_out_slab, &pcm_buf, K_NO_WAIT); + if (unlikely(ret != 0)) { + LOG_WRN("Could not allocate pcm_buf, ret: %d", ret); + return; + } - ret = data_fifo_pointer_last_filled_get(fifo_tx, &data_out, &data_out_size, K_NO_WAIT); - if (ret) { - tx_num_underruns++; - if ((tx_num_underruns % 100) == 1) { - LOG_WRN("USB TX underrun. Num: %d", tx_num_underruns); + ret = k_msgq_get(audio_q_in, &usb_data_out, K_NO_WAIT); + if (unlikely(ret)) { + /* Reduce logging overhead */ + if (unlikely((++out_num_underruns % 100) == 1)) { + LOG_WRN("USB OUT underrun. Num: %d", out_num_underruns); } - net_buf_unref(buf_out); + k_mem_slab_free(&usb_out_slab, pcm_buf); return; } - memcpy(buf_out->data, data_out, data_out_size); - data_fifo_block_free(fifo_tx, data_out); + struct audio_metadata *usb_data_out_meta = net_buf_user_data(usb_data_out); + size_t pcm_size = 0; - if (data_out_size == usb_audio_get_in_frame_size(dev)) { - ret = usb_audio_send(dev, buf_out, data_out_size); + if (audio_metadata_num_loc_get(usb_data_out_meta) == 1) { + /* Mono to stereo copy */ + ret = pscm_copy_pad((char *)usb_data_out->data, usb_data_out->len, + usb_data_out_meta->carried_bits_per_sample, (char *)pcm_buf, + &pcm_size); if (ret) { - LOG_WRN("USB TX failed, ret: %d", ret); - net_buf_unref(buf_out); + k_mem_slab_free(&usb_out_slab, pcm_buf); + net_buf_unref(usb_data_out); + return; } - } else { - LOG_WRN("Wrong size write: %d", data_out_size); + /* Direct copy for stereo */ + memcpy(pcm_buf, usb_data_out->data, usb_data_out->len); + pcm_size = usb_data_out->len; } - if (!tx_first_data) { - LOG_INF("USB TX first data sent."); - tx_first_data = true; + ret = usbd_uac2_send(dev, TERMINAL_ID_HEADSET_IN, pcm_buf, pcm_size); + if (ret) { + LOG_WRN("USB OUT failed, ret: %d", ret); + k_mem_slab_free(&usb_out_slab, pcm_buf); + } + + net_buf_unref(usb_data_out); +} + +static void send_buf_release_cb(const struct device *dev, uint8_t terminal, void *buf, + void *user_data) +{ + ARG_UNUSED(dev); + ARG_UNUSED(user_data); + + if (terminal != TERMINAL_ID_HEADSET_IN) { + LOG_ERR("Buffer release callback for unknown terminal: %d", terminal); + return; } + + k_mem_slab_free(&usb_out_slab, buf); } -#endif /* (CONFIG_STREAM_BIDIRECTIONAL) */ -static void data_received(const struct device *dev, struct net_buf *buffer, size_t size) +static void *get_recv_buf_cb(const struct device *dev, uint8_t terminal, uint16_t size, + void *user_data) { + ARG_UNUSED(dev); + ARG_UNUSED(size); + ARG_UNUSED(user_data); + int ret; - void *data_in; + void *buf = NULL; + + if (terminal == TERMINAL_ID_HEADSET_OUT || terminal == TERMINAL_ID_HEADPHONES_OUT) { + ret = k_mem_slab_alloc(&usb_in_slab, &buf, K_NO_WAIT); + } + + return buf; +} + +static void half_buf_add(struct net_buf *frame, void *data, uint16_t size, + uint32_t *blocks_in_frame) +{ + const uint32_t half_data_len_us = usb_in_meta.data_len_us / 2; + const uint32_t half_bytes_per_location = usb_in_meta.bytes_per_location / 2; + + net_buf_add_mem(frame, data, size); + + struct audio_metadata *meta = net_buf_user_data(frame); + + meta->data_len_us += half_data_len_us; + meta->bytes_per_location += half_bytes_per_location; + (*blocks_in_frame)++; +} + +static void data_recv_cb(const struct device *dev, uint8_t terminal, void *buf, uint16_t size, + void *user_data) +{ + ARG_UNUSED(dev); + ARG_UNUSED(terminal); + ARG_UNUSED(user_data); - if (fifo_rx == NULL) { - /* Throwing away data */ - net_buf_unref(buffer); + int ret; + /* Frame accumulation */ + static struct net_buf *frame_current; + static struct net_buf *frame_spillover; + static uint32_t blocks_in_frame_current; + static uint32_t blocks_in_frame_spillover; + + if (unlikely(buf == NULL)) { + LOG_ERR("Received NULL buffer"); return; } - if (buffer == NULL || size == 0 || buffer->data == NULL) { - /* This should never happen */ - ERR_CHK(-EINVAL); + /* Fast exit conditions */ + if (unlikely(size == 0 || !playing_state) || + !(terminal_headset_out_enabled || terminal_headphones_out_enabled)) { + k_mem_slab_free(&usb_in_slab, buf); + return; } - /* Receive data from USB */ - if (size != USB_FRAME_SIZE_STEREO) { - LOG_WRN("Wrong length: %d", size); - net_buf_unref(buffer); + /* Size validation */ + if (unlikely(size != USB_BLOCK_MULTI_CHAN_1MS_SIZE)) { + LOG_WRN("Incorrect buffer size: %d (%u)", size, USB_BLOCK_MULTI_CHAN_1MS_SIZE); + k_mem_slab_free(&usb_in_slab, buf); return; } - ret = data_fifo_pointer_first_vacant_get(fifo_rx, &data_in, K_NO_WAIT); + /* Allocate new frame if we don't have one */ + if (frame_current == NULL) { + if (frame_spillover != NULL) { + frame_current = frame_spillover; + blocks_in_frame_current = blocks_in_frame_spillover; + frame_spillover = NULL; + blocks_in_frame_spillover = 0; + } else { + /* Check space availability */ + if (unlikely(k_msgq_num_free_get(audio_q_out) == 0 || + pool_in.avail_count == 0)) { + goto overrun_cleanup; + } + + frame_current = net_buf_alloc(&pool_in, K_NO_WAIT); + if (unlikely(frame_current == NULL)) { + LOG_WRN("Out of IN buffers for frame"); + goto overrun_cleanup; + } + + /* Initialize metadata for the first block */ + struct audio_metadata *meta = net_buf_user_data(frame_current); + + *meta = usb_in_meta; + meta->data_len_us = 0; + meta->bytes_per_location = 0; + blocks_in_frame_current = 0; + } + } + + /* Check if we are about to spill over, if so: allocate spill_over frame */ + if ((blocks_in_frame_current + 2) > CONFIG_FIFO_FRAME_SPLIT_NUM) { + const size_t half_size = size / 2; - /* RX FIFO can fill up due to retransmissions or disconnect */ - if (ret == -ENOMEM) { - void *temp; - size_t temp_size; + /* Add half the buffer to current frame */ + half_buf_add(frame_current, buf, half_size, &blocks_in_frame_current); - rx_num_overruns++; - if ((rx_num_overruns % 100) == 1) { - LOG_WRN("USB RX overrun. Num: %d", rx_num_overruns); + if (frame_spillover != NULL) { + LOG_WRN("Previous spillover frame not consumed, dropping it"); + net_buf_unref(frame_spillover); + blocks_in_frame_spillover = 0; } - ret = data_fifo_pointer_last_filled_get(fifo_rx, &temp, &temp_size, K_NO_WAIT); - ERR_CHK(ret); + /* Allocate new frame for spill over data since current frame is full. If allocation + * fails, drop the spill over data to prevent blocking the USB endpoint, but keep + * the current frame to allow it to be sent to the audio system. + */ + frame_spillover = net_buf_alloc(&pool_in, K_NO_WAIT); + if (unlikely(frame_spillover == NULL)) { + LOG_WRN("Out of IN buffers for spill over frame"); + goto overrun_cleanup; + } + + /* Initialize metadata for the first block */ + struct audio_metadata *meta_spill_over = net_buf_user_data(frame_spillover); - data_fifo_block_free(fifo_rx, temp); + *meta_spill_over = usb_in_meta; + meta_spill_over->data_len_us = 0; + meta_spill_over->bytes_per_location = 0; + blocks_in_frame_spillover = 0; - ret = data_fifo_pointer_first_vacant_get(fifo_rx, &data_in, K_NO_WAIT); + /* Add half the buffer to spill over frame */ + half_buf_add(frame_spillover, (char *)buf + half_size, half_size, + &blocks_in_frame_spillover); + } else { + /* Add block data directly to current frame */ + net_buf_add_mem(frame_current, buf, size); + /* Update metadata */ + struct audio_metadata *meta = net_buf_user_data(frame_current); + + /* Accumulate one full 1 ms USB buffer, which counts as two 0.5 ms blocks. */ + meta->data_len_us += usb_in_meta.data_len_us; + meta->bytes_per_location += usb_in_meta.bytes_per_location; + blocks_in_frame_current += 2; } - ERR_CHK_MSG(ret, "RX failed to get block"); + /* Release USB buffer */ + k_mem_slab_free(&usb_in_slab, buf); - memcpy(data_in, buffer->data, size); + /* Check if we have a complete frame */ + if (blocks_in_frame_current >= CONFIG_FIFO_FRAME_SPLIT_NUM) { + /* Put complete frame into IN queue */ + ret = k_msgq_put(audio_q_out, (void *)&frame_current, K_NO_WAIT); + if (ret) { + LOG_ERR("Failed to store complete frame"); + net_buf_unref(frame_current); + } - ret = data_fifo_block_lock(fifo_rx, &data_in, size); - ERR_CHK_MSG(ret, "Failed to lock block"); + /* Reset for next frame */ + frame_current = NULL; + blocks_in_frame_current = 0; + } - net_buf_unref(buffer); + return; - if (!rx_first_data) { - LOG_INF("USB RX first data received."); - rx_first_data = true; +overrun_cleanup: + if (unlikely((++in_num_overruns % 100) == 1)) { + LOG_WRN("USB IN overrun. Num: %d", in_num_overruns); } + + k_mem_slab_free(&usb_in_slab, buf); } -static void feature_update(const struct device *dev, const struct usb_audio_fu_evt *evt) +static struct uac2_ops ops = { + .sof_cb = usb_send_cb, + .terminal_update_cb = terminal_update_cb, + .buf_release_cb = send_buf_release_cb, + .get_recv_buf = get_recv_buf_cb, + .data_recv_cb = data_recv_cb, +}; + +static struct usbd_context *audio_usbd; + +bool audio_usb_headphones_out_enabled(void) { - LOG_DBG("Control selector %d for channel %d updated", evt->cs, evt->channel); - switch (evt->cs) { - case USB_AUDIO_FU_MUTE_CONTROL: - /* Fall through */ - default: - break; - } + return terminal_headphones_out_enabled; } -static const struct usb_audio_ops ops = { - .data_received_cb = data_received, - .feature_update_cb = feature_update, -#if (CONFIG_STREAM_BIDIRECTIONAL) - .data_request_cb = data_write, -#endif /* (CONFIG_STREAM_BIDIRECTIONAL) */ -}; +bool audio_usb_headset_out_enabled(void) +{ + return terminal_headset_out_enabled; +} -int audio_usb_start(struct data_fifo *fifo_tx_in, struct data_fifo *fifo_rx_in) +bool audio_usb_headset_in_enabled(void) { - if (fifo_tx_in == NULL || fifo_rx_in == NULL) { + return terminal_headset_in_enabled; +} + +int audio_usb_start(struct k_msgq *audio_q_out_ptr, struct k_msgq *audio_q_in_ptr) +{ + if (audio_usbd == NULL) { + LOG_ERR("USB device not initialized"); + return -ENOTCONN; + } + + if (audio_q_out_ptr == NULL || audio_q_in_ptr == NULL) { return -EINVAL; } - fifo_tx = fifo_tx_in; - fifo_rx = fifo_rx_in; + audio_q_in = audio_q_out_ptr; + audio_q_out = audio_q_in_ptr; + + playing_state = true; return 0; } -void audio_usb_stop(void) +int audio_usb_stop(void) { - rx_first_data = false; - tx_first_data = false; - fifo_tx = NULL; - fifo_rx = NULL; + if (audio_usbd == NULL) { + LOG_ERR("USB device not initialized"); + return -ENOTCONN; + } + + playing_state = false; + + return 0; } int audio_usb_disable(void) { int ret; - audio_usb_stop(); + if (audio_usbd == NULL) { + LOG_ERR("USB device not initialized"); + return -ENOTCONN; + } - ret = usb_disable(); + ret = usbd_disable(audio_usbd); if (ret) { LOG_ERR("Failed to disable USB"); return ret; } - return 0; + ret = audio_usb_stop(); + + return ret; } -int audio_usb_init(void) +int audio_usb_init(bool host_in, bool host_out) { int ret; - const struct device *hs_dev = DEVICE_DT_GET(DT_NODELABEL(hs_0)); + const struct device *dev; + + local_host_in = host_in; + local_host_out = host_out; + + if (host_in && host_out) { + dev = DEVICE_DT_GET(DT_NODELABEL(uac2_headset)); + usb_sof_synchronized = DT_PROP(DT_NODELABEL(uac_aclk), sof_synchronized); + LOG_INF("USB initialized as bidirectional (headset)."); + } else if (host_out) { + dev = DEVICE_DT_GET(DT_NODELABEL(uac2_headphones)); + usb_sof_synchronized = DT_PROP(DT_NODELABEL(hp_uac_aclk), sof_synchronized); + LOG_INF("USB initialized as unidirectional (headphones only)."); + } else { + LOG_ERR("USB currently only supports output (host out) or bidirectional (host in " + "and host out)."); + return -ENOTSUP; + } - if (!device_is_ready(hs_dev)) { - LOG_ERR("USB Headset Device not ready"); + if (!device_is_ready(dev)) { + LOG_ERR("USB Device not ready"); return -EIO; } - usb_audio_register(hs_dev, &ops); + usbd_uac2_set_ops(dev, &ops, NULL); + + audio_usbd = audio_usbd_init_device(NULL); + if (audio_usbd == NULL) { + return -ENODEV; + } - ret = usb_enable(NULL); + ret = usbd_enable(audio_usbd); if (ret) { LOG_ERR("Failed to enable USB"); return ret; } - LOG_INF("Ready for USB host to send/receive."); + LOG_INF("Ready for USB host to send/receive: %s", + usb_sof_synchronized ? "No Sync (multi-clock) " : "Async (host-adjustable)"); return 0; } diff --git a/src/modules/audio_usb.h b/src/modules/audio_usb.h index 9fe4cc08..50f97404 100644 --- a/src/modules/audio_usb.h +++ b/src/modules/audio_usb.h @@ -4,46 +4,103 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @defgroup audio_app_usb Audio USB + * @{ + * @brief Audio USB interface API for Audio applications. + * + * This module provides USB audio functionality, enabling audio + * input/output through USB connections. + */ + #ifndef _AUDIO_USB_H_ #define _AUDIO_USB_H_ -#include +#include +#include -#if (CONFIG_AUDIO_SOURCE_USB && !CONFIG_AUDIO_SAMPLE_RATE_48000_HZ) -/* Only 48kHz is supported when using USB */ -#error USB only supports 48kHz +#if (CONFIG_AUDIO_SOURCE_USB && !CONFIG_AUDIO_SAMPLE_RATE_48000_HZ && \ + MAX(CONFIG_AUDIO_INPUT_CHANNELS, CONFIG_AUDIO_OUTPUT_CHANNELS) != 2) +/* Only 48kHz stereo is supported when using USB */ +#error USB only supports 48kHz stereo #endif /* (CONFIG_AUDIO_SOURCE_USB && !CONFIG_AUDIO_SAMPLE_RATE_48000_HZ) */ +#define USB_1MS_BLOCKS_NUM_MAX \ + ((CONFIG_AUDIO_FRAME_DURATION_US / USEC_PER_MSEC) + \ + (CONFIG_AUDIO_FRAME_DURATION_US % USEC_PER_MSEC ? 1 : 0)) +#define USB_BLOCK_1MS_MONO_SIZE \ + (CONFIG_AUDIO_SAMPLE_RATE_HZ * CONFIG_AUDIO_BIT_DEPTH_OCTETS / MSEC_PER_SEC) +#define USB_BLOCK_MULTI_CHAN_1MS_SIZE \ + (USB_BLOCK_1MS_MONO_SIZE * MAX(CONFIG_AUDIO_INPUT_CHANNELS, CONFIG_AUDIO_OUTPUT_CHANNELS)) + +struct usbd_context *audio_usbd_init_device(usbd_msg_cb_t msg_cb); + +/** + * @brief Get the state of the USB devices headphones out. + * + * @return true: USB headphones out enabled. + * false: USB headphones out disabled. + */ +bool audio_usb_headphones_out_enabled(void); + +/** + * @brief Get the state of the USB devices headset out. + * + * @return true: USB headset out enabled. + * false: USB headset out disabled. + */ +bool audio_usb_headset_out_enabled(void); + /** - * @brief Set fifo buffers to be used by USB module and start sending/receiving data + * @brief Get the state of the USB devices headset in. * - * @param fifo_tx_in Pointer to fifo structure for tx - * @param fifo_rx_in Pointer to fifo structure for rx + * @return true: USB headset in enabled. + * false: USB headset in disabled. + */ +bool audio_usb_headset_in_enabled(void); + +/** + * @brief Set pointers to the queues to be used by the USB module and start sending/receiving data. + * + * @param audio_q_out_ptr Pointer to queue structure for out. + * @param audio_q_in_ptr Pointer to queue structure for in. * * @return 0 if successful, error otherwise */ -int audio_usb_start(struct data_fifo *fifo_tx_in, struct data_fifo *fifo_rx_in); +int audio_usb_start(struct k_msgq *audio_q_out_ptr, struct k_msgq *audio_q_in_ptr); /** - * @brief Stop sending/receiving data + * @brief Stop sending/receiving data. * * @note The USB device will still be running, but all data sent to - * it will be discarded + * it will be discarded. + * + * @return 0 if successful, error otherwise */ -void audio_usb_stop(void); +int audio_usb_stop(void); /** - * @brief Stop and disable USB device + * @brief Stop and disable USB device. * - * @return 0 if successful, error otherwise + * @return 0 if successful, error otherwise. */ int audio_usb_disable(void); /** - * @brief Register and enable USB device + * @brief Register and enable USB device. * - * @return 0 if successful, error otherwise + * @param host_in If true, initializes USB as input (host in), + * @param host_out If true, initializes USB as output (host out). + * + * @note If both @p host_in and @p host_out are true, USB is initialized as bidirectional + * (headset). + * + * @return 0 if successful, error otherwise. + */ +int audio_usb_init(bool host_in, bool host_out); + +/** + * @} */ -int audio_usb_init(void); #endif /* _AUDIO_USB_H_ */ diff --git a/src/modules/audio_usb_init.c b/src/modules/audio_usb_init.c new file mode 100644 index 00000000..729b0663 --- /dev/null +++ b/src/modules/audio_usb_init.c @@ -0,0 +1,115 @@ +/* + * Copyright (c) 2023 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#include +#include +#include + +#include +LOG_MODULE_DECLARE(audio_usb); + +/* By default, do not register the USB DFU class DFU mode instance. */ +static const char *const blocklist[] = { + "dfu_dfu", + NULL, +}; + +/* + * Instantiate a context named audio_usbd using the default USB device + * controller, the project vendor ID, and the project product ID. + * Project vendor ID must not be used outside of this project. + */ +USBD_DEVICE_DEFINE(audio_usbd, DEVICE_DT_GET(DT_NODELABEL(zephyr_udc0)), CONFIG_USBD_VID, + CONFIG_USBD_PID); + +USBD_DESC_LANG_DEFINE(audio_lang); +USBD_DESC_MANUFACTURER_DEFINE(audio_mfr, CONFIG_USBD_MANUFACTURER); +USBD_DESC_PRODUCT_DEFINE(audio_product, CONFIG_USBD_PRODUCT); + +IF_ENABLED(CONFIG_HWINFO, (USBD_DESC_SERIAL_NUMBER_DEFINE(audio_sn))); + +USBD_DESC_CONFIG_DEFINE(fs_cfg_desc, "FS Configuration"); + +static const uint8_t attributes = + (IS_ENABLED(CONFIG_USBD_SELF_POWERED) ? USB_SCD_SELF_POWERED : 0) | + (IS_ENABLED(CONFIG_USBD_REMOTE_WAKEUP) ? USB_SCD_REMOTE_WAKEUP : 0); + +/* Full speed configuration */ +USBD_CONFIGURATION_DEFINE(audio_fs_config, attributes, CONFIG_USBD_MAX_POWER, &fs_cfg_desc); + +static struct usbd_context *audio_usbd_setup_device(usbd_msg_cb_t msg_cb) +{ + int ret; + + ret = usbd_add_descriptor(&audio_usbd, &audio_lang); + if (ret) { + LOG_ERR("Failed to initialize language descriptor (%d)", ret); + return NULL; + } + + ret = usbd_add_descriptor(&audio_usbd, &audio_mfr); + if (ret) { + LOG_ERR("Failed to initialize manufacturer descriptor (%d)", ret); + return NULL; + } + + ret = usbd_add_descriptor(&audio_usbd, &audio_product); + if (ret) { + LOG_ERR("Failed to initialize product descriptor (%d)", ret); + return NULL; + } + + IF_ENABLED(CONFIG_HWINFO, (ret = usbd_add_descriptor(&audio_usbd, &audio_sn);)) + if (ret) { + LOG_ERR("Failed to initialize SN descriptor (%d)", ret); + return NULL; + } + + ret = usbd_add_configuration(&audio_usbd, USBD_SPEED_FS, &audio_fs_config); + if (ret) { + LOG_ERR("Failed to add Full-Speed configuration"); + return NULL; + } + + ret = usbd_register_all_classes(&audio_usbd, USBD_SPEED_FS, 1, blocklist); + if (ret) { + LOG_ERR("Failed to add register classes"); + return NULL; + } + + /* Interface Association Descriptor */ + usbd_device_set_code_triple(&audio_usbd, USBD_SPEED_FS, USB_BCC_MISCELLANEOUS, 0x02, 0x01); + usbd_self_powered(&audio_usbd, attributes & USB_SCD_SELF_POWERED); + + if (msg_cb != NULL) { + ret = usbd_msg_register_cb(&audio_usbd, msg_cb); + if (ret) { + LOG_ERR("Failed to register message callback"); + return NULL; + } + } + + return &audio_usbd; +} + +struct usbd_context *audio_usbd_init_device(usbd_msg_cb_t msg_cb) +{ + int ret; + + if (audio_usbd_setup_device(msg_cb) == NULL) { + return NULL; + } + + ret = usbd_init(&audio_usbd); + if (ret) { + LOG_ERR("Failed to initialize device support"); + return NULL; + } + + return &audio_usbd; +} diff --git a/src/modules/hw_codec.c b/src/modules/hw_codec.c index b96f18da..aea95150 100644 --- a/src/modules/hw_codec.c +++ b/src/modules/hw_codec.c @@ -380,7 +380,7 @@ int hw_codec_init(void) &volume_msg_sub_thread_data, volume_msg_sub_thread_stack, CONFIG_VOLUME_MSG_SUB_STACK_SIZE, (k_thread_entry_t)volume_msg_sub_thread, NULL, NULL, NULL, K_PRIO_PREEMPT(CONFIG_VOLUME_MSG_SUB_THREAD_PRIO), 0, K_NO_WAIT); - ret = k_thread_name_set(volume_msg_sub_thread_id, "VOLUME_MSG_SUB"); + ret = k_thread_name_set(volume_msg_sub_thread_id, "Msg_sub_vol"); ERR_CHK(ret); return 0; @@ -478,10 +478,105 @@ static int cmd_input(const struct shell *shell, size_t argc, char **argv) return 0; } -SHELL_STATIC_SUBCMD_SET_CREATE(hw_codec_cmd, - SHELL_COND_CMD(CONFIG_SHELL, input, NULL, - " Select input\n\t0: LINE_IN\n\t\t1: PDM_MIC", - cmd_input), - SHELL_SUBCMD_SET_END); +static int analog_out1l_disconnect(void) +{ + int ret; + + ret = cs47l63_write_reg(&cs47l63_driver, CS47L63_OUT1L_INPUT1, 0x808000); + if (ret) { + return ret; + } + + ret = cs47l63_write_reg(&cs47l63_driver, CS47L63_OUT1L_INPUT2, 0x808000); + if (ret) { + return ret; + } + + return 0; +} + +/** + * @brief Shell command to choose between left, right, and mixed stereo as the I2S output + * + * @param shell Shell instance + * @param argc Number of arguments + * @param argv Array of arguments + * + * @return 0 on success, -EINVAL on error + */ +static int cmd_i2s_to_output_mapping(const struct shell *shell, size_t argc, char **argv) +{ + int ret; + uint8_t idx; + + enum hw_codec_i2s_channel_to_mono_output { + LEFT, + RIGHT, + MIXED_STEREO, + NUM_OUTPUTS, + }; + + if (argc != 2) { + shell_error(shell, "Only one argument required, provided: %d", argc); + return -EINVAL; + } + + if (!isdigit((int)argv[1][0])) { + shell_error(shell, "Supplied argument is not numeric"); + return -EINVAL; + } + /* Remove all outputs */ + ret = analog_out1l_disconnect(); + if (ret) { + shell_error(shell, "Failed to clear outputs"); + return ret; + } + + idx = strtoul(argv[1], NULL, BASE_10); + + switch (idx) { + case LEFT: + ret = cs47l63_write_reg(&cs47l63_driver, CS47L63_OUT1L_INPUT1, 0x808020); + if (ret) { + shell_error(shell, "Failed to route I2S to left channel"); + return ret; + } + break; + case RIGHT: + ret = cs47l63_write_reg(&cs47l63_driver, CS47L63_OUT1L_INPUT2, 0x808021); + if (ret) { + shell_error(shell, "Failed to route I2S to right channel"); + return ret; + } + break; + case MIXED_STEREO: + ret = cs47l63_write_reg(&cs47l63_driver, CS47L63_OUT1L_INPUT1, 0x808020); + if (ret) { + shell_error(shell, "Failed to route I2S to left channel"); + return ret; + } + + ret = cs47l63_write_reg(&cs47l63_driver, CS47L63_OUT1L_INPUT2, 0x808021); + if (ret) { + shell_error(shell, "Failed to route I2S to right channel"); + return ret; + } + break; + default: + shell_error(shell, "Invalid output"); + return -EINVAL; + } + + return 0; +} + +SHELL_STATIC_SUBCMD_SET_CREATE( + hw_codec_cmd, + SHELL_COND_CMD(CONFIG_SHELL, input, NULL, " Select input\n\t0: LINE_IN\n\t\t1: PDM_MIC", + cmd_input), + SHELL_COND_CMD(CONFIG_SHELL, i2s_output, NULL, + " Select I2S output\n\t0: Left\n\t1: Right\n\t2: Mixed stereo", + cmd_i2s_to_output_mapping), + SHELL_SUBCMD_SET_END); SHELL_CMD_REGISTER(hw_codec, &hw_codec_cmd, "Change settings on HW codec", NULL); diff --git a/src/modules/hw_codec_adau1860.cpp b/src/modules/hw_codec_adau1860.cpp index f69668d6..b57a8ab6 100644 --- a/src/modules/hw_codec_adau1860.cpp +++ b/src/modules/hw_codec_adau1860.cpp @@ -28,7 +28,6 @@ LOG_MODULE_REGISTER(hw_codec, CONFIG_MODULE_HW_CODEC_LOG_LEVEL); /* Allow the DSP mute ramp and I2S clock domain to settle at source changes. */ #define CODEC_MUTE_SETTLE_MS 100 -#define CODEC_I2S_PREROLL_MS 100 ZBUS_SUBSCRIBER_DEFINE(volume_evt_sub, CONFIG_VOLUME_MSG_SUB_QUEUE_SIZE); @@ -318,12 +317,13 @@ int hw_codec_default_conf_enable(void) return ret; } - /* audio_datapath_aquire() starts I2S before enabling the codec. Keep the - * output muted while the new clock stream becomes stable. + /* I2S is running; require stable codec clocks before releasing mute. */ - k_msleep(CODEC_I2S_PREROLL_MS); + ret = dac.setup(); + if (ret) { + return ret; + } - //ret = dac.setup(); if (!muted) { ret = dac.mute(false); if (ret) { diff --git a/src/modules/lc3_file.c b/src/modules/lc3_file.c index ebbce1d1..5e232704 100644 --- a/src/modules/lc3_file.c +++ b/src/modules/lc3_file.c @@ -146,7 +146,7 @@ int lc3_file_init(void) ret = sd_card_init(); if (ret) { - LOG_ERR("Failed to initialize SD card: %d", ret); + LOG_WRN("Failed to initialize SD card: %d", ret); return ret; } diff --git a/src/modules/lc3_file.h b/src/modules/lc3_file.h index d2803bba..98d54be4 100644 --- a/src/modules/lc3_file.h +++ b/src/modules/lc3_file.h @@ -4,6 +4,15 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @defgroup audio_app_lc3_file LC3 File + * @{ + * @brief LC3 file handling API for Audio applications. + * + * This module provides LC3 audio file I/O operations for reading and parsing LC3 + * encoded audio files from SD card storage. + */ + #ifndef LC3_FILE_H__ #define LC3_FILE_H__ @@ -13,18 +22,32 @@ #include #include +/** + * @brief LC3 file header structure. + * + * This structure defines the header format for LC3 audio files, + * containing metadata about the audio content including sample rate, + * bit rate, channel configuration, and signal length. + */ struct lc3_file_header { - uint16_t file_id; /* Constant value, 0xCC1C */ - uint16_t hdr_size; /* Header size, 0x0012 */ - uint16_t sample_rate; /* Sample frequency / 100 */ - uint16_t bit_rate; /* Bit rate / 100 (total for all channels) */ - uint16_t channels; /* Number of channels */ - uint16_t frame_duration; /* Frame duration in ms * 100 */ - uint16_t rfu; /* Reserved for future use */ - uint16_t signal_len_lsb; /* Number of samples in signal, 16 LSB */ - uint16_t signal_len_msb; /* Number of samples in signal, 16 MSB (>> 16) */ + uint16_t file_id; /**< Constant value, 0xCC1C */ + uint16_t hdr_size; /**< Header size, 0x0012 */ + uint16_t sample_rate; /**< Sample frequency / 100 */ + uint16_t bit_rate; /**< Bit rate / 100 (total for all channels) */ + uint16_t channels; /**< Number of channels */ + uint16_t frame_duration; /**< Frame duration in ms * 100 */ + uint16_t rfu; /**< Reserved for future use */ + uint16_t signal_len_lsb; /**< Number of samples in signal, 16 LSB */ + uint16_t signal_len_msb; /**< Number of samples in signal, 16 MSB (>> 16) */ } __packed; +/** + * @brief LC3 file context structure. + * + * This structure maintains the state of an open LC3 file, + * including the file object, parsed header information, + * and calculated sample count. + */ struct lc3_file_ctx { struct fs_file_t file_object; struct lc3_file_header lc3_header; @@ -87,4 +110,8 @@ int lc3_file_close(struct lc3_file_ctx *file); */ int lc3_file_init(void); +/** + * @} + */ + #endif /* LC3_FILE_H__ */ diff --git a/src/modules/lc3_streamer.c b/src/modules/lc3_streamer.c index 347f633e..ba291ff8 100644 --- a/src/modules/lc3_streamer.c +++ b/src/modules/lc3_streamer.c @@ -529,7 +529,7 @@ int lc3_streamer_init(void) ret = lc3_file_init(); if (ret) { - LOG_ERR("Failed to initialize LC3 file module %d", ret); + LOG_WRN("Failed to initialize LC3 file module %d", ret); return ret; } diff --git a/src/modules/lc3_streamer.h b/src/modules/lc3_streamer.h index d8417728..35cfb635 100644 --- a/src/modules/lc3_streamer.h +++ b/src/modules/lc3_streamer.h @@ -4,6 +4,19 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @defgroup audio_app_lc3_streamer LC3 Streamer + * @{ + * @brief LC3 streamer API for Audio applications. + * + * This module provides LC3 audio file streaming functionality for playback from SD card + * storage. + * + * The streamer integrates with @ref audio_app_lc3_file for file operations, + * @ref audio_app_sd_card for storage access, and @ref audio_app_system + * for audio playback integration. + */ + #ifndef LC3_STREAMER_H #define LC3_STREAMER_H @@ -12,6 +25,12 @@ #include #include +/** + * @brief LC3 stream configuration structure. + * + * This structure defines the configuration parameters for LC3 audio streams, + * including sample rate, bit rate, and frame duration. + */ struct lc3_stream_cfg { uint32_t sample_rate_hz; uint32_t bit_rate_bps; @@ -135,4 +154,8 @@ int lc3_streamer_close_all_streams(void); */ int lc3_streamer_init(void); +/** + * @} + */ + #endif /* LC3_STREAMER_H */ diff --git a/src/modules/led.c b/src/modules/led.c index 79a1385d..bcb9a2b9 100644 --- a/src/modules/led.c +++ b/src/modules/led.c @@ -11,159 +11,95 @@ #include #include #include +#include +#include #include #include "macros_common.h" +#include "led_assignments.h" #include LOG_MODULE_REGISTER(led, CONFIG_MODULE_LED_LOG_LEVEL); -#define BLINK_FREQ_MS 1000 -/* Maximum number of LED_UNITS. 1 RGB LED = 1 UNIT of 3 LEDS */ -#define LED_UNIT_MAX 10 -#define NUM_COLORS_RGB 3 -#define BASE_10 10 -#define DT_LABEL_AND_COMMA(node_id) DT_PROP(node_id, label), -#define GPIO_DT_SPEC_GET_AND_COMMA(node_id) GPIO_DT_SPEC_GET(node_id, gpios), - -/* The following arrays are populated compile time from the .dts*/ -static const char *const led_labels[] = {DT_FOREACH_CHILD(DT_PATH(leds), DT_LABEL_AND_COMMA)}; - -static const struct gpio_dt_spec leds[] = { - DT_FOREACH_CHILD(DT_PATH(leds), GPIO_DT_SPEC_GET_AND_COMMA)}; +#define BLINK_FREQ_MS 1000 -enum led_type { - LED_MONOCHROME, - LED_COLOR, -}; +/* Maximum number of LED_UNITS. 1 RGB LED = 1 UNIT of 3 LEDS */ +#define LED_MONOCHROME 1 +#define NUM_COLORS_RGB 3 struct user_config { bool blink; enum led_color color; }; -struct led_unit_cfg { - uint8_t led_no; - enum led_type unit_type; - union { - const struct gpio_dt_spec *mono; - const struct gpio_dt_spec *color[NUM_COLORS_RGB]; - } type; +struct led_unit_data { + size_t num_gpios; + const struct gpio_dt_spec *gpio; struct user_config user_cfg; }; -static uint8_t leds_num; -static bool initialized; -static struct led_unit_cfg led_units[LED_UNIT_MAX]; +struct led_units_cfg { + size_t num_units; + struct led_unit_data *led; +}; -/** - * @brief Configures fields for a RGB LED - */ -static int configure_led_color(uint8_t led_unit, uint8_t led_color, uint8_t led) -{ - if (!device_is_ready(leds[led].port)) { - LOG_ERR("LED GPIO controller not ready"); - return -ENODEV; - } +#if DT_NODE_EXISTS(DT_NODELABEL(audioleds)) +#define LED DT_NODELABEL(audioleds) +#else +#error "No DT_NODELABEL(audioleds) found in overlay" +#endif - led_units[led_unit].type.color[led_color] = &leds[led]; - led_units[led_unit].unit_type = LED_COLOR; +/* clang-format off */ +#define LED_GPIO_DT_SPEC(led_node_id) \ + static const struct gpio_dt_spec led_dt_gpio_spec_##led_node_id[] = { \ + DT_FOREACH_PROP_ELEM_SEP(led_node_id, gpios, GPIO_DT_SPEC_GET_BY_IDX, (,)), \ + }; +/* clang-format on */ - return gpio_pin_configure_dt(led_units[led_unit].type.color[led_color], - GPIO_OUTPUT_INACTIVE); -} +#define LED_GPIO(led_node_id) \ + { \ + .num_gpios = ARRAY_SIZE(led_dt_gpio_spec_##led_node_id), \ + .gpio = led_dt_gpio_spec_##led_node_id, \ + }, -/** - * @brief Configures fields for a monochrome LED - */ -static int config_led_monochrome(uint8_t led_unit, uint8_t led) -{ - if (!device_is_ready(leds[led].port)) { - LOG_ERR("LED GPIO controller not ready"); - return -ENODEV; - } +DT_FOREACH_CHILD(LED, LED_GPIO_DT_SPEC) - led_units[led_unit].type.mono = &leds[led]; - led_units[led_unit].unit_type = LED_MONOCHROME; +static struct led_unit_data led_units_data[] = {DT_FOREACH_CHILD(LED, LED_GPIO)}; - return gpio_pin_configure_dt(led_units[led_unit].type.mono, GPIO_OUTPUT_INACTIVE); -} +static const struct led_units_cfg led_units = {.num_units = ARRAY_SIZE(led_units_data), + .led = led_units_data}; + +static bool initialized; /** - * @brief Parses the device tree for LED settings. + * @brief Internal handling to set the status of a led unit */ -static int led_device_tree_parse(void) +static int led_set_int(uint8_t led, enum led_color color) { int ret; - for (uint8_t i = 0; i < leds_num; i++) { - char *end_ptr = NULL; - uint32_t led_unit = strtoul(led_labels[i], &end_ptr, BASE_10); - - if (led_labels[i] == end_ptr) { - LOG_ERR("No match for led unit. The dts is likely not properly formatted"); - return -ENXIO; - } - - if (strstr(led_labels[i], "LED_RGB_RED")) { - ret = configure_led_color(led_unit, RED, i); - if (ret) { - return ret; - } - - } else if (strstr(led_labels[i], "LED_RGB_GREEN")) { - ret = configure_led_color(led_unit, GRN, i); - if (ret) { - return ret; - } - - } else if (strstr(led_labels[i], "LED_RGB_BLUE")) { - ret = configure_led_color(led_unit, BLU, i); - if (ret) { - return ret; - } - - } else if (strstr(led_labels[i], "LED_MONO")) { - ret = config_led_monochrome(led_unit, i); - if (ret) { - return ret; - } - } else { - LOG_ERR("No color identifier for LED %d LED unit %d", i, led_unit); - return -ENODEV; - } + if (led == LED_AUDIO_NOT_ASSIGNED) { + LOG_WRN("LED unit not assigned"); + return 0; } - return 0; -} -/** - * @brief Internal handling to set the status of a led unit - */ -static int led_set_int(uint8_t led_unit, enum led_color color) -{ - int ret; + struct led_unit_data *led_unit = &led_units.led[led]; - if (led_units[led_unit].unit_type == LED_MONOCHROME) { + if (led_unit->num_gpios == LED_MONOCHROME) { if (color) { - ret = gpio_pin_set_dt(led_units[led_unit].type.mono, 1); - if (ret) { - return ret; - } + return gpio_pin_set_dt(led_unit->gpio, 1); } else { - ret = gpio_pin_set_dt(led_units[led_unit].type.mono, 0); - if (ret) { - return ret; - } + return gpio_pin_set_dt(led_unit->gpio, 0); } } else { for (uint8_t i = 0; i < NUM_COLORS_RGB; i++) { if (color & BIT(i)) { - ret = gpio_pin_set_dt(led_units[led_unit].type.color[i], 1); + ret = gpio_pin_set_dt(&led_unit->gpio[i], 1); if (ret) { return ret; } } else { - ret = gpio_pin_set_dt(led_units[led_unit].type.color[i], 0); + ret = gpio_pin_set_dt(&led_unit->gpio[i], 0); if (ret) { return ret; } @@ -196,10 +132,10 @@ static void led_blink_work_handler(struct k_work *work) int ret; static bool on_phase; - for (uint8_t i = 0; i < leds_num; i++) { - if (led_units[i].user_cfg.blink) { + for (uint8_t i = 0; i < led_units.num_units; i++) { + if (led_units.led[i].user_cfg.blink) { if (on_phase) { - ret = led_set_int(i, led_units[i].user_cfg.color); + ret = led_set_int(i, led_units.led[i].user_cfg.color); ERR_CHK(ret); } else { ret = led_set_int(i, LED_COLOR_OFF); @@ -211,7 +147,7 @@ static void led_blink_work_handler(struct k_work *work) on_phase = !on_phase; } -static int led_set(uint8_t led_unit, enum led_color color, bool blink) +static int led_set(uint8_t led, enum led_color color, bool blink) { int ret; @@ -219,26 +155,36 @@ static int led_set(uint8_t led_unit, enum led_color color, bool blink) return -EPERM; } - ret = led_set_int(led_unit, color); + if (led == LED_AUDIO_NOT_ASSIGNED) { + LOG_WRN("LED unit not assigned"); + return 0; + } + + ret = led_set_int(led, color); if (ret) { return ret; } - led_units[led_unit].user_cfg.blink = blink; - led_units[led_unit].user_cfg.color = color; + led_units.led[led].user_cfg.blink = blink; + led_units.led[led].user_cfg.color = color; return 0; } -int led_on(uint8_t led_unit, ...) +int led_on(uint8_t led, ...) { - if (led_units[led_unit].unit_type == LED_MONOCHROME) { - return led_set(led_unit, LED_ON, LED_SOLID); + if (led == LED_AUDIO_NOT_ASSIGNED) { + LOG_WRN("LED on unit not assigned: %d", led); + return 0; + } + + if (led_units.led[led].num_gpios == LED_MONOCHROME) { + return led_set(led, LED_ON, LED_SOLID); } va_list args; - va_start(args, led_unit); + va_start(args, led); int color = va_arg(args, int); va_end(args); @@ -247,18 +193,34 @@ int led_on(uint8_t led_unit, ...) LOG_ERR("Invalid color code %d", color); return -EINVAL; } - return led_set(led_unit, color, LED_SOLID); + + return led_set(led, color, LED_SOLID); +} + +int led_off(uint8_t led) +{ + if (led == LED_AUDIO_NOT_ASSIGNED) { + LOG_WRN("LED off unit not assigned: %d", led); + return 0; + } + + return led_set(led, LED_COLOR_OFF, LED_SOLID); } -int led_blink(uint8_t led_unit, ...) +int led_blink(uint8_t led, ...) { - if (led_units[led_unit].unit_type == LED_MONOCHROME) { - return led_set(led_unit, LED_ON, LED_BLINK); + if (led == LED_AUDIO_NOT_ASSIGNED) { + LOG_WRN("LED blink unit not assigned: %d", led); + return 0; + } + + if (led_units.led[led].num_gpios == LED_MONOCHROME) { + return led_set(led, LED_ON, LED_BLINK); } va_list args; - va_start(args, led_unit); + va_start(args, led); int color = va_arg(args, int); @@ -269,32 +231,39 @@ int led_blink(uint8_t led_unit, ...) return -EINVAL; } - return led_set(led_unit, color, LED_BLINK); -} - -int led_off(uint8_t led_unit) -{ - return led_set(led_unit, LED_COLOR_OFF, LED_SOLID); + return led_set(led, color, LED_BLINK); } int led_init(void) { - int ret; - if (initialized) { return -EPERM; } - __ASSERT(ARRAY_SIZE(leds) != 0, "No LEDs found in dts"); + __ASSERT(led_units.num_units != 0, "No LEDs found in dts"); - leds_num = ARRAY_SIZE(leds); + for (size_t i = 0; i < led_units.num_units; i++) { + int ret; + struct led_unit_data *led_unit = &led_units.led[i]; - ret = led_device_tree_parse(); - if (ret) { - return ret; + for (size_t j = 0; j < led_unit->num_gpios; j++) { + if (!device_is_ready(led_unit->gpio[j].port)) { + LOG_ERR("GPIO device not ready"); + return -ENODEV; + } + + ret = gpio_pin_configure_dt(&led_unit->gpio[j], + GPIO_OUTPUT_INACTIVE); + if (ret) { + LOG_ERR("Cannot configure GPIO (ret %d)", ret); + return ret; + } + } } k_timer_start(&led_blink_timer, K_MSEC(BLINK_FREQ_MS / 2), K_MSEC(BLINK_FREQ_MS / 2)); + initialized = true; + return 0; } diff --git a/src/modules/led.h b/src/modules/led.h index 59dff83f..d6f5ef6d 100644 --- a/src/modules/led.h +++ b/src/modules/led.h @@ -4,39 +4,51 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ +/** @file + * @defgroup audio_app_led Audio LED Control + * @{ + * @brief LED control API for Audio applications. + * + * This module provides LED control functionality for user interface and status indication. + */ + #ifndef _LED_H_ #define _LED_H_ #include -#define LED_APP_RGB 0 -#define LED_NET_RGB 1 -#define LED_APP_1_BLUE 2 -#define LED_APP_2_GREEN 3 -#define LED_APP_3_GREEN 4 - #define RED 0 + #define GREEN 1 + #define BLUE 2 #define GRN GREEN + #define BLU BLUE +/** + * @brief LED color enumeration for RGB LED control. + * + * This enumeration defines the available colors for RGB LED control, + * including individual colors and combinations for visual status indication. + */ enum led_color { - LED_COLOR_OFF, /* 000 */ - LED_COLOR_RED, /* 001 */ - LED_COLOR_GREEN, /* 010 */ - LED_COLOR_YELLOW, /* 011 */ - LED_COLOR_BLUE, /* 100 */ - LED_COLOR_MAGENTA, /* 101 */ - LED_COLOR_CYAN, /* 110 */ - LED_COLOR_WHITE, /* 111 */ + LED_COLOR_OFF, + LED_COLOR_RED, + LED_COLOR_GREEN, + LED_COLOR_YELLOW, + LED_COLOR_BLUE, + LED_COLOR_MAGENTA, + LED_COLOR_CYAN, + LED_COLOR_WHITE, LED_COLOR_NUM, }; #define LED_ON LED_COLOR_WHITE #define LED_BLINK true + #define LED_SOLID false /** @@ -99,4 +111,8 @@ int led_off(uint8_t led_unit); */ int led_init(void); +/** + * @} + */ + #endif /* _LED_H_ */ diff --git a/src/modules/led_assignments.h b/src/modules/led_assignments.h new file mode 100644 index 00000000..22251671 --- /dev/null +++ b/src/modules/led_assignments.h @@ -0,0 +1,39 @@ +/* + * Copyright (c) 2025 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause + */ + +#ifndef _LED_ASSIGNMENTS_H_ +#define _LED_ASSIGNMENTS_H_ + +#include +#include + +#define LED_AUDIO_NOT_ASSIGNED 0xFF + +#if DT_NODE_EXISTS(DT_NODELABEL(led_device_type)) +#define LED_AUDIO_DEVICE_TYPE DT_NODE_CHILD_IDX(DT_NODELABEL(led_device_type)) +#else +#define LED_AUDIO_DEVICE_TYPE LED_AUDIO_NOT_ASSIGNED +#endif + +#if DT_NODE_EXISTS(DT_NODELABEL(led_conn_status)) +#define LED_AUDIO_CONN_STATUS DT_NODE_CHILD_IDX(DT_NODELABEL(led_conn_status)) +#else +#define LED_AUDIO_CONN_STATUS LED_AUDIO_NOT_ASSIGNED +#endif + +#if DT_NODE_EXISTS(DT_NODELABEL(led_app_status)) +#define LED_AUDIO_APP_STATUS DT_NODE_CHILD_IDX(DT_NODELABEL(led_app_status)) +#else +#define LED_AUDIO_APP_STATUS LED_AUDIO_NOT_ASSIGNED +#endif + +#if DT_NODE_EXISTS(DT_NODELABEL(led_sync_status)) +#define LED_AUDIO_SYNC_STATUS DT_NODE_CHILD_IDX(DT_NODELABEL(led_sync_status)) +#else +#define LED_AUDIO_SYNC_STATUS LED_AUDIO_NOT_ASSIGNED +#endif + +#endif /* _LED_ASSIGNMENTS_H_ */ diff --git a/src/modules/power_meas.c b/src/modules/power_meas.c index 836dbfea..23435981 100644 --- a/src/modules/power_meas.c +++ b/src/modules/power_meas.c @@ -128,7 +128,7 @@ static int power_meas_init(void) pwr_meas_thread_id = k_thread_create( &meas_thread, meas_stack, CONFIG_POWER_MEAS_STACK_SIZE, meas_thread_fn, NULL, NULL, NULL, K_PRIO_PREEMPT(CONFIG_POWER_MEAS_THREAD_PRIO), 0, K_NO_WAIT); - ret = k_thread_name_set(pwr_meas_thread_id, "pwr_meas"); + ret = k_thread_name_set(pwr_meas_thread_id, "Pwr_meas"); if (ret) { return ret; } diff --git a/src/modules/sd_card.c b/src/modules/sd_card.c index a147e9a4..144792b0 100644 --- a/src/modules/sd_card.c +++ b/src/modules/sd_card.c @@ -19,9 +19,6 @@ #include LOG_MODULE_REGISTER(sd_card, CONFIG_MODULE_SD_CARD_LOG_LEVEL); -#define SD_ROOT_PATH "/SD:/" -/* Round down to closest 4-byte boundary */ -#define PATH_MAX_LEN ROUND_DOWN(CONFIG_FS_FATFS_MAX_LFN, 4) #define SD_CARD_LEVELS_MAX 8 #define SD_CARD_BUF_SIZE 700 @@ -38,6 +35,32 @@ static struct fs_mount_t mnt_pt = { .fs_data = &fat_fs, }; +static bool path_append(char *dst, size_t dst_size, const char *src) +{ + size_t dst_len; + size_t src_len; + size_t remaining; + + if (dst == NULL || src == NULL || dst_size == 0) { + return false; + } + + dst_len = strnlen(dst, dst_size); + if (dst_len == dst_size) { + return false; + } + + remaining = dst_size - dst_len - 1; + src_len = strnlen(src, remaining + 1); + if (src_len > remaining) { + return false; + } + + memcpy(dst + dst_len, src, src_len); + dst[dst_len + src_len] = '\0'; + return true; +} + /** * @brief Replaces first carriage return or line feed with null terminator. */ @@ -111,15 +134,27 @@ static int traverse_down(char const *const path, uint8_t curr_depth, uint16_t re } cr_lf_remove(token); - memset(slab_B_ptr, '\0', PATH_MAX_LEN); + memset(slab_B_ptr, '\0', SD_PATH_MAX_LEN); if (path != NULL) { - strcat(slab_B_ptr, path); + if (!path_append(slab_B_ptr, SD_PATH_MAX_LEN, path)) { + LOG_ERR("Path is too long"); + ret = -FR_INVALID_NAME; + goto cleanup; + } cr_lf_remove(slab_B_ptr); - strcat(slab_B_ptr, "/"); + if (!path_append(slab_B_ptr, SD_PATH_MAX_LEN, "/")) { + LOG_ERR("Path is too long"); + ret = -FR_INVALID_NAME; + goto cleanup; + } } - strcat(slab_B_ptr, token); + if (!path_append(slab_B_ptr, SD_PATH_MAX_LEN, token)) { + LOG_ERR("Path is too long"); + ret = -FR_INVALID_NAME; + goto cleanup; + } if (strstr(token, search_pattern) != NULL) { if (num_files_added >= result_file_num_max) { @@ -164,7 +199,7 @@ int sd_card_list_files_match(uint16_t result_file_num_max, uint16_t result_path_ return -EINVAL; } - if (result_path_len_max == 0 || (result_path_len_max > PATH_MAX_LEN)) { + if (result_path_len_max == 0 || (result_path_len_max > SD_PATH_MAX_LEN)) { return -EINVAL; } @@ -173,7 +208,7 @@ int sd_card_list_files_match(uint16_t result_file_num_max, uint16_t result_path_ } char __aligned(4) buf_A[SD_CARD_BUF_SIZE * SD_CARD_LEVELS_MAX] = {'\0'}; - char __aligned(4) buf_B[PATH_MAX_LEN * SD_CARD_LEVELS_MAX] = {'\0'}; + char __aligned(4) buf_B[SD_PATH_MAX_LEN * SD_CARD_LEVELS_MAX] = {'\0'}; ret = k_mem_slab_init(&slab_A, buf_A, SD_CARD_BUF_SIZE, SD_CARD_LEVELS_MAX); if (ret) { @@ -181,7 +216,7 @@ int sd_card_list_files_match(uint16_t result_file_num_max, uint16_t result_path_ return ret; } - ret = k_mem_slab_init(&slab_B, buf_B, PATH_MAX_LEN, SD_CARD_LEVELS_MAX); + ret = k_mem_slab_init(&slab_B, buf_B, SD_PATH_MAX_LEN, SD_CARD_LEVELS_MAX); if (ret) { LOG_ERR("Failed to init slab: %d", ret); return ret; @@ -201,7 +236,7 @@ int sd_card_list_files(char const *const path, char *buf, size_t *buf_size, bool int ret; struct fs_dir_t dirp; static struct fs_dirent entry; - char abs_path_name[PATH_MAX_LEN + 1] = SD_ROOT_PATH; + char abs_path_name[SD_PATH_MAX_LEN + 1] = SD_ROOT_PATH; size_t used_buf_size = 0; if (!sd_init_success) { @@ -216,13 +251,11 @@ int sd_card_list_files(char const *const path, char *buf, size_t *buf_size, bool return ret; } } else { - if (strlen(path) > PATH_MAX_LEN) { + if (!path_append(abs_path_name, sizeof(abs_path_name), path)) { LOG_ERR("Path is too long"); return -FR_INVALID_NAME; } - strcat(abs_path_name, path); - if (strchr(abs_path_name, '.')) { /* Path contains a dot. Regarded as not a folder*/ return -ENOENT; @@ -283,19 +316,18 @@ int sd_card_list_files(char const *const path, char *buf, size_t *buf_size, bool int sd_card_open_write_close(char const *const filename, char const *const data, size_t *size) { int ret; + struct fs_file_t f_entry; - char abs_path_name[PATH_MAX_LEN + 1] = SD_ROOT_PATH; + char abs_path_name[SD_PATH_MAX_LEN + 1] = SD_ROOT_PATH; if (!sd_init_success) { return -ENODEV; } - if (strlen(filename) > PATH_MAX_LEN) { + if (!path_append(abs_path_name, sizeof(abs_path_name), filename)) { LOG_ERR("Filename is too long"); return -ENAMETOOLONG; } - - strcat(abs_path_name, filename); fs_file_t_init(&f_entry); ret = fs_open(&f_entry, abs_path_name, FS_O_CREATE | FS_O_WRITE | FS_O_APPEND); @@ -332,18 +364,16 @@ int sd_card_open_read_close(char const *const filename, char *const buf, size_t { int ret; struct fs_file_t f_entry; - char abs_path_name[PATH_MAX_LEN + 1] = SD_ROOT_PATH; + char abs_path_name[SD_PATH_MAX_LEN + 1] = SD_ROOT_PATH; if (!sd_init_success) { return -ENODEV; } - if (strlen(filename) > PATH_MAX_LEN) { + if (!path_append(abs_path_name, sizeof(abs_path_name), filename)) { LOG_ERR("Filename is too long"); return -FR_INVALID_NAME; } - - strcat(abs_path_name, filename); fs_file_t_init(&f_entry); ret = fs_open(&f_entry, abs_path_name, FS_O_READ); @@ -375,25 +405,17 @@ int sd_card_open_read_close(char const *const filename, char *const buf, size_t int sd_card_open(char const *const filename, struct fs_file_t *f_seg_read_entry) { int ret; - char abs_path_name[PATH_MAX_LEN + 1] = SD_ROOT_PATH; - size_t available_path_space = PATH_MAX_LEN - strlen(SD_ROOT_PATH); + char abs_path_name[SD_PATH_MAX_LEN + 1] = SD_ROOT_PATH; if (!sd_init_success) { return -ENODEV; } - if (strlen(filename) > CONFIG_FS_FATFS_MAX_LFN) { - LOG_ERR("Filename is too long"); - return -ENAMETOOLONG; - } - - if ((strlen(abs_path_name) + strlen(filename)) > PATH_MAX_LEN) { + if (!path_append(abs_path_name, sizeof(abs_path_name), filename)) { LOG_ERR("Filepath is too long"); return -EINVAL; } - strncat(abs_path_name, filename, available_path_space); - LOG_INF("abs path name:\t%s", abs_path_name); fs_file_t_init(f_seg_read_entry); diff --git a/src/modules/sd_card.h b/src/modules/sd_card.h index b2de5442..e593dbda 100644 --- a/src/modules/sd_card.h +++ b/src/modules/sd_card.h @@ -9,6 +9,10 @@ #include #include +#include + +#define SD_ROOT_PATH "/SD:/" +#define SD_PATH_MAX_LEN ROUND_DOWN(CONFIG_FS_FATFS_MAX_LFN, 4) #ifdef __cplusplus extern "C" { diff --git a/src/modules/sd_card_playback.c b/src/modules/sd_card_playback.c index 406c7cb7..ba5da8c2 100644 --- a/src/modules/sd_card_playback.c +++ b/src/modules/sd_card_playback.c @@ -169,7 +169,7 @@ static int sd_card_playback_check_wav_header(struct wav_header wav_file_header) return -EPERM; } - if (wav_file_header.num_channels != SW_CODEC_MONO) { + if (wav_file_header.num_channels != 1) { LOG_ERR("This is not a MONO file"); return -EPERM; } diff --git a/src/modules/sd_card_playback.h b/src/modules/sd_card_playback.h index 39c27090..9cb8294c 100644 --- a/src/modules/sd_card_playback.h +++ b/src/modules/sd_card_playback.h @@ -4,16 +4,19 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ -#ifndef _SD_CARD_PLAYBACK_H_ -#define _SD_CARD_PLAYBACK_H_ - -/** - * @file - * @defgroup sd_card_playback SD card playback. +/** @file + * @defgroup audio_app_sd_card_playback SD Card Playback * @{ - * @brief The SD card playback module for nRF5340 Audio. + * @brief SD card playback API for Audio applications. + * + * This module provides audio file playback functionality from SD card storage, + * supporting both WAV and LC3 audio formats. It handles file reading and audio + * decoding, and provides mixing capabilities with live audio streams. */ +#ifndef _SD_CARD_PLAYBACK_H_ +#define _SD_CARD_PLAYBACK_H_ + #include /** diff --git a/src/utils/CMakeLists.txt b/src/utils/CMakeLists.txt index 9dd2a46c..e47027fa 100644 --- a/src/utils/CMakeLists.txt +++ b/src/utils/CMakeLists.txt @@ -5,6 +5,7 @@ # target_sources(app PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/device_location.c ${CMAKE_CURRENT_SOURCE_DIR}/channel_assignment.c ${CMAKE_CURRENT_SOURCE_DIR}/error_handler.c ${CMAKE_CURRENT_SOURCE_DIR}/uicr.c diff --git a/src/utils/Kconfig b/src/utils/Kconfig index ed02ced3..69f89ac6 100644 --- a/src/utils/Kconfig +++ b/src/utils/Kconfig @@ -12,14 +12,14 @@ menu "FIFO" config FIFO_FRAME_SPLIT_NUM int "Number of blocks to make up one frame of audio data" - default 10 + default 20 help Easy DMA in I2S requires two buffers to be filled before I2S transmission will begin. In order to reduce latency, an audio frame can be split into multiple blocks with this parameter. USB sends data in 1 ms blocks, so we need the split to match that. - Since we set frame size to 10 ms for USB, 10 is selected as - FRAME_SPLIT_NUM + If we set frame size to 10 ms for USB, 20 is selected as + FIFO_FRAME_SPLIT_NUM config FIFO_TX_FRAME_COUNT int "Max number of audio frames in TX slab" @@ -40,8 +40,16 @@ endmenu # FIFO #----------------------------------------------------------------------------# menu "Log levels" -module = BOARD_VERSION -module-str = board-version +module = PERIPHERALS +module-str = peripherals +source "subsys/logging/Kconfig.template.log_config" + +module = NRF5340_AUDIO_DK_VERSION +module-str = nrf5340-audio-dk-version +source "subsys/logging/Kconfig.template.log_config" + +module = DEVICE_LOCATION +module-str = device-location source "subsys/logging/Kconfig.template.log_config" module = CHAN_ASSIGNMENT diff --git a/src/utils/Kconfig.defaults b/src/utils/Kconfig.defaults index b4e2c58f..df52ff9b 100644 --- a/src/utils/Kconfig.defaults +++ b/src/utils/Kconfig.defaults @@ -4,6 +4,6 @@ # SPDX-License-Identifier: LicenseRef-Nordic-5-Clause # -# Channel assignment writes to UICR +# Location assignment writes to UICR config NRFX_NVMC default y diff --git a/src/utils/StateIndicator.cpp b/src/utils/StateIndicator.cpp index 89ca40f8..285545ff 100644 --- a/src/utils/StateIndicator.cpp +++ b/src/utils/StateIndicator.cpp @@ -72,6 +72,14 @@ void StateIndicator::set_custom_color(const RGBColor &custom_color) { if (_state.led_mode == CUSTOM) led_controller.setColor(custom_color); } +void StateIndicator::get_custom_color(RGBColor &value) const { + memcpy(value, color, sizeof(RGBColor)); +} + +enum led_mode StateIndicator::get_indication_mode() const { + return _state.led_mode; +} + void StateIndicator::set_dfu_active(bool active) { if (_dfu_active == active) { return; diff --git a/src/utils/StateIndicator.h b/src/utils/StateIndicator.h index 5ba569f8..742e8e23 100644 --- a/src/utils/StateIndicator.h +++ b/src/utils/StateIndicator.h @@ -24,6 +24,8 @@ class StateIndicator { void set_pairing_state(enum pairing_state state); void set_indication_mode(enum led_mode state); void set_custom_color(const RGBColor &color); + void get_custom_color(RGBColor &value) const; + enum led_mode get_indication_mode() const; void set_dfu_active(bool active); private: diff --git a/src/utils/channel_assignment.h b/src/utils/channel_assignment.h index 79429de6..5a331bbd 100644 --- a/src/utils/channel_assignment.h +++ b/src/utils/channel_assignment.h @@ -22,7 +22,7 @@ static const char CH_L_TAG[] = "HL"; static const char CH_R_TAG[] = "HR"; -static const char GW_TAG[] = "GW"; +static const char CHANNEL_GW_TAG[] = "GW"; #ifdef __cplusplus extern "C" { diff --git a/src/utils/device_location.c b/src/utils/device_location.c new file mode 100644 index 00000000..597644d4 --- /dev/null +++ b/src/utils/device_location.c @@ -0,0 +1,53 @@ +/* + * Copyright (c) 2022 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause + */ + +#include "device_location.h" +#include "uicr.h" + +#include +LOG_MODULE_REGISTER(location, CONFIG_DEVICE_LOCATION_LOG_LEVEL); + +static uint32_t static_location; + +void device_location_get(enum bt_audio_location *location) +{ + *location = (enum bt_audio_location)static_location; +} + +#if CONFIG_DEVICE_LOCATION_SET_RUNTIME +void device_location_set(enum bt_audio_location location) +{ + int ret; + + static_location = location; + + /* Try to write the location to UICR */ + ret = uicr_location_set(location); + if (ret) { + LOG_WRN("Unable to write location to UICR: %d. Will revert on boot", ret); + } +} +#endif /* CONFIG_DEVICE_LOCATION_SET_RUNTIME */ + +static int device_location_init(void) +{ +#if CONFIG_DEVICE_LOCATION_SET_RUNTIME + static_location = uicr_location_get(); + + if (static_location >= BT_AUDIO_LOCATION_RIGHT_SURROUND) { + /* Invalid value in UICR if UICR is not written */ + LOG_WRN("Invalid location in UICR, using default location"); + static_location = DEVICE_LOCATION_DEFAULT; + } +#else + static_location = CONFIG_DEVICE_LOCATION_AT_COMPILE_TIME; +#endif /* CONFIG_DEVICE_LOCATION_SET_RUNTIME */ + + LOG_DBG("Location bitfield is 0x%08x", static_location); + return 0; +} + +SYS_INIT(device_location_init, APPLICATION, CONFIG_APPLICATION_INIT_PRIORITY); diff --git a/src/utils/device_location.h b/src/utils/device_location.h new file mode 100644 index 00000000..1a75901c --- /dev/null +++ b/src/utils/device_location.h @@ -0,0 +1,46 @@ +/* + * Copyright (c) 2022 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause + */ + +#ifndef _DEVICE_LOCATION_H_ +#define _DEVICE_LOCATION_H_ + +/** @file + * @brief Audio location assignment + * + * Audio location can be assigned at runtime or compile-time, depending on configuration. + * + */ + +#include +#include + +#ifndef DEVICE_LOCATION_DEFAULT +#define DEVICE_LOCATION_DEFAULT BT_AUDIO_LOCATION_FRONT_LEFT +#endif /* DEVICE_LOCATION_DEFAULT */ + +static const char HS_LOC_L_TAG[] = "HL"; +static const char HS_LOC_R_TAG[] = "HR"; +static const char HS_LOC_LR_TAG[] = "HS"; +static const char HS_LOC_OTHER_TAG[] = "H*"; +static const char GW_TAG[] = "GW"; + +/** + * @brief Get assigned audio location. + * + * @param[out] location Location bitfield + */ +void device_location_get(enum bt_audio_location *location); + +#if CONFIG_DEVICE_LOCATION_SET_RUNTIME +/** + * @brief Assign audio location. + * + * @param[out] location Location bitfield + */ +void device_location_set(enum bt_audio_location location); +#endif /* LOCATION_SET_RUNTIME */ + +#endif /* _DEVICE_LOCATION_H_ */ diff --git a/src/utils/fw_info_app.c.in b/src/utils/fw_info_app.c.in index 3c360c87..98bfe5f2 100644 --- a/src/utils/fw_info_app.c.in +++ b/src/utils/fw_info_app.c.in @@ -9,7 +9,7 @@ #include #include #include -#include "channel_assignment.h" +#include "device_location.h" #include "ncs_version.h" #include "macros_common.h" @@ -17,9 +17,9 @@ #include LOG_MODULE_REGISTER(fw_info, CONFIG_FW_INFO_LOG_LEVEL); -static const char COMPILE_DATE[] = "@NRF5340_AUDIO_CORE_APP_COMP_DATE@"; +static const char COMPILE_DATE[] = "@NRF_AUDIO_CORE_APP_COMP_DATE@"; /* NOTE: The string below is used by the Nordic CI system */ -static const char NRF5340_CORE[] = "nRF5340 Audio nRF5340 Audio DK cpuapp"; +static const char NRF5340_CORE[] = "nRF Audio nRF5340 Audio DK cpuapp"; int fw_info_app_print(void) { @@ -35,25 +35,33 @@ int fw_info_app_print(void) LOG_INF("------- DEBUG BUILD -------"); #if (CONFIG_AUDIO_DEV == HEADSET) - enum audio_channel channel; + enum bt_audio_location location; - channel_assignment_get(&channel); - if (channel == AUDIO_CH_L) { - ret = log_set_tag(CH_L_TAG); - if (ret) { - return ret; - } - /* NOTE: The string below is used by the Nordic CI system */ - LOG_INF(COLOR_CYAN "HEADSET left device" COLOR_RESET); - } else if (channel == AUDIO_CH_R) { - ret = log_set_tag(CH_R_TAG); - if (ret) { - return ret; + device_location_get(&location); + + if (location == BT_AUDIO_LOCATION_MONO_AUDIO){ + LOG_ERR("HEADSET location %s is not supported", bt_audio_location_bit_to_str(location)); + return -EINVAL; + } + + for (int i = 0; i < (CHAR_BIT * sizeof(enum bt_audio_location)); i++) { + if (location & BIT(i)) { + LOG_INF(COLOR_CYAN "HEADSET location: %s" COLOR_RESET, bt_audio_location_bit_to_str(BIT(i))); } - /* NOTE: The string below is used by the Nordic CI system */ - LOG_INF(COLOR_CYAN "HEADSET right device" COLOR_RESET); + } + + if (location == BT_AUDIO_LOCATION_FRONT_LEFT) { + ret = log_set_tag(HS_LOC_L_TAG); + } else if (location == BT_AUDIO_LOCATION_FRONT_RIGHT) { + ret = log_set_tag(HS_LOC_R_TAG); + } else if (location == (BT_AUDIO_LOCATION_FRONT_LEFT | BT_AUDIO_LOCATION_FRONT_RIGHT)) { + ret = log_set_tag(HS_LOC_LR_TAG); } else { - __ASSERT(false, "Unknown channel"); + ret = log_set_tag(HS_LOC_OTHER_TAG); + } + + if (ret) { + return ret; } #elif CONFIG_AUDIO_DEV == GATEWAY @@ -61,7 +69,7 @@ int fw_info_app_print(void) if (ret) { return ret; } - LOG_INF(COLOR_CYAN "Compiled for GATEWAY device" COLOR_RESET); + LOG_INF(COLOR_CYAN "GATEWAY device" COLOR_RESET); #endif /* (CONFIG_AUDIO_DEV == HEADSET) */ #endif /* (CONFIG_DEBUG) */ diff --git a/src/utils/openearable.c b/src/utils/openearable.c index 0301bed6..9a168d74 100644 --- a/src/utils/openearable.c +++ b/src/utils/openearable.c @@ -4,8 +4,6 @@ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause */ -#include - #include "led.h" //#include "button_handler.h" //#include "button_manager.h" @@ -21,8 +19,6 @@ LOG_MODULE_REGISTER(nrf5340_audio_dk, CONFIG_MODULE_NRF5340_AUDIO_DK_LOG_LEVEL); int openearable_init(void) { - int ret; - /*ret = led_init(); if (ret) { LOG_ERR("Failed to initialize LED module"); @@ -58,12 +54,5 @@ int openearable_init(void) } }*/ - /* Use this to turn on 128 MHz clock for cpu_app */ - ret = nrfx_clock_divider_set(NRF_CLOCK_DOMAIN_HFCLK, NRF_CLOCK_HFCLK_DIV_1); - ret -= NRFX_ERROR_BASE_NUM; - if (ret) { - return ret; - } - return 0; } diff --git a/src/utils/uicr.c b/src/utils/uicr.c index 39449ad2..fd9e00c5 100644 --- a/src/utils/uicr.c +++ b/src/utils/uicr.c @@ -22,6 +22,32 @@ LOG_MODULE_REGISTER(uicr, CONFIG_LOG_DEFAULT_LEVEL); #define MEM_ADDR_UICR_STAL (MEM_ADDR_UICR_SIRK + sizeof(uint32_t)) #define MEM_ADDR_UICR_HW (MEM_ADDR_UICR_STAL + sizeof(uint32_t)) +uint32_t uicr_location_get(void) +{ + uint8_t channel = uicr_channel_get(); + + if (channel == 0U) { + return BT_AUDIO_LOCATION_FRONT_LEFT; + } + if (channel == 1U) { + return BT_AUDIO_LOCATION_FRONT_RIGHT; + } + + return UINT32_MAX; +} + +int uicr_location_set(uint32_t location) +{ + if (location == BT_AUDIO_LOCATION_FRONT_LEFT) { + return uicr_channel_set(0U); + } + if (location == BT_AUDIO_LOCATION_FRONT_RIGHT) { + return uicr_channel_set(1U); + } + + return -EINVAL; +} + uint8_t uicr_channel_get(void) { return *(uint8_t *)MEM_ADDR_UICR_CH; diff --git a/src/utils/uicr.h b/src/utils/uicr.h index ee933873..9716bfda 100644 --- a/src/utils/uicr.h +++ b/src/utils/uicr.h @@ -8,6 +8,7 @@ #define _UICR_H_ #include +#include // TODO: Discuss better alternative for UICR storage. This memory range is not documented #define UICR_APP_BASE_ADDR (NRF_UICR_S_BASE + 0xF0) @@ -17,19 +18,22 @@ extern "C" { #endif /** - * @brief Get raw channel value from UICR + * @brief Get raw location value from UICR as bitfield */ -uint8_t uicr_channel_get(void); +uint32_t uicr_location_get(void); /** - * @brief Write raw channel value to UICR + * @brief Write raw location bitfield value to UICR * - * @param channel Channel value + * @param location Location bitfield in accordance with LE Audio specification. * * @return 0 if successful - * @return -EROFS if different channel is already written - * @return -EIO if channel failed to be written + * @return -EROFS if different location is already written + * @return -EIO if location failed to be written */ +int uicr_location_set(uint32_t location); + +uint8_t uicr_channel_get(void); int uicr_channel_set(uint8_t channel); /** diff --git a/sysbuild.conf b/sysbuild.conf new file mode 100644 index 00000000..e6e9dc77 --- /dev/null +++ b/sysbuild.conf @@ -0,0 +1 @@ +SB_CONFIG_MERGED_HEX_FILES=y diff --git a/sysbuild/b0n_fota.overlay b/sysbuild/b0n_fota.overlay new file mode 100644 index 00000000..73e68cb7 --- /dev/null +++ b/sysbuild/b0n_fota.overlay @@ -0,0 +1,34 @@ +/* Fixed network-core boot layout for MCUboot multi-image updates. */ + +/ { + chosen { + zephyr,code-partition = &b0n_partition; + }; +}; + +/delete-node/ &boot_partition; +/delete-node/ &slot0_partition; +/delete-node/ &slot1_partition; +/delete-node/ &storage_partition; + +&flash1 { + partitions { + b0n_partition: partition@0 { + compatible = "zephyr,mapped-partition"; + label = "b0n"; + reg = <0x00000000 0x00008580>; + }; + + bl_storage: provision_partition: partition@8580 { + compatible = "zephyr,mapped-partition"; + label = "b0-provision-data"; + reg = <0x00008580 0x00000280>; + }; + + s0_partition: partition@8800 { + compatible = "zephyr,mapped-partition"; + label = "image-0"; + reg = <0x00008800 0x00037800>; + }; + }; +}; diff --git a/sysbuild/ipc_radio/prj.conf b/sysbuild/ipc_radio/prj.conf index 670e16c8..12cd3a34 100644 --- a/sysbuild/ipc_radio/prj.conf +++ b/sysbuild/ipc_radio/prj.conf @@ -24,6 +24,7 @@ CONFIG_BT_PER_ADV_SYNC_TRANSFER_RECEIVER=y CONFIG_BT_PER_ADV_SYNC_TRANSFER_SENDER=y CONFIG_BT_CTLR_CONN_ISO_GROUPS=1 +# Match the application's two ISO channels and leave RAM for full-size ACL packets. CONFIG_BT_CTLR_CONN_ISO_STREAMS=2 CONFIG_BT_CTLR_SYNC_ISO_STREAM_COUNT=2 CONFIG_BT_CTLR_ADV_ISO_SET=1 @@ -36,6 +37,11 @@ CONFIG_BT_CTLR_SDC_PERIPHERAL_COUNT=1 # Allow using more than default advertising event length CONFIG_BT_CTLR_ADV_DATA_LEN_MAX=251 +# Enable full-size ACL packets; the host fragments larger L2CAP messages. +CONFIG_BT_BUF_ACL_RX_SIZE=251 +CONFIG_BT_BUF_ACL_TX_SIZE=251 +CONFIG_BT_CTLR_DATA_LENGTH_MAX=251 + # To present the audio at the right point in time, we need the controller and # audio clock to be synchronized CONFIG_MPSL_TRIGGER_IPC_TASK_ON_RTC_START=y @@ -49,28 +55,7 @@ CONFIG_MPSL_TRIGGER_IPC_TASK_ON_RTC_START_CHANNEL=4 CONFIG_IPC_RADIO_BT=y CONFIG_IPC_RADIO_BT_HCI_IPC=y -#custom -CONFIG_BT_CTLR_DATA_LENGTH_MAX=251 -CONFIG_BT_BUF_ACL_RX_SIZE=502 -CONFIG_BT_BUF_ACL_TX_SIZE=502 -#CONFIG_BT_BUF_ACL_TX_SIZE=251 -CONFIG_BT_L2CAP_TX_MTU=498 -CONFIG_BT_CTLR_PHY_2M=y -CONFIG_BT_CTLR_PHY_CODED=n - -CONFIG_BT_CTLR_RX_BUFFERS=8 -#CONFIG_BT_CTLR_TX_BUFFERS=8 - -CONFIG_BT_CTLR_ISO_TX_BUFFERS=16 -CONFIG_BT_CTLR_ISO_RX_BUFFERS=16 - -#CONFIG_BT_ISO_RX_MTU=498 -#CONFIG_BT_ISO_TX_MTU=498 - -CONFIG_BT_ISO_TX_BUF_COUNT=1 -CONFIG_BT_ISO_RX_BUF_COUNT=1 -CONFIG_BT_ATT_PREPARE_COUNT=8 -CONFIG_BT_L2CAP_DYNAMIC_CHANNEL=y - -#CONFIG_BT_BUF_ACL_TX_COUNT=20 -#CONFIG_BT_L2CAP_TX_BUF_COUNT=20 +# Retain PR309's ACL reservation while migrating the controller configuration. +# A 2500 us reservation caused sustained alternating CIS loss with the phone. +CONFIG_BT_CTLR_SDC_MAX_CONN_EVENT_LEN_DEFAULT_OVERRIDE=y +CONFIG_BT_CTLR_SDC_MAX_CONN_EVENT_LEN_DEFAULT=2000 diff --git a/sysbuild/ipc_radio/prj_release.conf b/sysbuild/ipc_radio/prj_release.conf index 93f28af9..46c81a5e 100644 --- a/sysbuild/ipc_radio/prj_release.conf +++ b/sysbuild/ipc_radio/prj_release.conf @@ -54,3 +54,8 @@ CONFIG_CONSOLE=n CONFIG_PRINTK=n CONFIG_UART_CONSOLE=n CONFIG_BOOT_BANNER=n + +# Keep larger ACL data packets while leaving headroom for CIS audio events. +# A 2500 us reservation can cause sustained alternating CIS loss with a phone. +CONFIG_BT_CTLR_SDC_MAX_CONN_EVENT_LEN_DEFAULT_OVERRIDE=y +CONFIG_BT_CTLR_SDC_MAX_CONN_EVENT_LEN_DEFAULT=2000 diff --git a/sysbuild/ipc_radio_fota.overlay b/sysbuild/ipc_radio_fota.overlay new file mode 100644 index 00000000..0373f29f --- /dev/null +++ b/sysbuild/ipc_radio_fota.overlay @@ -0,0 +1,28 @@ +/* Match the network-core image to the b0n-secured slot. */ + +/delete-node/ &boot_partition; +/delete-node/ &slot0_partition; +/delete-node/ &slot1_partition; +/delete-node/ &storage_partition; + +&flash1 { + partitions { + b0n_partition: partition@0 { + compatible = "zephyr,mapped-partition"; + label = "b0n"; + reg = <0x00000000 0x00008580>; + }; + + bl_storage: provision_partition: partition@8580 { + compatible = "zephyr,mapped-partition"; + label = "b0-provision-data"; + reg = <0x00008580 0x00000280>; + }; + + s0_partition: partition@8800 { + compatible = "zephyr,mapped-partition"; + label = "image-0"; + reg = <0x00008800 0x00037800>; + }; + }; +}; diff --git a/sysbuild/mcuboot/boards/openearable_v2_nrf5340_cpuapp_fota.overlay b/sysbuild/mcuboot/boards/openearable_v2_nrf5340_cpuapp_fota.overlay index 0719884c..7cf7cb42 100644 --- a/sysbuild/mcuboot/boards/openearable_v2_nrf5340_cpuapp_fota.overlay +++ b/sysbuild/mcuboot/boards/openearable_v2_nrf5340_cpuapp_fota.overlay @@ -2,6 +2,8 @@ / { chosen { + /* Board overlays take precedence over app_fota.overlay. */ + zephyr,code-partition = &boot_partition; nordic,pm-ext-flash = &mx25r64; }; }; diff --git a/sysbuild/mcuboot/prj_fota.conf b/sysbuild/mcuboot/prj_fota.conf index 39b5dad4..9c57e9b0 100644 --- a/sysbuild/mcuboot/prj_fota.conf +++ b/sysbuild/mcuboot/prj_fota.conf @@ -7,8 +7,6 @@ CONFIG_PM=n CONFIG_MAIN_STACK_SIZE=10240 -CONFIG_MBEDTLS_CFG_FILE="mcuboot-mbedtls-cfg.h" - CONFIG_BOOT_SWAP_SAVE_ENCTLV=n CONFIG_BOOT_ENCRYPT_IMAGE=n @@ -17,18 +15,11 @@ CONFIG_BOOT_BOOTSTRAP=n ### mbedTLS has its own heap # CONFIG_HEAP_MEM_POOL_SIZE is not set -### We never want Zephyr's copy of tinycrypt. If tinycrypt is needed, -### MCUboot has its own copy in tree. -# CONFIG_TINYCRYPT is not set -# CONFIG_TINYCRYPT_ECC_DSA is not set -# CONFIG_TINYCRYPT_SHA256 is not set - CONFIG_FLASH=y CONFIG_FPROTECT=y ### Various Zephyr boards enable features that we don't want. # CONFIG_BT is not set -# CONFIG_BT_CTLR is not set # CONFIG_I2C is not set CONFIG_LOG=y @@ -43,13 +34,12 @@ CONFIG_CBPRINTF_NANO=y CONFIG_MINIMAL_LIBC=y CONFIG_NRF_RTC_TIMER_USER_CHAN_COUNT=0 -CONFIG_BOOT_MAX_IMG_SECTORS=2048 +CONFIG_BOOT_MAX_IMG_SECTORS_AUTO=y # Flash CONFIG_SPI=y CONFIG_SPI_NOR=y CONFIG_SPI_NOR_SFDP_DEVICETREE=y -CONFIG_SPI_NOR_CS_WAIT_DELAY=5 CONFIG_SPI_NOR_FLASH_LAYOUT_PAGE_SIZE=4096 CONFIG_MULTITHREADING=y CONFIG_BOOT_ERASE_PROGRESSIVELY=y @@ -65,8 +55,6 @@ CONFIG_FLASH_SIMULATOR_STATS=n # CONFIG_UART_CONSOLE=y -CONFIG_MCUBOOT_INDICATION_LED=y - # CONFIG_NORDIC_QSPI_NOR=y # CONFIG_BOOT_MAX_IMG_SECTORS=512 @@ -90,4 +78,4 @@ CONFIG_SPI_NRFX_RAM_BUFFER_SIZE=4096 #CONFIG_BOOT_SWAP_USING_SCRATCH=y # Required for scratch-based swap -#CONFIG_BOOT_SCRATCH_SIZE=0x1000 \ No newline at end of file +#CONFIG_BOOT_SCRATCH_SIZE=0x1000 diff --git a/sysbuild_fota.conf b/sysbuild_fota.conf index ac07f63f..35345876 100644 --- a/sysbuild_fota.conf +++ b/sysbuild_fota.conf @@ -8,8 +8,7 @@ SB_CONFIG_BOOTLOADER_MCUBOOT=y SB_CONFIG_SECURE_BOOT_NETCORE=y SB_CONFIG_NETCORE_APP_UPDATE=y +SB_CONFIG_MCUBOOT_NRF53_MULTI_IMAGE_UPDATE=y +SB_CONFIG_MCUBOOT_MODE_OVERWRITE_ONLY=y -# Settings required for external flash to be used for DFU -SB_CONFIG_PM_EXTERNAL_FLASH_MCUBOOT_SECONDARY=y -SB_CONFIG_PM_OVERRIDE_EXTERNAL_DRIVER_CHECK=y -SB_CONFIG_MCUBOOT_MODE_OVERWRITE_ONLY=n +SB_CONFIG_MERGED_HEX_FILES=y diff --git a/tests/README.md b/tests/README.md index f0fde713..5f5232b5 100644 --- a/tests/README.md +++ b/tests/README.md @@ -1,11 +1,11 @@ # Tests Unit tests live in `tests/unit`. Zephyr's -[Twister test runner](https://docs.nordicsemi.com/bundle/ncs-3.0.1/page/zephyr/develop/test/twister.html) +[Twister test runner](https://docs.nordicsemi.com/bundle/ncs-3.4.1/page/zephyr/develop/test/twister.html) builds each -[Unity suite](https://github.com/nrfconnect/sdk-nrf/blob/v3.0.1/doc/nrf/test_and_optimize/test_framework/testing_unity_cmock.rst) +[Unity suite](https://github.com/nrfconnect/sdk-nrf/blob/v3.4.1/doc/nrf/test_and_optimize/test_framework/testing_unity_cmock.rst) as a Linux executable using -[`native_sim/native/64`](https://docs.nordicsemi.com/bundle/ncs-3.0.1/page/zephyr/boards/native/native_sim/doc/index.html), +[`native_sim/native/64`](https://docs.nordicsemi.com/bundle/ncs-3.4.1/page/zephyr/boards/native/native_sim/doc/index.html), so the tests run on a development computer or GitHub Actions runner and do not require OpenEarable hardware. These links target the nRF Connect SDK version pinned in @@ -95,7 +95,7 @@ Unity runner can call the nRF Connect SDK's C teardown function. ## Generate coverage Install `gcovr` in the active Python environment and run Twister with -[coverage enabled](https://docs.nordicsemi.com/bundle/ncs-3.0.1/page/zephyr/develop/test/coverage.html): +[coverage enabled](https://docs.nordicsemi.com/bundle/ncs-3.4.1/page/zephyr/develop/test/coverage.html): ```sh python3 -m pip install gcovr diff --git a/tests/unit/audio_encoder/CMakeLists.txt b/tests/unit/audio_encoder/CMakeLists.txt new file mode 100644 index 00000000..a4bbd561 --- /dev/null +++ b/tests/unit/audio_encoder/CMakeLists.txt @@ -0,0 +1,25 @@ +# SPDX-License-Identifier: Apache-2.0 +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(audio_encoder_tests) + +test_runner_generate(src/main.c) +target_include_directories(app PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../../../src/audio + ${CMAKE_CURRENT_SOURCE_DIR}/../../../src/utils + ${CMAKE_CURRENT_SOURCE_DIR}/../../../src/utils/macros + ${ZEPHYR_NRFXLIB_MODULE_DIR}/lc3/include) +# Exercise the production wrapper with a fake LC3 codec on the host. +target_compile_definitions(app PRIVATE + CONFIG_SW_CODEC_LC3=1 + CONFIG_SW_CODEC_SELECT_LOG_LEVEL=0 + CONFIG_AUDIO_ENCODE_CHANNELS_MAX=2 + CONFIG_AUDIO_DECODE_CHANNELS_MAX=2 + CONFIG_AUDIO_OUTPUT_CHANNELS=2 + CONFIG_AUDIO_SAMPLE_RATE_HZ=48000 + CONFIG_AUDIO_BIT_DEPTH_BITS=16 + CONFIG_AUDIO_BIT_DEPTH_OCTETS=2 + CONFIG_AUDIO_FRAME_DURATION_US=10000 + CONFIG_LC3_BITRATE_MAX=96000) +target_sources(app PRIVATE src/main.c src/codec_stub.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../../src/audio/sw_codec_select.c) diff --git a/tests/unit/audio_encoder/prj.conf b/tests/unit/audio_encoder/prj.conf new file mode 100644 index 00000000..a923a87f --- /dev/null +++ b/tests/unit/audio_encoder/prj.conf @@ -0,0 +1,5 @@ +CONFIG_UNITY=y +CONFIG_NET_BUF=y +CONFIG_BT=y +CONFIG_PSCM=y +CONFIG_MAIN_STACK_SIZE=8192 diff --git a/tests/unit/audio_encoder/src/codec_stub.c b/tests/unit/audio_encoder/src/codec_stub.c new file mode 100644 index 00000000..56384404 --- /dev/null +++ b/tests/unit/audio_encoder/src/codec_stub.c @@ -0,0 +1,39 @@ +#include +#include +#include + +/* Keep channel selection observable without the hardware LC3 library. */ +unsigned int encode_calls; + +int sw_codec_lc3_enc_run(void const *const pcm_data, uint32_t pcm_data_size, + uint32_t enc_bitrate, uint8_t audio_ch, uint16_t lc3_data_buf_size, + uint8_t *const lc3_data, uint16_t *const lc3_data_wr_size) +{ + ++encode_calls; + memcpy(lc3_data, pcm_data, sizeof(uint16_t)); + *lc3_data_wr_size = sizeof(uint16_t); + return 0; +} + +int sw_codec_lc3_single_rate_init(uint16_t encoder_sample_rate, uint16_t decoder_sample_rate, + uint8_t *buffer, uint32_t *buffer_size, uint16_t framesize_us) +{ + return 0; +} + +int sw_codec_lc3_enc_init(uint16_t pcm_sample_rate, uint8_t pcm_bit_depth, uint16_t framesize_us, + uint32_t enc_bitrate, uint8_t num_channels, uint16_t *const pcm_bytes_req) +{ + *pcm_bytes_req = 4; + return 0; +} + +int sw_codec_lc3_dec_init(uint16_t pcm_sample_rate, uint8_t pcm_bit_depth, uint16_t framesize_us, + uint8_t num_channels) +{ + return 0; +} + +int sw_codec_lc3_enc_uninit_all(void) { return 0; } +int sw_codec_lc3_dec_uninit_all(void) { return 0; } +int sw_codec_lc3_uninit(void) { return 0; } diff --git a/tests/unit/audio_encoder/src/main.c b/tests/unit/audio_encoder/src/main.c new file mode 100644 index 00000000..c44f922a --- /dev/null +++ b/tests/unit/audio_encoder/src/main.c @@ -0,0 +1,122 @@ +#include +#include +#include "sw_codec_select.h" + +#define LEFT BT_AUDIO_LOCATION_FRONT_LEFT +#define RIGHT BT_AUDIO_LOCATION_FRONT_RIGHT +#define MONO BT_AUDIO_LOCATION_MONO_AUDIO + +NET_BUF_POOL_FIXED_DEFINE(frames, 2, 32, sizeof(struct audio_metadata), NULL); +static struct net_buf *input; +static struct net_buf *output; +static const struct sw_codec_config config = { + .sw_codec = SW_CODEC_LC3, + .encoder = {.enabled = true, .num_ch = 2, .sample_rate_hz = 48000, .bitrate = 96000}, +}; +extern unsigned int encode_calls; + +void setUp(void) +{ + TEST_ASSERT_EQUAL_INT(0, sw_codec_init(config)); + encode_calls = 0; + input = net_buf_alloc(&frames, K_NO_WAIT); + output = net_buf_alloc(&frames, K_NO_WAIT); + TEST_ASSERT_NOT_NULL(input); + TEST_ASSERT_NOT_NULL(output); + memset(output->data, 0xa5, output->size); +} + +void tearDown(void) +{ + net_buf_unref(input); + net_buf_unref(output); + TEST_ASSERT_EQUAL_INT(0, sw_codec_uninit(config)); +} + +static void prepare(uint32_t in_locations, uint32_t out_locations, bool interleaved) +{ + struct audio_metadata *in = net_buf_user_data(input); + struct audio_metadata *out = net_buf_user_data(output); + *in = (struct audio_metadata){ + .data_coding = PCM, .sample_rate_hz = 48000, .carried_bits_per_sample = 16, + .bits_per_sample = 16, .bytes_per_location = 4, .locations = in_locations, + .interleaved = interleaved, + }; + *out = *in; + out->data_coding = LC3; + out->bytes_per_location = 2; + out->locations = out_locations; + const uint16_t planar[] = {0x1111, 0x1112, 0x2221, 0x2222}; + const uint16_t stereo[] = {0x1111, 0x2221, 0x1112, 0x2222}; + net_buf_add_mem(input, interleaved ? stereo : planar, + 4 * audio_metadata_num_loc_get(in)); +} + +void test_disjoint_channels_reject_without_mutating_output(void) +{ + prepare(LEFT, RIGHT, false); + struct audio_metadata before = *(struct audio_metadata *)net_buf_user_data(output); + TEST_ASSERT_EQUAL_INT(-EINVAL, sw_codec_encode(input, output)); + TEST_ASSERT_EQUAL_UINT(0, encode_calls); + TEST_ASSERT_EQUAL_UINT(0, output->len); + TEST_ASSERT_EQUAL_MEMORY(&before, net_buf_user_data(output), sizeof(before)); + for (unsigned int i = 0; i < output->size; ++i) { + TEST_ASSERT_EQUAL_HEX8(0xa5, output->data[i]); + } +} + +void test_matching_right_channel_encodes_one_frame(void) +{ + prepare(RIGHT, RIGHT, false); + TEST_ASSERT_EQUAL_INT(0, sw_codec_encode(input, output)); + TEST_ASSERT_EQUAL_UINT(1, encode_calls); + TEST_ASSERT_EQUAL_UINT(2, output->len); + TEST_ASSERT_EQUAL_UINT32(RIGHT, + ((struct audio_metadata *)net_buf_user_data(output))->locations); +} + +void test_stereo_encodes_both_channels(void) +{ + prepare(LEFT | RIGHT, LEFT | RIGHT, true); + const uint16_t expected[] = {0x1111, 0x2221}; + TEST_ASSERT_EQUAL_INT(0, sw_codec_encode(input, output)); + TEST_ASSERT_EQUAL_UINT(2, encode_calls); + TEST_ASSERT_EQUAL_UINT(sizeof(expected), output->len); + TEST_ASSERT_EQUAL_MEMORY(expected, output->data, sizeof(expected)); +} + +static void check_right_selection(bool interleaved) +{ + prepare(LEFT | RIGHT, RIGHT, interleaved); + const uint16_t expected = 0x2221; + TEST_ASSERT_EQUAL_INT(0, sw_codec_encode(input, output)); + TEST_ASSERT_EQUAL_UINT(1, encode_calls); + TEST_ASSERT_EQUAL_UINT(sizeof(expected), output->len); + TEST_ASSERT_EQUAL_MEMORY(&expected, output->data, sizeof(expected)); +} + +void test_select_right_from_interleaved_stereo(void) { check_right_selection(true); } +void test_select_right_from_planar_stereo(void) { check_right_selection(false); } + +void test_mono_input_to_mono_output_is_valid(void) +{ + prepare(MONO, MONO, false); + TEST_ASSERT_EQUAL_INT(0, sw_codec_encode(input, output)); + TEST_ASSERT_EQUAL_UINT(1, encode_calls); + TEST_ASSERT_EQUAL_UINT(2, output->len); + TEST_ASSERT_EQUAL_UINT32(MONO, + ((struct audio_metadata *)net_buf_user_data(output))->locations); +} + +void test_stereo_to_mono_selects_first_channel(void) +{ + prepare(LEFT | RIGHT, MONO, true); + const uint16_t expected = 0x1111; + TEST_ASSERT_EQUAL_INT(0, sw_codec_encode(input, output)); + TEST_ASSERT_EQUAL_UINT(1, encode_calls); + TEST_ASSERT_EQUAL_UINT(sizeof(expected), output->len); + TEST_ASSERT_EQUAL_MEMORY(&expected, output->data, sizeof(expected)); +} + +extern int unity_main(void); +int main(void) { return unity_main(); } diff --git a/tests/unit/audio_encoder/testcase.yaml b/tests/unit/audio_encoder/testcase.yaml new file mode 100644 index 00000000..e204aa5a --- /dev/null +++ b/tests/unit/audio_encoder/testcase.yaml @@ -0,0 +1,9 @@ +tests: + openearable.unit.audio_encoder: + tags: + - unit + - audio + integration_platforms: + - native_sim/native/64 + platform_allow: + - native_sim/native/64 diff --git a/tests/unit/audio_startup/CMakeLists.txt b/tests/unit/audio_startup/CMakeLists.txt new file mode 100644 index 00000000..ca769f7e --- /dev/null +++ b/tests/unit/audio_startup/CMakeLists.txt @@ -0,0 +1,8 @@ +# SPDX-License-Identifier: Apache-2.0 +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(audio_startup_tests) + +test_runner_generate(src/main.c) +target_include_directories(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../../../src/audio) +target_sources(app PRIVATE src/main.c) diff --git a/tests/unit/audio_startup/prj.conf b/tests/unit/audio_startup/prj.conf new file mode 100644 index 00000000..3fb209dc --- /dev/null +++ b/tests/unit/audio_startup/prj.conf @@ -0,0 +1 @@ +CONFIG_UNITY=y diff --git a/tests/unit/audio_startup/src/main.c b/tests/unit/audio_startup/src/main.c new file mode 100644 index 00000000..76635694 --- /dev/null +++ b/tests/unit/audio_startup/src/main.c @@ -0,0 +1,75 @@ +/* SPDX-License-Identifier: Apache-2.0 */ +#include +#include +#include "audio_startup.h" + +void setUp(void) +{ +} + +void test_requires_twenty_ms_at_both_sdk_block_durations(void) +{ + const uint32_t periods_us[] = {500U, 1000U}; + + for (unsigned int p = 0; p < sizeof(periods_us) / sizeof(periods_us[0]); ++p) { + struct audio_startup gate = {0}; + uint32_t period_us = periods_us[p]; + uint32_t blocks = 20000U / period_us; + + for (uint32_t i = 1; i < blocks; ++i) { + TEST_ASSERT_FALSE(audio_startup_ready(&gate, true, true, period_us)); + } + /* A gap, concealed frame, or loss of lock restarts the settling period. */ + TEST_ASSERT_FALSE(audio_startup_ready(&gate, true, false, period_us)); + for (uint32_t i = 1; i < blocks; ++i) { + TEST_ASSERT_FALSE(audio_startup_ready(&gate, true, true, period_us)); + } + TEST_ASSERT_TRUE(audio_startup_ready(&gate, true, true, period_us)); + TEST_ASSERT_EQUAL_UINT32(20000U, gate.stable_us); + } +} + +void test_open_gate_survives_later_loss_of_lock(void) +{ + struct audio_startup gate = {.open = true}; + + TEST_ASSERT_TRUE(audio_startup_ready(&gate, true, false, 500U)); +} + +void test_stream_stop_resets_gate_and_fade_without_stopping_i2s(void) +{ + struct audio_startup gate = {.stable_us = 20000U, .fade_frames = 240, .open = true}; + + TEST_ASSERT_FALSE(audio_startup_ready(&gate, false, true, 500U)); + TEST_ASSERT_FALSE(gate.open); + TEST_ASSERT_EQUAL_UINT32(0, gate.stable_us); + TEST_ASSERT_EQUAL_UINT32(0, gate.fade_frames); + TEST_ASSERT_FALSE(audio_startup_ready(&gate, true, true, 500U)); +} + +void test_fade_handles_full_scale_pcm_and_stops_at_unity_gain(void) +{ + const int32_t samples[] = {INT32_MIN, INT32_MAX, INT16_MIN, INT16_MAX, 0}; + struct audio_startup gate = {0}; + + /* Five milliseconds at 48 kHz, with equal gain on both stereo channels. */ + for (uint32_t n = 1; n <= 240; ++n) { + uint32_t gain = audio_startup_gain(&gate, 240); + TEST_ASSERT_EQUAL_UINT32(n, gain); + for (unsigned int i = 0; i < sizeof(samples) / sizeof(samples[0]); ++i) { + TEST_ASSERT_EQUAL_INT32((int64_t)samples[i] * n / 240, + audio_startup_scale(samples[i], gain, 240)); + } + } + TEST_ASSERT_EQUAL_UINT32(240, audio_startup_gain(&gate, 240)); + TEST_ASSERT_EQUAL_INT32(INT32_MIN, audio_startup_scale(INT32_MIN, 240, 240)); + TEST_ASSERT_EQUAL_INT32(INT16_MAX, audio_startup_scale(INT16_MAX, 240, 240)); +} + +extern int unity_main(void); + +int main(void) +{ + (void)unity_main(); + return 0; +} diff --git a/tests/unit/audio_startup/testcase.yaml b/tests/unit/audio_startup/testcase.yaml new file mode 100644 index 00000000..e5812cf6 --- /dev/null +++ b/tests/unit/audio_startup/testcase.yaml @@ -0,0 +1,9 @@ +tests: + openearable.unit.audio_startup: + tags: + - unit + - audio + integration_platforms: + - native_sim/native/64 + platform_allow: + - native_sim/native/64 diff --git a/tests/unit/sensor_transport/CMakeLists.txt b/tests/unit/sensor_transport/CMakeLists.txt new file mode 100644 index 00000000..bcdf76fe --- /dev/null +++ b/tests/unit/sensor_transport/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(sensor_transport_tests) +test_runner_generate(src/main.c) +target_include_directories(app PRIVATE + ../../../src/bluetooth/gatt_services + ../../../protocol/generated/c/include) +target_sources(app PRIVATE src/main.c + ../../../src/bluetooth/gatt_services/sensor_transport.c + ../../../protocol/generated/c/src/ppg_protocol.c + ../../../protocol/generated/c/src/imu_protocol.c + ../../../protocol/generated/c/src/protocol_runtime.c) diff --git a/tests/unit/sensor_transport/prj.conf b/tests/unit/sensor_transport/prj.conf new file mode 100644 index 00000000..3fb209dc --- /dev/null +++ b/tests/unit/sensor_transport/prj.conf @@ -0,0 +1 @@ +CONFIG_UNITY=y diff --git a/tests/unit/sensor_transport/src/main.c b/tests/unit/sensor_transport/src/main.c new file mode 100644 index 00000000..dfcd5d60 --- /dev/null +++ b/tests/unit/sensor_transport/src/main.c @@ -0,0 +1,119 @@ +#include +#include +#include +#include "sensor_transport.h" + +void setUp(void) {} + +void test_compact_ppg_preserves_full_scale_and_header(void) +{ + struct oe_sensor_batch batch = {0}; + const uint8_t sample[16] = {255,255,7,0,255,255,7,0,255,255,7,0,255,255,7,0}; + const uint8_t expected[20] = {4,10,1,2,3,4,5,6,7,0, + 255,255,255,255,255,255,255,255,255,15}; + TEST_ASSERT_TRUE(oe_sensor_batch_append(&batch, 4, sample, 0x07060504030201ULL, 20)); + TEST_ASSERT_EQUAL_UINT(20, batch.len); + TEST_ASSERT_EQUAL_MEMORY(expected, batch.data, sizeof(expected)); + /* Internal/SD layout stays four uint32 channels, including batched records. */ + TEST_ASSERT_EQUAL_UINT(16, oe_sensor_sample_size(4)); + TEST_ASSERT_EQUAL_UINT(1, oe_sensor_sample_count(4, 16)); + TEST_ASSERT_EQUAL_UINT(3, oe_sensor_sample_count(4, 50)); + TEST_ASSERT_EQUAL_UINT(0, oe_sensor_sample_count(4, 10)); +} + +void test_compact_ppg_fits_twenty_three_samples_without_changing_timestamps(void) +{ + struct oe_sensor_batch batch = {0}; + const uint8_t sample[16] = {1,0,0,0,2,0,0,0,3,0,0,0,4,0,0,0}; + const uint8_t packed[10] = {1,0,16,0,192,0,0,8,0,0}; + for (unsigned i = 0; i < 23; ++i) { + TEST_ASSERT_TRUE(oe_sensor_batch_append(&batch, 4, sample, 100000 + 1953 * i, 244)); + TEST_ASSERT_EQUAL_MEMORY(packed, batch.data + 10 + 10 * i, 10); + } + TEST_ASSERT_EQUAL_UINT(23, batch.count); + TEST_ASSERT_EQUAL_UINT(242, batch.len); + TEST_ASSERT_EQUAL_UINT(232, batch.data[1]); + TEST_ASSERT_EQUAL_UINT(1953 & 255, batch.data[240]); + TEST_ASSERT_EQUAL_UINT(1953 >> 8, batch.data[241]); + struct oe_sensor_batch before = batch; + TEST_ASSERT_FALSE(oe_sensor_batch_append(&batch, 4, sample, 144919, 244)); + TEST_ASSERT_EQUAL_MEMORY(&before, &batch, sizeof(batch)); +} + +void test_invalid_ppg_is_rejected_instead_of_losing_high_bits(void) +{ + struct oe_sensor_batch batch = {0}; + const uint8_t sample[16] = {0,0,8,0}; + TEST_ASSERT_FALSE(oe_sensor_batch_append(&batch, 4, sample, 100, 244)); + TEST_ASSERT_EQUAL_UINT(0, batch.count); +} + +void test_other_sensor_samples_keep_the_existing_wire_bytes(void) +{ + const uint8_t ids[] = {1, 6, 7}; + uint8_t sample[36]; + for (unsigned i = 0; i < sizeof(sample); ++i) sample[i] = i; + for (unsigned n = 0; n < sizeof(ids); ++n) { + struct oe_sensor_batch batch = {0}; + unsigned width = oe_sensor_sample_size(ids[n]); + TEST_ASSERT_TRUE(oe_sensor_batch_append(&batch, ids[n], sample, 123456, 244)); + TEST_ASSERT_EQUAL_UINT(width + 10, batch.len); + TEST_ASSERT_EQUAL_MEMORY(sample, batch.data + 10, width); + } +} + +void test_compact_imu_preserves_every_raw_value_and_sd_sample(void) +{ + for (int raw = INT16_MIN; raw <= INT16_MAX; ++raw) { + const float a = raw * ((2.0f * 9.80665f) / 32768.0f); + const float g = raw * (2000.0f / 32768.0f); + const float values[9] = {a,a,a,g,g,g,12.345f,-67.89f,-0.0f}; + uint8_t sample[36]; + memcpy(sample, values, sizeof(sample)); + struct oe_sensor_batch batch = {0}; + TEST_ASSERT_TRUE(oe_sensor_batch_append(&batch, 0, sample, 123, 244)); + TEST_ASSERT_EQUAL_UINT(34, batch.len); + for (unsigned axis = 0; axis < 6; ++axis) { + const uint8_t *v = batch.data + 10 + axis * 2; + TEST_ASSERT_EQUAL_UINT16((uint16_t)raw, (uint16_t)(v[0] | v[1] << 8)); + } + TEST_ASSERT_EQUAL_MEMORY(sample + 24, batch.data + 22, 12); + TEST_ASSERT_EQUAL_MEMORY(values, sample, sizeof(sample)); + } + TEST_ASSERT_EQUAL_UINT(36, oe_sensor_sample_size(0)); + TEST_ASSERT_EQUAL_UINT(1, oe_sensor_sample_count(0, 36)); + TEST_ASSERT_EQUAL_UINT(3, oe_sensor_sample_count(0, 110)); + TEST_ASSERT_EQUAL_UINT(0, oe_sensor_sample_count(0, 24)); +} + +void test_compact_imu_fits_nine_samples_and_preserves_timestamps(void) +{ + const float sample[9] = {0}; + struct oe_sensor_batch batch = {0}; + for (unsigned i = 0; i < 9; ++i) + TEST_ASSERT_TRUE(oe_sensor_batch_append(&batch, 0, (const uint8_t *)sample, + 123456 + 10000 * i, 244)); + TEST_ASSERT_EQUAL_UINT(9, batch.count); + TEST_ASSERT_EQUAL_UINT(228, batch.len); + TEST_ASSERT_EQUAL_UINT(218, batch.data[1]); + TEST_ASSERT_EQUAL_UINT(10000 & 255, batch.data[226]); + TEST_ASSERT_EQUAL_UINT(10000 >> 8, batch.data[227]); + struct oe_sensor_batch before = batch; + TEST_ASSERT_FALSE(oe_sensor_batch_append(&batch, 0, (const uint8_t *)sample, 213456, 244)); + TEST_ASSERT_EQUAL_MEMORY(&before, &batch, sizeof(batch)); +} + +void test_compact_imu_rejects_values_that_cannot_be_encoded_losslessly(void) +{ + const float invalid[] = {NAN, INFINITY, -INFINITY, 100000.0f, 0.001f, -0.0f}; + for (unsigned i = 0; i < sizeof(invalid) / sizeof(invalid[0]); ++i) { + struct oe_sensor_batch batch = {0}; + float sample[9] = {0}; + sample[i % 6] = invalid[i]; + TEST_ASSERT_FALSE(oe_sensor_batch_append(&batch, 0, (const uint8_t *)sample, 123, 244)); + TEST_ASSERT_EQUAL_UINT(0, batch.count); + } +} + +extern int unity_main(void); +int main(void) { return unity_main(); } diff --git a/tests/unit/sensor_transport/testcase.yaml b/tests/unit/sensor_transport/testcase.yaml new file mode 100644 index 00000000..e9519767 --- /dev/null +++ b/tests/unit/sensor_transport/testcase.yaml @@ -0,0 +1,5 @@ +tests: + openearable.unit.sensor_transport: + tags: [unit, bluetooth] + integration_platforms: [native_sim/native/64] + platform_allow: [native_sim/native/64] diff --git a/tools/buildprog/nrf5340_audio_dk_devices.json b/tools/buildprog/nrf5340_audio_dk_devices.json index 0a42fe36..37fb2f48 100644 --- a/tools/buildprog/nrf5340_audio_dk_devices.json +++ b/tools/buildprog/nrf5340_audio_dk_devices.json @@ -1,17 +1,26 @@ -[ - { - "nrf5340_audio_dk_snr": 1000, - "nrf5340_audio_dk_dev": "headset", - "channel": "left" - }, - { - "nrf5340_audio_dk_snr": 1000, - "nrf5340_audio_dk_dev": "gateway", - "channel": "NA" - }, - { - "nrf5340_audio_dk_snr": 1000, - "nrf5340_audio_dk_dev": "headset", - "channel": "right" - } -] \ No newline at end of file +{ + "sirk": "NRF5340_TWS_DEMO", + "devices": [ + { + "nrf5340_audio_dk_snr": 1000, + "nrf5340_audio_dk_dev": "headset", + "location": [ + "FRONT_LEFT" + ] + }, + { + "nrf5340_audio_dk_snr": 1000, + "nrf5340_audio_dk_dev": "gateway", + "location": [ + "NA" + ] + }, + { + "nrf5340_audio_dk_snr": 1000, + "nrf5340_audio_dk_dev": "headset", + "location": [ + "FRONT_RIGHT" + ] + } + ] +} diff --git a/tools/buildprog/nrf5340_audio_dk_devices.py b/tools/buildprog/nrf5340_audio_dk_devices.py index 3da7ec4c..af88d71a 100644 --- a/tools/buildprog/nrf5340_audio_dk_devices.py +++ b/tools/buildprog/nrf5340_audio_dk_devices.py @@ -8,9 +8,8 @@ Settings for building and flashing nRF5340 Audio DK for different targets. """ from dataclasses import InitVar, dataclass, field -from enum import auto, Enum +from enum import Enum from pathlib import Path -from typing import List class SelectFlags(str, Enum): @@ -41,11 +40,49 @@ class BuildType(str, Enum): debug = "debug" -class Channel(Enum): - """Left or right Value represents UICR channel""" - left = 0 - right = 1 - NA = auto() +class Location(Enum): + """Location as bitfield stored in UICR. + This is the same as the bt_audio_location enum""" + + MONO_AUDIO = (0, "Mono Audio") + FRONT_LEFT = (1 << 0, "Front Left") + FRONT_RIGHT = (1 << 1, "Front Right") + FRONT_CENTER = (1 << 2, "Front Center") + LOW_FREQ_EFFECTS_1 = (1 << 3, "Low Frequency Effects 1") + BACK_LEFT = (1 << 4, "Back Left") + BACK_RIGHT = (1 << 5, "Back Right") + FRONT_LEFT_OF_CENTER = (1 << 6, "Front Left of Center") + FRONT_RIGHT_OF_CENTER = (1 << 7, "Front Right of Center") + BACK_CENTER = (1 << 8, "Back Center") + LOW_FREQ_EFFECTS_2 = (1 << 9, "Low Frequency Effects 2") + SIDE_LEFT = (1 << 10, "Side Left") + SIDE_RIGHT = (1 << 11, "Side Right") + TOP_FRONT_LEFT = (1 << 12, "Top Front Left") + TOP_FRONT_RIGHT = (1 << 13, "Top Front Right") + TOP_FRONT_CENTER = (1 << 14, "Top Front Center") + TOP_CENTER = (1 << 15, "Top Center") + TOP_BACK_LEFT = (1 << 16, "Top Back Left") + TOP_BACK_RIGHT = (1 << 17, "Top Back Right") + TOP_SIDE_LEFT = (1 << 18, "Top Side Left") + TOP_SIDE_RIGHT = (1 << 19, "Top Side Right") + TOP_BACK_CENTER = (1 << 20, "Top Back Center") + BOTTOM_FRONT_CENTER = (1 << 21, "Bottom Front Center") + BOTTOM_FRONT_LEFT = (1 << 22, "Bottom Front Left") + BOTTOM_FRONT_RIGHT = (1 << 23, "Bottom Front Right") + FRONT_LEFT_WIDE = (1 << 24, "Front Left Wide") + FRONT_RIGHT_WIDE = (1 << 25, "Front Right Wide") + LEFT_SURROUND = (1 << 26, "Left Surround") + RIGHT_SURROUND = (1 << 27, "Right Surround") + + "Kept for compatibility with old code, use the Location enum instead" + left = FRONT_LEFT + right = FRONT_RIGHT + + def __new__(cls, value, label): + obj = object.__new__(cls) + obj._value_ = value + obj.label = label + return obj @dataclass @@ -55,14 +92,16 @@ class DeviceConf: # Constructor variables nrf5340_audio_dk_snr: int - channel: Channel + location: list # Now a list of Locations snr_connected: bool nrf5340_audio_dk_dev: AudioDevice recover_on_fail: bool - cores: InitVar[List[Core]] - devices: InitVar[List[AudioDevice]] + cores: InitVar[list[Core]] + devices: InitVar[list[AudioDevice]] _only_reboot: InitVar[SelectFlags] + sirk: str = "" # SIRK for the device set + # Post init variables only_reboot: SelectFlags = field(init=False, default=SelectFlags.NOT) hex_path_app: Path = field(init=False, default=None) @@ -73,7 +112,7 @@ class DeviceConf: init=False, default=SelectFlags.NOT) def __post_init__( - self, cores: List[Core], devices: List[AudioDevice], _only_reboot: SelectFlags, + self, cores: list[Core], devices: list[AudioDevice], _only_reboot: SelectFlags, ): device_selected = self.nrf5340_audio_dk_dev in devices self.only_reboot = _only_reboot if device_selected else SelectFlags.NOT @@ -88,7 +127,11 @@ def __post_init__( def __str__(self): result = f"{self.nrf5340_audio_dk_snr} {self.nrf5340_audio_dk_dev.name}" if self.nrf5340_audio_dk_dev == AudioDevice.headset: - result += f" {self.channel.name}" + # Print all location labels if multiple + if isinstance(self.location, list): + result += " " + "+".join([loc.label for loc in self.location]) + else: + result += f" {self.location.name}" return result diff --git a/tools/flash/flash.sh b/tools/flash/flash.sh index 66b37a14..2dbe99be 100755 --- a/tools/flash/flash.sh +++ b/tools/flash/flash.sh @@ -1,8 +1,12 @@ #!/bin/bash +# Never erase or continue programming after a failed backup or flash step. +set -e + # Default parameters CLOCKSPEED=8000 CHIP=NRF53 +BUILD_DIR=./build # Function to show usage show_usage() { @@ -74,13 +78,39 @@ if [ -n "$HW_VERSION" ]; then HW_VALUE=$(printf "0x%02X%02X%02X00" $HW_MAJOR $HW_MINOR $HW_PATCH) fi +NEW_NET_HEX="$BUILD_DIR/merged_openearable_v2_nrf5340_cpunet.hex" +NEW_APP_HEX="$BUILD_DIR/merged_openearable_v2_nrf5340_cpuapp.hex" +LEGACY_NET_HEX="$BUILD_DIR/merged_CPUNET.hex" +LEGACY_APP_HEX="$BUILD_DIR/merged.hex" + +if [ -f "$NEW_NET_HEX" ] && [ -f "$NEW_APP_HEX" ]; then + NET_HEX="$NEW_NET_HEX" + APP_HEX="$NEW_APP_HEX" +elif [ -f "$LEGACY_NET_HEX" ] && [ -f "$LEGACY_APP_HEX" ]; then + NET_HEX="$LEGACY_NET_HEX" + APP_HEX="$LEGACY_APP_HEX" +else + NET_HEX="$NEW_NET_HEX" + APP_HEX="$NEW_APP_HEX" +fi + +if [ ! -f "$NET_HEX" ] || [ ! -f "$APP_HEX" ]; then + echo "Error: Missing merged standard image(s)." + echo "Expected: $NET_HEX" + echo "Expected: $APP_HEX" + echo "Legacy fallback: $LEGACY_NET_HEX" + echo "Legacy fallback: $LEGACY_APP_HEX" + echo "Rebuild the standard configuration with SB_CONFIG_MERGED_HEX_FILES=y." + exit 1 +fi + if [ -z "$LEFT" ] && [ -z "$RIGHT" ]; then nrfjprog --readuicr ./tools/flash/uicr_backup.hex -f $CHIP --snr $SNR --clockspeed $CLOCKSPEED fi -nrfjprog --program ./build/merged_CPUNET.hex --chiperase --verify -f $CHIP --coprocessor CP_NETWORK --snr $SNR --clockspeed $CLOCKSPEED +nrfjprog --program "$NET_HEX" --chiperase --verify -f $CHIP --coprocessor CP_NETWORK --snr $SNR --clockspeed $CLOCKSPEED -nrfjprog --program ./build/merged.hex --chiperase --verify -f $CHIP --coprocessor CP_APPLICATION --snr $SNR --clockspeed $CLOCKSPEED +nrfjprog --program "$APP_HEX" --chiperase --verify -f $CHIP --coprocessor CP_APPLICATION --snr $SNR --clockspeed $CLOCKSPEED if [ -z "$LEFT" ] && [ -z "$RIGHT" ]; then nrfjprog --program ./tools/flash/uicr_backup.hex -f $CHIP --snr $SNR --clockspeed $CLOCKSPEED --verify diff --git a/tools/flash/flash_fota.ps1 b/tools/flash/flash_fota.ps1 index ee11ecfd..80a91a71 100644 --- a/tools/flash/flash_fota.ps1 +++ b/tools/flash/flash_fota.ps1 @@ -1,4 +1,3 @@ -powershell [CmdletBinding()] param( [Parameter(Mandatory = $true)] @@ -71,17 +70,37 @@ if ($Hw) { $hwValue = ("0x{0:X2}{1:X2}{2:X2}00" -f $hwMajor, $hwMinor, $hwPatch) } -# --- Fixed paths relative to CURRENT WORKING DIRECTORY (repo root) --- -$netHex = Join-Path (Get-Location) 'build_fota\merged_CPUNET.hex' -$appHex = Join-Path (Get-Location) 'build_fota\merged.hex' +# --- Paths relative to CURRENT WORKING DIRECTORY (repo root) --- +$buildRoot = Join-Path (Get-Location) 'build_fota' +$newNetHex = Join-Path $buildRoot 'merged_openearable_v2_nrf5340_cpunet.hex' +$newAppHex = Join-Path $buildRoot 'merged_openearable_v2_nrf5340_cpuapp.hex' +$legacyNetHex = Join-Path $buildRoot 'merged_CPUNET.hex' +$legacyAppHex = Join-Path $buildRoot 'merged.hex' + +if ((Test-Path $newNetHex) -and (Test-Path $newAppHex)) { + $netHex = $newNetHex + $appHex = $newAppHex +} +elseif ((Test-Path $legacyNetHex) -and (Test-Path $legacyAppHex)) { + $netHex = $legacyNetHex + $appHex = $legacyAppHex +} +else { + $netHex = $newNetHex + $appHex = $newAppHex +} $uicrBackup = Join-Path (Get-Location) 'tools\flash\uicr_backup.hex' -# --- Require application hex; CPUNET is optional --- +# --- Require both merged sysbuild images --- if (-not (Test-Path $appHex)) { - Write-Error "Missing file: $appHex (run from the repo root where build_fota\merged.hex exists)" + Write-Error "Missing file: $appHex (rebuild FOTA with SB_CONFIG_MERGED_HEX_FILES=y)" + exit 1 +} +if (-not (Test-Path $netHex)) { + Write-Error "Missing file: $netHex (rebuild FOTA with SB_CONFIG_MERGED_HEX_FILES=y)" exit 1 } -$haveNet = Test-Path $netHex +$haveNet = $true Write-Host "nrfjprog starting..." Write-Host " SNR: $Snr" diff --git a/tools/flash/flash_fota.sh b/tools/flash/flash_fota.sh index b811060c..6738f691 100755 --- a/tools/flash/flash_fota.sh +++ b/tools/flash/flash_fota.sh @@ -6,6 +6,7 @@ set -e # Default parameters CLOCKSPEED=8000 CHIP=NRF53 +BUILD_DIR=./build_fota # Function to show usage show_usage() { @@ -83,16 +84,42 @@ if [ "$STANDALONE" == true ] && [ -z "$LEFT" ] && [ -z "$RIGHT" ]; then show_usage fi +NEW_NET_HEX="$BUILD_DIR/merged_openearable_v2_nrf5340_cpunet.hex" +NEW_APP_HEX="$BUILD_DIR/merged_openearable_v2_nrf5340_cpuapp.hex" +LEGACY_NET_HEX="$BUILD_DIR/merged_CPUNET.hex" +LEGACY_APP_HEX="$BUILD_DIR/merged.hex" + +if [ -f "$NEW_NET_HEX" ] && [ -f "$NEW_APP_HEX" ]; then + NET_HEX="$NEW_NET_HEX" + APP_HEX="$NEW_APP_HEX" +elif [ -f "$LEGACY_NET_HEX" ] && [ -f "$LEGACY_APP_HEX" ]; then + NET_HEX="$LEGACY_NET_HEX" + APP_HEX="$LEGACY_APP_HEX" +else + NET_HEX="$NEW_NET_HEX" + APP_HEX="$NEW_APP_HEX" +fi + +if [ ! -f "$NET_HEX" ] || [ ! -f "$APP_HEX" ]; then + echo "Error: Missing merged FOTA image(s)." + echo "Expected: $NET_HEX" + echo "Expected: $APP_HEX" + echo "Legacy fallback: $LEGACY_NET_HEX" + echo "Legacy fallback: $LEGACY_APP_HEX" + echo "Rebuild the FOTA configuration with SB_CONFIG_MERGED_HEX_FILES=y." + exit 1 +fi + # Backup UICR if neither left nor right is specified if [ -z "$LEFT" ] && [ -z "$RIGHT" ]; then nrfjprog --readuicr ./tools/flash/uicr_backup.hex -f $CHIP --snr $SNR --clockspeed $CLOCKSPEED fi # Flash network core (CPUNET) -nrfjprog --program ./build_fota/merged_CPUNET.hex --chiperase --verify -f $CHIP --coprocessor CP_NETWORK --snr $SNR --clockspeed $CLOCKSPEED +nrfjprog --program "$NET_HEX" --chiperase --verify -f $CHIP --coprocessor CP_NETWORK --snr $SNR --clockspeed $CLOCKSPEED # Flash application core (CPUAPP) -nrfjprog --program ./build_fota/merged.hex --chiperase --verify -f $CHIP --coprocessor CP_APPLICATION --snr $SNR --clockspeed $CLOCKSPEED +nrfjprog --program "$APP_HEX" --chiperase --verify -f $CHIP --coprocessor CP_APPLICATION --snr $SNR --clockspeed $CLOCKSPEED # Restore UICR if neither left nor right is specified if [ -z "$LEFT" ] && [ -z "$RIGHT" ]; then diff --git a/unicast_server/README.rst b/unicast_server/README.rst index 4b253de2..bc2d5375 100644 --- a/unicast_server/README.rst +++ b/unicast_server/README.rst @@ -1,13 +1,13 @@ -.. _nrf53_audio_unicast_server_app: +.. _nrf_audio_unicast_server_app: -nRF5340 Audio: Unicast server +nRF Audio: Unicast server ############################# .. contents:: :local: :depth: 2 -The nRF5340 Audio unicast server application implements the :ref:`CIS headset mode `. +The nRF Audio unicast server application implements the :ref:`CIS headset mode `. In this mode, one Connected Isochronous Group (CIG) can be used with two Connected Isochronous Streams (CIS). Receiving unidirectional or transceiving bidirectional audio happens using CIG and CIS. @@ -15,32 +15,33 @@ In addition, Coordinated Set Identification Service (CSIS) is implemented on the The following limitations apply to this application: -* One CIG, one of the two CIS streams (selectable). +* One CIG, one of the two CIS streams or a mixed stereo comprising of the two (selectable). + See :file:`overlay-unicast_server.conf` for more information. * Audio output: I2S/Analog headset output. * Audio input: PDM microphone over I2S. -* Configuration: 16 bit, several bit rates ranging from 32 kbps to 124 kbps. +* Configuration: 16-bit, several bit rates ranging from 32 kbps to 124 kbps. -.. _nrf53_audio_unicast_server_app_requirements: +.. _nrf_audio_unicast_server_app_requirements: Requirements ************ -The application shares the :ref:`requirements common to all nRF5340 Audio application `. +The application shares the :ref:`requirements common to all nRF Audio application `. -.. _nrf53_audio_unicast_server_app_ui: +.. _nrf_audio_unicast_server_app_ui: User interface ************** -Most of the user interface mappings are common across all nRF5340 Audio applications. -See the :ref:`nrf53_audio_app_ui` page for detailed overview. +Most of the user interface mappings are common across all nRF Audio applications. +See the :ref:`nrf_audio_app_ui` page for detailed overview. This application uses specific mapping for the following user interface elements: * Long-pressed on the unicast server device during startup: - * **VOL-** - Changes the headset to the left channel one. - * **VOL+** - Changes the headset to the right channel one. + * **VOL-** - Changes the headset location to left. + * **VOL+** - Changes the headset location to right. * **BTN5** - Clears the previously stored bonding information. * Pressed on the unicast server device during playback: @@ -57,40 +58,52 @@ This application uses specific mapping for the following user interface elements * Solid blue - The device is programmed as the left headset. * Solid magenta - The device is programmed as the right headset. -.. _nrf53_audio_unicast_server_app_configuration: +.. _nrf_audio_unicast_server_app_configuration: Configuration ************* -By default, if you have not made any changes to :file:`.conf` files at :file:`applications/nrf5340_audio/`, the nRF5340 build script tries to build the CIS applications in the CIS unidirectional mode. -To switch to the bidirectional mode, see :ref:`nrf53_audio_app_configuration_select_bidirectional`. +By default, if you have not made any changes to :file:`.conf` files at :file:`applications/nrf_audio/`, the nRF5340 build script tries to build the CIS applications in the CIS unidirectional mode. +To switch to the bidirectional mode, see :ref:`nrf_audio_app_configuration_select_bidirectional`. -For other configuration options, see :ref:`nrf53_audio_app_configuration` and :ref:`nrf53_audio_app_fota`. +For other configuration options, see :ref:`nrf_audio_app_configuration`. For information about how to configure applications in the |NCS|, see :ref:`configure_application`. -.. _nrf53_audio_unicast_server_app_building: +.. _nrf_audio_unicast_server_app_configuration_stereo: + +Stereo configuration +==================== + +The unicast server can receive audio from two CISes and play it on the left and right channels of the audio output. +In this mode, the I2S output is stereo, but :zephyr:board:`nrf5340_audio_dk` still only has one audio output channel, since it has a mono codec (CS47L63). + +To configure stereo, program the :ref:`correct headset location ` for each headset. + +See also :file:`overlay-unicast_server.conf` for more information. + +.. _nrf_audio_unicast_server_app_building: Building and running ******************** -This application can be found under :file:`applications/nrf5340_audio/unicast_server` in the nRF Connect SDK folder structure, but it uses :file:`.conf` files at :file:`applications/nrf5340_audio/`. +This application can be found under :file:`applications/nrf_audio/unicast_server` in the nRF Connect SDK folder structure, but it uses :file:`.conf` files at :file:`applications/nrf_audio/`. The nRF5340 Audio DK comes preprogrammed with basic firmware that indicates if the kit is functional. -See :ref:`nrf53_audio_app_dk_testing_out_of_the_box` for more information. +See :ref:`nrf_audio_app_dk_testing_out_of_the_box` for more information. -To build the application, see :ref:`nrf53_audio_app_building`. +To build the application, see :ref:`nrf_audio_app_building`. -.. _nrf53_audio_unicast_server_app_testing: +.. _nrf_audio_unicast_server_app_testing: Testing ******* -After building and programming the application, you can test the default CIS headset mode using one :ref:`unicast client application ` and one or two unicast server devices (this application). -The recommended approach is to use two other nRF5340 Audio DKs programmed with the :ref:`unicast client application ` for the CIS gateway and the unicast server application (this application) for the CIS headset, respectively, but you can also use an external device that supports the role of unicast server. +After building and programming the application, you can test the default CIS headset mode using one :ref:`unicast client application ` and one or two unicast server devices (this application). +The recommended approach is to use two other nRF5340 Audio DKs programmed with the :ref:`unicast client application ` for the CIS gateway and the unicast server application (this application) for the CIS headset, respectively, but you can also use an external device that supports the role of unicast server. .. note:: - |nrf5340_audio_external_devices_note| + |nrf_audio_external_devices_note| The following testing scenario assumes you are using USB as the audio source on the gateway. This is the default setting. @@ -125,7 +138,7 @@ Complete the following steps to test the unidirectional CIS mode for one gateway After startup, this headset will be configured as the left channel headset. You can also just press the **RESET** button to restore the original programmed settings. -For other testing options, refer to :ref:`nrf53_audio_unicast_server_app_ui`. +For other testing options, refer to :ref:`nrf_audio_unicast_server_app_ui`. After the kits have paired for the first time, they are now bonded. This means the Long-Term Key (LTK) is stored on each side, and that the kits will only connect to each other unless the bonding information is cleared. @@ -133,14 +146,14 @@ To clear the bonding information, press and hold **BTN 5** during boot or reprog When you finish testing, power off the nRF5340 Audio development kits by switching the power switch from On to Off. -.. _nrf53_audio_unicast_server_app_testing_steps_cis_walkie_talkie: +.. _nrf_audio_unicast_server_app_testing_steps_cis_walkie_talkie: Testing the walkie-talkie demo ============================== Testing the walkie-talkie demo is identical to the default testing procedure, except for the following differences: -* You must enable the Kconfig option mentioned in :ref:`nrf53_audio_app_configuration_enable_walkie_talkie` before building the application. +* You must enable the Kconfig option mentioned in :ref:`nrf_audio_app_configuration_enable_walkie_talkie` before building the application. * Instead of controlling the playback, you can speak through the PDM microphones. The line is open all the time, no need to press any buttons to talk, but the volume control works as in the default testing procedure. diff --git a/unicast_server/main.cpp b/unicast_server/main.cpp index 4da7011f..03f31d17 100644 --- a/unicast_server/main.cpp +++ b/unicast_server/main.cpp @@ -85,7 +85,8 @@ int main(void) { streamctrl_start(); uint32_t sirk = uicr_sirk_get(); - + uint8_t bonded_device_count = 0; + bt_mgmt_num_bonds_get(&bonded_device_count); if (sirk == 0xFFFFFFFFU) { state_indicator.set_pairing_state(SET_PAIRING); } else if (bonded_device_count > 0 && !oe_boot_state.timer_reset) { @@ -128,4 +129,4 @@ int main(void) { //*a = 10; return 0; -} \ No newline at end of file +} diff --git a/unicast_server/overlay-unicast_server.conf b/unicast_server/overlay-unicast_server.conf index dac80b13..e4f9f60f 100644 --- a/unicast_server/overlay-unicast_server.conf +++ b/unicast_server/overlay-unicast_server.conf @@ -10,7 +10,6 @@ CONFIG_BT_GATT_AUTO_DISCOVER_CCC=y CONFIG_BT_GATT_AUTO_RESUBSCRIBE=n CONFIG_BT_GATT_AUTO_SEC_REQ=n CONFIG_BT_GATT_AUTO_UPDATE_MTU=y -CONFIG_BT_GATT_CACHING=n CONFIG_BT_GATT_CLIENT=y CONFIG_BT_GATT_DYNAMIC_DB=y @@ -32,18 +31,25 @@ CONFIG_BT_PAC_SRC_NOTIFIABLE=y CONFIG_BT_CSIP_SET_MEMBER=y CONFIG_BT_DEVICE_APPEARANCE=2369 -CONFIG_BT_ISO_MAX_CHAN=2 +CONFIG_BT_ISO_MAX_CHAN=3 CONFIG_BT_ASCS=y -CONFIG_BT_ASCS_MAX_ASE_SNK_COUNT=1 +CONFIG_BT_ASCS_MAX_ASE_SNK_COUNT=2 CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT=1 CONFIG_BT_VCP_VOL_REND=y -CONFIG_BT_MCC=y -CONFIG_BT_MCC_READ_MEDIA_STATE=y -CONFIG_BT_MCC_SET_MEDIA_CONTROL_POINT=y +CONFIG_BT_CONTENT_CTRL_MEDIA=y CONFIG_BT_GAP_PERIPHERAL_PREF_PARAMS=n CONFIG_BT_AUDIO_CODEC_CFG_MAX_METADATA_SIZE=25 CONFIG_LC3_ENC_CHAN_MAX=1 -CONFIG_LC3_DEC_CHAN_MAX=1 +CONFIG_LC3_DEC_CHAN_MAX=2 CONFIG_MBEDTLS_ENABLE_HEAP=y CONFIG_MBEDTLS_HEAP_SIZE=2048 + +# Stereo configs +CONFIG_BT_AUDIO_CONCURRENT_RX_STREAMS_MAX=2 +CONFIG_AUDIO_DECODE_CHANNELS_MAX=2 +CONFIG_AUDIO_SOURCE_I2S=y + +# Always enable bidirectional stream support on the server. +# The client may not support it, but the server should be able to handle it if the client does. +CONFIG_STREAM_BIDIRECTIONAL=y diff --git a/version.cmake b/version.cmake index 50ec31a5..0513b9cb 100644 --- a/version.cmake +++ b/version.cmake @@ -128,10 +128,11 @@ if(GIT_FOUND) set(GIT_SHORT_SHA "${CMAKE_MATCH_5}") set(GIT_DIRTY_SUFFIX "${CMAKE_MATCH_6}") - if(GIT_COMMITS_SINCE_TAG EQUAL 0 AND GIT_DIRTY_SUFFIX STREQUAL "") - set(FIRMWARE_VERSION "${GIT_TAG_VERSION}${PR_SUFFIX}") + if(GIT_TAG_VERSION STREQUAL FIRMWARE_BASE_VERSION AND + GIT_COMMITS_SINCE_TAG EQUAL 0 AND GIT_DIRTY_SUFFIX STREQUAL "") + set(FIRMWARE_VERSION "${FIRMWARE_BASE_VERSION}${PR_SUFFIX}") else() - set(FIRMWARE_VERSION "${GIT_TAG_VERSION}-dev.${GIT_COMMITS_SINCE_TAG}${PR_SUFFIX}+${GIT_SHORT_SHA}") + set(FIRMWARE_VERSION "${FIRMWARE_BASE_VERSION}-dev.${GIT_COMMITS_SINCE_TAG}${PR_SUFFIX}+${GIT_SHORT_SHA}") if(NOT GIT_DIRTY_SUFFIX STREQUAL "") string(APPEND FIRMWARE_VERSION ".dirty") endif() diff --git a/west.yml b/west.yml index aa70c439..3abbfd71 100644 --- a/west.yml +++ b/west.yml @@ -5,5 +5,5 @@ manifest: - name: sdk-nrf path: nrf url: https://github.com/nrfconnect/sdk-nrf.git - revision: v3.0.1 + revision: v3.4.1 import: true