[nrf fromtree] bluetooth: CS: add bt_le_cs_get_antenna_path()

added function to calculate antenna path
based on the antenna permutation index and
total number of antenna paths

Signed-off-by: Ivan Iushkov <ivan.iushkov@nordicsemi.no>
(cherry picked from commit ded9c1f44d4a537a77c0e46b77d99b1899977977)

Author: ivaniushkov

include/zephyr/bluetooth/cs.h

@@ -878,6 +878,27 @@ int bt_le_cs_write_cached_remote_supported_capabilities(
  */
 int bt_le_cs_write_cached_remote_fae_table(struct bt_conn *conn, int8_t remote_fae_table[72]);
 
+/** @brief Get antenna path used for the CS tone exchange
+ *	   when using multiple antenna paths for mode-2 or mode-3
+ *	   CS procedure.
+ *
+ *	   The function implements antenna path permutation defined in
+ *	   Bluetooth Core Specification 6.0, Vol. 6, Part H, Section 4.7.5.
+ *
+ * @note To use this API @kconfig{CONFIG_BT_CHANNEL_SOUNDING} must be set.
+ *
+ * @param n_ap				 The number of antenna paths, range: [1, 4].
+ * @param antenna_path_permutation_index Antenna Path Permutation Index.
+ * @param tone_index			 Index of the tone in the CS step, range [0, n_ap].
+ *					 tone_index = n_ap corresponds to extension slot.
+ *
+ * @return Antenna path used to exchange CS tones, range: [0, 3].
+ * @return -EINVAL if arguments are invalid.
+ */
+int bt_le_cs_get_antenna_path(uint8_t n_ap,
+			      uint8_t antenna_path_permutation_index,
+			      uint8_t tone_index);
+
 #ifdef __cplusplus
 }
 #endif

subsys/bluetooth/host/cs.c

@@ -22,6 +22,11 @@ LOG_MODULE_REGISTER(bt_cs);
 static struct bt_le_cs_test_cb cs_test_callbacks;
 #endif
 
+#define A1 (0)
+#define A2 (1)
+#define A3 (2)
+#define A4 (3)
+
 struct reassembly_buf_meta_data {
 	uint16_t conn_handle;
 };
@@ -1344,4 +1349,99 @@ void bt_le_cs_step_data_parse(struct net_buf_simple *step_data_buf,
 	}
 }
 
+/* Bluetooth Core Specification 6.0, Table 4.13, Antenna Path Permutation for N_AP=2.
+ * The last element corresponds to extension slot
+ */
+static const uint8_t antenna_path_lut_n_ap_2[2][3] = {
+	{A1, A2, A2},
+	{A2, A1, A1},
+};
+
+/* Bluetooth Core Specification 6.0, Table 4.14, Antenna Path Permutation for N_AP=3.
+ * The last element corresponds to extension slot
+ */
+static const uint8_t antenna_path_lut_n_ap_3[6][4] = {
+	{A1, A2, A3, A3},
+	{A2, A1, A3, A3},
+	{A1, A3, A2, A2},
+	{A3, A1, A2, A2},
+	{A3, A2, A1, A1},
+	{A2, A3, A1, A1},
+};
+
+/* Bluetooth Core Specification 6.0, Table 4.15, Antenna Path Permutation for N_AP=4.
+ * The last element corresponds to extension slot
+ */
+static const uint8_t antenna_path_lut_n_ap_4[24][5] = {
+	{A1, A2, A3, A4, A4},
+	{A2, A1, A3, A4, A4},
+	{A1, A3, A2, A4, A4},
+	{A3, A1, A2, A4, A4},
+	{A3, A2, A1, A4, A4},
+	{A2, A3, A1, A4, A4},
+	{A1, A2, A4, A3, A3},
+	{A2, A1, A4, A3, A3},
+	{A1, A4, A2, A3, A3},
+	{A4, A1, A2, A3, A3},
+	{A4, A2, A1, A3, A3},
+	{A2, A4, A1, A3, A3},
+	{A1, A4, A3, A2, A2},
+	{A4, A1, A3, A2, A2},
+	{A1, A3, A4, A2, A2},
+	{A3, A1, A4, A2, A2},
+	{A3, A4, A1, A2, A2},
+	{A4, A3, A1, A2, A2},
+	{A4, A2, A3, A1, A1},
+	{A2, A4, A3, A1, A1},
+	{A4, A3, A2, A1, A1},
+	{A3, A4, A2, A1, A1},
+	{A3, A2, A4, A1, A1},
+	{A2, A3, A4, A1, A1},
+};
+
+int bt_le_cs_get_antenna_path(uint8_t n_ap,
+			      uint8_t antenna_path_permutation_index,
+			      uint8_t tone_index)
+{
+	switch (n_ap) {
+	case 1:
+	{
+		uint8_t antenna_path_permutations = 1;
+		uint8_t num_tones = n_ap + 1; /* one additional tone extension slot */
+
+		if (antenna_path_permutation_index >= antenna_path_permutations ||
+		    tone_index >= num_tones) {
+			return -EINVAL;
+		}
+		return A1;
+	}
+	case 2:
+	{
+		if (antenna_path_permutation_index >= ARRAY_SIZE(antenna_path_lut_n_ap_2) ||
+		    tone_index >= ARRAY_SIZE(antenna_path_lut_n_ap_2[0])) {
+			return -EINVAL;
+		}
+		return antenna_path_lut_n_ap_2[antenna_path_permutation_index][tone_index];
+	}
+	case 3:
+	{
+		if (antenna_path_permutation_index >= ARRAY_SIZE(antenna_path_lut_n_ap_3) ||
+		    tone_index >= ARRAY_SIZE(antenna_path_lut_n_ap_3[0])) {
+			return -EINVAL;
+		}
+		return antenna_path_lut_n_ap_3[antenna_path_permutation_index][tone_index];
+	}
+	case 4:
+	{
+		if (antenna_path_permutation_index >= ARRAY_SIZE(antenna_path_lut_n_ap_4) ||
+		    tone_index >= ARRAY_SIZE(antenna_path_lut_n_ap_4[0])) {
+			return -EINVAL;
+		}
+		return antenna_path_lut_n_ap_4[antenna_path_permutation_index][tone_index];
+	}
+	default:
+		return -EINVAL;
+	}
+}
+
 #endif /* CONFIG_BT_CHANNEL_SOUNDING */