xan/patty
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

This SEEMS valid, but we'll compile later

Browse source
This commit is contained in:
XANTRONIX Development 2015-07-12 23:48:12 -05:00
parent facedcebb6
commit 4f4a898819
1 changed files with 39 additions and 7 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

46
src/kiss.c
View file

@ -76,42 +76,74 @@ 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_write(int fd, const void *buf, size_t len, int port) {
size_t i;
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 };
unsigned char command = (port & 0x0f) << 4;
if (write_byte(fd, PATTY_KISS_FEND) < 0) {
goto error_io;
}
if (write(fd, &command, 1) < 0) {
if (write_command(PATTY_KISS_DATA, port) < 0) {
goto error_io;
}
for (i=0; i<len; i++) {
unsigned char c = ((unsigned char *)buf)[i];
ssize_t res = 0;
unsigned char *escape = NULL;
switch (c) {
case PATTY_KISS_FEND: {
res = write(fd, escape_fend, sizeof(escape_fend)); break;
escape = escape_fend; break;
}
case PATTY_KISS_FESC: {
res = write(fd, escape_fesc, sizeof(escape_fesc)); break;
escape = escape_fesc; break;
}
default: {
end = i;
res = write(fd, &c, 1); break;
}
}
if (res < 0) {
if (escape) {
if (write(fd, ((unsigned char *)buf) + start, end - start) < 0) {
goto error_io;
}
if (write(fd, escape, 2) < 0) {
goto error_io;
}
escape = NULL;
start = i + 1;
end = start;
}
}
if (end - start) {
if (write(fd, ((unsigned char *)buf) + start, end - start) < 0) {
goto error_io;
}
}
return 0;
if (write_byte(fd, PATTY_KISS_FEND) < 0) {
goto error_io;
}
return len;
error_io:
return -1;