341 lines
7.3 KiB
C
341 lines
7.3 KiB
C
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
#include <sys/uio.h>
|
|
#include <errno.h>
|
|
|
|
#include <patty/kiss.h>
|
|
|
|
enum kiss_flags {
|
|
KISS_NONE = 0x00,
|
|
KISS_FRAME = 0x01,
|
|
KISS_ESCAPE = 0x02
|
|
};
|
|
|
|
struct _patty_kiss_tnc {
|
|
int fd;
|
|
int dropped;
|
|
void * frame;
|
|
void * buf;
|
|
size_t bufsz;
|
|
size_t buflen;
|
|
};
|
|
|
|
patty_kiss_tnc *patty_kiss_tnc_open(const char *device, size_t bufsz) {
|
|
patty_kiss_tnc *tnc;
|
|
|
|
if ((tnc = malloc(sizeof(*tnc))) == NULL) {
|
|
goto error_malloc_tnc;
|
|
}
|
|
|
|
if ((tnc->buf = malloc(bufsz)) == NULL) {
|
|
goto error_malloc_buf;
|
|
}
|
|
|
|
if ((tnc->frame = malloc(bufsz)) == NULL) {
|
|
goto error_malloc_frame;
|
|
}
|
|
|
|
if ((tnc->fd = open(device, O_RDWR)) < 0) {
|
|
goto error_open;
|
|
}
|
|
|
|
tnc->dropped = 0;
|
|
tnc->bufsz = bufsz;
|
|
tnc->buflen = 0;
|
|
|
|
return tnc;
|
|
|
|
error_open:
|
|
free(tnc->frame);
|
|
|
|
error_malloc_frame:
|
|
free(tnc->buf);
|
|
|
|
error_malloc_buf:
|
|
free(tnc);
|
|
|
|
error_malloc_tnc:
|
|
return NULL;
|
|
}
|
|
|
|
void patty_kiss_tnc_close(patty_kiss_tnc *tnc) {
|
|
close(tnc->fd);
|
|
free(tnc->frame);
|
|
free(tnc->buf);
|
|
free(tnc);
|
|
}
|
|
|
|
static ssize_t tnc_buffer(patty_kiss_tnc *tnc) {
|
|
size_t fillsz = tnc->bufsz - tnc->buflen;
|
|
ssize_t readlen = 0;
|
|
|
|
/*
|
|
* If the buffer needs to be filled, then fill it.
|
|
*/
|
|
if (fillsz) {
|
|
void *dest = ((unsigned char *)tnc->buf) + tnc->buflen;
|
|
|
|
if ((readlen = read(tnc->fd, dest, fillsz)) < 0) {
|
|
errno = EIO;
|
|
|
|
goto error_io;
|
|
}
|
|
|
|
tnc->buflen += readlen;
|
|
}
|
|
|
|
return readlen;
|
|
|
|
error_io:
|
|
return -1;
|
|
}
|
|
|
|
static ssize_t tnc_decode(patty_kiss_tnc *tnc, void *frame, size_t *len, int *port) {
|
|
size_t i, b;
|
|
int flags = KISS_NONE;
|
|
|
|
/*
|
|
* The default port is always zero, unless a command byte issued from the
|
|
* TNC in the first byte of the frame body.
|
|
*/
|
|
*port = 0;
|
|
|
|
/*
|
|
* Ah, yes, the icky state machine wherein we unescape things off the wire
|
|
* and whatnot.
|
|
*/
|
|
for (i=0, b=0; i<tnc->buflen; i++) {
|
|
unsigned char c = ((unsigned char *)tnc->buf)[i];
|
|
|
|
/*
|
|
* If the first byte is not a frame end, then that's a bad thing.
|
|
*/
|
|
if (i == 0 && c != PATTY_KISS_FEND) {
|
|
errno = EIO;
|
|
|
|
goto error_io;
|
|
}
|
|
|
|
/*
|
|
* Not all KISS TNCs will emit a type byte at the start of a frame
|
|
* to the host; if the low nybble has a value of zero, then presume
|
|
* the high nybble to be the radio port number from which the
|
|
* frame originated.
|
|
*/
|
|
if (i == 1 && (c & 0x0f) == 0) {
|
|
*port = (c & 0xf0) >> 4;
|
|
|
|
continue;
|
|
}
|
|
|
|
if ((flags & KISS_FRAME) == 0) {
|
|
if (c == PATTY_KISS_FEND) {
|
|
flags |= KISS_FRAME;
|
|
|
|
continue;
|
|
}
|
|
|
|
errno = EIO;
|
|
|
|
goto error_io;
|
|
}
|
|
|
|
if (flags & KISS_ESCAPE) {
|
|
if (c == PATTY_KISS_TFEND) {
|
|
((unsigned char *)frame)[b++] = PATTY_KISS_FEND;
|
|
} else if (c == PATTY_KISS_TFESC) {
|
|
((unsigned char *)frame)[b++] = PATTY_KISS_FESC;
|
|
} else {
|
|
errno = EIO;
|
|
|
|
goto error_io;
|
|
}
|
|
|
|
flags &= ~KISS_ESCAPE;
|
|
|
|
continue;
|
|
}
|
|
|
|
if (flags & KISS_FRAME) {
|
|
if (c == PATTY_KISS_FESC) {
|
|
flags |= KISS_ESCAPE;
|
|
|
|
continue;
|
|
} else if (c == PATTY_KISS_FEND) {
|
|
break;
|
|
}
|
|
|
|
((unsigned char *)frame)[b++] = c;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Return empty-handed if we do not have a fully book-ended frame.
|
|
*/
|
|
if ((flags & KISS_FRAME) == 0) {
|
|
*len = 0;
|
|
*port = 0;
|
|
|
|
return 0;
|
|
}
|
|
|
|
*len = b;
|
|
|
|
return i;
|
|
|
|
error_io:
|
|
return -1;
|
|
}
|
|
|
|
static void tnc_drop(patty_kiss_tnc *tnc) {
|
|
memset(tnc->buf, '\0', tnc->bufsz);
|
|
memset(tnc->frame, '\0', tnc->bufsz);
|
|
|
|
tnc->buflen = 0;
|
|
tnc->dropped++;
|
|
}
|
|
|
|
int patty_kiss_tnc_dropped(patty_kiss_tnc *tnc) {
|
|
return tnc->dropped;
|
|
}
|
|
|
|
static void tnc_flush(patty_kiss_tnc *tnc, size_t len) {
|
|
/*
|
|
* Move everything from the buffer not processed up to this point, to the
|
|
* beginning of the buffer.
|
|
*/
|
|
memmove(tnc->buf, ((unsigned char *)tnc->buf) + len, tnc->buflen - len);
|
|
|
|
/*
|
|
* Then, decrement the buffer length by the number of bytes already
|
|
* processed.
|
|
*/
|
|
tnc->buflen -= len;
|
|
}
|
|
|
|
ssize_t patty_kiss_tnc_recv(patty_kiss_tnc *tnc, void **frame, int *port) {
|
|
ssize_t buffered, decoded;
|
|
size_t framelen;
|
|
|
|
/*
|
|
* Initialize the frame to be returned to the caller.
|
|
*/
|
|
memset(tnc->frame, '\0', tnc->bufsz);
|
|
|
|
do {
|
|
/*
|
|
* Fill the buffer with something to parse.
|
|
*/
|
|
if ((buffered = tnc_buffer(tnc)) < 0) {
|
|
goto error_io;
|
|
}
|
|
|
|
/*
|
|
* Try to decode the TNC buffer into the frame.
|
|
*/
|
|
if ((decoded = tnc_decode(tnc, tnc->frame, &framelen, port)) < 0) {
|
|
goto error_io;
|
|
}
|
|
|
|
/*
|
|
* If we can no longer buffer nor decode any data, then drop the whole
|
|
* frame.
|
|
*/
|
|
if (decoded == 0 && buffered == 0) {
|
|
tnc_drop(tnc);
|
|
}
|
|
} while (decoded == 0);
|
|
|
|
/*
|
|
* Flush the buffer up to the point of the next frame start.
|
|
*/
|
|
tnc_flush(tnc, decoded);
|
|
|
|
*frame = tnc->frame;
|
|
|
|
return framelen;
|
|
|
|
error_io:
|
|
return -1;
|
|
}
|
|
|
|
static inline ssize_t write_byte(int fd, unsigned char c) {
|
|
return write(fd, &c, sizeof(c));
|
|
}
|
|
|
|
static inline ssize_t write_command(int fd, int command, int port) {
|
|
return write_byte(fd, ((port & 0x0f) << 4) | (command & 0x0f));
|
|
}
|
|
|
|
ssize_t patty_kiss_tnc_send(patty_kiss_tnc *tnc, const void *buf, size_t len, int port) {
|
|
size_t i, start = 0, end = 0;
|
|
|
|
unsigned char escape_fend[2] = { PATTY_KISS_FESC, PATTY_KISS_TFEND };
|
|
unsigned char escape_fesc[2] = { PATTY_KISS_FESC, PATTY_KISS_TFESC };
|
|
|
|
if (write_byte(tnc->fd, PATTY_KISS_FEND) < 0) {
|
|
goto error_io;
|
|
}
|
|
|
|
if (write_command(tnc->fd, PATTY_KISS_DATA, port) < 0) {
|
|
goto error_io;
|
|
}
|
|
|
|
for (i=0; i<len; i++) {
|
|
unsigned char c = ((unsigned char *)buf)[i];
|
|
unsigned char *escape = NULL;
|
|
|
|
switch (c) {
|
|
case PATTY_KISS_FEND: {
|
|
escape = escape_fend; break;
|
|
}
|
|
|
|
case PATTY_KISS_FESC: {
|
|
escape = escape_fesc; break;
|
|
}
|
|
|
|
default: {
|
|
end = i;
|
|
|
|
if (write(tnc->fd, &c, 1) < 0) {
|
|
goto error_io;
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (escape) {
|
|
if (write(tnc->fd, ((unsigned char *)buf) + start, end - start) < 0) {
|
|
goto error_io;
|
|
}
|
|
|
|
if (write(tnc->fd, escape, 2) < 0) {
|
|
goto error_io;
|
|
}
|
|
|
|
escape = NULL;
|
|
start = i + 1;
|
|
end = start;
|
|
}
|
|
}
|
|
|
|
if (end - start) {
|
|
if (write(tnc->fd, ((unsigned char *)buf) + start, end - start) < 0) {
|
|
goto error_io;
|
|
}
|
|
}
|
|
|
|
if (write_byte(tnc->fd, PATTY_KISS_FEND) < 0) {
|
|
goto error_io;
|
|
}
|
|
|
|
return len;
|
|
|
|
error_io:
|
|
return -1;
|
|
}
|