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
|
#include "Packet.h"
#include <string.h>
#include <SHA256.h>
namespace mesh {
Packet::Packet() {
header = 0;
path_len = 0;
payload_len = 0;
}
bool Packet::isValidPathLen(uint8_t path_len) {
uint8_t hash_count = path_len & 63;
uint8_t hash_size = (path_len >> 6) + 1;
if (hash_size == 4) return false; // Reserved for future
return hash_count*hash_size <= MAX_PATH_SIZE;
}
size_t Packet::writePath(uint8_t* dest, const uint8_t* src, uint8_t path_len) {
uint8_t hash_count = path_len & 63;
uint8_t hash_size = (path_len >> 6) + 1;
size_t len = hash_count*hash_size;
if (len > MAX_PATH_SIZE) {
MESH_DEBUG_PRINTLN("Packet::copyPath, invalid path_len=%d", (uint32_t)path_len);
return 0; // Error
}
memcpy(dest, src, len);
return len;
}
uint8_t Packet::copyPath(uint8_t* dest, const uint8_t* src, uint8_t path_len) {
writePath(dest, src, path_len);
return path_len;
}
int Packet::getRawLength() const {
return 2 + getPathByteLen() + payload_len + (hasTransportCodes() ? 4 : 0);
}
void Packet::calculatePacketHash(uint8_t* hash) const {
SHA256 sha;
uint8_t t = getPayloadType();
sha.update(&t, 1);
if (t == PAYLOAD_TYPE_TRACE) {
sha.update(&path_len, sizeof(path_len)); // CAVEAT: TRACE packets can revisit same node on return path
}
sha.update(payload, payload_len);
sha.finalize(hash, MAX_HASH_SIZE);
}
uint8_t Packet::writeTo(uint8_t dest[]) const {
uint8_t i = 0;
dest[i++] = header;
if (hasTransportCodes()) {
memcpy(&dest[i], &transport_codes[0], 2); i += 2;
memcpy(&dest[i], &transport_codes[1], 2); i += 2;
}
dest[i++] = path_len;
i += writePath(&dest[i], path, path_len);
memcpy(&dest[i], payload, payload_len); i += payload_len;
return i;
}
bool Packet::readFrom(const uint8_t src[], uint8_t len) {
if (!src || len < 2) {
return false;
}
uint8_t i = 0;
header = src[i++];
if (hasTransportCodes()) {
if (len < 6) {
return false;
}
memcpy(&transport_codes[0], &src[i], 2); i += 2;
memcpy(&transport_codes[1], &src[i], 2); i += 2;
} else {
transport_codes[0] = transport_codes[1] = 0;
}
path_len = src[i++];
if (!isValidPathLen(path_len)) return false; // bad encoding
uint8_t bl = getPathByteLen();
if ((len - i) < bl) {
return false;
}
memcpy(path, &src[i], bl); i += bl;
if (i >= len) return false; // bad encoding
payload_len = len - i;
if (payload_len > sizeof(payload)) return false; // bad encoding
memcpy(payload, &src[i], payload_len); //i += payload_len;
return true; // success
}
}
|
