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
|
#include <stdbool.h>
#include <stdint.h>
#include "types.h"
#include "eeprom.h"
#include "can.h"
#include "signal.h"
#include "serial.h"
void
serU16Be(U8 buf[2u], U16 n) {
buf[0u] = (n >> 8u) & 0xFF;
buf[1u] = (n >> 0u) & 0xFF;
}
U16
deserU16Be(const U8 buf[2u]) {
return ((U16)buf[0u] << 8u)
| ((U16)buf[1u] << 0u);
}
void
serU32Be(U8 buf[4u], U32 n) {
buf[0u] = (n >> 24u) & 0xFF;
buf[1u] = (n >> 16u) & 0xFF;
buf[2u] = (n >> 8u) & 0xFF;
buf[3u] = (n >> 0u) & 0xFF;
}
U32
deserU32Be(const U8 buf[4u]) {
return ((U32)buf[0u] << 24u)
| ((U32)buf[1u] << 16u)
| ((U32)buf[2u] << 8u)
| ((U32)buf[3u] << 0u);
}
Status
serWriteCanId(U16 addr, const CanId *id) {
U8 buf[4u];
// Copy ID to buffer
if (id->isExt) { // extended
serU32Be(buf, id->eid & 0x1FFFFFFF);
buf[3u] |= 0x80; // set EID flag in bit 31
} else { // standard
serU32Be(buf, id->sid & 0x7FF);
}
return eepromWrite(addr, buf, sizeof(buf));
}
Status
serReadCanId(U16 addr, CanId *id) {
U8 buf[sizeof(U32)];
Status status;
// Read
status = eepromRead(addr, buf, sizeof(buf));
if (status != OK) {
return FAIL;
}
// Unpack
if (buf[3u] & 0x80) { // bit 31 is standard/extended flag
id->isExt = true; // extended
id->eid = deserU32Be(buf) & 0x1FFFFFFF;
} else {
id->isExt = false; // standard
id->sid = deserU32Be(buf) & 0x7FF;
}
return OK;
}
Status
serWriteSigFmt(EepromAddr addr, const SigFmt *sig) {
Status status;
U8 buf[3u];
// ID
status = serWriteCanId(addr, &sig->id);
if (status != OK) {
return status;
}
// Encoding
buf[0u] = sig->start;
buf[1u] = sig->size;
buf[2u] = (U8)((sig->order & 0x01) << 7u)
| ((sig->isSigned) ? 0x40 : 0x00);
return eepromWrite(addr+SER_CANID_SIZE, buf, sizeof(buf));
}
Status
serReadSigFmt(EepromAddr addr, SigFmt *sig) {
Status status;
U8 buf[3u];
// ID
status = serReadCanId(addr, &sig->id);
if (status != OK) {
return status;
}
// Encoding
status = eepromRead(addr+SER_CANID_SIZE, buf, sizeof(buf));
if (status != OK) {
return status;
}
sig->start = buf[0u];
sig->size = buf[1u];
sig->order = (buf[2u] & 0x80) ? LITTLE_ENDIAN : BIG_ENDIAN;
sig->isSigned = buf[2u] & 0x40;
return OK;
}
|