1 /* Bluetooth Mesh */
2
3 /*
4 * Copyright (c) 2017 Intel Corporation
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <string.h>
10 #include <errno.h>
11 #include <stdbool.h>
12
13 #include "mesh/mesh.h"
14
15 #include "syscfg/syscfg.h"
16 #define BT_DBG_ENABLED MYNEWT_VAL(BLE_MESH_DEBUG_MODEL)
17 #include "host/ble_hs_log.h"
18
19 #include "mesh_priv.h"
20 #include "adv.h"
21 #include "net.h"
22 #include "lpn.h"
23 #include "transport.h"
24 #include "crypto.h"
25 #include "access.h"
26 #include "beacon.h"
27 #include "proxy.h"
28 #include "foundation.h"
29 #include "friend.h"
30 #include "testing.h"
31 #include "settings.h"
32
33 #define DEFAULT_TTL 7
34
35 static struct bt_mesh_cfg_srv *conf;
36
37 static struct label {
38 u16_t ref;
39 u16_t addr;
40 u8_t uuid[16];
41 } labels[MYNEWT_VAL(BLE_MESH_LABEL_COUNT)];
42
hb_send(struct bt_mesh_model * model)43 static void hb_send(struct bt_mesh_model *model)
44 {
45
46 struct bt_mesh_cfg_srv *cfg = model->user_data;
47 u16_t feat = 0;
48 struct __packed {
49 u8_t init_ttl;
50 u16_t feat;
51 } hb;
52 struct bt_mesh_msg_ctx ctx = {
53 .net_idx = cfg->hb_pub.net_idx,
54 .app_idx = BT_MESH_KEY_UNUSED,
55 .addr = cfg->hb_pub.dst,
56 .send_ttl = cfg->hb_pub.ttl,
57 };
58 struct bt_mesh_net_tx tx = {
59 .sub = bt_mesh_subnet_get(cfg->hb_pub.net_idx),
60 .ctx = &ctx,
61 .src = bt_mesh_model_elem(model)->addr,
62 .xmit = bt_mesh_net_transmit_get(),
63 };
64
65 hb.init_ttl = cfg->hb_pub.ttl;
66
67 if (bt_mesh_relay_get() == BT_MESH_RELAY_ENABLED) {
68 feat |= BT_MESH_FEAT_RELAY;
69 }
70
71 if (bt_mesh_gatt_proxy_get() == BT_MESH_GATT_PROXY_ENABLED) {
72 feat |= BT_MESH_FEAT_PROXY;
73 }
74
75 if (bt_mesh_friend_get() == BT_MESH_FRIEND_ENABLED) {
76 feat |= BT_MESH_FEAT_FRIEND;
77 }
78
79 #if (MYNEWT_VAL(BLE_MESH_LOW_POWER))
80 if (bt_mesh.lpn.state != BT_MESH_LPN_DISABLED) {
81 feat |= BT_MESH_FEAT_LOW_POWER;
82 }
83 #endif
84
85 hb.feat = sys_cpu_to_be16(feat);
86
87 BT_DBG("InitTTL %u feat 0x%04x", cfg->hb_pub.ttl, feat);
88
89 bt_mesh_ctl_send(&tx, TRANS_CTL_OP_HEARTBEAT, &hb, sizeof(hb),
90 NULL, NULL, NULL);
91 }
92
comp_add_elem(struct os_mbuf * buf,struct bt_mesh_elem * elem,bool primary)93 static int comp_add_elem(struct os_mbuf *buf, struct bt_mesh_elem *elem,
94 bool primary)
95 {
96 struct bt_mesh_model *mod;
97 int i;
98
99 if (net_buf_simple_tailroom(buf) <
100 4 + (elem->model_count * 2) + (elem->vnd_model_count * 2)) {
101 BT_ERR("Too large device composition");
102 return -E2BIG;
103 }
104
105 net_buf_simple_add_le16(buf, elem->loc);
106
107 net_buf_simple_add_u8(buf, elem->model_count);
108 net_buf_simple_add_u8(buf, elem->vnd_model_count);
109
110 for (i = 0; i < elem->model_count; i++) {
111 mod = &elem->models[i];
112 net_buf_simple_add_le16(buf, mod->id);
113 }
114
115 for (i = 0; i < elem->vnd_model_count; i++) {
116 mod = &elem->vnd_models[i];
117 net_buf_simple_add_le16(buf, mod->vnd.company);
118 net_buf_simple_add_le16(buf, mod->vnd.id);
119 }
120
121 return 0;
122 }
123
comp_get_page_0(struct os_mbuf * buf)124 static int comp_get_page_0(struct os_mbuf *buf)
125 {
126 u16_t feat = 0;
127 const struct bt_mesh_comp *comp;
128 int i;
129
130 comp = bt_mesh_comp_get();
131
132 if ((MYNEWT_VAL(BLE_MESH_RELAY))) {
133 feat |= BT_MESH_FEAT_RELAY;
134 }
135
136 if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY))) {
137 feat |= BT_MESH_FEAT_PROXY;
138 }
139
140 if ((MYNEWT_VAL(BLE_MESH_FRIEND))) {
141 feat |= BT_MESH_FEAT_FRIEND;
142 }
143
144 if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) {
145 feat |= BT_MESH_FEAT_LOW_POWER;
146 }
147
148 net_buf_simple_add_le16(buf, comp->cid);
149 net_buf_simple_add_le16(buf, comp->pid);
150 net_buf_simple_add_le16(buf, comp->vid);
151 net_buf_simple_add_le16(buf, MYNEWT_VAL(BLE_MESH_CRPL));
152 net_buf_simple_add_le16(buf, feat);
153
154 for (i = 0; i < comp->elem_count; i++) {
155 int err;
156
157 err = comp_add_elem(buf, &comp->elem[i], i == 0);
158 if (err) {
159 return err;
160 }
161 }
162
163 return 0;
164 }
165
dev_comp_data_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)166 static void dev_comp_data_get(struct bt_mesh_model *model,
167 struct bt_mesh_msg_ctx *ctx,
168 struct os_mbuf *buf)
169 {
170 struct os_mbuf *sdu = NET_BUF_SIMPLE(BT_MESH_TX_SDU_MAX);
171 u8_t page;
172
173 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
174 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
175 bt_hex(buf->om_data, buf->om_len));
176
177 page = net_buf_simple_pull_u8(buf);
178 if (page != 0) {
179 BT_WARN("Composition page %u not available", page);
180 page = 0;
181 }
182
183 bt_mesh_model_msg_init(sdu, OP_DEV_COMP_DATA_STATUS);
184
185 net_buf_simple_add_u8(sdu, page);
186 if (comp_get_page_0(sdu) < 0) {
187 BT_ERR("Unable to get composition page 0");
188 goto done;
189 }
190
191 if (bt_mesh_model_send(model, ctx, sdu, NULL, NULL)) {
192 BT_ERR("Unable to send Device Composition Status response");
193 }
194
195 done:
196 os_mbuf_free_chain(sdu);
197 }
198
get_model(struct bt_mesh_elem * elem,struct os_mbuf * buf,bool * vnd)199 static struct bt_mesh_model *get_model(struct bt_mesh_elem *elem,
200 struct os_mbuf *buf, bool *vnd)
201 {
202 if (buf->om_len < 4) {
203 u16_t id;
204
205 id = net_buf_simple_pull_le16(buf);
206
207 BT_DBG("ID 0x%04x addr 0x%04x", id, elem->addr);
208
209 *vnd = false;
210
211 return bt_mesh_model_find(elem, id);
212 } else {
213 u16_t company, id;
214
215 company = net_buf_simple_pull_le16(buf);
216 id = net_buf_simple_pull_le16(buf);
217
218 BT_DBG("Company 0x%04x ID 0x%04x addr 0x%04x", company, id,
219 elem->addr);
220
221 *vnd = true;
222
223 return bt_mesh_model_find_vnd(elem, company, id);
224 }
225 }
226
app_key_is_valid(u16_t app_idx)227 static bool app_key_is_valid(u16_t app_idx)
228 {
229 int i;
230
231 for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) {
232 struct bt_mesh_app_key *key = &bt_mesh.app_keys[i];
233
234 if (key->net_idx != BT_MESH_KEY_UNUSED &&
235 key->app_idx == app_idx) {
236 return true;
237 }
238 }
239
240 return false;
241 }
242
_mod_pub_set(struct bt_mesh_model * model,u16_t pub_addr,u16_t app_idx,u8_t cred_flag,u8_t ttl,u8_t period,u8_t retransmit,bool store)243 static u8_t _mod_pub_set(struct bt_mesh_model *model, u16_t pub_addr,
244 u16_t app_idx, u8_t cred_flag, u8_t ttl, u8_t period,
245 u8_t retransmit, bool store)
246 {
247 if (!model->pub) {
248 return STATUS_NVAL_PUB_PARAM;
249 }
250
251 if (!(MYNEWT_VAL(BLE_MESH_LOW_POWER)) && cred_flag) {
252 return STATUS_FEAT_NOT_SUPP;
253 }
254
255 if (!model->pub->update && period) {
256 return STATUS_NVAL_PUB_PARAM;
257 }
258
259 if (pub_addr == BT_MESH_ADDR_UNASSIGNED) {
260 if (model->pub->addr == BT_MESH_ADDR_UNASSIGNED) {
261 return STATUS_SUCCESS;
262 }
263
264 model->pub->addr = BT_MESH_ADDR_UNASSIGNED;
265 model->pub->key = 0;
266 model->pub->cred = 0;
267 model->pub->ttl = 0;
268 model->pub->period = 0;
269 model->pub->retransmit = 0;
270 model->pub->count = 0;
271
272 if (model->pub->update) {
273 k_delayed_work_cancel(&model->pub->timer);
274 }
275
276 if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
277 bt_mesh_store_mod_pub(model);
278 }
279
280 return STATUS_SUCCESS;
281 }
282
283 if (!bt_mesh_app_key_find(app_idx)) {
284 return STATUS_INVALID_APPKEY;
285 }
286
287 model->pub->addr = pub_addr;
288 model->pub->key = app_idx;
289 model->pub->cred = cred_flag;
290 model->pub->ttl = ttl;
291 model->pub->period = period;
292 model->pub->retransmit = retransmit;
293
294 if (model->pub->update) {
295 s32_t period_ms;
296
297 period_ms = bt_mesh_model_pub_period_get(model);
298 BT_DBG("period %u ms", period_ms);
299
300 if (period_ms) {
301 k_delayed_work_submit(&model->pub->timer, period_ms);
302 } else {
303 k_delayed_work_cancel(&model->pub->timer);
304 }
305 }
306
307 if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
308 bt_mesh_store_mod_pub(model);
309 }
310
311 return STATUS_SUCCESS;
312 }
313
mod_bind(struct bt_mesh_model * model,u16_t key_idx)314 u8_t mod_bind(struct bt_mesh_model *model, u16_t key_idx)
315 {
316 int i;
317
318 BT_DBG("model %p key_idx 0x%03x", model, key_idx);
319
320 if (!app_key_is_valid(key_idx)) {
321 return STATUS_INVALID_APPKEY;
322 }
323
324 for (i = 0; i < ARRAY_SIZE(model->keys); i++) {
325 /* Treat existing binding as success */
326 if (model->keys[i] == key_idx) {
327 return STATUS_SUCCESS;
328 }
329 }
330
331 for (i = 0; i < ARRAY_SIZE(model->keys); i++) {
332 if (model->keys[i] == BT_MESH_KEY_UNUSED) {
333 model->keys[i] = key_idx;
334
335 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
336 bt_mesh_store_mod_bind(model);
337 }
338
339 return STATUS_SUCCESS;
340 }
341 }
342
343 return STATUS_INSUFF_RESOURCES;
344 }
345
mod_unbind(struct bt_mesh_model * model,u16_t key_idx,bool store)346 u8_t mod_unbind(struct bt_mesh_model *model, u16_t key_idx, bool store)
347 {
348 int i;
349
350 BT_DBG("model %p key_idx 0x%03x store %u", model, key_idx, store);
351
352 if (!app_key_is_valid(key_idx)) {
353 return STATUS_INVALID_APPKEY;
354 }
355
356 for (i = 0; i < ARRAY_SIZE(model->keys); i++) {
357 if (model->keys[i] != key_idx) {
358 continue;
359 }
360
361 model->keys[i] = BT_MESH_KEY_UNUSED;
362
363 if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
364 bt_mesh_store_mod_bind(model);
365 }
366
367 if (model->pub && model->pub->key == key_idx) {
368 _mod_pub_set(model, BT_MESH_ADDR_UNASSIGNED,
369 0, 0, 0, 0, 0, store);
370 }
371 }
372
373 return STATUS_SUCCESS;
374 }
375
bt_mesh_app_key_alloc(u16_t app_idx)376 struct bt_mesh_app_key *bt_mesh_app_key_alloc(u16_t app_idx)
377 {
378 int i;
379
380 for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) {
381 struct bt_mesh_app_key *key = &bt_mesh.app_keys[i];
382
383 if (key->net_idx == BT_MESH_KEY_UNUSED) {
384 return key;
385 }
386 }
387
388 return NULL;
389 }
390
app_key_set(u16_t net_idx,u16_t app_idx,const u8_t val[16],bool update)391 static u8_t app_key_set(u16_t net_idx, u16_t app_idx, const u8_t val[16],
392 bool update)
393 {
394 struct bt_mesh_app_keys *keys;
395 struct bt_mesh_app_key *key;
396 struct bt_mesh_subnet *sub;
397
398 BT_DBG("net_idx 0x%04x app_idx %04x update %u val %s",
399 net_idx, app_idx, update, bt_hex(val, 16));
400
401 sub = bt_mesh_subnet_get(net_idx);
402 if (!sub) {
403 return STATUS_INVALID_NETKEY;
404 }
405
406 key = bt_mesh_app_key_find(app_idx);
407 if (update) {
408 if (!key) {
409 return STATUS_INVALID_APPKEY;
410 }
411
412 if (key->net_idx != net_idx) {
413 return STATUS_INVALID_BINDING;
414 }
415
416 keys = &key->keys[1];
417
418 /* The AppKey Update message shall generate an error when node
419 * is in normal operation, Phase 2, or Phase 3 or in Phase 1
420 * when the AppKey Update message on a valid AppKeyIndex when
421 * the AppKey value is different.
422 */
423 if (sub->kr_phase != BT_MESH_KR_PHASE_1) {
424 return STATUS_CANNOT_UPDATE;
425 }
426
427 if (key->updated) {
428 if (memcmp(keys->val, val, 16)) {
429 return STATUS_CANNOT_UPDATE;
430 } else {
431 return STATUS_SUCCESS;
432 }
433 }
434
435 key->updated = true;
436 } else {
437 if (key) {
438 if (key->net_idx == net_idx &&
439 !memcmp(key->keys[0].val, val, 16)) {
440 return STATUS_SUCCESS;
441 }
442
443 if (key->net_idx == net_idx) {
444 return STATUS_IDX_ALREADY_STORED;
445 } else {
446 return STATUS_INVALID_NETKEY;
447 }
448 }
449
450 key = bt_mesh_app_key_alloc(app_idx);
451 if (!key) {
452 return STATUS_INSUFF_RESOURCES;
453 }
454
455 keys = &key->keys[0];
456 }
457
458 if (bt_mesh_app_id(val, &keys->id)) {
459 if (update) {
460 key->updated = false;
461 }
462
463 return STATUS_STORAGE_FAIL;
464 }
465
466 BT_DBG("app_idx 0x%04x AID 0x%02x", app_idx, keys->id);
467
468 key->net_idx = net_idx;
469 key->app_idx = app_idx;
470 memcpy(keys->val, val, 16);
471
472 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
473 BT_DBG("Storing AppKey persistently");
474 bt_mesh_store_app_key(key);
475 }
476
477 return STATUS_SUCCESS;
478 }
479
app_key_add(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)480 static void app_key_add(struct bt_mesh_model *model,
481 struct bt_mesh_msg_ctx *ctx,
482 struct os_mbuf *buf)
483 {
484 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 4 + 4);
485 u16_t key_net_idx, key_app_idx;
486 u8_t status;
487
488 key_idx_unpack(buf, &key_net_idx, &key_app_idx);
489
490 BT_DBG("AppIdx 0x%04x NetIdx 0x%04x", key_app_idx, key_net_idx);
491
492 bt_mesh_model_msg_init(msg, OP_APP_KEY_STATUS);
493
494 status = app_key_set(key_net_idx, key_app_idx, buf->om_data, false);
495 BT_DBG("status 0x%02x", status);
496 net_buf_simple_add_u8(msg, status);
497
498 key_idx_pack(msg, key_net_idx, key_app_idx);
499
500 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
501 BT_ERR("Unable to send App Key Status response");
502 }
503
504 os_mbuf_free_chain(msg);
505 }
506
app_key_update(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)507 static void app_key_update(struct bt_mesh_model *model,
508 struct bt_mesh_msg_ctx *ctx,
509 struct os_mbuf *buf)
510 {
511 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 4 + 4);
512 u16_t key_net_idx, key_app_idx;
513 u8_t status;
514
515 key_idx_unpack(buf, &key_net_idx, &key_app_idx);
516
517 BT_DBG("AppIdx 0x%04x NetIdx 0x%04x", key_app_idx, key_net_idx);
518
519 bt_mesh_model_msg_init(msg, OP_APP_KEY_STATUS);
520
521 status = app_key_set(key_net_idx, key_app_idx, buf->om_data, true);
522 BT_DBG("status 0x%02x", status);
523 net_buf_simple_add_u8(msg, status);
524
525 key_idx_pack(msg, key_net_idx, key_app_idx);
526
527 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
528 BT_ERR("Unable to send App Key Status response");
529 }
530
531 os_mbuf_free_chain(msg);
532 }
533
534 struct unbind_data {
535 u16_t app_idx;
536 bool store;
537 };
538
_mod_unbind(struct bt_mesh_model * mod,struct bt_mesh_elem * elem,bool vnd,bool primary,void * user_data)539 static void _mod_unbind(struct bt_mesh_model *mod, struct bt_mesh_elem *elem,
540 bool vnd, bool primary, void *user_data)
541 {
542 struct unbind_data *data = user_data;
543
544 mod_unbind(mod, data->app_idx, data->store);
545 }
546
bt_mesh_app_key_del(struct bt_mesh_app_key * key,bool store)547 void bt_mesh_app_key_del(struct bt_mesh_app_key *key, bool store)
548 {
549 struct unbind_data data = { .app_idx = key->app_idx, .store = store };
550
551 BT_DBG("AppIdx 0x%03x store %u", key->app_idx, store);
552
553 bt_mesh_model_foreach(_mod_unbind, &data);
554
555 if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
556 bt_mesh_clear_app_key(key);
557 }
558
559 key->net_idx = BT_MESH_KEY_UNUSED;
560 memset(key->keys, 0, sizeof(key->keys));
561 }
562
app_key_del(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)563 static void app_key_del(struct bt_mesh_model *model,
564 struct bt_mesh_msg_ctx *ctx,
565 struct os_mbuf *buf)
566 {
567 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 4 + 4);
568 u16_t key_net_idx, key_app_idx;
569 struct bt_mesh_app_key *key;
570 u8_t status;
571
572 key_idx_unpack(buf, &key_net_idx, &key_app_idx);
573
574 BT_DBG("AppIdx 0x%04x NetIdx 0x%04x", key_app_idx, key_net_idx);
575
576 if (!bt_mesh_subnet_get(key_net_idx)) {
577 status = STATUS_INVALID_NETKEY;
578 goto send_status;
579 }
580
581 key = bt_mesh_app_key_find(key_app_idx);
582 if (!key) {
583 /* Treat as success since the client might have missed a
584 * previous response and is resending the request.
585 */
586 status = STATUS_SUCCESS;
587 goto send_status;
588 }
589
590 if (key->net_idx != key_net_idx) {
591 status = STATUS_INVALID_BINDING;
592 goto send_status;
593 }
594
595 bt_mesh_app_key_del(key, true);
596 status = STATUS_SUCCESS;
597
598 send_status:
599 bt_mesh_model_msg_init(msg, OP_APP_KEY_STATUS);
600
601 net_buf_simple_add_u8(msg, status);
602
603 key_idx_pack(msg, key_net_idx, key_app_idx);
604
605 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
606 BT_ERR("Unable to send App Key Status response");
607 }
608
609 os_mbuf_free_chain(msg);
610 }
611
612 /* Index list length: 3 bytes for every pair and 2 bytes for an odd idx */
613 #define IDX_LEN(num) (((num) / 2) * 3 + ((num) % 2) * 2)
614
app_key_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)615 static void app_key_get(struct bt_mesh_model *model,
616 struct bt_mesh_msg_ctx *ctx,
617 struct os_mbuf *buf)
618 {
619 struct os_mbuf *msg =
620 NET_BUF_SIMPLE(2 + 3 + 4 +
621 IDX_LEN(MYNEWT_VAL(BLE_MESH_APP_KEY_COUNT)));
622 u16_t get_idx, i, prev;
623 u8_t status;
624
625 get_idx = net_buf_simple_pull_le16(buf);
626 if (get_idx > 0xfff) {
627 BT_ERR("Invalid NetKeyIndex 0x%04x", get_idx);
628 goto done;
629 }
630
631 BT_DBG("idx 0x%04x", get_idx);
632
633 bt_mesh_model_msg_init(msg, OP_APP_KEY_LIST);
634
635 if (!bt_mesh_subnet_get(get_idx)) {
636 status = STATUS_INVALID_NETKEY;
637 } else {
638 status = STATUS_SUCCESS;
639 }
640
641 net_buf_simple_add_u8(msg, status);
642 net_buf_simple_add_le16(msg, get_idx);
643
644 if (status != STATUS_SUCCESS) {
645 goto send_status;
646 }
647
648 prev = BT_MESH_KEY_UNUSED;
649 for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) {
650 struct bt_mesh_app_key *key = &bt_mesh.app_keys[i];
651
652 if (key->net_idx != get_idx) {
653 continue;
654 }
655
656 if (prev == BT_MESH_KEY_UNUSED) {
657 prev = key->app_idx;
658 continue;
659 }
660
661 key_idx_pack(msg, prev, key->app_idx);
662 prev = BT_MESH_KEY_UNUSED;
663 }
664
665 if (prev != BT_MESH_KEY_UNUSED) {
666 net_buf_simple_add_le16(msg, prev);
667 }
668
669 send_status:
670 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
671 BT_ERR("Unable to send AppKey List");
672 }
673
674 done:
675 os_mbuf_free_chain(msg);
676 }
677
beacon_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)678 static void beacon_get(struct bt_mesh_model *model,
679 struct bt_mesh_msg_ctx *ctx,
680 struct os_mbuf *buf)
681 {
682 /* Needed size: opcode (2 bytes) + msg + MIC */
683 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4);
684
685 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
686 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
687 bt_hex(buf->om_data, buf->om_len));
688
689 bt_mesh_model_msg_init(msg, OP_BEACON_STATUS);
690 net_buf_simple_add_u8(msg, bt_mesh_beacon_get());
691
692 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
693 BT_ERR("Unable to send Config Beacon Status response");
694 }
695 os_mbuf_free_chain(msg);
696 }
697
beacon_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)698 static void beacon_set(struct bt_mesh_model *model,
699 struct bt_mesh_msg_ctx *ctx,
700 struct os_mbuf *buf)
701 {
702 /* Needed size: opcode (2 bytes) + msg + MIC */
703 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4);
704 struct bt_mesh_cfg_srv *cfg = model->user_data;
705
706 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
707 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
708 bt_hex(buf->om_data, buf->om_len));
709
710 if (!cfg) {
711 BT_WARN("No Configuration Server context available");
712 } else if (buf->om_data[0] == 0x00 || buf->om_data[0] == 0x01) {
713 if (buf->om_data[0] != cfg->beacon) {
714 cfg->beacon = buf->om_data[0];
715
716 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
717 bt_mesh_store_cfg();
718 }
719
720 if (cfg->beacon) {
721 bt_mesh_beacon_enable();
722 } else {
723 bt_mesh_beacon_disable();
724 }
725 }
726 } else {
727 BT_WARN("Invalid Config Beacon value 0x%02x", buf->om_data[0]);
728 goto done;
729 }
730
731 bt_mesh_model_msg_init(msg, OP_BEACON_STATUS);
732 net_buf_simple_add_u8(msg, bt_mesh_beacon_get());
733
734 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
735 BT_ERR("Unable to send Config Beacon Status response");
736 }
737
738 done:
739 os_mbuf_free_chain(msg);
740
741 }
742
default_ttl_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)743 static void default_ttl_get(struct bt_mesh_model *model,
744 struct bt_mesh_msg_ctx *ctx,
745 struct os_mbuf *buf)
746 {
747 /* Needed size: opcode (2 bytes) + msg + MIC */
748 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4);
749
750 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
751 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
752 bt_hex(buf->om_data, buf->om_len));
753
754 bt_mesh_model_msg_init(msg, OP_DEFAULT_TTL_STATUS);
755 net_buf_simple_add_u8(msg, bt_mesh_default_ttl_get());
756
757 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
758 BT_ERR("Unable to send Default TTL Status response");
759 }
760
761 os_mbuf_free_chain(msg);
762
763 }
764
default_ttl_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)765 static void default_ttl_set(struct bt_mesh_model *model,
766 struct bt_mesh_msg_ctx *ctx,
767 struct os_mbuf *buf)
768 {
769 /* Needed size: opcode (2 bytes) + msg + MIC */
770 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4);
771 struct bt_mesh_cfg_srv *cfg = model->user_data;
772
773 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
774 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
775 bt_hex(buf->om_data, buf->om_len));
776
777 if (!cfg) {
778 BT_WARN("No Configuration Server context available");
779 } else if (buf->om_data[0] <= BT_MESH_TTL_MAX && buf->om_data[0] != 0x01) {
780 if (cfg->default_ttl != buf->om_data[0]) {
781 cfg->default_ttl = buf->om_data[0];
782
783 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
784 bt_mesh_store_cfg();
785 }
786 }
787 } else {
788 BT_WARN("Prohibited Default TTL value 0x%02x", buf->om_data[0]);
789 goto done;
790 }
791
792 bt_mesh_model_msg_init(msg, OP_DEFAULT_TTL_STATUS);
793 net_buf_simple_add_u8(msg, bt_mesh_default_ttl_get());
794
795 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
796 BT_ERR("Unable to send Default TTL Status response");
797 }
798
799 done:
800 os_mbuf_free_chain(msg);
801 }
802
send_gatt_proxy_status(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx)803 static void send_gatt_proxy_status(struct bt_mesh_model *model,
804 struct bt_mesh_msg_ctx *ctx)
805 {
806 /* Needed size: opcode (2 bytes) + msg + MIC */
807 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4);
808
809 bt_mesh_model_msg_init(msg, OP_GATT_PROXY_STATUS);
810 net_buf_simple_add_u8(msg, bt_mesh_gatt_proxy_get());
811
812 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
813 BT_ERR("Unable to send GATT Proxy Status");
814 }
815
816 os_mbuf_free_chain(msg);
817
818 }
819
gatt_proxy_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)820 static void gatt_proxy_get(struct bt_mesh_model *model,
821 struct bt_mesh_msg_ctx *ctx,
822 struct os_mbuf *buf)
823 {
824 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
825 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
826 bt_hex(buf->om_data, buf->om_len));
827
828 send_gatt_proxy_status(model, ctx);
829 }
830
gatt_proxy_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)831 static void gatt_proxy_set(struct bt_mesh_model *model,
832 struct bt_mesh_msg_ctx *ctx,
833 struct os_mbuf *buf)
834 {
835 struct bt_mesh_cfg_srv *cfg = model->user_data;
836 struct bt_mesh_subnet *sub;
837
838 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
839 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
840 bt_hex(buf->om_data, buf->om_len));
841
842 if (buf->om_data[0] != 0x00 && buf->om_data[0] != 0x01) {
843 BT_WARN("Invalid GATT Proxy value 0x%02x", buf->om_data[0]);
844 return;
845 }
846
847 if (!(MYNEWT_VAL(BLE_MESH_GATT_PROXY)) ||
848 bt_mesh_gatt_proxy_get() == BT_MESH_GATT_PROXY_NOT_SUPPORTED) {
849 goto send_status;
850 }
851
852 if (!cfg) {
853 BT_WARN("No Configuration Server context available");
854 goto send_status;
855 }
856
857 BT_DBG("GATT Proxy 0x%02x -> 0x%02x", cfg->gatt_proxy, buf->om_data[0]);
858
859 if (cfg->gatt_proxy == buf->om_data[0]) {
860 goto send_status;
861 }
862
863 cfg->gatt_proxy = buf->om_data[0];
864
865 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
866 bt_mesh_store_cfg();
867 }
868
869 if (cfg->gatt_proxy == BT_MESH_GATT_PROXY_DISABLED) {
870 int i;
871
872 /* Section 4.2.11.1: "When the GATT Proxy state is set to
873 * 0x00, the Node Identity state for all subnets shall be set
874 * to 0x00 and shall not be changed."
875 */
876 for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
877 struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
878
879 if (sub->net_idx != BT_MESH_KEY_UNUSED) {
880 bt_mesh_proxy_identity_stop(sub);
881 }
882 }
883
884 /* Section 4.2.11: "Upon transition from GATT Proxy state 0x01
885 * to GATT Proxy state 0x00 the GATT Bearer Server shall
886 * disconnect all GATT Bearer Clients.
887 */
888 bt_mesh_proxy_gatt_disconnect();
889 }
890
891 bt_mesh_adv_update();
892
893 sub = bt_mesh_subnet_get(cfg->hb_pub.net_idx);
894 if ((cfg->hb_pub.feat & BT_MESH_FEAT_PROXY) && sub) {
895 hb_send(model);
896 }
897
898 send_status:
899 send_gatt_proxy_status(model, ctx);
900 }
901
net_transmit_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)902 static void net_transmit_get(struct bt_mesh_model *model,
903 struct bt_mesh_msg_ctx *ctx,
904 struct os_mbuf *buf)
905 {
906 /* Needed size: opcode (2 bytes) + msg + MIC */
907 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4);
908
909 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
910 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
911 bt_hex(buf->om_data, buf->om_len));
912
913 bt_mesh_model_msg_init(msg, OP_NET_TRANSMIT_STATUS);
914 net_buf_simple_add_u8(msg, bt_mesh_net_transmit_get());
915
916 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
917 BT_ERR("Unable to send Config Network Transmit Status");
918 }
919
920 os_mbuf_free_chain(msg);
921
922 }
923
net_transmit_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)924 static void net_transmit_set(struct bt_mesh_model *model,
925 struct bt_mesh_msg_ctx *ctx,
926 struct os_mbuf *buf)
927 {
928 /* Needed size: opcode (2 bytes) + msg + MIC */
929 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4);
930 struct bt_mesh_cfg_srv *cfg = model->user_data;
931
932 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
933 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
934 bt_hex(buf->om_data, buf->om_len));
935
936 BT_DBG("Transmit 0x%02x (count %u interval %ums)", buf->om_data[0],
937 BT_MESH_TRANSMIT_COUNT(buf->om_data[0]),
938 BT_MESH_TRANSMIT_INT(buf->om_data[0]));
939
940 if (!cfg) {
941 BT_WARN("No Configuration Server context available");
942 } else {
943 cfg->net_transmit = buf->om_data[0];
944
945 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
946 bt_mesh_store_cfg();
947 }
948 }
949
950 bt_mesh_model_msg_init(msg, OP_NET_TRANSMIT_STATUS);
951 net_buf_simple_add_u8(msg, bt_mesh_net_transmit_get());
952
953 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
954 BT_ERR("Unable to send Network Transmit Status");
955 }
956
957 os_mbuf_free_chain(msg);
958 }
959
relay_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)960 static void relay_get(struct bt_mesh_model *model,
961 struct bt_mesh_msg_ctx *ctx,
962 struct os_mbuf *buf)
963 {
964 /* Needed size: opcode (2 bytes) + msg + MIC */
965 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 2 + 4);
966
967 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
968 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
969 bt_hex(buf->om_data, buf->om_len));
970
971 bt_mesh_model_msg_init(msg, OP_RELAY_STATUS);
972 net_buf_simple_add_u8(msg, bt_mesh_relay_get());
973 net_buf_simple_add_u8(msg, bt_mesh_relay_retransmit_get());
974
975 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
976 BT_ERR("Unable to send Config Relay Status response");
977 }
978
979 os_mbuf_free_chain(msg);
980
981 }
982
relay_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)983 static void relay_set(struct bt_mesh_model *model,
984 struct bt_mesh_msg_ctx *ctx,
985 struct os_mbuf *buf)
986 {
987 /* Needed size: opcode (2 bytes) + msg + MIC */
988 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 2 + 4);
989 struct bt_mesh_cfg_srv *cfg = model->user_data;
990
991 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
992 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
993 bt_hex(buf->om_data, buf->om_len));
994
995 if (!cfg) {
996 BT_WARN("No Configuration Server context available");
997 } else if (buf->om_data[0] == 0x00 || buf->om_data[0] == 0x01) {
998 struct bt_mesh_subnet *sub;
999 bool change;
1000
1001 if (cfg->relay == BT_MESH_RELAY_NOT_SUPPORTED) {
1002 change = false;
1003 } else {
1004 change = (cfg->relay != buf->om_data[0]);
1005 cfg->relay = buf->om_data[0];
1006 cfg->relay_retransmit = buf->om_data[1];
1007
1008 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
1009 bt_mesh_store_cfg();
1010 }
1011 }
1012
1013 BT_DBG("Relay 0x%02x (%s) xmit 0x%02x (count %u interval %u)",
1014 cfg->relay, change ? "changed" : "not changed",
1015 cfg->relay_retransmit,
1016 BT_MESH_TRANSMIT_COUNT(cfg->relay_retransmit),
1017 BT_MESH_TRANSMIT_INT(cfg->relay_retransmit));
1018
1019 sub = bt_mesh_subnet_get(cfg->hb_pub.net_idx);
1020 if ((cfg->hb_pub.feat & BT_MESH_FEAT_RELAY) && sub && change) {
1021 hb_send(model);
1022 }
1023 } else {
1024 BT_WARN("Invalid Relay value 0x%02x", buf->om_data[0]);
1025 goto done;
1026 }
1027
1028 bt_mesh_model_msg_init(msg, OP_RELAY_STATUS);
1029 net_buf_simple_add_u8(msg, bt_mesh_relay_get());
1030 net_buf_simple_add_u8(msg, bt_mesh_relay_retransmit_get());
1031
1032 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
1033 BT_ERR("Unable to send Relay Status response");
1034 }
1035
1036 done:
1037 os_mbuf_free_chain(msg);
1038
1039 }
1040
send_mod_pub_status(struct bt_mesh_model * cfg_mod,struct bt_mesh_msg_ctx * ctx,u16_t elem_addr,u16_t pub_addr,bool vnd,struct bt_mesh_model * mod,u8_t status,u8_t * mod_id)1041 static void send_mod_pub_status(struct bt_mesh_model *cfg_mod,
1042 struct bt_mesh_msg_ctx *ctx,
1043 u16_t elem_addr, u16_t pub_addr,
1044 bool vnd, struct bt_mesh_model *mod,
1045 u8_t status, u8_t *mod_id)
1046 {
1047 /* Needed size: opcode (2 bytes) + msg + MIC */
1048 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 14 + 4);
1049
1050 bt_mesh_model_msg_init(msg, OP_MOD_PUB_STATUS);
1051
1052 net_buf_simple_add_u8(msg, status);
1053 net_buf_simple_add_le16(msg, elem_addr);
1054
1055 if (status != STATUS_SUCCESS) {
1056 memset(net_buf_simple_add(msg, 7), 0, 7);
1057 } else {
1058 u16_t idx_cred;
1059
1060 net_buf_simple_add_le16(msg, pub_addr);
1061
1062 idx_cred = mod->pub->key | (u16_t)mod->pub->cred << 12;
1063 net_buf_simple_add_le16(msg, idx_cred);
1064 net_buf_simple_add_u8(msg, mod->pub->ttl);
1065 net_buf_simple_add_u8(msg, mod->pub->period);
1066 net_buf_simple_add_u8(msg, mod->pub->retransmit);
1067 }
1068
1069 if (vnd) {
1070 memcpy(net_buf_simple_add(msg, 4), mod_id, 4);
1071 } else {
1072 memcpy(net_buf_simple_add(msg, 2), mod_id, 2);
1073 }
1074
1075 if (bt_mesh_model_send(cfg_mod, ctx, msg, NULL, NULL)) {
1076 BT_ERR("Unable to send Model Publication Status");
1077 }
1078
1079 os_mbuf_free_chain(msg);
1080 }
1081
mod_pub_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1082 static void mod_pub_get(struct bt_mesh_model *model,
1083 struct bt_mesh_msg_ctx *ctx,
1084 struct os_mbuf *buf)
1085 {
1086 u16_t elem_addr, pub_addr = 0;
1087 struct bt_mesh_model *mod;
1088 struct bt_mesh_elem *elem;
1089 u8_t *mod_id, status;
1090 bool vnd;
1091
1092 elem_addr = net_buf_simple_pull_le16(buf);
1093 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
1094 BT_WARN("Prohibited element address");
1095 return;
1096 }
1097
1098 mod_id = buf->om_data;
1099
1100 BT_DBG("elem_addr 0x%04x", elem_addr);
1101
1102 elem = bt_mesh_elem_find(elem_addr);
1103 if (!elem) {
1104 mod = NULL;
1105 vnd = (buf->om_len == 4);
1106 status = STATUS_INVALID_ADDRESS;
1107 goto send_status;
1108 }
1109
1110 mod = get_model(elem, buf, &vnd);
1111 if (!mod) {
1112 status = STATUS_INVALID_MODEL;
1113 goto send_status;
1114 }
1115
1116 if (!mod->pub) {
1117 status = STATUS_NVAL_PUB_PARAM;
1118 goto send_status;
1119 }
1120
1121 pub_addr = mod->pub->addr;
1122 status = STATUS_SUCCESS;
1123
1124 send_status:
1125 send_mod_pub_status(model, ctx, elem_addr, pub_addr, vnd, mod,
1126 status, mod_id);
1127 }
1128
mod_pub_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1129 static void mod_pub_set(struct bt_mesh_model *model,
1130 struct bt_mesh_msg_ctx *ctx,
1131 struct os_mbuf *buf)
1132 {
1133 u8_t retransmit, status, pub_ttl, pub_period, cred_flag;
1134 u16_t elem_addr, pub_addr, pub_app_idx;
1135 struct bt_mesh_model *mod;
1136 struct bt_mesh_elem *elem;
1137 u8_t *mod_id;
1138 bool vnd;
1139
1140 elem_addr = net_buf_simple_pull_le16(buf);
1141 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
1142 BT_WARN("Prohibited element address");
1143 return;
1144 }
1145
1146 pub_addr = net_buf_simple_pull_le16(buf);
1147 pub_app_idx = net_buf_simple_pull_le16(buf);
1148 cred_flag = ((pub_app_idx >> 12) & BIT_MASK(1));
1149 pub_app_idx &= BIT_MASK(12);
1150
1151 pub_ttl = net_buf_simple_pull_u8(buf);
1152 if (pub_ttl > BT_MESH_TTL_MAX && pub_ttl != BT_MESH_TTL_DEFAULT) {
1153 BT_ERR("Invalid TTL value 0x%02x", pub_ttl);
1154 return;
1155 }
1156
1157 pub_period = net_buf_simple_pull_u8(buf);
1158 retransmit = net_buf_simple_pull_u8(buf);
1159 mod_id = buf->om_data;
1160
1161 BT_DBG("elem_addr 0x%04x pub_addr 0x%04x cred_flag %u",
1162 elem_addr, pub_addr, cred_flag);
1163 BT_DBG("pub_app_idx 0x%03x, pub_ttl %u pub_period 0x%02x",
1164 pub_app_idx, pub_ttl, pub_period);
1165 BT_DBG("retransmit 0x%02x (count %u interval %ums)", retransmit,
1166 BT_MESH_PUB_TRANSMIT_COUNT(retransmit),
1167 BT_MESH_PUB_TRANSMIT_INT(retransmit));
1168
1169 elem = bt_mesh_elem_find(elem_addr);
1170 if (!elem) {
1171 mod = NULL;
1172 vnd = (buf->om_len == 4);
1173 status = STATUS_INVALID_ADDRESS;
1174 goto send_status;
1175 }
1176
1177 mod = get_model(elem, buf, &vnd);
1178 if (!mod) {
1179 status = STATUS_INVALID_MODEL;
1180 goto send_status;
1181 }
1182
1183 status = _mod_pub_set(mod, pub_addr, pub_app_idx, cred_flag, pub_ttl,
1184 pub_period, retransmit, true);
1185
1186 send_status:
1187 send_mod_pub_status(model, ctx, elem_addr, pub_addr, vnd, mod,
1188 status, mod_id);
1189 }
1190
1191 #if MYNEWT_VAL(BLE_MESH_LABEL_COUNT) > 0
va_add(u8_t * label_uuid,u16_t * addr)1192 static u8_t va_add(u8_t *label_uuid, u16_t *addr)
1193 {
1194 struct label *free_slot = NULL;
1195 int i;
1196
1197 for (i = 0; i < ARRAY_SIZE(labels); i++) {
1198 if (!labels[i].ref) {
1199 free_slot = &labels[i];
1200 continue;
1201 }
1202
1203 if (!memcmp(labels[i].uuid, label_uuid, 16)) {
1204 *addr = labels[i].addr;
1205 labels[i].ref++;
1206 return STATUS_SUCCESS;
1207 }
1208 }
1209
1210 if (!free_slot) {
1211 return STATUS_INSUFF_RESOURCES;
1212 }
1213
1214 if (bt_mesh_virtual_addr(label_uuid, addr) < 0) {
1215 return STATUS_UNSPECIFIED;
1216 }
1217
1218 free_slot->ref = 1;
1219 free_slot->addr = *addr;
1220 memcpy(free_slot->uuid, label_uuid, 16);
1221
1222 return STATUS_SUCCESS;
1223 }
1224
va_del(u8_t * label_uuid,u16_t * addr)1225 static u8_t va_del(u8_t *label_uuid, u16_t *addr)
1226 {
1227 int i;
1228
1229 for (i = 0; i < ARRAY_SIZE(labels); i++) {
1230 if (!memcmp(labels[i].uuid, label_uuid, 16)) {
1231 if (addr) {
1232 *addr = labels[i].addr;
1233 }
1234
1235 labels[i].ref--;
1236 return STATUS_SUCCESS;
1237 }
1238 }
1239
1240 if (addr) {
1241 *addr = BT_MESH_ADDR_UNASSIGNED;
1242 }
1243
1244 return STATUS_CANNOT_REMOVE;
1245 }
1246
mod_sub_list_clear(struct bt_mesh_model * mod)1247 static void mod_sub_list_clear(struct bt_mesh_model *mod)
1248 {
1249 u8_t *label_uuid;
1250 int i;
1251
1252 /* Unref stored labels related to this model */
1253 for (i = 0; i < ARRAY_SIZE(mod->groups); i++) {
1254 if (!BT_MESH_ADDR_IS_VIRTUAL(mod->groups[i])) {
1255 continue;
1256 }
1257
1258 label_uuid = bt_mesh_label_uuid_get(mod->groups[i]);
1259 if (!label_uuid) {
1260 BT_ERR("Label UUID not found");
1261 continue;
1262 }
1263
1264 va_del(label_uuid, NULL);
1265 }
1266
1267 /* Clear all subscriptions (0x0000 is the unassigned address) */
1268 memset(mod->groups, 0, sizeof(mod->groups));
1269 }
1270
mod_pub_va_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1271 static void mod_pub_va_set(struct bt_mesh_model *model,
1272 struct bt_mesh_msg_ctx *ctx,
1273 struct os_mbuf *buf)
1274 {
1275 u8_t retransmit, status, pub_ttl, pub_period, cred_flag;
1276 u16_t elem_addr, pub_addr, pub_app_idx;
1277 struct bt_mesh_model *mod;
1278 struct bt_mesh_elem *elem;
1279 u8_t *label_uuid;
1280 u8_t *mod_id;
1281 bool vnd;
1282
1283 elem_addr = net_buf_simple_pull_le16(buf);
1284 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
1285 BT_WARN("Prohibited element address");
1286 return;
1287 }
1288
1289 label_uuid = buf->om_data;
1290 net_buf_simple_pull(buf, 16);
1291
1292 pub_app_idx = net_buf_simple_pull_le16(buf);
1293 cred_flag = ((pub_app_idx >> 12) & BIT_MASK(1));
1294 pub_app_idx &= BIT_MASK(12);
1295 pub_ttl = net_buf_simple_pull_u8(buf);
1296 if (pub_ttl > BT_MESH_TTL_MAX && pub_ttl != BT_MESH_TTL_DEFAULT) {
1297 BT_ERR("Invalid TTL value 0x%02x", pub_ttl);
1298 return;
1299 }
1300
1301 pub_period = net_buf_simple_pull_u8(buf);
1302 retransmit = net_buf_simple_pull_u8(buf);
1303 mod_id = buf->om_data;
1304
1305 BT_DBG("elem_addr 0x%04x pub_addr 0x%04x cred_flag %u",
1306 elem_addr, pub_addr, cred_flag);
1307 BT_DBG("pub_app_idx 0x%03x, pub_ttl %u pub_period 0x%02x",
1308 pub_app_idx, pub_ttl, pub_period);
1309 BT_DBG("retransmit 0x%02x (count %u interval %ums)", retransmit,
1310 BT_MESH_PUB_TRANSMIT_COUNT(retransmit),
1311 BT_MESH_PUB_TRANSMIT_INT(retransmit));
1312
1313 elem = bt_mesh_elem_find(elem_addr);
1314 if (!elem) {
1315 mod = NULL;
1316 vnd = (buf->om_len == 4);
1317 pub_addr = 0;
1318 status = STATUS_INVALID_ADDRESS;
1319 goto send_status;
1320 }
1321
1322 mod = get_model(elem, buf, &vnd);
1323 if (!mod) {
1324 pub_addr = 0;
1325 status = STATUS_INVALID_MODEL;
1326 goto send_status;
1327 }
1328
1329 status = va_add(label_uuid, &pub_addr);
1330 if (status == STATUS_SUCCESS) {
1331 status = _mod_pub_set(mod, pub_addr, pub_app_idx, cred_flag,
1332 pub_ttl, pub_period, retransmit, true);
1333 }
1334
1335 send_status:
1336 send_mod_pub_status(model, ctx, elem_addr, pub_addr, vnd, mod,
1337 status, mod_id);
1338 }
1339 #else
mod_sub_list_clear(struct bt_mesh_model * mod)1340 static void mod_sub_list_clear(struct bt_mesh_model *mod)
1341 {
1342 /* Clear all subscriptions (0x0000 is the unassigned address) */
1343 memset(mod->groups, 0, sizeof(mod->groups));
1344 }
1345
mod_pub_va_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1346 static void mod_pub_va_set(struct bt_mesh_model *model,
1347 struct bt_mesh_msg_ctx *ctx,
1348 struct os_mbuf *buf)
1349 {
1350 u8_t *mod_id, status;
1351 struct bt_mesh_model *mod;
1352 struct bt_mesh_elem *elem;
1353 u16_t elem_addr, pub_addr = 0;
1354 bool vnd;
1355
1356 elem_addr = net_buf_simple_pull_le16(buf);
1357 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
1358 BT_WARN("Prohibited element address");
1359 return;
1360 }
1361
1362 net_buf_simple_pull(buf, 16);
1363 mod_id = net_buf_simple_pull(buf, 4);
1364
1365 BT_DBG("elem_addr 0x%04x", elem_addr);
1366
1367 elem = bt_mesh_elem_find(elem_addr);
1368 if (!elem) {
1369 mod = NULL;
1370 vnd = (buf->om_len == 4);
1371 status = STATUS_INVALID_ADDRESS;
1372 goto send_status;
1373 }
1374
1375 mod = get_model(elem, buf, &vnd);
1376 if (!mod) {
1377 status = STATUS_INVALID_MODEL;
1378 goto send_status;
1379 }
1380
1381 if (!mod->pub) {
1382 status = STATUS_NVAL_PUB_PARAM;
1383 goto send_status;
1384 }
1385
1386 pub_addr = mod->pub->addr;
1387 status = STATUS_INSUFF_RESOURCES;
1388
1389 send_status:
1390 send_mod_pub_status(model, ctx, elem_addr, pub_addr, vnd, mod,
1391 status, mod_id);
1392 }
1393 #endif /* MYNEWT_VAL(BLE_MESH_LABEL_COUNT) > 0 */
1394
send_mod_sub_status(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,u8_t status,u16_t elem_addr,u16_t sub_addr,u8_t * mod_id,bool vnd)1395 static void send_mod_sub_status(struct bt_mesh_model *model,
1396 struct bt_mesh_msg_ctx *ctx, u8_t status,
1397 u16_t elem_addr, u16_t sub_addr, u8_t *mod_id,
1398 bool vnd)
1399 {
1400 /* Needed size: opcode (2 bytes) + msg + MIC */
1401 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 9 + 4);
1402
1403 BT_DBG("status 0x%02x elem_addr 0x%04x sub_addr 0x%04x", status,
1404 elem_addr, sub_addr);
1405
1406 bt_mesh_model_msg_init(msg, OP_MOD_SUB_STATUS);
1407
1408 net_buf_simple_add_u8(msg, status);
1409 net_buf_simple_add_le16(msg, elem_addr);
1410 net_buf_simple_add_le16(msg, sub_addr);
1411
1412 if (vnd) {
1413 memcpy(net_buf_simple_add(msg, 4), mod_id, 4);
1414 } else {
1415 memcpy(net_buf_simple_add(msg, 2), mod_id, 2);
1416 }
1417
1418 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
1419 BT_ERR("Unable to send Model Subscription Status");
1420 }
1421
1422 os_mbuf_free_chain(msg);
1423 }
1424
mod_sub_add(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1425 static void mod_sub_add(struct bt_mesh_model *model,
1426 struct bt_mesh_msg_ctx *ctx,
1427 struct os_mbuf *buf)
1428 {
1429 u16_t elem_addr, sub_addr;
1430 struct bt_mesh_model *mod;
1431 struct bt_mesh_elem *elem;
1432 u8_t *mod_id;
1433 u8_t status;
1434 bool vnd;
1435 int i;
1436
1437 elem_addr = net_buf_simple_pull_le16(buf);
1438 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
1439 BT_WARN("Prohibited element address");
1440 return;
1441 }
1442
1443 sub_addr = net_buf_simple_pull_le16(buf);
1444
1445 BT_DBG("elem_addr 0x%04x, sub_addr 0x%04x", elem_addr, sub_addr);
1446
1447 mod_id = buf->om_data;
1448
1449 elem = bt_mesh_elem_find(elem_addr);
1450 if (!elem) {
1451 mod = NULL;
1452 vnd = (buf->om_len == 4);
1453 status = STATUS_INVALID_ADDRESS;
1454 goto send_status;
1455 }
1456
1457 mod = get_model(elem, buf, &vnd);
1458 if (!mod) {
1459 status = STATUS_INVALID_MODEL;
1460 goto send_status;
1461 }
1462
1463 if (!BT_MESH_ADDR_IS_GROUP(sub_addr)) {
1464 status = STATUS_INVALID_ADDRESS;
1465 goto send_status;
1466 }
1467
1468 if (bt_mesh_model_find_group(mod, sub_addr)) {
1469 /* Tried to add existing subscription */
1470 status = STATUS_SUCCESS;
1471 goto send_status;
1472 }
1473
1474 for (i = 0; i < ARRAY_SIZE(mod->groups); i++) {
1475 if (mod->groups[i] == BT_MESH_ADDR_UNASSIGNED) {
1476 mod->groups[i] = sub_addr;
1477 break;
1478 }
1479 }
1480
1481 if (i == ARRAY_SIZE(mod->groups)) {
1482 status = STATUS_INSUFF_RESOURCES;
1483 } else {
1484 status = STATUS_SUCCESS;
1485
1486 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
1487 bt_mesh_store_mod_sub(mod);
1488 }
1489
1490 if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER)) {
1491 bt_mesh_lpn_group_add(sub_addr);
1492 }
1493 }
1494
1495 send_status:
1496 send_mod_sub_status(model, ctx, status, elem_addr, sub_addr,
1497 mod_id, vnd);
1498 }
1499
mod_sub_del(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1500 static void mod_sub_del(struct bt_mesh_model *model,
1501 struct bt_mesh_msg_ctx *ctx,
1502 struct os_mbuf *buf)
1503 {
1504 u16_t elem_addr, sub_addr;
1505 struct bt_mesh_model *mod;
1506 struct bt_mesh_elem *elem;
1507 u8_t *mod_id;
1508 u16_t *match;
1509 u8_t status;
1510 bool vnd;
1511
1512 elem_addr = net_buf_simple_pull_le16(buf);
1513 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
1514 BT_WARN("Prohibited element address");
1515 return;
1516 }
1517
1518 sub_addr = net_buf_simple_pull_le16(buf);
1519
1520 BT_DBG("elem_addr 0x%04x sub_addr 0x%04x", elem_addr, sub_addr);
1521
1522 mod_id = buf->om_data;
1523
1524 elem = bt_mesh_elem_find(elem_addr);
1525 if (!elem) {
1526 mod = NULL;
1527 vnd = (buf->om_len == 4);
1528 status = STATUS_INVALID_ADDRESS;
1529 goto send_status;
1530 }
1531
1532 mod = get_model(elem, buf, &vnd);
1533 if (!mod) {
1534 status = STATUS_INVALID_MODEL;
1535 goto send_status;
1536 }
1537
1538 if (!BT_MESH_ADDR_IS_GROUP(sub_addr)) {
1539 status = STATUS_INVALID_ADDRESS;
1540 goto send_status;
1541 }
1542
1543 /* An attempt to remove a non-existing address shall be treated
1544 * as a success.
1545 */
1546 status = STATUS_SUCCESS;
1547
1548 if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) {
1549 bt_mesh_lpn_group_del(&sub_addr, 1);
1550 }
1551
1552 match = bt_mesh_model_find_group(mod, sub_addr);
1553 if (match) {
1554 *match = BT_MESH_ADDR_UNASSIGNED;
1555
1556 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
1557 bt_mesh_store_mod_sub(mod);
1558 }
1559 }
1560
1561 send_status:
1562 send_mod_sub_status(model, ctx, status, elem_addr, sub_addr,
1563 mod_id, vnd);
1564 }
1565
mod_sub_overwrite(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1566 static void mod_sub_overwrite(struct bt_mesh_model *model,
1567 struct bt_mesh_msg_ctx *ctx,
1568 struct os_mbuf *buf)
1569 {
1570 u16_t elem_addr, sub_addr;
1571 struct bt_mesh_model *mod;
1572 struct bt_mesh_elem *elem;
1573 u8_t *mod_id;
1574 u8_t status;
1575 bool vnd;
1576
1577 elem_addr = net_buf_simple_pull_le16(buf);
1578 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
1579 BT_WARN("Prohibited element address");
1580 return;
1581 }
1582
1583 sub_addr = net_buf_simple_pull_le16(buf);
1584
1585 BT_DBG("elem_addr 0x%04x sub_addr 0x%04x", elem_addr, sub_addr);
1586
1587 mod_id = buf->om_data;
1588
1589 elem = bt_mesh_elem_find(elem_addr);
1590 if (!elem) {
1591 mod = NULL;
1592 vnd = (buf->om_len == 4);
1593 status = STATUS_INVALID_ADDRESS;
1594 goto send_status;
1595 }
1596
1597 mod = get_model(elem, buf, &vnd);
1598 if (!mod) {
1599 status = STATUS_INVALID_MODEL;
1600 goto send_status;
1601 }
1602
1603 if (!BT_MESH_ADDR_IS_GROUP(sub_addr)) {
1604 status = STATUS_INVALID_ADDRESS;
1605 goto send_status;
1606 }
1607
1608 if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) {
1609 bt_mesh_lpn_group_del(mod->groups, ARRAY_SIZE(mod->groups));
1610 }
1611
1612 mod_sub_list_clear(mod);
1613
1614 if (ARRAY_SIZE(mod->groups) > 0) {
1615 mod->groups[0] = sub_addr;
1616 status = STATUS_SUCCESS;
1617
1618 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
1619 bt_mesh_store_mod_sub(mod);
1620 }
1621
1622 if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER)) {
1623 bt_mesh_lpn_group_add(sub_addr);
1624 }
1625 } else {
1626 status = STATUS_INSUFF_RESOURCES;
1627 }
1628
1629
1630 send_status:
1631 send_mod_sub_status(model, ctx, status, elem_addr, sub_addr,
1632 mod_id, vnd);
1633 }
1634
mod_sub_del_all(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1635 static void mod_sub_del_all(struct bt_mesh_model *model,
1636 struct bt_mesh_msg_ctx *ctx,
1637 struct os_mbuf *buf)
1638 {
1639 struct bt_mesh_model *mod;
1640 struct bt_mesh_elem *elem;
1641 u16_t elem_addr;
1642 u8_t *mod_id;
1643 u8_t status;
1644 bool vnd;
1645
1646 elem_addr = net_buf_simple_pull_le16(buf);
1647 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
1648 BT_WARN("Prohibited element address");
1649 return;
1650 }
1651
1652 BT_DBG("elem_addr 0x%04x", elem_addr);
1653
1654 mod_id = buf->om_data;
1655
1656 elem = bt_mesh_elem_find(elem_addr);
1657 if (!elem) {
1658 mod = NULL;
1659 vnd = (buf->om_len == 4);
1660 status = STATUS_INVALID_ADDRESS;
1661 goto send_status;
1662 }
1663
1664 mod = get_model(elem, buf, &vnd);
1665 if (!mod) {
1666 status = STATUS_INVALID_MODEL;
1667 goto send_status;
1668 }
1669
1670 if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) {
1671 bt_mesh_lpn_group_del(mod->groups, ARRAY_SIZE(mod->groups));
1672 }
1673
1674 mod_sub_list_clear(mod);
1675
1676 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
1677 bt_mesh_store_mod_sub(mod);
1678 }
1679
1680 status = STATUS_SUCCESS;
1681
1682 send_status:
1683 send_mod_sub_status(model, ctx, status, elem_addr,
1684 BT_MESH_ADDR_UNASSIGNED, mod_id, vnd);
1685 }
1686
mod_sub_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1687 static void mod_sub_get(struct bt_mesh_model *model,
1688 struct bt_mesh_msg_ctx *ctx,
1689 struct os_mbuf *buf)
1690 {
1691 struct os_mbuf *msg =
1692 NET_BUF_SIMPLE(2 + 5 + 4 +
1693 MYNEWT_VAL(BLE_MESH_MODEL_GROUP_COUNT) * 2);
1694 struct bt_mesh_model *mod;
1695 struct bt_mesh_elem *elem;
1696 u16_t addr, id;
1697 int i;
1698
1699 addr = net_buf_simple_pull_le16(buf);
1700 if (!BT_MESH_ADDR_IS_UNICAST(addr)) {
1701 BT_WARN("Prohibited element address");
1702 goto done;
1703 }
1704
1705 id = net_buf_simple_pull_le16(buf);
1706
1707 BT_DBG("addr 0x%04x id 0x%04x", addr, id);
1708
1709 bt_mesh_model_msg_init(msg, OP_MOD_SUB_LIST);
1710
1711 elem = bt_mesh_elem_find(addr);
1712 if (!elem) {
1713 net_buf_simple_add_u8(msg, STATUS_INVALID_ADDRESS);
1714 net_buf_simple_add_le16(msg, addr);
1715 net_buf_simple_add_le16(msg, id);
1716 goto send_list;
1717 }
1718
1719 mod = bt_mesh_model_find(elem, id);
1720 if (!mod) {
1721 net_buf_simple_add_u8(msg, STATUS_INVALID_MODEL);
1722 net_buf_simple_add_le16(msg, addr);
1723 net_buf_simple_add_le16(msg, id);
1724 goto send_list;
1725 }
1726
1727 net_buf_simple_add_u8(msg, STATUS_SUCCESS);
1728
1729 net_buf_simple_add_le16(msg, addr);
1730 net_buf_simple_add_le16(msg, id);
1731
1732 for (i = 0; i < ARRAY_SIZE(mod->groups); i++) {
1733 if (mod->groups[i] != BT_MESH_ADDR_UNASSIGNED) {
1734 net_buf_simple_add_le16(msg, mod->groups[i]);
1735 }
1736 }
1737
1738 send_list:
1739 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
1740 BT_ERR("Unable to send Model Subscription List");
1741 }
1742
1743 done:
1744 os_mbuf_free_chain(msg);
1745
1746 }
1747
mod_sub_get_vnd(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1748 static void mod_sub_get_vnd(struct bt_mesh_model *model,
1749 struct bt_mesh_msg_ctx *ctx,
1750 struct os_mbuf *buf)
1751 {
1752 struct os_mbuf *msg =
1753 NET_BUF_SIMPLE(2 + 7 + 4 +
1754 MYNEWT_VAL(BLE_MESH_MODEL_GROUP_COUNT) * 2);
1755 struct bt_mesh_model *mod;
1756 struct bt_mesh_elem *elem;
1757 u16_t company, addr, id;
1758 int i;
1759
1760 addr = net_buf_simple_pull_le16(buf);
1761 if (!BT_MESH_ADDR_IS_UNICAST(addr)) {
1762 BT_WARN("Prohibited element address");
1763 goto done;
1764 }
1765
1766 company = net_buf_simple_pull_le16(buf);
1767 id = net_buf_simple_pull_le16(buf);
1768
1769 BT_DBG("addr 0x%04x company 0x%04x id 0x%04x", addr, company, id);
1770
1771 bt_mesh_model_msg_init(msg, OP_MOD_SUB_LIST_VND);
1772
1773 elem = bt_mesh_elem_find(addr);
1774 if (!elem) {
1775 net_buf_simple_add_u8(msg, STATUS_INVALID_ADDRESS);
1776 net_buf_simple_add_le16(msg, addr);
1777 net_buf_simple_add_le16(msg, company);
1778 net_buf_simple_add_le16(msg, id);
1779 goto send_list;
1780 }
1781
1782 mod = bt_mesh_model_find_vnd(elem, company, id);
1783 if (!mod) {
1784 net_buf_simple_add_u8(msg, STATUS_INVALID_MODEL);
1785 net_buf_simple_add_le16(msg, addr);
1786 net_buf_simple_add_le16(msg, company);
1787 net_buf_simple_add_le16(msg, id);
1788 goto send_list;
1789 }
1790
1791 net_buf_simple_add_u8(msg, STATUS_SUCCESS);
1792
1793 net_buf_simple_add_le16(msg, addr);
1794 net_buf_simple_add_le16(msg, company);
1795 net_buf_simple_add_le16(msg, id);
1796
1797 for (i = 0; i < ARRAY_SIZE(mod->groups); i++) {
1798 if (mod->groups[i] != BT_MESH_ADDR_UNASSIGNED) {
1799 net_buf_simple_add_le16(msg, mod->groups[i]);
1800 }
1801 }
1802
1803 send_list:
1804 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
1805 BT_ERR("Unable to send Vendor Model Subscription List");
1806 }
1807
1808 done:
1809 os_mbuf_free_chain(msg);
1810
1811 }
1812
1813 #if MYNEWT_VAL(BLE_MESH_LABEL_COUNT) > 0
mod_sub_va_add(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1814 static void mod_sub_va_add(struct bt_mesh_model *model,
1815 struct bt_mesh_msg_ctx *ctx,
1816 struct os_mbuf *buf)
1817 {
1818 u16_t elem_addr, sub_addr;
1819 struct bt_mesh_model *mod;
1820 struct bt_mesh_elem *elem;
1821 u8_t *label_uuid;
1822 u8_t *mod_id;
1823 u8_t status;
1824 bool vnd;
1825 int i;
1826
1827 elem_addr = net_buf_simple_pull_le16(buf);
1828 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
1829 BT_WARN("Prohibited element address");
1830 return;
1831 }
1832
1833 label_uuid = buf->om_data;
1834 net_buf_simple_pull(buf, 16);
1835
1836 BT_DBG("elem_addr 0x%04x", elem_addr);
1837
1838 mod_id = buf->om_data;
1839 elem = bt_mesh_elem_find(elem_addr);
1840 if (!elem) {
1841 mod = NULL;
1842 vnd = (buf->om_len == 4);
1843 sub_addr = BT_MESH_ADDR_UNASSIGNED;
1844 status = STATUS_INVALID_ADDRESS;
1845 goto send_status;
1846 }
1847
1848 mod = get_model(elem, buf, &vnd);
1849 if (!mod) {
1850 sub_addr = BT_MESH_ADDR_UNASSIGNED;
1851 status = STATUS_INVALID_MODEL;
1852 goto send_status;
1853 }
1854
1855 status = va_add(label_uuid, &sub_addr);
1856 if (status != STATUS_SUCCESS) {
1857 goto send_status;
1858 }
1859
1860 if (bt_mesh_model_find_group(mod, sub_addr)) {
1861 /* Tried to add existing subscription */
1862 status = STATUS_SUCCESS;
1863 goto send_status;
1864 }
1865
1866 for (i = 0; i < ARRAY_SIZE(mod->groups); i++) {
1867 if (mod->groups[i] == BT_MESH_ADDR_UNASSIGNED) {
1868 mod->groups[i] = sub_addr;
1869 break;
1870 }
1871 }
1872
1873 if (i == ARRAY_SIZE(mod->groups)) {
1874 status = STATUS_INSUFF_RESOURCES;
1875 } else {
1876 if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) {
1877 bt_mesh_lpn_group_add(sub_addr);
1878 }
1879
1880 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
1881 bt_mesh_store_mod_sub(mod);
1882 }
1883
1884 status = STATUS_SUCCESS;
1885 }
1886
1887 send_status:
1888 send_mod_sub_status(model, ctx, status, elem_addr, sub_addr,
1889 mod_id, vnd);
1890 }
1891
mod_sub_va_del(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1892 static void mod_sub_va_del(struct bt_mesh_model *model,
1893 struct bt_mesh_msg_ctx *ctx,
1894 struct os_mbuf *buf)
1895 {
1896 u16_t elem_addr, sub_addr;
1897 struct bt_mesh_model *mod;
1898 struct bt_mesh_elem *elem;
1899 u8_t *label_uuid;
1900 u8_t *mod_id;
1901 u16_t *match;
1902 u8_t status;
1903 bool vnd;
1904
1905 elem_addr = net_buf_simple_pull_le16(buf);
1906 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
1907 BT_WARN("Prohibited element address");
1908 return;
1909 }
1910
1911 label_uuid = buf->om_data;
1912 net_buf_simple_pull(buf, 16);
1913
1914 BT_DBG("elem_addr 0x%04x", elem_addr);
1915
1916 mod_id = buf->om_data;
1917
1918 elem = bt_mesh_elem_find(elem_addr);
1919 if (!elem) {
1920 mod = NULL;
1921 vnd = (buf->om_len == 4);
1922 sub_addr = BT_MESH_ADDR_UNASSIGNED;
1923 status = STATUS_INVALID_ADDRESS;
1924 goto send_status;
1925 }
1926
1927 mod = get_model(elem, buf, &vnd);
1928 if (!mod) {
1929 sub_addr = BT_MESH_ADDR_UNASSIGNED;
1930 status = STATUS_INVALID_MODEL;
1931 goto send_status;
1932 }
1933
1934 status = va_del(label_uuid, &sub_addr);
1935 if (sub_addr == BT_MESH_ADDR_UNASSIGNED) {
1936 goto send_status;
1937 }
1938
1939 if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER)) {
1940 bt_mesh_lpn_group_del(&sub_addr, 1);
1941 }
1942
1943 match = bt_mesh_model_find_group(mod, sub_addr);
1944 if (match) {
1945 *match = BT_MESH_ADDR_UNASSIGNED;
1946
1947 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
1948 bt_mesh_store_mod_sub(mod);
1949 }
1950
1951 status = STATUS_SUCCESS;
1952 } else {
1953 status = STATUS_CANNOT_REMOVE;
1954 }
1955
1956 send_status:
1957 send_mod_sub_status(model, ctx, status, elem_addr, sub_addr,
1958 mod_id, vnd);
1959 }
1960
mod_sub_va_overwrite(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)1961 static void mod_sub_va_overwrite(struct bt_mesh_model *model,
1962 struct bt_mesh_msg_ctx *ctx,
1963 struct os_mbuf *buf)
1964 {
1965 u16_t elem_addr, sub_addr = BT_MESH_ADDR_UNASSIGNED;
1966 struct bt_mesh_model *mod;
1967 struct bt_mesh_elem *elem;
1968 u8_t *label_uuid;
1969 u8_t *mod_id;
1970 u8_t status;
1971 bool vnd;
1972
1973 elem_addr = net_buf_simple_pull_le16(buf);
1974 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
1975 BT_WARN("Prohibited element address");
1976 return;
1977 }
1978
1979 label_uuid = buf->om_data;
1980 net_buf_simple_pull(buf, 16);
1981 mod_id = buf->om_data;
1982
1983 BT_DBG("elem_addr 0x%04x, addr %s, mod_id %s", elem_addr,
1984 bt_hex(label_uuid, 16), bt_hex(mod_id, buf->om_len));
1985
1986 elem = bt_mesh_elem_find(elem_addr);
1987 if (!elem) {
1988 mod = NULL;
1989 vnd = (buf->om_len == 4);
1990 status = STATUS_INVALID_ADDRESS;
1991 goto send_status;
1992 }
1993
1994 mod = get_model(elem, buf, &vnd);
1995 if (!mod) {
1996 status = STATUS_INVALID_MODEL;
1997 goto send_status;
1998 }
1999
2000 if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) {
2001 bt_mesh_lpn_group_del(mod->groups, ARRAY_SIZE(mod->groups));
2002 }
2003
2004 mod_sub_list_clear(mod);
2005
2006 if (ARRAY_SIZE(mod->groups) > 0) {
2007 status = va_add(label_uuid, &sub_addr);
2008 if (status == STATUS_SUCCESS) {
2009 mod->groups[0] = sub_addr;
2010
2011 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
2012 bt_mesh_store_mod_sub(mod);
2013 }
2014
2015 if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER)) {
2016 bt_mesh_lpn_group_add(sub_addr);
2017 }
2018 }
2019 } else {
2020 status = STATUS_INSUFF_RESOURCES;
2021 }
2022
2023 send_status:
2024 send_mod_sub_status(model, ctx, status, elem_addr, sub_addr,
2025 mod_id, vnd);
2026 }
2027 #else
mod_sub_va_add(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2028 static void mod_sub_va_add(struct bt_mesh_model *model,
2029 struct bt_mesh_msg_ctx *ctx,
2030 struct os_mbuf *buf)
2031 {
2032 struct bt_mesh_model *mod;
2033 struct bt_mesh_elem *elem;
2034 u16_t elem_addr;
2035 u8_t *mod_id;
2036 u8_t status;
2037 bool vnd;
2038
2039 elem_addr = net_buf_simple_pull_le16(buf);
2040 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
2041 BT_WARN("Prohibited element address");
2042 return;
2043 }
2044
2045 net_buf_simple_pull(buf, 16);
2046
2047 mod_id = buf->om_data;
2048
2049 elem = bt_mesh_elem_find(elem_addr);
2050 if (!elem) {
2051 mod = NULL;
2052 vnd = (buf->om_len == 4);
2053 status = STATUS_INVALID_ADDRESS;
2054 goto send_status;
2055 }
2056
2057 mod = get_model(elem, buf, &vnd);
2058 if (!mod) {
2059 status = STATUS_INVALID_MODEL;
2060 goto send_status;
2061 }
2062
2063 status = STATUS_INSUFF_RESOURCES;
2064
2065 send_status:
2066 send_mod_sub_status(model, ctx, status, elem_addr,
2067 BT_MESH_ADDR_UNASSIGNED, mod_id, vnd);
2068 }
2069
mod_sub_va_del(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2070 static void mod_sub_va_del(struct bt_mesh_model *model,
2071 struct bt_mesh_msg_ctx *ctx,
2072 struct os_mbuf *buf)
2073 {
2074 struct bt_mesh_elem *elem;
2075 u16_t elem_addr;
2076 u8_t *mod_id;
2077 u8_t status;
2078 bool vnd;
2079
2080 elem_addr = net_buf_simple_pull_le16(buf);
2081 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
2082 BT_WARN("Prohibited element address");
2083 return;
2084 }
2085
2086 net_buf_simple_pull(buf, 16);
2087
2088 mod_id = buf->om_data;
2089
2090 elem = bt_mesh_elem_find(elem_addr);
2091 if (!elem) {
2092 vnd = (buf->om_len == 4);
2093 status = STATUS_INVALID_ADDRESS;
2094 goto send_status;
2095 }
2096
2097 if (!get_model(elem, buf, &vnd)) {
2098 status = STATUS_INVALID_MODEL;
2099 goto send_status;
2100 }
2101
2102 status = STATUS_INSUFF_RESOURCES;
2103
2104 send_status:
2105 send_mod_sub_status(model, ctx, status, elem_addr,
2106 BT_MESH_ADDR_UNASSIGNED, mod_id, vnd);
2107 }
2108
mod_sub_va_overwrite(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2109 static void mod_sub_va_overwrite(struct bt_mesh_model *model,
2110 struct bt_mesh_msg_ctx *ctx,
2111 struct os_mbuf *buf)
2112 {
2113 struct bt_mesh_elem *elem;
2114 u16_t elem_addr;
2115 u8_t *mod_id;
2116 u8_t status;
2117 bool vnd;
2118
2119 elem_addr = net_buf_simple_pull_le16(buf);
2120 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
2121 BT_WARN("Prohibited element address");
2122 return;
2123 }
2124
2125 net_buf_simple_pull(buf, 18);
2126
2127 mod_id = buf->om_data;
2128
2129 elem = bt_mesh_elem_find(elem_addr);
2130 if (!elem) {
2131 vnd = (buf->om_len == 4);
2132 status = STATUS_INVALID_ADDRESS;
2133 goto send_status;
2134 }
2135
2136 if (!get_model(elem, buf, &vnd)) {
2137 status = STATUS_INVALID_MODEL;
2138 goto send_status;
2139 }
2140
2141 status = STATUS_INSUFF_RESOURCES;
2142
2143 send_status:
2144 send_mod_sub_status(model, ctx, status, elem_addr,
2145 BT_MESH_ADDR_UNASSIGNED, mod_id, vnd);
2146 }
2147 #endif /* MYNEWT_VAL(BLE_MESH_LABEL_COUNT) > 0 */
2148
send_net_key_status(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,u16_t idx,u8_t status)2149 static void send_net_key_status(struct bt_mesh_model *model,
2150 struct bt_mesh_msg_ctx *ctx,
2151 u16_t idx, u8_t status)
2152 {
2153 /* Needed size: opcode (2 bytes) + msg + MIC */
2154 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 3 + 4);
2155
2156 bt_mesh_model_msg_init(msg, OP_NET_KEY_STATUS);
2157
2158 net_buf_simple_add_u8(msg, status);
2159 net_buf_simple_add_le16(msg, idx);
2160
2161 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
2162 BT_ERR("Unable to send NetKey Status");
2163 }
2164
2165 os_mbuf_free_chain(msg);
2166 }
2167
net_key_add(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2168 static void net_key_add(struct bt_mesh_model *model,
2169 struct bt_mesh_msg_ctx *ctx,
2170 struct os_mbuf *buf)
2171 {
2172 struct bt_mesh_subnet *sub;
2173 u16_t idx;
2174 int err;
2175
2176 idx = net_buf_simple_pull_le16(buf);
2177 if (idx > 0xfff) {
2178 BT_ERR("Invalid NetKeyIndex 0x%04x", idx);
2179 return;
2180 }
2181
2182 BT_DBG("idx 0x%04x", idx);
2183
2184 sub = bt_mesh_subnet_get(idx);
2185 if (!sub) {
2186 int i;
2187
2188 for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
2189 if (bt_mesh.sub[i].net_idx == BT_MESH_KEY_UNUSED) {
2190 sub = &bt_mesh.sub[i];
2191 break;
2192 }
2193 }
2194
2195 if (!sub) {
2196 send_net_key_status(model, ctx, idx,
2197 STATUS_INSUFF_RESOURCES);
2198 return;
2199 }
2200 }
2201
2202 /* Check for already existing subnet */
2203 if (sub->net_idx == idx) {
2204 u8_t status;
2205
2206 if (memcmp(buf->om_data, sub->keys[0].net, 16)) {
2207 status = STATUS_IDX_ALREADY_STORED;
2208 } else {
2209 status = STATUS_SUCCESS;
2210 }
2211
2212 send_net_key_status(model, ctx, idx, status);
2213 return;
2214 }
2215
2216 err = bt_mesh_net_keys_create(&sub->keys[0], buf->om_data);
2217 if (err) {
2218 send_net_key_status(model, ctx, idx, STATUS_UNSPECIFIED);
2219 return;
2220 }
2221
2222 sub->net_idx = idx;
2223
2224 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
2225 BT_DBG("Storing NetKey persistently");
2226 bt_mesh_store_subnet(sub);
2227 }
2228
2229 /* Make sure we have valid beacon data to be sent */
2230 bt_mesh_net_beacon_update(sub);
2231
2232 if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY))) {
2233 sub->node_id = BT_MESH_NODE_IDENTITY_STOPPED;
2234 bt_mesh_proxy_beacon_send(sub);
2235 bt_mesh_adv_update();
2236 } else {
2237 sub->node_id = BT_MESH_NODE_IDENTITY_NOT_SUPPORTED;
2238 }
2239
2240 send_net_key_status(model, ctx, idx, STATUS_SUCCESS);
2241 }
2242
net_key_update(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2243 static void net_key_update(struct bt_mesh_model *model,
2244 struct bt_mesh_msg_ctx *ctx,
2245 struct os_mbuf *buf)
2246 {
2247 struct bt_mesh_subnet *sub;
2248 u16_t idx;
2249 int err;
2250
2251 idx = net_buf_simple_pull_le16(buf);
2252 if (idx > 0xfff) {
2253 BT_ERR("Invalid NetKeyIndex 0x%04x", idx);
2254 return;
2255 }
2256
2257 BT_DBG("idx 0x%04x", idx);
2258
2259 sub = bt_mesh_subnet_get(idx);
2260 if (!sub) {
2261 send_net_key_status(model, ctx, idx, STATUS_INVALID_NETKEY);
2262 return;
2263 }
2264
2265 /* The node shall successfully process a NetKey Update message on a
2266 * valid NetKeyIndex when the NetKey value is different and the Key
2267 * Refresh procedure has not been started, or when the NetKey value is
2268 * the same in Phase 1. The NetKey Update message shall generate an
2269 * error when the node is in Phase 2, or Phase 3.
2270 */
2271 switch (sub->kr_phase) {
2272 case BT_MESH_KR_NORMAL:
2273 if (!memcmp(buf->om_data, sub->keys[0].net, 16)) {
2274 return;
2275 }
2276 break;
2277 case BT_MESH_KR_PHASE_1:
2278 if (!memcmp(buf->om_data, sub->keys[1].net, 16)) {
2279 send_net_key_status(model, ctx, idx, STATUS_SUCCESS);
2280 return;
2281 }
2282 /* fall through */
2283 case BT_MESH_KR_PHASE_2:
2284 case BT_MESH_KR_PHASE_3:
2285 send_net_key_status(model, ctx, idx, STATUS_CANNOT_UPDATE);
2286 return;
2287 }
2288
2289 err = bt_mesh_net_keys_create(&sub->keys[1], buf->om_data);
2290 if (!err && ((MYNEWT_VAL(BLE_MESH_LOW_POWER)) ||
2291 (MYNEWT_VAL(BLE_MESH_FRIEND)))) {
2292 err = friend_cred_update(sub);
2293 }
2294
2295 if (err) {
2296 send_net_key_status(model, ctx, idx, STATUS_UNSPECIFIED);
2297 return;
2298 }
2299
2300 sub->kr_phase = BT_MESH_KR_PHASE_1;
2301
2302 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
2303 BT_DBG("Storing NetKey persistently");
2304 bt_mesh_store_subnet(sub);
2305 }
2306
2307 bt_mesh_net_beacon_update(sub);
2308
2309 send_net_key_status(model, ctx, idx, STATUS_SUCCESS);
2310 }
2311
hb_pub_disable(struct bt_mesh_cfg_srv * cfg)2312 static void hb_pub_disable(struct bt_mesh_cfg_srv *cfg)
2313 {
2314 BT_DBG("");
2315
2316 cfg->hb_pub.dst = BT_MESH_ADDR_UNASSIGNED;
2317 cfg->hb_pub.count = 0;
2318 cfg->hb_pub.ttl = 0;
2319 cfg->hb_pub.period = 0;
2320
2321 k_delayed_work_cancel(&cfg->hb_pub.timer);
2322 }
2323
net_key_del(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2324 static void net_key_del(struct bt_mesh_model *model,
2325 struct bt_mesh_msg_ctx *ctx,
2326 struct os_mbuf *buf)
2327 {
2328 struct bt_mesh_subnet *sub;
2329 u16_t del_idx;
2330 u8_t status;
2331
2332 del_idx = net_buf_simple_pull_le16(buf);
2333 if (del_idx > 0xfff) {
2334 BT_ERR("Invalid NetKeyIndex 0x%04x", del_idx);
2335 return;
2336 }
2337
2338 BT_DBG("idx 0x%04x", del_idx);
2339
2340 sub = bt_mesh_subnet_get(del_idx);
2341 if (!sub) {
2342 /* This could be a retry of a previous attempt that had its
2343 * response lost, so pretend that it was a success.
2344 */
2345 status = STATUS_SUCCESS;
2346 goto send_status;
2347 }
2348
2349 /* The key that the message was encrypted with cannot be removed.
2350 * The NetKey List must contain a minimum of one NetKey.
2351 */
2352 if (ctx->net_idx == del_idx) {
2353 status = STATUS_CANNOT_REMOVE;
2354 goto send_status;
2355 }
2356
2357 bt_mesh_subnet_del(sub, true);
2358 status = STATUS_SUCCESS;
2359
2360 send_status:
2361 send_net_key_status(model, ctx, del_idx, status);
2362 }
2363
net_key_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2364 static void net_key_get(struct bt_mesh_model *model,
2365 struct bt_mesh_msg_ctx *ctx,
2366 struct os_mbuf *buf)
2367 {
2368 struct os_mbuf *msg =
2369 NET_BUF_SIMPLE(2 + 4 +
2370 IDX_LEN(MYNEWT_VAL(BLE_MESH_SUBNET_COUNT)));
2371 u16_t prev, i;
2372
2373 bt_mesh_model_msg_init(msg, OP_NET_KEY_LIST);
2374
2375 prev = BT_MESH_KEY_UNUSED;
2376 for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
2377 struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
2378
2379 if (sub->net_idx == BT_MESH_KEY_UNUSED) {
2380 continue;
2381 }
2382
2383 if (prev == BT_MESH_KEY_UNUSED) {
2384 prev = sub->net_idx;
2385 continue;
2386 }
2387
2388 key_idx_pack(msg, prev, sub->net_idx);
2389 prev = BT_MESH_KEY_UNUSED;
2390 }
2391
2392 if (prev != BT_MESH_KEY_UNUSED) {
2393 net_buf_simple_add_le16(msg, prev);
2394 }
2395
2396 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
2397 BT_ERR("Unable to send NetKey List");
2398 }
2399
2400 os_mbuf_free_chain(msg);
2401 }
2402
node_identity_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2403 static void node_identity_get(struct bt_mesh_model *model,
2404 struct bt_mesh_msg_ctx *ctx,
2405 struct os_mbuf *buf)
2406 {
2407 /* Needed size: opcode (2 bytes) + msg + MIC */
2408 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 4 + 4);
2409 struct bt_mesh_subnet *sub;
2410 u8_t node_id;
2411 u16_t idx;
2412
2413 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
2414 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
2415 bt_hex(buf->om_data, buf->om_len));
2416
2417 idx = net_buf_simple_pull_le16(buf);
2418 if (idx > 0xfff) {
2419 BT_ERR("Invalid NetKeyIndex 0x%04x", idx);
2420 goto done;
2421 }
2422
2423 bt_mesh_model_msg_init(msg, OP_NODE_IDENTITY_STATUS);
2424
2425 sub = bt_mesh_subnet_get(idx);
2426 if (!sub) {
2427 net_buf_simple_add_u8(msg, STATUS_INVALID_NETKEY);
2428 node_id = 0x00;
2429 } else {
2430 net_buf_simple_add_u8(msg, STATUS_SUCCESS);
2431 node_id = sub->node_id;
2432 }
2433
2434 net_buf_simple_add_le16(msg, idx);
2435 net_buf_simple_add_u8(msg, node_id);
2436
2437 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
2438 BT_ERR("Unable to send Node Identity Status");
2439 }
2440
2441 done:
2442 os_mbuf_free_chain(msg);
2443 }
2444
node_identity_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2445 static void node_identity_set(struct bt_mesh_model *model,
2446 struct bt_mesh_msg_ctx *ctx,
2447 struct os_mbuf *buf)
2448 {
2449 /* Needed size: opcode (2 bytes) + msg + MIC */
2450 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 4 + 4);
2451 struct bt_mesh_subnet *sub;
2452 u8_t node_id;
2453 u16_t idx;
2454
2455 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
2456 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
2457 bt_hex(buf->om_data, buf->om_len));
2458
2459 idx = net_buf_simple_pull_le16(buf);
2460 if (idx > 0xfff) {
2461 BT_WARN("Invalid NetKeyIndex 0x%04x", idx);
2462 goto done;
2463 }
2464
2465 node_id = net_buf_simple_pull_u8(buf);
2466 if (node_id != 0x00 && node_id != 0x01) {
2467 BT_WARN("Invalid Node ID value 0x%02x", node_id);
2468 goto done;
2469 }
2470
2471 bt_mesh_model_msg_init(msg, OP_NODE_IDENTITY_STATUS);
2472
2473 sub = bt_mesh_subnet_get(idx);
2474 if (!sub) {
2475 net_buf_simple_add_u8(msg, STATUS_INVALID_NETKEY);
2476 net_buf_simple_add_le16(msg, idx);
2477 net_buf_simple_add_u8(msg, node_id);
2478 } else {
2479 net_buf_simple_add_u8(msg, STATUS_SUCCESS);
2480 net_buf_simple_add_le16(msg, idx);
2481
2482 /* Section 4.2.11.1: "When the GATT Proxy state is set to
2483 * 0x00, the Node Identity state for all subnets shall be set
2484 * to 0x00 and shall not be changed."
2485 */
2486 if (MYNEWT_VAL(BLE_MESH_GATT_PROXY) &&
2487 bt_mesh_gatt_proxy_get() == BT_MESH_GATT_PROXY_ENABLED) {
2488 if (node_id) {
2489 bt_mesh_proxy_identity_start(sub);
2490 } else {
2491 bt_mesh_proxy_identity_stop(sub);
2492 }
2493 bt_mesh_adv_update();
2494 }
2495
2496 net_buf_simple_add_u8(msg, sub->node_id);
2497 }
2498
2499 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
2500 BT_ERR("Unable to send Node Identity Status");
2501 }
2502
2503 done:
2504 os_mbuf_free_chain(msg);
2505
2506 }
2507
create_mod_app_status(struct os_mbuf * msg,struct bt_mesh_model * mod,bool vnd,u16_t elem_addr,u16_t app_idx,u8_t status,u8_t * mod_id)2508 static void create_mod_app_status(struct os_mbuf *msg,
2509 struct bt_mesh_model *mod, bool vnd,
2510 u16_t elem_addr, u16_t app_idx,
2511 u8_t status, u8_t *mod_id)
2512 {
2513 bt_mesh_model_msg_init(msg, OP_MOD_APP_STATUS);
2514
2515 net_buf_simple_add_u8(msg, status);
2516 net_buf_simple_add_le16(msg, elem_addr);
2517 net_buf_simple_add_le16(msg, app_idx);
2518
2519 if (vnd) {
2520 memcpy(net_buf_simple_add(msg, 4), mod_id, 4);
2521 } else {
2522 memcpy(net_buf_simple_add(msg, 2), mod_id, 2);
2523 }
2524 }
2525
mod_app_bind(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2526 static void mod_app_bind(struct bt_mesh_model *model,
2527 struct bt_mesh_msg_ctx *ctx,
2528 struct os_mbuf *buf)
2529 {
2530 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 9 + 4);
2531 u16_t elem_addr, key_app_idx;
2532 struct bt_mesh_model *mod;
2533 struct bt_mesh_elem *elem;
2534 u8_t *mod_id, status;
2535 bool vnd;
2536
2537 elem_addr = net_buf_simple_pull_le16(buf);
2538 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
2539 BT_WARN("Prohibited element address");
2540 goto done;
2541 }
2542
2543 key_app_idx = net_buf_simple_pull_le16(buf);
2544 mod_id = buf->om_data;
2545
2546 elem = bt_mesh_elem_find(elem_addr);
2547 if (!elem) {
2548 mod = NULL;
2549 vnd = (buf->om_len == 4);
2550 status = STATUS_INVALID_ADDRESS;
2551 goto send_status;
2552 }
2553
2554 mod = get_model(elem, buf, &vnd);
2555 if (!mod) {
2556 status = STATUS_INVALID_MODEL;
2557 goto send_status;
2558 }
2559
2560 /* Configuration Server only allows device key based access */
2561 if (model == mod) {
2562 BT_ERR("Client tried to bind AppKey to Configuration Model");
2563 status = STATUS_CANNOT_BIND;
2564 goto send_status;
2565 }
2566
2567 status = mod_bind(mod, key_app_idx);
2568
2569 if (IS_ENABLED(CONFIG_BT_TESTING) && status == STATUS_SUCCESS) {
2570 bt_test_mesh_model_bound(ctx->addr, mod, key_app_idx);
2571 }
2572
2573 send_status:
2574 BT_DBG("status 0x%02x", status);
2575 create_mod_app_status(msg, mod, vnd, elem_addr, key_app_idx, status,
2576 mod_id);
2577
2578 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
2579 BT_ERR("Unable to send Model App Bind Status response");
2580 }
2581
2582 done:
2583 os_mbuf_free_chain(msg);
2584
2585 }
2586
mod_app_unbind(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2587 static void mod_app_unbind(struct bt_mesh_model *model,
2588 struct bt_mesh_msg_ctx *ctx,
2589 struct os_mbuf *buf)
2590 {
2591 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 9 + 4);
2592 u16_t elem_addr, key_app_idx;
2593 struct bt_mesh_model *mod;
2594 struct bt_mesh_elem *elem;
2595 u8_t *mod_id, status;
2596 bool vnd;
2597
2598 elem_addr = net_buf_simple_pull_le16(buf);
2599 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
2600 BT_WARN("Prohibited element address");
2601 goto done;
2602 }
2603
2604 key_app_idx = net_buf_simple_pull_le16(buf);
2605 mod_id = buf->om_data;
2606
2607 elem = bt_mesh_elem_find(elem_addr);
2608 if (!elem) {
2609 mod = NULL;
2610 vnd = (buf->om_len == 4);
2611 status = STATUS_INVALID_ADDRESS;
2612 goto send_status;
2613 }
2614
2615 mod = get_model(elem, buf, &vnd);
2616 if (!mod) {
2617 status = STATUS_INVALID_MODEL;
2618 goto send_status;
2619 }
2620
2621 status = mod_unbind(mod, key_app_idx, true);
2622
2623 if (IS_ENABLED(CONFIG_BT_TESTING) && status == STATUS_SUCCESS) {
2624 bt_test_mesh_model_unbound(ctx->addr, mod, key_app_idx);
2625 }
2626
2627 send_status:
2628 BT_DBG("status 0x%02x", status);
2629 create_mod_app_status(msg, mod, vnd, elem_addr, key_app_idx, status,
2630 mod_id);
2631
2632 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
2633 BT_ERR("Unable to send Model App Unbind Status response");
2634 }
2635
2636 done:
2637 os_mbuf_free_chain(msg);
2638 }
2639
2640 #define KEY_LIST_LEN (MYNEWT_VAL(BLE_MESH_MODEL_KEY_COUNT) * 2)
2641
mod_app_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2642 static void mod_app_get(struct bt_mesh_model *model,
2643 struct bt_mesh_msg_ctx *ctx,
2644 struct os_mbuf *buf)
2645 {
2646 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 9 + KEY_LIST_LEN + 4);
2647 struct bt_mesh_model *mod;
2648 struct bt_mesh_elem *elem;
2649 u8_t *mod_id, status;
2650 u16_t elem_addr;
2651 bool vnd;
2652
2653 elem_addr = net_buf_simple_pull_le16(buf);
2654 if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
2655 BT_WARN("Prohibited element address");
2656 goto done;
2657 }
2658
2659 mod_id = buf->om_data;
2660
2661 BT_DBG("elem_addr 0x%04x", elem_addr);
2662
2663 elem = bt_mesh_elem_find(elem_addr);
2664 if (!elem) {
2665 mod = NULL;
2666 vnd = (buf->om_len == 4);
2667 status = STATUS_INVALID_ADDRESS;
2668 goto send_list;
2669 }
2670
2671 mod = get_model(elem, buf, &vnd);
2672 if (!mod) {
2673 status = STATUS_INVALID_MODEL;
2674 goto send_list;
2675 }
2676
2677 status = STATUS_SUCCESS;
2678
2679 send_list:
2680 if (vnd) {
2681 bt_mesh_model_msg_init(msg, OP_VND_MOD_APP_LIST);
2682 } else {
2683 bt_mesh_model_msg_init(msg, OP_SIG_MOD_APP_LIST);
2684 }
2685
2686 net_buf_simple_add_u8(msg, status);
2687 net_buf_simple_add_le16(msg, elem_addr);
2688
2689 if (vnd) {
2690 net_buf_simple_add_mem(msg, mod_id, 4);
2691 } else {
2692 net_buf_simple_add_mem(msg, mod_id, 2);
2693 }
2694
2695 if (mod) {
2696 int i;
2697
2698 for (i = 0; i < ARRAY_SIZE(mod->keys); i++) {
2699 if (mod->keys[i] != BT_MESH_KEY_UNUSED) {
2700 net_buf_simple_add_le16(msg, mod->keys[i]);
2701 }
2702 }
2703 }
2704
2705 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
2706 BT_ERR("Unable to send Model Application List message");
2707 }
2708
2709 done:
2710 os_mbuf_free_chain(msg);
2711 }
2712
node_reset(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2713 static void node_reset(struct bt_mesh_model *model,
2714 struct bt_mesh_msg_ctx *ctx,
2715 struct os_mbuf *buf)
2716 {
2717 /* Needed size: opcode (2 bytes) + msg + MIC */
2718 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 0 + 4);
2719
2720 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
2721 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
2722 bt_hex(buf->om_data, buf->om_len));
2723
2724
2725 bt_mesh_model_msg_init(msg, OP_NODE_RESET_STATUS);
2726
2727 /* Send the response first since we wont have any keys left to
2728 * send it later.
2729 */
2730 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
2731 BT_ERR("Unable to send Node Reset Status");
2732 }
2733
2734 bt_mesh_reset();
2735 os_mbuf_free_chain(msg);
2736 }
2737
send_friend_status(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx)2738 static void send_friend_status(struct bt_mesh_model *model,
2739 struct bt_mesh_msg_ctx *ctx)
2740 {
2741 /* Needed size: opcode (2 bytes) + msg + MIC */
2742 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4);
2743 struct bt_mesh_cfg_srv *cfg = model->user_data;
2744
2745 bt_mesh_model_msg_init(msg, OP_FRIEND_STATUS);
2746 net_buf_simple_add_u8(msg, cfg->frnd);
2747
2748 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
2749 BT_ERR("Unable to send Friend Status");
2750 }
2751 os_mbuf_free_chain(msg);
2752 }
2753
friend_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2754 static void friend_get(struct bt_mesh_model *model,
2755 struct bt_mesh_msg_ctx *ctx,
2756 struct os_mbuf *buf)
2757 {
2758 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
2759 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
2760 bt_hex(buf->om_data, buf->om_len));
2761
2762 send_friend_status(model, ctx);
2763 }
2764
friend_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2765 static void friend_set(struct bt_mesh_model *model,
2766 struct bt_mesh_msg_ctx *ctx,
2767 struct os_mbuf *buf)
2768 {
2769 struct bt_mesh_cfg_srv *cfg = model->user_data;
2770 struct bt_mesh_subnet *sub;
2771
2772 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
2773 ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
2774 bt_hex(buf->om_data, buf->om_len));
2775
2776 if (buf->om_data[0] != 0x00 && buf->om_data[0] != 0x01) {
2777 BT_WARN("Invalid Friend value 0x%02x", buf->om_data[0]);
2778 return;
2779 }
2780
2781 if (!cfg) {
2782 BT_WARN("No Configuration Server context available");
2783 goto send_status;
2784 }
2785
2786 BT_DBG("Friend 0x%02x -> 0x%02x", cfg->frnd, buf->om_data[0]);
2787
2788 if (cfg->frnd == buf->om_data[0]) {
2789 goto send_status;
2790 }
2791
2792 if (MYNEWT_VAL(BLE_MESH_FRIEND)) {
2793 cfg->frnd = buf->om_data[0];
2794
2795 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
2796 bt_mesh_store_cfg();
2797 }
2798
2799 if (cfg->frnd == BT_MESH_FRIEND_DISABLED) {
2800 bt_mesh_friend_clear_net_idx(BT_MESH_KEY_ANY);
2801 }
2802 }
2803
2804 sub = bt_mesh_subnet_get(cfg->hb_pub.net_idx);
2805 if ((cfg->hb_pub.feat & BT_MESH_FEAT_FRIEND) && sub) {
2806 hb_send(model);
2807 }
2808
2809 send_status:
2810 send_friend_status(model, ctx);
2811 }
2812
lpn_timeout_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2813 static void lpn_timeout_get(struct bt_mesh_model *model,
2814 struct bt_mesh_msg_ctx *ctx,
2815 struct os_mbuf *buf)
2816 {
2817 /* Needed size: opcode (2 bytes) + msg + MIC */
2818 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 5 + 4);
2819 struct bt_mesh_friend *frnd;
2820 u16_t lpn_addr;
2821 s32_t timeout;
2822
2823 lpn_addr = net_buf_simple_pull_le16(buf);
2824
2825 BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x lpn_addr 0x%02x",
2826 ctx->net_idx, ctx->app_idx, ctx->addr, lpn_addr);
2827
2828 /* check if it's the address of the Low Power Node? */
2829 if (!BT_MESH_ADDR_IS_UNICAST(lpn_addr)) {
2830 BT_WARN("Invalid LPNAddress; ignoring msg");
2831 goto done;
2832 }
2833
2834 bt_mesh_model_msg_init(msg, OP_LPN_TIMEOUT_STATUS);
2835 net_buf_simple_add_le16(msg, lpn_addr);
2836
2837 if (!IS_ENABLED(CONFIG_BT_MESH_FRIEND)) {
2838 timeout = 0;
2839 goto send_rsp;
2840 }
2841
2842 frnd = bt_mesh_friend_find(BT_MESH_KEY_ANY, lpn_addr, true, true);
2843 if (!frnd) {
2844 timeout = 0;
2845 goto send_rsp;
2846 }
2847
2848 timeout = k_delayed_work_remaining_get(&frnd->timer) / 100;
2849
2850 send_rsp:
2851 net_buf_simple_add_u8(msg, timeout);
2852 net_buf_simple_add_u8(msg, timeout >> 8);
2853 net_buf_simple_add_u8(msg, timeout >> 16);
2854
2855 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
2856 BT_ERR("Unable to send LPN PollTimeout Status");
2857 }
2858
2859 done:
2860 os_mbuf_free_chain(msg);
2861 }
2862
send_krp_status(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,u16_t idx,u8_t phase,u8_t status)2863 static void send_krp_status(struct bt_mesh_model *model,
2864 struct bt_mesh_msg_ctx *ctx,
2865 u16_t idx, u8_t phase, u8_t status)
2866 {
2867 /* Needed size: opcode (2 bytes) + msg + MIC */
2868 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 4 + 4);
2869
2870 bt_mesh_model_msg_init(msg, OP_KRP_STATUS);
2871
2872 net_buf_simple_add_u8(msg, status);
2873 net_buf_simple_add_le16(msg, idx);
2874 net_buf_simple_add_u8(msg, phase);
2875
2876 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
2877 BT_ERR("Unable to send Key Refresh State Status");
2878 }
2879
2880 os_mbuf_free_chain(msg);
2881 }
2882
krp_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2883 static void krp_get(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx,
2884 struct os_mbuf *buf)
2885 {
2886 struct bt_mesh_subnet *sub;
2887 u16_t idx;
2888
2889 idx = net_buf_simple_pull_le16(buf);
2890 if (idx > 0xfff) {
2891 BT_ERR("Invalid NetKeyIndex 0x%04x", idx);
2892 return;
2893 }
2894
2895 BT_DBG("idx 0x%04x", idx);
2896
2897 sub = bt_mesh_subnet_get(idx);
2898 if (!sub) {
2899 send_krp_status(model, ctx, idx, 0x00, STATUS_INVALID_NETKEY);
2900 } else {
2901 send_krp_status(model, ctx, idx, sub->kr_phase,
2902 STATUS_SUCCESS);
2903 }
2904 }
2905
krp_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)2906 static void krp_set(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx,
2907 struct os_mbuf *buf)
2908 {
2909 struct bt_mesh_subnet *sub;
2910 u8_t phase;
2911 u16_t idx;
2912
2913 idx = net_buf_simple_pull_le16(buf);
2914 phase = net_buf_simple_pull_u8(buf);
2915
2916 if (idx > 0xfff) {
2917 BT_ERR("Invalid NetKeyIndex 0x%04x", idx);
2918 return;
2919 }
2920
2921 BT_DBG("idx 0x%04x transition 0x%02x", idx, phase);
2922
2923 sub = bt_mesh_subnet_get(idx);
2924 if (!sub) {
2925 send_krp_status(model, ctx, idx, 0x00, STATUS_INVALID_NETKEY);
2926 return;
2927 }
2928
2929 BT_DBG("%u -> %u", sub->kr_phase, phase);
2930
2931 if (phase < BT_MESH_KR_PHASE_2 || phase > BT_MESH_KR_PHASE_3 ||
2932 (sub->kr_phase == BT_MESH_KR_NORMAL &&
2933 phase == BT_MESH_KR_PHASE_2)) {
2934 BT_WARN("Prohibited transition %u -> %u", sub->kr_phase, phase);
2935 return;
2936 }
2937
2938 if (sub->kr_phase == BT_MESH_KR_PHASE_1 &&
2939 phase == BT_MESH_KR_PHASE_2) {
2940 sub->kr_phase = BT_MESH_KR_PHASE_2;
2941 sub->kr_flag = 1;
2942 bt_mesh_net_beacon_update(sub);
2943 } else if ((sub->kr_phase == BT_MESH_KR_PHASE_1 ||
2944 sub->kr_phase == BT_MESH_KR_PHASE_2) &&
2945 phase == BT_MESH_KR_PHASE_3) {
2946 bt_mesh_net_revoke_keys(sub);
2947 if ((MYNEWT_VAL(BLE_MESH_LOW_POWER)) ||
2948 (MYNEWT_VAL(BLE_MESH_FRIEND))) {
2949 friend_cred_refresh(ctx->net_idx);
2950 }
2951 sub->kr_phase = BT_MESH_KR_NORMAL;
2952 sub->kr_flag = 0;
2953 bt_mesh_net_beacon_update(sub);
2954 }
2955
2956 send_krp_status(model, ctx, idx, sub->kr_phase, STATUS_SUCCESS);
2957 }
2958
hb_log(u16_t val)2959 static u8_t hb_log(u16_t val)
2960 {
2961 if (!val) {
2962 return 0x00;
2963 } else if (val == 0xffff) {
2964 return 0xff;
2965 } else {
2966 return 32 - __builtin_clz(val);
2967 }
2968 }
2969
hb_pub_count_log(u16_t val)2970 static u8_t hb_pub_count_log(u16_t val)
2971 {
2972 if (!val) {
2973 return 0x00;
2974 } else if (val == 0x01) {
2975 return 0x01;
2976 } else if (val == 0xffff) {
2977 return 0xff;
2978 } else {
2979 return 32 - __builtin_clz(val - 1) + 1;
2980 }
2981 }
2982
hb_pwr2(u8_t val,u8_t sub)2983 static u16_t hb_pwr2(u8_t val, u8_t sub)
2984 {
2985 if (!val) {
2986 return 0x0000;
2987 } else if (val == 0xff || val == 0x11) {
2988 return 0xffff;
2989 } else {
2990 return (1 << (val - sub));
2991 }
2992 }
2993
2994 struct hb_pub_param {
2995 u16_t dst;
2996 u8_t count_log;
2997 u8_t period_log;
2998 u8_t ttl;
2999 u16_t feat;
3000 u16_t net_idx;
3001 } __packed;
3002
hb_pub_send_status(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,u8_t status,struct hb_pub_param * orig_msg)3003 static void hb_pub_send_status(struct bt_mesh_model *model,
3004 struct bt_mesh_msg_ctx *ctx, u8_t status,
3005 struct hb_pub_param *orig_msg)
3006 {
3007 /* Needed size: opcode (1 byte) + msg + MIC */
3008 struct os_mbuf *msg = NET_BUF_SIMPLE(1 + 10 + 4);
3009 struct bt_mesh_cfg_srv *cfg = model->user_data;
3010
3011 BT_DBG("src 0x%04x status 0x%02x", ctx->addr, status);
3012
3013 bt_mesh_model_msg_init(msg, OP_HEARTBEAT_PUB_STATUS);
3014
3015 net_buf_simple_add_u8(msg, status);
3016
3017 if (orig_msg) {
3018 memcpy(net_buf_simple_add(msg, sizeof(*orig_msg)), orig_msg,
3019 sizeof(*orig_msg));
3020 goto send;
3021 }
3022
3023 net_buf_simple_add_le16(msg, cfg->hb_pub.dst);
3024 net_buf_simple_add_u8(msg, hb_pub_count_log(cfg->hb_pub.count));
3025 net_buf_simple_add_u8(msg, cfg->hb_pub.period);
3026 net_buf_simple_add_u8(msg, cfg->hb_pub.ttl);
3027 net_buf_simple_add_le16(msg, cfg->hb_pub.feat);
3028 net_buf_simple_add_le16(msg, cfg->hb_pub.net_idx);
3029
3030 send:
3031 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
3032 BT_ERR("Unable to send Heartbeat Publication Status");
3033 }
3034
3035 os_mbuf_free_chain(msg);
3036 }
3037
heartbeat_pub_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)3038 static void heartbeat_pub_get(struct bt_mesh_model *model,
3039 struct bt_mesh_msg_ctx *ctx,
3040 struct os_mbuf *buf)
3041 {
3042 BT_DBG("src 0x%04x", ctx->addr);
3043
3044 hb_pub_send_status(model, ctx, STATUS_SUCCESS, NULL);
3045 }
3046
heartbeat_pub_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)3047 static void heartbeat_pub_set(struct bt_mesh_model *model,
3048 struct bt_mesh_msg_ctx *ctx,
3049 struct os_mbuf *buf)
3050 {
3051 struct hb_pub_param *param = (void *)buf->om_data;
3052 struct bt_mesh_cfg_srv *cfg = model->user_data;
3053 u16_t dst, feat, idx;
3054 u8_t status;
3055
3056 BT_DBG("src 0x%04x", ctx->addr);
3057
3058 dst = sys_le16_to_cpu(param->dst);
3059 /* All other address types but virtual are valid */
3060 if (BT_MESH_ADDR_IS_VIRTUAL(dst)) {
3061 status = STATUS_INVALID_ADDRESS;
3062 goto failed;
3063 }
3064
3065 if (param->count_log > 0x11 && param->count_log != 0xff) {
3066 status = STATUS_CANNOT_SET;
3067 goto failed;
3068 }
3069
3070 if (param->period_log > 0x10) {
3071 status = STATUS_CANNOT_SET;
3072 goto failed;
3073 }
3074
3075 if (param->ttl > BT_MESH_TTL_MAX && param->ttl != BT_MESH_TTL_DEFAULT) {
3076 BT_ERR("Invalid TTL value 0x%02x", param->ttl);
3077 return;
3078 }
3079
3080 feat = sys_le16_to_cpu(param->feat);
3081
3082 idx = sys_le16_to_cpu(param->net_idx);
3083 if (idx > 0xfff) {
3084 BT_ERR("Invalid NetKeyIndex 0x%04x", idx);
3085 return;
3086 }
3087
3088 if (!bt_mesh_subnet_get(idx)) {
3089 status = STATUS_INVALID_NETKEY;
3090 goto failed;
3091 }
3092
3093 cfg->hb_pub.dst = dst;
3094 cfg->hb_pub.period = param->period_log;
3095 cfg->hb_pub.feat = feat & BT_MESH_FEAT_SUPPORTED;
3096 cfg->hb_pub.net_idx = idx;
3097
3098 if (dst == BT_MESH_ADDR_UNASSIGNED) {
3099 hb_pub_disable(cfg);
3100 } else {
3101 /* 2^(n-1) */
3102 cfg->hb_pub.count = hb_pwr2(param->count_log, 1);
3103 cfg->hb_pub.ttl = param->ttl;
3104
3105 BT_DBG("period %u ms", hb_pwr2(param->period_log, 1) * 1000);
3106
3107 /* The first Heartbeat message shall be published as soon
3108 * as possible after the Heartbeat Publication Period state
3109 * has been configured for periodic publishing.
3110 */
3111 if (param->period_log && param->count_log) {
3112 k_work_submit(&cfg->hb_pub.timer.work);
3113 } else {
3114 k_delayed_work_cancel(&cfg->hb_pub.timer);
3115 }
3116 }
3117
3118 if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
3119 bt_mesh_store_hb_pub();
3120 }
3121
3122 hb_pub_send_status(model, ctx, STATUS_SUCCESS, NULL);
3123
3124 return;
3125
3126 failed:
3127 hb_pub_send_status(model, ctx, status, param);
3128 }
3129
hb_sub_send_status(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,u8_t status)3130 static void hb_sub_send_status(struct bt_mesh_model *model,
3131 struct bt_mesh_msg_ctx *ctx, u8_t status)
3132 {
3133 /* Needed size: opcode (2 bytes) + msg + MIC */
3134 struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 9 + 4);
3135 struct bt_mesh_cfg_srv *cfg = model->user_data;
3136 u16_t period;
3137 s64_t uptime;
3138
3139 BT_DBG("src 0x%04x status 0x%02x", ctx->addr, status);
3140
3141 uptime = k_uptime_get();
3142 if (uptime > cfg->hb_sub.expiry) {
3143 period = 0;
3144 } else {
3145 period = (cfg->hb_sub.expiry - uptime) / 1000;
3146 }
3147
3148 bt_mesh_model_msg_init(msg, OP_HEARTBEAT_SUB_STATUS);
3149
3150 net_buf_simple_add_u8(msg, status);
3151
3152 net_buf_simple_add_le16(msg, cfg->hb_sub.src);
3153 net_buf_simple_add_le16(msg, cfg->hb_sub.dst);
3154
3155 net_buf_simple_add_u8(msg, hb_log(period));
3156 net_buf_simple_add_u8(msg, hb_log(cfg->hb_sub.count));
3157 net_buf_simple_add_u8(msg, cfg->hb_sub.min_hops);
3158 net_buf_simple_add_u8(msg, cfg->hb_sub.max_hops);
3159
3160
3161 if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
3162 BT_ERR("Unable to send Heartbeat Subscription Status");
3163 }
3164
3165 os_mbuf_free_chain(msg);
3166 }
3167
heartbeat_sub_get(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)3168 static void heartbeat_sub_get(struct bt_mesh_model *model,
3169 struct bt_mesh_msg_ctx *ctx,
3170 struct os_mbuf *buf)
3171 {
3172 BT_DBG("src 0x%04x", ctx->addr);
3173
3174 hb_sub_send_status(model, ctx, STATUS_SUCCESS);
3175 }
3176
heartbeat_sub_set(struct bt_mesh_model * model,struct bt_mesh_msg_ctx * ctx,struct os_mbuf * buf)3177 static void heartbeat_sub_set(struct bt_mesh_model *model,
3178 struct bt_mesh_msg_ctx *ctx,
3179 struct os_mbuf *buf)
3180 {
3181 struct bt_mesh_cfg_srv *cfg = model->user_data;
3182 u16_t sub_src, sub_dst;
3183 u8_t sub_period;
3184 s32_t period_ms;
3185
3186 BT_DBG("src 0x%04x", ctx->addr);
3187
3188 sub_src = net_buf_simple_pull_le16(buf);
3189 sub_dst = net_buf_simple_pull_le16(buf);
3190 sub_period = net_buf_simple_pull_u8(buf);
3191
3192 BT_DBG("sub_src 0x%04x sub_dst 0x%04x period 0x%02x",
3193 sub_src, sub_dst, sub_period);
3194
3195 if (sub_src != BT_MESH_ADDR_UNASSIGNED &&
3196 !BT_MESH_ADDR_IS_UNICAST(sub_src)) {
3197 BT_WARN("Prohibited source address");
3198 return;
3199 }
3200
3201 if (BT_MESH_ADDR_IS_VIRTUAL(sub_dst) || BT_MESH_ADDR_IS_RFU(sub_dst) ||
3202 (BT_MESH_ADDR_IS_UNICAST(sub_dst) &&
3203 sub_dst != bt_mesh_primary_addr())) {
3204 BT_WARN("Prohibited destination address");
3205 return;
3206 }
3207
3208 if (sub_period > 0x11) {
3209 BT_WARN("Prohibited subscription period 0x%02x", sub_period);
3210 return;
3211 }
3212
3213 if (sub_src == BT_MESH_ADDR_UNASSIGNED ||
3214 sub_dst == BT_MESH_ADDR_UNASSIGNED ||
3215 sub_period == 0x00) {
3216 /* Only an explicit address change to unassigned should
3217 * trigger clearing of the values according to
3218 * MESH/NODE/CFG/HBS/BV-02-C.
3219 */
3220 if (sub_src == BT_MESH_ADDR_UNASSIGNED ||
3221 sub_dst == BT_MESH_ADDR_UNASSIGNED) {
3222 cfg->hb_sub.src = BT_MESH_ADDR_UNASSIGNED;
3223 cfg->hb_sub.dst = BT_MESH_ADDR_UNASSIGNED;
3224 cfg->hb_sub.min_hops = BT_MESH_TTL_MAX;
3225 cfg->hb_sub.max_hops = 0;
3226 cfg->hb_sub.count = 0;
3227 }
3228
3229 period_ms = 0;
3230 } else {
3231 cfg->hb_sub.src = sub_src;
3232 cfg->hb_sub.dst = sub_dst;
3233 cfg->hb_sub.min_hops = BT_MESH_TTL_MAX;
3234 cfg->hb_sub.max_hops = 0;
3235 cfg->hb_sub.count = 0;
3236 period_ms = hb_pwr2(sub_period, 1) * 1000;
3237 }
3238
3239 /* Let the transport layer know it needs to handle this address */
3240 bt_mesh_set_hb_sub_dst(cfg->hb_sub.dst);
3241
3242 BT_DBG("period_ms %u", period_ms);
3243
3244 if (period_ms) {
3245 cfg->hb_sub.expiry = k_uptime_get() + period_ms;
3246 } else {
3247 cfg->hb_sub.expiry = 0;
3248 }
3249
3250 hb_sub_send_status(model, ctx, STATUS_SUCCESS);
3251
3252 /* MESH/NODE/CFG/HBS/BV-01-C expects the MinHops to be 0x7f after
3253 * disabling subscription, but 0x00 for subsequent Get requests.
3254 */
3255 if (!period_ms) {
3256 cfg->hb_sub.min_hops = 0;
3257 }
3258 }
3259
3260 const struct bt_mesh_model_op bt_mesh_cfg_srv_op[] = {
3261 { OP_DEV_COMP_DATA_GET, 1, dev_comp_data_get },
3262 { OP_APP_KEY_ADD, 19, app_key_add },
3263 { OP_APP_KEY_UPDATE, 19, app_key_update },
3264 { OP_APP_KEY_DEL, 3, app_key_del },
3265 { OP_APP_KEY_GET, 2, app_key_get },
3266 { OP_BEACON_GET, 0, beacon_get },
3267 { OP_BEACON_SET, 1, beacon_set },
3268 { OP_DEFAULT_TTL_GET, 0, default_ttl_get },
3269 { OP_DEFAULT_TTL_SET, 1, default_ttl_set },
3270 { OP_GATT_PROXY_GET, 0, gatt_proxy_get },
3271 { OP_GATT_PROXY_SET, 1, gatt_proxy_set },
3272 { OP_NET_TRANSMIT_GET, 0, net_transmit_get },
3273 { OP_NET_TRANSMIT_SET, 1, net_transmit_set },
3274 { OP_RELAY_GET, 0, relay_get },
3275 { OP_RELAY_SET, 2, relay_set },
3276 { OP_MOD_PUB_GET, 4, mod_pub_get },
3277 { OP_MOD_PUB_SET, 11, mod_pub_set },
3278 { OP_MOD_PUB_VA_SET, 24, mod_pub_va_set },
3279 { OP_MOD_SUB_ADD, 6, mod_sub_add },
3280 { OP_MOD_SUB_VA_ADD, 20, mod_sub_va_add },
3281 { OP_MOD_SUB_DEL, 6, mod_sub_del },
3282 { OP_MOD_SUB_VA_DEL, 20, mod_sub_va_del },
3283 { OP_MOD_SUB_OVERWRITE, 6, mod_sub_overwrite },
3284 { OP_MOD_SUB_VA_OVERWRITE, 20, mod_sub_va_overwrite },
3285 { OP_MOD_SUB_DEL_ALL, 4, mod_sub_del_all },
3286 { OP_MOD_SUB_GET, 4, mod_sub_get },
3287 { OP_MOD_SUB_GET_VND, 6, mod_sub_get_vnd },
3288 { OP_NET_KEY_ADD, 18, net_key_add },
3289 { OP_NET_KEY_UPDATE, 18, net_key_update },
3290 { OP_NET_KEY_DEL, 2, net_key_del },
3291 { OP_NET_KEY_GET, 0, net_key_get },
3292 { OP_NODE_IDENTITY_GET, 2, node_identity_get },
3293 { OP_NODE_IDENTITY_SET, 3, node_identity_set },
3294 { OP_MOD_APP_BIND, 6, mod_app_bind },
3295 { OP_MOD_APP_UNBIND, 6, mod_app_unbind },
3296 { OP_SIG_MOD_APP_GET, 4, mod_app_get },
3297 { OP_VND_MOD_APP_GET, 6, mod_app_get },
3298 { OP_NODE_RESET, 0, node_reset },
3299 { OP_FRIEND_GET, 0, friend_get },
3300 { OP_FRIEND_SET, 1, friend_set },
3301 { OP_LPN_TIMEOUT_GET, 2, lpn_timeout_get },
3302 { OP_KRP_GET, 2, krp_get },
3303 { OP_KRP_SET, 3, krp_set },
3304 { OP_HEARTBEAT_PUB_GET, 0, heartbeat_pub_get },
3305 { OP_HEARTBEAT_PUB_SET, 9, heartbeat_pub_set },
3306 { OP_HEARTBEAT_SUB_GET, 0, heartbeat_sub_get },
3307 { OP_HEARTBEAT_SUB_SET, 5, heartbeat_sub_set },
3308 BT_MESH_MODEL_OP_END,
3309 };
3310
hb_publish(struct ble_npl_event * work)3311 static void hb_publish(struct ble_npl_event *work)
3312 {
3313 struct bt_mesh_cfg_srv *cfg = ble_npl_event_get_arg(work);
3314 struct bt_mesh_model *model = cfg->model;
3315 struct bt_mesh_subnet *sub;
3316 u16_t period_ms;
3317
3318 BT_DBG("hb_pub.count: %u", cfg->hb_pub.count);
3319
3320 sub = bt_mesh_subnet_get(cfg->hb_pub.net_idx);
3321 if (!sub) {
3322 BT_ERR("No matching subnet for idx 0x%02x",
3323 cfg->hb_pub.net_idx);
3324 cfg->hb_pub.dst = BT_MESH_ADDR_UNASSIGNED;
3325 return;
3326 }
3327
3328 if (cfg->hb_pub.count == 0) {
3329 return;
3330 }
3331
3332 period_ms = hb_pwr2(cfg->hb_pub.period, 1) * 1000;
3333 if (period_ms && cfg->hb_pub.count > 1) {
3334 k_delayed_work_submit(&cfg->hb_pub.timer, period_ms);
3335 }
3336
3337 hb_send(model);
3338
3339 if (cfg->hb_pub.count != 0xffff) {
3340 cfg->hb_pub.count--;
3341 }
3342 }
3343
conf_is_valid(struct bt_mesh_cfg_srv * cfg)3344 static bool conf_is_valid(struct bt_mesh_cfg_srv *cfg)
3345 {
3346 if (cfg->relay > 0x02) {
3347 return false;
3348 }
3349
3350 if (cfg->beacon > 0x01) {
3351 return false;
3352 }
3353
3354 if (cfg->default_ttl > BT_MESH_TTL_MAX) {
3355 return false;
3356 }
3357
3358 return true;
3359 }
3360
bt_mesh_cfg_srv_init(struct bt_mesh_model * model,bool primary)3361 int bt_mesh_cfg_srv_init(struct bt_mesh_model *model, bool primary)
3362 {
3363 struct bt_mesh_cfg_srv *cfg = model->user_data;
3364
3365 if (!cfg) {
3366 BT_ERR("No Configuration Server context provided");
3367 return -EINVAL;
3368 }
3369
3370 if (!conf_is_valid(cfg)) {
3371 BT_ERR("Invalid values in configuration");
3372 return -EINVAL;
3373 }
3374
3375 /* Configuration Model security is device-key based */
3376 model->keys[0] = BT_MESH_KEY_DEV;
3377
3378 if (!(MYNEWT_VAL(BLE_MESH_RELAY))) {
3379 cfg->relay = BT_MESH_RELAY_NOT_SUPPORTED;
3380 }
3381
3382 if (!(MYNEWT_VAL(BLE_MESH_FRIEND))) {
3383 cfg->frnd = BT_MESH_FRIEND_NOT_SUPPORTED;
3384 }
3385
3386 if (!(MYNEWT_VAL(BLE_MESH_GATT_PROXY))) {
3387 cfg->gatt_proxy = BT_MESH_GATT_PROXY_NOT_SUPPORTED;
3388 }
3389
3390 k_delayed_work_init(&cfg->hb_pub.timer, hb_publish);
3391 k_delayed_work_add_arg(&cfg->hb_pub.timer, cfg);
3392 cfg->hb_pub.net_idx = BT_MESH_KEY_UNUSED;
3393 cfg->hb_sub.expiry = 0;
3394
3395 cfg->model = model;
3396
3397 conf = cfg;
3398
3399 return 0;
3400 }
3401
mod_reset(struct bt_mesh_model * mod,struct bt_mesh_elem * elem,bool vnd,bool primary,void * user_data)3402 static void mod_reset(struct bt_mesh_model *mod, struct bt_mesh_elem *elem,
3403 bool vnd, bool primary, void *user_data)
3404 {
3405 /* Clear model state that isn't otherwise cleared. E.g. AppKey
3406 * binding and model publication is cleared as a consequence
3407 * of removing all app keys, however model subscription clearing
3408 * must be taken care of here.
3409 */
3410
3411 mod_sub_list_clear(mod);
3412
3413 if (IS_ENABLED(BT_SETTINGS)) {
3414 bt_mesh_store_mod_sub(mod);
3415 }
3416 }
3417
bt_mesh_cfg_reset(void)3418 void bt_mesh_cfg_reset(void)
3419 {
3420 struct bt_mesh_cfg_srv *cfg = conf;
3421 int i;
3422
3423 if (!cfg) {
3424 return;
3425 }
3426
3427 bt_mesh_set_hb_sub_dst(BT_MESH_ADDR_UNASSIGNED);
3428
3429 cfg->hb_sub.src = BT_MESH_ADDR_UNASSIGNED;
3430 cfg->hb_sub.dst = BT_MESH_ADDR_UNASSIGNED;
3431 cfg->hb_sub.expiry = 0;
3432
3433 /* Delete all net keys, which also takes care of all app keys which
3434 * are associated with each net key.
3435 */
3436 for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
3437 struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
3438
3439 if (sub->net_idx != BT_MESH_KEY_UNUSED) {
3440 bt_mesh_subnet_del(sub, true);
3441 }
3442 }
3443
3444 bt_mesh_model_foreach(mod_reset, NULL);
3445
3446 memset(labels, 0, sizeof(labels));
3447 }
3448
bt_mesh_heartbeat(u16_t src,u16_t dst,u8_t hops,u16_t feat)3449 void bt_mesh_heartbeat(u16_t src, u16_t dst, u8_t hops, u16_t feat)
3450 {
3451 struct bt_mesh_cfg_srv *cfg = conf;
3452
3453 if (!cfg) {
3454 BT_WARN("No configuaration server context available");
3455 return;
3456 }
3457
3458 if (src != cfg->hb_sub.src || dst != cfg->hb_sub.dst) {
3459 BT_WARN("No subscription for received heartbeat");
3460 return;
3461 }
3462
3463 if (k_uptime_get() > cfg->hb_sub.expiry) {
3464 BT_WARN("Heartbeat subscription period expired");
3465 return;
3466 }
3467
3468 cfg->hb_sub.min_hops = min(cfg->hb_sub.min_hops, hops);
3469 cfg->hb_sub.max_hops = max(cfg->hb_sub.max_hops, hops);
3470
3471 if (cfg->hb_sub.count < 0xffff) {
3472 cfg->hb_sub.count++;
3473 }
3474
3475 BT_DBG("src 0x%04x dst 0x%04x hops %u min %u max %u count %u", src,
3476 dst, hops, cfg->hb_sub.min_hops, cfg->hb_sub.max_hops,
3477 cfg->hb_sub.count);
3478
3479 if (cfg->hb_sub.func) {
3480 cfg->hb_sub.func(hops, feat);
3481 }
3482 }
3483
bt_mesh_net_transmit_get(void)3484 u8_t bt_mesh_net_transmit_get(void)
3485 {
3486 if (conf) {
3487 return conf->net_transmit;
3488 }
3489
3490 return 0;
3491 }
3492
bt_mesh_relay_get(void)3493 u8_t bt_mesh_relay_get(void)
3494 {
3495 if (conf) {
3496 return conf->relay;
3497 }
3498
3499 return BT_MESH_RELAY_NOT_SUPPORTED;
3500 }
3501
bt_mesh_friend_get(void)3502 u8_t bt_mesh_friend_get(void)
3503 {
3504 BT_DBG("conf %p conf->frnd 0x%02x", conf, conf->frnd);
3505
3506 if (conf) {
3507 return conf->frnd;
3508 }
3509
3510 return BT_MESH_FRIEND_NOT_SUPPORTED;
3511 }
3512
bt_mesh_relay_retransmit_get(void)3513 u8_t bt_mesh_relay_retransmit_get(void)
3514 {
3515 if (conf) {
3516 return conf->relay_retransmit;
3517 }
3518
3519 return 0;
3520 }
3521
bt_mesh_beacon_get(void)3522 u8_t bt_mesh_beacon_get(void)
3523 {
3524 if (conf) {
3525 return conf->beacon;
3526 }
3527
3528 return BT_MESH_BEACON_DISABLED;
3529 }
3530
bt_mesh_gatt_proxy_get(void)3531 u8_t bt_mesh_gatt_proxy_get(void)
3532 {
3533 if (conf) {
3534 return conf->gatt_proxy;
3535 }
3536
3537 return BT_MESH_GATT_PROXY_NOT_SUPPORTED;
3538 }
3539
bt_mesh_default_ttl_get(void)3540 u8_t bt_mesh_default_ttl_get(void)
3541 {
3542 if (conf) {
3543 return conf->default_ttl;
3544 }
3545
3546 return DEFAULT_TTL;
3547 }
3548
bt_mesh_label_uuid_get(u16_t addr)3549 u8_t *bt_mesh_label_uuid_get(u16_t addr)
3550 {
3551 int i;
3552
3553 BT_DBG("addr 0x%04x", addr);
3554
3555 for (i = 0; i < ARRAY_SIZE(labels); i++) {
3556 if (labels[i].addr == addr) {
3557 BT_DBG("Found Label UUID for 0x%04x: %s", addr,
3558 bt_hex(labels[i].uuid, 16));
3559 return labels[i].uuid;
3560 }
3561 }
3562
3563 BT_WARN("No matching Label UUID for 0x%04x", addr);
3564
3565 return NULL;
3566 }
3567
bt_mesh_hb_pub_get(void)3568 struct bt_mesh_hb_pub *bt_mesh_hb_pub_get(void)
3569 {
3570 if (!conf) {
3571 return NULL;
3572 }
3573
3574 return &conf->hb_pub;
3575 }
3576
bt_mesh_cfg_get(void)3577 struct bt_mesh_cfg_srv *bt_mesh_cfg_get(void)
3578 {
3579 return conf;
3580 }
3581
bt_mesh_subnet_del(struct bt_mesh_subnet * sub,bool store)3582 void bt_mesh_subnet_del(struct bt_mesh_subnet *sub, bool store)
3583 {
3584 int i;
3585
3586 BT_DBG("NetIdx 0x%03x store %u", sub->net_idx, store);
3587
3588 if (conf && conf->hb_pub.net_idx == sub->net_idx) {
3589 hb_pub_disable(conf);
3590
3591 if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
3592 bt_mesh_store_hb_pub();
3593 }
3594 }
3595
3596 /* Delete any app keys bound to this NetKey index */
3597 for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) {
3598 struct bt_mesh_app_key *key = &bt_mesh.app_keys[i];
3599
3600 if (key->net_idx == sub->net_idx) {
3601 bt_mesh_app_key_del(key, store);
3602 }
3603 }
3604
3605 if (IS_ENABLED(CONFIG_BT_MESH_FRIEND)) {
3606 bt_mesh_friend_clear_net_idx(sub->net_idx);
3607 }
3608
3609 if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
3610 bt_mesh_clear_subnet(sub);
3611 }
3612
3613 memset(sub, 0, sizeof(*sub));
3614 sub->net_idx = BT_MESH_KEY_UNUSED;
3615 }
3616