xref: /nrf52832-nimble/packages/NimBLE-latest/nimble/host/test/src/ble_hs_test_util.c (revision 042d53a763ad75cb1465103098bb88c245d95138)
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 <string.h>
21 #include <errno.h>
22 #include "sysinit/sysinit.h"
23 #include "stats/stats.h"
24 #include "testutil/testutil.h"
25 #include "nimble/ble.h"
26 #include "nimble/hci_common.h"
27 #include "nimble/ble_hci_trans.h"
28 #include "host/ble_hs_adv.h"
29 #include "host/ble_hs_id.h"
30 #include "store/config/ble_store_config.h"
31 #include "transport/ram/ble_hci_ram.h"
32 #include "ble_hs_test_util.h"
33 
34 /* Our global device address. */
35 uint8_t g_dev_addr[BLE_DEV_ADDR_LEN];
36 
37 static STAILQ_HEAD(, os_mbuf_pkthdr) ble_hs_test_util_prev_tx_queue;
38 struct os_mbuf *ble_hs_test_util_prev_tx_cur;
39 
40 int ble_sm_test_store_obj_type;
41 union ble_store_key ble_sm_test_store_key;
42 union ble_store_value ble_sm_test_store_value;
43 
44 const struct ble_gap_adv_params ble_hs_test_util_adv_params = {
45     .conn_mode = BLE_GAP_CONN_MODE_UND,
46     .disc_mode = BLE_GAP_DISC_MODE_GEN,
47 
48     .itvl_min = 0,
49     .itvl_max = 0,
50     .channel_map = 0,
51     .filter_policy = 0,
52     .high_duty_cycle = 0,
53 };
54 
55 void
ble_hs_test_util_prev_tx_enqueue(struct os_mbuf * om)56 ble_hs_test_util_prev_tx_enqueue(struct os_mbuf *om)
57 {
58     struct os_mbuf_pkthdr *omp;
59 
60     assert(OS_MBUF_IS_PKTHDR(om));
61 
62     omp = OS_MBUF_PKTHDR(om);
63     if (STAILQ_EMPTY(&ble_hs_test_util_prev_tx_queue)) {
64         STAILQ_INSERT_HEAD(&ble_hs_test_util_prev_tx_queue, omp, omp_next);
65     } else {
66         STAILQ_INSERT_TAIL(&ble_hs_test_util_prev_tx_queue, omp, omp_next);
67     }
68 }
69 
70 static struct os_mbuf *
ble_hs_test_util_prev_tx_dequeue_once(struct hci_data_hdr * out_hci_hdr)71 ble_hs_test_util_prev_tx_dequeue_once(struct hci_data_hdr *out_hci_hdr)
72 {
73     struct os_mbuf_pkthdr *omp;
74     struct os_mbuf *om;
75     int rc;
76 
77     omp = STAILQ_FIRST(&ble_hs_test_util_prev_tx_queue);
78     if (omp == NULL) {
79         return NULL;
80     }
81     STAILQ_REMOVE_HEAD(&ble_hs_test_util_prev_tx_queue, omp_next);
82 
83     om = OS_MBUF_PKTHDR_TO_MBUF(omp);
84 
85     rc = ble_hs_hci_util_data_hdr_strip(om, out_hci_hdr);
86     TEST_ASSERT_FATAL(rc == 0);
87     TEST_ASSERT_FATAL(out_hci_hdr->hdh_len == OS_MBUF_PKTLEN(om));
88 
89     return om;
90 }
91 
92 struct os_mbuf *
ble_hs_test_util_prev_tx_dequeue(void)93 ble_hs_test_util_prev_tx_dequeue(void)
94 {
95     struct ble_l2cap_hdr l2cap_hdr;
96     struct hci_data_hdr hci_hdr;
97     struct os_mbuf *om;
98     uint8_t pb;
99     int rc;
100 
101     rc = os_mbuf_free_chain(ble_hs_test_util_prev_tx_cur);
102     TEST_ASSERT_FATAL(rc == 0);
103 
104     om = ble_hs_test_util_prev_tx_dequeue_once(&hci_hdr);
105     if (om != NULL) {
106         pb = BLE_HCI_DATA_PB(hci_hdr.hdh_handle_pb_bc);
107         TEST_ASSERT_FATAL(pb == BLE_HCI_PB_FIRST_NON_FLUSH);
108 
109         rc = ble_l2cap_parse_hdr(om, 0, &l2cap_hdr);
110         TEST_ASSERT_FATAL(rc == 0);
111 
112         os_mbuf_adj(om, BLE_L2CAP_HDR_SZ);
113 
114         ble_hs_test_util_prev_tx_cur = om;
115         while (OS_MBUF_PKTLEN(ble_hs_test_util_prev_tx_cur) <
116                l2cap_hdr.len) {
117 
118             om = ble_hs_test_util_prev_tx_dequeue_once(&hci_hdr);
119             TEST_ASSERT_FATAL(om != NULL);
120 
121             pb = BLE_HCI_DATA_PB(hci_hdr.hdh_handle_pb_bc);
122             TEST_ASSERT_FATAL(pb == BLE_HCI_PB_MIDDLE);
123 
124             os_mbuf_concat(ble_hs_test_util_prev_tx_cur, om);
125         }
126     } else {
127         ble_hs_test_util_prev_tx_cur = NULL;
128     }
129 
130     return ble_hs_test_util_prev_tx_cur;
131 }
132 
133 struct os_mbuf *
ble_hs_test_util_prev_tx_dequeue_pullup(void)134 ble_hs_test_util_prev_tx_dequeue_pullup(void)
135 {
136     struct os_mbuf *om;
137 
138     om = ble_hs_test_util_prev_tx_dequeue();
139     if (om != NULL) {
140         om = os_mbuf_pullup(om, OS_MBUF_PKTLEN(om));
141         TEST_ASSERT_FATAL(om != NULL);
142         ble_hs_test_util_prev_tx_cur = om;
143     }
144 
145     return om;
146 }
147 
148 int
ble_hs_test_util_prev_tx_queue_sz(void)149 ble_hs_test_util_prev_tx_queue_sz(void)
150 {
151     struct os_mbuf_pkthdr *omp;
152     int cnt;
153 
154     cnt = 0;
155     STAILQ_FOREACH(omp, &ble_hs_test_util_prev_tx_queue, omp_next) {
156         cnt++;
157     }
158 
159     return cnt;
160 }
161 
162 void
ble_hs_test_util_prev_tx_queue_clear(void)163 ble_hs_test_util_prev_tx_queue_clear(void)
164 {
165     while (!STAILQ_EMPTY(&ble_hs_test_util_prev_tx_queue)) {
166         ble_hs_test_util_prev_tx_dequeue();
167     }
168 }
169 
170 static void
ble_hs_test_util_conn_params_dflt(struct ble_gap_conn_params * conn_params)171 ble_hs_test_util_conn_params_dflt(struct ble_gap_conn_params *conn_params)
172 {
173     conn_params->scan_itvl = 0x0010;
174     conn_params->scan_window = 0x0010;
175     conn_params->itvl_min = BLE_GAP_INITIAL_CONN_ITVL_MIN;
176     conn_params->itvl_max = BLE_GAP_INITIAL_CONN_ITVL_MAX;
177     conn_params->latency = BLE_GAP_INITIAL_CONN_LATENCY;
178     conn_params->supervision_timeout = BLE_GAP_INITIAL_SUPERVISION_TIMEOUT;
179     conn_params->min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN;
180     conn_params->max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN;
181 }
182 
183 static void
ble_hs_test_util_hcc_from_conn_params(struct hci_create_conn * hcc,uint8_t own_addr_type,const ble_addr_t * peer_addr,const struct ble_gap_conn_params * conn_params)184 ble_hs_test_util_hcc_from_conn_params(
185     struct hci_create_conn *hcc, uint8_t own_addr_type,
186     const ble_addr_t *peer_addr, const struct ble_gap_conn_params *conn_params)
187 {
188     hcc->scan_itvl = conn_params->scan_itvl;
189     hcc->scan_window = conn_params->scan_window;
190 
191     if (peer_addr == NULL) {
192         hcc->filter_policy = BLE_HCI_CONN_FILT_USE_WL;
193         hcc->peer_addr_type = 0;
194         memset(hcc->peer_addr, 0, 6);
195     } else {
196         hcc->filter_policy = BLE_HCI_CONN_FILT_NO_WL;
197         hcc->peer_addr_type = peer_addr->type;
198         memcpy(hcc->peer_addr, peer_addr->val, 6);
199     }
200     hcc->own_addr_type = own_addr_type;
201     hcc->conn_itvl_min = conn_params->itvl_min;
202     hcc->conn_itvl_max = conn_params->itvl_max;
203     hcc->conn_latency = conn_params->latency;
204     hcc->supervision_timeout = conn_params->supervision_timeout;
205     hcc->min_ce_len = conn_params->min_ce_len;
206     hcc->max_ce_len = conn_params->max_ce_len;
207 }
208 
209 void
ble_hs_test_util_create_rpa_conn(uint16_t handle,uint8_t own_addr_type,const uint8_t * our_rpa,uint8_t peer_addr_type,const uint8_t * peer_id_addr,const uint8_t * peer_rpa,uint8_t conn_features,ble_gap_event_fn * cb,void * cb_arg)210 ble_hs_test_util_create_rpa_conn(uint16_t handle, uint8_t own_addr_type,
211                                  const uint8_t *our_rpa,
212                                  uint8_t peer_addr_type,
213                                  const uint8_t *peer_id_addr,
214                                  const uint8_t *peer_rpa,
215                                  uint8_t conn_features,
216                                  ble_gap_event_fn *cb, void *cb_arg)
217 {
218     ble_addr_t addr;
219     struct hci_le_conn_complete evt;
220     struct hci_le_rd_rem_supp_feat_complete evt2;
221     int rc;
222 
223     addr.type = peer_addr_type;
224     memcpy(addr.val, peer_id_addr, 6);
225 
226     ble_hs_test_util_connect(own_addr_type, &addr, 0, NULL, cb, cb_arg, 0);
227 
228     /* ble_gap_rx_conn_complete() will send extra HCI command, need phony ack */
229     ble_hs_test_util_hci_ack_set(ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
230                              BLE_HCI_OCF_LE_RD_REM_FEAT), 0);
231 
232     memset(&evt, 0, sizeof evt);
233     evt.subevent_code = BLE_HCI_LE_SUBEV_CONN_COMPLETE;
234     evt.status = BLE_ERR_SUCCESS;
235     evt.connection_handle = handle;
236     evt.role = BLE_HCI_LE_CONN_COMPLETE_ROLE_MASTER;
237     evt.peer_addr_type = peer_addr_type;
238     memcpy(evt.peer_addr, peer_id_addr, 6);
239     evt.conn_itvl = BLE_GAP_INITIAL_CONN_ITVL_MAX;
240     evt.conn_latency = BLE_GAP_INITIAL_CONN_LATENCY;
241     evt.supervision_timeout = BLE_GAP_INITIAL_SUPERVISION_TIMEOUT;
242     memcpy(evt.local_rpa, our_rpa, 6);
243     memcpy(evt.peer_rpa, peer_rpa, 6);
244 
245     rc = ble_gap_rx_conn_complete(&evt, 0);
246     TEST_ASSERT(rc == 0);
247 
248     evt2.subevent_code = BLE_HCI_LE_SUBEV_RD_REM_USED_FEAT;
249     evt2.status = BLE_ERR_SUCCESS;
250     evt2.connection_handle = handle;
251     memcpy(evt2.features, ((uint8_t[]){ conn_features, 0, 0, 0, 0, 0, 0, 0 }),
252            8);
253 
254     ble_gap_rx_rd_rem_sup_feat_complete(&evt2);
255 
256     ble_hs_test_util_hci_out_clear();
257 }
258 
259 void
ble_hs_test_util_create_conn(uint16_t handle,const uint8_t * peer_id_addr,ble_gap_event_fn * cb,void * cb_arg)260 ble_hs_test_util_create_conn(uint16_t handle, const uint8_t *peer_id_addr,
261                              ble_gap_event_fn *cb, void *cb_arg)
262 {
263     static uint8_t null_addr[6];
264 
265     ble_hs_test_util_create_rpa_conn(handle, BLE_OWN_ADDR_PUBLIC, null_addr,
266                                      BLE_ADDR_PUBLIC, peer_id_addr,
267                                      null_addr, BLE_HS_TEST_CONN_FEAT_ALL,
268                                      cb, cb_arg);
269 }
270 
271 void
ble_hs_test_util_create_conn_feat(uint16_t handle,const uint8_t * peer_id_addr,uint8_t conn_features,ble_gap_event_fn * cb,void * cb_arg)272 ble_hs_test_util_create_conn_feat(uint16_t handle, const uint8_t *peer_id_addr,
273                                   uint8_t conn_features, ble_gap_event_fn *cb,
274                                   void *cb_arg)
275 {
276     static uint8_t null_addr[6];
277 
278     ble_hs_test_util_create_rpa_conn(handle, BLE_OWN_ADDR_PUBLIC, null_addr,
279                                      BLE_ADDR_PUBLIC, peer_id_addr,
280                                      null_addr, conn_features, cb, cb_arg);
281 }
282 
283 int
ble_hs_test_util_connect(uint8_t own_addr_type,const ble_addr_t * peer_addr,int32_t duration_ms,const struct ble_gap_conn_params * params,ble_gap_event_fn * cb,void * cb_arg,uint8_t ack_status)284 ble_hs_test_util_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr,
285                          int32_t duration_ms,
286                          const struct ble_gap_conn_params *params,
287                          ble_gap_event_fn *cb, void *cb_arg,
288                          uint8_t ack_status)
289 {
290     struct ble_gap_conn_params dflt_params;
291     struct hci_create_conn hcc;
292     int rc;
293 
294     /* This function ensures the most recently sent HCI command is the expected
295      * create connection command.  If the current test case has unverified HCI
296      * commands, assume we are not interested in them and clear the queue.
297      */
298     ble_hs_test_util_hci_out_clear();
299 
300     ble_hs_test_util_hci_ack_set(
301         ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
302                                     BLE_HCI_OCF_LE_CREATE_CONN),
303         ack_status);
304 
305     rc = ble_gap_connect(own_addr_type, peer_addr, duration_ms, params, cb,
306                          cb_arg);
307     if (ack_status != 0) {
308         TEST_ASSERT(rc == BLE_HS_HCI_ERR(ack_status));
309     } else if (rc != 0) {
310         return rc;
311     }
312 
313     if (params == NULL) {
314         ble_hs_test_util_conn_params_dflt(&dflt_params);
315         params = &dflt_params;
316     }
317 
318     ble_hs_test_util_hcc_from_conn_params(&hcc, own_addr_type, peer_addr,
319                                           params);
320     ble_hs_test_util_hci_verify_tx_create_conn(&hcc);
321 
322     return rc;
323 }
324 
325 int
ble_hs_test_util_conn_cancel(uint8_t ack_status)326 ble_hs_test_util_conn_cancel(uint8_t ack_status)
327 {
328     int rc;
329 
330     ble_hs_test_util_hci_ack_set(
331         ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
332                                     BLE_HCI_OCF_LE_CREATE_CONN_CANCEL),
333         ack_status);
334 
335     rc = ble_gap_conn_cancel();
336     return rc;
337 }
338 
339 void
ble_hs_test_util_rx_conn_cancel_evt(void)340 ble_hs_test_util_rx_conn_cancel_evt(void)
341 {
342     ble_hs_test_util_conn_cancel(0);
343     ble_hs_test_util_hci_rx_conn_cancel_evt();
344 }
345 
346 void
ble_hs_test_util_conn_cancel_full(void)347 ble_hs_test_util_conn_cancel_full(void)
348 {
349     ble_hs_test_util_conn_cancel(0);
350     ble_hs_test_util_rx_conn_cancel_evt();
351 }
352 
353 int
ble_hs_test_util_conn_terminate(uint16_t conn_handle,uint8_t hci_status)354 ble_hs_test_util_conn_terminate(uint16_t conn_handle, uint8_t hci_status)
355 {
356     int rc;
357 
358     ble_hs_test_util_hci_ack_set_disconnect(hci_status);
359     rc = ble_gap_terminate(conn_handle, BLE_ERR_REM_USER_CONN_TERM);
360     return rc;
361 }
362 
363 void
ble_hs_test_util_conn_disconnect(uint16_t conn_handle)364 ble_hs_test_util_conn_disconnect(uint16_t conn_handle)
365 {
366     struct hci_disconn_complete evt;
367     int rc;
368 
369     rc = ble_hs_test_util_conn_terminate(conn_handle, 0);
370     TEST_ASSERT_FATAL(rc == 0);
371 
372     /* Receive disconnection complete event. */
373     evt.connection_handle = conn_handle;
374     evt.status = 0;
375     evt.reason = BLE_ERR_CONN_TERM_LOCAL;
376     ble_hs_test_util_hci_rx_disconn_complete_event(&evt);
377 }
378 
379 int
ble_hs_test_util_disc(uint8_t own_addr_type,int32_t duration_ms,const struct ble_gap_disc_params * disc_params,ble_gap_event_fn * cb,void * cb_arg,int fail_idx,uint8_t fail_status)380 ble_hs_test_util_disc(uint8_t own_addr_type, int32_t duration_ms,
381                       const struct ble_gap_disc_params *disc_params,
382                       ble_gap_event_fn *cb, void *cb_arg, int fail_idx,
383                       uint8_t fail_status)
384 {
385     int rc;
386 
387     ble_hs_test_util_hci_ack_set_disc(own_addr_type, fail_idx, fail_status);
388     rc = ble_gap_disc(own_addr_type, duration_ms, disc_params,
389                       cb, cb_arg);
390     return rc;
391 }
392 
393 int
ble_hs_test_util_disc_cancel(uint8_t ack_status)394 ble_hs_test_util_disc_cancel(uint8_t ack_status)
395 {
396     int rc;
397 
398     ble_hs_test_util_hci_ack_set(
399         ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
400                                     BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
401         ack_status);
402 
403     rc = ble_gap_disc_cancel();
404     return rc;
405 }
406 
407 static void
ble_hs_test_util_verify_tx_rd_pwr(void)408 ble_hs_test_util_verify_tx_rd_pwr(void)
409 {
410     uint8_t param_len;
411 
412     ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
413                                    BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR,
414                                    &param_len);
415     TEST_ASSERT(param_len == 0);
416 }
417 
418 int
ble_hs_test_util_adv_set_fields(const struct ble_hs_adv_fields * adv_fields,int cmd_fail_idx,uint8_t hci_status)419 ble_hs_test_util_adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
420                                 int cmd_fail_idx, uint8_t hci_status)
421 {
422     struct ble_hs_test_util_hci_ack acks[3];
423     int auto_pwr;
424     int rc;
425     int i;
426 
427     auto_pwr = adv_fields->tx_pwr_lvl_is_present &&
428                adv_fields->tx_pwr_lvl == BLE_HS_ADV_TX_PWR_LVL_AUTO;
429 
430     i = 0;
431     if (auto_pwr) {
432         acks[i] = (struct ble_hs_test_util_hci_ack) {
433             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR),
434             ble_hs_test_util_hci_misc_exp_status(i, cmd_fail_idx, hci_status),
435             {0},
436             1,
437         };
438         i++;
439     }
440 
441     acks[i] = (struct ble_hs_test_util_hci_ack) {
442         BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_DATA),
443         ble_hs_test_util_hci_misc_exp_status(i, cmd_fail_idx, hci_status),
444     };
445     i++;
446 
447     memset(acks + i, 0, sizeof acks[i]);
448     ble_hs_test_util_hci_ack_set_seq(acks);
449 
450     rc = ble_gap_adv_set_fields(adv_fields);
451     if (rc == 0 && auto_pwr) {
452         /* Verify tx of set advertising params command. */
453         ble_hs_test_util_verify_tx_rd_pwr();
454     }
455 
456     return rc;
457 }
458 
459 int
ble_hs_test_util_adv_rsp_set_fields(const struct ble_hs_adv_fields * adv_fields,int cmd_fail_idx,uint8_t hci_status)460 ble_hs_test_util_adv_rsp_set_fields(const struct ble_hs_adv_fields *adv_fields,
461                                     int cmd_fail_idx, uint8_t hci_status)
462 {
463     struct ble_hs_test_util_hci_ack acks[3];
464     int auto_pwr;
465     int rc;
466     int i;
467 
468     auto_pwr = adv_fields->tx_pwr_lvl_is_present &&
469                adv_fields->tx_pwr_lvl == BLE_HS_ADV_TX_PWR_LVL_AUTO;
470 
471     i = 0;
472     if (auto_pwr) {
473         acks[i] = (struct ble_hs_test_util_hci_ack) {
474             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR),
475             ble_hs_test_util_hci_misc_exp_status(i, cmd_fail_idx, hci_status),
476             {0},
477             1,
478         };
479         i++;
480     }
481 
482     acks[i] = (struct ble_hs_test_util_hci_ack) {
483         BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA),
484         ble_hs_test_util_hci_misc_exp_status(i, cmd_fail_idx, hci_status),
485     };
486     i++;
487 
488     memset(acks + i, 0, sizeof acks[i]);
489     ble_hs_test_util_hci_ack_set_seq(acks);
490 
491     rc = ble_gap_adv_rsp_set_fields(adv_fields);
492     if (rc == 0 && auto_pwr) {
493         /* Verify tx of set advertising params command. */
494         ble_hs_test_util_verify_tx_rd_pwr();
495     }
496 
497     return rc;
498 }
499 
500 int
ble_hs_test_util_adv_start(uint8_t own_addr_type,const ble_addr_t * peer_addr,const struct ble_gap_adv_params * adv_params,int32_t duration_ms,ble_gap_event_fn * cb,void * cb_arg,int fail_idx,uint8_t fail_status)501 ble_hs_test_util_adv_start(uint8_t own_addr_type, const ble_addr_t *peer_addr,
502                            const struct ble_gap_adv_params *adv_params,
503                            int32_t duration_ms,
504                            ble_gap_event_fn *cb, void *cb_arg,
505                            int fail_idx, uint8_t fail_status)
506 {
507     struct ble_hs_test_util_hci_ack acks[6];
508     int rc;
509     int i;
510 
511     i = 0;
512 
513     acks[i] = (struct ble_hs_test_util_hci_ack) {
514         BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_PARAMS),
515         fail_idx == i ? fail_status : 0,
516     };
517     i++;
518 
519     acks[i] = (struct ble_hs_test_util_hci_ack) {
520         BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_ENABLE),
521         ble_hs_test_util_hci_misc_exp_status(i, fail_idx, fail_status),
522     };
523     i++;
524 
525     memset(acks + i, 0, sizeof acks[i]);
526 
527     ble_hs_test_util_hci_ack_set_seq(acks);
528 
529     rc = ble_gap_adv_start(own_addr_type, peer_addr,
530                            duration_ms, adv_params, cb, cb_arg);
531 
532     return rc;
533 }
534 
535 int
ble_hs_test_util_adv_stop(uint8_t hci_status)536 ble_hs_test_util_adv_stop(uint8_t hci_status)
537 {
538     int rc;
539 
540     ble_hs_test_util_hci_ack_set(
541         BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_ENABLE),
542         hci_status);
543 
544     rc = ble_gap_adv_stop();
545     return rc;
546 }
547 
548 int
ble_hs_test_util_wl_set(ble_addr_t * addrs,uint8_t addrs_count,int fail_idx,uint8_t fail_status)549 ble_hs_test_util_wl_set(ble_addr_t *addrs, uint8_t addrs_count,
550                         int fail_idx, uint8_t fail_status)
551 {
552     struct ble_hs_test_util_hci_ack acks[64];
553     int cmd_idx;
554     int rc;
555     int i;
556 
557     TEST_ASSERT_FATAL(addrs_count < 63);
558 
559     cmd_idx = 0;
560     acks[cmd_idx] = (struct ble_hs_test_util_hci_ack) {
561         BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_CLEAR_WHITE_LIST),
562         ble_hs_test_util_hci_misc_exp_status(cmd_idx, fail_idx, fail_status),
563     };
564     cmd_idx++;
565 
566     for (i = 0; i < addrs_count; i++) {
567         acks[cmd_idx] = (struct ble_hs_test_util_hci_ack) {
568             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_ADD_WHITE_LIST),
569             ble_hs_test_util_hci_misc_exp_status(cmd_idx, fail_idx, fail_status),
570         };
571 
572         cmd_idx++;
573     }
574     memset(acks + cmd_idx, 0, sizeof acks[cmd_idx]);
575 
576     ble_hs_test_util_hci_ack_set_seq(acks);
577     rc = ble_gap_wl_set(addrs, addrs_count);
578     return rc;
579 }
580 
581 int
ble_hs_test_util_conn_update(uint16_t conn_handle,struct ble_gap_upd_params * params,uint8_t hci_status)582 ble_hs_test_util_conn_update(uint16_t conn_handle,
583                              struct ble_gap_upd_params *params,
584                              uint8_t hci_status)
585 {
586     int rc;
587 
588     /*
589      * 0xFF is magic value used for cases where we expect update over L2CAP to
590      * be triggered - in this case we don't need phony ack.
591      */
592     if (hci_status != 0xFF) {
593         ble_hs_test_util_hci_ack_set(
594                 BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_CONN_UPDATE),
595                 hci_status);
596     }
597 
598     rc = ble_gap_update_params(conn_handle, params);
599     return rc;
600 }
601 
602 int
ble_hs_test_util_set_our_irk(const uint8_t * irk,int fail_idx,uint8_t hci_status)603 ble_hs_test_util_set_our_irk(const uint8_t *irk, int fail_idx,
604                              uint8_t hci_status)
605 {
606     int rc;
607 
608     ble_hs_test_util_hci_ack_set_seq(((struct ble_hs_test_util_hci_ack[]) {
609         {
610             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
611             ble_hs_test_util_hci_misc_exp_status(0, fail_idx, hci_status),
612         },
613         {
614             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_CLR_RESOLV_LIST),
615             ble_hs_test_util_hci_misc_exp_status(1, fail_idx, hci_status),
616         },
617         {
618             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
619             ble_hs_test_util_hci_misc_exp_status(2, fail_idx, hci_status),
620         },
621         {
622             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_ENABLE),
623             ble_hs_test_util_hci_misc_exp_status(3, fail_idx, hci_status),
624         },
625         {
626             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_ADD_RESOLV_LIST),
627             ble_hs_test_util_hci_misc_exp_status(4, fail_idx, hci_status),
628         },
629         {
630             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_PRIVACY_MODE),
631             ble_hs_test_util_hci_misc_exp_status(5, fail_idx, hci_status),
632         },
633         {
634             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_PRIVACY_MODE),
635             ble_hs_test_util_hci_misc_exp_status(6, fail_idx, hci_status),
636         },
637         {
638             0
639         }
640     }));
641 
642     rc = ble_hs_pvcy_set_our_irk(irk);
643     return rc;
644 }
645 
646 int
ble_hs_test_util_security_initiate(uint16_t conn_handle,uint8_t hci_status)647 ble_hs_test_util_security_initiate(uint16_t conn_handle, uint8_t hci_status)
648 {
649     int rc;
650 
651     ble_hs_test_util_hci_ack_set(
652         BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_START_ENCRYPT), hci_status);
653 
654     rc = ble_gap_security_initiate(conn_handle);
655     return rc;
656 }
657 
658 int
ble_hs_test_util_l2cap_rx_first_frag(uint16_t conn_handle,uint16_t cid,struct hci_data_hdr * hci_hdr,struct os_mbuf * om)659 ble_hs_test_util_l2cap_rx_first_frag(uint16_t conn_handle, uint16_t cid,
660                                      struct hci_data_hdr *hci_hdr,
661                                      struct os_mbuf *om)
662 {
663     int rc;
664 
665     om = ble_l2cap_prepend_hdr(om, cid, OS_MBUF_PKTLEN(om));
666     TEST_ASSERT_FATAL(om != NULL);
667 
668     rc = ble_hs_test_util_l2cap_rx(conn_handle, hci_hdr, om);
669     return rc;
670 }
671 
672 int
ble_hs_test_util_l2cap_rx(uint16_t conn_handle,struct hci_data_hdr * hci_hdr,struct os_mbuf * om)673 ble_hs_test_util_l2cap_rx(uint16_t conn_handle,
674                           struct hci_data_hdr *hci_hdr,
675                           struct os_mbuf *om)
676 {
677     struct ble_hs_conn *conn;
678     ble_l2cap_rx_fn *rx_cb;
679     int reject_cid;
680     int rc;
681 
682     ble_hs_lock();
683 
684     conn = ble_hs_conn_find(conn_handle);
685     if (conn != NULL) {
686         rc = ble_l2cap_rx(conn, hci_hdr, om, &rx_cb, &reject_cid);
687     } else {
688         rc = os_mbuf_free_chain(om);
689         TEST_ASSERT_FATAL(rc == 0);
690     }
691 
692     ble_hs_unlock();
693 
694     if (conn == NULL) {
695         rc = BLE_HS_ENOTCONN;
696     } else if (rc == 0) {
697         TEST_ASSERT_FATAL(rx_cb != NULL);
698         rc = rx_cb(conn->bhc_rx_chan);
699         ble_l2cap_remove_rx(conn, conn->bhc_rx_chan);
700     } else if (rc == BLE_HS_EAGAIN) {
701         /* More fragments on the way. */
702         rc = 0;
703     } else {
704         if (reject_cid != -1) {
705             ble_l2cap_sig_reject_invalid_cid_tx(conn_handle, 0, 0, reject_cid);
706         }
707     }
708 
709     return rc;
710 }
711 
712 int
ble_hs_test_util_l2cap_rx_payload_flat(uint16_t conn_handle,uint16_t cid,const void * data,int len)713 ble_hs_test_util_l2cap_rx_payload_flat(uint16_t conn_handle, uint16_t cid,
714                                        const void *data, int len)
715 {
716     struct hci_data_hdr hci_hdr;
717     struct os_mbuf *om;
718     int rc;
719 
720     om = ble_hs_mbuf_l2cap_pkt();
721     TEST_ASSERT_FATAL(om != NULL);
722 
723     rc = os_mbuf_append(om, data, len);
724     TEST_ASSERT_FATAL(rc == 0);
725 
726     hci_hdr.hdh_handle_pb_bc =
727         ble_hs_hci_util_handle_pb_bc_join(conn_handle,
728                                           BLE_HCI_PB_FIRST_FLUSH, 0);
729     hci_hdr.hdh_len = OS_MBUF_PKTHDR(om)->omp_len;
730 
731     rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, cid, &hci_hdr, om);
732     return rc;
733 }
734 
735 void
ble_hs_test_util_set_att_mtu(uint16_t conn_handle,uint16_t mtu)736 ble_hs_test_util_set_att_mtu(uint16_t conn_handle, uint16_t mtu)
737 {
738     struct ble_l2cap_chan *chan;
739     struct ble_hs_conn *conn;
740     int rc;
741 
742     if (mtu <= BLE_ATT_MTU_DFLT) {
743         return;
744     }
745 
746     ble_hs_lock();
747 
748     rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
749     assert(rc == 0);
750     chan->my_mtu = mtu;
751     chan->peer_mtu = mtu;
752     chan->flags |= BLE_L2CAP_CHAN_F_TXED_MTU;
753 
754     ble_hs_unlock();
755 }
756 
757 int
ble_hs_test_util_rx_att_mtu_cmd(uint16_t conn_handle,int is_req,uint16_t mtu)758 ble_hs_test_util_rx_att_mtu_cmd(uint16_t conn_handle, int is_req, uint16_t mtu)
759 {
760     struct ble_att_mtu_cmd cmd;
761     uint8_t buf[BLE_ATT_MTU_CMD_SZ];
762     int rc;
763 
764     cmd.bamc_mtu = mtu;
765 
766     if (is_req) {
767         ble_att_mtu_req_write(buf, sizeof buf, &cmd);
768     } else {
769         ble_att_mtu_rsp_write(buf, sizeof buf, &cmd);
770     }
771 
772     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
773                                                 buf, sizeof buf);
774     return rc;
775 }
776 
777 int
ble_hs_test_util_rx_att_find_info_req(uint16_t conn_handle,uint16_t start_handle,uint16_t end_handle)778 ble_hs_test_util_rx_att_find_info_req(uint16_t conn_handle,
779                                       uint16_t start_handle,
780                                       uint16_t end_handle)
781 {
782     struct ble_att_find_info_req req;
783     uint8_t buf[BLE_ATT_FIND_INFO_REQ_SZ];
784     int rc;
785 
786     req.bafq_start_handle = start_handle;
787     req.bafq_end_handle = end_handle;
788 
789     ble_att_find_info_req_write(buf, sizeof buf, &req);
790 
791     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
792                                                 buf, sizeof buf);
793 
794     return rc;
795 }
796 
797 int
ble_hs_test_util_rx_att_find_type_value_req(uint16_t conn_handle,uint16_t start_handle,uint16_t end_handle,uint16_t attr_type,const void * attr_val,uint16_t attr_len)798 ble_hs_test_util_rx_att_find_type_value_req(uint16_t conn_handle,
799                                             uint16_t start_handle,
800                                             uint16_t end_handle,
801                                             uint16_t attr_type,
802                                             const void *attr_val,
803                                             uint16_t attr_len)
804 {
805     struct ble_att_find_type_value_req req;
806     uint8_t buf[BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ + 16];
807     int rc;
808 
809     TEST_ASSERT(attr_len <= 16);
810 
811     req.bavq_start_handle = start_handle;
812     req.bavq_end_handle = end_handle;
813     req.bavq_attr_type = attr_type;
814 
815     ble_att_find_type_value_req_write(buf, sizeof buf, &req);
816     memcpy(buf + BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ, attr_val, attr_len);
817 
818     rc = ble_hs_test_util_l2cap_rx_payload_flat(
819         conn_handle, BLE_L2CAP_CID_ATT, buf,
820         BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ + attr_len);
821 
822     return rc;
823 }
824 
825 int
ble_hs_test_util_rx_att_read_type_req(uint16_t conn_handle,uint16_t start_handle,uint16_t end_handle,const ble_uuid_t * uuid)826 ble_hs_test_util_rx_att_read_type_req(uint16_t conn_handle,
827                                       uint16_t start_handle,
828                                       uint16_t end_handle,
829                                       const ble_uuid_t *uuid)
830 {
831     struct ble_att_read_type_req req;
832     uint8_t buf[BLE_ATT_READ_TYPE_REQ_SZ_128];
833     int req_len;
834     int rc;
835 
836     req.batq_start_handle = start_handle;
837     req.batq_end_handle = end_handle;
838 
839     ble_att_read_type_req_write(buf, sizeof buf, &req);
840 
841     ble_uuid_flat(uuid, buf + BLE_ATT_READ_TYPE_REQ_BASE_SZ);
842     req_len = BLE_ATT_READ_TYPE_REQ_BASE_SZ + ble_uuid_length(uuid);
843 
844     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
845                                                 buf, req_len);
846     return rc;
847 }
848 
849 int
ble_hs_test_util_rx_att_read_type_req16(uint16_t conn_handle,uint16_t start_handle,uint16_t end_handle,uint16_t uuid16)850 ble_hs_test_util_rx_att_read_type_req16(uint16_t conn_handle,
851                                         uint16_t start_handle,
852                                         uint16_t end_handle,
853                                         uint16_t uuid16)
854 {
855     int rc;
856 
857     rc = ble_hs_test_util_rx_att_read_type_req(conn_handle, start_handle,
858                                                end_handle,
859                                                BLE_UUID16_DECLARE(uuid16));
860     return rc;
861 }
862 
863 int
ble_hs_test_util_rx_att_read_req(uint16_t conn_handle,uint16_t attr_handle)864 ble_hs_test_util_rx_att_read_req(uint16_t conn_handle, uint16_t attr_handle)
865 {
866     struct ble_att_read_req req;
867     uint8_t buf[BLE_ATT_READ_REQ_SZ];
868     int rc;
869 
870     req.barq_handle = attr_handle;
871     ble_att_read_req_write(buf, sizeof buf, &req);
872 
873     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
874                                                 buf, sizeof buf);
875     return rc;
876 }
877 
878 int
ble_hs_test_util_rx_att_read_blob_req(uint16_t conn_handle,uint16_t attr_handle,uint16_t offset)879 ble_hs_test_util_rx_att_read_blob_req(uint16_t conn_handle,
880                                       uint16_t attr_handle,
881                                       uint16_t offset)
882 {
883     struct ble_att_read_blob_req req;
884     uint8_t buf[BLE_ATT_READ_BLOB_REQ_SZ];
885     int rc;
886 
887     req.babq_handle = attr_handle;
888     req.babq_offset = offset;
889     ble_att_read_blob_req_write(buf, sizeof buf, &req);
890 
891     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
892                                                 buf, sizeof buf);
893     return rc;
894 }
895 
896 int
ble_hs_test_util_rx_att_read_mult_req(uint16_t conn_handle,const uint16_t * handles,int num_handles)897 ble_hs_test_util_rx_att_read_mult_req(uint16_t conn_handle,
898                                       const uint16_t *handles,
899                                       int num_handles)
900 {
901     uint8_t buf[256];
902     int off;
903     int rc;
904     int i;
905 
906     ble_att_read_mult_req_write(buf, sizeof buf);
907 
908     off = BLE_ATT_READ_MULT_REQ_BASE_SZ;
909     for (i = 0; i < num_handles; i++) {
910         put_le16(buf + off, handles[i]);
911         off += 2;
912     }
913 
914     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
915                                                 buf, off);
916     return rc;
917 }
918 
919 int
ble_hs_test_util_rx_att_read_group_type_req(uint16_t conn_handle,uint16_t start_handle,uint16_t end_handle,const ble_uuid_t * uuid)920 ble_hs_test_util_rx_att_read_group_type_req(uint16_t conn_handle,
921                                             uint16_t start_handle,
922                                             uint16_t end_handle,
923                                             const ble_uuid_t *uuid)
924 {
925     struct ble_att_read_group_type_req req;
926     uint8_t buf[BLE_ATT_READ_GROUP_TYPE_REQ_SZ_128];
927     int req_len;
928     int rc;
929 
930     req.bagq_start_handle = start_handle;
931     req.bagq_end_handle = end_handle;
932 
933     ble_uuid_flat(uuid, buf + BLE_ATT_READ_TYPE_REQ_BASE_SZ);
934     req_len = BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ + ble_uuid_length(uuid);
935 
936     ble_att_read_group_type_req_write(buf, sizeof buf, &req);
937     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
938                                                 buf, req_len);
939     return rc;
940 }
941 
942 int
ble_hs_test_util_rx_att_read_group_type_req16(uint16_t conn_handle,uint16_t start_handle,uint16_t end_handle,uint16_t uuid16)943 ble_hs_test_util_rx_att_read_group_type_req16(uint16_t conn_handle,
944                                               uint16_t start_handle,
945                                               uint16_t end_handle,
946                                               uint16_t uuid16)
947 {
948     int rc;
949 
950     rc = ble_hs_test_util_rx_att_read_group_type_req(conn_handle, start_handle,
951                                                      end_handle,
952                                                      BLE_UUID16_DECLARE(uuid16));
953     return rc;
954 }
955 
956 int
ble_hs_test_util_rx_att_write_req(uint16_t conn_handle,uint16_t attr_handle,const void * attr_val,uint16_t attr_len)957 ble_hs_test_util_rx_att_write_req(uint16_t conn_handle, uint16_t attr_handle,
958                                   const void *attr_val, uint16_t attr_len)
959 {
960     struct ble_att_write_req req;
961     uint8_t buf[BLE_ATT_WRITE_REQ_BASE_SZ + BLE_ATT_ATTR_MAX_LEN];
962     int rc;
963 
964     req.bawq_handle = attr_handle;
965     ble_att_write_req_write(buf, sizeof buf, &req);
966 
967     memcpy(buf + BLE_ATT_WRITE_REQ_BASE_SZ, attr_val, attr_len);
968 
969     rc = ble_hs_test_util_l2cap_rx_payload_flat(
970         conn_handle, BLE_L2CAP_CID_ATT, buf,
971         BLE_ATT_WRITE_REQ_BASE_SZ + attr_len);
972 
973     return rc;
974 }
975 
976 int
ble_hs_test_util_rx_att_write_cmd(uint16_t conn_handle,uint16_t attr_handle,const void * attr_val,uint16_t attr_len)977 ble_hs_test_util_rx_att_write_cmd(uint16_t conn_handle, uint16_t attr_handle,
978                                   const void *attr_val, uint16_t attr_len)
979 {
980     struct ble_att_write_req req;
981     uint8_t buf[BLE_ATT_WRITE_REQ_BASE_SZ + BLE_ATT_ATTR_MAX_LEN];
982     int rc;
983 
984     req.bawq_handle = attr_handle;
985     ble_att_write_cmd_write(buf, sizeof buf, &req);
986 
987     memcpy(buf + BLE_ATT_WRITE_REQ_BASE_SZ, attr_val, attr_len);
988 
989     rc = ble_hs_test_util_l2cap_rx_payload_flat(
990         conn_handle, BLE_L2CAP_CID_ATT, buf,
991         BLE_ATT_WRITE_REQ_BASE_SZ + attr_len);
992 
993     return rc;
994 }
995 
996 int
ble_hs_test_util_rx_att_prep_write_req(uint16_t conn_handle,uint16_t attr_handle,uint16_t offset,const void * attr_val,uint16_t attr_len)997 ble_hs_test_util_rx_att_prep_write_req(uint16_t conn_handle,
998                                        uint16_t attr_handle,
999                                        uint16_t offset,
1000                                        const void *attr_val,
1001                                        uint16_t attr_len)
1002 {
1003     struct ble_att_prep_write_cmd prep_req;
1004     uint8_t buf[BLE_ATT_PREP_WRITE_CMD_BASE_SZ + BLE_ATT_ATTR_MAX_LEN];
1005     int rc;
1006 
1007     prep_req.bapc_handle = attr_handle;
1008     prep_req.bapc_offset = offset;
1009     ble_att_prep_write_req_write(buf, sizeof buf, &prep_req);
1010     memcpy(buf + BLE_ATT_PREP_WRITE_CMD_BASE_SZ, attr_val, attr_len);
1011 
1012     rc = ble_hs_test_util_l2cap_rx_payload_flat(
1013         conn_handle, BLE_L2CAP_CID_ATT, buf,
1014         BLE_ATT_PREP_WRITE_CMD_BASE_SZ + attr_len);
1015 
1016     return rc;
1017 }
1018 
1019 int
ble_hs_test_util_rx_att_exec_write_req(uint16_t conn_handle,uint8_t flags)1020 ble_hs_test_util_rx_att_exec_write_req(uint16_t conn_handle, uint8_t flags)
1021 {
1022     struct ble_att_exec_write_req exec_req;
1023     uint8_t buf[BLE_ATT_EXEC_WRITE_REQ_SZ];
1024     int rc;
1025 
1026     exec_req.baeq_flags = flags;
1027     ble_att_exec_write_req_write(buf, sizeof buf, &exec_req);
1028     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
1029                                                 buf,
1030                                                 BLE_ATT_EXEC_WRITE_REQ_SZ);
1031     return rc;
1032 }
1033 
1034 int
ble_hs_test_util_rx_att_notify_req(uint16_t conn_handle,uint16_t attr_handle,void * attr_val,uint16_t attr_len)1035 ble_hs_test_util_rx_att_notify_req(uint16_t conn_handle,
1036                                    uint16_t attr_handle,
1037                                    void *attr_val,
1038                                    uint16_t attr_len)
1039 {
1040     struct ble_att_notify_req req;
1041     uint8_t buf[BLE_ATT_NOTIFY_REQ_BASE_SZ + BLE_ATT_ATTR_MAX_LEN];
1042     int rc;
1043 
1044     req.banq_handle = attr_handle;
1045     ble_att_notify_req_write(buf, sizeof buf, &req);
1046     memcpy(buf + BLE_ATT_NOTIFY_REQ_BASE_SZ, attr_val, attr_len);
1047 
1048     rc = ble_hs_test_util_l2cap_rx_payload_flat(
1049         conn_handle, BLE_L2CAP_CID_ATT, buf,
1050         BLE_ATT_NOTIFY_REQ_BASE_SZ + attr_len);
1051 
1052     return rc;
1053 }
1054 
1055 int
ble_hs_test_util_rx_att_indicate_req(uint16_t conn_handle,uint16_t attr_handle,void * attr_val,uint16_t attr_len)1056 ble_hs_test_util_rx_att_indicate_req(uint16_t conn_handle,
1057                                      uint16_t attr_handle,
1058                                      void *attr_val,
1059                                      uint16_t attr_len)
1060 {
1061     struct ble_att_indicate_req req;
1062     uint8_t buf[BLE_ATT_INDICATE_REQ_BASE_SZ + BLE_ATT_ATTR_MAX_LEN];
1063     int rc;
1064 
1065     req.baiq_handle = attr_handle;
1066     ble_att_indicate_req_write(buf, sizeof buf, &req);
1067     memcpy(buf + BLE_ATT_INDICATE_REQ_BASE_SZ, attr_val, attr_len);
1068 
1069     rc = ble_hs_test_util_l2cap_rx_payload_flat(
1070         conn_handle, BLE_L2CAP_CID_ATT, buf,
1071         BLE_ATT_INDICATE_REQ_BASE_SZ + attr_len);
1072 
1073     return rc;
1074 }
1075 
1076 void
ble_hs_test_util_rx_att_err_rsp(uint16_t conn_handle,uint8_t req_op,uint8_t error_code,uint16_t err_handle)1077 ble_hs_test_util_rx_att_err_rsp(uint16_t conn_handle, uint8_t req_op,
1078                                 uint8_t error_code, uint16_t err_handle)
1079 {
1080     struct ble_att_error_rsp rsp;
1081     uint8_t buf[BLE_ATT_ERROR_RSP_SZ];
1082     int rc;
1083 
1084     rsp.baep_req_op = req_op;
1085     rsp.baep_handle = err_handle;
1086     rsp.baep_error_code = error_code;
1087 
1088     ble_att_error_rsp_write(buf, sizeof buf, &rsp);
1089 
1090     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
1091                                                 buf, sizeof buf);
1092     TEST_ASSERT(rc == 0);
1093 }
1094 
1095 void
ble_hs_test_util_verify_tx_prep_write(uint16_t attr_handle,uint16_t offset,const void * data,int data_len)1096 ble_hs_test_util_verify_tx_prep_write(uint16_t attr_handle, uint16_t offset,
1097                                       const void *data, int data_len)
1098 {
1099     struct ble_att_prep_write_cmd req;
1100     struct os_mbuf *om;
1101 
1102     om = ble_hs_test_util_prev_tx_dequeue();
1103     TEST_ASSERT_FATAL(om != NULL);
1104     TEST_ASSERT(OS_MBUF_PKTLEN(om) ==
1105                 BLE_ATT_PREP_WRITE_CMD_BASE_SZ + data_len);
1106 
1107     om = os_mbuf_pullup(om, BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
1108     TEST_ASSERT_FATAL(om != NULL);
1109 
1110     ble_att_prep_write_req_parse(om->om_data, om->om_len, &req);
1111     TEST_ASSERT(req.bapc_handle == attr_handle);
1112     TEST_ASSERT(req.bapc_offset == offset);
1113     TEST_ASSERT(os_mbuf_cmpf(om, BLE_ATT_PREP_WRITE_CMD_BASE_SZ,
1114                              data, data_len) == 0);
1115 }
1116 
1117 void
ble_hs_test_util_verify_tx_exec_write(uint8_t expected_flags)1118 ble_hs_test_util_verify_tx_exec_write(uint8_t expected_flags)
1119 {
1120     struct ble_att_exec_write_req req;
1121     struct os_mbuf *om;
1122 
1123     om = ble_hs_test_util_prev_tx_dequeue_pullup();
1124     TEST_ASSERT_FATAL(om != NULL);
1125     TEST_ASSERT(om->om_len == BLE_ATT_EXEC_WRITE_REQ_SZ);
1126 
1127     ble_att_exec_write_req_parse(om->om_data, om->om_len, &req);
1128     TEST_ASSERT(req.baeq_flags == expected_flags);
1129 }
1130 
1131 void
ble_hs_test_util_verify_tx_find_type_value(uint16_t start_handle,uint16_t end_handle,uint16_t attr_type,const void * value,uint16_t value_len)1132 ble_hs_test_util_verify_tx_find_type_value(uint16_t start_handle,
1133                                            uint16_t end_handle,
1134                                            uint16_t attr_type,
1135                                            const void *value,
1136                                            uint16_t value_len)
1137 {
1138     struct ble_att_find_type_value_req req;
1139     struct os_mbuf *om;
1140 
1141     om = ble_hs_test_util_prev_tx_dequeue_pullup();
1142     TEST_ASSERT_FATAL(om != NULL);
1143     TEST_ASSERT(om->om_len == BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ + value_len);
1144 
1145     ble_att_find_type_value_req_parse(om->om_data, om->om_len, &req);
1146     TEST_ASSERT(req.bavq_start_handle == start_handle);
1147     TEST_ASSERT(req.bavq_end_handle == end_handle);
1148     TEST_ASSERT(req.bavq_attr_type == attr_type);
1149     TEST_ASSERT(memcmp(om->om_data + BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ,
1150                        value,
1151                        value_len) == 0);
1152 }
1153 
1154 void
ble_hs_test_util_verify_tx_disc_svc_uuid(const ble_uuid_t * uuid)1155 ble_hs_test_util_verify_tx_disc_svc_uuid(const ble_uuid_t *uuid)
1156 {
1157     uint8_t uuid_buf[16];
1158 
1159     ble_uuid_flat(uuid, uuid_buf);
1160     ble_hs_test_util_verify_tx_find_type_value(
1161         1, 0xffff, BLE_ATT_UUID_PRIMARY_SERVICE,
1162         uuid_buf, ble_uuid_length(uuid));
1163 }
1164 
1165 void
ble_hs_test_util_verify_tx_read_rsp_gen(uint8_t att_op,uint8_t * attr_data,int attr_len)1166 ble_hs_test_util_verify_tx_read_rsp_gen(uint8_t att_op,
1167                                         uint8_t *attr_data, int attr_len)
1168 {
1169     struct os_mbuf *om;
1170     uint8_t u8;
1171     int rc;
1172     int i;
1173 
1174     om = ble_hs_test_util_prev_tx_dequeue();
1175 
1176     rc = os_mbuf_copydata(om, 0, 1, &u8);
1177     TEST_ASSERT(rc == 0);
1178     TEST_ASSERT(u8 == att_op);
1179 
1180     for (i = 0; i < attr_len; i++) {
1181         rc = os_mbuf_copydata(om, i + 1, 1, &u8);
1182         TEST_ASSERT(rc == 0);
1183         TEST_ASSERT(u8 == attr_data[i]);
1184     }
1185 
1186     rc = os_mbuf_copydata(om, i + 1, 1, &u8);
1187     TEST_ASSERT(rc != 0);
1188 }
1189 
1190 void
ble_hs_test_util_verify_tx_read_rsp(uint8_t * attr_data,int attr_len)1191 ble_hs_test_util_verify_tx_read_rsp(uint8_t *attr_data, int attr_len)
1192 {
1193     ble_hs_test_util_verify_tx_read_rsp_gen(BLE_ATT_OP_READ_RSP,
1194                                             attr_data, attr_len);
1195 }
1196 
1197 void
ble_hs_test_util_verify_tx_read_blob_rsp(uint8_t * attr_data,int attr_len)1198 ble_hs_test_util_verify_tx_read_blob_rsp(uint8_t *attr_data, int attr_len)
1199 {
1200     ble_hs_test_util_verify_tx_read_rsp_gen(BLE_ATT_OP_READ_BLOB_RSP,
1201                                             attr_data, attr_len);
1202 }
1203 
1204 void
ble_hs_test_util_verify_tx_write_rsp(void)1205 ble_hs_test_util_verify_tx_write_rsp(void)
1206 {
1207     struct os_mbuf *om;
1208     uint8_t u8;
1209     int rc;
1210 
1211     om = ble_hs_test_util_prev_tx_dequeue();
1212 
1213     rc = os_mbuf_copydata(om, 0, 1, &u8);
1214     TEST_ASSERT(rc == 0);
1215     TEST_ASSERT(u8 == BLE_ATT_OP_WRITE_RSP);
1216 }
1217 
1218 void
ble_hs_test_util_verify_tx_mtu_cmd(int is_req,uint16_t mtu)1219 ble_hs_test_util_verify_tx_mtu_cmd(int is_req, uint16_t mtu)
1220 {
1221     struct ble_att_mtu_cmd cmd;
1222     struct os_mbuf *om;
1223 
1224     om = ble_hs_test_util_prev_tx_dequeue_pullup();
1225     TEST_ASSERT_FATAL(om != NULL);
1226 
1227     if (is_req) {
1228         ble_att_mtu_req_parse(om->om_data, om->om_len, &cmd);
1229     } else {
1230         ble_att_mtu_rsp_parse(om->om_data, om->om_len, &cmd);
1231     }
1232 
1233     TEST_ASSERT(cmd.bamc_mtu == mtu);
1234 }
1235 
1236 void
ble_hs_test_util_verify_tx_find_info_rsp(struct ble_hs_test_util_att_info_entry * entries)1237 ble_hs_test_util_verify_tx_find_info_rsp(
1238     struct ble_hs_test_util_att_info_entry *entries)
1239 {
1240     struct ble_hs_test_util_att_info_entry *entry;
1241     struct ble_att_find_info_rsp rsp;
1242     struct os_mbuf *om;
1243     uint16_t handle;
1244     uint8_t buf[BLE_ATT_FIND_INFO_RSP_BASE_SZ];
1245     ble_uuid_any_t uuid;
1246     int off;
1247     int rc;
1248 
1249     off = 0;
1250 
1251     om = ble_hs_test_util_prev_tx_dequeue_pullup();
1252     TEST_ASSERT_FATAL(om);
1253 
1254     rc = os_mbuf_copydata(om, off, sizeof buf, buf);
1255     TEST_ASSERT(rc == 0);
1256     off += sizeof buf;
1257 
1258     ble_att_find_info_rsp_parse(buf, sizeof buf, &rsp);
1259 
1260     for (entry = entries; entry->handle != 0; entry++) {
1261         rc = os_mbuf_copydata(om, off, 2, &handle);
1262         TEST_ASSERT(rc == 0);
1263         off += 2;
1264 
1265         handle = get_le16((void *)&handle);
1266         TEST_ASSERT(handle == entry->handle);
1267 
1268         if (entry->uuid->type == BLE_UUID_TYPE_16) {
1269             TEST_ASSERT(rsp.bafp_format ==
1270                         BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT);
1271 
1272             ble_uuid_init_from_att_mbuf(&uuid, om, off, 2);
1273             TEST_ASSERT(rc == 0);
1274             off += 2;
1275         } else {
1276             TEST_ASSERT(rsp.bafp_format ==
1277                         BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT);
1278 
1279             rc = ble_uuid_init_from_att_mbuf(&uuid, om, off, 16);
1280             TEST_ASSERT(rc == 0);
1281             off += 16;
1282         }
1283 
1284         TEST_ASSERT(ble_uuid_cmp(entry->uuid, &uuid.u) == 0);
1285     }
1286 
1287     /* Ensure there is no extra data in the response. */
1288     TEST_ASSERT(off == OS_MBUF_PKTHDR(om)->omp_len);
1289 }
1290 
1291 void
ble_hs_test_util_verify_tx_read_group_type_rsp(struct ble_hs_test_util_att_group_type_entry * entries)1292 ble_hs_test_util_verify_tx_read_group_type_rsp(
1293     struct ble_hs_test_util_att_group_type_entry *entries)
1294 {
1295     struct ble_hs_test_util_att_group_type_entry *entry;
1296     struct ble_att_read_group_type_rsp rsp;
1297     struct os_mbuf *om;
1298     uint16_t u16;
1299     ble_uuid_any_t uuid;
1300     int off;
1301     int rc;
1302 
1303     om = ble_hs_test_util_prev_tx_dequeue_pullup();
1304     TEST_ASSERT_FATAL(om);
1305 
1306     ble_att_read_group_type_rsp_parse(om->om_data, om->om_len, &rsp);
1307 
1308     off = BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ;
1309     for (entry = entries; entry->start_handle != 0; entry++) {
1310         if (entry->uuid->type == BLE_UUID_TYPE_16) {
1311             TEST_ASSERT(rsp.bagp_length ==
1312                         BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16);
1313         } else {
1314             TEST_ASSERT(rsp.bagp_length ==
1315                         BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128);
1316         }
1317 
1318         rc = os_mbuf_copydata(om, off, 2, &u16);
1319         TEST_ASSERT(rc == 0);
1320         put_le16(&u16, u16);
1321         TEST_ASSERT(u16 == entry->start_handle);
1322         off += 2;
1323 
1324         rc = os_mbuf_copydata(om, off, 2, &u16);
1325         TEST_ASSERT(rc == 0);
1326         put_le16(&u16, u16);
1327         TEST_ASSERT(u16 == entry->end_handle);
1328         off += 2;
1329 
1330         if (entry->uuid->type == BLE_UUID_TYPE_16) {
1331             rc = ble_uuid_init_from_att_mbuf(&uuid, om, off, 2);
1332             TEST_ASSERT(rc == 0);
1333         } else {
1334             rc = ble_uuid_init_from_att_mbuf(&uuid, om, off, 16);
1335             TEST_ASSERT(rc == 0);
1336         }
1337 
1338         TEST_ASSERT(ble_uuid_cmp(&uuid.u, entry->uuid) == 0);
1339         off += ble_uuid_length(&uuid.u);
1340     }
1341 
1342     /* Ensure there is no extra data in the response. */
1343     TEST_ASSERT(off == OS_MBUF_PKTLEN(om));
1344 }
1345 
1346 void
ble_hs_test_util_verify_tx_err_rsp(uint8_t req_op,uint16_t handle,uint8_t error_code)1347 ble_hs_test_util_verify_tx_err_rsp(uint8_t req_op, uint16_t handle,
1348                                    uint8_t error_code)
1349 {
1350     struct ble_att_error_rsp rsp;
1351     struct os_mbuf *om;
1352     uint8_t buf[BLE_ATT_ERROR_RSP_SZ];
1353     int rc;
1354 
1355     om = ble_hs_test_util_prev_tx_dequeue();
1356 
1357     rc = os_mbuf_copydata(om, 0, sizeof buf, buf);
1358     TEST_ASSERT(rc == 0);
1359 
1360     ble_att_error_rsp_parse(buf, sizeof buf, &rsp);
1361 
1362     TEST_ASSERT(rsp.baep_req_op == req_op);
1363     TEST_ASSERT(rsp.baep_handle == handle);
1364     TEST_ASSERT(rsp.baep_error_code == error_code);
1365 }
1366 
1367 void
ble_hs_test_util_verify_tx_write_cmd(uint16_t handle,const void * data,uint16_t data_len)1368 ble_hs_test_util_verify_tx_write_cmd(uint16_t handle, const void *data,
1369                                      uint16_t data_len)
1370 {
1371     struct ble_att_write_req req;
1372     struct os_mbuf *om;
1373     uint8_t buf[BLE_ATT_WRITE_CMD_BASE_SZ];
1374     int rc;
1375 
1376     om = ble_hs_test_util_prev_tx_dequeue();
1377 
1378     rc = os_mbuf_copydata(om, 0, sizeof buf, buf);
1379     TEST_ASSERT(rc == 0);
1380 
1381     ble_att_write_cmd_parse(buf, sizeof buf, &req);
1382 
1383     TEST_ASSERT(req.bawq_handle == handle);
1384 
1385     os_mbuf_adj(om, sizeof buf);
1386     TEST_ASSERT(OS_MBUF_PKTLEN(om) == data_len);
1387     TEST_ASSERT(os_mbuf_cmpf(om, 0, data, data_len) == 0);
1388 }
1389 
1390 static struct os_mbuf *
ble_hs_test_util_verify_tx_l2cap_sig_hdr(uint8_t op,uint8_t id,uint16_t payload_len,struct ble_l2cap_sig_hdr * out_hdr)1391 ble_hs_test_util_verify_tx_l2cap_sig_hdr(uint8_t op, uint8_t id,
1392                                    uint16_t payload_len,
1393                                    struct ble_l2cap_sig_hdr *out_hdr)
1394 {
1395     struct ble_l2cap_sig_hdr hdr;
1396     struct os_mbuf *om;
1397 
1398     om = ble_hs_test_util_prev_tx_dequeue();
1399     TEST_ASSERT_FATAL(om != NULL);
1400 
1401     TEST_ASSERT(OS_MBUF_PKTLEN(om) == BLE_L2CAP_SIG_HDR_SZ + payload_len);
1402     ble_l2cap_sig_hdr_parse(om->om_data, om->om_len, &hdr);
1403     TEST_ASSERT(hdr.op == op);
1404     if (id != 0) {
1405         TEST_ASSERT(hdr.identifier == id);
1406     }
1407     TEST_ASSERT(hdr.length == payload_len);
1408 
1409     om->om_data += BLE_L2CAP_SIG_HDR_SZ;
1410     om->om_len -= BLE_L2CAP_SIG_HDR_SZ;
1411 
1412     if (out_hdr != NULL) {
1413         *out_hdr = hdr;
1414     }
1415 
1416     return om;
1417 }
1418 
1419 int
ble_hs_test_util_inject_rx_l2cap_sig(uint16_t conn_handle,uint8_t opcode,uint8_t id,void * cmd,uint16_t cmd_size)1420 ble_hs_test_util_inject_rx_l2cap_sig(uint16_t conn_handle, uint8_t opcode,
1421                               uint8_t id, void *cmd, uint16_t cmd_size)
1422 {
1423     void *r;
1424     struct hci_data_hdr hci_hdr;
1425     struct os_mbuf *om;
1426     int rc;
1427 
1428     hci_hdr = BLE_HS_TEST_UTIL_L2CAP_HCI_HDR(2, BLE_HCI_PB_FIRST_FLUSH,
1429                          BLE_L2CAP_HDR_SZ + BLE_L2CAP_SIG_HDR_SZ + cmd_size);
1430 
1431     r = ble_l2cap_sig_cmd_get(opcode, id, cmd_size, &om);
1432     TEST_ASSERT_FATAL(r != NULL);
1433 
1434     memcpy(r, cmd, cmd_size);
1435 
1436     rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SIG,
1437                                               &hci_hdr, om);
1438     return rc;
1439 }
1440 
1441 /**
1442  * @return  The L2CAP sig identifier in the request/response.
1443  */
1444 uint8_t
ble_hs_test_util_verify_tx_l2cap_sig(uint16_t opcode,void * cmd,uint16_t cmd_size)1445 ble_hs_test_util_verify_tx_l2cap_sig(uint16_t opcode, void *cmd,
1446                                      uint16_t cmd_size)
1447 {
1448     struct ble_l2cap_sig_hdr hdr;
1449     struct os_mbuf *om;
1450 
1451     om = ble_hs_test_util_verify_tx_l2cap_sig_hdr(opcode, 0, cmd_size, &hdr);
1452     om = os_mbuf_pullup(om, cmd_size);
1453 
1454     /* Verify payload. */
1455     TEST_ASSERT(memcmp(om->om_data, cmd, cmd_size) == 0);
1456 
1457     return hdr.identifier;
1458 }
1459 
1460 void
ble_hs_test_util_verify_tx_l2cap(struct os_mbuf * txom)1461 ble_hs_test_util_verify_tx_l2cap(struct os_mbuf *txom)
1462 {
1463     struct os_mbuf *om;
1464 
1465     om = ble_hs_test_util_prev_tx_dequeue();
1466     TEST_ASSERT_FATAL(om != NULL);
1467 
1468     /* TODO Handle fragmentation */
1469     TEST_ASSERT_FATAL(os_mbuf_cmpm(om, 0, txom, 0, OS_MBUF_PKTLEN(om)) == 0);
1470 }
1471 
1472 void
ble_hs_test_util_inject_rx_l2cap(uint16_t conn_handle,uint16_t cid,struct os_mbuf * rxom)1473 ble_hs_test_util_inject_rx_l2cap(uint16_t conn_handle, uint16_t cid,
1474                                  struct os_mbuf *rxom)
1475 {
1476     struct hci_data_hdr hci_hdr;
1477     int rc;
1478 
1479     hci_hdr = BLE_HS_TEST_UTIL_L2CAP_HCI_HDR(2, BLE_HCI_PB_FIRST_FLUSH,
1480                                              BLE_L2CAP_HDR_SZ +
1481                                              BLE_L2CAP_SIG_HDR_SZ +
1482                                              OS_MBUF_PKTLEN(rxom));
1483 
1484     rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, cid, &hci_hdr, rxom);
1485     TEST_ASSERT(rc == 0);
1486 }
1487 
1488 static void
ble_l2cap_test_update_req_swap(struct ble_l2cap_sig_update_req * dst,struct ble_l2cap_sig_update_req * src)1489 ble_l2cap_test_update_req_swap(struct ble_l2cap_sig_update_req *dst,
1490                                struct ble_l2cap_sig_update_req *src)
1491 {
1492     dst->itvl_min = le16toh(src->itvl_min);
1493     dst->itvl_max = le16toh(src->itvl_max);
1494     dst->slave_latency = le16toh(src->slave_latency);
1495     dst->timeout_multiplier = le16toh(src->timeout_multiplier);
1496 }
1497 
1498 static void
ble_l2cap_test_update_req_parse(void * payload,int len,struct ble_l2cap_sig_update_req * dst)1499 ble_l2cap_test_update_req_parse(void *payload, int len,
1500                                struct ble_l2cap_sig_update_req *dst)
1501 {
1502     BLE_HS_DBG_ASSERT(len >= BLE_L2CAP_SIG_UPDATE_REQ_SZ);
1503     ble_l2cap_test_update_req_swap(dst, payload);
1504 }
1505 
1506 /**
1507  * @return                      The L2CAP sig identifier in the request.
1508  */
1509 uint8_t
ble_hs_test_util_verify_tx_l2cap_update_req(struct ble_l2cap_sig_update_params * params)1510 ble_hs_test_util_verify_tx_l2cap_update_req(
1511     struct ble_l2cap_sig_update_params *params)
1512 {
1513     struct ble_l2cap_sig_update_req req;
1514     struct ble_l2cap_sig_hdr hdr;
1515     struct os_mbuf *om;
1516 
1517     om = ble_hs_test_util_verify_tx_l2cap_sig_hdr(BLE_L2CAP_SIG_OP_UPDATE_REQ,
1518                                                   0,
1519                                                   BLE_L2CAP_SIG_UPDATE_REQ_SZ,
1520                                                   &hdr);
1521 
1522     /* Verify payload. */
1523     ble_l2cap_test_update_req_parse(om->om_data, om->om_len, &req);
1524     TEST_ASSERT(req.itvl_min == params->itvl_min);
1525     TEST_ASSERT(req.itvl_max == params->itvl_max);
1526     TEST_ASSERT(req.slave_latency == params->slave_latency);
1527     TEST_ASSERT(req.timeout_multiplier == params->timeout_multiplier);
1528 
1529     return hdr.identifier;
1530 }
1531 
1532 static void
ble_l2cap_sig_update_rsp_parse(void * payload,int len,struct ble_l2cap_sig_update_rsp * dst)1533 ble_l2cap_sig_update_rsp_parse(void *payload, int len,
1534                                struct ble_l2cap_sig_update_rsp *dst)
1535 {
1536     struct ble_l2cap_sig_update_rsp *src = payload;
1537 
1538     BLE_HS_DBG_ASSERT(len >= BLE_L2CAP_SIG_UPDATE_RSP_SZ);
1539     dst->result = le16toh(src->result);
1540 }
1541 
1542 int
ble_hs_test_util_rx_l2cap_update_rsp(uint16_t conn_handle,uint8_t id,uint16_t result)1543 ble_hs_test_util_rx_l2cap_update_rsp(uint16_t conn_handle,
1544                                      uint8_t id, uint16_t result)
1545 {
1546     struct ble_l2cap_sig_update_rsp *rsp;
1547     struct hci_data_hdr hci_hdr;
1548     struct os_mbuf *om;
1549     int rc;
1550 
1551     hci_hdr = BLE_HS_TEST_UTIL_L2CAP_HCI_HDR(
1552         2, BLE_HCI_PB_FIRST_FLUSH,
1553         BLE_L2CAP_HDR_SZ + BLE_L2CAP_SIG_HDR_SZ + BLE_L2CAP_SIG_UPDATE_RSP_SZ);
1554 
1555     rsp = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_UPDATE_RSP, id,
1556                                 BLE_L2CAP_SIG_UPDATE_RSP_SZ, &om);
1557     TEST_ASSERT_FATAL(rsp != NULL);
1558 
1559     rsp->result = htole16(result);
1560 
1561     rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SIG,
1562                                               &hci_hdr, om);
1563     return rc;
1564 }
1565 
1566 void
ble_hs_test_util_verify_tx_l2cap_update_rsp(uint8_t exp_id,uint16_t exp_result)1567 ble_hs_test_util_verify_tx_l2cap_update_rsp(uint8_t exp_id,
1568                                             uint16_t exp_result)
1569 {
1570     struct ble_l2cap_sig_update_rsp rsp;
1571     struct os_mbuf *om;
1572 
1573     om = ble_hs_test_util_verify_tx_l2cap_sig_hdr(BLE_L2CAP_SIG_OP_UPDATE_RSP,
1574                                             exp_id,
1575                                             BLE_L2CAP_SIG_UPDATE_RSP_SZ,
1576                                             NULL);
1577 
1578     ble_l2cap_sig_update_rsp_parse(om->om_data, om->om_len, &rsp);
1579     TEST_ASSERT(rsp.result == exp_result);
1580 }
1581 
1582 void
ble_hs_test_util_set_static_rnd_addr(const uint8_t * addr)1583 ble_hs_test_util_set_static_rnd_addr(const uint8_t *addr)
1584 {
1585     int rc;
1586 
1587     ble_hs_test_util_hci_ack_set(
1588         BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_RAND_ADDR), 0);
1589 
1590     rc = ble_hs_id_set_rnd(addr);
1591     TEST_ASSERT_FATAL(rc == 0);
1592 
1593     ble_hs_test_util_hci_out_first();
1594 }
1595 
1596 struct os_mbuf *
ble_hs_test_util_om_from_flat(const void * buf,uint16_t len)1597 ble_hs_test_util_om_from_flat(const void *buf, uint16_t len)
1598 {
1599     struct os_mbuf *om;
1600 
1601     om = ble_hs_mbuf_from_flat(buf, len);
1602     TEST_ASSERT_FATAL(om != NULL);
1603 
1604     return om;
1605 }
1606 
1607 int
ble_hs_test_util_flat_attr_cmp(const struct ble_hs_test_util_flat_attr * a,const struct ble_hs_test_util_flat_attr * b)1608 ble_hs_test_util_flat_attr_cmp(const struct ble_hs_test_util_flat_attr *a,
1609                                const struct ble_hs_test_util_flat_attr *b)
1610 {
1611     if (a->handle != b->handle) {
1612         return -1;
1613     }
1614     if (a->offset != b->offset) {
1615         return -1;
1616     }
1617     if (a->value_len != b->value_len) {
1618         return -1;
1619     }
1620     return memcmp(a->value, b->value, a->value_len);
1621 }
1622 
1623 void
ble_hs_test_util_attr_to_flat(struct ble_hs_test_util_flat_attr * flat,const struct ble_gatt_attr * attr)1624 ble_hs_test_util_attr_to_flat(struct ble_hs_test_util_flat_attr *flat,
1625                               const struct ble_gatt_attr *attr)
1626 {
1627     int rc;
1628 
1629     flat->handle = attr->handle;
1630     flat->offset = attr->offset;
1631     rc = ble_hs_mbuf_to_flat(attr->om, flat->value, sizeof flat->value,
1632                            &flat->value_len);
1633     TEST_ASSERT_FATAL(rc == 0);
1634 }
1635 
1636 void
ble_hs_test_util_attr_from_flat(struct ble_gatt_attr * attr,const struct ble_hs_test_util_flat_attr * flat)1637 ble_hs_test_util_attr_from_flat(struct ble_gatt_attr *attr,
1638                                 const struct ble_hs_test_util_flat_attr *flat)
1639 {
1640     attr->handle = flat->handle;
1641     attr->offset = flat->offset;
1642     attr->om = ble_hs_test_util_om_from_flat(flat->value, flat->value_len);
1643 }
1644 
1645 int
ble_hs_test_util_read_local_flat(uint16_t attr_handle,uint16_t max_len,void * buf,uint16_t * out_len)1646 ble_hs_test_util_read_local_flat(uint16_t attr_handle, uint16_t max_len,
1647                                  void *buf, uint16_t *out_len)
1648 {
1649     struct os_mbuf *om;
1650     int rc;
1651 
1652     rc = ble_att_svr_read_local(attr_handle, &om);
1653     if (rc != 0) {
1654         return rc;
1655     }
1656 
1657     TEST_ASSERT_FATAL(OS_MBUF_PKTLEN(om) <= max_len);
1658 
1659     rc = os_mbuf_copydata(om, 0, OS_MBUF_PKTLEN(om), buf);
1660     TEST_ASSERT_FATAL(rc == 0);
1661 
1662     *out_len = OS_MBUF_PKTLEN(om);
1663 
1664     rc = os_mbuf_free_chain(om);
1665     TEST_ASSERT_FATAL(rc == 0);
1666     return 0;
1667 }
1668 
1669 int
ble_hs_test_util_write_local_flat(uint16_t attr_handle,const void * buf,uint16_t buf_len)1670 ble_hs_test_util_write_local_flat(uint16_t attr_handle,
1671                                   const void *buf, uint16_t buf_len)
1672 {
1673     struct os_mbuf *om;
1674     int rc;
1675 
1676     om = ble_hs_test_util_om_from_flat(buf, buf_len);
1677     rc = ble_att_svr_write_local(attr_handle, om);
1678     return rc;
1679 }
1680 
1681 int
ble_hs_test_util_gatt_write_flat(uint16_t conn_handle,uint16_t attr_handle,const void * data,uint16_t data_len,ble_gatt_attr_fn * cb,void * cb_arg)1682 ble_hs_test_util_gatt_write_flat(uint16_t conn_handle, uint16_t attr_handle,
1683                                  const void *data, uint16_t data_len,
1684                                  ble_gatt_attr_fn *cb, void *cb_arg)
1685 {
1686     struct os_mbuf *om;
1687     int rc;
1688 
1689     om = ble_hs_test_util_om_from_flat(data, data_len);
1690     rc = ble_gattc_write(conn_handle, attr_handle, om, cb, cb_arg);
1691 
1692     return rc;
1693 }
1694 
1695 int
ble_hs_test_util_gatt_write_no_rsp_flat(uint16_t conn_handle,uint16_t attr_handle,const void * data,uint16_t data_len)1696 ble_hs_test_util_gatt_write_no_rsp_flat(uint16_t conn_handle,
1697                                         uint16_t attr_handle,
1698                                         const void *data, uint16_t data_len)
1699 {
1700     struct os_mbuf *om;
1701     int rc;
1702 
1703     om = ble_hs_test_util_om_from_flat(data, data_len);
1704     rc = ble_gattc_write_no_rsp(conn_handle, attr_handle, om);
1705 
1706     return rc;
1707 }
1708 
1709 int
ble_hs_test_util_gatt_write_long_flat(uint16_t conn_handle,uint16_t attr_handle,const void * data,uint16_t data_len,ble_gatt_attr_fn * cb,void * cb_arg)1710 ble_hs_test_util_gatt_write_long_flat(uint16_t conn_handle,
1711                                       uint16_t attr_handle,
1712                                       const void *data, uint16_t data_len,
1713                                       ble_gatt_attr_fn *cb, void *cb_arg)
1714 {
1715     struct os_mbuf *om;
1716     uint16_t offset = 0;
1717     int rc;
1718 
1719     om = ble_hs_test_util_om_from_flat(data, data_len);
1720     rc = ble_gattc_write_long(conn_handle, attr_handle, offset, om, cb, cb_arg);
1721 
1722     return rc;
1723 }
1724 
1725 static int
ble_hs_test_util_mbuf_chain_len(const struct os_mbuf * om)1726 ble_hs_test_util_mbuf_chain_len(const struct os_mbuf *om)
1727 {
1728     int count;
1729 
1730     count = 0;
1731     while (om != NULL) {
1732         count++;
1733         om = SLIST_NEXT(om, om_next);
1734     }
1735 
1736     return count;
1737 }
1738 
1739 struct os_mbuf *
ble_hs_test_util_mbuf_alloc_all_but(int count)1740 ble_hs_test_util_mbuf_alloc_all_but(int count)
1741 {
1742     struct os_mbuf *prev;
1743     struct os_mbuf *om;
1744     int rc;
1745     int i;
1746 
1747     /* Allocate all available mbufs and put them in a single chain. */
1748     prev = NULL;
1749     while (1) {
1750         om = os_msys_get(0, 0);
1751         if (om == NULL) {
1752             break;
1753         }
1754 
1755         if (prev != NULL) {
1756             SLIST_NEXT(om, om_next) = prev;
1757         }
1758 
1759         prev = om;
1760     }
1761 
1762     /* Now free 'count' mbufs. */
1763     for (i = 0; i < count; i++) {
1764         TEST_ASSERT_FATAL(prev != NULL);
1765         om = SLIST_NEXT(prev, om_next);
1766         rc = os_mbuf_free(prev);
1767         TEST_ASSERT_FATAL(rc == 0);
1768 
1769         prev = om;
1770     }
1771 
1772     return prev;
1773 }
1774 
1775 int
ble_hs_test_util_mbuf_count(const struct ble_hs_test_util_mbuf_params * params)1776 ble_hs_test_util_mbuf_count(const struct ble_hs_test_util_mbuf_params *params)
1777 {
1778     const struct ble_att_prep_entry *prep;
1779     const struct os_mbuf_pkthdr *omp;
1780     const struct ble_l2cap_chan *chan;
1781     const struct ble_hs_conn *conn;
1782     const struct os_mbuf *om;
1783     int count;
1784     int i;
1785 
1786     ble_hs_process_rx_data_queue();
1787 
1788     count = os_msys_num_free();
1789 
1790     if (params->prev_tx) {
1791         count += ble_hs_test_util_mbuf_chain_len(ble_hs_test_util_prev_tx_cur);
1792         STAILQ_FOREACH(omp, &ble_hs_test_util_prev_tx_queue, omp_next) {
1793             om = OS_MBUF_PKTHDR_TO_MBUF(omp);
1794             count += ble_hs_test_util_mbuf_chain_len(om);
1795         }
1796     }
1797 
1798     ble_hs_lock();
1799     for (i = 0; ; i++) {
1800         conn = ble_hs_conn_find_by_idx(i);
1801         if (conn == NULL) {
1802             break;
1803         }
1804 
1805         if (params->rx_queue) {
1806             SLIST_FOREACH(chan, &conn->bhc_channels, next) {
1807                 count += ble_hs_test_util_mbuf_chain_len(chan->rx_buf);
1808             }
1809         }
1810 
1811         if (params->prep_list) {
1812             SLIST_FOREACH(prep, &conn->bhc_att_svr.basc_prep_list, bape_next) {
1813                 count += ble_hs_test_util_mbuf_chain_len(prep->bape_value);
1814             }
1815         }
1816     }
1817     ble_hs_unlock();
1818 
1819     return count;
1820 }
1821 
1822 void
ble_hs_test_util_assert_mbufs_freed(const struct ble_hs_test_util_mbuf_params * params)1823 ble_hs_test_util_assert_mbufs_freed(
1824     const struct ble_hs_test_util_mbuf_params *params)
1825 {
1826     static const struct ble_hs_test_util_mbuf_params dflt = {
1827         .prev_tx = 1,
1828         .rx_queue = 1,
1829         .prep_list = 1,
1830     };
1831 
1832     int count;
1833 
1834     if (params == NULL) {
1835         params = &dflt;
1836     }
1837 
1838     count = ble_hs_test_util_mbuf_count(params);
1839     TEST_ASSERT(count == os_msys_count());
1840 }
1841 
1842 void
ble_hs_test_util_post_test(void * arg)1843 ble_hs_test_util_post_test(void *arg)
1844 {
1845     ble_hs_test_util_assert_mbufs_freed(arg);
1846 }
1847 
1848 static int
ble_hs_test_util_pkt_txed(struct os_mbuf * om,void * arg)1849 ble_hs_test_util_pkt_txed(struct os_mbuf *om, void *arg)
1850 {
1851     ble_hs_test_util_prev_tx_enqueue(om);
1852     return 0;
1853 }
1854 
1855 static int
ble_hs_test_util_hci_txed(uint8_t * cmdbuf,void * arg)1856 ble_hs_test_util_hci_txed(uint8_t *cmdbuf, void *arg)
1857 {
1858     ble_hs_test_util_hci_out_enqueue(cmdbuf);
1859     ble_hci_trans_buf_free(cmdbuf);
1860     return 0;
1861 }
1862 
1863 int
ble_hs_test_util_num_cccds(void)1864 ble_hs_test_util_num_cccds(void)
1865 {
1866     struct ble_store_value_cccd val;
1867     struct ble_store_key_cccd key = { };
1868     int rc;
1869 
1870     key.peer_addr = *BLE_ADDR_ANY;
1871     for (key.idx = 0; ; key.idx++) {
1872         rc = ble_store_read_cccd(&key, &val);
1873         switch (rc) {
1874         case 0:
1875             break;
1876 
1877         case BLE_HS_ENOENT:
1878             return key.idx;
1879 
1880         default:
1881             TEST_ASSERT_FATAL(0);
1882         }
1883     }
1884 }
1885 
1886 int
ble_hs_test_util_num_our_secs(void)1887 ble_hs_test_util_num_our_secs(void)
1888 {
1889     struct ble_store_value_sec val;
1890     struct ble_store_key_sec key = { };
1891     int rc;
1892 
1893     key.peer_addr = *BLE_ADDR_ANY;
1894     for (key.idx = 0; ; key.idx++) {
1895         rc = ble_store_read_our_sec(&key, &val);
1896         switch (rc) {
1897         case 0:
1898             break;
1899 
1900         case BLE_HS_ENOENT:
1901             return key.idx;
1902 
1903         default:
1904             TEST_ASSERT_FATAL(0);
1905         }
1906     }
1907 }
1908 
1909 int
ble_hs_test_util_num_peer_secs(void)1910 ble_hs_test_util_num_peer_secs(void)
1911 {
1912     struct ble_store_value_sec val;
1913     struct ble_store_key_sec key = { };
1914     int rc;
1915 
1916     key.peer_addr = *BLE_ADDR_ANY;
1917     for (key.idx = 0; ; key.idx++) {
1918         rc = ble_store_read_peer_sec(&key, &val);
1919         switch (rc) {
1920         case 0:
1921             break;
1922 
1923         case BLE_HS_ENOENT:
1924             return key.idx;
1925 
1926         default:
1927             TEST_ASSERT_FATAL(0);
1928         }
1929     }
1930 }
1931 
1932 static int
ble_hs_test_util_store_read(int obj_type,const union ble_store_key * key,union ble_store_value * value)1933 ble_hs_test_util_store_read(int obj_type, const union ble_store_key *key,
1934                             union ble_store_value *value)
1935 {
1936     int rc;
1937 
1938     ble_sm_test_store_obj_type = obj_type;
1939     ble_sm_test_store_key = *key;
1940 
1941     rc = ble_store_config_read(obj_type, key, value);
1942     ble_sm_test_store_value = *value;
1943 
1944     return rc;
1945 }
1946 
1947 static int
ble_hs_test_util_store_write(int obj_type,const union ble_store_value * value)1948 ble_hs_test_util_store_write(int obj_type, const union ble_store_value *value)
1949 {
1950     int rc;
1951 
1952     ble_sm_test_store_obj_type = obj_type;
1953 
1954     rc = ble_store_config_write(obj_type, value);
1955     ble_sm_test_store_value = *value;
1956 
1957     return rc;
1958 }
1959 
1960 static int
ble_hs_test_util_store_delete(int obj_type,const union ble_store_key * key)1961 ble_hs_test_util_store_delete(int obj_type, const union ble_store_key *key)
1962 {
1963     int rc;
1964 
1965     ble_sm_test_store_obj_type = obj_type;
1966     ble_sm_test_store_key = *key;
1967 
1968     rc = ble_store_config_delete(obj_type, key);
1969     return rc;
1970 }
1971 
1972 void
ble_hs_test_util_reg_svcs(const struct ble_gatt_svc_def * svcs,ble_gatt_register_fn * reg_cb,void * cb_arg)1973 ble_hs_test_util_reg_svcs(const struct ble_gatt_svc_def *svcs,
1974                           ble_gatt_register_fn *reg_cb,
1975                           void *cb_arg)
1976 {
1977     int rc;
1978 
1979     ble_hs_cfg.gatts_register_cb = reg_cb;
1980     ble_hs_cfg.gatts_register_arg = cb_arg;
1981 
1982     rc = ble_gatts_reset();
1983     TEST_ASSERT_FATAL(rc == 0);
1984 
1985     rc = ble_gatts_add_svcs(svcs);
1986     TEST_ASSERT_FATAL(rc == 0);
1987 
1988     rc = ble_gatts_start();
1989     TEST_ASSERT_FATAL(rc == 0);
1990 }
1991 
1992 
1993 void
ble_hs_test_util_init_no_sysinit_no_start(void)1994 ble_hs_test_util_init_no_sysinit_no_start(void)
1995 {
1996     STAILQ_INIT(&ble_hs_test_util_prev_tx_queue);
1997     ble_hs_test_util_prev_tx_cur = NULL;
1998 
1999     ble_hs_hci_set_phony_ack_cb(NULL);
2000 
2001     ble_hci_trans_cfg_ll(ble_hs_test_util_hci_txed, NULL,
2002                          ble_hs_test_util_pkt_txed, NULL);
2003 
2004     ble_hs_test_util_hci_ack_set_startup();
2005 
2006     ble_hs_enabled_state = BLE_HS_ENABLED_STATE_OFF;
2007 
2008     ble_hs_max_services = 16;
2009     ble_hs_max_client_configs = 32;
2010     ble_hs_max_attrs = 64;
2011 
2012     ble_hs_test_util_hci_out_clear();
2013 
2014     ble_hs_cfg.store_read_cb = ble_hs_test_util_store_read;
2015     ble_hs_cfg.store_write_cb = ble_hs_test_util_store_write;
2016     ble_hs_cfg.store_delete_cb = ble_hs_test_util_store_delete;
2017 
2018     ble_store_clear();
2019 }
2020 
2021 void
ble_hs_test_util_init_no_start(void)2022 ble_hs_test_util_init_no_start(void)
2023 {
2024     sysinit();
2025     ble_hs_test_util_init_no_sysinit_no_start();
2026 }
2027 
2028 void
ble_hs_test_util_init(void)2029 ble_hs_test_util_init(void)
2030 {
2031     int rc;
2032 
2033     ble_hs_test_util_init_no_start();
2034 
2035     rc = ble_hs_start();
2036     TEST_ASSERT_FATAL(rc == 0);
2037 
2038     ble_hs_test_util_hci_out_clear();
2039 
2040     /* Clear random address. */
2041     ble_hs_test_util_set_static_rnd_addr((uint8_t[6]){ 0, 0, 0, 0, 0, 0 });
2042 }
2043