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