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
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
|
// SPDX-License-Identifier: BSD-3-Clause
#include <memory>
#include <iostream>
#include <limits>
#include <cstring>
#include <sstream>
#include "crc.h"
#include "device.h"
#include "time.h"
#include "exceptions.h"
#include "hexdump/hexdump.h"
#include "../logging.h"
namespace voltronic {
Device::Device() :
flags_(FLAG_WRITE_CRC | FLAG_READ_CRC | FLAG_VERIFY_CRC),
timeout_(TIMEOUT) {}
void Device::setFlags(int flags) {
flags_ = flags;
}
int Device::getFlags() const {
return flags_;
}
void Device::setVerbose(bool verbose) {
verbose_ = verbose;
}
void Device::setTimeout(u64 timeout) {
timeout_ = timeout;
}
u64 Device::getElapsedTime() const {
return timestamp() - timeStarted_;
}
u64 Device::getTimeLeft() const {
if (!timeout_)
return std::numeric_limits<uint64_t>::max();
u64 elapsed = getElapsedTime();
if (elapsed > timeout_)
return 0;
return timeout_ - elapsed;
}
size_t Device::run(const u8* inbuf, size_t inbufSize, u8* outbuf, size_t outbufSize) {
timeStarted_ = timestamp();
send(inbuf, inbufSize);
if (!getTimeLeft()) {
// FIXME
// we should read incoming data from the device,
// or clean the buffer in some other way.
// otherwise we may get invalid response next time
throw TimeoutError("sending already took " + std::to_string(getElapsedTime()) + " ms");
}
return recv(outbuf, outbufSize);
}
void Device::send(const u8* buf, size_t bufSize) {
size_t dataLen;
std::shared_ptr<u8> data;
if ((flags_ & FLAG_WRITE_CRC) == FLAG_WRITE_CRC) {
const CRC crc = crc_calculate(buf, bufSize);
dataLen = bufSize + sizeof(u16) + 1;
data = std::unique_ptr<u8>(new u8[dataLen]);
crc_write(crc, &data.get()[bufSize]);
} else {
dataLen = bufSize + 1;
data = std::unique_ptr<u8>(new u8[dataLen]);
}
u8* dataPtr = data.get();
memcpy((void*)dataPtr, buf, bufSize);
dataPtr[dataLen - 1] = '\r';
if (verbose_) {
myerr << "writing " << dataLen << (dataLen > 1 ? " bytes" : " byte");
std::cerr << hexdump(dataPtr, dataLen);
}
writeLoop(dataPtr, dataLen);
}
void Device::writeLoop(const u8* data, size_t dataSize) {
int bytesLeft = static_cast<int>(dataSize);
while (true) {
size_t bytesWritten = write(data, bytesLeft);
if (verbose_)
myerr << "bytesWritten=" << bytesWritten;
bytesLeft -= static_cast<int>(bytesWritten);
if (bytesLeft <= 0)
break;
if (!getTimeLeft())
throw TimeoutError("data writing already took " + std::to_string(getElapsedTime()) + " ms");
data = &data[bytesWritten];
}
}
size_t Device::recv(u8* buf, size_t bufSize) {
size_t bytesRead = readLoop(buf, bufSize);
if (verbose_) {
myerr << "got " << bytesRead << (bytesRead > 1 ? " bytes" : " byte");
std::cerr << hexdump(buf, bytesRead);
}
bool crcNeeded = (flags_ & FLAG_READ_CRC) == FLAG_READ_CRC;
size_t minSize = crcNeeded ? sizeof(u16) + 1 : 1;
if (bytesRead < minSize)
throw InvalidDataError("response is too small");
const size_t dataSize = bytesRead - minSize;
if (crcNeeded) {
const CRC crcActual = crc_read(&buf[dataSize]);
const CRC crcExpected = crc_calculate(buf, dataSize);
// buf[dataSize] = 0;
if ((flags_ & FLAG_VERIFY_CRC) == FLAG_VERIFY_CRC && crcActual == crcExpected)
return dataSize;
std::ostringstream error;
error << std::hex;
error << "crc is invalid: expected 0x" << crcExpected << ", got 0x" << crcActual;
throw InvalidDataError(error.str());
}
// buf[dataSize] = 0;
return dataSize;
}
size_t Device::readLoop(u8 *buf, size_t bufSize) {
size_t size = 0;
while(true) {
size_t bytesRead = read(buf, bufSize);
if (verbose_)
myerr << "bytesRead=" << bytesRead;
while (bytesRead) {
bytesRead--;
size++;
if (*buf == '\r')
return size;
buf++;
bufSize--;
}
if (!getTimeLeft())
throw TimeoutError("data reading already took " + std::to_string(getElapsedTime()) + " ms");
if (bufSize <= 0)
throw std::overflow_error("input buffer is not large enough");
}
}
}
|
