1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
|
// SPDX-License-Identifier: LGPL-2.1-or-later
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2017 Intel Corporation. All rights reserved.
*
*
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <stdio.h>
#include <stdbool.h>
#include <inttypes.h>
#include <glib.h>
#include "src/shared/shell.h"
#include "src/shared/util.h"
#include "tools/mesh-gatt/mesh-net.h"
#include "tools/mesh-gatt/node.h"
#include "tools/mesh-gatt/util.h"
void set_menu_prompt(const char *name, const char *id)
{
char *prompt;
prompt = g_strdup_printf("[%s%s%s]> ", name,
id ? ": Target = " : "", id ? id : "");
bt_shell_set_prompt(prompt, COLOR_BLUE);
g_free(prompt);
}
void print_byte_array(const char *prefix, const void *ptr, int len)
{
const uint8_t *data = ptr;
char *line, *bytes;
int i;
if (prefix) {
line = g_malloc(strlen(prefix) + (16 * 3) + 2);
sprintf(line, "%s ", prefix);
bytes = line + strlen(prefix) + 1;
} else {
line = g_malloc((16 * 3) + 2);
bytes = line + 1;
}
for (i = 0; i < len; ++i) {
sprintf(bytes, "%2.2x ", data[i]);
if ((i + 1) % 16) {
bytes += 3;
} else {
bt_shell_printf("\r%s\n", line);
bytes = line + strlen(prefix) + 1;
}
}
if (i % 16)
bt_shell_printf("\r%s\n", line);
g_free(line);
}
bool str2hex(const char *str, uint16_t in_len, uint8_t *out,
uint16_t out_len)
{
uint16_t i;
if (in_len < out_len * 2)
return false;
for (i = 0; i < out_len; i++) {
if (sscanf(&str[i * 2], "%02hhx", &out[i]) != 1)
return false;
}
return true;
}
size_t hex2str(uint8_t *in, size_t in_len, char *out,
size_t out_len)
{
static const char hexdigits[] = "0123456789abcdef";
size_t i;
if(in_len * 2 > out_len - 1)
return 0;
for (i = 0; i < in_len; i++) {
out[i * 2] = hexdigits[in[i] >> 4];
out[i * 2 + 1] = hexdigits[in[i] & 0xf];
}
out[in_len * 2] = '\0';
return i;
}
uint16_t mesh_opcode_set(uint32_t opcode, uint8_t *buf)
{
if (opcode <= 0x7e) {
buf[0] = opcode;
return 1;
} else if (opcode >= 0x8000 && opcode <= 0xbfff) {
put_be16(opcode, buf);
return 2;
} else if (opcode >= 0xc00000 && opcode <= 0xffffff) {
buf[0] = (opcode >> 16) & 0xff;
put_be16(opcode, buf + 1);
return 3;
} else {
bt_shell_printf("Illegal Opcode %x", opcode);
return 0;
}
}
bool mesh_opcode_get(const uint8_t *buf, uint16_t sz, uint32_t *opcode, int *n)
{
if (!n || !opcode || sz < 1) return false;
switch (buf[0] & 0xc0) {
case 0x00:
case 0x40:
/* RFU */
if (buf[0] == 0x7f)
return false;
*n = 1;
*opcode = buf[0];
break;
case 0x80:
if (sz < 2)
return false;
*n = 2;
*opcode = get_be16(buf);
break;
case 0xc0:
if (sz < 3)
return false;
*n = 3;
*opcode = get_be16(buf + 1);
*opcode |= buf[0] << 16;
break;
default:
bt_shell_printf("Bad Packet:\n");
print_byte_array("\t", (void *) buf, sz);
return false;
}
return true;
}
const char *mesh_status_str(uint8_t status)
{
switch (status) {
case MESH_STATUS_SUCCESS: return "Success";
case MESH_STATUS_INVALID_ADDRESS: return "Invalid Address";
case MESH_STATUS_INVALID_MODEL: return "Invalid Model";
case MESH_STATUS_INVALID_APPKEY: return "Invalid AppKey";
case MESH_STATUS_INVALID_NETKEY: return "Invalid NetKey";
case MESH_STATUS_INSUFF_RESOURCES: return "Insufficient Resources";
case MESH_STATUS_IDX_ALREADY_STORED: return "Key Idx Already Stored";
case MESH_STATUS_INVALID_PUB_PARAM: return "Invalid Publish Parameters";
case MESH_STATUS_NOT_SUB_MOD: return "Not a Subscribe Model";
case MESH_STATUS_STORAGE_FAIL: return "Storage Failure";
case MESH_STATUS_FEAT_NOT_SUP: return "Feature Not Supported";
case MESH_STATUS_CANNOT_UPDATE: return "Cannot Update";
case MESH_STATUS_CANNOT_REMOVE: return "Cannot Remove";
case MESH_STATUS_CANNOT_BIND: return "Cannot bind";
case MESH_STATUS_UNABLE_CHANGE_STATE: return "Unable to change state";
case MESH_STATUS_CANNOT_SET: return "Cannot set";
case MESH_STATUS_UNSPECIFIED_ERROR: return "Unspecified error";
case MESH_STATUS_INVALID_BINDING: return "Invalid Binding";
default: return "Unknown";
}
}
void print_model_pub(uint16_t ele_addr, uint32_t mod_id,
struct mesh_publication *pub)
{
bt_shell_printf("\tElement: %4.4x\n", ele_addr);
bt_shell_printf("\tPub Addr: %4.4x", pub->u.addr16);
if (mod_id > 0xffff0000)
bt_shell_printf("\tModel: %8.8x \n", mod_id);
else
bt_shell_printf("\tModel: %4.4x \n",
(uint16_t) (mod_id & 0xffff));
bt_shell_printf("\tApp Key Idx: %4.4x", pub->app_idx);
bt_shell_printf("\tTTL: %2.2x", pub->ttl);
}
void swap_u256_bytes(uint8_t *u256)
{
int i;
/* End-to-End byte reflection of 32 octet buffer */
for (i = 0; i < 16; i++) {
u256[i] ^= u256[31 - i];
u256[31 - i] ^= u256[i];
u256[i] ^= u256[31 - i];
}
}
|
