int zp_ClusterNode::run()
{
if (bTermSet==true)
{
//qDebug()<<QString("%1(%2) Node Martked Deleted, return.").arg((unsigned int)this).arg(ref());
return 0;
}
int nCurrSz = -1;
int nMessage = m_nMessageBlockSize;
while (--nMessage>=0 && nCurrSz!=0 )
{
QByteArray block;
m_mutex_rawData.lock();
if (m_list_RawData.size())
block = *m_list_RawData.begin();
m_mutex_rawData.unlock();
if (block.isEmpty()==false && block.isNull()==false)
{
m_currentReadOffset = filter_message(block,m_currentReadOffset);
if (m_currentReadOffset >= block.size())
{
m_mutex_rawData.lock();
if (m_list_RawData.empty()==false)
m_list_RawData.pop_front();
else
assert(false);
m_currentReadOffset = 0;
m_mutex_rawData.unlock();
}
}
else
{
m_mutex_rawData.lock();
//pop empty cabs
if (m_list_RawData.empty()==false)
m_list_RawData.pop_front();
m_mutex_rawData.unlock();
}
m_mutex_rawData.lock();
nCurrSz = m_list_RawData.size();
m_mutex_rawData.unlock();
}
m_mutex_rawData.lock();
nCurrSz = m_list_RawData.size();
m_mutex_rawData.unlock();
if (nCurrSz==0)
return 0;
return -1;
}
static void get_messages_listing_resp(const char **reply, void *user_data)
{
struct session *session = user_data;
struct request *request = session->request;
struct messages_message *msg_data;
DBG("reply %p", reply);
if (reply == NULL)
goto done;
if (session->aborted)
goto aborted;
msg_data = pull_message_data(reply);
request->size++;
if (!msg_data->read)
request->new_message = TRUE;
if (request->count == TRUE) {
free_msg_data(msg_data);
return;
}
if (request->size > request->offset && filter_message(msg_data,
request->filter))
request->cb.messages_list(session, -EAGAIN, 1,
request->new_message, msg_data,
request->user_data);
free_msg_data(msg_data);
return;
done:
request->cb.messages_list(session, 0, request->size,
request->new_message, NULL,
request->user_data);
aborted:
g_free(request->filter->period_begin);
g_free(request->filter->period_end);
g_free(request->filter->originator);
g_free(request->filter->recipient);
g_free(request->filter);
g_free(request);
}
/*
* Some notes on this format, as it is poorly documented by Kinetic:
*
* The stream can start at an arbitrary point, the first byte might be mid-packet.
* You need to look for a DLE STX to synchronize with the stream.
* This implementation does that in the Python code to keep this bit simpler; the
* C code assumes it is always given bytes starting at the start of a packet.
*
* You might get arbitrary packet types e.g. AIS interleaved with Mode S messages.
* This implementation doesn't try to interpret them at all, it just reads all
* data until DLE ETX regardless of type and skips those types it doesn't
* understand.
*
* The Mode S CRC values are not the raw bytes from the message; they are the
* residual CRC value after XORing the raw bytes with the calculated CRC over
* the body of the message. That is, a DF17 message with a correct CRC will have
* zeros in the CRC bytes; a DF11 with correct CRC will have the IID in the CRC
* bytes; messages that use Address/Parity will have the address in the CRC bytes.
* To recover the original message, calculate the CRC and XOR it back into the CRC
* bytes. Andrew Whewell says this is probably controlled by a Basestation setting.
*
* The timestamps are measured at the _end_ of the frame, not at the start.
* As frames are variable length, if you want a timestamp anchored to the
* start of the frame (as dump1090 / Beast do), you have to compensate for
* the frame length.
*/
static PyObject *feed_sbs(modesreader *self, Py_buffer *buffer, int max_messages)
{
PyObject *rv = NULL;
uint8_t *buffer_start, *p, *eod;
int message_count = 0;
PyObject *message_tuple = NULL;
PyObject **messages = NULL;
int error_pending = 0;
buffer_start = buffer->buf;
if (max_messages <= 0) {
/* allocate the maximum size we might need, given a minimal encoding of:
* <DLE> <STX> <0x09> <n/a> <3 bytes timestamp> <2 bytes message> <DLE> <ETX> <2 bytes CRC> = 13 bytes total
*/
max_messages = buffer->len / 13 + 1;
}
messages = calloc(max_messages, sizeof(PyObject*));
if (!messages) {
PyErr_NoMemory();
goto out;
}
/* parse messages */
p = buffer_start;
eod = buffer_start + buffer->len;
while (p+13 <= eod && message_count < max_messages) {
int message_len = -1;
uint64_t timestamp;
/* largest message we care about is:
* type 1 byte 0x05 = ADS-B
* spare 1 byte
* timestamp 3 bytes
* data 14 bytes
* total 19 bytes
*/
uint8_t data[19];
uint8_t *m;
int i;
uint8_t type;
uint32_t crc;
PyObject *message;
if (p[0] != 0x10 || p[1] != 0x02) {
error_pending = 1;
if (message_count > 0)
goto nomoredata;
PyErr_Format(PyExc_ValueError, "Lost sync with input stream: expected DLE STX at offset %d but found 0x%02x 0x%02x instead", (int) (p - buffer_start), (int)p[0], (int)p[1]);
goto out;
}
/* scan for DLE ETX, copy data */
m = p + 2;
i = 0;
while (m < eod) {
if (*m == 0x10) {
/* DLE <something> */
if ((m+1) >= eod)
goto nomoredata;
if (m[1] == 0x03) {
/* DLE ETX */
break;
}
if (m[1] != 0x10) {
/* DLE <something we don't understand> */
error_pending = 1;
if (message_count > 0)
goto nomoredata;
PyErr_Format(PyExc_ValueError, "Lost sync with input stream: unexpected DLE 0x%02x at offset %d", (int) (m - buffer_start), (int)m[1]);
goto out;
}
/* DLE DLE */
++m;
}
if (i < 19)
data[i++] = *m;
++m;
}
/* now pointing at DLE of DLE ETX */
m += 2;
/* first CRC byte */
if (m >= eod)
goto nomoredata;
if (*m++ == 0x10) {
if (m >= eod)
goto nomoredata;
if (m[0] != 0x10) {
error_pending = 1;
if (message_count > 0)
goto nomoredata;
PyErr_Format(PyExc_ValueError, "Lost sync with input stream: unexpected DLE 0x%02x at offset %d", (int) (m - buffer_start), (int)*m);
goto out;
}
++m;
}
/* second CRC byte */
if (m >= eod)
goto nomoredata;
if (*m++ == 0x10) {
if (m >= eod)
goto nomoredata;
if (m[0] != 0x10) {
error_pending = 1;
if (message_count > 0)
goto nomoredata;
PyErr_Format(PyExc_ValueError, "Lost sync with input stream: unexpected DLE 0x%02x at offset %d", (int) (m - buffer_start), (int)*m);
goto out;
}
++m;
}
/* try to make sense of the message */
type = data[0];
switch (type) {
case 0x01:
/* ADS-B or TIS-B */
message_len = 14;
break;
case 0x05:
/* Mode S, long */
message_len = 14;
break;
case 0x07:
/* Mode S, short */
message_len = 7;
break;
case 0x09:
/* Mode A/C */
message_len = 2;
break;
default:
/* something else, skip it */
p = m;
continue;
}
if ((5 + message_len) > i) {
/* not enough data */
p = m;
continue;
}
/* regenerate message CRC */
crc = modescrc_buffer_crc(&data[5], message_len - 3);
data[5 + message_len - 3] ^= crc >> 16;
data[5 + message_len - 2] ^= crc >> 8;
data[5 + message_len - 1] ^= crc;
/* little-endian, apparently */
timestamp = (data[4] << 16) | (data[3] << 8) | (data[2]);
/* Baseless speculation! Let's assume that it's like the Radarcape
* and measures at the end of the frame.
*
* It's easier to add to the timestamp than subtract from it, so
* add on enough of an offset so that the timestamps we report are
* consistently (start of frame + 112us) regardless of the actual
* frame length.
*/
timestamp = (timestamp + ((14-message_len) * 160)) & 0xFFFFFF;
/* we don't use timestamp_update or timestamp_check here because SBS is "special" */
/* The SBS timestamp is only 24 bits wide; at 20MHz this overflows more than once
* a second (about every 839ms). To get a useful timestamp for mlat synchronization,
* we have to widen the timestamp.
*
* It wasn't reliable to do this based on the system clock, there are enough
* unpredictable delays between the SBS and mlat-client that it didn't work well.
* Instead, we assume that we will be receiving at least one message per 839ms.
* so if we ever see a timestamp that has gone backwards, it must be due to
* exactly one overflow of the timestamp counter.
*
* This is usually true in cases where we see enough traffic for mlat/sync. When it
* isn't true, you will get synchronization jumps that are a multiple of 839ms.
*/
/* merge in top bits of the current widened counter */
timestamp = timestamp | (self->last_timestamp & 0xFFFFFFFFFF000000ULL);
/* check for rollover, if it happened then increase the widened part */
if (timestamp < self->last_timestamp)
timestamp += (1 << 24);
self->last_timestamp = timestamp;
/* decode it */
if (! (message = modesmessage_from_buffer(timestamp, 0, &data[5], message_len)))
goto out;
/* apply filters, update seen-set */
++self->received_messages;
int wanted = filter_message(self, message);
if (wanted < 0)
goto out;
else if (wanted)
messages[message_count++] = message;
else {
++self->suppressed_messages;
Py_DECREF(message);
}
p = m;
}
nomoredata:
if (! (message_tuple = PyTuple_New(message_count)))
goto out;
while (--message_count >= 0) {
PyTuple_SET_ITEM(message_tuple, message_count, messages[message_count]); /* steals ref */
}
rv = Py_BuildValue("(l,N,N)", (long) (p - buffer_start), message_tuple, PyBool_FromLong(error_pending));
out:
while (--message_count >= 0) {
Py_XDECREF(messages[message_count]);
}
free(messages);
return rv;
}
/* feed implementation for Beast-format data (including Radarcape) */
static PyObject *feed_beast(modesreader *self, Py_buffer *buffer, int max_messages)
{
PyObject *rv = NULL;
uint8_t *buffer_start, *p, *eod;
int message_count = 0;
PyObject *message_tuple = NULL;
PyObject **messages = NULL;
int error_pending = 0;
buffer_start = buffer->buf;
if (max_messages <= 0) {
/* allocate the maximum size we might need, given a minimal encoding of:
* <1A> <'1'> <6 bytes timestamp> <1 byte signal> <2 bytes message> = 11 bytes total
*/
max_messages = buffer->len / 11 + 2;
}
messages = calloc(max_messages, sizeof(PyObject*));
if (!messages) {
PyErr_NoMemory();
goto out;
}
/* parse messages */
p = buffer_start;
eod = buffer_start + buffer->len;
while (p+2 <= eod && message_count+2 < max_messages) {
int message_len = -1;
uint64_t timestamp;
uint8_t signal;
uint8_t data[14];
uint8_t *m, *eom;
int i;
uint8_t type;
PyObject *message;
int wanted;
if (p[0] != 0x1a) {
error_pending = 1;
if (message_count > 0)
goto nomoredata;
PyErr_Format(PyExc_ValueError, "Lost sync with input stream: expected a 0x1A marker at offset %d but found 0x%02x instead", (int) (p - buffer_start), (int)p[0]);
goto out;
}
type = p[1];
switch (type) {
case '1': message_len = 2; break; /* mode A/C */
case '2': message_len = 7; break; /* mode S short */
case '3': message_len = 14; break; /* mode S long */
case '4': message_len = 14; break; /* radarcape status message */
default:
error_pending = 1;
if (message_count > 0)
goto nomoredata;
PyErr_Format(PyExc_ValueError, "Lost sync with input stream: unexpected message type 0x%02x after 0x1A marker at offset %d", (int)p[1], (int) (p - buffer_start));
goto out;
}
m = p + 2;
eom = m + 7 + message_len;
if (eom > eod)
break;
#define ADVANCE \
do { \
if (*m++ == 0x1a) { \
if (m < eod && *m != 0x1a) { \
error_pending = 1; \
if (message_count > 0) \
goto nomoredata; \
PyErr_SetString(PyExc_ValueError, "Lost sync with input stream: expected 0x1A after 0x1A escape"); \
goto out; \
} \
++m, ++eom; \
if (eom > eod) \
goto nomoredata; \
} \
} while(0)
/* timestamp, 6 bytes */
timestamp = *m;
ADVANCE;
timestamp = (timestamp << 8) | *m;
ADVANCE;
timestamp = (timestamp << 8) | *m;
ADVANCE;
timestamp = (timestamp << 8) | *m;
ADVANCE;
timestamp = (timestamp << 8) | *m;
ADVANCE;
timestamp = (timestamp << 8) | *m;
ADVANCE;
/* signal, 1 byte */
signal = *m;
ADVANCE;
/* message, N bytes */
for (i = 0; i < message_len; ++i) {
data[i] = *m;
ADVANCE;
}
/* do some filtering */
if (type == '4') {
/* radarcape-style status message, use this to switch our decoder type */
self->radarcape_utc_bugfix = (data[2] & 0x80) == 0x80;
if (self->allow_mode_change) {
decoder_mode newmode;
if (data[0] & 0x10) {
/* radarcape in GPS timestamp mode */
if ((data[2] & 0x20) == 0x20) {
newmode = DECODER_RADARCAPE_EMULATED;
} else {
newmode = DECODER_RADARCAPE;
}
} else {
/* radarcape in 12MHz timestamp mode */
newmode = DECODER_BEAST;
}
/* handle mode changes by inserting an event message */
if (newmode != self->decoder_mode) {
set_decoder_mode(self, newmode);
if (self->want_events) {
if (! (messages[message_count++] = make_mode_change_event(self)))
goto out;
}
}
}
}
if (self->decoder_mode == DECODER_BEAST) {
/* 12MHz mode */
/* check for very out of range value
* (dump1090 can hold messages for up to 60 seconds! so be conservative here)
* also work around dump1090-mutability issue #47 which can send very stale Mode A/C messages
*/
if (self->want_events && type != '1' && !timestamp_check(self, timestamp)) {
if (! (messages[message_count++] = make_timestamp_jump_event(self, timestamp)))
goto out;
}
} else {
/* gps mode */
/* adjust timestamp so that it is a contiguous nanoseconds-since-
* midnight value, rather than the raw form which skips values once
* a second
*/
uint64_t nanos = timestamp & 0x00003FFFFFFF;
uint64_t secs = timestamp >> 30;
if (!self->radarcape_utc_bugfix) {
/* fix up the timestamp so it is UTC, not 1 second ahead */
--secs;
}
timestamp = nanos + secs * 1000000000;
/* adjust for the timestamp being at the _end_ of the frame;
* we don't really care about getting a particular starting point
* (that's just a fixed offset), so long as it is _the same in
* every frame_.
*
* (but don't do this for status messages as they have a timestamp
* as at the start of the message, which is basically the time of
* the PPS)
*/
if (type != '4') {
uint64_t adjust = (8000 + message_len * 8000); /* each byte takes 8us to transmit, plus 8us preamble */
if (adjust <= timestamp) {
timestamp = timestamp - adjust;
} else {
/* wrap it to the previous day */
timestamp = timestamp + 86400 * 1000000000ULL - adjust;
}
}
/* check for end of day rollover */
if (self->want_events && self->last_timestamp >= (86340 * 1000000000ULL) && timestamp <= (60 * 1000000000ULL)) {
if (! (messages[message_count++] = make_epoch_rollover_event(self, timestamp)))
goto out;
} else if (self->want_events && type != '1' && !timestamp_check(self, timestamp)) {
if (! (messages[message_count++] = make_timestamp_jump_event(self, timestamp)))
goto out;
}
}
if (type != '1') {
timestamp_update(self, timestamp);
}
if (type == '4') {
/* radarcape-style status message, emit the status event if wanted */
if (self->want_events) {
if (! (messages[message_count++] = make_radarcape_status_event(self, timestamp, data)))
goto out;
}
/* don't try to process this as a Mode S message */
p = m;
continue;
}
/* it's a Mode A/C or Mode S message, parse it */
if (! (message = modesmessage_from_buffer(timestamp, signal, data, message_len)))
goto out;
/* apply filters, update seen-set */
++self->received_messages;
wanted = filter_message(self, message);
if (wanted < 0)
goto out;
else if (wanted)
messages[message_count++] = message;
else {
++self->suppressed_messages;
Py_DECREF(message);
}
p = m;
}
nomoredata:
if (! (message_tuple = PyTuple_New(message_count)))
goto out;
while (--message_count >= 0) {
PyTuple_SET_ITEM(message_tuple, message_count, messages[message_count]); /* steals ref */
}
rv = Py_BuildValue("(l,N,N)", (long) (p - buffer_start), message_tuple, PyBool_FromLong(error_pending));
out:
while (--message_count >= 0) {
Py_XDECREF(messages[message_count]);
}
free(messages);
return rv;
}
static PyObject *feed_avr(modesreader *self, Py_buffer *buffer, int max_messages)
{
PyObject *rv = NULL;
uint8_t *buffer_start, *p, *eod;
int message_count = 0;
PyObject *message_tuple = NULL;
PyObject **messages = NULL;
int error_pending = 0;
buffer_start = buffer->buf;
if (max_messages <= 0) {
/* allocate the maximum size we might need, given a minimal encoding of:
* '*' <2 bytes message> ';' LF
*/
max_messages = buffer->len / 5 + 1;
}
messages = calloc(max_messages, sizeof(PyObject*));
if (!messages) {
PyErr_NoMemory();
goto out;
}
p = buffer_start;
eod = buffer_start + buffer->len;
while (p+17 <= eod && message_count+1 < max_messages) {
int message_len = -1;
uint64_t timestamp;
uint8_t data[14];
uint8_t message_format;
int i;
uint8_t *m;
PyObject *message;
message_format = p[0];
if (message_format != '@' &&
message_format != '%' &&
message_format != '<' &&
message_format != '*' &&
message_format != ':') {
error_pending = 1;
if (message_count > 0)
goto nomoredata;
PyErr_Format(PyExc_ValueError, "Lost sync with input stream: expected '@'/'%%'/'<'/'*'/':' at offset %d but found 0x%02x instead",
(int) (p - buffer_start), (int)p[0]);
goto out;
}
m = p + 1;
if (message_format == '@' ||
message_format == '%' ||
message_format == '<') {
/* read 6 bytes of timestamp */
timestamp = 0;
for (i = 0; i < 12; ++i, ++m) {
int c;
if (m >= eod) {
goto nomoredata;
}
timestamp <<= 4;
c = hexvalue(*m);
if (c >= 0) {
timestamp |= c;
} else {
error_pending = 1;
if (message_count > 0)
goto nomoredata;
PyErr_Format(PyExc_ValueError, "Lost sync with input stream: expected a hex digit at offset %d but found 0x%02x instead",
(int) (m - buffer_start), (int)*m);
goto out;
}
}
} else {
/* AVR format with no timestamp */
timestamp = 0;
}
if (message_format == '<') {
/* in format '<', skip 1 byte of signal */
m += 2;
if (m >= eod)
goto nomoredata;
}
/* read 2-14 bytes of data */
message_len = 0;
while (message_len < 14) {
int c0, c1;
if (m+1 >= eod) {
goto nomoredata;
}
if (m[0] == ';') {
break; /* end of message marker */
} else {
c0 = hexvalue(m[0]);
if (c0 < 0) {
error_pending = 1;
if (message_count > 0)
goto nomoredata;
PyErr_Format(PyExc_ValueError, "Lost sync with input stream: expected a hex digit at offset %d but found 0x%02x instead",
(int) (m - buffer_start), (int)m[0]);
goto out;
}
}
c1 = hexvalue(m[1]);
if (c1 < 0) {
error_pending = 1;
if (message_count > 0)
goto nomoredata;
PyErr_Format(PyExc_ValueError, "Lost sync with input stream: expected a hex digit at offset %d but found 0x%02x instead",
(int) (m - buffer_start), (int)m[1]);
goto out;
}
if (message_len < 14) {
data[message_len] = (c0 << 4) | c1;
}
++message_len;
m += 2;
}
/* consume ';' */
if (m >= eod)
goto nomoredata;
if (*m != ';') {
error_pending = 1;
if (message_count > 0)
goto nomoredata;
PyErr_Format(PyExc_ValueError, "Lost sync with input stream: expected ';' at offset %d but found 0x%02x instead",
(int) (m - buffer_start), (int)*m);
goto out;
}
/* CR LF, LF CR, LF all seen! ugh. */
/* skip until CR or LF */
while (m < eod && *m != '\r' && *m != '\n')
++m;
/* consume however many CRs and LFs */
while (m < eod && (*m == '\r' || *m == '\n'))
++m;
/* check length */
if (message_len != 2 && message_len != 7 && message_len != 14) {
error_pending = 1;
if (message_count > 0)
goto nomoredata;
PyErr_Format(PyExc_ValueError, "Lost sync with input stream: unexpected %d-byte message starting at offset %d",
message_len, (int) (p - buffer_start));
goto out;
}
/* check for very out of range value
* (dump1090 can hold messages for up to 60 seconds! so be conservative here)
* also work around dump1090-mutability issue #47 which can send very stale Mode A/C messages
*/
if (self->want_events && message_len != 2 && !timestamp_check(self, timestamp)) {
if (! (messages[message_count++] = make_timestamp_jump_event(self, timestamp)))
goto out;
}
timestamp_update(self, timestamp);
/* decode it */
if (! (message = modesmessage_from_buffer(timestamp, 0, data, message_len)))
goto out;
/* apply filters, update seen-set */
++self->received_messages;
int wanted = filter_message(self, message);
if (wanted < 0)
goto out;
else if (wanted)
messages[message_count++] = message;
else {
++self->suppressed_messages;
Py_DECREF(message);
}
/* next message */
p = m;
}
nomoredata:
if (! (message_tuple = PyTuple_New(message_count)))
goto out;
while (--message_count >= 0) {
PyTuple_SET_ITEM(message_tuple, message_count, messages[message_count]); /* steals ref */
}
rv = Py_BuildValue("(l,N,N)", (long) (p - buffer_start), message_tuple, PyBool_FromLong(error_pending));
out:
while (--message_count >= 0) {
Py_XDECREF(messages[message_count]);
}
free(messages);
return rv;
}