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

ssize_t patty_kiss_write(int fd, const void *buf, size_t len, int port) {
    size_t i;

    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(fd, &command, 1) < 0) {
        goto error_io;
    }

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

        switch (c) {
            case PATTY_KISS_FEND: {
                res = write(fd, escape_fend, sizeof(escape_fend)); break;
            }

            case PATTY_KISS_FESC: {
                res = write(fd, escape_fesc, sizeof(escape_fesc)); break;
            }

            default: {
                res = write(fd, &c, 1); break;
            }
        }

        if (res < 0) {
            goto error_io;
        }
    }

    return 0;

error_io:
    return -1;
}