patty/src/kiss.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/uio.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
};

struct _patty_kiss_tnc {
    int fd;

    void *buf,
         *frame;

    size_t bufsz,
           buflen,
           dropped;
};

patty_kiss_tnc *patty_kiss_tnc_open_fd(int fd, 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;
    }

    tnc->fd      = fd;
    tnc->bufsz   = bufsz;
    tnc->buflen  = 0;
    tnc->dropped = 0;

    return tnc;

error_malloc_frame:
    free(tnc->buf);

error_malloc_buf:
    free(tnc);

error_malloc_tnc:
    return NULL;
}

patty_kiss_tnc *patty_kiss_tnc_open(const char *device, size_t bufsz) {
    patty_kiss_tnc *tnc;
    int fd;

    if ((fd = open(device, O_RDWR)) < 0) {
        goto error_open;
    }

    if ((tnc = patty_kiss_tnc_open_fd(fd, bufsz)) == NULL) {
        goto error_tnc_open_fd;
    }

    return tnc;

error_tnc_open_fd:
    close(fd);

error_open:
    return NULL;
}

int patty_kiss_tnc_fd_unix(patty_kiss_tnc *tnc) {
    return tnc->fd;
}

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) {
    ssize_t readlen;

    if (tnc->buflen == tnc->bufsz) {
        goto full;
    }

    if ((readlen = read(tnc->fd, tnc->buf, tnc->bufsz - tnc->buflen)) < 0) {
        goto error_read;
    }

    tnc->buflen += readlen;

    return readlen;

full:
    return 0;

error_read:
    return -1;
}

static void tnc_flush(patty_kiss_tnc *tnc, size_t len) {
    memmove(tnc->buf, ((uint8_t *)tnc->buf) + len, tnc->buflen - len);

    tnc->buflen -= len;
}

static void tnc_drop(patty_kiss_tnc *tnc) {
    tnc->buflen = 0;
    tnc->dropped++;
}

size_t patty_kiss_tnc_dropped(patty_kiss_tnc *tnc) {
    return tnc->dropped;
}

ssize_t patty_kiss_tnc_recv(patty_kiss_tnc *tnc,
                            void **frame,
                            int *port) {
    size_t i = 0,
           b = 0;

    enum kiss_flags flags = KISS_NONE;

    if (tnc_buffer(tnc) < 0) {
        goto error_io;
    }

    while (i < tnc->buflen) {
        uint8_t c = ((uint8_t *)tnc->buf)[i++];

        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 (b > 0) {
                    flags &= ~KISS_FRAME;

                    goto done;
                }

                continue;
            }
        }

        if (!(flags & KISS_COMMAND)) {
            if (PATTY_KISS_COMMAND(c) != PATTY_KISS_DATA) {
                errno = EIO;

                goto error_io;
            }

            *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 *)tnc->frame)[b++] = PATTY_KISS_FEND;
                    flags &= ~KISS_ESCAPE;
                    continue;

                case PATTY_KISS_TFESC:
                    ((uint8_t *)tnc->frame)[b++] = PATTY_KISS_FESC;
                    flags &= ~KISS_ESCAPE;
                    continue;

                default:
                    errno = EIO;

                    goto error_io;
            }
        }

        ((uint8_t *)tnc->frame)[b++] = c;
    }

done:
    if (flags & KISS_FRAME) {
        tnc_drop(tnc);

        *frame = NULL;
    } else {
        tnc_flush(tnc, i);

        *frame = tnc->frame;
    }

    return (ssize_t)b;

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_command(int fd, int command, int port) {
    return write_byte(fd, ((port & 0x0f) << 4) | (command & 0x0f));
}

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_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++) {
        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;

                if (write(tnc->fd, &c, 1) < 0) {
                    goto error_io;
                }

                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;
}