1 /*
2 * Copyright (c) 2022 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/types.h>
8 #include <stddef.h>
9 #include <errno.h>
10 #include <zephyr/kernel.h>
11 #include <zephyr/sys/printk.h>
12 #include <zephyr/sys/byteorder.h>
13
14 #include <zephyr/bluetooth/bluetooth.h>
15 #include <zephyr/bluetooth/hci.h>
16 #include <zephyr/bluetooth/direction.h>
17 #include <zephyr/bluetooth/conn.h>
18 #include <zephyr/bluetooth/uuid.h>
19 #include <zephyr/bluetooth/gatt.h>
20 #include <zephyr/bluetooth/hci.h>
21 #include <zephyr/bluetooth/hci_vs.h>
22
23 /* Latency set to zero, to enforce PDU exchange every connection event */
24 #define CONN_LATENCY 0U
25 /* Arbitrary selected timeout value */
26 #define CONN_TIMEOUT 400U
27 /* Interval used to run CTE request procedure periodically.
28 * Value is a number of connection events.
29 */
30 #define CTE_REQ_INTERVAL (CONN_LATENCY + 10U)
31 /* Length of CTE in unit of 8 us */
32 #define CTE_LEN (0x14U)
33
34 #define DF_FEAT_ENABLED BIT64(BT_LE_FEAT_BIT_CONN_CTE_RESP)
35
36 static struct bt_conn *default_conn;
37 static const struct bt_le_conn_param conn_params = BT_LE_CONN_PARAM_INIT(
38 BT_GAP_INIT_CONN_INT_MIN, BT_GAP_INIT_CONN_INT_MAX, CONN_LATENCY, CONN_TIMEOUT);
39
40 #if defined(CONFIG_BT_DF_CTE_RX_AOA)
41 static const uint8_t ant_patterns[] = { 0x1, 0x2, 0x3, 0x4, 0x5, 0x6, 0x7, 0x8, 0x9, 0xA };
42 #endif /* CONFIG_BT_DF_CTE_RX_AOA */
43
44 static void start_scan(void);
45
cte_type2str(uint8_t type)46 static const char *cte_type2str(uint8_t type)
47 {
48 switch (type) {
49 case BT_DF_CTE_TYPE_AOA:
50 return "AOA";
51 case BT_DF_CTE_TYPE_AOD_1US:
52 return "AOD 1 [us]";
53 case BT_DF_CTE_TYPE_AOD_2US:
54 return "AOD 2 [us]";
55 default:
56 return "Unknown";
57 }
58 }
59
sample_type2str(enum bt_df_iq_sample type)60 static const char *sample_type2str(enum bt_df_iq_sample type)
61 {
62 switch (type) {
63 case BT_DF_IQ_SAMPLE_8_BITS_INT:
64 return "8 bits int";
65 case BT_DF_IQ_SAMPLE_16_BITS_INT:
66 return "16 bits int";
67 default:
68 return "Unknown";
69 }
70 }
71
packet_status2str(uint8_t status)72 static const char *packet_status2str(uint8_t status)
73 {
74 switch (status) {
75 case BT_DF_CTE_CRC_OK:
76 return "CRC OK";
77 case BT_DF_CTE_CRC_ERR_CTE_BASED_TIME:
78 return "CRC not OK, CTE Info OK";
79 case BT_DF_CTE_CRC_ERR_CTE_BASED_OTHER:
80 return "CRC not OK, Sampled other way";
81 case BT_DF_CTE_INSUFFICIENT_RESOURCES:
82 return "No resources";
83 default:
84 return "Unknown";
85 }
86 }
87
eir_found(struct bt_data * data,void * user_data)88 static bool eir_found(struct bt_data *data, void *user_data)
89 {
90 bt_addr_le_t *addr = user_data;
91 uint64_t u64 = 0U;
92 int err;
93
94 printk("[AD]: %u data_len %u\n", data->type, data->data_len);
95
96 switch (data->type) {
97 case BT_DATA_LE_SUPPORTED_FEATURES:
98 if (data->data_len > sizeof(u64)) {
99 return true;
100 }
101
102 (void)memcpy(&u64, data->data, data->data_len);
103
104 u64 = sys_le64_to_cpu(u64);
105
106 if (!(u64 & DF_FEAT_ENABLED)) {
107 return true;
108 }
109
110 err = bt_le_scan_stop();
111 if (err) {
112 printk("Stop LE scan failed (err %d)\n", err);
113 return true;
114 }
115
116 err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, &conn_params, &default_conn);
117 if (err) {
118 printk("Create conn failed (err %d)\n", err);
119 start_scan();
120 }
121 return false;
122 }
123
124 return true;
125 }
126
device_found(const bt_addr_le_t * addr,int8_t rssi,uint8_t type,struct net_buf_simple * ad)127 static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
128 struct net_buf_simple *ad)
129 {
130 char dev[BT_ADDR_LE_STR_LEN];
131
132 bt_addr_le_to_str(addr, dev, sizeof(dev));
133 printk("[DEVICE]: %s, AD evt type %u, AD data len %u, RSSI %i\n", dev, type, ad->len, rssi);
134
135 /* We're only interested in connectable events */
136 if (type == BT_GAP_ADV_TYPE_ADV_IND || type == BT_GAP_ADV_TYPE_ADV_DIRECT_IND) {
137 bt_data_parse(ad, eir_found, (void *)addr);
138 }
139 }
140
enable_cte_reqest(void)141 static void enable_cte_reqest(void)
142 {
143 int err;
144
145 const struct bt_df_conn_cte_rx_param cte_rx_params = {
146 #if defined(CONFIG_BT_DF_CTE_RX_AOA)
147 .cte_types = BT_DF_CTE_TYPE_ALL,
148 .slot_durations = 0x2,
149 .num_ant_ids = ARRAY_SIZE(ant_patterns),
150 .ant_ids = ant_patterns,
151 #else
152 .cte_types = BT_DF_CTE_TYPE_AOD_1US | BT_DF_CTE_TYPE_AOD_2US,
153 #endif /* CONFIG_BT_DF_CTE_RX_AOA */
154 };
155
156 const struct bt_df_conn_cte_req_params cte_req_params = {
157 .interval = CTE_REQ_INTERVAL,
158 .cte_length = CTE_LEN,
159 #if defined(CONFIG_BT_DF_CTE_RX_AOA)
160 .cte_type = BT_DF_CTE_TYPE_AOA,
161 #else
162 .cte_type = BT_DF_CTE_TYPE_AOD_2US,
163 #endif /* CONFIG_BT_DF_CTE_RX_AOA */
164 };
165
166 printk("Enable receiving of CTE...\n");
167 err = bt_df_conn_cte_rx_enable(default_conn, &cte_rx_params);
168 if (err) {
169 printk("failed (err %d)\n", err);
170 return;
171 }
172 printk("success. CTE receive enabled.\n");
173
174 printk("Request CTE from peer device...\n");
175 err = bt_df_conn_cte_req_enable(default_conn, &cte_req_params);
176 if (err) {
177 printk("failed (err %d)\n", err);
178 return;
179 }
180 printk("success. CTE request enabled.\n");
181 }
182
start_scan(void)183 static void start_scan(void)
184 {
185 int err;
186
187 /* Use active scanning and disable duplicate filtering to handle any
188 * devices that might update their advertising data at runtime.
189 */
190 struct bt_le_scan_param scan_param = {
191 .type = BT_LE_SCAN_TYPE_ACTIVE,
192 .options = BT_LE_SCAN_OPT_NONE,
193 .interval = BT_GAP_SCAN_FAST_INTERVAL,
194 .window = BT_GAP_SCAN_FAST_WINDOW,
195 };
196
197 err = bt_le_scan_start(&scan_param, device_found);
198 if (err) {
199 printk("Scanning failed to start (err %d)\n", err);
200 return;
201 }
202
203 printk("Scanning successfully started\n");
204 }
205
connected(struct bt_conn * conn,uint8_t conn_err)206 static void connected(struct bt_conn *conn, uint8_t conn_err)
207 {
208 char addr[BT_ADDR_LE_STR_LEN];
209
210 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
211
212 if (conn_err) {
213 printk("Failed to connect to %s (%u)\n", addr, conn_err);
214
215 bt_conn_unref(default_conn);
216 default_conn = NULL;
217
218 start_scan();
219 return;
220 }
221
222 printk("Connected: %s\n", addr);
223
224 if (conn == default_conn) {
225 enable_cte_reqest();
226 }
227 }
228
disconnected(struct bt_conn * conn,uint8_t reason)229 static void disconnected(struct bt_conn *conn, uint8_t reason)
230 {
231 char addr[BT_ADDR_LE_STR_LEN];
232
233 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
234
235 printk("Disconnected: %s, reason 0x%02x %s\n", addr, reason, bt_hci_err_to_str(reason));
236
237 if (default_conn != conn) {
238 return;
239 }
240
241 bt_conn_unref(default_conn);
242 default_conn = NULL;
243
244 start_scan();
245 }
246
cte_recv_cb(struct bt_conn * conn,struct bt_df_conn_iq_samples_report const * report)247 static void cte_recv_cb(struct bt_conn *conn, struct bt_df_conn_iq_samples_report const *report)
248 {
249 char addr[BT_ADDR_LE_STR_LEN];
250
251 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
252
253 if (report->err == BT_DF_IQ_REPORT_ERR_SUCCESS) {
254 printk("CTE[%s]: samples type: %s, samples count %d, cte type %s, slot durations: "
255 "%u [us], packet status %s, RSSI %i\n", addr,
256 sample_type2str(report->sample_type), report->sample_count,
257 cte_type2str(report->cte_type), report->slot_durations,
258 packet_status2str(report->packet_status), report->rssi);
259
260 if (IS_ENABLED(CONFIG_DF_CENTRAL_APP_IQ_REPORT_PRINT_IQ_SAMPLES)) {
261 for (uint8_t idx = 0; idx < report->sample_count; idx++) {
262 if (report->sample_type == BT_DF_IQ_SAMPLE_8_BITS_INT) {
263 printk(" IQ[%d]: %d, %d\n", idx, report->sample[idx].i,
264 report->sample[idx].q);
265 } else if (IS_ENABLED(
266 CONFIG_BT_DF_VS_CONN_IQ_REPORT_16_BITS_IQ_SAMPLES)) {
267 printk(" IQ[%" PRIu8 "]: %d, %d\n", idx,
268 report->sample16[idx].i, report->sample16[idx].q);
269 } else {
270 printk("Unhandled vendor specific IQ samples type\n");
271 break;
272 }
273 }
274 }
275 } else {
276 printk("CTE[%s]: request failed, err %u\n", addr, report->err);
277 }
278 }
279
280 BT_CONN_CB_DEFINE(conn_callbacks) = {
281 .connected = connected,
282 .disconnected = disconnected,
283 .cte_report_cb = cte_recv_cb,
284 };
285
main(void)286 int main(void)
287 {
288 int err;
289
290 err = bt_enable(NULL);
291 if (err) {
292 printk("Bluetooth init failed (err %d)\n", err);
293 return 0;
294 }
295
296 printk("Bluetooth initialized\n");
297
298 start_scan();
299 return 0;
300 }
301
