From 584556d7972b3027bad25ec4ecf7d30c8735debb Mon Sep 17 00:00:00 2001 From: Olivier Lesage Date: Tue, 22 Oct 2024 13:44:35 +0200 Subject: [PATCH] [nrf fromtree] bluetooth: host: Add helper function for parsing PCTs The 12-bit signed values for the results of PBR are a bit cumbersome, so this adds a helper function to make it easier to work with the HCI formatted steps. Signed-off-by: Olivier Lesage (cherry picked from commit 7e72d46e2e79e780307246374e1a0ab7e8aae0a4) Signed-off-by: Sean Madigan --- include/zephyr/bluetooth/cs.h | 18 +++ include/zephyr/bluetooth/hci_types.h | 3 +- subsys/bluetooth/host/cs.c | 18 +++ .../host/cs/bt_le_cs_parse_pct/CMakeLists.txt | 25 +++++ .../host/cs/bt_le_cs_parse_pct/prj.conf | 10 ++ .../host/cs/bt_le_cs_parse_pct/src/main.c | 106 ++++++++++++++++++ .../host/cs/bt_le_cs_parse_pct/testcase.yaml | 7 ++ 7 files changed, 186 insertions(+), 1 deletion(-) create mode 100644 tests/bluetooth/host/cs/bt_le_cs_parse_pct/CMakeLists.txt create mode 100644 tests/bluetooth/host/cs/bt_le_cs_parse_pct/prj.conf create mode 100644 tests/bluetooth/host/cs/bt_le_cs_parse_pct/src/main.c create mode 100644 tests/bluetooth/host/cs/bt_le_cs_parse_pct/testcase.yaml diff --git a/include/zephyr/bluetooth/cs.h b/include/zephyr/bluetooth/cs.h index d5d34039d27..e7043f66762 100644 --- a/include/zephyr/bluetooth/cs.h +++ b/include/zephyr/bluetooth/cs.h @@ -551,6 +551,24 @@ struct bt_le_cs_subevent_step { const uint8_t *data; }; +/** Sign-extended IQ value extracted from step data. */ +struct bt_le_cs_iq_sample { + int16_t i; + int16_t q; +}; + +/** @brief Extract in-phase and quadrature terms from HCI-formatted PCT. + * + * Convenience function for processing 24-bit phase correction terms found + * in CS step data. The 12-bit signed real and imaginary components are + * converted to host endianness and sign-extended. + * + * @param pct 24-bit little-endian phase correction term. + * + * @return struct bt_le_cs_iq_sample containing real and imaginary terms as int16_t + */ +struct bt_le_cs_iq_sample bt_le_cs_parse_pct(const uint8_t pct[3]); + /** @brief Set all valid channel map bits * * This command is used to enable all valid channels in a diff --git a/include/zephyr/bluetooth/hci_types.h b/include/zephyr/bluetooth/hci_types.h index 1b22e172fcc..719accabfc6 100644 --- a/include/zephyr/bluetooth/hci_types.h +++ b/include/zephyr/bluetooth/hci_types.h @@ -3607,7 +3607,8 @@ struct bt_hci_evt_le_cs_config_complete { #define BT_HCI_LE_CS_REF_POWER_LEVEL_UNAVAILABLE 0x7F -#define BT_HCI_LE_CS_PCT_MASK 0xFFF +#define BT_HCI_LE_CS_PCT_I_MASK 0x000FFF +#define BT_HCI_LE_CS_PCT_Q_MASK 0xFFF000 #define BT_HCI_LE_CS_TONE_QUALITY_HIGH 0x0 #define BT_HCI_LE_CS_TONE_QUALITY_MED 0x1 diff --git a/subsys/bluetooth/host/cs.c b/subsys/bluetooth/host/cs.c index aa1e1beadf3..c11f262b5d2 100644 --- a/subsys/bluetooth/host/cs.c +++ b/subsys/bluetooth/host/cs.c @@ -240,6 +240,24 @@ static void reset_reassembly_results(void) memset(&reassembled_result, 0, sizeof(struct bt_conn_le_cs_subevent_result)); } +/** @brief Converts PCT to a pair of int16_t + * + */ +struct bt_le_cs_iq_sample bt_le_cs_parse_pct(const uint8_t pct[3]) +{ + uint32_t pct_u32 = sys_get_le24(pct); + + /* Extract I and Q. */ + uint16_t i_u16 = pct_u32 & BT_HCI_LE_CS_PCT_I_MASK; + uint16_t q_u16 = (pct_u32 & BT_HCI_LE_CS_PCT_Q_MASK) >> 12; + + /* Convert from 12-bit 2's complement to int16_t */ + int16_t i = (i_u16 ^ BIT(11)) - BIT(11); + int16_t q = (q_u16 ^ BIT(11)) - BIT(11); + + return (struct bt_le_cs_iq_sample){.i = i, .q = q}; +} + void bt_le_cs_set_valid_chmap_bits(uint8_t channel_map[10]) { memset(channel_map, 0xFF, 10); diff --git a/tests/bluetooth/host/cs/bt_le_cs_parse_pct/CMakeLists.txt b/tests/bluetooth/host/cs/bt_le_cs_parse_pct/CMakeLists.txt new file mode 100644 index 00000000000..089406a316f --- /dev/null +++ b/tests/bluetooth/host/cs/bt_le_cs_parse_pct/CMakeLists.txt @@ -0,0 +1,25 @@ +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) + +find_package(Zephyr COMPONENTS unittest REQUIRED HINTS $ENV{ZEPHYR_BASE}) + +project(bt_step_data_parse) + +include_directories(BEFORE + ${ZEPHYR_BASE}/tests/bluetooth/host/cs/mocks +) + +add_subdirectory(${ZEPHYR_BASE}/tests/bluetooth/host host_mocks) +add_subdirectory(${ZEPHYR_BASE}/tests/bluetooth/host/cs/mocks mocks) + +target_link_libraries(testbinary PRIVATE mocks host_mocks) + +target_sources(testbinary + PRIVATE + src/main.c + + ${ZEPHYR_BASE}/subsys/bluetooth/host/cs.c + ${ZEPHYR_BASE}/lib/net_buf/buf_simple.c + ${ZEPHYR_BASE}/subsys/logging/log_minimal.c +) diff --git a/tests/bluetooth/host/cs/bt_le_cs_parse_pct/prj.conf b/tests/bluetooth/host/cs/bt_le_cs_parse_pct/prj.conf new file mode 100644 index 00000000000..8ff995b6f04 --- /dev/null +++ b/tests/bluetooth/host/cs/bt_le_cs_parse_pct/prj.conf @@ -0,0 +1,10 @@ +CONFIG_ZTEST=y +CONFIG_BT=y +CONFIG_BT_HCI=y +CONFIG_BT_CENTRAL=y +CONFIG_BT_CHANNEL_SOUNDING=y +CONFIG_ASSERT=y +CONFIG_ASSERT_LEVEL=2 +CONFIG_ASSERT_VERBOSE=y +CONFIG_ASSERT_ON_ERRORS=y +CONFIG_NET_BUF=y diff --git a/tests/bluetooth/host/cs/bt_le_cs_parse_pct/src/main.c b/tests/bluetooth/host/cs/bt_le_cs_parse_pct/src/main.c new file mode 100644 index 00000000000..70f07748f53 --- /dev/null +++ b/tests/bluetooth/host/cs/bt_le_cs_parse_pct/src/main.c @@ -0,0 +1,106 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include + +DEFINE_FFF_GLOBALS; + +ZTEST_SUITE(bt_le_cs_parse_pct, NULL, NULL, NULL, NULL, NULL); + +/* + * Test success case + * + * Constraints: + * - Valid PCT is passed in + * + * Expected behaviour: + * - IQ term matches expected values + */ +ZTEST(bt_le_cs_parse_pct, test_parsing_success) +{ + struct bt_le_cs_iq_sample iq; + + struct { + uint8_t input[3]; + struct bt_le_cs_iq_sample output; + } test_vector[] = { + /* Edge cases */ + {.input = {0x00, 0x00, 0x00}, .output = {.i = 0, .q = 0}}, + {.input = {0xFF, 0xFF, 0xFF}, .output = {.i = -1, .q = -1}}, + {.input = {0xFF, 0x00, 0xFF}, .output = {.i = 255, .q = -16}}, + {.input = {0xFF, 0x00, 0x00}, .output = {.i = 255, .q = 0}}, + {.input = {0x00, 0xFF, 0x00}, .output = {.i = -256, .q = 15}}, + {.input = {0x00, 0x00, 0xFF}, .output = {.i = 0, .q = -16}}, + {.input = {0x00, 0x08, 0x80}, .output = {.i = -2048, .q = -2048}}, + {.input = {0xFF, 0xF7, 0x7F}, .output = {.i = 2047, .q = 2047}}, + + /* Randomly generated using python */ + {.input = {0xEF, 0xCD, 0xAB}, .output = {.i = -529, .q = -1348}}, + {.input = {0x30, 0x75, 0x44}, .output = {.i = 1328, .q = 1095}}, + {.input = {0x46, 0x5D, 0xEB}, .output = {.i = -698, .q = -331}}, + {.input = {0xE8, 0x14, 0x45}, .output = {.i = 1256, .q = 1105}}, + {.input = {0x23, 0xCA, 0x5C}, .output = {.i = -1501, .q = 1484}}, + {.input = {0x68, 0xA0, 0x15}, .output = {.i = 104, .q = 346}}, + {.input = {0x39, 0x73, 0x1B}, .output = {.i = 825, .q = 439}}, + {.input = {0x23, 0x72, 0x3D}, .output = {.i = 547, .q = 983}}, + {.input = {0xF5, 0xF8, 0x3D}, .output = {.i = -1803, .q = 991}}, + {.input = {0xF7, 0xB4, 0xB9}, .output = {.i = 1271, .q = -1125}}, + {.input = {0x61, 0x9F, 0xD5}, .output = {.i = -159, .q = -679}}, + {.input = {0x9B, 0x21, 0xC6}, .output = {.i = 411, .q = -926}}, + {.input = {0x14, 0x86, 0x0F}, .output = {.i = 1556, .q = 248}}, + {.input = {0x8E, 0xBB, 0xC6}, .output = {.i = -1138, .q = -917}}, + {.input = {0x5B, 0xD1, 0xC2}, .output = {.i = 347, .q = -979}}, + {.input = {0x99, 0x4A, 0x28}, .output = {.i = -1383, .q = 644}}, + {.input = {0x32, 0x16, 0x2B}, .output = {.i = 1586, .q = 689}}, + {.input = {0x3E, 0x8C, 0xD4}, .output = {.i = -962, .q = -696}}, + {.input = {0x2B, 0x1F, 0x95}, .output = {.i = -213, .q = -1711}}, + {.input = {0x22, 0xE6, 0xD6}, .output = {.i = 1570, .q = -658}}, + {.input = {0x0B, 0x31, 0xD6}, .output = {.i = 267, .q = -669}}, + {.input = {0x1B, 0x98, 0x9D}, .output = {.i = -2021, .q = -1575}}, + {.input = {0x8E, 0x97, 0x63}, .output = {.i = 1934, .q = 1593}}, + {.input = {0x97, 0x91, 0x8D}, .output = {.i = 407, .q = -1831}}, + {.input = {0x67, 0xF7, 0x1F}, .output = {.i = 1895, .q = 511}}, + {.input = {0xD6, 0x5C, 0x23}, .output = {.i = -810, .q = 565}}, + {.input = {0x92, 0xD3, 0x0B}, .output = {.i = 914, .q = 189}}, + {.input = {0xE8, 0xF3, 0x23}, .output = {.i = 1000, .q = 575}}, + {.input = {0xE6, 0xE3, 0xAD}, .output = {.i = 998, .q = -1314}}, + {.input = {0x6E, 0x70, 0xA9}, .output = {.i = 110, .q = -1385}}, + {.input = {0x63, 0x65, 0x28}, .output = {.i = 1379, .q = 646}}, + {.input = {0x27, 0x0F, 0x32}, .output = {.i = -217, .q = 800}}, + {.input = {0x3F, 0x8C, 0xE1}, .output = {.i = -961, .q = -488}}, + {.input = {0x4E, 0x86, 0xAA}, .output = {.i = 1614, .q = -1368}}, + {.input = {0x9E, 0xD1, 0xF6}, .output = {.i = 414, .q = -147}}, + {.input = {0x86, 0x09, 0x56}, .output = {.i = -1658, .q = 1376}}, + {.input = {0xFF, 0x09, 0x41}, .output = {.i = -1537, .q = 1040}}, + {.input = {0x89, 0xC5, 0x1F}, .output = {.i = 1417, .q = 508}}, + {.input = {0x1A, 0xE2, 0x9A}, .output = {.i = 538, .q = -1618}}, + {.input = {0x7E, 0x03, 0xB8}, .output = {.i = 894, .q = -1152}}, + {.input = {0x5E, 0x28, 0xB3}, .output = {.i = -1954, .q = -1230}}, + {.input = {0xFF, 0x50, 0xF0}, .output = {.i = 255, .q = -251}}, + {.input = {0xB0, 0x07, 0x87}, .output = {.i = 1968, .q = -1936}}, + {.input = {0x7E, 0xD7, 0x0C}, .output = {.i = 1918, .q = 205}}, + {.input = {0x26, 0xA2, 0xC9}, .output = {.i = 550, .q = -870}}, + {.input = {0x97, 0x71, 0x72}, .output = {.i = 407, .q = 1831}}, + {.input = {0x73, 0x0E, 0xC1}, .output = {.i = -397, .q = -1008}}, + {.input = {0xAC, 0x20, 0x6B}, .output = {.i = 172, .q = 1714}}, + {.input = {0x85, 0x7D, 0xB4}, .output = {.i = -635, .q = -1209}}, + {.input = {0xCC, 0xE3, 0x1B}, .output = {.i = 972, .q = 446}}, + {.input = {0x88, 0x48, 0x65}, .output = {.i = -1912, .q = 1620}}, + }; + + for (uint16_t k = 0; k < ARRAY_SIZE(test_vector); k++) { + iq = bt_le_cs_parse_pct(test_vector[k].input); + + zassert_equal(iq.i, test_vector[k].output.i, + "Failed for k = %u, expected %d, not %d", k, test_vector[k].output.i, + iq.i); + zassert_equal(iq.q, test_vector[k].output.q, + "Failed for k = %u, expected %d, not %d", k, test_vector[k].output.q, + iq.q); + } +} diff --git a/tests/bluetooth/host/cs/bt_le_cs_parse_pct/testcase.yaml b/tests/bluetooth/host/cs/bt_le_cs_parse_pct/testcase.yaml new file mode 100644 index 00000000000..deab8ebd294 --- /dev/null +++ b/tests/bluetooth/host/cs/bt_le_cs_parse_pct/testcase.yaml @@ -0,0 +1,7 @@ +common: + tags: + - bluetooth + - host +tests: + bluetooth.host.cs.bt_le_cs_parse_pct: + type: unit