void BinaryReceived(const void *_data, size_t length) {
const uint8_t *data = (const uint8_t *)_data, *end = data + length;
do {
/* append new data to buffer, as much as fits there */
auto range = binary_buffer.Write();
if (range.IsEmpty()) {
/* overflow: reset buffer to recover quickly */
binary_buffer.Clear();
continue;
}
size_t nbytes = std::min(size_t(range.length), size_t(end - data));
memcpy(range.data, data, nbytes);
data += nbytes;
binary_buffer.Append(nbytes);
while (true) {
range = binary_buffer.Read();
if (range.IsEmpty())
break;
size_t nbytes = HandleBinary(range.data, range.length);
if (nbytes == 0) {
if (binary_buffer.IsFull())
binary_buffer.Clear();
break;
}
binary_buffer.Consume(nbytes);
}
} while (data < end);
}
virtual typename Source<T>::Range Read() override {
auto r = buffer.Write();
if (!r.IsEmpty()) {
unsigned n = Read(r.data, r.size);
buffer.Append(n);
}
return buffer.Read();
}
virtual void DataReceived(const void *data, size_t length) {
mutex.Lock();
auto range = buffer.Write();
if (range.length < length)
length = range.length;
memcpy(range.data, data, length);
buffer.Append(length);
mutex.Unlock();
SendNotification();
}
virtual typename Source<T>::Range read() {
auto r = buffer.Write();
if (!r.IsEmpty()) {
unsigned n = read(r.data, r.length);
buffer.Append(n);
}
r = buffer.Read();
return typename Source<T>::Range(r.data, r.length);
}
bool Fill(TimeoutClock &timeout) {
const auto dest = buffer.Write();
if (dest.IsEmpty())
/* already full */
return false;
const Port::WaitResult wresult =
port.WaitRead(env, timeout.GetRemainingOrZero());
if (wresult != Port::WaitResult::READY)
return false;
const int nbytes = port.Read(dest.data, dest.length);
if (nbytes <= 0)
return false;
buffer.Append(nbytes);
return true;
}
bool
WesterboerVW921Device::DataReceived(const void *_data, size_t length,
struct NMEAInfo &info)
{
assert(_data != NULL);
assert(length > 0);
bool result = false;
const char *data = (const char *)_data, *end = data + length;
do {
// append new data to buffer, as much as fits there
auto range = buffer.Write();
if (range.IsEmpty()) {
// overflow: reset buffer to recover quickly
buffer.Clear();
continue;
}
size_t nbytes = std::min(size_t(range.length), size_t(end - data));
memcpy(range.data, data, nbytes);
data += nbytes;
buffer.Append(nbytes);
while (true) {
// read data from the buffer, to see if there's a dollar character
range = buffer.Read();
if (range.IsEmpty())
break;
// Search for the dollar sign (sync byte)
char *dollar = (char *)memchr(range.data, '$', range.length);
if (dollar == NULL)
// no dollar sign here: wait for more data
break;
// Make sure there are at least 5 bytes in the buffer to
// read the sentence header
unsigned remaining_length = range.length - (dollar - range.data);
if (remaining_length < 5)
break;
// Check this is a Westerboer sentence
if (dollar[1] != 'w') {
// Skip this sentence
buffer.Consume(dollar - range.data + 1);
continue;
}
// Read the length of the sentence
uint8_t sentence_length = (uint8_t)dollar[2];
// Check if the sentence was completely received already
if (remaining_length < sentence_length)
break;
if (!CheckChecksum(dollar, sentence_length)) {
// Skip this sentence
buffer.Consume(dollar - range.data + 1);
continue;
}
// Read the sentence identification number
uint8_t sentence_number = (uint8_t)dollar[4];
// Parse the sentence
SentenceReceived(sentence_number, dollar, sentence_length, info);
buffer.Consume(dollar - range.data + sentence_length);
result = true;
}
} while (data < end);
return result;
}
