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