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

    /*
     * If the buffer needs to be filled, then fill it.
     */
    if (fillsz) {
        void *dest = ((unsigned char *)tnc->buf) + tnc->buflen;
        ssize_t readlen;

        if ((readlen = read(tnc->fd, dest, fillsz)) < 0) {
            errno = EIO;

            goto error_io;
        }

        tnc->buflen += readlen;
    }

    return tnc->buflen;

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

        /*
         * 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
         * packet 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_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 decoded;
     size_t framelen;

    /*
     * Fill the buffer with something to parse!
     */
    if (tnc_buffer(tnc) < 0) {
        goto error_io;
    }

    /*
     * Initialize the frame to be returned to the caller.
     */
    memset(tnc->frame, '\0', tnc->bufsz);

    if ((decoded = tnc_decode(tnc, tnc->frame, &framelen, port)) < 0) {
        goto error_io;
    }

    if (decoded) {
        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;
}