patty/src/kiss.c

#include <patty/kiss.h>

enum kiss_flags {
    KISS_NONE   = 0x00,
    KISS_FRAME  = 0x01,
    KISS_ESCAPE = 0x02
};

ssize_t patty_kiss_read(int fd, void *buf, size_t len, int *port) {
    int flags = KISS_NONE;
    size_t i, b = 0;

    if (read(fd, buf, len) < 0) {
        goto error_io;
    }

    *port = 0;

    for (i=0; i<len; i++) {
        unsigned char c = ((char *)buf)[i];

        if (flags & KISS_FRAME == 0) {
            if (c == PATTY_KISS_FEND) {
                flags |= KISS_FRAME;
            }

            continue;
        }

        if (flags & KISS_ESCAPE) {
            if (c == PATTY_KISS_TFEND) {
                ((unsigned char *)buf)[b++] = PATTY_KISS_FEND;
            } else if (c == PATTY_KISS_TFESC) {
                ((unsigned char *)buf)[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) {
                flags &= ~KISS_FRAME;

                break;
            }

            /*
             * 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 (c & 0x0f == 0) {
                *port = (c & 0xf0) >> 4;

                continue;
            }

            ((unsigned char *)buf)[b++] = c;
        }
    }

    return b + 1;

error_io:
    return -1;
}