1 /*  Bluetooth Mesh */
2 
3 /*
4  * Copyright (c) 2017 Intel Corporation
5  *
6  * SPDX-License-Identifier: Apache-2.0
7  */
8 
9 #include <stdint.h>
10 #include <errno.h>
11 
12 #include "crypto.h"
13 #include "adv.h"
14 #include "scan.h"
15 #include "mesh.h"
16 #include "transport.h"
17 #include "access.h"
18 #include "beacon.h"
19 #include "lpn.h"
20 #include "foundation.h"
21 #include "mesh_main.h"
22 #include "cfg_srv.h"
23 
24 #ifdef CONFIG_BLE_MESH_LOW_POWER
25 
26 #if defined(CONFIG_BLE_MESH_LPN_AUTO)
27 #define LPN_AUTO_TIMEOUT          K_SECONDS(CONFIG_BLE_MESH_LPN_AUTO_TIMEOUT)
28 #else
29 #define LPN_AUTO_TIMEOUT          0
30 #endif
31 
32 #define LPN_RECV_DELAY            CONFIG_BLE_MESH_LPN_RECV_DELAY
33 #define SCAN_LATENCY              MIN(CONFIG_BLE_MESH_LPN_SCAN_LATENCY, LPN_RECV_DELAY)
34 
35 #define FRIEND_REQ_RETRY_TIMEOUT  K_SECONDS(CONFIG_BLE_MESH_LPN_RETRY_TIMEOUT)
36 
37 #define FRIEND_REQ_WAIT           K_MSEC(100)
38 #define FRIEND_REQ_SCAN           K_SECONDS(1)
39 #define FRIEND_REQ_TIMEOUT        (FRIEND_REQ_WAIT + FRIEND_REQ_SCAN)
40 
41 #define POLL_RETRY_TIMEOUT        K_MSEC(100)
42 
43 #define REQ_RETRY_DURATION(lpn)   (4 * (LPN_RECV_DELAY + (lpn)->adv_duration + \
44                                    (lpn)->recv_win + POLL_RETRY_TIMEOUT))
45 
46 #define POLL_TIMEOUT_INIT         (CONFIG_BLE_MESH_LPN_INIT_POLL_TIMEOUT * 100)
47 #define POLL_TIMEOUT_MAX(lpn)     ((CONFIG_BLE_MESH_LPN_POLL_TIMEOUT * 100) - \
48                                    REQ_RETRY_DURATION(lpn))
49 
50 /**
51  * 1. Should use 20 attempts for BQB test case MESH/NODE/FRND/LPM/BI-02-C.
52  * 2. We should use more specific value for each PollTimeout range.
53  */
54 #define REQ_ATTEMPTS(lpn)         (POLL_TIMEOUT_MAX(lpn) < K_SECONDS(3) ? 2 : 6)
55 
56 #define CLEAR_ATTEMPTS            2
57 
58 #define LPN_CRITERIA              ((CONFIG_BLE_MESH_LPN_MIN_QUEUE_SIZE) | \
59                                    (CONFIG_BLE_MESH_LPN_RSSI_FACTOR << 3) | \
60                                    (CONFIG_BLE_MESH_LPN_RECV_WIN_FACTOR << 5))
61 
62 #define POLL_TO(to)               { (uint8_t)((to) >> 16), (uint8_t)((to) >> 8), (uint8_t)(to) }
63 #define LPN_POLL_TO               POLL_TO(CONFIG_BLE_MESH_LPN_POLL_TIMEOUT)
64 
65 /* 2 transmissions, 20ms interval */
66 #define POLL_XMIT                 BLE_MESH_TRANSMIT(1, 20)
67 
68 #define FIRST_POLL_ATTEMPTS       6
69 
70 static void (*lpn_cb)(uint16_t friend_addr, bool established);
71 
72 #if !CONFIG_BLE_MESH_NO_LOG
state2str(int state)73 static const char *state2str(int state)
74 {
75     switch (state) {
76     case BLE_MESH_LPN_DISABLED:
77         return "disabled";
78     case BLE_MESH_LPN_CLEAR:
79         return "clear";
80     case BLE_MESH_LPN_TIMER:
81         return "timer";
82     case BLE_MESH_LPN_ENABLED:
83         return "enabled";
84     case BLE_MESH_LPN_REQ_WAIT:
85         return "req wait";
86     case BLE_MESH_LPN_WAIT_OFFER:
87         return "wait offer";
88     case BLE_MESH_LPN_ESTABLISHED:
89         return "established";
90     case BLE_MESH_LPN_RECV_DELAY:
91         return "recv delay";
92     case BLE_MESH_LPN_WAIT_UPDATE:
93         return "wait update";
94     case BLE_MESH_LPN_OFFER_RECV:
95         return "offer recv";
96     default:
97         return "(unknown)";
98     }
99 }
100 #endif
101 
lpn_set_state(int state)102 static inline void lpn_set_state(int state)
103 {
104     BT_DBG("%s -> %s", state2str(bt_mesh.lpn.state), state2str(state));
105     bt_mesh.lpn.state = state;
106 }
107 
group_zero(bt_mesh_atomic_t * target)108 static inline void group_zero(bt_mesh_atomic_t *target)
109 {
110 #if CONFIG_BLE_MESH_LPN_GROUPS > 32
111     int i;
112 
113     for (i = 0; i < ARRAY_SIZE(bt_mesh.lpn.added); i++) {
114         bt_mesh_atomic_set(&target[i], 0);
115     }
116 #else
117     bt_mesh_atomic_set(target, 0);
118 #endif
119 }
120 
group_set(bt_mesh_atomic_t * target,bt_mesh_atomic_t * source)121 static inline void group_set(bt_mesh_atomic_t *target, bt_mesh_atomic_t *source)
122 {
123 #if CONFIG_BLE_MESH_LPN_GROUPS > 32
124     int i;
125 
126     for (i = 0; i < ARRAY_SIZE(bt_mesh.lpn.added); i++) {
127         (void)bt_mesh_atomic_or(&target[i], bt_mesh_atomic_get(&source[i]));
128     }
129 #else
130     (void)bt_mesh_atomic_or(target, bt_mesh_atomic_get(source));
131 #endif
132 }
133 
group_clear(bt_mesh_atomic_t * target,bt_mesh_atomic_t * source)134 static inline void group_clear(bt_mesh_atomic_t *target, bt_mesh_atomic_t *source)
135 {
136 #if CONFIG_BLE_MESH_LPN_GROUPS > 32
137     int i;
138 
139     for (i = 0; i < ARRAY_SIZE(bt_mesh.lpn.added); i++) {
140         (void)bt_mesh_atomic_and(&target[i], ~bt_mesh_atomic_get(&source[i]));
141     }
142 #else
143     (void)bt_mesh_atomic_and(target, ~bt_mesh_atomic_get(source));
144 #endif
145 }
146 
147 static void clear_friendship(bool force, bool disable);
148 
149 static bool scan_after_clear;
150 
friend_clear_sent(int err,void * user_data)151 static void friend_clear_sent(int err, void *user_data)
152 {
153     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
154 
155     /* We're switching away from Low Power behavior, so permanently
156      * enable scanning.
157      */
158     if (scan_after_clear == false) {
159         bt_mesh_scan_enable();
160         scan_after_clear = true;
161     }
162 
163     lpn->req_attempts++;
164 
165     if (err) {
166         BT_ERR("Sending Friend Clear failed (err %d)", err);
167         lpn_set_state(BLE_MESH_LPN_ENABLED);
168         clear_friendship(false, lpn->disable);
169         return;
170     }
171 
172     lpn_set_state(BLE_MESH_LPN_CLEAR);
173     k_delayed_work_submit(&lpn->timer, FRIEND_REQ_TIMEOUT);
174 }
175 
176 static const struct bt_mesh_send_cb clear_sent_cb = {
177     .end = friend_clear_sent,
178 };
179 
send_friend_clear(void)180 static int send_friend_clear(void)
181 {
182     struct bt_mesh_msg_ctx ctx = {
183         .net_idx     = bt_mesh.sub[0].net_idx,
184         .app_idx     = BLE_MESH_KEY_UNUSED,
185         .addr        = bt_mesh.lpn.frnd,
186         .send_ttl    = 0,
187     };
188     struct bt_mesh_net_tx tx = {
189         .sub = &bt_mesh.sub[0],
190         .ctx = &ctx,
191         .src = bt_mesh_primary_addr(),
192         .xmit = bt_mesh_net_transmit_get(),
193     };
194     struct bt_mesh_ctl_friend_clear req = {
195         .lpn_addr    = sys_cpu_to_be16(tx.src),
196         .lpn_counter = sys_cpu_to_be16(bt_mesh.lpn.counter),
197     };
198 
199     BT_DBG("%s", __func__);
200 
201     return bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_CLEAR, &req,
202                             sizeof(req), &clear_sent_cb, NULL);
203 }
204 
clear_friendship(bool force,bool disable)205 static void clear_friendship(bool force, bool disable)
206 {
207     struct bt_mesh_cfg_srv *cfg = bt_mesh_cfg_get();
208     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
209 
210     BT_DBG("force %u disable %u", force, disable);
211 
212     if (!force && lpn->established && !lpn->clear_success &&
213             lpn->req_attempts < CLEAR_ATTEMPTS) {
214         send_friend_clear();
215         lpn->disable = disable;
216         return;
217     }
218 
219     bt_mesh_rx_reset(true);
220 
221     k_delayed_work_cancel(&lpn->timer);
222 
223     friend_cred_del(bt_mesh.sub[0].net_idx, lpn->frnd);
224 
225     if (lpn->clear_success) {
226         lpn->old_friend = BLE_MESH_ADDR_UNASSIGNED;
227     } else {
228         lpn->old_friend = lpn->frnd;
229     }
230 
231     if (lpn_cb && lpn->frnd != BLE_MESH_ADDR_UNASSIGNED) {
232         lpn_cb(lpn->frnd, false);
233     }
234 
235     lpn->frnd = BLE_MESH_ADDR_UNASSIGNED;
236     lpn->fsn = 0U;
237     lpn->req_attempts = 0U;
238     lpn->recv_win = 0U;
239     lpn->queue_size = 0U;
240     lpn->disable = 0U;
241     lpn->sent_req = 0U;
242     lpn->established = 0U;
243     lpn->clear_success = 0U;
244 
245     group_zero(lpn->added);
246     group_zero(lpn->pending);
247     group_zero(lpn->to_remove);
248 
249     /* Set this to 1 to force group subscription when the next
250      * Friendship is created, in case lpn->groups doesn't get
251      * modified meanwhile.
252      */
253     lpn->groups_changed = 1U;
254 
255     if (cfg->hb_pub.feat & BLE_MESH_FEAT_LOW_POWER) {
256         bt_mesh_heartbeat_send();
257     }
258 
259     if (disable) {
260         lpn_set_state(BLE_MESH_LPN_DISABLED);
261         return;
262     }
263 
264     lpn_set_state(BLE_MESH_LPN_ENABLED);
265     k_delayed_work_submit(&lpn->timer, FRIEND_REQ_RETRY_TIMEOUT);
266 
267     scan_after_clear = false;
268     if (IS_ENABLED(CONFIG_BLE_MESH_LPN_ESTABLISHMENT)) {
269         bt_mesh_scan_disable();
270     }
271 }
272 
friend_req_sent(uint16_t duration,int err,void * user_data)273 static void friend_req_sent(uint16_t duration, int err, void *user_data)
274 {
275     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
276 
277     if (err) {
278         BT_ERR("Sending Friend Request failed (err %d)", err);
279 
280         if (IS_ENABLED(CONFIG_BLE_MESH_LPN_ESTABLISHMENT)) {
281             bt_mesh_scan_enable();
282         }
283         return;
284     }
285 
286     lpn->adv_duration = duration;
287 
288     if (IS_ENABLED(CONFIG_BLE_MESH_LPN_ESTABLISHMENT)) {
289         k_delayed_work_submit(&lpn->timer, FRIEND_REQ_WAIT);
290         lpn_set_state(BLE_MESH_LPN_REQ_WAIT);
291     } else {
292         k_delayed_work_submit(&lpn->timer,
293                               duration + FRIEND_REQ_TIMEOUT);
294         lpn_set_state(BLE_MESH_LPN_WAIT_OFFER);
295     }
296 }
297 
298 static const struct bt_mesh_send_cb friend_req_sent_cb = {
299     .start = friend_req_sent,
300 };
301 
send_friend_req(struct bt_mesh_lpn * lpn)302 static int send_friend_req(struct bt_mesh_lpn *lpn)
303 {
304     const struct bt_mesh_comp *comp = bt_mesh_comp_get();
305     struct bt_mesh_msg_ctx ctx = {
306         .net_idx  = bt_mesh.sub[0].net_idx,
307         .app_idx  = BLE_MESH_KEY_UNUSED,
308         .addr     = BLE_MESH_ADDR_FRIENDS,
309         .send_ttl = 0,
310     };
311     struct bt_mesh_net_tx tx = {
312         .sub = &bt_mesh.sub[0],
313         .ctx = &ctx,
314         .src = bt_mesh_primary_addr(),
315         .xmit = POLL_XMIT,
316     };
317     struct bt_mesh_ctl_friend_req req = {
318         .criteria    = LPN_CRITERIA,
319         .recv_delay  = LPN_RECV_DELAY,
320         .poll_to     = LPN_POLL_TO,
321         .prev_addr   = sys_cpu_to_be16(lpn->old_friend),
322         .num_elem    = comp->elem_count,
323         .lpn_counter = sys_cpu_to_be16(lpn->counter),
324     };
325 
326     BT_DBG("%s", __func__);
327 
328     return bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_REQ, &req,
329                             sizeof(req), &friend_req_sent_cb, NULL);
330 }
331 
req_sent(uint16_t duration,int err,void * user_data)332 static void req_sent(uint16_t duration, int err, void *user_data)
333 {
334     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
335 
336     BT_DBG("req 0x%02x duration %u err %d state %s",
337            lpn->sent_req, duration, err, state2str(lpn->state));
338 
339     if (err) {
340         BT_ERR("Sending request failed (err %d)", err);
341         lpn->sent_req = 0U;
342         group_zero(lpn->pending);
343         return;
344     }
345 
346     lpn->req_attempts++;
347     lpn->adv_duration = duration;
348 
349     if (lpn->established || IS_ENABLED(CONFIG_BLE_MESH_LPN_ESTABLISHMENT)) {
350         lpn_set_state(BLE_MESH_LPN_RECV_DELAY);
351         /* We start scanning a bit early to elimitate risk of missing
352          * response data due to HCI and other latencies.
353          */
354         k_delayed_work_submit(&lpn->timer,
355                               LPN_RECV_DELAY - SCAN_LATENCY);
356     } else {
357         lpn_set_state(BLE_MESH_LPN_OFFER_RECV);
358         /**
359          * Friend Update is replied by Friend Node with TTL set to 0 and Network
360          * Transmit set to 30ms which will cause the packet easy to be missed.
361          * Regarding this situation, here we can reduce the duration of receiving
362          * the first Friend Update.
363          */
364         k_delayed_work_submit(&lpn->timer,
365                               LPN_RECV_DELAY + duration +
366                               lpn->recv_win);
367     }
368 }
369 
370 static const struct bt_mesh_send_cb req_sent_cb = {
371     .start = req_sent,
372 };
373 
send_friend_poll(void)374 static int send_friend_poll(void)
375 {
376     struct bt_mesh_msg_ctx ctx = {
377         .net_idx     = bt_mesh.sub[0].net_idx,
378         .app_idx     = BLE_MESH_KEY_UNUSED,
379         .addr        = bt_mesh.lpn.frnd,
380         .send_ttl    = 0,
381     };
382     struct bt_mesh_net_tx tx = {
383         .sub = &bt_mesh.sub[0],
384         .ctx = &ctx,
385         .src = bt_mesh_primary_addr(),
386         .xmit = POLL_XMIT,
387         .friend_cred = true,
388     };
389     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
390     uint8_t fsn = lpn->fsn;
391     int err = 0;
392 
393     BT_DBG("lpn->sent_req 0x%02x", lpn->sent_req);
394 
395     if (lpn->sent_req) {
396         if (lpn->sent_req != TRANS_CTL_OP_FRIEND_POLL) {
397             lpn->pending_poll = 1U;
398         }
399 
400         return 0;
401     }
402 
403     err = bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_POLL, &fsn, 1,
404                            &req_sent_cb, NULL);
405     if (err == 0) {
406         lpn->pending_poll = 0U;
407         lpn->sent_req = TRANS_CTL_OP_FRIEND_POLL;
408     }
409 
410     return err;
411 }
412 
bt_mesh_lpn_disable(bool force)413 void bt_mesh_lpn_disable(bool force)
414 {
415     if (bt_mesh.lpn.state == BLE_MESH_LPN_DISABLED) {
416         return;
417     }
418 
419     clear_friendship(force, true);
420 }
421 
bt_mesh_lpn_set(bool enable,bool force)422 int bt_mesh_lpn_set(bool enable, bool force)
423 {
424     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
425 
426     if (enable) {
427         if (lpn->state != BLE_MESH_LPN_DISABLED) {
428             return 0;
429         }
430     } else {
431         if (lpn->state == BLE_MESH_LPN_DISABLED) {
432             return 0;
433         }
434     }
435 
436     if (!bt_mesh_is_provisioned()) {
437         if (enable) {
438             lpn_set_state(BLE_MESH_LPN_ENABLED);
439         } else {
440             lpn_set_state(BLE_MESH_LPN_DISABLED);
441         }
442 
443         return 0;
444     }
445 
446     if (enable) {
447         lpn_set_state(BLE_MESH_LPN_ENABLED);
448 
449         if (IS_ENABLED(CONFIG_BLE_MESH_LPN_ESTABLISHMENT)) {
450             bt_mesh_scan_disable();
451         }
452 
453         send_friend_req(lpn);
454     } else {
455         if (IS_ENABLED(CONFIG_BLE_MESH_LPN_AUTO) &&
456                 lpn->state == BLE_MESH_LPN_TIMER) {
457             k_delayed_work_cancel(&lpn->timer);
458             lpn_set_state(BLE_MESH_LPN_DISABLED);
459         } else {
460             bt_mesh_lpn_disable(force);
461         }
462     }
463 
464     return 0;
465 }
466 
friend_response_received(struct bt_mesh_lpn * lpn)467 static void friend_response_received(struct bt_mesh_lpn *lpn)
468 {
469     BT_DBG("lpn->sent_req 0x%02x", lpn->sent_req);
470 
471     if (lpn->sent_req == TRANS_CTL_OP_FRIEND_POLL) {
472         lpn->fsn++;
473     }
474 
475     k_delayed_work_cancel(&lpn->timer);
476     bt_mesh_scan_disable();
477     lpn_set_state(BLE_MESH_LPN_ESTABLISHED);
478     lpn->req_attempts = 0U;
479     lpn->sent_req = 0U;
480 }
481 
bt_mesh_lpn_msg_received(struct bt_mesh_net_rx * rx)482 void bt_mesh_lpn_msg_received(struct bt_mesh_net_rx *rx)
483 {
484     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
485 
486     if (lpn->state == BLE_MESH_LPN_TIMER) {
487         BT_DBG("Restarting establishment timer");
488         k_delayed_work_submit(&lpn->timer, LPN_AUTO_TIMEOUT);
489         return;
490     }
491 
492     if (lpn->sent_req != TRANS_CTL_OP_FRIEND_POLL) {
493         BT_WARN("Unexpected message without a preceding Poll");
494         return;
495     }
496 
497     friend_response_received(lpn);
498 
499     BT_DBG("Requesting more messages from Friend");
500 
501     send_friend_poll();
502 }
503 
bt_mesh_lpn_friend_offer(struct bt_mesh_net_rx * rx,struct net_buf_simple * buf)504 int bt_mesh_lpn_friend_offer(struct bt_mesh_net_rx *rx,
505                              struct net_buf_simple *buf)
506 {
507     struct bt_mesh_ctl_friend_offer *msg = (void *)buf->data;
508     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
509     struct bt_mesh_subnet *sub = rx->sub;
510     struct friend_cred *cred = NULL;
511     uint16_t frnd_counter = 0U;
512     int err = 0;
513 
514     if (buf->len < sizeof(*msg)) {
515         BT_WARN("Too short Friend Offer");
516         return -EINVAL;
517     }
518 
519     if (lpn->state != BLE_MESH_LPN_WAIT_OFFER) {
520         BT_WARN("Ignoring unexpected Friend Offer");
521         return 0;
522     }
523 
524     if (!msg->recv_win) {
525         BT_WARN("Prohibited ReceiveWindow value");
526         return -EINVAL;
527     }
528 
529     frnd_counter = sys_be16_to_cpu(msg->frnd_counter);
530 
531     BT_INFO("recv_win %u queue_size %u sub_list_size %u rssi %d counter %u",
532            msg->recv_win, msg->queue_size, msg->sub_list_size, msg->rssi,
533            frnd_counter);
534 
535     lpn->frnd = rx->ctx.addr;
536 
537     cred = friend_cred_create(sub, lpn->frnd, lpn->counter, frnd_counter);
538     if (!cred) {
539         lpn->frnd = BLE_MESH_ADDR_UNASSIGNED;
540         return -ENOMEM;
541     }
542 
543     /* TODO: Add offer acceptance criteria check */
544 
545     k_delayed_work_cancel(&lpn->timer);
546 
547     lpn->recv_win = msg->recv_win;
548     lpn->queue_size = msg->queue_size;
549 
550     err = send_friend_poll();
551     if (err) {
552         friend_cred_clear(cred);
553         lpn->frnd = BLE_MESH_ADDR_UNASSIGNED;
554         lpn->recv_win = 0U;
555         lpn->queue_size = 0U;
556         return err;
557     }
558 
559     lpn->counter++;
560 
561     if (IS_ENABLED(CONFIG_BLE_MESH_LPN_ESTABLISHMENT)) {
562         bt_mesh_scan_disable();
563     }
564 
565     return 0;
566 }
567 
bt_mesh_lpn_friend_clear_cfm(struct bt_mesh_net_rx * rx,struct net_buf_simple * buf)568 int bt_mesh_lpn_friend_clear_cfm(struct bt_mesh_net_rx *rx,
569                                  struct net_buf_simple *buf)
570 {
571     struct bt_mesh_ctl_friend_clear_confirm *msg = (void *)buf->data;
572     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
573     uint16_t addr = 0U, counter = 0U;
574 
575     if (buf->len < sizeof(*msg)) {
576         BT_WARN("Too short Friend Clear Confirm");
577         return -EINVAL;
578     }
579 
580     if (lpn->state != BLE_MESH_LPN_CLEAR) {
581         BT_WARN("Ignoring unexpected Friend Clear Confirm");
582         return 0;
583     }
584 
585     addr = sys_be16_to_cpu(msg->lpn_addr);
586     counter = sys_be16_to_cpu(msg->lpn_counter);
587 
588     BT_DBG("LPNAddress 0x%04x LPNCounter 0x%04x", addr, counter);
589 
590     if (addr != bt_mesh_primary_addr() || counter != lpn->counter) {
591         BT_WARN("Invalid parameters in Friend Clear Confirm");
592         return 0;
593     }
594 
595     lpn->clear_success = 1U;
596     clear_friendship(false, lpn->disable);
597 
598     return 0;
599 }
600 
lpn_group_add(uint16_t group)601 static void lpn_group_add(uint16_t group)
602 {
603     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
604     uint16_t *free_slot = NULL;
605     int i;
606 
607     for (i = 0; i < ARRAY_SIZE(lpn->groups); i++) {
608         if (lpn->groups[i] == group) {
609             bt_mesh_atomic_clear_bit(lpn->to_remove, i);
610             return;
611         }
612 
613         if (!free_slot && lpn->groups[i] == BLE_MESH_ADDR_UNASSIGNED) {
614             free_slot = &lpn->groups[i];
615         }
616     }
617 
618     if (!free_slot) {
619         BT_WARN("Friend Subscription List exceeded!");
620         return;
621     }
622 
623     *free_slot = group;
624     lpn->groups_changed = 1U;
625 }
626 
lpn_group_del(uint16_t group)627 static void lpn_group_del(uint16_t group)
628 {
629     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
630     int i;
631 
632     for (i = 0; i < ARRAY_SIZE(lpn->groups); i++) {
633         if (lpn->groups[i] == group) {
634             if (bt_mesh_atomic_test_bit(lpn->added, i) ||
635                     bt_mesh_atomic_test_bit(lpn->pending, i)) {
636                 bt_mesh_atomic_set_bit(lpn->to_remove, i);
637                 lpn->groups_changed = 1U;
638             } else {
639                 lpn->groups[i] = BLE_MESH_ADDR_UNASSIGNED;
640             }
641         }
642     }
643 }
644 
group_popcount(bt_mesh_atomic_t * target)645 static inline int group_popcount(bt_mesh_atomic_t *target)
646 {
647 #if CONFIG_BLE_MESH_LPN_GROUPS > 32
648     int i, count = 0;
649 
650     for (i = 0; i < ARRAY_SIZE(bt_mesh.lpn.added); i++) {
651         count += popcount(bt_mesh_atomic_get(&target[i]));
652     }
653 #else
654     return popcount(bt_mesh_atomic_get(target));
655 #endif
656 }
657 
sub_update(uint8_t op)658 static bool sub_update(uint8_t op)
659 {
660     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
661     int added_count = group_popcount(lpn->added);
662     struct bt_mesh_msg_ctx ctx = {
663         .net_idx     = bt_mesh.sub[0].net_idx,
664         .app_idx     = BLE_MESH_KEY_UNUSED,
665         .addr        = lpn->frnd,
666         .send_ttl    = 0,
667     };
668     struct bt_mesh_net_tx tx = {
669         .sub = &bt_mesh.sub[0],
670         .ctx = &ctx,
671         .src = bt_mesh_primary_addr(),
672         .xmit = POLL_XMIT,
673         .friend_cred = true,
674     };
675     struct bt_mesh_ctl_friend_sub req = {0};
676     size_t i = 0U, g = 0U;
677 
678     BT_DBG("op 0x%02x sent_req 0x%02x", op, lpn->sent_req);
679 
680     if (lpn->sent_req) {
681         return false;
682     }
683 
684     for (i = 0U, g = 0U; i < ARRAY_SIZE(lpn->groups); i++) {
685         if (lpn->groups[i] == BLE_MESH_ADDR_UNASSIGNED) {
686             continue;
687         }
688 
689         if (op == TRANS_CTL_OP_FRIEND_SUB_ADD) {
690             if (bt_mesh_atomic_test_bit(lpn->added, i)) {
691                 continue;
692             }
693         } else {
694             if (!bt_mesh_atomic_test_bit(lpn->to_remove, i)) {
695                 continue;
696             }
697         }
698 
699         if (added_count + g >= lpn->queue_size) {
700             BT_WARN("Friend Queue Size exceeded");
701             break;
702         }
703 
704         req.addr_list[g++] = sys_cpu_to_be16(lpn->groups[i]);
705         bt_mesh_atomic_set_bit(lpn->pending, i);
706 
707         if (g == ARRAY_SIZE(req.addr_list)) {
708             break;
709         }
710     }
711 
712     if (g == 0U) {
713         group_zero(lpn->pending);
714         return false;
715     }
716 
717     req.xact = lpn->xact_next++;
718 
719     if (bt_mesh_ctl_send(&tx, op, &req, 1 + g * 2,
720                          &req_sent_cb, NULL) < 0) {
721         group_zero(lpn->pending);
722         return false;
723     }
724 
725     lpn->xact_pending = req.xact;
726     lpn->sent_req = op;
727     return true;
728 }
729 
update_timeout(struct bt_mesh_lpn * lpn)730 static void update_timeout(struct bt_mesh_lpn *lpn)
731 {
732     if (lpn->established) {
733         BT_WARN("No response from Friend during ReceiveWindow");
734         bt_mesh_scan_disable();
735         lpn_set_state(BLE_MESH_LPN_ESTABLISHED);
736         k_delayed_work_submit(&lpn->timer, POLL_RETRY_TIMEOUT);
737     } else {
738         if (IS_ENABLED(CONFIG_BLE_MESH_LPN_ESTABLISHMENT)) {
739             bt_mesh_scan_disable();
740         }
741 
742         if (lpn->req_attempts < FIRST_POLL_ATTEMPTS) {
743             BT_WARN("Retrying first Friend Poll");
744             lpn->sent_req = 0U;
745             if (send_friend_poll() == 0) {
746                 return;
747             }
748         }
749 
750         BT_ERR("Timed out waiting for first Friend Update");
751         clear_friendship(false, false);
752     }
753 }
754 
lpn_timeout(struct k_work * work)755 static void lpn_timeout(struct k_work *work)
756 {
757     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
758 
759     BT_DBG("state: %s", state2str(lpn->state));
760 
761     switch (lpn->state) {
762     case BLE_MESH_LPN_DISABLED:
763         break;
764     case BLE_MESH_LPN_CLEAR:
765         clear_friendship(false, bt_mesh.lpn.disable);
766         break;
767     case BLE_MESH_LPN_TIMER:
768         BT_DBG("Starting to look for Friend nodes");
769         lpn_set_state(BLE_MESH_LPN_ENABLED);
770         if (IS_ENABLED(CONFIG_BLE_MESH_LPN_ESTABLISHMENT)) {
771             bt_mesh_scan_disable();
772         }
773     /* fall through */
774     case BLE_MESH_LPN_ENABLED:
775         send_friend_req(lpn);
776         break;
777     case BLE_MESH_LPN_REQ_WAIT:
778         bt_mesh_scan_enable();
779         k_delayed_work_submit(&lpn->timer,
780                               lpn->adv_duration + FRIEND_REQ_SCAN);
781         lpn_set_state(BLE_MESH_LPN_WAIT_OFFER);
782         break;
783     case BLE_MESH_LPN_WAIT_OFFER:
784         BT_WARN("No acceptable Friend Offers received");
785         if (IS_ENABLED(CONFIG_BLE_MESH_LPN_ESTABLISHMENT)) {
786             bt_mesh_scan_disable();
787         }
788         lpn->counter++;
789         lpn_set_state(BLE_MESH_LPN_ENABLED);
790         lpn->sent_req = 0U;
791         k_delayed_work_submit(&lpn->timer, FRIEND_REQ_RETRY_TIMEOUT);
792         break;
793     case BLE_MESH_LPN_OFFER_RECV:
794         if (lpn->req_attempts < FIRST_POLL_ATTEMPTS) {
795             BT_WARN("Retrying the first Friend Poll, %d attempts", lpn->req_attempts);
796             lpn->sent_req = 0U;
797             send_friend_poll();
798             break;
799         }
800 
801         BT_ERR("Timeout waiting for the first Friend Update");
802         clear_friendship(true, false);
803         break;
804     case BLE_MESH_LPN_ESTABLISHED:
805         if (lpn->req_attempts < REQ_ATTEMPTS(lpn)) {
806             uint8_t req = lpn->sent_req;
807 
808             lpn->sent_req = 0U;
809 
810             if (!req || req == TRANS_CTL_OP_FRIEND_POLL) {
811                 send_friend_poll();
812             } else {
813                 sub_update(req);
814             }
815 
816             break;
817         }
818 
819         BT_ERR("No response from Friend after %u retries",
820                lpn->req_attempts);
821         lpn->req_attempts = 0U;
822         clear_friendship(false, false);
823         break;
824     case BLE_MESH_LPN_RECV_DELAY:
825         k_delayed_work_submit(&lpn->timer,
826                               lpn->adv_duration + SCAN_LATENCY +
827                               lpn->recv_win);
828         bt_mesh_scan_enable();
829         lpn_set_state(BLE_MESH_LPN_WAIT_UPDATE);
830         break;
831     case BLE_MESH_LPN_WAIT_UPDATE:
832         update_timeout(lpn);
833         break;
834     default:
835         BT_ERR("Unhandled LPN state");
836         break;
837     }
838 }
839 
bt_mesh_lpn_group_add(uint16_t group)840 void bt_mesh_lpn_group_add(uint16_t group)
841 {
842     BT_DBG("group 0x%04x", group);
843 
844     lpn_group_add(group);
845 
846     if (!bt_mesh_lpn_established() || bt_mesh.lpn.sent_req) {
847         return;
848     }
849 
850     sub_update(TRANS_CTL_OP_FRIEND_SUB_ADD);
851 }
852 
bt_mesh_lpn_group_del(uint16_t * groups,size_t group_count)853 void bt_mesh_lpn_group_del(uint16_t *groups, size_t group_count)
854 {
855     int i;
856 
857     for (i = 0; i < group_count; i++) {
858         if (groups[i] != BLE_MESH_ADDR_UNASSIGNED) {
859             BT_DBG("group 0x%04x", groups[i]);
860             lpn_group_del(groups[i]);
861         }
862     }
863 
864     if (!bt_mesh_lpn_established() || bt_mesh.lpn.sent_req) {
865         return;
866     }
867 
868     sub_update(TRANS_CTL_OP_FRIEND_SUB_REM);
869 }
870 
poll_timeout(struct bt_mesh_lpn * lpn)871 static int32_t poll_timeout(struct bt_mesh_lpn *lpn)
872 {
873     /* If we're waiting for segment acks keep polling at high freq */
874     if (bt_mesh_tx_in_progress()) {
875         return MIN(POLL_TIMEOUT_MAX(lpn), K_SECONDS(1));
876     }
877 
878     if (lpn->poll_timeout < POLL_TIMEOUT_MAX(lpn)) {
879         lpn->poll_timeout *= 2;
880         lpn->poll_timeout = MIN(lpn->poll_timeout,
881                                 POLL_TIMEOUT_MAX(lpn));
882     }
883 
884     BT_DBG("Poll Timeout is %ums", lpn->poll_timeout);
885 
886     return lpn->poll_timeout;
887 }
888 
bt_mesh_lpn_friend_sub_cfm(struct bt_mesh_net_rx * rx,struct net_buf_simple * buf)889 int bt_mesh_lpn_friend_sub_cfm(struct bt_mesh_net_rx *rx,
890                                struct net_buf_simple *buf)
891 {
892     struct bt_mesh_ctl_friend_sub_confirm *msg = (void *)buf->data;
893     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
894 
895     if (buf->len < sizeof(*msg)) {
896         BT_WARN("Too short Friend Subscription Confirm");
897         return -EINVAL;
898     }
899 
900     BT_DBG("xact 0x%02x", msg->xact);
901 
902     if (!lpn->sent_req) {
903         BT_WARN("No pending subscription list message");
904         return 0;
905     }
906 
907     if (msg->xact != lpn->xact_pending) {
908         BT_WARN("Transaction mismatch (0x%02x != 0x%02x)",
909                 msg->xact, lpn->xact_pending);
910         return 0;
911     }
912 
913     if (lpn->sent_req == TRANS_CTL_OP_FRIEND_SUB_ADD) {
914         group_set(lpn->added, lpn->pending);
915         group_zero(lpn->pending);
916     } else if (lpn->sent_req == TRANS_CTL_OP_FRIEND_SUB_REM) {
917         int i;
918 
919         group_clear(lpn->added, lpn->pending);
920 
921         for (i = 0; i < ARRAY_SIZE(lpn->groups); i++) {
922             if (bt_mesh_atomic_test_and_clear_bit(lpn->pending, i) &&
923                     bt_mesh_atomic_test_and_clear_bit(lpn->to_remove, i)) {
924                 lpn->groups[i] = BLE_MESH_ADDR_UNASSIGNED;
925             }
926         }
927     } else {
928         BT_WARN("Unexpected Friend Subscription Confirm");
929         return 0;
930     }
931 
932     friend_response_received(lpn);
933 
934     if (lpn->groups_changed) {
935         sub_update(TRANS_CTL_OP_FRIEND_SUB_ADD);
936         sub_update(TRANS_CTL_OP_FRIEND_SUB_REM);
937 
938         if (!lpn->sent_req) {
939             lpn->groups_changed = 0U;
940         }
941     }
942 
943     if (lpn->pending_poll) {
944         send_friend_poll();
945     }
946 
947     if (!lpn->sent_req) {
948         k_delayed_work_submit(&lpn->timer, poll_timeout(lpn));
949     }
950 
951     return 0;
952 }
953 
bt_mesh_lpn_friend_update(struct bt_mesh_net_rx * rx,struct net_buf_simple * buf)954 int bt_mesh_lpn_friend_update(struct bt_mesh_net_rx *rx,
955                               struct net_buf_simple *buf)
956 {
957     struct bt_mesh_ctl_friend_update *msg = (void *)buf->data;
958     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
959     struct bt_mesh_subnet *sub = rx->sub;
960     uint32_t iv_index = 0U;
961 
962     if (buf->len < sizeof(*msg)) {
963         BT_WARN("Too short Friend Update");
964         return -EINVAL;
965     }
966 
967     if (lpn->sent_req != TRANS_CTL_OP_FRIEND_POLL) {
968         BT_WARN("Unexpected friend update");
969         return 0;
970     }
971 
972     if (sub->kr_phase == BLE_MESH_KR_PHASE_2 && !rx->new_key) {
973         BT_WARN("Ignoring Phase 2 KR Update secured using old key");
974         return 0;
975     }
976 
977     if (bt_mesh_atomic_test_bit(bt_mesh.flags, BLE_MESH_IVU_INITIATOR) &&
978             (bt_mesh_atomic_test_bit(bt_mesh.flags, BLE_MESH_IVU_IN_PROGRESS) ==
979              BLE_MESH_IV_UPDATE(msg->flags))) {
980         bt_mesh_beacon_ivu_initiator(false);
981     }
982 
983     if (!lpn->established) {
984         struct bt_mesh_cfg_srv *cfg = bt_mesh_cfg_get();
985 
986         /* This is normally checked on the transport layer, however
987          * in this state we're also still accepting master
988          * credentials so we need to ensure the right ones (Friend
989          * Credentials) were used for this message.
990          */
991         if (!rx->friend_cred) {
992             BT_WARN("Friend Update with wrong credentials");
993             return -EINVAL;
994         }
995 
996         lpn->established = 1U;
997 
998         BT_INFO("Friendship established with 0x%04x", lpn->frnd);
999 
1000         if (cfg->hb_pub.feat & BLE_MESH_FEAT_LOW_POWER) {
1001             bt_mesh_heartbeat_send();
1002         }
1003 
1004         if (lpn_cb) {
1005             lpn_cb(lpn->frnd, true);
1006         }
1007 
1008         /* Set initial poll timeout */
1009         lpn->poll_timeout = MIN(POLL_TIMEOUT_MAX(lpn),
1010                                 POLL_TIMEOUT_INIT);
1011     }
1012 
1013     friend_response_received(lpn);
1014 
1015     iv_index = sys_be32_to_cpu(msg->iv_index);
1016 
1017     BT_INFO("flags 0x%02x iv_index 0x%08x md %u", msg->flags, iv_index,
1018            msg->md);
1019 
1020     if (bt_mesh_kr_update(sub, BLE_MESH_KEY_REFRESH(msg->flags),
1021                           rx->new_key)) {
1022         bt_mesh_net_beacon_update(sub);
1023     }
1024 
1025     bt_mesh_net_iv_update(iv_index, BLE_MESH_IV_UPDATE(msg->flags));
1026 
1027     if (lpn->groups_changed) {
1028         sub_update(TRANS_CTL_OP_FRIEND_SUB_ADD);
1029         sub_update(TRANS_CTL_OP_FRIEND_SUB_REM);
1030 
1031         if (!lpn->sent_req) {
1032             lpn->groups_changed = 0U;
1033         }
1034     }
1035 
1036     if (msg->md) {
1037         BT_DBG("Requesting for more messages");
1038         send_friend_poll();
1039     }
1040 
1041     if (!lpn->sent_req) {
1042         k_delayed_work_submit(&lpn->timer, poll_timeout(lpn));
1043     }
1044 
1045     return 0;
1046 }
1047 
bt_mesh_lpn_poll(void)1048 int bt_mesh_lpn_poll(void)
1049 {
1050     if (!bt_mesh.lpn.established) {
1051         return -EAGAIN;
1052     }
1053 
1054     BT_DBG("Requesting more messages");
1055 
1056     return send_friend_poll();
1057 }
1058 
bt_mesh_lpn_set_cb(void (* cb)(uint16_t friend_addr,bool established))1059 void bt_mesh_lpn_set_cb(void (*cb)(uint16_t friend_addr, bool established))
1060 {
1061     lpn_cb = cb;
1062 }
1063 
bt_mesh_lpn_init(void)1064 int bt_mesh_lpn_init(void)
1065 {
1066     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
1067 
1068     BT_DBG("%s", __func__);
1069 
1070     k_delayed_work_init(&lpn->timer, lpn_timeout);
1071 
1072     if (lpn->state == BLE_MESH_LPN_ENABLED) {
1073         if (!IS_ENABLED(CONFIG_BLE_MESH_LPN_ESTABLISHMENT)) {
1074             bt_mesh_scan_enable();
1075         }
1076 
1077         send_friend_req(lpn);
1078     } else {
1079         bt_mesh_scan_enable();
1080 
1081         if (IS_ENABLED(CONFIG_BLE_MESH_LPN_AUTO)) {
1082             BT_DBG("Waiting %u ms for messages", LPN_AUTO_TIMEOUT);
1083             lpn_set_state(BLE_MESH_LPN_TIMER);
1084             k_delayed_work_submit(&lpn->timer, LPN_AUTO_TIMEOUT);
1085         }
1086     }
1087 
1088     return 0;
1089 }
1090 
1091 #if CONFIG_BLE_MESH_DEINIT
bt_mesh_lpn_deinit(void)1092 int bt_mesh_lpn_deinit(void)
1093 {
1094     struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
1095 
1096     bt_mesh_lpn_disable(true);
1097 
1098     k_delayed_work_free(&lpn->timer);
1099 
1100     return 0;
1101 }
1102 #endif /* CONFIG_BLE_MESH_DEINIT */
1103 
1104 #endif /* CONFIG_BLE_MESH_LOW_POWER */
1105