Packet.cpp 6.47 KB
Newer Older
/jdi/'s avatar
basics  
/jdi/ committed
1
2
3
4
5
6
7
/*
 * Packet.cpp
 *
 *  Created on: 03.09.2015
 *      Author: jdi
 */

/jdi/'s avatar
/jdi/ committed
8
#include <cstring>
/jdi/'s avatar
basics  
/jdi/ committed
9
#include <cstdlib>
/jdi/'s avatar
/jdi/ committed
10
11
#include <cstdio>
#include <ctime>
/jdi/'s avatar
basics  
/jdi/ committed
12
#include "Packet.h"
/jdi/'s avatar
/jdi/ committed
13
14
#include "Types.h"
#include "Utils.h"
/jdi/'s avatar
basics  
/jdi/ committed
15
16
17
18
19
20
21

Packet::Packet(OpCode c) {
	srand(time(NULL));
	sequenceId = rand() % 1000;
	opCode = c;
}

/jdi/'s avatar
/jdi/ committed
22
void Packet::printHeader() {
/jdi/'s avatar
/jdi/ committed
23
24
25
26
	printf(
			"Header:\n\tOpCode:\t\t%s\n\tID:\t\t%d\n\tVersion:\t%hhd\n\tError:\t\t%.8X\n\tSwitch MAC:\t",
			opCodeToString().c_str(), sequenceId, version, errorCode);
	utils::print(switchMac);
/jdi/'s avatar
/jdi/ committed
27
	printf("\n\tHost MAC:\t");
/jdi/'s avatar
/jdi/ committed
28
29
30
31
32
	utils::print(hostMac);
	printf("\n\tLength:\t%hd", this->getLength());
	printf("\n\tOffset:\t%hd", fragmentOffset);
	printf("\n\tFlags:\t%.4hX", flag);
	printf("\n\tChecksum:\t%d", checkSum);
/jdi/'s avatar
/jdi/ committed
33
34
}

/jdi/'s avatar
basics  
/jdi/ committed
35
36
bytes Packet::getBytes() {
	int i = 0;
/jdi/'s avatar
/jdi/ committed
37
38
	for (unsigned j = 0; j < payload.size(); j++)
		push(body, i, payload[j]);
/jdi/'s avatar
basics  
/jdi/ committed
39
40
41
42
43
44
45
	push(body, i, (int) PACKET_END);
	i = 0;
	push(head, i, version);
	push(head, i, opCode);
	push(head, i, switchMac);
	push(head, i, hostMac);
	push(head, i, sequenceId);
/jdi/'s avatar
/jdi/ committed
46
	push(head, i, errorCode);
/jdi/'s avatar
basics  
/jdi/ committed
47
48
49
50
51
52
	push(head, i, this->getLength());
	push(head, i, fragmentOffset);
	push(head, i, flag);
	push(head, i, tokenId);
	push(head, i, checkSum);
	bytes data = head + body;
/jdi/'s avatar
/jdi/ committed
53
54
55
	//printf("Send Header:\t");
	//utils::printHex(head);
	//printf("\n");
/jdi/'s avatar
basics  
/jdi/ committed
56
57
58
	return data;
}

/jdi/'s avatar
/jdi/ committed
59
60
61
62
void Packet::parse(bytes data) {
	memcpy(&head[0], &data[0], HEADER_LEN);
	body.resize(data.size() - HEADER_LEN);
	memcpy(&body[0], &data[HEADER_LEN], data.size() - HEADER_LEN);
/jdi/'s avatar
/jdi/ committed
63
64
65
	//printf("Receive Header:\t");
	//utils::printHex(head);
	//printf("\n");
/jdi/'s avatar
/jdi/ committed
66
67
68
69
	int i = 0;
	short checkLen = 0x0;
	pull(head, i, version);
	pull(head, i, opCode);
/jdi/'s avatar
/jdi/ committed
70
71
	pull(head, i, switchMac);
	pull(head, i, hostMac);
/jdi/'s avatar
/jdi/ committed
72
73
74
75
76
77
78
	pull(head, i, sequenceId);
	pull(head, i, errorCode);
	pull(head, i, checkLen);
	pull(head, i, fragmentOffset);
	pull(head, i, flag);
	pull(head, i, tokenId);
	pull(head, i, checkSum);
/jdi/'s avatar
/jdi/ committed
79
	if (this->getLength() != checkLen) {
/jdi/'s avatar
/jdi/ committed
80
		printf("Packet Length doesn't match: %lu != %hd\n", data.size(),
/jdi/'s avatar
/jdi/ committed
81
				checkLen);
/jdi/'s avatar
/jdi/ committed
82
	}
/jdi/'s avatar
/jdi/ committed
83
	i = 0;
/jdi/'s avatar
/jdi/ committed
84
85
	dataset d;
	payload = {};
/jdi/'s avatar
/jdi/ committed
86
87
88
89
90
91
	while (i < (int) body.size() - 4) {
		pull(body, i, d.type);
		pull(body, i, d.len);
		pull(body, i, d.value, d.len);
		payload[d.type] = d;
	}
/jdi/'s avatar
/jdi/ committed
92
93
94
95
96
}

