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

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_write(int fd, 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(fd, PATTY_KISS_FEND) < 0) {
        goto error_io;
    }

    if (write_command(PATTY_KISS_DATA, port) < 0) {
        goto error_io;
    }

    for (i=0; i<len; i++) {
        unsigned char c = ((unsigned char *)buf)[i];
        ssize_t res = 0;
        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;
                res = write(fd, &c, 1); break;
            }
        }

        if (escape) {
            if (write(fd, ((unsigned char *)buf) + start, end - start) < 0) {
                goto error_io;
            }

            if (write(fd, escape, 2) < 0) {
                goto error_io;
            }

            escape = NULL;
            start  = i + 1;
            end    = start;
        }
    }

    if (end - start) {
        if (write(fd, ((unsigned char *)buf) + start, end - start) < 0) {
            goto error_io;
        }
    }

    if (write_byte(fd, PATTY_KISS_FEND) < 0) {
        goto error_io;
    }

    return len;

error_io:
    return -1;
}