Packet.cpp 6.71 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
#include "Types.h"
/jdi/'s avatar
/jdi/ committed
14
15
#include "Types/bytes.h"
#include "Types/datasets.h"
/jdi/'s avatar
basics  
/jdi/ committed
16
17
18
19
20
21
22

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

/jdi/'s avatar
/jdi/ committed
23
void Packet::printHeader() {
/jdi/'s avatar
/jdi/ committed
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);
/jdi/'s avatar
/jdi/ committed
27
28
	std::cout << switchMac << "\n";
	std::cout << "\tHost MAC:\t"<< hostMac << "\n";
/jdi/'s avatar
/jdi/ committed
29
30
31
32
	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(macAddr 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
macAddr Packet::getSwitchMac() const {
/jdi/'s avatar
basics  
/jdi/ committed
131
132
133
	return switchMac;
}

/jdi/'s avatar
/jdi/ committed
134
void Packet::setSwitchMac(macAddr 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
void Packet::encode(bytes &data) {
	int len = data.size();
/jdi/'s avatar
/jdi/ committed
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
	std::vector<unsigned char> t = { 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 key = t;
/jdi/'s avatar
basics  
/jdi/ committed
183
	bytes s = key;
/jdi/'s avatar
/jdi/ committed
184
	int i, j = 0;
/jdi/'s avatar
basics  
/jdi/ committed
185
186
187
	for (int k = 0; k < len; k++) {
		i = (k + 1) % 256;
		j = (j + s[i]) % 256;
/jdi/'s avatar
/jdi/ committed
188
		std::swap(s[i], s[j]);
/jdi/'s avatar
/jdi/ committed
189
		data[k] = data[k] ^ s[(s[i] + s[j]) % 256];
/jdi/'s avatar
basics  
/jdi/ committed
190
191
192
193
	}
}

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

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
207
void Packet::push(bytes &arr, int &index, ipAddr data) {
/jdi/'s avatar
/jdi/ committed
208
	for (unsigned j = 0; j < 4; j++)
/jdi/'s avatar
/jdi/ committed
209
		push(arr, index, (byte) data[j]);
/jdi/'s avatar
/jdi/ committed
210
211
}

/jdi/'s avatar
/jdi/ committed
212
void Packet::push(bytes &arr, int &index, macAddr data) {
/jdi/'s avatar
/jdi/ committed
213
	for (unsigned j = 0; j < 6; j++)
/jdi/'s avatar
/jdi/ committed
214
		push(arr, index, (byte) data[j]);
/jdi/'s avatar
/jdi/ committed
215
216
}

/jdi/'s avatar
basics  
/jdi/ committed
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
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
241
242
243
244
245
246
247
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
248
	index += len;
/jdi/'s avatar
/jdi/ committed
249
250
}

/jdi/'s avatar
/jdi/ committed
251
252
253
254
255
void Packet::pull(bytes &arr, int &index, macAddr &ret) {
	memcpy(&ret[0], &arr[index], 6);
	index += 6;
}

/jdi/'s avatar
/jdi/ committed
256
void Packet::pull(bytes &arr, int &index, ipAddr &ret) {
/jdi/'s avatar
/jdi/ committed
257
258
	memcpy(&ret[0], &arr[index], 4);
	index += 4;
/jdi/'s avatar
/jdi/ committed
259
260
}

/jdi/'s avatar
/jdi/ committed
261
262
263
void Packet::pull(bytes &arr, int &index, short &ret) {
	ret = (arr[index++] << 8);
	ret |= arr[index++] & 0xFF;
/jdi/'s avatar
/jdi/ committed
264
	ret &= 0xFFFF;
/jdi/'s avatar
/jdi/ committed
265
266
267
268
269
270
271
272
273
274
275
276
277
}

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
278
}