1 /* main.c - Application main entry point */
2
3 /*
4 * Copyright (c) 2019 Andrei Stoica
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <zephyr/bluetooth/gap.h>
10 #include <zephyr/types.h>
11 #include <stddef.h>
12 #include <zephyr/sys/printk.h>
13 #include <zephyr/sys/util.h>
14 #include <zephyr/sys/byteorder.h>
15
16 #include <zephyr/bluetooth/bluetooth.h>
17 #include <zephyr/bluetooth/hci.h>
18 #include <zephyr/bluetooth/hci_vs.h>
19
20 #include <zephyr/bluetooth/conn.h>
21 #include <zephyr/bluetooth/uuid.h>
22 #include <zephyr/bluetooth/gatt.h>
23 #include <zephyr/bluetooth/services/hrs.h>
24
25 BUILD_ASSERT(IS_ENABLED(CONFIG_BT_HAS_HCI_VS),
26 "This app requires Zephyr-specific HCI vendor extensions");
27
28 static struct bt_conn *default_conn;
29 static uint16_t default_conn_handle;
30
31 static const struct bt_data ad[] = {
32 BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
33 BT_DATA_BYTES(BT_DATA_UUID16_ALL, BT_UUID_16_ENCODE(BT_UUID_HRS_VAL)),
34 };
35
36 static const struct bt_data sd[] = {
37 BT_DATA(BT_DATA_NAME_COMPLETE, CONFIG_BT_DEVICE_NAME, sizeof(CONFIG_BT_DEVICE_NAME) - 1),
38 };
39
40 #define DEVICE_NAME CONFIG_BT_DEVICE_NAME
41 #define DEVICE_NAME_LEN (sizeof(DEVICE_NAME) - 1)
42 #define DEVICE_BEACON_TXPOWER_NUM 8
43
44 static struct k_thread pwr_thread_data;
45 static K_THREAD_STACK_DEFINE(pwr_thread_stack, 512);
46
47 static const int8_t txpower[DEVICE_BEACON_TXPOWER_NUM] = {4, 0, -3, -8,
48 -15, -18, -23, -30};
49 static const struct bt_le_adv_param *param = BT_LE_ADV_PARAM(
50 BT_LE_ADV_OPT_CONN, BT_GAP_MS_TO_ADV_INTERVAL(20), BT_GAP_MS_TO_ADV_INTERVAL(20), NULL);
51
read_conn_rssi(uint16_t handle,int8_t * rssi)52 static void read_conn_rssi(uint16_t handle, int8_t *rssi)
53 {
54 struct net_buf *buf, *rsp = NULL;
55 struct bt_hci_cp_read_rssi *cp;
56 struct bt_hci_rp_read_rssi *rp;
57
58 int err;
59
60 buf = bt_hci_cmd_alloc(K_FOREVER);
61 if (!buf) {
62 printk("Unable to allocate command buffer\n");
63 return;
64 }
65
66 cp = net_buf_add(buf, sizeof(*cp));
67 cp->handle = sys_cpu_to_le16(handle);
68
69 err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_RSSI, buf, &rsp);
70 if (err) {
71 printk("Read RSSI err: %d\n", err);
72 return;
73 }
74
75 rp = (void *)rsp->data;
76 *rssi = rp->rssi;
77
78 net_buf_unref(rsp);
79 }
80
81
set_tx_power(uint8_t handle_type,uint16_t handle,int8_t tx_pwr_lvl)82 static void set_tx_power(uint8_t handle_type, uint16_t handle, int8_t tx_pwr_lvl)
83 {
84 struct bt_hci_cp_vs_write_tx_power_level *cp;
85 struct bt_hci_rp_vs_write_tx_power_level *rp;
86 struct net_buf *buf, *rsp = NULL;
87 int err;
88
89 buf = bt_hci_cmd_alloc(K_FOREVER);
90 if (!buf) {
91 printk("Unable to allocate command buffer\n");
92 return;
93 }
94
95 cp = net_buf_add(buf, sizeof(*cp));
96 cp->handle = sys_cpu_to_le16(handle);
97 cp->handle_type = handle_type;
98 cp->tx_power_level = tx_pwr_lvl;
99
100 err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_WRITE_TX_POWER_LEVEL,
101 buf, &rsp);
102 if (err) {
103 printk("Set Tx power err: %d\n", err);
104 return;
105 }
106
107 rp = (void *)rsp->data;
108 printk("Actual Tx Power: %d\n", rp->selected_tx_power);
109
110 net_buf_unref(rsp);
111 }
112
get_tx_power(uint8_t handle_type,uint16_t handle,int8_t * tx_pwr_lvl)113 static void get_tx_power(uint8_t handle_type, uint16_t handle, int8_t *tx_pwr_lvl)
114 {
115 struct bt_hci_cp_vs_read_tx_power_level *cp;
116 struct bt_hci_rp_vs_read_tx_power_level *rp;
117 struct net_buf *buf, *rsp = NULL;
118 int err;
119
120 *tx_pwr_lvl = 0xFF;
121 buf = bt_hci_cmd_alloc(K_FOREVER);
122 if (!buf) {
123 printk("Unable to allocate command buffer\n");
124 return;
125 }
126
127 cp = net_buf_add(buf, sizeof(*cp));
128 cp->handle = sys_cpu_to_le16(handle);
129 cp->handle_type = handle_type;
130
131 err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_TX_POWER_LEVEL,
132 buf, &rsp);
133 if (err) {
134 printk("Read Tx power err: %d\n", err);
135 return;
136 }
137
138 rp = (void *)rsp->data;
139 *tx_pwr_lvl = rp->tx_power_level;
140
141 net_buf_unref(rsp);
142 }
143
connected(struct bt_conn * conn,uint8_t err)144 static void connected(struct bt_conn *conn, uint8_t err)
145 {
146 char addr[BT_ADDR_LE_STR_LEN];
147 int8_t txp;
148 int ret;
149
150 if (err) {
151 printk("Connection failed, err 0x%02x %s\n", err, bt_hci_err_to_str(err));
152 } else {
153 default_conn = bt_conn_ref(conn);
154 ret = bt_hci_get_conn_handle(default_conn,
155 &default_conn_handle);
156 if (ret) {
157 printk("No connection handle (err %d)\n", ret);
158 } else {
159 /* Send first at the default selected power */
160 bt_addr_le_to_str(bt_conn_get_dst(conn),
161 addr, sizeof(addr));
162 printk("Connected via connection (%d) at %s\n",
163 default_conn_handle, addr);
164 get_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_CONN,
165 default_conn_handle, &txp);
166 printk("Connection (%d) - Initial Tx Power = %d\n",
167 default_conn_handle, txp);
168
169 set_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_CONN,
170 default_conn_handle,
171 BT_HCI_VS_LL_TX_POWER_LEVEL_NO_PREF);
172 get_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_CONN,
173 default_conn_handle, &txp);
174 printk("Connection (%d) - Tx Power = %d\n",
175 default_conn_handle, txp);
176 }
177 }
178 }
179
disconnected(struct bt_conn * conn,uint8_t reason)180 static void disconnected(struct bt_conn *conn, uint8_t reason)
181 {
182 printk("Disconnected, reason 0x%02x %s\n", reason, bt_hci_err_to_str(reason));
183
184 if (default_conn) {
185 bt_conn_unref(default_conn);
186 default_conn = NULL;
187 }
188 }
189
190 BT_CONN_CB_DEFINE(conn_callbacks) = {
191 .connected = connected,
192 .disconnected = disconnected,
193 };
194
bt_ready(int err)195 static void bt_ready(int err)
196 {
197 if (err) {
198 printk("Bluetooth init failed (err %d)\n", err);
199 return;
200 }
201
202 printk("Bluetooth initialized\n");
203
204 /* Start advertising */
205 err = bt_le_adv_start(param, ad, ARRAY_SIZE(ad),
206 sd, ARRAY_SIZE(sd));
207 if (err) {
208 printk("Advertising failed to start (err %d)\n", err);
209 return;
210 }
211
212 printk("Dynamic Tx power Beacon started\n");
213 }
214
hrs_notify(void)215 static void hrs_notify(void)
216 {
217 static uint8_t heartrate = 90U;
218
219 /* Heartrate measurements simulation */
220 heartrate++;
221 if (heartrate == 160U) {
222 heartrate = 90U;
223 }
224
225 bt_hrs_notify(heartrate);
226 }
227
modulate_tx_power(void * p1,void * p2,void * p3)228 void modulate_tx_power(void *p1, void *p2, void *p3)
229 {
230 int8_t txp_get = 0;
231 uint8_t idx = 0;
232
233 while (1) {
234 if (!default_conn) {
235 printk("Set Tx power level to %d\n", txpower[idx]);
236 set_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_ADV,
237 0, txpower[idx]);
238
239 k_sleep(K_SECONDS(5));
240
241 printk("Get Tx power level -> ");
242 get_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_ADV,
243 0, &txp_get);
244 printk("TXP = %d\n", txp_get);
245
246 idx = (idx+1) % DEVICE_BEACON_TXPOWER_NUM;
247 } else {
248 int8_t rssi = 0xFF;
249 int8_t txp_adaptive;
250
251 idx = 0;
252
253 read_conn_rssi(default_conn_handle, &rssi);
254 printk("Connected (%d) - RSSI = %d\n",
255 default_conn_handle, rssi);
256 if (rssi > -70) {
257 txp_adaptive = -20;
258 } else if (rssi > -90) {
259 txp_adaptive = -12;
260 } else {
261 txp_adaptive = -4;
262 }
263 printk("Adaptive Tx power selected = %d\n",
264 txp_adaptive);
265 set_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_CONN,
266 default_conn_handle, txp_adaptive);
267 get_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_CONN,
268 default_conn_handle, &txp_get);
269 printk("Connection (%d) TXP = %d\n",
270 default_conn_handle, txp_get);
271
272 k_sleep(K_SECONDS(1));
273 }
274 }
275 }
276
main(void)277 int main(void)
278 {
279 int8_t txp_get = 0xFF;
280 int err;
281
282 default_conn = NULL;
283 printk("Starting Dynamic Tx Power Beacon Demo\n");
284
285 /* Initialize the Bluetooth Subsystem */
286 err = bt_enable(bt_ready);
287 if (err) {
288 printk("Bluetooth init failed (err %d)\n", err);
289 }
290
291 printk("Get Tx power level ->");
292 get_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_ADV, 0, &txp_get);
293 printk("-> default TXP = %d\n", txp_get);
294
295 /* Wait for 5 seconds to give a chance users/testers
296 * to check that default Tx power is indeed the one
297 * selected in Kconfig.
298 */
299 k_sleep(K_SECONDS(5));
300
301 k_thread_create(&pwr_thread_data, pwr_thread_stack,
302 K_THREAD_STACK_SIZEOF(pwr_thread_stack),
303 modulate_tx_power, NULL, NULL, NULL,
304 K_PRIO_COOP(10),
305 0, K_NO_WAIT);
306 k_thread_name_set(&pwr_thread_data, "DYN TX");
307
308 while (1) {
309 hrs_notify();
310 k_sleep(K_SECONDS(2));
311 }
312 return 0;
313 }
314