const bytes& Packet::getBody() const {
	return body;
}
/jdi/'s avatar
/jdi/ committed
97

/jdi/'s avatar
/jdi/ committed
98
99
const bytes& Packet::getHead() const {
	return head;
/jdi/'s avatar
/jdi/ committed
100
101
}

/jdi/'s avatar
basics  
/jdi/ committed
102
103
104
105
void Packet::setBody(bytes data) {
	this->body = data;
}

/jdi/'s avatar
/jdi/ committed
106
void Packet::setHostMac(byteArray<6> mac) {
/jdi/'s avatar
basics  
/jdi/ committed
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
	this->hostMac = mac;
}

short Packet::getLength() const {
	return HEADER_LEN + body.size();
}

int Packet::getCheckSum() const {
	return checkSum;
}

void Packet::setCheckSum(int checkSum) {
	this->checkSum = checkSum;
}

short Packet::getSequenceId() const {
	return sequenceId;
}

void Packet::setSequenceId(short sequenceId) {
	this->sequenceId = sequenceId;
}

/jdi/'s avatar
/jdi/ committed
130
const byteArray<6>& Packet::getSwitchMac() const {
/jdi/'s avatar
basics  
/jdi/ committed
131
132
133
	return switchMac;
}

/jdi/'s avatar
/jdi/ committed
134
void Packet::setSwitchMac(byteArray<6> switchMac) {
/jdi/'s avatar
basics  
/jdi/ committed
135
136
137
138
139
140
141
142
143
144
145
	this->switchMac = switchMac;
}

const datasets& Packet::getPayload() const {
	return payload;
}

void Packet::setPayload(const datasets& payload) {
	this->payload = payload;
}

/jdi/'s avatar
/jdi/ committed
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
std::string Packet::opCodeToString() {
	switch (opCode) {
	case DISCOVERY:
		return "DISCOVERY";
	case GET:
		return "GET";
	case SET:
		return "SET";
	case READ:
		return "READ";
	default:
		return "NONE";
	}
	return "NONE";
}

/jdi/'s avatar
basics  
/jdi/ committed
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
void Packet::encode(bytes &data) {
	int len = data.size();
	bytes key = { 191, 155, 227, 202, 99, 162, 79, 104, 49, 18, 190, 164, 30,
			76, 189, 131, 23, 52, 86, 106, 207, 125, 126, 169, 196, 28, 172, 58,
			188, 132, 160, 3, 36, 120, 144, 168, 12, 231, 116, 44, 41, 97, 108,
			213, 42, 198, 32, 148, 218, 107, 247, 112, 204, 14, 66, 68, 91, 224,
			206, 235, 33, 130, 203, 178, 1, 134, 199, 78, 249, 123, 7, 145, 73,
			208, 209, 100, 74, 115, 72, 118, 8, 22, 243, 147, 64, 96, 5, 87, 60,
			113, 233, 152, 31, 219, 143, 174, 232, 153, 245, 158, 254, 70, 170,
			75, 77, 215, 211, 59, 71, 133, 214, 157, 151, 6, 46, 81, 94, 136,
			166, 210, 4, 43, 241, 29, 223, 176, 67, 63, 186, 137, 129, 40, 248,
			255, 55, 15, 62, 183, 222, 105, 236, 197, 127, 54, 179, 194, 229,
			185, 37, 90, 237, 184, 25, 156, 173, 26, 187, 220, 2, 225, 0, 240,
			50, 251, 212, 253, 167, 17, 193, 205, 177, 21, 181, 246, 82, 226,
			38, 101, 163, 182, 242, 92, 20, 11, 95, 13, 230, 16, 121, 124, 109,
			195, 117, 39, 98, 239, 84, 56, 139, 161, 47, 201, 51, 135, 250, 10,
			19, 150, 45, 111, 27, 24, 142, 80, 85, 83, 234, 138, 216, 57, 93,
			65, 154, 141, 122, 34, 140, 128, 238, 88, 89, 9, 146, 171, 149, 53,
			102, 61, 114, 69, 217, 175, 103, 228, 35, 180, 252, 200, 192, 165,
			159, 221, 244, 110, 119, 48 };
	bytes s = key;
	int i, t, j = 0;
	for (int k = 0; k < len; k++) {
		i = (k + 1) % 256;
		j = (j + s[i]) % 256;
		t = s[i];
		s[i] = s[j];
		s[j] = t;
		data[k] = ((data[k] ^ s[(s[i] + s[j]) % 256]));
	}
}

