virtual void DataReceived(const void *data, size_t length) {
mutex.Lock();
buffer.Shift();
auto range = buffer.Write();
if (range.size < length)
length = range.size;
memcpy(range.data, data, length);
buffer.Append(length);
mutex.Unlock();
SendNotification();
}
virtual void OnNotification() {
while (true) {
char data[64];
size_t length;
{
ScopeLock protect(mutex);
auto range = buffer.Read();
if (range.IsEmpty())
break;
length = std::min(ARRAY_SIZE(data), size_t(range.size));
memcpy(data, range.data, length);
buffer.Consume(length);
}
terminal.Write(data, length);
}
}
bool
KRT2Device::DataReceived(const void *_data, size_t length,
struct NMEAInfo &info)
{
assert(_data != nullptr);
assert(length > 0);
const uint8_t *data = (const uint8_t *)_data;
const uint8_t *end = data + length;
do {
// Append new data to the buffer, as much as fits in there
auto range = rx_buf.Write();
if (rx_buf.IsFull()) {
// Overflow: reset buffer to recover quickly
rx_buf.Clear();
expected_msg_length = 0;
continue;
}
size_t nbytes = std::min(range.size, size_t(end - data));
memcpy(range.data, data, nbytes);
data += nbytes;
rx_buf.Append(nbytes);
for (;;) {
// Read data from buffer to handle the messages
range = rx_buf.Read();
if (range.IsEmpty())
break;
if (range.size < expected_msg_length)
break;
expected_msg_length = ExpectedMsgLength(range.data, range.size);
if (range.size >= expected_msg_length) {
switch (*(const uint8_t *) range.data) {
case ACK:
case NAK:
// Received a response to a normal command (STX)
response_mutex.Lock();
response = *(const uint8_t *) range.data;
// Signal the response to the TX thread
rx_cond.signal();
response_mutex.Unlock();
break;
default:
// Received a command from the radio -> ignore it
break;
}
// Message handled -> remove message
rx_buf.Consume(expected_msg_length);
expected_msg_length = 0;
// Received something from the radio -> the connection is alive
info.alive.Update(info.clock);
}
}
} while (data < end);
return true;
}