1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one
3 * or more contributor license agreements. See the NOTICE file
4 * distributed with this work for additional information
5 * regarding copyright ownership. The ASF licenses this file
6 * to you under the Apache License, Version 2.0 (the
7 * "License"); you may not use this file except in compliance
8 * with the License. You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing,
13 * software distributed under the License is distributed on an
14 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
15 * KIND, either express or implied. See the License for the
16 * specific language governing permissions and limitations
17 * under the License.
18 */
19
20 #include <stdlib.h>
21 #include <string.h>
22 #include <errno.h>
23 #include "os/os.h"
24 #include "nimble/ble.h"
25 #include "host/ble_uuid.h"
26 #include "ble_hs_priv.h"
27
28 /**
29 * ATT server - Attribute Protocol
30 *
31 * Notes on buffer reuse:
32 * Most request handlers reuse the request buffer for the reponse. This is
33 * done to prevent out-of-memory conditions. However, there are two handlers
34 * which do not reuse the request buffer:
35 * 1. Write request.
36 * 2. Indicate request.
37 *
38 * Both of these handlers attempt to allocate a new buffer for the response
39 * prior to processing the request. If allocation fails, the request is not
40 * processed, and the request buffer is reused for the transmission of an
41 * "insufficient resources" ATT error response. These handlers don't reuse the
42 * request mbuf for an affirmative response because the buffer contains the
43 * attribute data that gets passed to the application callback. The
44 * application may choose to retain the mbuf during the callback, so the stack
45 */
46
47 STAILQ_HEAD(ble_att_svr_entry_list, ble_att_svr_entry);
48 static struct ble_att_svr_entry_list ble_att_svr_list;
49 static struct ble_att_svr_entry_list ble_att_svr_hidden_list;
50
51 static uint16_t ble_att_svr_id;
52
53 static void *ble_att_svr_entry_mem;
54 static struct os_mempool ble_att_svr_entry_pool;
55
56 static os_membuf_t ble_att_svr_prep_entry_mem[
57 OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES),
58 sizeof (struct ble_att_prep_entry))
59 ];
60
61 static struct os_mempool ble_att_svr_prep_entry_pool;
62
63 static struct ble_att_svr_entry *
ble_att_svr_entry_alloc(void)64 ble_att_svr_entry_alloc(void)
65 {
66 struct ble_att_svr_entry *entry;
67
68 entry = os_memblock_get(&ble_att_svr_entry_pool);
69 if (entry != NULL) {
70 memset(entry, 0, sizeof *entry);
71 }
72
73 return entry;
74 }
75
76 static void
ble_att_svr_entry_free(struct ble_att_svr_entry * entry)77 ble_att_svr_entry_free(struct ble_att_svr_entry *entry)
78 {
79 os_memblock_put(&ble_att_svr_entry_pool, entry);
80 }
81
82 /**
83 * Allocate the next handle id and return it.
84 *
85 * @return A new 16-bit handle ID.
86 */
87 static uint16_t
ble_att_svr_next_id(void)88 ble_att_svr_next_id(void)
89 {
90 /* Rollover is fatal. */
91 BLE_HS_DBG_ASSERT(ble_att_svr_id != UINT16_MAX);
92 return ++ble_att_svr_id;
93 }
94
95 /**
96 * Register a host attribute with the BLE stack.
97 *
98 * @param ha A filled out ble_att structure to register
99 * @param handle_id A pointer to a 16-bit handle ID, which will be
100 * the handle that is allocated.
101 * @param fn The callback function that gets executed when
102 * the attribute is operated on.
103 *
104 * @return 0 on success, non-zero error code on failure.
105 */
106 int
ble_att_svr_register(const ble_uuid_t * uuid,uint8_t flags,uint8_t min_key_size,uint16_t * handle_id,ble_att_svr_access_fn * cb,void * cb_arg)107 ble_att_svr_register(const ble_uuid_t *uuid, uint8_t flags,
108 uint8_t min_key_size, uint16_t *handle_id,
109 ble_att_svr_access_fn *cb, void *cb_arg)
110 {
111 struct ble_att_svr_entry *entry;
112
113 entry = ble_att_svr_entry_alloc();
114 if (entry == NULL) {
115 return BLE_HS_ENOMEM;
116 }
117
118 entry->ha_uuid = uuid;
119 entry->ha_flags = flags;
120 entry->ha_min_key_size = min_key_size;
121 entry->ha_handle_id = ble_att_svr_next_id();
122 entry->ha_cb = cb;
123 entry->ha_cb_arg = cb_arg;
124
125 STAILQ_INSERT_TAIL(&ble_att_svr_list, entry, ha_next);
126
127 if (handle_id != NULL) {
128 *handle_id = entry->ha_handle_id;
129 }
130
131 return 0;
132 }
133
134 uint16_t
ble_att_svr_prev_handle(void)135 ble_att_svr_prev_handle(void)
136 {
137 return ble_att_svr_id;
138 }
139
140 /**
141 * Find a host attribute by handle id.
142 *
143 * @param handle_id The handle_id to search for
144 * @param ha_ptr On input: Indicates the starting point of the
145 * walk; null means start at the beginning of
146 * the list, non-null means start at the
147 * following entry.
148 * On output: Indicates the last ble_att element
149 * processed, or NULL if the entire list has
150 * been processed.
151 *
152 * @return 0 on success; BLE_HS_ENOENT on not found.
153 */
154 struct ble_att_svr_entry *
ble_att_svr_find_by_handle(uint16_t handle_id)155 ble_att_svr_find_by_handle(uint16_t handle_id)
156 {
157 struct ble_att_svr_entry *entry;
158
159 for (entry = STAILQ_FIRST(&ble_att_svr_list);
160 entry != NULL;
161 entry = STAILQ_NEXT(entry, ha_next)) {
162
163 if (entry->ha_handle_id == handle_id) {
164 return entry;
165 }
166 }
167
168 return NULL;
169 }
170
171 /**
172 * Find a host attribute by UUID.
173 *
174 * @param uuid The ble_uuid_t to search for
175 * @param prev On input: Indicates the starting point of the
176 * walk; null means start at the beginning of
177 * the list, non-null means start at the
178 * following entry.
179 * On output: Indicates the last ble_att element
180 * processed, or NULL if the entire list has
181 * been processed.
182 *
183 * @return 0 on success; BLE_HS_ENOENT on not found.
184 */
185 struct ble_att_svr_entry *
ble_att_svr_find_by_uuid(struct ble_att_svr_entry * prev,const ble_uuid_t * uuid,uint16_t end_handle)186 ble_att_svr_find_by_uuid(struct ble_att_svr_entry *prev, const ble_uuid_t *uuid,
187 uint16_t end_handle)
188 {
189 struct ble_att_svr_entry *entry;
190
191 if (prev == NULL) {
192 entry = STAILQ_FIRST(&ble_att_svr_list);
193 } else {
194 entry = STAILQ_NEXT(prev, ha_next);
195 }
196
197 for (;
198 entry != NULL && entry->ha_handle_id <= end_handle;
199 entry = STAILQ_NEXT(entry, ha_next)) {
200
201 if (ble_uuid_cmp(entry->ha_uuid, uuid) == 0) {
202 return entry;
203 }
204 }
205
206 return NULL;
207 }
208
209 static int
ble_att_svr_pullup_req_base(struct os_mbuf ** om,int base_len,uint8_t * out_att_err)210 ble_att_svr_pullup_req_base(struct os_mbuf **om, int base_len,
211 uint8_t *out_att_err)
212 {
213 uint8_t att_err;
214 int rc;
215
216 rc = ble_hs_mbuf_pullup_base(om, base_len);
217 if (rc == BLE_HS_ENOMEM) {
218 att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
219 } else {
220 att_err = 0;
221 }
222
223 if (out_att_err != NULL) {
224 *out_att_err = att_err;
225 }
226
227 return rc;
228 }
229
230 static void
ble_att_svr_get_sec_state(uint16_t conn_handle,struct ble_gap_sec_state * out_sec_state)231 ble_att_svr_get_sec_state(uint16_t conn_handle,
232 struct ble_gap_sec_state *out_sec_state)
233 {
234 struct ble_hs_conn *conn;
235
236 ble_hs_lock();
237
238 conn = ble_hs_conn_find_assert(conn_handle);
239 *out_sec_state = conn->bhc_sec_state;
240
241 ble_hs_unlock();
242 }
243
244 static int
ble_att_svr_check_perms(uint16_t conn_handle,int is_read,struct ble_att_svr_entry * entry,uint8_t * out_att_err)245 ble_att_svr_check_perms(uint16_t conn_handle, int is_read,
246 struct ble_att_svr_entry *entry,
247 uint8_t *out_att_err)
248 {
249 struct ble_gap_sec_state sec_state;
250 struct ble_store_value_sec value_sec;
251 struct ble_store_key_sec key_sec;
252 struct ble_hs_conn_addrs addrs;
253 struct ble_hs_conn *conn;
254 int author;
255 int authen;
256 int enc;
257 int rc;
258
259 if (is_read) {
260 if (!(entry->ha_flags & BLE_ATT_F_READ)) {
261 *out_att_err = BLE_ATT_ERR_READ_NOT_PERMITTED;
262 return BLE_HS_EREJECT;
263 }
264
265 enc = entry->ha_flags & BLE_ATT_F_READ_ENC;
266 authen = entry->ha_flags & BLE_ATT_F_READ_AUTHEN;
267 author = entry->ha_flags & BLE_ATT_F_READ_AUTHOR;
268 } else {
269 if (!(entry->ha_flags & BLE_ATT_F_WRITE)) {
270 *out_att_err = BLE_ATT_ERR_WRITE_NOT_PERMITTED;
271 return BLE_HS_EREJECT;
272 }
273
274 enc = entry->ha_flags & BLE_ATT_F_WRITE_ENC;
275 authen = entry->ha_flags & BLE_ATT_F_WRITE_AUTHEN;
276 author = entry->ha_flags & BLE_ATT_F_WRITE_AUTHOR;
277 }
278
279 /* Bail early if this operation doesn't require security. */
280 if (!enc && !authen && !author) {
281 return 0;
282 }
283
284 ble_att_svr_get_sec_state(conn_handle, &sec_state);
285 if ((enc || authen) && !sec_state.encrypted) {
286 ble_hs_lock();
287 conn = ble_hs_conn_find(conn_handle);
288 if (conn != NULL) {
289 ble_hs_conn_addrs(conn, &addrs);
290
291 memset(&key_sec, 0, sizeof key_sec);
292 key_sec.peer_addr = addrs.peer_id_addr;
293 }
294 ble_hs_unlock();
295
296 rc = ble_store_read_peer_sec(&key_sec, &value_sec);
297 if (rc == 0 && value_sec.ltk_present) {
298 *out_att_err = BLE_ATT_ERR_INSUFFICIENT_ENC;
299 } else {
300 *out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHEN;
301 }
302
303 return BLE_HS_ATT_ERR(*out_att_err);
304 }
305
306 if (authen && !sec_state.authenticated) {
307 *out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHEN;
308 return BLE_HS_ATT_ERR(*out_att_err);
309 }
310
311 if (entry->ha_min_key_size > sec_state.key_size) {
312 *out_att_err = BLE_ATT_ERR_INSUFFICIENT_KEY_SZ;
313 return BLE_HS_ATT_ERR(*out_att_err);
314 }
315
316 if (author) {
317 /* XXX: Prompt user for authorization. */
318 }
319
320 return 0;
321 }
322
323 /**
324 * Calculates the number of ticks until a queued write times out on the
325 * specified ATT server. If this server is not in the process of receiving a
326 * queued write, then BLE_HS_FOREVER is returned. If a timeout just occurred,
327 * 0 is returned.
328 *
329 * @param svr The ATT server to check.
330 * @param now The current OS time.
331 *
332 * @return The number of ticks until the current queued
333 * write times out.
334 */
335 int32_t
ble_att_svr_ticks_until_tmo(const struct ble_att_svr_conn * svr,ble_npl_time_t now)336 ble_att_svr_ticks_until_tmo(const struct ble_att_svr_conn *svr, ble_npl_time_t now)
337 {
338 #if BLE_HS_ATT_SVR_QUEUED_WRITE_TMO == 0
339 return BLE_HS_FOREVER;
340 #endif
341
342 int32_t time_diff;
343
344 if (SLIST_EMPTY(&svr->basc_prep_list)) {
345 return BLE_HS_FOREVER;
346 }
347
348 time_diff = svr->basc_prep_timeout_at - now;
349 if (time_diff < 0) {
350 return 0;
351 }
352
353 return time_diff;
354 }
355
356 /**
357 * Allocates an mbuf to be used for an ATT response. If an mbuf cannot be
358 * allocated, the received request mbuf is reused for the error response.
359 */
360 static int
ble_att_svr_pkt(struct os_mbuf ** rxom,struct os_mbuf ** out_txom,uint8_t * out_att_err)361 ble_att_svr_pkt(struct os_mbuf **rxom, struct os_mbuf **out_txom,
362 uint8_t *out_att_err)
363 {
364 *out_txom = ble_hs_mbuf_l2cap_pkt();
365 if (*out_txom != NULL) {
366 return 0;
367 }
368
369 /* Allocation failure. Reuse receive buffer for response. */
370 *out_txom = *rxom;
371 *rxom = NULL;
372 *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
373 return BLE_HS_ENOMEM;
374 }
375
376 static int
ble_att_svr_read(uint16_t conn_handle,struct ble_att_svr_entry * entry,uint16_t offset,struct os_mbuf * om,uint8_t * out_att_err)377 ble_att_svr_read(uint16_t conn_handle,
378 struct ble_att_svr_entry *entry,
379 uint16_t offset,
380 struct os_mbuf *om,
381 uint8_t *out_att_err)
382 {
383 uint8_t att_err;
384 int rc;
385
386 att_err = 0; /* Silence gcc warning. */
387
388 if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
389 rc = ble_att_svr_check_perms(conn_handle, 1, entry, &att_err);
390 if (rc != 0) {
391 goto err;
392 }
393 }
394
395 BLE_HS_DBG_ASSERT(entry->ha_cb != NULL);
396 rc = entry->ha_cb(conn_handle, entry->ha_handle_id,
397 BLE_ATT_ACCESS_OP_READ, offset, &om, entry->ha_cb_arg);
398 if (rc != 0) {
399 att_err = rc;
400 rc = BLE_HS_EAPP;
401 goto err;
402 }
403
404 return 0;
405
406 err:
407 if (out_att_err != NULL) {
408 *out_att_err = att_err;
409 }
410 return rc;
411 }
412
413 static int
ble_att_svr_read_flat(uint16_t conn_handle,struct ble_att_svr_entry * entry,uint16_t offset,uint16_t max_len,void * dst,uint16_t * out_len,uint8_t * out_att_err)414 ble_att_svr_read_flat(uint16_t conn_handle,
415 struct ble_att_svr_entry *entry,
416 uint16_t offset,
417 uint16_t max_len,
418 void *dst,
419 uint16_t *out_len,
420 uint8_t *out_att_err)
421 {
422 struct os_mbuf *om;
423 uint16_t len;
424 int rc;
425
426 om = ble_hs_mbuf_l2cap_pkt();
427 if (om == NULL) {
428 rc = BLE_HS_ENOMEM;
429 goto done;
430 }
431
432 rc = ble_att_svr_read(conn_handle, entry, offset, om, out_att_err);
433 if (rc != 0) {
434 goto done;
435 }
436
437 len = OS_MBUF_PKTLEN(om);
438 if (len > max_len) {
439 rc = BLE_HS_EMSGSIZE;
440 *out_att_err = BLE_ATT_ERR_UNLIKELY;
441 goto done;
442 }
443
444 rc = os_mbuf_copydata(om, 0, len, dst);
445 BLE_HS_DBG_ASSERT(rc == 0);
446
447 *out_len = len;
448 rc = 0;
449
450 done:
451 os_mbuf_free_chain(om);
452 return rc;
453 }
454
455 int
ble_att_svr_read_handle(uint16_t conn_handle,uint16_t attr_handle,uint16_t offset,struct os_mbuf * om,uint8_t * out_att_err)456 ble_att_svr_read_handle(uint16_t conn_handle, uint16_t attr_handle,
457 uint16_t offset, struct os_mbuf *om,
458 uint8_t *out_att_err)
459 {
460 struct ble_att_svr_entry *entry;
461 int rc;
462
463 entry = ble_att_svr_find_by_handle(attr_handle);
464 if (entry == NULL) {
465 if (out_att_err != NULL) {
466 *out_att_err = BLE_ATT_ERR_INVALID_HANDLE;
467 }
468 return BLE_HS_ENOENT;
469 }
470
471 rc = ble_att_svr_read(conn_handle, entry, offset, om, out_att_err);
472 if (rc != 0) {
473 return rc;
474 }
475
476 return 0;
477 }
478
479 int
ble_att_svr_read_local(uint16_t attr_handle,struct os_mbuf ** out_om)480 ble_att_svr_read_local(uint16_t attr_handle, struct os_mbuf **out_om)
481 {
482 struct os_mbuf *om;
483 int rc;
484
485 om = ble_hs_mbuf_bare_pkt();
486 if (om == NULL) {
487 rc = BLE_HS_ENOMEM;
488 goto err;
489 }
490
491 rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0, om,
492 NULL);
493 if (rc != 0) {
494 goto err;
495 }
496
497 *out_om = om;
498 return 0;
499
500 err:
501 os_mbuf_free_chain(om);
502 return rc;
503 }
504
505 static int
ble_att_svr_write(uint16_t conn_handle,struct ble_att_svr_entry * entry,uint16_t offset,struct os_mbuf ** om,uint8_t * out_att_err)506 ble_att_svr_write(uint16_t conn_handle, struct ble_att_svr_entry *entry,
507 uint16_t offset, struct os_mbuf **om, uint8_t *out_att_err)
508 {
509 uint8_t att_err = 0;
510 int rc;
511
512 BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
513
514 if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
515 rc = ble_att_svr_check_perms(conn_handle, 0, entry, &att_err);
516 if (rc != 0) {
517 goto done;
518 }
519 }
520
521 BLE_HS_DBG_ASSERT(entry->ha_cb != NULL);
522 rc = entry->ha_cb(conn_handle, entry->ha_handle_id,
523 BLE_ATT_ACCESS_OP_WRITE, offset, om, entry->ha_cb_arg);
524 if (rc != 0) {
525 att_err = rc;
526 rc = BLE_HS_EAPP;
527 goto done;
528 }
529
530 done:
531 if (out_att_err != NULL) {
532 *out_att_err = att_err;
533 }
534 return rc;
535 }
536
537 static int
ble_att_svr_write_handle(uint16_t conn_handle,uint16_t attr_handle,uint16_t offset,struct os_mbuf ** om,uint8_t * out_att_err)538 ble_att_svr_write_handle(uint16_t conn_handle, uint16_t attr_handle,
539 uint16_t offset, struct os_mbuf **om,
540 uint8_t *out_att_err)
541 {
542 struct ble_att_svr_entry *entry;
543 int rc;
544
545 entry = ble_att_svr_find_by_handle(attr_handle);
546 if (entry == NULL) {
547 if (out_att_err != NULL) {
548 *out_att_err = BLE_ATT_ERR_INVALID_HANDLE;
549 }
550 return BLE_HS_ENOENT;
551 }
552
553 rc = ble_att_svr_write(conn_handle, entry, offset, om, out_att_err);
554 if (rc != 0) {
555 return rc;
556 }
557
558 return 0;
559 }
560
561 int
ble_att_svr_tx_error_rsp(uint16_t conn_handle,struct os_mbuf * txom,uint8_t req_op,uint16_t handle,uint8_t error_code)562 ble_att_svr_tx_error_rsp(uint16_t conn_handle, struct os_mbuf *txom,
563 uint8_t req_op, uint16_t handle, uint8_t error_code)
564 {
565 struct ble_att_error_rsp *rsp;
566
567 BLE_HS_DBG_ASSERT(error_code != 0);
568 BLE_HS_DBG_ASSERT(OS_MBUF_PKTLEN(txom) == 0);
569
570 rsp = ble_att_cmd_prepare(BLE_ATT_OP_ERROR_RSP, sizeof(*rsp), txom);
571 if (rsp == NULL) {
572 return BLE_HS_ENOMEM;
573 }
574
575 rsp->baep_req_op = req_op;
576 rsp->baep_handle = htole16(handle);
577 rsp->baep_error_code = error_code;
578
579 BLE_ATT_LOG_CMD(1, "error rsp", conn_handle, ble_att_error_rsp_log, rsp);
580
581 return ble_att_tx(conn_handle, txom);
582 }
583
584 /**
585 * Transmits a response or error message over the specified connection.
586 *
587 * The specified rc and err_status values control what gets sent as follows:
588 * o If rc == 0: tx an affirmative response.
589 * o Else if err_status != 0: tx an error response.
590 * o Else: tx nothing.
591 *
592 * In addition, if transmission of an affirmative response fails, an error is
593 * sent instead.
594 *
595 * @param conn_handle The handle of the connection to send over.
596 * @param hs_status The status indicating whether to transmit an
597 * affirmative response or an error.
598 * @param txom Contains the affirmative response payload.
599 * @param att_op If an error is transmitted, this is the value
600 * of the error message's op field.
601 * @param err_status If an error is transmitted, this is the value
602 * of the error message's status field.
603 * @param err_handle If an error is transmitted, this is the value
604 * of the error message's attribute handle
605 * field.
606 */
607 static int
ble_att_svr_tx_rsp(uint16_t conn_handle,int hs_status,struct os_mbuf * om,uint8_t att_op,uint8_t err_status,uint16_t err_handle)608 ble_att_svr_tx_rsp(uint16_t conn_handle, int hs_status, struct os_mbuf *om,
609 uint8_t att_op, uint8_t err_status, uint16_t err_handle)
610 {
611 struct ble_l2cap_chan *chan;
612 struct ble_hs_conn *conn;
613 int do_tx;
614 int rc;
615
616 if (hs_status != 0 && err_status == 0) {
617 /* Processing failed, but err_status of 0 means don't send error. */
618 do_tx = 0;
619 } else {
620 do_tx = 1;
621 }
622
623 if (do_tx) {
624 ble_hs_lock();
625
626 rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
627 if (rc != 0) {
628 /* No longer connected. */
629 hs_status = rc;
630 } else {
631 if (hs_status == 0) {
632 BLE_HS_DBG_ASSERT(om != NULL);
633
634 ble_att_inc_tx_stat(om->om_data[0]);
635 ble_att_truncate_to_mtu(chan, om);
636 hs_status = ble_l2cap_tx(conn, chan, om);
637 om = NULL;
638 if (hs_status != 0) {
639 err_status = BLE_ATT_ERR_UNLIKELY;
640 }
641 }
642 }
643
644 ble_hs_unlock();
645
646 if (hs_status != 0) {
647 STATS_INC(ble_att_stats, error_rsp_tx);
648
649 /* Reuse om for error response. */
650 if (om == NULL) {
651 om = ble_hs_mbuf_l2cap_pkt();
652 } else {
653 os_mbuf_adj(om, OS_MBUF_PKTLEN(om));
654 }
655 if (om != NULL) {
656 ble_att_svr_tx_error_rsp(conn_handle, om, att_op,
657 err_handle, err_status);
658 om = NULL;
659 }
660 }
661 }
662
663 /* Free mbuf if it was not consumed (i.e., if the send failed). */
664 os_mbuf_free_chain(om);
665
666 return hs_status;
667 }
668
669 static int
ble_att_svr_build_mtu_rsp(uint16_t conn_handle,struct os_mbuf ** rxom,struct os_mbuf ** out_txom,uint8_t * att_err)670 ble_att_svr_build_mtu_rsp(uint16_t conn_handle, struct os_mbuf **rxom,
671 struct os_mbuf **out_txom, uint8_t *att_err)
672 {
673 struct ble_att_mtu_cmd *cmd;
674 struct ble_l2cap_chan *chan;
675 struct os_mbuf *txom;
676 uint16_t mtu;
677 int rc;
678
679 *att_err = 0; /* Silence unnecessary warning. */
680 txom = NULL;
681
682 ble_hs_lock();
683 rc = ble_att_conn_chan_find(conn_handle, NULL, &chan);
684 if (rc == 0) {
685 mtu = chan->my_mtu;
686 }
687 ble_hs_unlock();
688
689 if (rc != 0) {
690 goto done;
691 }
692
693 /* Just reuse the request buffer for the response. */
694 txom = *rxom;
695 *rxom = NULL;
696 os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
697
698 cmd = ble_att_cmd_prepare(BLE_ATT_OP_MTU_RSP, sizeof(*cmd), txom);
699 if (cmd == NULL) {
700 *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
701 rc = BLE_HS_ENOMEM;
702 goto done;
703 }
704
705 cmd->bamc_mtu = htole16(mtu);
706
707 BLE_ATT_LOG_CMD(1, "mtu rsp", conn_handle, ble_att_mtu_cmd_log, cmd);
708
709 rc = 0;
710
711 done:
712 *out_txom = txom;
713 return rc;
714 }
715
716 int
ble_att_svr_rx_mtu(uint16_t conn_handle,struct os_mbuf ** rxom)717 ble_att_svr_rx_mtu(uint16_t conn_handle, struct os_mbuf **rxom)
718 {
719 struct ble_att_mtu_cmd *cmd;
720 struct ble_l2cap_chan *chan;
721 struct ble_hs_conn *conn;
722 struct os_mbuf *txom;
723 uint16_t mtu;
724 uint8_t att_err;
725 int rc;
726
727 txom = NULL;
728 mtu = 0;
729
730 rc = ble_att_svr_pullup_req_base(rxom, sizeof(*cmd), &att_err);
731 if (rc != 0) {
732 goto done;
733 }
734
735 cmd = (struct ble_att_mtu_cmd *)(*rxom)->om_data;
736 BLE_ATT_LOG_CMD(0, "mtu req", conn_handle, ble_att_mtu_cmd_log, cmd);
737
738 mtu = le16toh(cmd->bamc_mtu);
739
740 rc = ble_att_svr_build_mtu_rsp(conn_handle, rxom, &txom, &att_err);
741 if (rc != 0) {
742 goto done;
743 }
744
745 rc = 0;
746
747 done:
748 rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_MTU_REQ,
749 att_err, 0);
750 if (rc == 0) {
751 ble_hs_lock();
752
753 rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
754 if (rc == 0) {
755 ble_att_set_peer_mtu(chan, mtu);
756 chan->flags |= BLE_L2CAP_CHAN_F_TXED_MTU;
757 mtu = ble_att_chan_mtu(chan);
758 }
759
760 ble_hs_unlock();
761
762 if (rc == 0) {
763 ble_gap_mtu_event(conn_handle, BLE_L2CAP_CID_ATT, mtu);
764 }
765 }
766 return rc;
767 }
768
769 /**
770 * Fills the supplied mbuf with the variable length Information Data field of a
771 * Find Information ATT response.
772 *
773 * @param req The Find Information request being responded
774 * to.
775 * @param om The destination mbuf where the Information
776 * Data field gets written.
777 * @param mtu The ATT L2CAP channel MTU.
778 * @param format On success, the format field of the response
779 * gets stored here. One of:
780 * o BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT
781 * o BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT
782 *
783 * @return 0 on success; nonzero on failure.
784 */
785 static int
ble_att_svr_fill_info(uint16_t start_handle,uint16_t end_handle,struct os_mbuf * om,uint16_t mtu,uint8_t * format)786 ble_att_svr_fill_info(uint16_t start_handle, uint16_t end_handle,
787 struct os_mbuf *om, uint16_t mtu, uint8_t *format)
788 {
789 struct ble_att_svr_entry *ha;
790 uint8_t *buf;
791 int num_entries;
792 int entry_sz;
793 int rc;
794
795 *format = 0;
796 num_entries = 0;
797 rc = 0;
798
799 STAILQ_FOREACH(ha, &ble_att_svr_list, ha_next) {
800 if (ha->ha_handle_id > end_handle) {
801 rc = 0;
802 goto done;
803 }
804 if (ha->ha_handle_id >= start_handle) {
805 if (ha->ha_uuid->type == BLE_UUID_TYPE_16) {
806 if (*format == 0) {
807 *format = BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT;
808 } else if (*format != BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT) {
809 rc = 0;
810 goto done;
811 }
812
813 entry_sz = 4;
814 } else {
815 if (*format == 0) {
816 *format = BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT;
817 } else if (*format != BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT) {
818 rc = 0;
819 goto done;
820 }
821 entry_sz = 18;
822 }
823
824 if (OS_MBUF_PKTLEN(om) + entry_sz > mtu) {
825 rc = 0;
826 goto done;
827 }
828
829 buf = os_mbuf_extend(om, entry_sz);
830 if (buf == NULL) {
831 rc = BLE_HS_ENOMEM;
832 goto done;
833 }
834
835 put_le16(buf + 0, ha->ha_handle_id);
836
837 ble_uuid_flat(ha->ha_uuid, buf + 2);
838
839 num_entries++;
840 }
841 }
842
843 done:
844 if (rc == 0 && num_entries == 0) {
845 return BLE_HS_ENOENT;
846 } else {
847 return rc;
848 }
849 }
850
851 static int
ble_att_svr_build_find_info_rsp(uint16_t conn_handle,uint16_t start_handle,uint16_t end_handle,struct os_mbuf ** rxom,struct os_mbuf ** out_txom,uint8_t * att_err)852 ble_att_svr_build_find_info_rsp(uint16_t conn_handle,
853 uint16_t start_handle, uint16_t end_handle,
854 struct os_mbuf **rxom,
855 struct os_mbuf **out_txom,
856 uint8_t *att_err)
857 {
858 struct ble_att_find_info_rsp *rsp;
859 struct os_mbuf *txom;
860 uint16_t mtu;
861 int rc;
862
863 /* Just reuse the request buffer for the response. */
864 txom = *rxom;
865 *rxom = NULL;
866 os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
867
868 /* Write the response base at the start of the buffer. The format field is
869 * unknown at this point; it will be filled in later.
870 */
871 rsp = ble_att_cmd_prepare(BLE_ATT_OP_FIND_INFO_RSP, sizeof(*rsp), txom);
872 if (rsp == NULL) {
873 *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
874 rc = BLE_HS_ENOMEM;
875 goto done;
876 }
877
878 /* Write the variable length Information Data field, populating the format
879 * field as appropriate.
880 */
881 mtu = ble_att_mtu(conn_handle);
882 rc = ble_att_svr_fill_info(start_handle, end_handle, txom, mtu,
883 &rsp->bafp_format);
884 if (rc != 0) {
885 *att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
886 rc = BLE_HS_ENOENT;
887 goto done;
888 }
889
890 BLE_ATT_LOG_CMD(1, "find info rsp", conn_handle, ble_att_find_info_rsp_log,
891 rsp);
892
893 rc = 0;
894
895 done:
896 *out_txom = txom;
897 return rc;
898 }
899
900 int
ble_att_svr_rx_find_info(uint16_t conn_handle,struct os_mbuf ** rxom)901 ble_att_svr_rx_find_info(uint16_t conn_handle, struct os_mbuf **rxom)
902 {
903 #if !MYNEWT_VAL(BLE_ATT_SVR_FIND_INFO)
904 return BLE_HS_ENOTSUP;
905 #endif
906
907 struct ble_att_find_info_req *req;
908 struct os_mbuf *txom;
909 uint16_t err_handle, start_handle, end_handle;
910 uint8_t att_err;
911 int rc;
912
913 /* Initialize some values in case of early error. */
914 txom = NULL;
915 att_err = 0;
916 err_handle = 0;
917
918 rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
919 if (rc != 0) {
920 err_handle = 0;
921 goto done;
922 }
923
924 req = (struct ble_att_find_info_req *)(*rxom)->om_data;
925 start_handle = le16toh(req->bafq_start_handle);
926 end_handle = le16toh(req->bafq_end_handle);
927
928 BLE_ATT_LOG_CMD(0, "find info req", conn_handle, ble_att_find_info_req_log,
929 req);
930
931 /* Tx error response if start handle is greater than end handle or is equal
932 * to 0 (Vol. 3, Part F, 3.4.3.1).
933 */
934 if (start_handle > end_handle || start_handle == 0) {
935 att_err = BLE_ATT_ERR_INVALID_HANDLE;
936 err_handle = start_handle;
937 rc = BLE_HS_EBADDATA;
938 goto done;
939 }
940
941 rc = ble_att_svr_build_find_info_rsp(conn_handle,
942 start_handle, end_handle,
943 rxom, &txom, &att_err);
944 if (rc != 0) {
945 err_handle = start_handle;
946 goto done;
947 }
948
949 rc = 0;
950
951 done:
952 rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_FIND_INFO_REQ,
953 att_err, err_handle);
954 return rc;
955 }
956
957 /**
958 * Fills a Find-By-Type-Value-Response with single entry.
959 *
960 * @param om The response mbuf.
961 * @param first First handle ID in the current group of IDs.
962 * @param last Last handle ID in the current group of ID.
963 * @param mtu The ATT L2CAP channel MTU.
964 *
965 * @return 0 if the response should be sent;
966 * BLE_HS_EAGAIN if the entry was successfully
967 * processed and subsequent entries can be
968 * inspected.
969 * Other nonzero on error.
970 */
971 static int
ble_att_svr_fill_type_value_entry(struct os_mbuf * om,uint16_t first,uint16_t last,int mtu,uint8_t * out_att_err)972 ble_att_svr_fill_type_value_entry(struct os_mbuf *om, uint16_t first,
973 uint16_t last, int mtu,
974 uint8_t *out_att_err)
975 {
976 uint16_t u16;
977 int rsp_sz;
978 int rc;
979
980 rsp_sz = OS_MBUF_PKTHDR(om)->omp_len + 4;
981 if (rsp_sz > mtu) {
982 return 0;
983 }
984
985 put_le16(&u16, first);
986 rc = os_mbuf_append(om, &u16, 2);
987 if (rc != 0) {
988 *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
989 return BLE_HS_ENOMEM;
990 }
991
992 put_le16(&u16, last);
993 rc = os_mbuf_append(om, &u16, 2);
994 if (rc != 0) {
995 *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
996 return BLE_HS_ENOMEM;
997 }
998
999 return BLE_HS_EAGAIN;
1000 }
1001
1002 static int
ble_att_svr_is_valid_find_group_type(const ble_uuid_t * uuid)1003 ble_att_svr_is_valid_find_group_type(const ble_uuid_t *uuid)
1004 {
1005 uint16_t uuid16;
1006
1007 uuid16 = ble_uuid_u16(uuid);
1008
1009 return uuid16 == BLE_ATT_UUID_PRIMARY_SERVICE ||
1010 uuid16 == BLE_ATT_UUID_SECONDARY_SERVICE ||
1011 uuid16 == BLE_ATT_UUID_CHARACTERISTIC;
1012 }
1013
1014 static int
ble_att_svr_is_valid_group_end(const ble_uuid_t * uuid_group,const ble_uuid_t * uuid)1015 ble_att_svr_is_valid_group_end(const ble_uuid_t *uuid_group,
1016 const ble_uuid_t *uuid)
1017 {
1018 uint16_t uuid16;
1019
1020 /* Grouping is defined only for 16-bit UUIDs, so any attribute ends group
1021 * for non-16-bit UUIDs.
1022 */
1023 if (uuid_group->type != BLE_UUID_TYPE_16) {
1024 return 1;
1025 }
1026
1027 /* Grouping is defined only for 16-bit UUIDs, so non-16-bit UUID attribute
1028 * cannot end group.
1029 */
1030 if (uuid->type != BLE_UUID_TYPE_16) {
1031 return 0;
1032 }
1033
1034 switch (ble_uuid_u16(uuid_group)) {
1035 case BLE_ATT_UUID_PRIMARY_SERVICE:
1036 case BLE_ATT_UUID_SECONDARY_SERVICE:
1037 uuid16 = ble_uuid_u16(uuid);
1038
1039 /* Only Primary or Secondary Service types end service group. */
1040 return uuid16 == BLE_ATT_UUID_PRIMARY_SERVICE ||
1041 uuid16 == BLE_ATT_UUID_SECONDARY_SERVICE;
1042 case BLE_ATT_UUID_CHARACTERISTIC:
1043 /* Any valid grouping type ends characteristic group */
1044 return ble_att_svr_is_valid_find_group_type(uuid);
1045 default:
1046 /* Any attribute type ends group of non-grouping type */
1047 return 1;
1048 }
1049 }
1050
1051 /**
1052 * Fills the supplied mbuf with the variable length Handles-Information-List
1053 * field of a Find-By-Type-Value ATT response.
1054 *
1055 * @param req The Find-By-Type-Value-Request being responded
1056 * to.
1057 * @param rxom The mbuf containing the received request.
1058 * @param txom The destination mbuf where the
1059 * Handles-Information-List field gets
1060 * written.
1061 * @param mtu The ATT L2CAP channel MTU.
1062 *
1063 * @return 0 on success;
1064 * BLE_HS_ENOENT if attribute not found;
1065 * BLE_HS_EAPP on other error.
1066 */
1067 static int
ble_att_svr_fill_type_value(uint16_t conn_handle,uint16_t start_handle,uint16_t end_handle,ble_uuid16_t attr_type,struct os_mbuf * rxom,struct os_mbuf * txom,uint16_t mtu,uint8_t * out_att_err)1068 ble_att_svr_fill_type_value(uint16_t conn_handle,
1069 uint16_t start_handle, uint16_t end_handle,
1070 ble_uuid16_t attr_type,
1071 struct os_mbuf *rxom, struct os_mbuf *txom,
1072 uint16_t mtu, uint8_t *out_att_err)
1073 {
1074 struct ble_att_svr_entry *ha;
1075 uint8_t buf[16];
1076 uint16_t attr_len;
1077 uint16_t first;
1078 uint16_t prev;
1079 int any_entries;
1080 int rc;
1081
1082 first = 0;
1083 prev = 0;
1084 rc = 0;
1085
1086 /* Iterate through the attribute list, keeping track of the current
1087 * matching group. For each attribute entry, determine if data needs to be
1088 * written to the response.
1089 */
1090 STAILQ_FOREACH(ha, &ble_att_svr_list, ha_next) {
1091 if (ha->ha_handle_id < start_handle) {
1092 continue;
1093 }
1094
1095 /* Continue to look for end of group in case group is in progress. */
1096 if (!first && ha->ha_handle_id > end_handle) {
1097 break;
1098 }
1099
1100 /* With group in progress, check if current attribute ends it. */
1101 if (first) {
1102 if (!ble_att_svr_is_valid_group_end(&attr_type.u, ha->ha_uuid)) {
1103 prev = ha->ha_handle_id;
1104 continue;
1105 }
1106
1107 rc = ble_att_svr_fill_type_value_entry(txom, first, prev, mtu,
1108 out_att_err);
1109 if (rc != BLE_HS_EAGAIN) {
1110 goto done;
1111 }
1112
1113 first = 0;
1114 prev = 0;
1115
1116 /* Break in case we were just looking for end of group past the end
1117 * handle ID. */
1118 if (ha->ha_handle_id > end_handle) {
1119 break;
1120 }
1121 }
1122
1123 /* Compare the attribute type and value to the request fields to
1124 * determine if this attribute matches.
1125 */
1126 if (ble_uuid_cmp(ha->ha_uuid, &attr_type.u) == 0) {
1127 rc = ble_att_svr_read_flat(conn_handle, ha, 0, sizeof buf, buf,
1128 &attr_len, out_att_err);
1129 if (rc != 0) {
1130 goto done;
1131 }
1132 /* value is at the end of req */
1133 rc = os_mbuf_cmpf(rxom, sizeof(struct ble_att_find_type_value_req),
1134 buf, attr_len);
1135 if (rc == 0) {
1136 first = ha->ha_handle_id;
1137 prev = ha->ha_handle_id;
1138 }
1139 }
1140 }
1141
1142 /* Process last group in case a group was in progress when the end of the
1143 * attribute list was reached.
1144 */
1145 if (first) {
1146 rc = ble_att_svr_fill_type_value_entry(txom, first, prev, mtu,
1147 out_att_err);
1148 if (rc == BLE_HS_EAGAIN) {
1149 rc = 0;
1150 }
1151 } else {
1152 rc = 0;
1153 }
1154
1155 done:
1156 any_entries = OS_MBUF_PKTHDR(txom)->omp_len >
1157 BLE_ATT_FIND_TYPE_VALUE_RSP_BASE_SZ;
1158 if (rc == 0 && !any_entries) {
1159 *out_att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
1160 return BLE_HS_ENOENT;
1161 } else {
1162 return rc;
1163 }
1164 }
1165
1166 static int
ble_att_svr_build_find_type_value_rsp(uint16_t conn_handle,uint16_t start_handle,uint16_t end_handle,ble_uuid16_t attr_type,struct os_mbuf ** rxom,struct os_mbuf ** out_txom,uint8_t * out_att_err)1167 ble_att_svr_build_find_type_value_rsp(uint16_t conn_handle,
1168 uint16_t start_handle,
1169 uint16_t end_handle,
1170 ble_uuid16_t attr_type,
1171 struct os_mbuf **rxom,
1172 struct os_mbuf **out_txom,
1173 uint8_t *out_att_err)
1174 {
1175 struct os_mbuf *txom;
1176 uint16_t mtu;
1177 uint8_t *buf;
1178 int rc;
1179
1180 rc = ble_att_svr_pkt(rxom, &txom, out_att_err);
1181 if (rc != 0) {
1182 goto done;
1183 }
1184
1185 /* info list is filled later on */
1186 buf = ble_att_cmd_prepare(BLE_ATT_OP_FIND_TYPE_VALUE_RSP, 0, txom);
1187 if (buf == NULL) {
1188 *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
1189 rc = BLE_HS_ENOMEM;
1190 goto done;
1191 }
1192
1193 /* Write the variable length Information Data field. */
1194 mtu = ble_att_mtu(conn_handle);
1195
1196 rc = ble_att_svr_fill_type_value(conn_handle, start_handle, end_handle,
1197 attr_type, *rxom, txom, mtu,
1198 out_att_err);
1199 if (rc != 0) {
1200 goto done;
1201 }
1202
1203 BLE_ATT_LOG_EMPTY_CMD(1, "find type value rsp", conn_handle);
1204
1205 rc = 0;
1206
1207 done:
1208 *out_txom = txom;
1209 return rc;
1210 }
1211
1212 int
ble_att_svr_rx_find_type_value(uint16_t conn_handle,struct os_mbuf ** rxom)1213 ble_att_svr_rx_find_type_value(uint16_t conn_handle, struct os_mbuf **rxom)
1214 {
1215 #if !MYNEWT_VAL(BLE_ATT_SVR_FIND_TYPE)
1216 return BLE_HS_ENOTSUP;
1217 #endif
1218
1219 struct ble_att_find_type_value_req *req;
1220 uint16_t start_handle, end_handle;
1221 ble_uuid16_t attr_type;
1222 struct os_mbuf *txom;
1223 uint16_t err_handle;
1224 uint8_t att_err;
1225 int rc;
1226
1227 /* Initialize some values in case of early error. */
1228 txom = NULL;
1229 att_err = 0;
1230 err_handle = 0;
1231
1232 rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
1233 if (rc != 0) {
1234 goto done;
1235 }
1236
1237 req = (struct ble_att_find_type_value_req *)(*rxom)->om_data;
1238 start_handle = le16toh(req->bavq_start_handle);
1239 end_handle = le16toh(req->bavq_end_handle);
1240 attr_type = (ble_uuid16_t) BLE_UUID16_INIT(le16toh(req->bavq_attr_type));
1241 BLE_ATT_LOG_CMD(0, "find type value req", conn_handle,
1242 ble_att_find_type_value_req_log, req);
1243
1244 /* Tx error response if start handle is greater than end handle or is equal
1245 * to 0 (Vol. 3, Part F, 3.4.3.3).
1246 */
1247 if (start_handle > end_handle || start_handle == 0) {
1248 att_err = BLE_ATT_ERR_INVALID_HANDLE;
1249 err_handle = start_handle;
1250 rc = BLE_HS_EBADDATA;
1251 goto done;
1252 }
1253 rc = ble_att_svr_build_find_type_value_rsp(conn_handle, start_handle,
1254 end_handle, attr_type, rxom,
1255 &txom, &att_err);
1256 if (rc != 0) {
1257 err_handle = start_handle;
1258 goto done;
1259 }
1260
1261 rc = 0;
1262
1263 done:
1264 rc = ble_att_svr_tx_rsp(conn_handle, rc, txom,
1265 BLE_ATT_OP_FIND_TYPE_VALUE_REQ, att_err,
1266 err_handle);
1267 return rc;
1268 }
1269
1270 static int
ble_att_svr_build_read_type_rsp(uint16_t conn_handle,uint16_t start_handle,uint16_t end_handle,const ble_uuid_t * uuid,struct os_mbuf ** rxom,struct os_mbuf ** out_txom,uint8_t * att_err,uint16_t * err_handle)1271 ble_att_svr_build_read_type_rsp(uint16_t conn_handle,
1272 uint16_t start_handle, uint16_t end_handle,
1273 const ble_uuid_t *uuid,
1274 struct os_mbuf **rxom,
1275 struct os_mbuf **out_txom,
1276 uint8_t *att_err,
1277 uint16_t *err_handle)
1278 {
1279 struct ble_att_attr_data_list *data;
1280 struct ble_att_read_type_rsp *rsp;
1281 struct ble_att_svr_entry *entry;
1282 struct os_mbuf *txom;
1283 uint16_t attr_len;
1284 uint16_t mtu;
1285 uint8_t buf[19];
1286 int entry_written;
1287 int txomlen;
1288 int prev_attr_len;
1289 int rc;
1290
1291 *att_err = 0; /* Silence unnecessary warning. */
1292
1293 *err_handle = start_handle;
1294 entry_written = 0;
1295 prev_attr_len = 0;
1296
1297 /* Just reuse the request buffer for the response. */
1298 txom = *rxom;
1299 *rxom = NULL;
1300 os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
1301
1302 /* Allocate space for the respose base, but don't fill in the fields. They
1303 * get filled in at the end, when we know the value of the length field.
1304 */
1305
1306 rsp = ble_att_cmd_prepare(BLE_ATT_OP_READ_TYPE_RSP, sizeof(*rsp), txom);
1307 if (rsp == NULL) {
1308 *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
1309 *err_handle = 0;
1310 rc = BLE_HS_ENOMEM;
1311 goto done;
1312 }
1313
1314 mtu = ble_att_mtu(conn_handle);
1315
1316 /* Find all matching attributes, writing a record for each. */
1317 entry = NULL;
1318 while (1) {
1319 entry = ble_att_svr_find_by_uuid(entry, uuid, end_handle);
1320 if (entry == NULL) {
1321 rc = BLE_HS_ENOENT;
1322 break;
1323 }
1324
1325 if (entry->ha_handle_id >= start_handle) {
1326 rc = ble_att_svr_read_flat(conn_handle, entry, 0, sizeof buf, buf,
1327 &attr_len, att_err);
1328 if (rc != 0) {
1329 *err_handle = entry->ha_handle_id;
1330 goto done;
1331 }
1332
1333 if (attr_len > mtu - 4) {
1334 attr_len = mtu - 4;
1335 }
1336
1337 if (prev_attr_len == 0) {
1338 prev_attr_len = attr_len;
1339 } else if (prev_attr_len != attr_len) {
1340 break;
1341 }
1342
1343 txomlen = OS_MBUF_PKTHDR(txom)->omp_len + 2 + attr_len;
1344 if (txomlen > mtu) {
1345 break;
1346 }
1347
1348 data = os_mbuf_extend(txom, 2 + attr_len);
1349 if (data == NULL) {
1350 *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
1351 *err_handle = entry->ha_handle_id;
1352 rc = BLE_HS_ENOMEM;
1353 goto done;
1354 }
1355
1356 data->handle = htole16(entry->ha_handle_id);
1357 memcpy(data->value, buf, attr_len);
1358 entry_written = 1;
1359 }
1360 }
1361
1362 done:
1363 if (!entry_written) {
1364 /* No matching attributes. */
1365 if (*att_err == 0) {
1366 *att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
1367 }
1368 if (rc == 0) {
1369 rc = BLE_HS_ENOENT;
1370 }
1371 } else {
1372 /* Send what we can, even if an error was encountered. */
1373 rc = 0;
1374 *att_err = 0;
1375
1376 /* Fill the response base. */
1377 rsp->batp_length = htole16(sizeof(*data) + prev_attr_len);
1378 BLE_ATT_LOG_CMD(1, "read type rsp", conn_handle,
1379 ble_att_read_type_rsp_log, rsp);
1380 }
1381
1382 *out_txom = txom;
1383
1384 return rc;
1385 }
1386
1387 int
ble_att_svr_rx_read_type(uint16_t conn_handle,struct os_mbuf ** rxom)1388 ble_att_svr_rx_read_type(uint16_t conn_handle, struct os_mbuf **rxom)
1389 {
1390 #if !MYNEWT_VAL(BLE_ATT_SVR_READ_TYPE)
1391 return BLE_HS_ENOTSUP;
1392 #endif
1393
1394 struct ble_att_read_type_req *req;
1395 uint16_t start_handle, end_handle;
1396 struct os_mbuf *txom;
1397 uint16_t err_handle;
1398 uint16_t pktlen;
1399 ble_uuid_any_t uuid;
1400 uint8_t att_err;
1401 int rc;
1402
1403 /* Initialize some values in case of early error. */
1404 txom = NULL;
1405 err_handle = 0;
1406 att_err = 0;
1407
1408 pktlen = OS_MBUF_PKTLEN(*rxom);
1409 if (pktlen != sizeof(*req) + 2 && pktlen != sizeof(*req) + 16) {
1410 /* Malformed packet */
1411 rc = BLE_HS_EBADDATA;
1412 goto done;
1413 }
1414
1415 rc = ble_att_svr_pullup_req_base(rxom, pktlen, &att_err);
1416 if (rc != 0) {
1417 goto done;
1418 }
1419
1420 req = (struct ble_att_read_type_req *)(*rxom)->om_data;
1421
1422 BLE_ATT_LOG_CMD(0, "read type req", conn_handle, ble_att_read_type_req_log,
1423 req);
1424
1425 start_handle = le16toh(req->batq_start_handle);
1426 end_handle = le16toh(req->batq_end_handle);
1427
1428 if (start_handle > end_handle || start_handle == 0) {
1429 att_err = BLE_ATT_ERR_INVALID_HANDLE;
1430 err_handle = start_handle;
1431 rc = BLE_HS_EBADDATA;
1432 goto done;
1433 }
1434
1435 rc = ble_uuid_init_from_att_mbuf(&uuid, *rxom, sizeof(*req),
1436 pktlen - sizeof(*req));
1437 if (rc != 0) {
1438 att_err = BLE_ATT_ERR_INVALID_PDU;
1439 rc = BLE_HS_EMSGSIZE;
1440 goto done;
1441 }
1442
1443 rc = ble_att_svr_build_read_type_rsp(conn_handle, start_handle, end_handle,
1444 &uuid.u, rxom, &txom, &att_err,
1445 &err_handle);
1446 if (rc != 0) {
1447 goto done;
1448 }
1449
1450 rc = 0;
1451
1452 done:
1453 rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_TYPE_REQ,
1454 att_err, err_handle);
1455 return rc;
1456 }
1457
1458 int
ble_att_svr_rx_read(uint16_t conn_handle,struct os_mbuf ** rxom)1459 ble_att_svr_rx_read(uint16_t conn_handle, struct os_mbuf **rxom)
1460 {
1461 #if !MYNEWT_VAL(BLE_ATT_SVR_READ)
1462 return BLE_HS_ENOTSUP;
1463 #endif
1464
1465 struct ble_att_read_req *req;
1466 struct os_mbuf *txom;
1467 uint16_t err_handle;
1468 uint8_t att_err;
1469 int rc;
1470
1471 /* Initialize some values in case of early error. */
1472 txom = NULL;
1473 att_err = 0;
1474 err_handle = 0;
1475
1476 rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
1477 if (rc != 0) {
1478 goto done;
1479 }
1480
1481 req = (struct ble_att_read_req *)(*rxom)->om_data;
1482 BLE_ATT_LOG_CMD(0, "read req", conn_handle, ble_att_read_req_log, req);
1483
1484 err_handle = le16toh(req->barq_handle);
1485
1486 /* Just reuse the request buffer for the response. */
1487 txom = *rxom;
1488 *rxom = NULL;
1489 os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
1490
1491 if (ble_att_cmd_prepare(BLE_ATT_OP_READ_RSP, 0, txom) == NULL) {
1492 att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
1493 rc = BLE_HS_ENOMEM;
1494 goto done;
1495 }
1496
1497 rc = ble_att_svr_read_handle(conn_handle, err_handle, 0, txom, &att_err);
1498 if (rc != 0) {
1499 goto done;
1500 }
1501
1502 done:
1503 rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_REQ,
1504 att_err, err_handle);
1505 return rc;
1506 }
1507
1508 int
ble_att_svr_rx_read_blob(uint16_t conn_handle,struct os_mbuf ** rxom)1509 ble_att_svr_rx_read_blob(uint16_t conn_handle, struct os_mbuf **rxom)
1510 {
1511 #if !MYNEWT_VAL(BLE_ATT_SVR_READ_BLOB)
1512 return BLE_HS_ENOTSUP;
1513 #endif
1514
1515 struct ble_att_read_blob_req *req;
1516 struct os_mbuf *txom;
1517 uint16_t err_handle, offset;
1518 uint8_t att_err;
1519 int rc;
1520
1521 /* Initialize some values in case of early error. */
1522 txom = NULL;
1523 att_err = 0;
1524 err_handle = 0;
1525
1526 rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
1527 if (rc != 0) {
1528 goto done;
1529 }
1530
1531 req = (struct ble_att_read_blob_req *)(*rxom)->om_data;
1532 BLE_ATT_LOG_CMD(0, "read blob req", conn_handle, ble_att_read_blob_req_log,
1533 req);
1534
1535 err_handle = le16toh(req->babq_handle);
1536 offset = le16toh(req->babq_offset);
1537
1538 /* Just reuse the request buffer for the response. */
1539 txom = *rxom;
1540 *rxom = NULL;
1541 os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
1542
1543 if (ble_att_cmd_prepare(BLE_ATT_OP_READ_BLOB_RSP, 0, txom) == NULL) {
1544 att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
1545 rc = BLE_HS_ENOMEM;
1546 goto done;
1547 }
1548
1549 rc = ble_att_svr_read_handle(conn_handle, err_handle, offset,
1550 txom, &att_err);
1551 if (rc != 0) {
1552 goto done;
1553 }
1554
1555 BLE_ATT_LOG_EMPTY_CMD(1, "read blob rsp", conn_handle);
1556
1557 rc = 0;
1558
1559 done:
1560 rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_BLOB_REQ,
1561 att_err, err_handle);
1562 return rc;
1563 }
1564
1565 static int
ble_att_svr_build_read_mult_rsp(uint16_t conn_handle,struct os_mbuf ** rxom,struct os_mbuf ** out_txom,uint8_t * att_err,uint16_t * err_handle)1566 ble_att_svr_build_read_mult_rsp(uint16_t conn_handle,
1567 struct os_mbuf **rxom,
1568 struct os_mbuf **out_txom,
1569 uint8_t *att_err,
1570 uint16_t *err_handle)
1571 {
1572 struct os_mbuf *txom;
1573 uint16_t handle;
1574 uint16_t mtu;
1575 int rc;
1576
1577 mtu = ble_att_mtu(conn_handle);
1578
1579 rc = ble_att_svr_pkt(rxom, &txom, att_err);
1580 if (rc != 0) {
1581 *err_handle = 0;
1582 goto done;
1583 }
1584
1585 if (ble_att_cmd_prepare(BLE_ATT_OP_READ_MULT_RSP, 0, txom) == NULL) {
1586 *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
1587 *err_handle = 0;
1588 rc = BLE_HS_ENOMEM;
1589 goto done;
1590 }
1591
1592 /* Iterate through requested handles, reading the corresponding attribute
1593 * for each. Stop when there are no more handles to process, or the
1594 * response is full.
1595 */
1596 while (OS_MBUF_PKTLEN(*rxom) >= 2 && OS_MBUF_PKTLEN(txom) < mtu) {
1597 /* Ensure the full 16-bit handle is contiguous at the start of the
1598 * mbuf.
1599 */
1600 rc = ble_att_svr_pullup_req_base(rxom, 2, att_err);
1601 if (rc != 0) {
1602 *err_handle = 0;
1603 goto done;
1604 }
1605
1606 /* Extract the 16-bit handle and strip it from the front of the
1607 * mbuf.
1608 */
1609 handle = get_le16((*rxom)->om_data);
1610 os_mbuf_adj(*rxom, 2);
1611
1612 rc = ble_att_svr_read_handle(conn_handle, handle, 0, txom, att_err);
1613 if (rc != 0) {
1614 *err_handle = handle;
1615 goto done;
1616 }
1617 }
1618
1619 BLE_ATT_LOG_EMPTY_CMD(1, "read mult rsp", conn_handle);
1620 rc = 0;
1621
1622 done:
1623 *out_txom = txom;
1624 return rc;
1625 }
1626
1627 int
ble_att_svr_rx_read_mult(uint16_t conn_handle,struct os_mbuf ** rxom)1628 ble_att_svr_rx_read_mult(uint16_t conn_handle, struct os_mbuf **rxom)
1629 {
1630 #if !MYNEWT_VAL(BLE_ATT_SVR_READ_MULT)
1631 return BLE_HS_ENOTSUP;
1632 #endif
1633
1634 struct os_mbuf *txom;
1635 uint16_t err_handle;
1636 uint8_t att_err;
1637 int rc;
1638
1639 BLE_ATT_LOG_EMPTY_CMD(0, "read mult req", conn_handle);
1640
1641 /* Initialize some values in case of early error. */
1642 txom = NULL;
1643 err_handle = 0;
1644 att_err = 0;
1645
1646 rc = ble_att_svr_build_read_mult_rsp(conn_handle, rxom, &txom, &att_err,
1647 &err_handle);
1648
1649 return ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_MULT_REQ,
1650 att_err, err_handle);
1651 }
1652
1653 static int
ble_att_svr_is_valid_read_group_type(const ble_uuid_t * uuid)1654 ble_att_svr_is_valid_read_group_type(const ble_uuid_t *uuid)
1655 {
1656 uint16_t uuid16;
1657
1658 uuid16 = ble_uuid_u16(uuid);
1659
1660 return uuid16 == BLE_ATT_UUID_PRIMARY_SERVICE ||
1661 uuid16 == BLE_ATT_UUID_SECONDARY_SERVICE;
1662 }
1663
1664 static int
ble_att_svr_service_uuid(struct ble_att_svr_entry * entry,ble_uuid_any_t * uuid,uint8_t * out_att_err)1665 ble_att_svr_service_uuid(struct ble_att_svr_entry *entry,
1666 ble_uuid_any_t *uuid, uint8_t *out_att_err)
1667 {
1668 uint8_t val[16];
1669 uint16_t attr_len;
1670 int rc;
1671
1672 rc = ble_att_svr_read_flat(BLE_HS_CONN_HANDLE_NONE, entry, 0, sizeof(val), val,
1673 &attr_len, out_att_err);
1674 if (rc != 0) {
1675 return rc;
1676 }
1677
1678 rc = ble_uuid_init_from_buf(uuid, val, attr_len);
1679
1680 return rc;
1681 }
1682
1683 static int
ble_att_svr_read_group_type_entry_write(struct os_mbuf * om,uint16_t mtu,uint16_t start_group_handle,uint16_t end_group_handle,const ble_uuid_t * service_uuid)1684 ble_att_svr_read_group_type_entry_write(struct os_mbuf *om, uint16_t mtu,
1685 uint16_t start_group_handle,
1686 uint16_t end_group_handle,
1687 const ble_uuid_t *service_uuid)
1688 {
1689 uint8_t *buf;
1690 int len;
1691
1692 if (service_uuid->type == BLE_UUID_TYPE_16) {
1693 len = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16;
1694 } else {
1695 len = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128;
1696 }
1697 if (OS_MBUF_PKTLEN(om) + len > mtu) {
1698 return BLE_HS_EMSGSIZE;
1699 }
1700
1701 buf = os_mbuf_extend(om, len);
1702 if (buf == NULL) {
1703 return BLE_HS_ENOMEM;
1704 }
1705
1706 put_le16(buf + 0, start_group_handle);
1707 put_le16(buf + 2, end_group_handle);
1708 ble_uuid_flat(service_uuid, buf + 4);
1709
1710 return 0;
1711 }
1712
1713 /**
1714 * @return 0 on success; BLE_HS error code on failure.
1715 */
1716 static int
ble_att_svr_build_read_group_type_rsp(uint16_t conn_handle,uint16_t start_handle,uint16_t end_handle,const ble_uuid_t * group_uuid,struct os_mbuf ** rxom,struct os_mbuf ** out_txom,uint8_t * att_err,uint16_t * err_handle)1717 ble_att_svr_build_read_group_type_rsp(uint16_t conn_handle,
1718 uint16_t start_handle,
1719 uint16_t end_handle,
1720 const ble_uuid_t *group_uuid,
1721 struct os_mbuf **rxom,
1722 struct os_mbuf **out_txom,
1723 uint8_t *att_err,
1724 uint16_t *err_handle)
1725 {
1726 struct ble_att_read_group_type_rsp *rsp;
1727 struct ble_att_svr_entry *entry;
1728 struct os_mbuf *txom;
1729 uint16_t start_group_handle;
1730 uint16_t end_group_handle;
1731 uint16_t mtu;
1732 ble_uuid_any_t service_uuid;
1733 int rc;
1734
1735 /* Silence warnings. */
1736 end_group_handle = 0;
1737
1738 *att_err = 0;
1739 *err_handle = start_handle;
1740
1741 mtu = ble_att_mtu(conn_handle);
1742
1743 /* Just reuse the request buffer for the response. */
1744 txom = *rxom;
1745 *rxom = NULL;
1746 os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
1747
1748 /* Reserve space for the response base. */
1749 rsp = ble_att_cmd_prepare(BLE_ATT_OP_READ_GROUP_TYPE_RSP, sizeof(*rsp),
1750 txom);
1751 if (rsp == NULL) {
1752 *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
1753 rc = BLE_HS_ENOMEM;
1754 goto done;
1755 }
1756
1757 start_group_handle = 0;
1758 rsp->bagp_length = 0;
1759 STAILQ_FOREACH(entry, &ble_att_svr_list, ha_next) {
1760 if (entry->ha_handle_id < start_handle) {
1761 continue;
1762 }
1763 if (entry->ha_handle_id > end_handle) {
1764 /* The full input range has been searched. */
1765 rc = 0;
1766 goto done;
1767 }
1768
1769 if (start_group_handle != 0) {
1770 /* We have already found the start of a group. */
1771 if (!ble_att_svr_is_valid_read_group_type(entry->ha_uuid)) {
1772 /* This attribute is part of the current group. */
1773 end_group_handle = entry->ha_handle_id;
1774 } else {
1775 /* This attribute marks the end of the group. Write an entry
1776 * representing the group to the response.
1777 */
1778 rc = ble_att_svr_read_group_type_entry_write(
1779 txom, mtu, start_group_handle, end_group_handle,
1780 &service_uuid.u);
1781 start_group_handle = 0;
1782 end_group_handle = 0;
1783 if (rc != 0) {
1784 *err_handle = entry->ha_handle_id;
1785 if (rc == BLE_HS_ENOMEM) {
1786 *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
1787 } else {
1788 BLE_HS_DBG_ASSERT(rc == BLE_HS_EMSGSIZE);
1789 }
1790 goto done;
1791 }
1792 }
1793 }
1794
1795 if (start_group_handle == 0) {
1796 /* We are looking for the start of a group. */
1797 if (ble_uuid_cmp(entry->ha_uuid, group_uuid) == 0) {
1798 /* Found a group start. Read the group UUID. */
1799 rc = ble_att_svr_service_uuid(entry, &service_uuid, att_err);
1800 if (rc != 0) {
1801 *err_handle = entry->ha_handle_id;
1802 goto done;
1803 }
1804
1805 /* Make sure the group UUID lengths are consistent. If this
1806 * group has a different length UUID, then cut the response
1807 * short.
1808 */
1809 switch (rsp->bagp_length) {
1810 case 0:
1811 if (service_uuid.u.type == BLE_UUID_TYPE_16) {
1812 rsp->bagp_length = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16;
1813 } else {
1814 rsp->bagp_length = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128;
1815 }
1816 break;
1817
1818 case BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16:
1819 if (service_uuid.u.type != BLE_UUID_TYPE_16) {
1820 rc = 0;
1821 goto done;
1822 }
1823 break;
1824
1825 case BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128:
1826 if (service_uuid.u.type == BLE_UUID_TYPE_16) {
1827 rc = 0;
1828 goto done;
1829 }
1830 break;
1831
1832 default:
1833 BLE_HS_DBG_ASSERT(0);
1834 goto done;
1835 }
1836
1837 start_group_handle = entry->ha_handle_id;
1838 end_group_handle = entry->ha_handle_id;
1839 }
1840 }
1841 }
1842
1843 rc = 0;
1844
1845 done:
1846 if (rc == 0) {
1847 if (start_group_handle != 0) {
1848 /* A group was being processed. Add its corresponding entry to the
1849 * response.
1850 */
1851
1852 if (entry == NULL) {
1853 /* We have reached the end of the attribute list. Indicate an
1854 * end handle of 0xffff so that the client knows there are no
1855 * more attributes without needing to send a follow-up request.
1856 */
1857 end_group_handle = 0xffff;
1858 }
1859
1860 rc = ble_att_svr_read_group_type_entry_write(txom, mtu,
1861 start_group_handle,
1862 end_group_handle,
1863 &service_uuid.u);
1864 if (rc == BLE_HS_ENOMEM) {
1865 *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
1866 }
1867 }
1868
1869 if (OS_MBUF_PKTLEN(txom) <= BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ) {
1870 *att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
1871 rc = BLE_HS_ENOENT;
1872 }
1873 }
1874
1875 if (rc == 0 || rc == BLE_HS_EMSGSIZE) {
1876 BLE_ATT_LOG_CMD(1, "read group type rsp", conn_handle,
1877 ble_att_read_group_type_rsp_log, rsp);
1878 rc = 0;
1879 }
1880
1881 *out_txom = txom;
1882 return rc;
1883 }
1884
1885 int
ble_att_svr_rx_read_group_type(uint16_t conn_handle,struct os_mbuf ** rxom)1886 ble_att_svr_rx_read_group_type(uint16_t conn_handle, struct os_mbuf **rxom)
1887 {
1888 #if !MYNEWT_VAL(BLE_ATT_SVR_READ_GROUP_TYPE)
1889 return BLE_HS_ENOTSUP;
1890 #endif
1891
1892 struct ble_att_read_group_type_req *req;
1893 struct os_mbuf *txom;
1894 ble_uuid_any_t uuid;
1895 uint16_t err_handle, start_handle, end_handle;
1896 uint16_t pktlen;
1897 uint8_t att_err;
1898 int om_uuid_len;
1899 int rc;
1900
1901 /* Initialize some values in case of early error. */
1902 txom = NULL;
1903 err_handle = 0;
1904 att_err = 0;
1905
1906 pktlen = OS_MBUF_PKTLEN(*rxom);
1907 if (pktlen != sizeof(*req) + 2 && pktlen != sizeof(*req) + 16) {
1908 /* Malformed packet */
1909 rc = BLE_HS_EBADDATA;
1910 goto done;
1911 }
1912
1913 rc = ble_att_svr_pullup_req_base(rxom, pktlen, &att_err);
1914 if (rc != 0) {
1915 goto done;
1916 }
1917
1918 req = (struct ble_att_read_group_type_req *)(*rxom)->om_data;
1919 BLE_ATT_LOG_CMD(0, "read group type req", conn_handle,
1920 ble_att_read_group_type_req_log, req);
1921
1922 start_handle = le16toh(req->bagq_start_handle);
1923 end_handle = le16toh(req->bagq_end_handle);
1924
1925 if (start_handle > end_handle || start_handle == 0) {
1926 att_err = BLE_ATT_ERR_INVALID_HANDLE;
1927 err_handle = start_handle;
1928 rc = BLE_HS_EBADDATA;
1929 goto done;
1930 }
1931
1932 om_uuid_len = OS_MBUF_PKTHDR(*rxom)->omp_len - sizeof(*req);
1933 rc = ble_uuid_init_from_att_mbuf(&uuid, *rxom, sizeof(*req), om_uuid_len);
1934 if (rc != 0) {
1935 att_err = BLE_ATT_ERR_INVALID_PDU;
1936 err_handle = start_handle;
1937 rc = BLE_HS_EBADDATA;
1938 goto done;
1939 }
1940
1941 if (!ble_att_svr_is_valid_read_group_type(&uuid.u)) {
1942 att_err = BLE_ATT_ERR_UNSUPPORTED_GROUP;
1943 err_handle = start_handle;
1944 rc = BLE_HS_EREJECT;
1945 goto done;
1946 }
1947
1948 rc = ble_att_svr_build_read_group_type_rsp(conn_handle, start_handle,
1949 end_handle, &uuid.u,
1950 rxom, &txom, &att_err,
1951 &err_handle);
1952 if (rc != 0) {
1953 goto done;
1954 }
1955
1956 rc = 0;
1957
1958 done:
1959 rc = ble_att_svr_tx_rsp(conn_handle, rc, txom,
1960 BLE_ATT_OP_READ_GROUP_TYPE_REQ, att_err,
1961 err_handle);
1962 return rc;
1963 }
1964
1965 static int
ble_att_svr_build_write_rsp(struct os_mbuf ** rxom,struct os_mbuf ** out_txom,uint8_t * att_err)1966 ble_att_svr_build_write_rsp(struct os_mbuf **rxom, struct os_mbuf **out_txom,
1967 uint8_t *att_err)
1968 {
1969 struct os_mbuf *txom;
1970 int rc;
1971
1972 /* Allocate a new buffer for the response. A write response never reuses
1973 * the request buffer. See the note at the top of this file for details.
1974 */
1975 rc = ble_att_svr_pkt(rxom, &txom, att_err);
1976 if (rc != 0) {
1977 goto done;
1978 }
1979
1980 if (ble_att_cmd_prepare(BLE_ATT_OP_WRITE_RSP, 0, txom) == NULL) {
1981 *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
1982 rc = BLE_HS_ENOMEM;
1983 goto done;
1984 }
1985
1986 rc = 0;
1987
1988 done:
1989 *out_txom = txom;
1990 return rc;
1991 }
1992
1993 int
ble_att_svr_rx_write(uint16_t conn_handle,struct os_mbuf ** rxom)1994 ble_att_svr_rx_write(uint16_t conn_handle, struct os_mbuf **rxom)
1995 {
1996 #if !MYNEWT_VAL(BLE_ATT_SVR_WRITE)
1997 return BLE_HS_ENOTSUP;
1998 #endif
1999
2000 struct ble_att_write_req *req;
2001 struct os_mbuf *txom;
2002 uint16_t handle;
2003 uint8_t att_err;
2004 int rc;
2005
2006 /* Initialize some values in case of early error. */
2007 txom = NULL;
2008 att_err = 0;
2009 handle = 0;
2010
2011 rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
2012 if (rc != 0) {
2013 goto done;
2014 }
2015
2016 req = (struct ble_att_write_req *)(*rxom)->om_data;
2017
2018 BLE_ATT_LOG_CMD(0, "write req", conn_handle,
2019 ble_att_write_req_log, req);
2020
2021 handle = le16toh(req->bawq_handle);
2022
2023 /* Allocate the write response. This must be done prior to processing the
2024 * request. See the note at the top of this file for details.
2025 */
2026 rc = ble_att_svr_build_write_rsp(rxom, &txom, &att_err);
2027 if (rc != 0) {
2028 goto done;
2029 }
2030
2031 /* Strip the request base from the front of the mbuf. */
2032 os_mbuf_adj(*rxom, sizeof(*req));
2033
2034 rc = ble_att_svr_write_handle(conn_handle, handle, 0, rxom, &att_err);
2035 if (rc != 0) {
2036 goto done;
2037 }
2038
2039 BLE_ATT_LOG_EMPTY_CMD(1, "write rsp", conn_handle);
2040
2041 rc = 0;
2042
2043 done:
2044 rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_WRITE_REQ,
2045 att_err, handle);
2046 return rc;
2047 }
2048
2049 int
ble_att_svr_rx_write_no_rsp(uint16_t conn_handle,struct os_mbuf ** rxom)2050 ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, struct os_mbuf **rxom)
2051 {
2052 #if !MYNEWT_VAL(BLE_ATT_SVR_WRITE_NO_RSP)
2053 return BLE_HS_ENOTSUP;
2054 #endif
2055
2056 struct ble_att_write_req *req;
2057 uint8_t att_err;
2058 uint16_t handle;
2059 int rc;
2060
2061 rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
2062 if (rc != 0) {
2063 return rc;
2064 }
2065
2066 req = (struct ble_att_write_req *)(*rxom)->om_data;
2067 BLE_ATT_LOG_CMD(0, "write cmd", conn_handle,
2068 ble_att_write_req_log, req);
2069
2070 handle = le16toh(req->bawq_handle);
2071
2072 /* Strip the request base from the front of the mbuf. */
2073 os_mbuf_adj(*rxom, sizeof(*req));
2074
2075 return ble_att_svr_write_handle(conn_handle, handle, 0, rxom, &att_err);
2076 }
2077
2078 int
ble_att_svr_write_local(uint16_t attr_handle,struct os_mbuf * om)2079 ble_att_svr_write_local(uint16_t attr_handle, struct os_mbuf *om)
2080 {
2081 int rc;
2082
2083 rc = ble_att_svr_write_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0,
2084 &om, NULL);
2085
2086 /* Free the mbuf if it wasn't relinquished to the application. */
2087 os_mbuf_free_chain(om);
2088
2089 return rc;
2090 }
2091
2092 static void
ble_att_svr_prep_free(struct ble_att_prep_entry * entry)2093 ble_att_svr_prep_free(struct ble_att_prep_entry *entry)
2094 {
2095 if (entry != NULL) {
2096 os_mbuf_free_chain(entry->bape_value);
2097 #if MYNEWT_VAL(BLE_HS_DEBUG)
2098 memset(entry, 0xff, sizeof *entry);
2099 #endif
2100 os_memblock_put(&ble_att_svr_prep_entry_pool, entry);
2101 }
2102 }
2103
2104 static struct ble_att_prep_entry *
ble_att_svr_prep_alloc(uint8_t * att_err)2105 ble_att_svr_prep_alloc(uint8_t *att_err)
2106 {
2107 struct ble_att_prep_entry *entry;
2108
2109 entry = os_memblock_get(&ble_att_svr_prep_entry_pool);
2110 if (entry == NULL) {
2111 *att_err = BLE_ATT_ERR_PREPARE_QUEUE_FULL;
2112 return NULL;
2113 }
2114
2115 memset(entry, 0, sizeof *entry);
2116 entry->bape_value = ble_hs_mbuf_l2cap_pkt();
2117 if (entry->bape_value == NULL) {
2118 ble_att_svr_prep_free(entry);
2119 *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
2120 return NULL;
2121 }
2122
2123 return entry;
2124 }
2125
2126 static struct ble_att_prep_entry *
ble_att_svr_prep_find_prev(struct ble_att_svr_conn * basc,uint16_t handle,uint16_t offset)2127 ble_att_svr_prep_find_prev(struct ble_att_svr_conn *basc, uint16_t handle,
2128 uint16_t offset)
2129 {
2130 struct ble_att_prep_entry *entry;
2131 struct ble_att_prep_entry *prev;
2132
2133 prev = NULL;
2134 SLIST_FOREACH(entry, &basc->basc_prep_list, bape_next) {
2135 if (entry->bape_handle > handle) {
2136 break;
2137 }
2138
2139 if (entry->bape_handle == handle && entry->bape_offset > offset) {
2140 break;
2141 }
2142
2143 prev = entry;
2144 }
2145
2146 return prev;
2147 }
2148
2149 void
ble_att_svr_prep_clear(struct ble_att_prep_entry_list * prep_list)2150 ble_att_svr_prep_clear(struct ble_att_prep_entry_list *prep_list)
2151 {
2152 struct ble_att_prep_entry *entry;
2153
2154 while ((entry = SLIST_FIRST(prep_list)) != NULL) {
2155 SLIST_REMOVE_HEAD(prep_list, bape_next);
2156 ble_att_svr_prep_free(entry);
2157 }
2158 }
2159
2160 /**
2161 * @return 0 on success; ATT error code on failure.
2162 */
2163 static int
ble_att_svr_prep_validate(struct ble_att_prep_entry_list * prep_list,uint16_t * err_handle)2164 ble_att_svr_prep_validate(struct ble_att_prep_entry_list *prep_list,
2165 uint16_t *err_handle)
2166 {
2167 struct ble_att_prep_entry *entry;
2168 struct ble_att_prep_entry *prev;
2169 int cur_len;
2170
2171 prev = NULL;
2172 SLIST_FOREACH(entry, prep_list, bape_next) {
2173 if (prev == NULL || prev->bape_handle != entry->bape_handle) {
2174 /* Ensure attribute write starts at offset 0. */
2175 if (entry->bape_offset != 0) {
2176 *err_handle = entry->bape_handle;
2177 return BLE_ATT_ERR_INVALID_OFFSET;
2178 }
2179 } else {
2180 /* Ensure entry continues where previous left off. */
2181 if (prev->bape_offset + OS_MBUF_PKTLEN(prev->bape_value) !=
2182 entry->bape_offset) {
2183
2184 *err_handle = entry->bape_handle;
2185 return BLE_ATT_ERR_INVALID_OFFSET;
2186 }
2187 }
2188
2189 cur_len = entry->bape_offset + OS_MBUF_PKTLEN(entry->bape_value);
2190 if (cur_len > BLE_ATT_ATTR_MAX_LEN) {
2191 *err_handle = entry->bape_handle;
2192 return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
2193 }
2194
2195 prev = entry;
2196 }
2197
2198 return 0;
2199 }
2200
2201 static void
ble_att_svr_prep_extract(struct ble_att_prep_entry_list * prep_list,uint16_t * out_attr_handle,struct os_mbuf ** out_om)2202 ble_att_svr_prep_extract(struct ble_att_prep_entry_list *prep_list,
2203 uint16_t *out_attr_handle,
2204 struct os_mbuf **out_om)
2205 {
2206 struct ble_att_prep_entry *entry;
2207 struct ble_att_prep_entry *first;
2208 struct os_mbuf *om;
2209 uint16_t attr_handle;
2210
2211 BLE_HS_DBG_ASSERT(!SLIST_EMPTY(prep_list));
2212
2213 first = SLIST_FIRST(prep_list);
2214 attr_handle = first->bape_handle;
2215 om = NULL;
2216
2217 while ((entry = SLIST_FIRST(prep_list)) != NULL) {
2218 if (entry->bape_handle != attr_handle) {
2219 break;
2220 }
2221
2222 if (om == NULL) {
2223 om = entry->bape_value;
2224 } else {
2225 os_mbuf_concat(om, entry->bape_value);
2226 }
2227 entry->bape_value = NULL;
2228
2229 SLIST_REMOVE_HEAD(prep_list, bape_next);
2230 ble_att_svr_prep_free(entry);
2231 }
2232
2233 *out_attr_handle = attr_handle;
2234 *out_om = om;
2235 }
2236
2237 /**
2238 * @return 0 on success; ATT error code on failure.
2239 */
2240 static int
ble_att_svr_prep_write(uint16_t conn_handle,struct ble_att_prep_entry_list * prep_list,uint16_t * err_handle)2241 ble_att_svr_prep_write(uint16_t conn_handle,
2242 struct ble_att_prep_entry_list *prep_list,
2243 uint16_t *err_handle)
2244 {
2245 struct ble_att_svr_entry *attr;
2246 struct os_mbuf *om;
2247 uint16_t attr_handle;
2248 uint8_t att_err;
2249 int rc;
2250
2251 *err_handle = 0; /* Silence unnecessary warning. */
2252
2253 /* First, validate the contents of the prepare queue. */
2254 rc = ble_att_svr_prep_validate(prep_list, err_handle);
2255 if (rc != 0) {
2256 return rc;
2257 }
2258
2259 /* Contents are valid; perform the writes. */
2260 while (!SLIST_EMPTY(prep_list)) {
2261 ble_att_svr_prep_extract(prep_list, &attr_handle, &om);
2262
2263 /* Attribute existence was verified during prepare-write request
2264 * processing.
2265 */
2266 attr = ble_att_svr_find_by_handle(attr_handle);
2267 BLE_HS_DBG_ASSERT(attr != NULL);
2268
2269 rc = ble_att_svr_write(conn_handle, attr, 0, &om, &att_err);
2270 os_mbuf_free_chain(om);
2271 if (rc != 0) {
2272 *err_handle = attr_handle;
2273 return att_err;
2274 }
2275 }
2276
2277 return 0;
2278 }
2279
2280 static int
ble_att_svr_insert_prep_entry(uint16_t conn_handle,uint16_t handle,uint16_t offset,const struct os_mbuf * rxom,uint8_t * out_att_err)2281 ble_att_svr_insert_prep_entry(uint16_t conn_handle,
2282 uint16_t handle, uint16_t offset,
2283 const struct os_mbuf *rxom,
2284 uint8_t *out_att_err)
2285 {
2286 struct ble_att_prep_entry *prep_entry;
2287 struct ble_att_prep_entry *prep_prev;
2288 struct ble_hs_conn *conn;
2289 int rc;
2290
2291 conn = ble_hs_conn_find_assert(conn_handle);
2292
2293 prep_entry = ble_att_svr_prep_alloc(out_att_err);
2294 if (prep_entry == NULL) {
2295 return BLE_HS_ENOMEM;
2296 }
2297 prep_entry->bape_handle = handle;
2298 prep_entry->bape_offset = offset;
2299
2300 /* Append attribute value from request onto prep mbuf. */
2301 rc = os_mbuf_appendfrom(
2302 prep_entry->bape_value,
2303 rxom,
2304 sizeof(struct ble_att_prep_write_cmd),
2305 OS_MBUF_PKTLEN(rxom) - sizeof(struct ble_att_prep_write_cmd));
2306 if (rc != 0) {
2307 /* Failed to allocate an mbuf to hold the additional data. */
2308 ble_att_svr_prep_free(prep_entry);
2309
2310 /* XXX: We need to differentiate between "prepare queue full" and
2311 * "insufficient resources." Currently, we always indicate prepare
2312 * queue full.
2313 */
2314 *out_att_err = BLE_ATT_ERR_PREPARE_QUEUE_FULL;
2315 return rc;
2316 }
2317
2318 prep_prev = ble_att_svr_prep_find_prev(&conn->bhc_att_svr,
2319 handle, offset);
2320 if (prep_prev == NULL) {
2321 SLIST_INSERT_HEAD(&conn->bhc_att_svr.basc_prep_list, prep_entry,
2322 bape_next);
2323 } else {
2324 SLIST_INSERT_AFTER(prep_prev, prep_entry, bape_next);
2325 }
2326
2327 #if BLE_HS_ATT_SVR_QUEUED_WRITE_TMO != 0
2328 conn->bhc_att_svr.basc_prep_timeout_at =
2329 ble_npl_time_get() + BLE_HS_ATT_SVR_QUEUED_WRITE_TMO;
2330
2331 ble_hs_timer_resched();
2332 #endif
2333
2334 return 0;
2335 }
2336
2337 int
ble_att_svr_rx_prep_write(uint16_t conn_handle,struct os_mbuf ** rxom)2338 ble_att_svr_rx_prep_write(uint16_t conn_handle, struct os_mbuf **rxom)
2339 {
2340 #if !MYNEWT_VAL(BLE_ATT_SVR_QUEUED_WRITE)
2341 return BLE_HS_ENOTSUP;
2342 #endif
2343
2344 struct ble_att_prep_write_cmd *req;
2345 struct ble_att_svr_entry *attr_entry;
2346 struct os_mbuf *txom;
2347 uint16_t err_handle;
2348 uint8_t att_err;
2349 int rc;
2350
2351 /* Initialize some values in case of early error. */
2352 txom = NULL;
2353 att_err = 0;
2354 err_handle = 0;
2355
2356 rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
2357 if (rc != 0) {
2358 goto done;
2359 }
2360
2361 req = (struct ble_att_prep_write_cmd *)(*rxom)->om_data;
2362
2363 BLE_ATT_LOG_CMD(0, "prep write req", conn_handle,
2364 ble_att_prep_write_cmd_log, req);
2365
2366 err_handle = le16toh(req->bapc_handle);
2367
2368 attr_entry = ble_att_svr_find_by_handle(le16toh(req->bapc_handle));
2369
2370 /* A prepare write request gets rejected for the following reasons:
2371 * 1. Insufficient authorization.
2372 * 2. Insufficient authentication.
2373 * 3. Insufficient encryption key size (XXX: Not checked).
2374 * 4. Insufficient encryption (XXX: Not checked).
2375 * 5. Invalid handle.
2376 * 6. Write not permitted.
2377 */
2378
2379 /* <5> */
2380 if (attr_entry == NULL) {
2381 rc = BLE_HS_ENOENT;
2382 att_err = BLE_ATT_ERR_INVALID_HANDLE;
2383 goto done;
2384 }
2385
2386 /* <1>, <2>, <4>, <6> */
2387 rc = ble_att_svr_check_perms(conn_handle, 0, attr_entry, &att_err);
2388 if (rc != 0) {
2389 goto done;
2390 }
2391
2392 ble_hs_lock();
2393 rc = ble_att_svr_insert_prep_entry(conn_handle, le16toh(req->bapc_handle),
2394 le16toh(req->bapc_offset), *rxom,
2395 &att_err);
2396 ble_hs_unlock();
2397
2398 /* Reuse rxom for response. On success, the response is identical to
2399 * request except for op code. On error, the buffer contents will get
2400 * cleared before the error gets written.
2401 */
2402 txom = *rxom;
2403 *rxom = NULL;
2404
2405 if (rc != 0) {
2406 goto done;
2407 }
2408
2409 /* adjust for ATT header */
2410 os_mbuf_prepend(txom, 1);
2411 txom->om_data[0] = BLE_ATT_OP_PREP_WRITE_RSP;
2412
2413 BLE_ATT_LOG_CMD(1, "prep write rsp", conn_handle,
2414 ble_att_prep_write_cmd_log, req);
2415
2416 rc = 0;
2417
2418 done:
2419 rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_PREP_WRITE_REQ,
2420 att_err, err_handle);
2421 return rc;
2422 }
2423
2424 int
ble_att_svr_rx_exec_write(uint16_t conn_handle,struct os_mbuf ** rxom)2425 ble_att_svr_rx_exec_write(uint16_t conn_handle, struct os_mbuf **rxom)
2426 {
2427 #if !MYNEWT_VAL(BLE_ATT_SVR_QUEUED_WRITE)
2428 return BLE_HS_ENOTSUP;
2429 #endif
2430
2431 struct ble_att_prep_entry_list prep_list;
2432 struct ble_att_exec_write_req *req;
2433 struct ble_hs_conn *conn;
2434 struct os_mbuf *txom;
2435 uint16_t err_handle;
2436 uint8_t att_err;
2437 uint8_t flags;
2438 int rc;
2439
2440 /* Initialize some values in case of early error. */
2441 txom = NULL;
2442 err_handle = 0;
2443
2444 rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
2445 if (rc != 0) {
2446 flags = 0;
2447 goto done;
2448 }
2449
2450 req = (struct ble_att_exec_write_req *)(*rxom)->om_data;
2451 BLE_ATT_LOG_CMD(0, "exec write req", conn_handle,
2452 ble_att_exec_write_req_log, req);
2453
2454 flags = req->baeq_flags;
2455
2456 /* Just reuse the request buffer for the response. */
2457 txom = *rxom;
2458 *rxom = NULL;
2459 os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
2460
2461 if (ble_att_cmd_prepare(BLE_ATT_OP_EXEC_WRITE_RSP, 0, txom) == NULL) {
2462 att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
2463 rc = BLE_HS_ENOMEM;
2464 goto done;
2465 }
2466
2467 rc = 0;
2468
2469 done:
2470 if (rc == 0) {
2471 ble_hs_lock();
2472 conn = ble_hs_conn_find_assert(conn_handle);
2473
2474 /* Extract the list of prepared writes from the connection so
2475 * that they can be processed after the mutex is unlocked. They
2476 * aren't processed now because attribute writes involve executing
2477 * an application callback.
2478 */
2479 prep_list = conn->bhc_att_svr.basc_prep_list;
2480 SLIST_INIT(&conn->bhc_att_svr.basc_prep_list);
2481 ble_hs_unlock();
2482
2483 if (flags) {
2484 /* Perform attribute writes. */
2485 att_err = ble_att_svr_prep_write(conn_handle, &prep_list,
2486 &err_handle);
2487 if (att_err != 0) {
2488 rc = BLE_HS_EAPP;
2489 }
2490 }
2491
2492 /* Free the prep entries. */
2493 ble_att_svr_prep_clear(&prep_list);
2494 }
2495
2496 rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_EXEC_WRITE_REQ,
2497 att_err, err_handle);
2498 return rc;
2499 }
2500
2501 int
ble_att_svr_rx_notify(uint16_t conn_handle,struct os_mbuf ** rxom)2502 ble_att_svr_rx_notify(uint16_t conn_handle, struct os_mbuf **rxom)
2503 {
2504 #if !MYNEWT_VAL(BLE_ATT_SVR_NOTIFY)
2505 return BLE_HS_ENOTSUP;
2506 #endif
2507
2508 struct ble_att_notify_req *req;
2509 uint16_t handle;
2510 int rc;
2511
2512 rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), NULL);
2513 if (rc != 0) {
2514 return BLE_HS_ENOMEM;
2515 }
2516
2517 req = (struct ble_att_notify_req *)(*rxom)->om_data;
2518
2519 BLE_ATT_LOG_CMD(0, "notify req", conn_handle,
2520 ble_att_notify_req_log, req);
2521
2522 handle = le16toh(req->banq_handle);
2523
2524 if (handle == 0) {
2525 return BLE_HS_EBADDATA;
2526 }
2527
2528 /* Strip the request base from the front of the mbuf. */
2529 os_mbuf_adj(*rxom, sizeof(*req));
2530
2531 ble_gap_notify_rx_event(conn_handle, handle, *rxom, 0);
2532 *rxom = NULL;
2533
2534 return 0;
2535 }
2536
2537 /**
2538 * @return 0 on success; nonzero on failure.
2539 */
2540 static int
ble_att_svr_build_indicate_rsp(struct os_mbuf ** rxom,struct os_mbuf ** out_txom,uint8_t * out_att_err)2541 ble_att_svr_build_indicate_rsp(struct os_mbuf **rxom,
2542 struct os_mbuf **out_txom, uint8_t *out_att_err)
2543 {
2544 struct os_mbuf *txom;
2545 int rc;
2546
2547 /* Allocate a new buffer for the response. An indicate response never
2548 * reuses the request buffer. See the note at the top of this file for
2549 * details.
2550 */
2551 rc = ble_att_svr_pkt(rxom, &txom, out_att_err);
2552 if (rc != 0) {
2553 goto done;
2554 }
2555
2556 if (ble_att_cmd_prepare(BLE_ATT_OP_INDICATE_RSP, 0, txom) == NULL) {
2557 rc = BLE_HS_ENOMEM;
2558 *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
2559 goto done;
2560 }
2561
2562 rc = 0;
2563
2564 done:
2565 *out_txom = txom;
2566 return rc;
2567 }
2568
2569 int
ble_att_svr_rx_indicate(uint16_t conn_handle,struct os_mbuf ** rxom)2570 ble_att_svr_rx_indicate(uint16_t conn_handle, struct os_mbuf **rxom)
2571 {
2572 #if !MYNEWT_VAL(BLE_ATT_SVR_INDICATE)
2573 return BLE_HS_ENOTSUP;
2574 #endif
2575
2576 struct ble_att_indicate_req *req;
2577 struct os_mbuf *txom;
2578 uint16_t handle;
2579 uint8_t att_err;
2580 int rc;
2581
2582 /* Initialize some values in case of early error. */
2583 txom = NULL;
2584 att_err = 0;
2585 handle = 0;
2586
2587 rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), NULL);
2588 if (rc != 0) {
2589 goto done;
2590 }
2591
2592 req = (struct ble_att_indicate_req *)(*rxom)->om_data;
2593 BLE_ATT_LOG_CMD(0, "indicate req", conn_handle,
2594 ble_att_indicate_req_log, req);
2595
2596 handle = le16toh(req->baiq_handle);
2597
2598 if (handle == 0) {
2599 rc = BLE_HS_EBADDATA;
2600 goto done;
2601 }
2602
2603 /* Allocate the indicate response. This must be done prior to processing
2604 * the request. See the note at the top of this file for details.
2605 */
2606 rc = ble_att_svr_build_indicate_rsp(rxom, &txom, &att_err);
2607 if (rc != 0) {
2608 goto done;
2609 }
2610
2611 /* Strip the request base from the front of the mbuf. */
2612 os_mbuf_adj(*rxom, sizeof(*req));
2613
2614 ble_gap_notify_rx_event(conn_handle, handle, *rxom, 1);
2615 *rxom = NULL;
2616
2617 BLE_ATT_LOG_EMPTY_CMD(1, "indicate rsp", conn_handle);
2618
2619 rc = 0;
2620
2621 done:
2622 rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_INDICATE_REQ,
2623 att_err, handle);
2624 return rc;
2625 }
2626
2627 static void
ble_att_svr_move_entries(struct ble_att_svr_entry_list * src,struct ble_att_svr_entry_list * dst,uint16_t start_handle,uint16_t end_handle)2628 ble_att_svr_move_entries(struct ble_att_svr_entry_list *src,
2629 struct ble_att_svr_entry_list *dst,
2630 uint16_t start_handle, uint16_t end_handle)
2631 {
2632
2633 struct ble_att_svr_entry *entry;
2634 struct ble_att_svr_entry *prev;
2635 struct ble_att_svr_entry *remove;
2636 struct ble_att_svr_entry *insert;
2637
2638 /* Find first matching element to move */
2639 remove = NULL;
2640 entry = STAILQ_FIRST(src);
2641 while (entry && entry->ha_handle_id < start_handle) {
2642 remove = entry;
2643 entry = STAILQ_NEXT(entry, ha_next);
2644 }
2645
2646 /* Nothing to remove? */
2647 if (!entry) {
2648 return;
2649 }
2650
2651 /* Find element after which we'll put moved elements */
2652 prev = NULL;
2653 insert = STAILQ_FIRST(dst);
2654 while (insert && insert->ha_handle_id < start_handle) {
2655 prev = insert;
2656 insert = STAILQ_NEXT(insert, ha_next);
2657 }
2658 insert = prev;
2659
2660 /* Move elements */
2661 while (entry && entry->ha_handle_id <= end_handle) {
2662 /* Remove either from head or after prev (which is current one) */
2663 if (remove == NULL) {
2664 STAILQ_REMOVE_HEAD(src, ha_next);
2665 } else {
2666 STAILQ_REMOVE_AFTER(src, remove, ha_next);
2667 }
2668
2669 /* Insert current element */
2670 if (insert == NULL) {
2671 STAILQ_INSERT_HEAD(dst, entry, ha_next);
2672 insert = STAILQ_FIRST(dst);
2673 } else {
2674 STAILQ_INSERT_AFTER(dst, insert, entry, ha_next);
2675 insert = entry;
2676 }
2677
2678 /* Calculate next candidate to remove */
2679 if (remove == NULL) {
2680 entry = STAILQ_FIRST(src);
2681 } else {
2682 entry = STAILQ_NEXT(remove, ha_next);
2683 }
2684 }
2685 }
2686
2687 void
ble_att_svr_hide_range(uint16_t start_handle,uint16_t end_handle)2688 ble_att_svr_hide_range(uint16_t start_handle, uint16_t end_handle)
2689 {
2690 ble_att_svr_move_entries(&ble_att_svr_list, &ble_att_svr_hidden_list,
2691 start_handle, end_handle);
2692 }
2693
2694 void
ble_att_svr_restore_range(uint16_t start_handle,uint16_t end_handle)2695 ble_att_svr_restore_range(uint16_t start_handle, uint16_t end_handle)
2696 {
2697 ble_att_svr_move_entries(&ble_att_svr_hidden_list, &ble_att_svr_list,
2698 start_handle, end_handle);
2699 }
2700
2701 void
ble_att_svr_reset(void)2702 ble_att_svr_reset(void)
2703 {
2704 struct ble_att_svr_entry *entry;
2705
2706 while ((entry = STAILQ_FIRST(&ble_att_svr_list)) != NULL) {
2707 STAILQ_REMOVE_HEAD(&ble_att_svr_list, ha_next);
2708 ble_att_svr_entry_free(entry);
2709 }
2710
2711 while ((entry = STAILQ_FIRST(&ble_att_svr_hidden_list)) != NULL) {
2712 STAILQ_REMOVE_HEAD(&ble_att_svr_hidden_list, ha_next);
2713 ble_att_svr_entry_free(entry);
2714 }
2715
2716 /* Note: prep entries do not get freed here because it is assumed there are
2717 * no established connections.
2718 */
2719 }
2720
2721 static void
ble_att_svr_free_start_mem(void)2722 ble_att_svr_free_start_mem(void)
2723 {
2724 free(ble_att_svr_entry_mem);
2725 ble_att_svr_entry_mem = NULL;
2726 }
2727
2728 int
ble_att_svr_start(void)2729 ble_att_svr_start(void)
2730 {
2731 int rc;
2732
2733 ble_att_svr_free_start_mem();
2734
2735 if (ble_hs_max_attrs > 0) {
2736 ble_att_svr_entry_mem = malloc(
2737 OS_MEMPOOL_BYTES(ble_hs_max_attrs,
2738 sizeof (struct ble_att_svr_entry)));
2739 if (ble_att_svr_entry_mem == NULL) {
2740 rc = BLE_HS_ENOMEM;
2741 goto err;
2742 }
2743
2744 rc = os_mempool_init(&ble_att_svr_entry_pool, ble_hs_max_attrs,
2745 sizeof (struct ble_att_svr_entry),
2746 ble_att_svr_entry_mem, "ble_att_svr_entry_pool");
2747 if (rc != 0) {
2748 rc = BLE_HS_EOS;
2749 goto err;
2750 }
2751 }
2752
2753 return 0;
2754
2755 err:
2756 ble_att_svr_free_start_mem();
2757 return rc;
2758 }
2759
2760 int
ble_att_svr_init(void)2761 ble_att_svr_init(void)
2762 {
2763 int rc;
2764
2765 if (MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES) > 0) {
2766 rc = os_mempool_init(&ble_att_svr_prep_entry_pool,
2767 MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES),
2768 sizeof (struct ble_att_prep_entry),
2769 ble_att_svr_prep_entry_mem,
2770 "ble_att_svr_prep_entry_pool");
2771 if (rc != 0) {
2772 return BLE_HS_EOS;
2773 }
2774 }
2775
2776 STAILQ_INIT(&ble_att_svr_list);
2777 STAILQ_INIT(&ble_att_svr_hidden_list);
2778
2779 ble_att_svr_id = 0;
2780
2781 return 0;
2782 }
2783