1 /*
2 * Copyright (c) 2022 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/bluetooth/mesh.h>
8
9 #include "foundation.h"
10 #include "op_agg.h"
11
12 #define LOG_LEVEL CONFIG_BT_MESH_MODEL_LOG_LEVEL
13 #include <zephyr/logging/log.h>
14 LOG_MODULE_REGISTER(bt_mesh_op_agg);
15
16 #define IS_LENGTH_LONG(buf) ((buf)->data[0] & 1)
17 #define LENGTH_SHORT_MAX BIT_MASK(7)
18
bt_mesh_op_agg_encode_msg(struct net_buf_simple * msg,struct net_buf_simple * buf)19 int bt_mesh_op_agg_encode_msg(struct net_buf_simple *msg, struct net_buf_simple *buf)
20 {
21 if (msg->len > LENGTH_SHORT_MAX) {
22 if (net_buf_simple_tailroom(buf) < (msg->len + 2)) {
23 return -ENOMEM;
24 }
25
26 net_buf_simple_add_le16(buf, (msg->len << 1) | 1);
27 } else {
28 if (net_buf_simple_tailroom(buf) < (msg->len + 1)) {
29 return -ENOMEM;
30 }
31
32 net_buf_simple_add_u8(buf, msg->len << 1);
33 }
34 net_buf_simple_add_mem(buf, msg->data, msg->len);
35
36 return 0;
37 }
38
bt_mesh_op_agg_decode_msg(struct net_buf_simple * msg,struct net_buf_simple * buf)39 int bt_mesh_op_agg_decode_msg(struct net_buf_simple *msg,
40 struct net_buf_simple *buf)
41 {
42 uint16_t len;
43
44 if (IS_LENGTH_LONG(buf)) {
45 if (buf->len < 2) {
46 return -EINVAL;
47 }
48
49 len = net_buf_simple_pull_le16(buf) >> 1;
50 } else {
51 if (buf->len < 1) {
52 return -EINVAL;
53 }
54
55 len = net_buf_simple_pull_u8(buf) >> 1;
56 }
57
58 if (buf->len < len) {
59 return -EINVAL;
60 }
61
62 net_buf_simple_init_with_data(msg, net_buf_simple_pull_mem(buf, len), len);
63
64 return 0;
65 }
66
bt_mesh_op_agg_is_op_agg_msg(struct net_buf_simple * buf)67 bool bt_mesh_op_agg_is_op_agg_msg(struct net_buf_simple *buf)
68 {
69 if (buf->len >= 2 && (buf->data[0] >> 6) == 2) {
70 uint16_t opcode;
71 struct net_buf_simple_state state;
72
73 net_buf_simple_save(buf, &state);
74 opcode = net_buf_simple_pull_be16(buf);
75 net_buf_simple_restore(buf, &state);
76
77 if ((opcode == OP_OPCODES_AGGREGATOR_STATUS) ||
78 (opcode == OP_OPCODES_AGGREGATOR_SEQUENCE)) {
79 return true;
80 }
81 }
82 return false;
83 }
84
