370 lines
8.1 KiB
C
370 lines
8.1 KiB
C
|
|
/*
|
|
* protodec.c
|
|
*
|
|
* (c) Ruben Undheim 2008
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 2 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program; if not, write to the Free Software
|
|
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <time.h>
|
|
#include <string.h> /* String function definitions */
|
|
#include "callbacks.h"
|
|
#include "config.h"
|
|
|
|
#include "protodec.h"
|
|
#include "hmalloc.h"
|
|
|
|
decoder_on_nmea_sentence_received on_nmea_sentence_received=NULL;
|
|
|
|
#ifdef DMALLOC
|
|
#include <dmalloc.h>
|
|
#endif
|
|
|
|
void protodec_initialize(struct demod_state_t *d, struct serial_state_t *serial, char chanid)
|
|
{
|
|
memset(d, 0, sizeof(struct demod_state_t));
|
|
|
|
d->chanid = chanid;
|
|
d->serial = serial;
|
|
|
|
d->receivedframes = 0;
|
|
d->lostframes = 0;
|
|
d->lostframes2 = 0;
|
|
|
|
protodec_reset(d);
|
|
|
|
d->seqnr = 0;
|
|
|
|
d->buffer = hmalloc(DEMOD_BUFFER_LEN);
|
|
d->rbuffer = hmalloc(DEMOD_BUFFER_LEN);
|
|
d->nmea = hmalloc(NMEABUFFER_LEN);
|
|
}
|
|
|
|
void protodec_deinit(struct demod_state_t *d)
|
|
{
|
|
hfree(d->buffer);
|
|
hfree(d->rbuffer);
|
|
hfree(d->nmea);
|
|
}
|
|
|
|
void protodec_reset(struct demod_state_t *d)
|
|
{
|
|
d->state = ST_SKURR;
|
|
d->nskurr = 0;
|
|
d->ndata = 0;
|
|
d->npreamble = 0;
|
|
d->nstartsign = 0;
|
|
d->nstopsign = 0;
|
|
d->antallpreamble = 0;
|
|
d->antallenner = 0;
|
|
d->last = 0;
|
|
d->bitstuff = 0;
|
|
d->bufferpos = 0;
|
|
}
|
|
|
|
/*
|
|
* Calculates CRC-checksum
|
|
*/
|
|
|
|
unsigned short protodec_sdlc_crc(const unsigned char *data, unsigned len)
|
|
{
|
|
unsigned short c, crc = 0xffff;
|
|
|
|
while (len--)
|
|
for (c = 0x100 + *data++; c > 1; c >>= 1)
|
|
if ((crc ^ c) & 1)
|
|
crc = (crc >> 1) ^ 0x8408;
|
|
else
|
|
crc >>= 1;
|
|
return ~crc;
|
|
|
|
}
|
|
|
|
int protodec_calculate_crc(int length_bits, struct demod_state_t *d)
|
|
{
|
|
int length_bytes;
|
|
unsigned char *buf;
|
|
int buflen;
|
|
int i, j, x;
|
|
unsigned char tmp;
|
|
|
|
if (length_bits <= 0) {
|
|
return 0;
|
|
}
|
|
|
|
length_bytes = length_bits / 8;
|
|
buflen = length_bytes + 2;
|
|
|
|
/* what is this? */
|
|
buf = (unsigned char *) hmalloc(sizeof(*buf) * buflen);
|
|
for (j = 0; j < buflen; j++) {
|
|
tmp = 0;
|
|
for (i = 0; i < 8; i++)
|
|
tmp |= (((d->buffer[i + 8 * j]) << (i)));
|
|
buf[j] = tmp;
|
|
}
|
|
|
|
/* ok, here's the actual CRC calculation */
|
|
unsigned short crc = protodec_sdlc_crc(buf, buflen);
|
|
//DBG(printf("CRC: %04x\n",crc));
|
|
|
|
/* what is this? */
|
|
memset(d->rbuffer, 0, DEMOD_BUFFER_LEN);
|
|
for (j = 0; j < length_bytes; j++) {
|
|
for (i = 0; i < 8; i++) {
|
|
x = j * 8 + i;
|
|
if (x >= DEMOD_BUFFER_LEN) {
|
|
hfree(buf);
|
|
return 0;
|
|
} else {
|
|
d->rbuffer[x] = (buf[j] >> (7 - i)) & 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
hfree(buf);
|
|
|
|
return (crc == 0x0f47);
|
|
}
|
|
|
|
unsigned long protodec_henten(int from, int size, unsigned char *frame)
|
|
{
|
|
int i = 0;
|
|
unsigned long tmp = 0;
|
|
|
|
for (i = 0; i < size; i++)
|
|
tmp |= (frame[from + i]) << (size - 1 - i);
|
|
|
|
return tmp;
|
|
}
|
|
|
|
|
|
void protodec_generate_nmea(struct demod_state_t *d, int bufferlen, int fillbits, time_t received_t)
|
|
{
|
|
int senlen;
|
|
int pos;
|
|
int k, offset;
|
|
int m;
|
|
unsigned char sentences, sentencenum, nmeachk, letter;
|
|
|
|
//6bits to nmea-ascii. One sentence len max 82char
|
|
//inc. head + tail.This makes inside datamax 62char multipart, 62 single
|
|
senlen = 56; //this is normally not needed.For testing only. May be fixed number
|
|
if (bufferlen <= (senlen * 6)) {
|
|
sentences = 1;
|
|
} else {
|
|
sentences = bufferlen / (senlen * 6);
|
|
//sentences , if overflow put one more
|
|
if (bufferlen % (senlen * 6) != 0)
|
|
sentences++;
|
|
};
|
|
|
|
sentencenum = 0;
|
|
pos = 0;
|
|
offset = (sentences>1) ? 15 : 14;
|
|
do {
|
|
k = offset; //leave room for nmea header
|
|
while (k < senlen + offset && bufferlen > pos) {
|
|
letter = (unsigned char)protodec_henten(pos, 6, d->rbuffer);
|
|
// 6bit-to-ascii conversion by IEC
|
|
letter += (letter < 40) ? 48 : 56;
|
|
d->nmea[k] = letter;
|
|
pos += 6;
|
|
k++;
|
|
}
|
|
sentencenum++;
|
|
|
|
memcpy(&d->nmea[0], "!AIVDM,0,0,", 11);
|
|
d->nmea[7] += sentences;
|
|
d->nmea[9] += sentencenum;
|
|
|
|
memcpy(&d->nmea[k], ",0*00\0", 6);
|
|
|
|
if (sentences > 1) {
|
|
d->nmea[11] = '0' + d->seqnr;
|
|
d->nmea[12] = ',';
|
|
d->nmea[13] = d->chanid;
|
|
d->nmea[14] = ',';
|
|
if (sentencenum == sentences) d->nmea[k + 1] = '0' + fillbits;
|
|
} else {
|
|
d->nmea[11] = ',';
|
|
d->nmea[12] = d->chanid;
|
|
d->nmea[13] = ',';
|
|
}
|
|
|
|
m = 1;
|
|
nmeachk = d->nmea[m++];
|
|
while (d->nmea[m] != '*') nmeachk ^= d->nmea[m++];
|
|
|
|
sprintf(&d->nmea[k + 3], "%02X\r\n", nmeachk);
|
|
if (on_nmea_sentence_received != NULL)
|
|
on_nmea_sentence_received(d->nmea, k+7, sentences, sentencenum);
|
|
} while (sentencenum < sentences);
|
|
}
|
|
|
|
void protodec_getdata(int bufferlen, struct demod_state_t *d)
|
|
{
|
|
unsigned char type = protodec_henten(0, 6, d->rbuffer);
|
|
if (type < 1 || type > MAX_AIS_PACKET_TYPE /* 9 */)
|
|
return;
|
|
// unsigned long mmsi = protodec_henten(8, 30, d->rbuffer);
|
|
int fillbits = 0;
|
|
int k;
|
|
time_t received_t;
|
|
time(&received_t);
|
|
|
|
if (bufferlen % 6 > 0) {
|
|
fillbits = 6 - (bufferlen % 6);
|
|
for (k = bufferlen; k < bufferlen + fillbits; k++)
|
|
d->rbuffer[k] = 0;
|
|
|
|
bufferlen = bufferlen + fillbits;
|
|
}
|
|
|
|
protodec_generate_nmea(d, bufferlen, fillbits, received_t);
|
|
|
|
d->seqnr++;
|
|
if (d->seqnr > 9)
|
|
d->seqnr = 0;
|
|
|
|
if (type < 1 || type > MAX_AIS_PACKET_TYPE)
|
|
return; // unsupported packet type
|
|
}
|
|
|
|
void protodec_decode(char *in, int count, struct demod_state_t *d)
|
|
{
|
|
int i = 0;
|
|
int bufferlength, correct;
|
|
|
|
while (i < count) {
|
|
switch (d->state) {
|
|
case ST_DATA:
|
|
if (d->bitstuff) {
|
|
if (in[i] == 1) {
|
|
d->state = ST_STOPSIGN;
|
|
d->ndata = 0;
|
|
d->bitstuff = 0;
|
|
} else {
|
|
d->ndata++;
|
|
d->last = in[i];
|
|
d->bitstuff = 0;
|
|
}
|
|
} else {
|
|
if (in[i] == d->last && in[i] == 1) {
|
|
d->antallenner++;
|
|
if (d->antallenner == 4) {
|
|
d->bitstuff = 1;
|
|
d->antallenner = 0;
|
|
}
|
|
|
|
} else
|
|
d->antallenner = 0;
|
|
|
|
d->buffer[d->bufferpos] = in[i];
|
|
d->bufferpos++;
|
|
d->ndata++;
|
|
|
|
if (d->bufferpos >= 449) {
|
|
protodec_reset(d);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case ST_SKURR:
|
|
if (in[i] != d->last)
|
|
d->antallpreamble++;
|
|
else
|
|
d->antallpreamble = 0;
|
|
d->last = in[i];
|
|
if (d->antallpreamble > 14 && in[i] == 0) {
|
|
d->state = ST_PREAMBLE;
|
|
d->nskurr = 0;
|
|
d->antallpreamble = 0;
|
|
}
|
|
d->nskurr++;
|
|
break;
|
|
|
|
case ST_PREAMBLE:
|
|
if (in[i] != d->last && d->nstartsign == 0) {
|
|
d->antallpreamble++;
|
|
} else {
|
|
if (in[i] == 1) {
|
|
if (d->nstartsign == 0) {
|
|
d->nstartsign = 3;
|
|
d->last = in[i];
|
|
} else if (d->nstartsign == 5) {
|
|
d->nstartsign++;
|
|
d->npreamble = 0;
|
|
d->antallpreamble = 0;
|
|
d->state = ST_STARTSIGN;
|
|
} else {
|
|
d->nstartsign++;
|
|
}
|
|
|
|
} else {
|
|
if (d->nstartsign == 0) {
|
|
d->nstartsign = 1;
|
|
} else {
|
|
protodec_reset(d);
|
|
}
|
|
}
|
|
}
|
|
d->npreamble++;
|
|
break;
|
|
|
|
case ST_STARTSIGN:
|
|
if (d->nstartsign >= 7) {
|
|
if (in[i] == 0) {
|
|
d->state = ST_DATA;
|
|
d->nstartsign = 0;
|
|
d->antallenner = 0;
|
|
memset(d->buffer, 0, DEMOD_BUFFER_LEN);
|
|
d->bufferpos = 0;
|
|
} else {
|
|
protodec_reset(d);
|
|
}
|
|
|
|
} else if (in[i] == 0) {
|
|
protodec_reset(d);
|
|
}
|
|
d->nstartsign++;
|
|
break;
|
|
|
|
case ST_STOPSIGN:
|
|
bufferlength = d->bufferpos - 6 - 16;
|
|
if (in[i] == 0 && bufferlength > 0) {
|
|
correct = protodec_calculate_crc(bufferlength, d);
|
|
if (correct) {
|
|
d->receivedframes++;
|
|
protodec_getdata(bufferlength, d);
|
|
} else {
|
|
d->lostframes++;
|
|
}
|
|
} else {
|
|
d->lostframes2++;
|
|
}
|
|
protodec_reset(d);
|
|
break;
|
|
|
|
|
|
}
|
|
d->last = in[i];
|
|
i++;
|
|
}
|
|
}
|
|
|