358 lines
7.2 KiB
C
358 lines
7.2 KiB
C
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <inttypes.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
#include <sys/socket.h>
|
|
#include <sys/un.h>
|
|
#include <errno.h>
|
|
|
|
#include <patty/kiss.h>
|
|
|
|
enum kiss_flags {
|
|
KISS_NONE = 0,
|
|
KISS_FRAME = 1 << 0,
|
|
KISS_COMMAND = 1 << 1,
|
|
KISS_ESCAPE = 1 << 2
|
|
};
|
|
|
|
enum tnc_opts {
|
|
TNC_NONE = 0,
|
|
TNC_CLOSE_ON_DESTROY = 1 << 0
|
|
};
|
|
|
|
struct _patty_kiss_tnc {
|
|
int fd,
|
|
opts;
|
|
|
|
void *buf,
|
|
*frame;
|
|
|
|
size_t bufsz,
|
|
offset,
|
|
dropped;
|
|
|
|
ssize_t readlen;
|
|
};
|
|
|
|
patty_kiss_tnc *patty_kiss_tnc_new_fd(int fd) {
|
|
patty_kiss_tnc *tnc;
|
|
|
|
if ((tnc = malloc(sizeof(*tnc))) == NULL) {
|
|
goto error_malloc_tnc;
|
|
}
|
|
|
|
if ((tnc->buf = malloc(PATTY_KISS_BUFSZ)) == NULL) {
|
|
goto error_malloc_buf;
|
|
}
|
|
|
|
tnc->fd = fd;
|
|
tnc->opts = TNC_NONE;
|
|
tnc->bufsz = PATTY_KISS_BUFSZ;
|
|
tnc->offset = 0;
|
|
tnc->dropped = 0;
|
|
tnc->readlen = -1;
|
|
|
|
return tnc;
|
|
|
|
error_malloc_buf:
|
|
free(tnc);
|
|
|
|
error_malloc_tnc:
|
|
return NULL;
|
|
}
|
|
|
|
patty_kiss_tnc *patty_kiss_tnc_new(const char *device) {
|
|
patty_kiss_tnc *tnc;
|
|
int fd;
|
|
struct stat st;
|
|
|
|
if (stat(device, &st) < 0) {
|
|
goto error_stat;
|
|
}
|
|
|
|
if ((st.st_mode & S_IFMT) == S_IFSOCK) {
|
|
struct sockaddr_un addr;
|
|
|
|
if (strlen(device) > sizeof(addr.sun_path)) {
|
|
errno = EOVERFLOW;
|
|
|
|
goto error_overflow;
|
|
}
|
|
|
|
if ((fd = socket(PF_UNIX, SOCK_STREAM, 0)) < 0) {
|
|
goto error_socket;
|
|
}
|
|
|
|
memset(&addr, '\0', sizeof(addr));
|
|
addr.sun_family = AF_UNIX;
|
|
strncpy(addr.sun_path, device, sizeof(addr.sun_path));
|
|
|
|
if (connect(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
|
goto error_connect;
|
|
}
|
|
} else {
|
|
if ((fd = open(device, O_RDWR)) < 0) {
|
|
goto error_open;
|
|
}
|
|
}
|
|
|
|
if ((tnc = patty_kiss_tnc_new_fd(fd)) == NULL) {
|
|
goto error_tnc_new_fd;
|
|
}
|
|
|
|
tnc->opts |= TNC_CLOSE_ON_DESTROY;
|
|
|
|
return tnc;
|
|
|
|
error_tnc_new_fd:
|
|
error_connect:
|
|
close(fd);
|
|
|
|
error_socket:
|
|
error_overflow:
|
|
error_open:
|
|
error_stat:
|
|
return NULL;
|
|
}
|
|
|
|
int patty_kiss_tnc_fd(patty_kiss_tnc *tnc) {
|
|
return tnc->fd;
|
|
}
|
|
|
|
void patty_kiss_tnc_destroy(patty_kiss_tnc *tnc) {
|
|
if (tnc->opts & TNC_CLOSE_ON_DESTROY) {
|
|
close(tnc->fd);
|
|
}
|
|
|
|
free(tnc->buf);
|
|
free(tnc);
|
|
}
|
|
|
|
static void tnc_drop(patty_kiss_tnc *tnc) {
|
|
tnc->offset = 0;
|
|
tnc->readlen = -1;
|
|
tnc->dropped++;
|
|
}
|
|
|
|
size_t patty_kiss_tnc_dropped(patty_kiss_tnc *tnc) {
|
|
return tnc->dropped;
|
|
}
|
|
|
|
ssize_t patty_kiss_tnc_pending(patty_kiss_tnc *tnc) {
|
|
if (tnc->readlen < 0) {
|
|
return -1;
|
|
}
|
|
|
|
if (tnc->offset == 0) {
|
|
return 0;
|
|
}
|
|
|
|
return tnc->readlen - tnc->offset;
|
|
}
|
|
|
|
ssize_t patty_kiss_tnc_recv(patty_kiss_tnc *tnc,
|
|
void *buf,
|
|
size_t len,
|
|
int *port) {
|
|
size_t r = 0, /* Number of bytes read */
|
|
w = 0; /* Number of bytes written to buf */
|
|
|
|
enum kiss_flags flags = KISS_NONE;
|
|
|
|
if (tnc->offset == tnc->readlen) {
|
|
errno = 0;
|
|
|
|
return 0;
|
|
}
|
|
|
|
while (1) {
|
|
uint8_t c;
|
|
|
|
r++;
|
|
|
|
if (w == len) {
|
|
tnc_drop(tnc);
|
|
|
|
flags &= ~KISS_FRAME;
|
|
w = 0;
|
|
}
|
|
|
|
if (tnc->offset == 0) {
|
|
if ((tnc->readlen = read(tnc->fd, tnc->buf, tnc->bufsz)) < 0) {
|
|
goto error_io;
|
|
} else if (tnc->readlen == 0) {
|
|
if (errno) {
|
|
goto error_io;
|
|
}
|
|
|
|
goto done;
|
|
}
|
|
}
|
|
|
|
c = ((uint8_t *)tnc->buf)[tnc->offset++];
|
|
|
|
if (tnc->offset == tnc->readlen) {
|
|
tnc->offset = 0;
|
|
}
|
|
|
|
if (!(flags & KISS_FRAME)) {
|
|
if (c == PATTY_KISS_FEND) {
|
|
flags |= KISS_FRAME;
|
|
|
|
continue;
|
|
} else {
|
|
errno = EIO;
|
|
|
|
goto error_io;
|
|
}
|
|
} else {
|
|
if (c == PATTY_KISS_FEND) {
|
|
if (w > 0) {
|
|
flags &= ~KISS_FRAME;
|
|
|
|
goto done;
|
|
}
|
|
|
|
continue;
|
|
}
|
|
}
|
|
|
|
if (!(flags & KISS_COMMAND)) {
|
|
if (PATTY_KISS_COMMAND(c) != PATTY_KISS_DATA) {
|
|
errno = EIO;
|
|
|
|
goto error_io;
|
|
}
|
|
|
|
if (port) {
|
|
*port = PATTY_KISS_COMMAND_PORT(c);
|
|
}
|
|
|
|
flags |= KISS_COMMAND;
|
|
|
|
continue;
|
|
}
|
|
|
|
if (!(flags & KISS_ESCAPE)) {
|
|
if (c == PATTY_KISS_FESC) {
|
|
flags |= KISS_ESCAPE;
|
|
|
|
continue;
|
|
}
|
|
} else {
|
|
switch (c) {
|
|
case PATTY_KISS_TFEND:
|
|
((uint8_t *)buf)[w++] = PATTY_KISS_FEND;
|
|
flags &= ~KISS_ESCAPE;
|
|
continue;
|
|
|
|
case PATTY_KISS_TFESC:
|
|
((uint8_t *)buf)[w++] = PATTY_KISS_FESC;
|
|
flags &= ~KISS_ESCAPE;
|
|
continue;
|
|
|
|
default:
|
|
errno = EIO;
|
|
|
|
goto error_io;
|
|
}
|
|
}
|
|
|
|
((uint8_t *)buf)[w++] = c;
|
|
}
|
|
|
|
done:
|
|
if (flags & KISS_FRAME) {
|
|
tnc_drop(tnc);
|
|
|
|
errno = 0;
|
|
|
|
return 0;
|
|
}
|
|
|
|
return (ssize_t)w;
|
|
|
|
error_io:
|
|
return -1;
|
|
}
|
|
|
|
static inline ssize_t write_byte(int fd, uint8_t c) {
|
|
return write(fd, &c, sizeof(c));
|
|
}
|
|
|
|
static inline ssize_t write_start(int fd,
|
|
enum patty_kiss_command command,
|
|
int port) {
|
|
uint8_t start[2] = {
|
|
PATTY_KISS_FEND,
|
|
((port & 0x0f) << 4) | (command & 0x0f)
|
|
};
|
|
|
|
return write(fd, start, sizeof(start));
|
|
}
|
|
|
|
static uint8_t escape_fend[2] = { PATTY_KISS_FESC, PATTY_KISS_TFEND };
|
|
static uint8_t escape_fesc[2] = { PATTY_KISS_FESC, PATTY_KISS_TFESC };
|
|
|
|
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;
|
|
|
|
if (write_start(tnc->fd, PATTY_KISS_DATA, port) < 0) {
|
|
goto error_io;
|
|
}
|
|
|
|
for (i=0; i<len; i++) {
|
|
uint8_t c = ((uint8_t *)buf)[i];
|
|
uint8_t *escape = NULL;
|
|
|
|
switch (c) {
|
|
case PATTY_KISS_FEND:
|
|
escape = escape_fend;
|
|
break;
|
|
|
|
case PATTY_KISS_FESC:
|
|
escape = escape_fesc;
|
|
break;
|
|
|
|
default:
|
|
end = i + 1;
|
|
|
|
break;
|
|
}
|
|
|
|
if (escape) {
|
|
if (write(tnc->fd, ((uint8_t *)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, ((uint8_t *)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;
|
|
}
|