1 /* main.c - Bluetooth Cycling Speed and Cadence app main entry point */
2
3 /*
4 * Copyright (c) 2016 Intel Corporation
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <stdbool.h>
10 #include <zephyr/types.h>
11 #include <stddef.h>
12 #include <string.h>
13 #include <errno.h>
14 #include <zephyr/random/random.h>
15 #include <zephyr/sys/printk.h>
16 #include <zephyr/sys/byteorder.h>
17 #include <zephyr/kernel.h>
18
19 #include <zephyr/bluetooth/bluetooth.h>
20 #include <zephyr/bluetooth/hci.h>
21 #include <zephyr/bluetooth/conn.h>
22 #include <zephyr/bluetooth/uuid.h>
23 #include <zephyr/bluetooth/gatt.h>
24 #include <zephyr/bluetooth/hci.h>
25 #include <zephyr/bluetooth/services/bas.h>
26
27 #define CSC_SUPPORTED_LOCATIONS { CSC_LOC_OTHER, \
28 CSC_LOC_FRONT_WHEEL, \
29 CSC_LOC_REAR_WHEEL, \
30 CSC_LOC_LEFT_CRANK, \
31 CSC_LOC_RIGHT_CRANK }
32 #define CSC_FEATURE (CSC_FEAT_WHEEL_REV | \
33 CSC_FEAT_CRANK_REV | \
34 CSC_FEAT_MULTI_SENSORS)
35
36 /* CSC Sensor Locations */
37 #define CSC_LOC_OTHER 0x00
38 #define CSC_LOC_TOP_OF_SHOE 0x01
39 #define CSC_LOC_IN_SHOE 0x02
40 #define CSC_LOC_HIP 0x03
41 #define CSC_LOC_FRONT_WHEEL 0x04
42 #define CSC_LOC_LEFT_CRANK 0x05
43 #define CSC_LOC_RIGHT_CRANK 0x06
44 #define CSC_LOC_LEFT_PEDAL 0x07
45 #define CSC_LOC_RIGHT_PEDAL 0x08
46 #define CSC_LOC_FRONT_HUB 0x09
47 #define CSC_LOC_REAR_DROPOUT 0x0a
48 #define CSC_LOC_CHAINSTAY 0x0b
49 #define CSC_LOC_REAR_WHEEL 0x0c
50 #define CSC_LOC_REAR_HUB 0x0d
51 #define CSC_LOC_CHEST 0x0e
52
53 /* CSC Application error codes */
54 #define CSC_ERR_IN_PROGRESS 0x80
55 #define CSC_ERR_CCC_CONFIG 0x81
56
57 /* SC Control Point Opcodes */
58 #define SC_CP_OP_SET_CWR 0x01
59 #define SC_CP_OP_CALIBRATION 0x02
60 #define SC_CP_OP_UPDATE_LOC 0x03
61 #define SC_CP_OP_REQ_SUPP_LOC 0x04
62 #define SC_CP_OP_RESPONSE 0x10
63
64 /* SC Control Point Response Values */
65 #define SC_CP_RSP_SUCCESS 0x01
66 #define SC_CP_RSP_OP_NOT_SUPP 0x02
67 #define SC_CP_RSP_INVAL_PARAM 0x03
68 #define SC_CP_RSP_FAILED 0x04
69
70 /* CSC Feature */
71 #define CSC_FEAT_WHEEL_REV BIT(0)
72 #define CSC_FEAT_CRANK_REV BIT(1)
73 #define CSC_FEAT_MULTI_SENSORS BIT(2)
74
75 /* CSC Measurement Flags */
76 #define CSC_WHEEL_REV_DATA_PRESENT BIT(0)
77 #define CSC_CRANK_REV_DATA_PRESENT BIT(1)
78
79 /* Cycling Speed and Cadence Service declaration */
80
81 static uint32_t c_wheel_revs; /* Cumulative Wheel Revolutions */
82 static uint8_t supported_locations[] = CSC_SUPPORTED_LOCATIONS;
83 static uint8_t sensor_location; /* Current Sensor Location */
84 static bool csc_simulate;
85 static bool ctrl_point_configured;
86
csc_meas_ccc_cfg_changed(const struct bt_gatt_attr * attr,uint16_t value)87 static void csc_meas_ccc_cfg_changed(const struct bt_gatt_attr *attr,
88 uint16_t value)
89 {
90 csc_simulate = value == BT_GATT_CCC_NOTIFY;
91 }
92
ctrl_point_ccc_cfg_changed(const struct bt_gatt_attr * attr,uint16_t value)93 static void ctrl_point_ccc_cfg_changed(const struct bt_gatt_attr *attr,
94 uint16_t value)
95 {
96 ctrl_point_configured = value == BT_GATT_CCC_INDICATE;
97 }
98
read_location(struct bt_conn * conn,const struct bt_gatt_attr * attr,void * buf,uint16_t len,uint16_t offset)99 static ssize_t read_location(struct bt_conn *conn,
100 const struct bt_gatt_attr *attr, void *buf,
101 uint16_t len, uint16_t offset)
102 {
103 uint8_t *value = attr->user_data;
104
105 return bt_gatt_attr_read(conn, attr, buf, len, offset, value,
106 sizeof(*value));
107 }
108
read_csc_feature(struct bt_conn * conn,const struct bt_gatt_attr * attr,void * buf,uint16_t len,uint16_t offset)109 static ssize_t read_csc_feature(struct bt_conn *conn,
110 const struct bt_gatt_attr *attr, void *buf,
111 uint16_t len, uint16_t offset)
112 {
113 uint16_t csc_feature = CSC_FEATURE;
114
115 return bt_gatt_attr_read(conn, attr, buf, len, offset,
116 &csc_feature, sizeof(csc_feature));
117 }
118
119 static void ctrl_point_ind(struct bt_conn *conn, uint8_t req_op, uint8_t status,
120 const void *data, uint16_t data_len);
121
122 struct write_sc_ctrl_point_req {
123 uint8_t op;
124 union {
125 uint32_t cwr;
126 uint8_t location;
127 };
128 } __packed;
129
write_ctrl_point(struct bt_conn * conn,const struct bt_gatt_attr * attr,const void * buf,uint16_t len,uint16_t offset,uint8_t flags)130 static ssize_t write_ctrl_point(struct bt_conn *conn,
131 const struct bt_gatt_attr *attr,
132 const void *buf, uint16_t len, uint16_t offset,
133 uint8_t flags)
134 {
135 const struct write_sc_ctrl_point_req *req = buf;
136 uint8_t status;
137 int i;
138
139 if (!ctrl_point_configured) {
140 return BT_GATT_ERR(CSC_ERR_CCC_CONFIG);
141 }
142
143 if (!len) {
144 return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN);
145 }
146
147 switch (req->op) {
148 case SC_CP_OP_SET_CWR:
149 if (len != sizeof(req->op) + sizeof(req->cwr)) {
150 status = SC_CP_RSP_INVAL_PARAM;
151 break;
152 }
153
154 c_wheel_revs = sys_le32_to_cpu(req->cwr);
155 status = SC_CP_RSP_SUCCESS;
156 break;
157 case SC_CP_OP_UPDATE_LOC:
158 if (len != sizeof(req->op) + sizeof(req->location)) {
159 status = SC_CP_RSP_INVAL_PARAM;
160 break;
161 }
162
163 /* Break if the requested location is the same as current one */
164 if (req->location == sensor_location) {
165 status = SC_CP_RSP_SUCCESS;
166 break;
167 }
168
169 /* Pre-set status */
170 status = SC_CP_RSP_INVAL_PARAM;
171
172 /* Check if requested location is supported */
173 for (i = 0; i < ARRAY_SIZE(supported_locations); i++) {
174 if (supported_locations[i] == req->location) {
175 sensor_location = req->location;
176 status = SC_CP_RSP_SUCCESS;
177 break;
178 }
179 }
180
181 break;
182 case SC_CP_OP_REQ_SUPP_LOC:
183 if (len != sizeof(req->op)) {
184 status = SC_CP_RSP_INVAL_PARAM;
185 break;
186 }
187
188 /* Indicate supported locations and return */
189 ctrl_point_ind(conn, req->op, SC_CP_RSP_SUCCESS,
190 &supported_locations,
191 sizeof(supported_locations));
192
193 return len;
194 default:
195 status = SC_CP_RSP_OP_NOT_SUPP;
196 }
197
198 ctrl_point_ind(conn, req->op, status, NULL, 0);
199
200 return len;
201 }
202
203 BT_GATT_SERVICE_DEFINE(csc_svc,
204 BT_GATT_PRIMARY_SERVICE(BT_UUID_CSC),
205 BT_GATT_CHARACTERISTIC(BT_UUID_CSC_MEASUREMENT, BT_GATT_CHRC_NOTIFY,
206 0x00, NULL, NULL, NULL),
207 BT_GATT_CCC(csc_meas_ccc_cfg_changed,
208 BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
209 BT_GATT_CHARACTERISTIC(BT_UUID_SENSOR_LOCATION, BT_GATT_CHRC_READ,
210 BT_GATT_PERM_READ, read_location, NULL,
211 &sensor_location),
212 BT_GATT_CHARACTERISTIC(BT_UUID_CSC_FEATURE, BT_GATT_CHRC_READ,
213 BT_GATT_PERM_READ, read_csc_feature, NULL, NULL),
214 BT_GATT_CHARACTERISTIC(BT_UUID_SC_CONTROL_POINT,
215 BT_GATT_CHRC_WRITE | BT_GATT_CHRC_INDICATE,
216 BT_GATT_PERM_WRITE, NULL, write_ctrl_point,
217 &sensor_location),
218 BT_GATT_CCC(ctrl_point_ccc_cfg_changed,
219 BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
220 );
221
222 struct sc_ctrl_point_ind {
223 uint8_t op;
224 uint8_t req_op;
225 uint8_t status;
226 uint8_t data[];
227 } __packed;
228
ctrl_point_ind(struct bt_conn * conn,uint8_t req_op,uint8_t status,const void * data,uint16_t data_len)229 static void ctrl_point_ind(struct bt_conn *conn, uint8_t req_op, uint8_t status,
230 const void *data, uint16_t data_len)
231 {
232 struct sc_ctrl_point_ind *ind;
233 uint8_t buf[sizeof(*ind) + data_len];
234
235 ind = (void *) buf;
236 ind->op = SC_CP_OP_RESPONSE;
237 ind->req_op = req_op;
238 ind->status = status;
239
240 /* Send data (supported locations) if present */
241 if (data && data_len) {
242 memcpy(ind->data, data, data_len);
243 }
244
245 bt_gatt_notify(conn, &csc_svc.attrs[8], buf, sizeof(buf));
246 }
247
248 struct csc_measurement_nfy {
249 uint8_t flags;
250 uint8_t data[];
251 } __packed;
252
253 struct wheel_rev_data_nfy {
254 uint32_t cwr;
255 uint16_t lwet;
256 } __packed;
257
258 struct crank_rev_data_nfy {
259 uint16_t ccr;
260 uint16_t lcet;
261 } __packed;
262
measurement_nfy(struct bt_conn * conn,uint32_t cwr,uint16_t lwet,uint16_t ccr,uint16_t lcet)263 static void measurement_nfy(struct bt_conn *conn, uint32_t cwr, uint16_t lwet,
264 uint16_t ccr, uint16_t lcet)
265 {
266 struct csc_measurement_nfy *nfy;
267 uint8_t buf[sizeof(*nfy) +
268 (cwr ? sizeof(struct wheel_rev_data_nfy) : 0) +
269 (ccr ? sizeof(struct crank_rev_data_nfy) : 0)];
270 uint16_t len = 0U;
271
272 nfy = (void *) buf;
273 nfy->flags = 0U;
274
275 /* Send Wheel Revolution data is present */
276 if (cwr) {
277 struct wheel_rev_data_nfy data;
278
279 nfy->flags |= CSC_WHEEL_REV_DATA_PRESENT;
280 data.cwr = sys_cpu_to_le32(cwr);
281 data.lwet = sys_cpu_to_le16(lwet);
282
283 memcpy(nfy->data, &data, sizeof(data));
284 len += sizeof(data);
285 }
286
287 /* Send Crank Revolution data is present */
288 if (ccr) {
289 struct crank_rev_data_nfy data;
290
291 nfy->flags |= CSC_CRANK_REV_DATA_PRESENT;
292 data.ccr = sys_cpu_to_le16(ccr);
293 data.lcet = sys_cpu_to_le16(lcet);
294
295 memcpy(nfy->data + len, &data, sizeof(data));
296 }
297
298 bt_gatt_notify(NULL, &csc_svc.attrs[1], buf, sizeof(buf));
299 }
300
301 static uint16_t lwet; /* Last Wheel Event Time */
302 static uint16_t ccr; /* Cumulative Crank Revolutions */
303 static uint16_t lcet; /* Last Crank Event Time */
304
csc_simulation(void)305 static void csc_simulation(void)
306 {
307 static uint8_t i;
308 uint8_t rnd = sys_rand8_get();
309 bool nfy_crank = false, nfy_wheel = false;
310
311 /* Measurements don't have to be updated every second */
312 if (!(i % 2)) {
313 lwet += 1050 + rnd % 50;
314 c_wheel_revs += 2U;
315 nfy_wheel = true;
316 }
317
318 if (!(i % 3)) {
319 lcet += 1000 + rnd % 50;
320 ccr += 1U;
321 nfy_crank = true;
322 }
323
324 /*
325 * In typical applications, the CSC Measurement characteristic is
326 * notified approximately once per second. This interval may vary
327 * and is determined by the Server and not required to be configurable
328 * by the Client.
329 */
330 measurement_nfy(NULL, nfy_wheel ? c_wheel_revs : 0, nfy_wheel ? lwet : 0,
331 nfy_crank ? ccr : 0, nfy_crank ? lcet : 0);
332
333 /*
334 * The Last Crank Event Time value and Last Wheel Event Time roll over
335 * every 64 seconds.
336 */
337 if (!(i % 64)) {
338 lcet = 0U;
339 lwet = 0U;
340 i = 0U;
341 }
342
343 i++;
344 }
345
connected(struct bt_conn * conn,uint8_t err)346 static void connected(struct bt_conn *conn, uint8_t err)
347 {
348 if (err) {
349 printk("Connection failed, err 0x%02x %s\n", err, bt_hci_err_to_str(err));
350 } else {
351 printk("Connected\n");
352 }
353 }
354
disconnected(struct bt_conn * conn,uint8_t reason)355 static void disconnected(struct bt_conn *conn, uint8_t reason)
356 {
357 printk("Disconnected, reason 0x%02x %s\n", reason, bt_hci_err_to_str(reason));
358 }
359
360 BT_CONN_CB_DEFINE(conn_callbacks) = {
361 .connected = connected,
362 .disconnected = disconnected,
363 };
364
365 static const struct bt_data ad[] = {
366 BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
367 BT_DATA_BYTES(BT_DATA_UUID16_ALL,
368 BT_UUID_16_ENCODE(BT_UUID_CSC_VAL),
369 BT_UUID_16_ENCODE(BT_UUID_BAS_VAL))
370 };
371
372 static const struct bt_data sd[] = {
373 BT_DATA(BT_DATA_NAME_COMPLETE, CONFIG_BT_DEVICE_NAME, sizeof(CONFIG_BT_DEVICE_NAME) - 1),
374 };
375
bt_ready(void)376 static void bt_ready(void)
377 {
378 int err;
379
380 printk("Bluetooth initialized\n");
381
382 err = bt_le_adv_start(BT_LE_ADV_CONN_FAST_1, ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd));
383 if (err) {
384 printk("Advertising failed to start (err %d)\n", err);
385 return;
386 }
387
388 printk("Advertising successfully started\n");
389 }
390
bas_notify(void)391 static void bas_notify(void)
392 {
393 uint8_t battery_level = bt_bas_get_battery_level();
394
395 battery_level--;
396
397 if (!battery_level) {
398 battery_level = 100U;
399 }
400
401 bt_bas_set_battery_level(battery_level);
402 }
403
main(void)404 int main(void)
405 {
406 int err;
407
408 err = bt_enable(NULL);
409 if (err) {
410 printk("Bluetooth init failed (err %d)\n", err);
411 return 0;
412 }
413
414 bt_ready();
415
416 while (1) {
417 k_sleep(K_SECONDS(1));
418
419 /* CSC simulation */
420 if (csc_simulate) {
421 csc_simulation();
422 }
423
424 /* Battery level simulation */
425 bas_notify();
426 }
427 return 0;
428 }
429
