diff --git a/include/patty/kiss.h b/include/patty/kiss.h index 2fb34bc..b7ea23a 100644 --- a/include/patty/kiss.h +++ b/include/patty/kiss.h @@ -27,6 +27,11 @@ enum patty_kiss_command { PATTY_KISS_RETURN = 0xff }; +ssize_t patty_kiss_frame_send(int fd, + const void *buf, + size_t len, + int port); + typedef struct _patty_kiss_tnc patty_kiss_tnc; patty_kiss_tnc *patty_kiss_tnc_new_fd(int fd); diff --git a/src/kiss.c b/src/kiss.c index 3dedd43..fb6502b 100644 --- a/src/kiss.c +++ b/src/kiss.c @@ -42,6 +42,88 @@ struct _patty_kiss_tnc { ssize_t readlen; }; +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_frame_send(int fd, + const void *buf, + size_t len, + int port) { + size_t i, start = 0, end = 0; + + if (write_start(fd, PATTY_KISS_DATA, port) < 0) { + goto error_io; + } + + for (i=0; ifd, PATTY_KISS_DATA, port) < 0) { - goto error_io; - } - - for (i=0; ifd, ((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; - } - - if (tcdrain(tnc->fd) < 0) { - goto error_io; - } - - return len; - -error_io: - return -1; + return patty_kiss_frame_send(tnc->fd, buf, len, port); }