blob: ab8a5f378d053f188e24a826768926d7ba777a80 (
plain) (
blame)
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
|
#include "frame.h"
unsigned char hex_byte_to_byte(unsigned char byte) {
if (byte >= 'a' && byte <= 'f') return byte - 'a' + 10;
if (byte >= 'A' && byte <= 'F') return byte - 'A' + 10;
if (byte >= '0' && byte <= '9') return byte - '0';
throw std::invalid_argument("Character isn't a hex value!");
}
unsigned char hex_value_to_byte(const unsigned char* hex) {
return (hex_byte_to_byte(hex[0]) << 4) | hex_byte_to_byte(hex[1]);
}
unsigned char next_hex_byte(const unsigned char*& iso_tp_frame) {
if (*iso_tp_frame == '\0')
throw "Bad";
char byte = hex_value_to_byte(iso_tp_frame);
iso_tp_frame += 2;
return byte;
}
// b0 b1 b2 b3 b4 b5 b6 b7
// 02 10 C0 00 00 00 00 00
void Frame::parse_frame_str(const unsigned char* iso_tp_frame) {
unsigned char byte0 = next_hex_byte(iso_tp_frame);
this->type = (FrameType)((byte0 & 0b11110000) >> 4);
this->size = byte0 & 0b00001111;
switch (this->type) {
// Single frame
case SF: this->data_size = size;
break;
// First frame
case FF: {
unsigned char byte1 = next_hex_byte(iso_tp_frame);
this->size = (this->size << 8) | byte1;
this->data_size = 6;
}
break;
// Consecutive frame
case CF: this->data_size = 7;
break;
// Flow control frame (ignore)
case FC: this->data_size = 0;
this->size = 0;
break;
}
this->data = (unsigned char*)malloc(sizeof(unsigned char) * this->data_size);
for (int i = 0; i < this->data_size; ++i) {
this->data[i] = next_hex_byte(iso_tp_frame);
}
}
std::ostream& operator<<(std::ostream& os, const Frame& f) {
for (int i = 0; i < f.data_size; ++i) {
os << (unsigned)f.data[i];
}
return os;
}
|
