1 /* Bluetooth Mesh */
2
3 /*
4 * Copyright (c) 2017 Intel Corporation
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include "syscfg/syscfg.h"
10 #define MESH_LOG_MODULE BLE_MESH_CRYPTO_LOG
11
12 #if MYNEWT_VAL(BLE_MESH_FRIEND)
13
14 #include <stdint.h>
15 #include <errno.h>
16 #include <assert.h>
17
18 #include "mesh/mesh.h"
19 #include "mesh/slist.h"
20 #include "mesh_priv.h"
21 #include "crypto.h"
22 #include "adv.h"
23 #include "net.h"
24 #include "transport.h"
25 #include "access.h"
26 #include "foundation.h"
27 #include "friend.h"
28
29 /* We reserve one extra buffer for each friendship, since we need to be able
30 * to resend the last sent PDU, which sits separately outside of the queue.
31 */
32 #define FRIEND_BUF_COUNT ((MYNEWT_VAL(BLE_MESH_FRIEND_QUEUE_SIZE) + 1) * MYNEWT_VAL(BLE_MESH_FRIEND_LPN_COUNT))
33
34 static os_membuf_t friend_buf_mem[OS_MEMPOOL_SIZE(
35 FRIEND_BUF_COUNT,
36 BT_MESH_ADV_DATA_SIZE + BT_MESH_MBUF_HEADER_SIZE)];
37
38 struct os_mbuf_pool friend_os_mbuf_pool;
39 static struct os_mempool friend_buf_mempool;
40
41 #define NET_BUF_FRAGS BIT(0)
42
43 #define FRIEND_ADV(buf) CONTAINER_OF(BT_MESH_ADV(buf), struct friend_adv, adv)
44
45 /* PDUs from Friend to the LPN should only be transmitted once with the
46 * smallest possible interval (20ms).
47 */
48 #define FRIEND_XMIT BT_MESH_TRANSMIT(0, 20)
49
50 struct friend_pdu_info {
51 u16_t src;
52 u16_t dst;
53
54 u8_t seq[3];
55
56 u8_t ttl : 7,
57 ctl : 1;
58
59 u32_t iv_index;
60 };
61
62 static struct friend_adv {
63 struct bt_mesh_adv adv;
64 u16_t app_idx;
65 } adv_pool[FRIEND_BUF_COUNT];
66
adv_alloc(int id)67 static struct bt_mesh_adv *adv_alloc(int id)
68 {
69 adv_pool[id].app_idx = BT_MESH_KEY_UNUSED;
70 return &adv_pool[id].adv;
71 }
72
is_lpn_unicast(struct bt_mesh_friend * frnd,u16_t addr)73 static bool is_lpn_unicast(struct bt_mesh_friend *frnd, u16_t addr)
74 {
75 if (frnd->lpn == BT_MESH_ADDR_UNASSIGNED) {
76 return false;
77 }
78
79 return (addr >= frnd->lpn && addr < (frnd->lpn + frnd->num_elem));
80 }
81
bt_mesh_friend_find(u16_t net_idx,u16_t lpn_addr,bool valid,bool established)82 struct bt_mesh_friend *bt_mesh_friend_find(u16_t net_idx, u16_t lpn_addr,
83 bool valid, bool established)
84 {
85 int i;
86 BT_DBG("net_idx 0x%04x lpn_addr 0x%04x", net_idx, lpn_addr);
87
88 for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
89 struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
90
91 if (valid && !frnd->valid) {
92 continue;
93 }
94
95 if (established && !frnd->established) {
96 continue;
97 }
98
99 if (net_idx != BT_MESH_KEY_ANY && frnd->net_idx != net_idx) {
100 continue;
101 }
102
103 if (is_lpn_unicast(frnd, lpn_addr)) {
104 return frnd;
105 }
106 }
107
108 return NULL;
109 }
110
purge_buffers(struct net_buf_slist_t * list)111 static void purge_buffers(struct net_buf_slist_t *list)
112 {
113 struct os_mbuf *buf;
114
115 while (!net_buf_slist_is_empty(list)) {
116 buf = (void *)net_buf_slist_get(list);
117 BT_MESH_ADV(buf)->flags &= ~NET_BUF_FRAGS;
118 net_buf_unref(buf);
119 }
120 }
121
122 /* Intentionally start a little bit late into the ReceiveWindow when
123 * it's large enough. This may improve reliability with some platforms,
124 * like the PTS, where the receiver might not have sufficiently compensated
125 * for internal latencies required to start scanning.
126 */
recv_delay(struct bt_mesh_friend * frnd)127 static s32_t recv_delay(struct bt_mesh_friend *frnd)
128 {
129 #if CONFIG_BT_MESH_FRIEND_RECV_WIN > 50
130 return (s32_t)frnd->recv_delay + (CONFIG_BT_MESH_FRIEND_RECV_WIN / 5);
131 #else
132 return frnd->recv_delay;
133 #endif
134 }
135
friend_clear(struct bt_mesh_friend * frnd)136 static void friend_clear(struct bt_mesh_friend *frnd)
137 {
138 int i;
139 BT_DBG("LPN 0x%04x", frnd->lpn);
140 k_delayed_work_cancel(&frnd->timer);
141 friend_cred_del(frnd->net_idx, frnd->lpn);
142
143 if (frnd->last) {
144 /* Cancel the sending if necessary */
145 if (frnd->pending_buf) {
146 BT_MESH_ADV(frnd->last)->busy = 0;
147 }
148
149 net_buf_unref(frnd->last);
150 frnd->last = NULL;
151 }
152
153 purge_buffers(&frnd->queue);
154
155 for (i = 0; i < ARRAY_SIZE(frnd->seg); i++) {
156 struct bt_mesh_friend_seg *seg = &frnd->seg[i];
157 purge_buffers(&seg->queue);
158 seg->seg_count = 0U;
159 }
160
161 frnd->valid = 0;
162 frnd->established = 0;
163 frnd->pending_buf = 0;
164 frnd->fsn = 0;
165 frnd->queue_size = 0;
166 frnd->pending_req = 0;
167 memset(frnd->sub_list, 0, sizeof(frnd->sub_list));
168 }
169
bt_mesh_friend_clear_net_idx(u16_t net_idx)170 void bt_mesh_friend_clear_net_idx(u16_t net_idx)
171 {
172 int i;
173 BT_DBG("net_idx 0x%04x", net_idx);
174
175 for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
176 struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
177
178 if (frnd->net_idx == BT_MESH_KEY_UNUSED) {
179 continue;
180 }
181
182 if (net_idx == BT_MESH_KEY_ANY || frnd->net_idx == net_idx) {
183 friend_clear(frnd);
184 }
185 }
186 }
187
bt_mesh_friend_sec_update(u16_t net_idx)188 void bt_mesh_friend_sec_update(u16_t net_idx)
189 {
190 int i;
191 BT_DBG("net_idx 0x%04x", net_idx);
192
193 for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
194 struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
195
196 if (frnd->net_idx == BT_MESH_KEY_UNUSED) {
197 continue;
198 }
199
200 if (net_idx == BT_MESH_KEY_ANY || frnd->net_idx == net_idx) {
201 frnd->sec_update = 1;
202 }
203 }
204 }
205
bt_mesh_friend_clear(struct bt_mesh_net_rx * rx,struct os_mbuf * buf)206 int bt_mesh_friend_clear(struct bt_mesh_net_rx *rx, struct os_mbuf *buf)
207 {
208 struct bt_mesh_ctl_friend_clear *msg = (void *)buf->om_data;
209 struct bt_mesh_friend *frnd;
210 u16_t lpn_addr, lpn_counter;
211 struct bt_mesh_net_tx tx = {
212 .sub = rx->sub,
213 .ctx = &rx->ctx,
214 .src = bt_mesh_primary_addr(),
215 .xmit = bt_mesh_net_transmit_get(),
216 };
217 struct bt_mesh_ctl_friend_clear_confirm cfm;
218
219 if (buf->om_len < sizeof(*msg)) {
220 BT_WARN("Too short Friend Clear");
221 return -EINVAL;
222 }
223
224 lpn_addr = sys_be16_to_cpu(msg->lpn_addr);
225 lpn_counter = sys_be16_to_cpu(msg->lpn_counter);
226 BT_DBG("LPN addr 0x%04x counter 0x%04x", lpn_addr, lpn_counter);
227 frnd = bt_mesh_friend_find(rx->sub->net_idx, lpn_addr, false, false);
228 if (!frnd) {
229 BT_WARN("No matching LPN addr 0x%04x", lpn_addr);
230 return 0;
231 }
232
233 /* A Friend Clear message is considered valid if the result of the
234 * subtraction of the value of the LPNCounter field of the Friend
235 * Request message (the one that initiated the friendship) from the
236 * value of the LPNCounter field of the Friend Clear message, modulo
237 * 65536, is in the range 0 to 255 inclusive.
238 */
239 if (lpn_counter - frnd->lpn_counter > 255) {
240 BT_WARN("LPN Counter out of range (old %u new %u)",
241 frnd->lpn_counter, lpn_counter);
242 return 0;
243 }
244
245 tx.ctx->send_ttl = BT_MESH_TTL_MAX;
246 cfm.lpn_addr = msg->lpn_addr;
247 cfm.lpn_counter = msg->lpn_counter;
248 bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_CLEAR_CFM, &cfm,
249 sizeof(cfm), NULL, NULL, NULL);
250 friend_clear(frnd);
251 return 0;
252 }
253
friend_sub_add(struct bt_mesh_friend * frnd,u16_t addr)254 static void friend_sub_add(struct bt_mesh_friend *frnd, u16_t addr)
255 {
256 int i;
257
258 for (i = 0; i < ARRAY_SIZE(frnd->sub_list); i++) {
259 if (frnd->sub_list[i] == BT_MESH_ADDR_UNASSIGNED) {
260 frnd->sub_list[i] = addr;
261 return;
262 }
263 }
264
265 BT_WARN("No space in friend subscription list");
266 }
267
friend_sub_rem(struct bt_mesh_friend * frnd,u16_t addr)268 static void friend_sub_rem(struct bt_mesh_friend *frnd, u16_t addr)
269 {
270 int i;
271
272 for (i = 0; i < ARRAY_SIZE(frnd->sub_list); i++) {
273 if (frnd->sub_list[i] == addr) {
274 frnd->sub_list[i] = BT_MESH_ADDR_UNASSIGNED;
275 return;
276 }
277 }
278 }
279
create_friend_pdu(struct bt_mesh_friend * frnd,struct friend_pdu_info * info,struct os_mbuf * sdu)280 static struct os_mbuf *create_friend_pdu(struct bt_mesh_friend *frnd,
281 struct friend_pdu_info *info,
282 struct os_mbuf *sdu)
283 {
284 struct os_mbuf *buf;
285 buf = bt_mesh_adv_create_from_pool(&friend_os_mbuf_pool, adv_alloc,
286 BT_MESH_ADV_DATA,
287 FRIEND_XMIT, K_NO_WAIT);
288 if (!buf) {
289 return NULL;
290 }
291
292 net_buf_add_u8(buf, (info->iv_index & 1) << 7); /* Will be reset in encryption */
293
294 if (info->ctl) {
295 net_buf_add_u8(buf, info->ttl | 0x80);
296 } else {
297 net_buf_add_u8(buf, info->ttl);
298 }
299
300 net_buf_add_mem(buf, info->seq, sizeof(info->seq));
301 net_buf_add_be16(buf, info->src);
302 net_buf_add_be16(buf, info->dst);
303 net_buf_add_mem(buf, sdu->om_data, sdu->om_len);
304 return buf;
305 }
306
307 struct unseg_app_sdu_meta {
308 struct bt_mesh_net_rx net;
309 const u8_t *key;
310 struct bt_mesh_subnet *subnet;
311 bool is_dev_key;
312 u8_t aid;
313 u8_t *ad;
314 };
315
unseg_app_sdu_unpack(struct bt_mesh_friend * frnd,struct os_mbuf * buf,struct unseg_app_sdu_meta * meta)316 static int unseg_app_sdu_unpack(struct bt_mesh_friend *frnd,
317 struct os_mbuf *buf,
318 struct unseg_app_sdu_meta *meta)
319 {
320 u16_t app_idx = FRIEND_ADV(buf)->app_idx;
321 int err;
322 meta->subnet = bt_mesh_subnet_get(frnd->net_idx);
323 meta->is_dev_key = (app_idx == BT_MESH_KEY_DEV);
324 meta->is_dev_key = BT_MESH_IS_DEV_KEY(app_idx);
325 bt_mesh_net_header_parse(buf, &meta->net);
326 err = bt_mesh_app_key_get(meta->subnet, app_idx, meta->net.ctx.recv_dst,
327 &meta->key, &meta->aid);
328 if (err) {
329 return err;
330 }
331
332 if (BT_MESH_ADDR_IS_VIRTUAL(meta->net.ctx.recv_dst)) {
333 meta->ad = bt_mesh_label_uuid_get(meta->net.ctx.recv_dst);
334
335 if (!meta->ad) {
336 return -ENOENT;
337 }
338 } else {
339 meta->ad = NULL;
340 }
341
342 return 0;
343 }
344
unseg_app_sdu_decrypt(struct bt_mesh_friend * frnd,struct os_mbuf * buf,const struct unseg_app_sdu_meta * meta)345 static int unseg_app_sdu_decrypt(struct bt_mesh_friend *frnd,
346 struct os_mbuf *buf,
347 const struct unseg_app_sdu_meta *meta)
348 {
349 struct net_buf_simple_state state;
350 int err;
351 BT_DBG("");
352 net_buf_simple_save(buf, &state);
353 net_buf_simple_pull_mem(buf, 10);
354 buf->om_len -= 4;
355 err = bt_mesh_app_decrypt_in_place(meta->key, meta->is_dev_key,
356 0, buf, meta->ad, meta->net.ctx.addr,
357 meta->net.ctx.recv_dst, meta->net.seq,
358 BT_MESH_NET_IVI_TX);
359 net_buf_simple_restore(buf, &state);
360 return err;
361 }
362
unseg_app_sdu_encrypt(struct bt_mesh_friend * frnd,struct os_mbuf * buf,const struct unseg_app_sdu_meta * meta)363 static int unseg_app_sdu_encrypt(struct bt_mesh_friend *frnd,
364 struct os_mbuf *buf,
365 const struct unseg_app_sdu_meta *meta)
366 {
367 struct net_buf_simple_state state;
368 int err;
369 BT_DBG("");
370 net_buf_simple_save(buf, &state);
371 net_buf_simple_pull_mem(buf, 10);
372 buf->om_len -= 4;
373 err = bt_mesh_app_encrypt_in_place(meta->key, meta->is_dev_key, 0, buf,
374 meta->ad, meta->net.ctx.addr,
375 meta->net.ctx.recv_dst, bt_mesh.seq,
376 BT_MESH_NET_IVI_TX);
377 net_buf_simple_restore(buf, &state);
378 return err;
379 }
380
unseg_app_sdu_prepare(struct bt_mesh_friend * frnd,struct os_mbuf * buf)381 static int unseg_app_sdu_prepare(struct bt_mesh_friend *frnd,
382 struct os_mbuf *buf)
383 {
384 struct unseg_app_sdu_meta meta;
385 int err;
386 BT_DBG("");
387
388 if (FRIEND_ADV(buf)->app_idx == BT_MESH_KEY_UNUSED) {
389 return 0;
390 }
391
392 err = unseg_app_sdu_unpack(frnd, buf, &meta);
393 if (err) {
394 return err;
395 }
396
397 /* No need to reencrypt the message if the sequence number is
398 * unchanged.
399 */
400 if (meta.net.seq == bt_mesh.seq) {
401 return 0;
402 }
403
404 err = unseg_app_sdu_decrypt(frnd, buf, &meta);
405 if (err) {
406 BT_WARN("Decryption failed! %d", err);
407 return err;
408 }
409
410 err = unseg_app_sdu_encrypt(frnd, buf, &meta);
411 if (err) {
412 BT_WARN("Re-encryption failed! %d", err);
413 }
414
415 return err;
416 }
417
encrypt_friend_pdu(struct bt_mesh_friend * frnd,struct os_mbuf * buf,bool master_cred)418 static int encrypt_friend_pdu(struct bt_mesh_friend *frnd, struct os_mbuf *buf,
419 bool master_cred)
420 {
421 struct bt_mesh_subnet *sub = bt_mesh_subnet_get(frnd->net_idx);
422 const u8_t *enc, *priv;
423 u32_t iv_index;
424 u16_t src;
425 u8_t nid;
426 int err;
427
428 if (master_cred) {
429 enc = sub->keys[sub->kr_flag].enc;
430 priv = sub->keys[sub->kr_flag].privacy;
431 nid = sub->keys[sub->kr_flag].nid;
432 } else {
433 if (friend_cred_get(sub, frnd->lpn, &nid, &enc, &priv)) {
434 BT_ERR("friend_cred_get failed");
435 return -ENOENT;
436 }
437 }
438
439 src = sys_get_be16(&buf->om_data[5]);
440 if (bt_mesh_elem_find(src)) {
441 u32_t seq;
442
443 if (FRIEND_ADV(buf)->app_idx != BT_MESH_KEY_UNUSED) {
444 err = unseg_app_sdu_prepare(frnd, buf);
445 if (err) {
446 return err;
447 }
448 }
449
450 seq = bt_mesh_next_seq();
451 buf->om_data[2] = seq >> 16;
452 buf->om_data[3] = seq >> 8;
453 buf->om_data[4] = seq;
454 iv_index = BT_MESH_NET_IVI_TX;
455 FRIEND_ADV(buf)->app_idx = BT_MESH_KEY_UNUSED;
456 } else {
457 u8_t ivi = (buf->om_data[0] >> 7);
458 iv_index = (bt_mesh.iv_index - ((bt_mesh.iv_index & 1) != ivi));
459 }
460
461 buf->om_data[0] = (nid | ((iv_index & 1) << 7)); // 7:byte alignment
462
463 if (bt_mesh_net_encrypt(enc, buf, iv_index, false)) {
464 BT_ERR("Encrypting failed");
465 return -EINVAL;
466 }
467
468 if (bt_mesh_net_obfuscate(buf->om_data, iv_index, priv)) {
469 BT_ERR("Obfuscating failed");
470 return -EINVAL;
471 }
472
473 return 0;
474 }
475
encode_friend_ctl(struct bt_mesh_friend * frnd,u8_t ctl_op,struct os_mbuf * sdu)476 static struct os_mbuf *encode_friend_ctl(struct bt_mesh_friend *frnd,
477 u8_t ctl_op,
478 struct os_mbuf *sdu)
479 {
480 struct friend_pdu_info info;
481 BT_DBG("LPN 0x%04x", frnd->lpn);
482 net_buf_simple_push_u8(sdu, TRANS_CTL_HDR(ctl_op, 0));
483 info.src = bt_mesh_primary_addr();
484 info.dst = frnd->lpn;
485 info.ctl = 1;
486 info.ttl = 0;
487 memset_s(info.seq, sizeof(info.seq), 0, sizeof(info.seq));
488 info.iv_index = BT_MESH_NET_IVI_TX;
489 return create_friend_pdu(frnd, &info, sdu);
490 }
491
encode_update(struct bt_mesh_friend * frnd,u8_t md)492 static struct os_mbuf *encode_update(struct bt_mesh_friend *frnd, u8_t md)
493 {
494 struct bt_mesh_ctl_friend_update *upd;
495 struct os_mbuf *sdu = NET_BUF_SIMPLE(1 + sizeof(*upd));
496 struct bt_mesh_subnet *sub = bt_mesh_subnet_get(frnd->net_idx);
497 struct os_mbuf *buf;
498 __ASSERT_NO_MSG(sub != NULL);
499 BT_DBG("lpn 0x%04x md 0x%02x", frnd->lpn, md);
500 net_buf_simple_init(sdu, 1);
501 upd = net_buf_simple_add(sdu, sizeof(*upd));
502 upd->flags = bt_mesh_net_flags(sub);
503 upd->iv_index = sys_cpu_to_be32(bt_mesh.iv_index);
504 upd->md = md;
505 buf = encode_friend_ctl(frnd, TRANS_CTL_OP_FRIEND_UPDATE, sdu);
506 os_mbuf_free_chain(sdu);
507 return buf;
508 }
509
enqueue_sub_cfm(struct bt_mesh_friend * frnd,u8_t xact)510 static void enqueue_sub_cfm(struct bt_mesh_friend *frnd, u8_t xact)
511 {
512 struct bt_mesh_ctl_friend_sub_confirm *cfm;
513 struct os_mbuf *sdu = NET_BUF_SIMPLE(1 + sizeof(*cfm));
514 struct os_mbuf *buf;
515 BT_DBG("lpn 0x%04x xact 0x%02x", frnd->lpn, xact);
516 net_buf_simple_init(sdu, 1);
517 cfm = net_buf_simple_add(sdu, sizeof(*cfm));
518 cfm->xact = xact;
519 buf = encode_friend_ctl(frnd, TRANS_CTL_OP_FRIEND_SUB_CFM, sdu);
520 if (!buf) {
521 BT_ERR("Unable to encode Subscription List Confirmation");
522 goto done;
523 }
524
525 if (encrypt_friend_pdu(frnd, buf, false)) {
526 return;
527 }
528
529 if (frnd->last) {
530 BT_DBG("Discarding last PDU");
531 net_buf_unref(frnd->last);
532 }
533
534 frnd->last = buf;
535 frnd->send_last = 1;
536 done:
537 os_mbuf_free_chain(sdu);
538 }
539
friend_recv_delay(struct bt_mesh_friend * frnd)540 static void friend_recv_delay(struct bt_mesh_friend *frnd)
541 {
542 frnd->pending_req = 1;
543 k_delayed_work_submit(&frnd->timer, recv_delay(frnd));
544 BT_DBG("Waiting RecvDelay of %d ms", (int) recv_delay(frnd));
545 }
546
bt_mesh_friend_sub_add(struct bt_mesh_net_rx * rx,struct os_mbuf * buf)547 int bt_mesh_friend_sub_add(struct bt_mesh_net_rx *rx,
548 struct os_mbuf *buf)
549 {
550 struct bt_mesh_friend *frnd;
551 u8_t xact;
552
553 if (buf->om_len < BT_MESH_FRIEND_SUB_MIN_LEN) {
554 BT_WARN("Too short Friend Subscription Add");
555 return -EINVAL;
556 }
557
558 frnd = bt_mesh_friend_find(rx->sub->net_idx, rx->ctx.addr, true, true);
559 if (!frnd) {
560 BT_WARN("No matching LPN addr 0x%04x", rx->ctx.addr);
561 return 0;
562 }
563
564 if (frnd->pending_buf) {
565 BT_WARN("Previous buffer not yet sent!");
566 return 0;
567 }
568
569 friend_recv_delay(frnd);
570 xact = net_buf_simple_pull_u8(buf);
571
572 while (buf->om_len >= 2) {
573 friend_sub_add(frnd, net_buf_simple_pull_be16(buf));
574 }
575
576 enqueue_sub_cfm(frnd, xact);
577 return 0;
578 }
579
bt_mesh_friend_sub_rem(struct bt_mesh_net_rx * rx,struct os_mbuf * buf)580 int bt_mesh_friend_sub_rem(struct bt_mesh_net_rx *rx,
581 struct os_mbuf *buf)
582 {
583 struct bt_mesh_friend *frnd;
584 u8_t xact;
585
586 if (buf->om_len < BT_MESH_FRIEND_SUB_MIN_LEN) {
587 BT_WARN("Too short Friend Subscription Remove");
588 return -EINVAL;
589 }
590
591 frnd = bt_mesh_friend_find(rx->sub->net_idx, rx->ctx.addr, true, true);
592 if (!frnd) {
593 BT_WARN("No matching LPN addr 0x%04x", rx->ctx.addr);
594 return 0;
595 }
596
597 if (frnd->pending_buf) {
598 BT_WARN("Previous buffer not yet sent!");
599 return 0;
600 }
601
602 friend_recv_delay(frnd);
603 xact = net_buf_simple_pull_u8(buf);
604
605 while (buf->om_len >= 2) {
606 friend_sub_rem(frnd, net_buf_simple_pull_be16(buf));
607 }
608
609 enqueue_sub_cfm(frnd, xact);
610 return 0;
611 }
612
enqueue_buf(struct bt_mesh_friend * frnd,struct os_mbuf * buf)613 static void enqueue_buf(struct bt_mesh_friend *frnd, struct os_mbuf *buf)
614 {
615 net_buf_slist_put(&frnd->queue, buf);
616 frnd->queue_size++;
617 }
618
enqueue_update(struct bt_mesh_friend * frnd,u8_t md)619 static void enqueue_update(struct bt_mesh_friend *frnd, u8_t md)
620 {
621 struct os_mbuf *buf;
622 buf = encode_update(frnd, md);
623 if (!buf) {
624 BT_ERR("Unable to encode Friend Update");
625 return;
626 }
627
628 frnd->sec_update = 0;
629 enqueue_buf(frnd, buf);
630 }
631
bt_mesh_friend_poll(struct bt_mesh_net_rx * rx,struct os_mbuf * buf)632 int bt_mesh_friend_poll(struct bt_mesh_net_rx *rx, struct os_mbuf *buf)
633 {
634 struct bt_mesh_ctl_friend_poll *msg = (void *)buf->om_data;
635 struct bt_mesh_friend *frnd;
636
637 if (buf->om_len < sizeof(*msg)) {
638 BT_WARN("Too short Friend Poll");
639 return -EINVAL;
640 }
641
642 frnd = bt_mesh_friend_find(rx->sub->net_idx, rx->ctx.addr, true, false);
643 if (!frnd) {
644 BT_WARN("No matching LPN addr 0x%04x", rx->ctx.addr);
645 return 0;
646 }
647
648 if (msg->fsn & ~1) {
649 BT_WARN("Prohibited (non-zero) padding bits");
650 return -EINVAL;
651 }
652
653 if (frnd->pending_buf) {
654 BT_WARN("Previous buffer not yet sent");
655 return 0;
656 }
657
658 BT_DBG("msg->fsn %u frnd->fsn %u", (msg->fsn & 1), frnd->fsn);
659 friend_recv_delay(frnd);
660
661 if (!frnd->established) {
662 BT_DBG("Friendship established with 0x%04x", frnd->lpn);
663 frnd->established = 1;
664 }
665
666 if (msg->fsn == frnd->fsn && frnd->last) {
667 BT_DBG("Re-sending last PDU");
668 frnd->send_last = 1;
669 } else {
670 if (frnd->last) {
671 net_buf_unref(frnd->last);
672 frnd->last = NULL;
673 }
674
675 frnd->fsn = msg->fsn;
676
677 if (net_buf_slist_is_empty(&frnd->queue)) {
678 enqueue_update(frnd, 0);
679 BT_DBG("Enqueued Friend Update to empty queue");
680 }
681 }
682
683 return 0;
684 }
685
find_clear(u16_t prev_friend)686 static struct bt_mesh_friend *find_clear(u16_t prev_friend)
687 {
688 int i;
689
690 for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
691 struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
692
693 if (frnd->clear.frnd == prev_friend) {
694 return frnd;
695 }
696 }
697
698 return NULL;
699 }
700
friend_clear_sent(int err,void * user_data)701 static void friend_clear_sent(int err, void *user_data)
702 {
703 struct bt_mesh_friend *frnd = user_data;
704 k_delayed_work_submit(&frnd->clear.timer,
705 K_SECONDS(frnd->clear.repeat_sec));
706 frnd->clear.repeat_sec *= 2;
707 }
708
709 static const struct bt_mesh_send_cb clear_sent_cb = {
710 .end = friend_clear_sent,
711 };
712
send_friend_clear(struct bt_mesh_friend * frnd)713 static void send_friend_clear(struct bt_mesh_friend *frnd)
714 {
715 struct bt_mesh_msg_ctx ctx = {
716 .net_idx = frnd->net_idx,
717 .app_idx = BT_MESH_KEY_UNUSED,
718 .addr = frnd->clear.frnd,
719 .send_ttl = BT_MESH_TTL_MAX,
720 };
721 struct bt_mesh_net_tx tx = {
722 .sub = &bt_mesh.sub[0],
723 .ctx = &ctx,
724 .src = bt_mesh_primary_addr(),
725 .xmit = bt_mesh_net_transmit_get(),
726 };
727 struct bt_mesh_ctl_friend_clear req = {
728 .lpn_addr = sys_cpu_to_be16(frnd->lpn),
729 .lpn_counter = sys_cpu_to_be16(frnd->lpn_counter),
730 };
731 BT_DBG("");
732 bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_CLEAR, &req,
733 sizeof(req), NULL, &clear_sent_cb, frnd);
734 }
735
clear_timeout(struct ble_npl_event * work)736 static void clear_timeout(struct ble_npl_event *work)
737 {
738 struct bt_mesh_friend *frnd = ble_npl_event_get_arg(work);
739 u32_t duration;
740 BT_DBG("LPN 0x%04x (old) Friend 0x%04x", frnd->lpn, frnd->clear.frnd);
741 duration = k_uptime_get_32() - frnd->clear.start;
742 if (duration > 2 * frnd->poll_to) {
743 BT_DBG("Clear Procedure timer expired");
744 frnd->clear.frnd = BT_MESH_ADDR_UNASSIGNED;
745 return;
746 }
747
748 send_friend_clear(frnd);
749 }
750
clear_procedure_start(struct bt_mesh_friend * frnd)751 static void clear_procedure_start(struct bt_mesh_friend *frnd)
752 {
753 BT_DBG("LPN 0x%04x (old) Friend 0x%04x", frnd->lpn, frnd->clear.frnd);
754 frnd->clear.start = k_uptime_get_32();
755 frnd->clear.repeat_sec = 1U;
756 send_friend_clear(frnd);
757 }
758
bt_mesh_friend_clear_cfm(struct bt_mesh_net_rx * rx,struct os_mbuf * buf)759 int bt_mesh_friend_clear_cfm(struct bt_mesh_net_rx *rx,
760 struct os_mbuf *buf)
761 {
762 struct bt_mesh_ctl_friend_clear_confirm *msg = (void *)buf->om_data;
763 struct bt_mesh_friend *frnd;
764 u16_t lpn_addr, lpn_counter;
765 BT_DBG("");
766
767 if (buf->om_len < sizeof(*msg)) {
768 BT_WARN("Too short Friend Clear Confirm");
769 return -EINVAL;
770 }
771
772 frnd = find_clear(rx->ctx.addr);
773 if (!frnd) {
774 BT_WARN("No pending clear procedure for 0x%02x", rx->ctx.addr);
775 return 0;
776 }
777
778 lpn_addr = sys_be16_to_cpu(msg->lpn_addr);
779 if (lpn_addr != frnd->lpn) {
780 BT_WARN("LPN address mismatch (0x%04x != 0x%04x)",
781 lpn_addr, frnd->lpn);
782 return 0;
783 }
784
785 lpn_counter = sys_be16_to_cpu(msg->lpn_counter);
786 if (lpn_counter != frnd->lpn_counter) {
787 BT_WARN("LPN counter mismatch (0x%04x != 0x%04x)",
788 lpn_counter, frnd->lpn_counter);
789 return 0;
790 }
791
792 k_delayed_work_cancel(&frnd->clear.timer);
793 frnd->clear.frnd = BT_MESH_ADDR_UNASSIGNED;
794 return 0;
795 }
796
enqueue_offer(struct bt_mesh_friend * frnd,s8_t rssi)797 static void enqueue_offer(struct bt_mesh_friend *frnd, s8_t rssi)
798 {
799 struct bt_mesh_ctl_friend_offer *off;
800 struct os_mbuf *sdu = NET_BUF_SIMPLE(1 + sizeof(*off));
801 struct os_mbuf *buf;
802 BT_DBG("");
803 net_buf_simple_init(sdu, 1);
804 off = net_buf_simple_add(sdu, sizeof(*off));
805 off->recv_win = CONFIG_BT_MESH_FRIEND_RECV_WIN,
806 off->queue_size = CONFIG_BT_MESH_FRIEND_QUEUE_SIZE,
807 off->sub_list_size = ARRAY_SIZE(frnd->sub_list),
808 off->rssi = rssi,
809 off->frnd_counter = sys_cpu_to_be16(frnd->counter);
810 buf = encode_friend_ctl(frnd, TRANS_CTL_OP_FRIEND_OFFER, sdu);
811 if (!buf) {
812 BT_ERR("Unable to encode Friend Offer");
813 goto done;
814 }
815
816 if (encrypt_friend_pdu(frnd, buf, true)) {
817 return;
818 }
819
820 frnd->counter++;
821
822 if (frnd->last) {
823 net_buf_unref(frnd->last);
824 }
825
826 frnd->last = buf;
827 frnd->send_last = 1;
828 done:
829 os_mbuf_free_chain(sdu);
830 }
831
832 #define RECV_WIN CONFIG_BT_MESH_FRIEND_RECV_WIN
833 #define RSSI_FACT(crit) (((crit) >> 5) & (u8_t)BIT_MASK(2))
834 #define RECV_WIN_FACT(crit) (((crit) >> 3) & (u8_t)BIT_MASK(2))
835 #define MIN_QUEUE_SIZE_LOG(crit) ((crit) & (u8_t)BIT_MASK(3))
836 #define MIN_QUEUE_SIZE(crit) ((u32_t)BIT(MIN_QUEUE_SIZE_LOG(crit)))
837
offer_delay(struct bt_mesh_friend * frnd,s8_t rssi,u8_t crit)838 static s32_t offer_delay(struct bt_mesh_friend *frnd, s8_t rssi, u8_t crit)
839 {
840 /* Scaling factors. The actual values are 1, 1.5, 2 & 2.5, but we
841 * want to avoid floating-point arithmetic.
842 */
843 static const u8_t fact[] = { 10, 15, 20, 25 };
844 s32_t delay;
845 BT_DBG("ReceiveWindowFactor %u ReceiveWindow %u RSSIFactor %u RSSI %d",
846 fact[RECV_WIN_FACT(crit)], RECV_WIN,
847 fact[RSSI_FACT(crit)], rssi);
848 /* Delay = ReceiveWindowFactor * ReceiveWindow - RSSIFactor * RSSI */
849 delay = (s32_t)fact[RECV_WIN_FACT(crit)] * RECV_WIN;
850 delay -= (s32_t)fact[RSSI_FACT(crit)] * rssi;
851 delay /= 10;
852 BT_DBG("Local Delay calculated as %d ms", (int) delay);
853 if (delay < 100) {
854 return K_MSEC(100);
855 }
856
857 return K_MSEC(delay);
858 }
859
bt_mesh_friend_req(struct bt_mesh_net_rx * rx,struct os_mbuf * buf)860 int bt_mesh_friend_req(struct bt_mesh_net_rx *rx, struct os_mbuf *buf)
861 {
862 struct bt_mesh_ctl_friend_req *msg = (void *)buf->om_data;
863 struct bt_mesh_friend *frnd = NULL;
864 u32_t poll_to;
865 int i;
866
867 if (buf->om_len < sizeof(*msg)) {
868 BT_WARN("Too short Friend Request");
869 return -EINVAL;
870 }
871
872 if (msg->recv_delay <= 0x09) {
873 BT_WARN("Prohibited ReceiveDelay (0x%02x)", msg->recv_delay);
874 return -EINVAL;
875 }
876
877 poll_to = (((u32_t)msg->poll_to[0] << 16) |
878 ((u32_t)msg->poll_to[1] << 8) |
879 ((u32_t)msg->poll_to[2]));
880 if (poll_to <= 0x000009 || poll_to >= 0x34bc00) {
881 BT_WARN("Prohibited PollTimeout (0x%06x)", (unsigned) poll_to);
882 return -EINVAL;
883 }
884
885 if (msg->num_elem == 0x00) {
886 BT_WARN("Prohibited NumElements value (0x00)");
887 return -EINVAL;
888 }
889
890 if (!BT_MESH_ADDR_IS_UNICAST(rx->ctx.addr + msg->num_elem - 1)) {
891 BT_WARN("LPN elements stretch outside of unicast range");
892 return -EINVAL;
893 }
894
895 if (!MIN_QUEUE_SIZE_LOG(msg->criteria)) {
896 BT_WARN("Prohibited Minimum Queue Size in Friend Request");
897 return -EINVAL;
898 }
899
900 if (CONFIG_BT_MESH_FRIEND_QUEUE_SIZE < MIN_QUEUE_SIZE(msg->criteria)) {
901 BT_WARN("We have a too small Friend Queue size (%u < %u)",
902 CONFIG_BT_MESH_FRIEND_QUEUE_SIZE,
903 (unsigned) MIN_QUEUE_SIZE(msg->criteria));
904 return 0;
905 }
906
907 frnd = bt_mesh_friend_find(rx->sub->net_idx, rx->ctx.addr, true, false);
908 if (frnd) {
909 BT_WARN("Existing LPN re-requesting Friendship");
910 friend_clear(frnd);
911 goto init_friend;
912 }
913
914 for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
915 if (!bt_mesh.frnd[i].valid) {
916 frnd = &bt_mesh.frnd[i];
917 frnd->valid = 1;
918 break;
919 }
920 }
921
922 if (!frnd) {
923 BT_WARN("No free Friend contexts for new LPN");
924 return -ENOMEM;
925 }
926
927 init_friend:
928 frnd->lpn = rx->ctx.addr;
929 frnd->num_elem = msg->num_elem;
930 frnd->net_idx = rx->sub->net_idx;
931 frnd->recv_delay = msg->recv_delay;
932 frnd->poll_to = poll_to * 100;
933 frnd->lpn_counter = sys_be16_to_cpu(msg->lpn_counter);
934 frnd->clear.frnd = sys_be16_to_cpu(msg->prev_addr);
935 BT_DBG("LPN 0x%04x rssi %d recv_delay %u poll_to %ums",
936 frnd->lpn, rx->ctx.recv_rssi, frnd->recv_delay,
937 (unsigned) frnd->poll_to);
938
939 if (BT_MESH_ADDR_IS_UNICAST(frnd->clear.frnd) &&
940 !bt_mesh_elem_find(frnd->clear.frnd)) {
941 clear_procedure_start(frnd);
942 }
943
944 k_delayed_work_submit(&frnd->timer,
945 offer_delay(frnd, rx->ctx.recv_rssi,
946 msg->criteria));
947 friend_cred_create(rx->sub, frnd->lpn, frnd->lpn_counter,
948 frnd->counter);
949 enqueue_offer(frnd, rx->ctx.recv_rssi);
950 return 0;
951 }
952
is_seg(struct bt_mesh_friend_seg * seg,u16_t src,u16_t seq_zero)953 static bool is_seg(struct bt_mesh_friend_seg *seg, u16_t src, u16_t seq_zero)
954 {
955 struct os_mbuf *buf = (void *)net_buf_slist_peek_head(&seg->queue);
956 struct net_buf_simple_state state;
957 u16_t buf_seq_zero;
958 u16_t buf_src;
959 if (!buf) {
960 return false;
961 }
962
963 net_buf_simple_save(buf, &state);
964 net_buf_skip(buf, 5); /* skip IVI, NID, CTL, TTL, SEQ */
965 buf_src = net_buf_pull_be16(buf);
966 net_buf_skip(buf, 3); /* skip DST, OP/AID */
967 buf_seq_zero = ((net_buf_pull_be16(buf) >> 2) & TRANS_SEQ_ZERO_MASK);
968 net_buf_simple_restore(buf, &state);
969 return ((src == buf_src) && (seq_zero == buf_seq_zero));
970 }
971
get_seg(struct bt_mesh_friend * frnd,u16_t src,u16_t seq_zero,u8_t seg_count)972 static struct bt_mesh_friend_seg *get_seg(struct bt_mesh_friend *frnd,
973 u16_t src, u16_t seq_zero,
974 u8_t seg_count)
975 {
976 struct bt_mesh_friend_seg *unassigned = NULL;
977 int i;
978
979 for (i = 0; i < ARRAY_SIZE(frnd->seg); i++) {
980 struct bt_mesh_friend_seg *seg = &frnd->seg[i];
981
982 if (is_seg(seg, src, seq_zero)) {
983 return seg;
984 }
985
986 if (!unassigned && !net_buf_slist_peek_head(&seg->queue)) {
987 unassigned = seg;
988 }
989 }
990
991 if (unassigned) {
992 unassigned->seg_count = seg_count;
993 }
994
995 return unassigned;
996 }
997
enqueue_friend_pdu(struct bt_mesh_friend * frnd,enum bt_mesh_friend_pdu_type type,u16_t src,u8_t seg_count,struct os_mbuf * buf)998 static void enqueue_friend_pdu(struct bt_mesh_friend *frnd,
999 enum bt_mesh_friend_pdu_type type,
1000 u16_t src, u8_t seg_count,
1001 struct os_mbuf *buf)
1002 {
1003 struct bt_mesh_friend_seg *seg;
1004 BT_DBG("type %u", type);
1005
1006 if (type == BT_MESH_FRIEND_PDU_SINGLE) {
1007 if (frnd->sec_update) {
1008 enqueue_update(frnd, 1);
1009 }
1010
1011 enqueue_buf(frnd, buf);
1012 return;
1013 }
1014
1015 u16_t seq_zero = (((buf->om_data[10] << 8 | buf->om_data[11]) >> 2) & TRANS_SEQ_ZERO_MASK);
1016 seg = get_seg(frnd, src, seq_zero, seg_count);
1017 if (!seg) {
1018 BT_ERR("No free friend segment RX contexts for 0x%04x", src);
1019 net_buf_unref(buf);
1020 return;
1021 }
1022
1023 net_buf_slist_put(&seg->queue, buf);
1024
1025 if (type == BT_MESH_FRIEND_PDU_COMPLETE) {
1026 if (frnd->sec_update) {
1027 enqueue_update(frnd, 1);
1028 }
1029
1030 net_buf_slist_merge_slist(&frnd->queue, &seg->queue);
1031 frnd->queue_size += seg->seg_count;
1032 seg->seg_count = 0U;
1033 } else {
1034 /* Mark the buffer as having more to come after it */
1035 BT_MESH_ADV(buf)->flags |= NET_BUF_FRAGS;
1036 }
1037 }
1038
buf_send_start(u16_t duration,int err,void * user_data)1039 static void buf_send_start(u16_t duration, int err, void *user_data)
1040 {
1041 struct bt_mesh_friend *frnd = user_data;
1042 BT_DBG("err %d", err);
1043 frnd->pending_buf = 0;
1044
1045 /* Friend Offer doesn't follow the re-sending semantics */
1046 if (!frnd->established) {
1047 net_buf_unref(frnd->last);
1048 frnd->last = NULL;
1049 }
1050 }
1051
buf_send_end(int err,void * user_data)1052 static void buf_send_end(int err, void *user_data)
1053 {
1054 struct bt_mesh_friend *frnd = user_data;
1055 BT_DBG("err %d", err);
1056
1057 if (frnd->pending_req) {
1058 BT_WARN("Another request before previous completed sending");
1059 return;
1060 }
1061
1062 if (frnd->established) {
1063 k_delayed_work_submit(&frnd->timer, frnd->poll_to);
1064 BT_DBG("Waiting %u ms for next poll",
1065 (unsigned) frnd->poll_to);
1066 } else {
1067 /* Friend offer timeout is 1 second */
1068 k_delayed_work_submit(&frnd->timer, K_SECONDS(1));
1069 BT_DBG("Waiting for first poll");
1070 }
1071 }
1072
friend_timeout(struct ble_npl_event * work)1073 static void friend_timeout(struct ble_npl_event *work)
1074 {
1075 struct bt_mesh_friend *frnd = ble_npl_event_get_arg(work);
1076 static const struct bt_mesh_send_cb buf_sent_cb = {
1077 .start = buf_send_start,
1078 .end = buf_send_end,
1079 };
1080 __ASSERT_NO_MSG(frnd->pending_buf == 0);
1081 BT_DBG("lpn 0x%04x send_last %u last %p", frnd->lpn,
1082 frnd->send_last, frnd->last);
1083
1084 if (frnd->send_last && frnd->last) {
1085 BT_DBG("Sending frnd->last %p", frnd->last);
1086 frnd->send_last = 0;
1087 goto send_last;
1088 }
1089
1090 if (frnd->established && !frnd->pending_req) {
1091 BT_WARN("Friendship lost with 0x%04x", frnd->lpn);
1092 friend_clear(frnd);
1093 return;
1094 }
1095
1096 frnd->last = (void *)net_buf_slist_get(&frnd->queue);
1097
1098 if (!frnd->last) {
1099 BT_WARN("Friendship not established with 0x%04x",
1100 frnd->lpn);
1101 friend_clear(frnd);
1102 return;
1103 }
1104
1105 if (encrypt_friend_pdu(frnd, frnd->last, false)) {
1106 return;
1107 }
1108
1109 /* Clear the flag we use for segment tracking */
1110 BT_MESH_ADV(frnd->last)->flags &= ~NET_BUF_FRAGS;
1111 BT_DBG("Sending buf %p from Friend Queue of LPN 0x%04x",
1112 frnd->last, frnd->lpn);
1113 frnd->queue_size--;
1114 send_last:
1115 frnd->pending_req = 0;
1116 frnd->pending_buf = 1;
1117 bt_mesh_adv_send(frnd->last, &buf_sent_cb, frnd);
1118 }
1119
bt_mesh_friend_init(void)1120 int bt_mesh_friend_init(void)
1121 {
1122 int rc;
1123 int i;
1124 rc = os_mempool_init(&friend_buf_mempool, FRIEND_BUF_COUNT,
1125 BT_MESH_ADV_DATA_SIZE + BT_MESH_MBUF_HEADER_SIZE,
1126 friend_buf_mem, "friend_buf_pool");
1127 assert(rc == 0);
1128 rc = os_mbuf_pool_init(&friend_os_mbuf_pool, &friend_buf_mempool,
1129 BT_MESH_ADV_DATA_SIZE + BT_MESH_MBUF_HEADER_SIZE,
1130 FRIEND_BUF_COUNT);
1131 assert(rc == 0);
1132
1133 for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
1134 struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
1135 int j;
1136 frnd->net_idx = BT_MESH_KEY_UNUSED;
1137 net_buf_slist_init(&frnd->queue);
1138 k_delayed_work_init(&frnd->timer, friend_timeout);
1139 k_delayed_work_add_arg(&frnd->timer, frnd);
1140 k_delayed_work_init(&frnd->clear.timer, clear_timeout);
1141 k_delayed_work_add_arg(&frnd->clear.timer, frnd);
1142
1143 for (j = 0; j < ARRAY_SIZE(frnd->seg); j++) {
1144 net_buf_slist_init(&frnd->seg[j].queue);
1145 }
1146 }
1147
1148 return 0;
1149 }
1150
is_segack(struct os_mbuf * buf,u64_t * seqauth,u16_t src)1151 static bool is_segack(struct os_mbuf *buf, u64_t *seqauth, u16_t src)
1152 {
1153 struct net_buf_simple_state state;
1154 bool found = false;
1155
1156 if (buf->om_len != 16) {
1157 return false;
1158 }
1159
1160 net_buf_simple_save(buf, &state);
1161 net_buf_skip(buf, 1); /* skip IVI, NID */
1162
1163 if (!(net_buf_pull_u8(buf) >> 7)) {
1164 goto end;
1165 }
1166
1167 net_buf_pull(buf, 3); /* skip SEQNUM */
1168
1169 if (src != net_buf_pull_be16(buf)) {
1170 goto end;
1171 }
1172
1173 net_buf_skip(buf, 2); /* skip dst */
1174
1175 if (TRANS_CTL_OP((u8_t *) net_buf_pull_mem(buf, 1)) != TRANS_CTL_OP_ACK) {
1176 goto end;
1177 }
1178
1179 found = ((net_buf_pull_be16(buf) >> 2) & TRANS_SEQ_ZERO_MASK) ==
1180 (*seqauth & TRANS_SEQ_ZERO_MASK);
1181 end:
1182 net_buf_simple_restore(buf, &state);
1183 return found;
1184 }
1185
friend_purge_old_ack(struct bt_mesh_friend * frnd,u64_t * seq_auth,u16_t src)1186 static void friend_purge_old_ack(struct bt_mesh_friend *frnd, u64_t *seq_auth,
1187 u16_t src)
1188 {
1189 struct os_mbuf *cur, *prev = NULL;
1190 BT_DBG("SeqAuth %llx src 0x%04x", *seq_auth, src);
1191
1192 for (cur = net_buf_slist_peek_head(&frnd->queue);
1193 cur != NULL; prev = cur, cur = net_buf_slist_peek_next(cur)) {
1194 struct os_mbuf *buf = (void *)cur;
1195
1196 if (is_segack(buf, seq_auth, src)) {
1197 BT_DBG("Removing old ack from Friend Queue");
1198 net_buf_slist_remove(&frnd->queue, prev, cur);
1199 frnd->queue_size--;
1200 net_buf_unref(buf);
1201 break;
1202 }
1203 }
1204 }
1205
friend_lpn_enqueue_rx(struct bt_mesh_friend * frnd,struct bt_mesh_net_rx * rx,enum bt_mesh_friend_pdu_type type,u64_t * seq_auth,u8_t seg_count,struct os_mbuf * sbuf)1206 static void friend_lpn_enqueue_rx(struct bt_mesh_friend *frnd,
1207 struct bt_mesh_net_rx *rx,
1208 enum bt_mesh_friend_pdu_type type,
1209 u64_t *seq_auth, u8_t seg_count,
1210 struct os_mbuf *sbuf)
1211 {
1212 struct friend_pdu_info info;
1213 struct os_mbuf *buf;
1214
1215 /* Because of network loopback, tx packets will also be passed into
1216 * this rx function. These packets have already been added to the
1217 * queue, and should be ignored.
1218 */
1219 if (bt_mesh_elem_find(rx->ctx.addr)) {
1220 return;
1221 }
1222
1223 BT_DBG("LPN 0x%04x queue_size %u", frnd->lpn,
1224 (unsigned) frnd->queue_size);
1225
1226 if (type == BT_MESH_FRIEND_PDU_SINGLE && seq_auth) {
1227 friend_purge_old_ack(frnd, seq_auth, rx->ctx.addr);
1228 }
1229
1230 info.src = rx->ctx.addr;
1231 info.dst = rx->ctx.recv_dst;
1232
1233 if (rx->net_if == BT_MESH_NET_IF_LOCAL) {
1234 info.ttl = rx->ctx.recv_ttl;
1235 } else {
1236 info.ttl = rx->ctx.recv_ttl - 1;
1237 }
1238
1239 info.ctl = rx->ctl;
1240 info.seq[0] = (rx->seq >> 16);
1241 info.seq[1] = (rx->seq >> 8);
1242 info.seq[2] = rx->seq;
1243 info.iv_index = BT_MESH_NET_IVI_RX(rx);
1244 buf = create_friend_pdu(frnd, &info, sbuf);
1245 if (!buf) {
1246 BT_ERR("Failed to encode Friend buffer");
1247 return;
1248 }
1249
1250 enqueue_friend_pdu(frnd, type, info.src, seg_count, buf);
1251 BT_DBG("Queued message for LPN 0x%04x, queue_size %u",
1252 frnd->lpn, (unsigned) frnd->queue_size);
1253 }
1254
friend_lpn_enqueue_tx(struct bt_mesh_friend * frnd,struct bt_mesh_net_tx * tx,enum bt_mesh_friend_pdu_type type,u64_t * seq_auth,u8_t seg_count,struct os_mbuf * sbuf)1255 static void friend_lpn_enqueue_tx(struct bt_mesh_friend *frnd,
1256 struct bt_mesh_net_tx *tx,
1257 enum bt_mesh_friend_pdu_type type,
1258 u64_t *seq_auth, u8_t seg_count,
1259 struct os_mbuf *sbuf)
1260 {
1261 struct friend_pdu_info info;
1262 struct os_mbuf *buf;
1263 BT_DBG("LPN 0x%04x", frnd->lpn);
1264
1265 if (type == BT_MESH_FRIEND_PDU_SINGLE && seq_auth) {
1266 friend_purge_old_ack(frnd, seq_auth, tx->src);
1267 }
1268
1269 info.src = tx->src;
1270 info.dst = tx->ctx->addr;
1271 info.ttl = tx->ctx->send_ttl;
1272 info.ctl = (tx->ctx->app_idx == BT_MESH_KEY_UNUSED);
1273 info.seq[0] = (bt_mesh.seq >> 16);
1274 info.seq[1] = (bt_mesh.seq >> 8);
1275 info.seq[2] = bt_mesh.seq;
1276 info.iv_index = BT_MESH_NET_IVI_TX;
1277 buf = create_friend_pdu(frnd, &info, sbuf);
1278 if (!buf) {
1279 BT_ERR("Failed to encode Friend buffer");
1280 return;
1281 }
1282
1283 if (type == BT_MESH_FRIEND_PDU_SINGLE && !info.ctl) {
1284 /* Unsegmented application packets may be reencrypted later,
1285 * as they depend on the the sequence number being the same
1286 * when encrypting in transport and network.
1287 */
1288 FRIEND_ADV(buf)->app_idx = tx->ctx->app_idx;
1289 }
1290
1291 enqueue_friend_pdu(frnd, type, info.src, seg_count, buf);
1292 BT_DBG("Queued message for LPN 0x%04x", frnd->lpn);
1293 }
1294
friend_lpn_matches(struct bt_mesh_friend * frnd,u16_t net_idx,u16_t addr)1295 static bool friend_lpn_matches(struct bt_mesh_friend *frnd, u16_t net_idx, u16_t addr)
1296 {
1297 int i;
1298
1299 if (!frnd->established) {
1300 return false;
1301 }
1302
1303 if (net_idx != frnd->net_idx) {
1304 return false;
1305 }
1306
1307 if (BT_MESH_ADDR_IS_UNICAST(addr)) {
1308 return is_lpn_unicast(frnd, addr);
1309 }
1310
1311 for (i = 0; i < ARRAY_SIZE(frnd->sub_list); i++) {
1312 if (frnd->sub_list[i] == addr) {
1313 return true;
1314 }
1315 }
1316
1317 return false;
1318 }
1319
bt_mesh_friend_match(u16_t net_idx,u16_t addr)1320 bool bt_mesh_friend_match(u16_t net_idx, u16_t addr)
1321 {
1322 int i;
1323
1324 for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
1325 struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
1326
1327 if (friend_lpn_matches(frnd, net_idx, addr)) {
1328 BT_DBG("LPN 0x%04x matched address 0x%04x",
1329 frnd->lpn, addr);
1330 return true;
1331 }
1332 }
1333
1334 BT_DBG("No matching LPN for address 0x%04x", addr);
1335 return false;
1336 }
1337
friend_queue_has_space(struct bt_mesh_friend * frnd,u16_t addr,u64_t * seq_auth,u8_t seg_count)1338 static bool friend_queue_has_space(struct bt_mesh_friend *frnd, u16_t addr,
1339 u64_t *seq_auth, u8_t seg_count)
1340 {
1341 u32_t total = 0;
1342 int i;
1343
1344 if (seg_count > CONFIG_BT_MESH_FRIEND_QUEUE_SIZE) {
1345 return false;
1346 }
1347
1348 for (i = 0; i < ARRAY_SIZE(frnd->seg); i++) {
1349 struct bt_mesh_friend_seg *seg = &frnd->seg[i];
1350
1351 if (seq_auth && is_seg(seg, addr, *seq_auth & TRANS_SEQ_ZERO_MASK)) {
1352 /* If there's a segment queue for this message then the
1353 * space verification has already happened.
1354 */
1355 return true;
1356 }
1357
1358 total += seg->seg_count;
1359 }
1360
1361 /* If currently pending segments combined with this segmented message
1362 * are more than the Friend Queue Size, then there's no space. This
1363 * is because we don't have a mechanism of aborting already pending
1364 * segmented messages to free up buffers.
1365 */
1366 return (CONFIG_BT_MESH_FRIEND_QUEUE_SIZE - total) > seg_count;
1367 }
1368
bt_mesh_friend_queue_has_space(u16_t net_idx,u16_t src,u16_t dst,u64_t * seq_auth,u8_t seg_count)1369 bool bt_mesh_friend_queue_has_space(u16_t net_idx, u16_t src, u16_t dst,
1370 u64_t *seq_auth, u8_t seg_count)
1371 {
1372 bool someone_has_space = false, friend_match = false;
1373 int i;
1374 for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
1375 struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
1376
1377 if (!friend_lpn_matches(frnd, net_idx, dst)) {
1378 continue;
1379 }
1380
1381 friend_match = true;
1382
1383 if (friend_queue_has_space(frnd, src, seq_auth, seg_count)) {
1384 someone_has_space = true;
1385 }
1386 }
1387
1388 /* If there were no matched LPNs treat this as success, so the
1389 * transport layer can continue its work.
1390 */
1391 if (!friend_match) {
1392 return true;
1393 }
1394
1395 /* From the transport layers perspective it's good enough that at
1396 * least one Friend Queue has space. If there were multiple Friend
1397 * matches then the destination must be a group address, in which
1398 * case e.g. segment acks are not sent.
1399 */
1400 return someone_has_space;
1401 }
1402
friend_queue_prepare_space(struct bt_mesh_friend * frnd,u16_t addr,u64_t * seq_auth,u8_t seg_count)1403 static bool friend_queue_prepare_space(struct bt_mesh_friend *frnd, u16_t addr,
1404 u64_t *seq_auth, u8_t seg_count)
1405 {
1406 bool pending_segments;
1407 u8_t avail_space;
1408
1409 if (!friend_queue_has_space(frnd, addr, seq_auth, seg_count)) {
1410 return false;
1411 }
1412
1413 avail_space = CONFIG_BT_MESH_FRIEND_QUEUE_SIZE - frnd->queue_size;
1414 pending_segments = false;
1415 while (pending_segments || avail_space < seg_count) {
1416 struct os_mbuf *buf = (void *)net_buf_slist_get(&frnd->queue);
1417 if (!buf) {
1418 BT_ERR("Unable to free up enough buffers");
1419 return false;
1420 }
1421
1422 frnd->queue_size--;
1423 avail_space++;
1424 pending_segments = (BT_MESH_ADV(buf)->flags & NET_BUF_FRAGS);
1425 BT_DBG("PENDING SEGMENTS %d", pending_segments);
1426 /* Make sure old slist entry state doesn't remain */
1427 BT_MESH_ADV(buf)->flags &= ~NET_BUF_FRAGS;
1428 net_buf_unref(buf);
1429 }
1430
1431 return true;
1432 }
1433
bt_mesh_friend_enqueue_rx(struct bt_mesh_net_rx * rx,enum bt_mesh_friend_pdu_type type,u64_t * seq_auth,u8_t seg_count,struct os_mbuf * sbuf)1434 void bt_mesh_friend_enqueue_rx(struct bt_mesh_net_rx *rx,
1435 enum bt_mesh_friend_pdu_type type,
1436 u64_t *seq_auth, u8_t seg_count,
1437 struct os_mbuf *sbuf)
1438 {
1439 int i;
1440
1441 if (!rx->friend_match || (rx->ctx.recv_ttl <= 1 && rx->net_if != BT_MESH_NET_IF_LOCAL) ||
1442 bt_mesh_friend_get() != BT_MESH_FRIEND_ENABLED) {
1443 return;
1444 }
1445
1446 BT_DBG("recv_ttl %u net_idx 0x%04x src 0x%04x dst 0x%04x",
1447 rx->ctx.recv_ttl, rx->sub->net_idx, rx->ctx.addr,
1448 rx->ctx.recv_dst);
1449
1450 for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
1451 struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
1452
1453 if (!friend_lpn_matches(frnd, rx->sub->net_idx,
1454 rx->ctx.recv_dst)) {
1455 continue;
1456 }
1457
1458 if (!friend_queue_prepare_space(frnd, rx->ctx.addr, seq_auth,
1459 seg_count)) {
1460 continue;
1461 }
1462
1463 friend_lpn_enqueue_rx(frnd, rx, type, seq_auth, seg_count,
1464 sbuf);
1465 }
1466 }
1467
bt_mesh_friend_enqueue_tx(struct bt_mesh_net_tx * tx,enum bt_mesh_friend_pdu_type type,u64_t * seq_auth,u8_t seg_count,struct os_mbuf * sbuf)1468 bool bt_mesh_friend_enqueue_tx(struct bt_mesh_net_tx *tx,
1469 enum bt_mesh_friend_pdu_type type,
1470 u64_t *seq_auth, u8_t seg_count,
1471 struct os_mbuf *sbuf)
1472 {
1473 bool matched = false;
1474 int i;
1475
1476 if (!bt_mesh_friend_match(tx->sub->net_idx, tx->ctx->addr) ||
1477 bt_mesh_friend_get() != BT_MESH_FRIEND_ENABLED) {
1478 return matched;
1479 }
1480
1481 BT_DBG("net_idx 0x%04x dst 0x%04x src 0x%04x", tx->sub->net_idx,
1482 tx->ctx->addr, tx->src);
1483
1484 for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
1485 struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
1486
1487 if (!friend_lpn_matches(frnd, tx->sub->net_idx,
1488 tx->ctx->addr)) {
1489 continue;
1490 }
1491
1492 if (!friend_queue_prepare_space(frnd, tx->src, seq_auth,
1493 seg_count)) {
1494 continue;
1495 }
1496
1497 friend_lpn_enqueue_tx(frnd, tx, type, seq_auth, seg_count,
1498 sbuf);
1499 matched = true;
1500 }
1501
1502 return matched;
1503 }
1504
bt_mesh_friend_clear_incomplete(struct bt_mesh_subnet * sub,u16_t src,u16_t dst,u64_t * seq_auth)1505 void bt_mesh_friend_clear_incomplete(struct bt_mesh_subnet *sub, u16_t src,
1506 u16_t dst, u64_t *seq_auth)
1507 {
1508 int i;
1509 BT_DBG("");
1510
1511 for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
1512 struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
1513 int j;
1514
1515 if (!friend_lpn_matches(frnd, sub->net_idx, dst)) {
1516 continue;
1517 }
1518
1519 for (j = 0; j < ARRAY_SIZE(frnd->seg); j++) {
1520 struct bt_mesh_friend_seg *seg = &frnd->seg[j];
1521
1522 if (!is_seg(seg, src, *seq_auth & TRANS_SEQ_ZERO_MASK)) {
1523 continue;
1524 }
1525
1526 BT_WARN("Clearing incomplete segments for 0x%04x", src);
1527 purge_buffers(&seg->queue);
1528 seg->seg_count = 0U;
1529 break;
1530 }
1531 }
1532 }
1533
1534 #endif /* MYNEWT_VAL(BLE_MESH_FRIEND) */