void Packet::push(bytes &arr, int &index, byte data) {
/jdi/'s avatar
/jdi/ committed
195
196
197
198
199
200
	if (arr.size() > index) {
		arr[index++] = data;
	} else {
		arr.push_back(data);
		index++;
	}
/jdi/'s avatar
basics  
/jdi/ committed
201
202
}

/jdi/'s avatar
/jdi/ committed
203

/jdi/'s avatar
basics  
/jdi/ committed
204
205
206
207
208
void Packet::push(bytes &arr, int &index, bytes data) {
	for (unsigned j = 0; j < data.size(); j++)
		push(arr, index, data[j]);
}

/jdi/'s avatar
/jdi/ committed
209
210
211
212
213
214
template<size_t N>
void Packet::push(bytes &arr, int &index, byteArray<N> data) {
	for (unsigned j = 0; j < N; j++)
		push(arr, index, data[j]);
}

/jdi/'s avatar
basics  
/jdi/ committed
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
void Packet::push(bytes &arr, int &index, short data) {
	byte a = (data >> 8) & 0xFF;
	push(arr, index, a);
	a = data & 0xFF;
	push(arr, index, a);
}

void Packet::push(bytes &arr, int &index, int data) {
	byte a = (data >> 24) & 0xFF;
	push(arr, index, a);
	a = (data >> 16) & 0xFF;
	push(arr, index, a);
	a = (data >> 8) & 0xFF;
	push(arr, index, a);
	a = data & 0xFF;
	push(arr, index, a);
}

void Packet::push(bytes &arr, int &index, dataset data) {
	push(arr, index, data.type);
	push(arr, index, data.len);
	push(arr, index, data.value);
}

/jdi/'s avatar
/jdi/ committed
239
240
241
242
243
244
245
void Packet::pull(bytes &arr, int &index, byte &ret) {
	ret = arr[index++];
}

void Packet::pull(bytes &arr, int &index, bytes &ret, unsigned len) {
	ret.resize(len);
	memcpy(&ret[0], &arr[index], len);
/jdi/'s avatar
/jdi/ committed
246
	index += len;
/jdi/'s avatar
/jdi/ committed
247
248
}

/jdi/'s avatar
/jdi/ committed
249
250
251
252
253
254
template<size_t N>
void Packet::pull(bytes &arr, int &index, byteArray<N> &ret) {
	memcpy(&ret[0], &arr[index], N);
	index += N;
}

/jdi/'s avatar
/jdi/ committed
255
256
257
void Packet::pull(bytes &arr, int &index, short &ret) {
	ret = (arr[index++] << 8);
	ret |= arr[index++] & 0xFF;
/jdi/'s avatar
/jdi/ committed
258
	ret &= 0xFFFF;
/jdi/'s avatar
/jdi/ committed
259
260
261
262
263
264
265
266
267
268
269
270
271
}

void Packet::pull(bytes &arr, int &index, int &ret) {
	ret = arr[index++] << 24;
	ret |= (arr[index++] & 0xFF) << 16;
	ret |= (arr[index++] & 0xFF) << 8;
	ret |= arr[index++] & 0xFF;
}

void Packet::pull(bytes &arr, int &index, dataset &ret) {
	pull(arr, index, ret.type);
	pull(arr, index, ret.len);
	pull(arr, index, ret.value, (unsigned) ret.len);
/jdi/'s avatar
basics  
/jdi/ committed
272
}