#include <stdint.h>
#include "uartp.h"

extern void uart_wbyte(uint8_t x);
extern uint8_t uart_rbyte();

// Simple serial protocol with packets and checksum
const uint8_t AMUX_TAG = 0xf9;
const uint8_t AMUX_END = 0xf3;
const uint8_t AMUX_EXT = 0x80;

void uartp_send(const void *src, uint16_t len) {
    uint8_t len_ind_0, len_ind_1, fcs, info;
    const uint8_t *buf = (const uint8_t *) src;

    uart_wbyte(AMUX_TAG);
    len_ind_0 = (uint8_t) (0xff & len);
    len_ind_1 = (uint8_t) (0xff & (len >> 7));
    if (len < 128) {
        uart_wbyte(len_ind_0);
    } else {
        uart_wbyte(len_ind_0 | AMUX_EXT);
        uart_wbyte(len_ind_1);
    }
    fcs = 0;
    for (uint16_t i = 0; i < len; i++) {
        info = buf[i];
        fcs += info;
        uart_wbyte(buf[i]);
    }
    fcs = 255 - fcs;
    uart_wbyte(fcs);
    uart_wbyte(AMUX_END);
}


uint16_t uartp_recv(void *dst, uint16_t buf_len) {
    uint8_t *buf = (uint8_t *) dst;
    uint8_t tag_old, tag, info, cs;
    uint16_t len;

    tag = AMUX_END;
    while (1) {

        do {
            tag_old = tag;
            tag = uart_rbyte();
        } while(tag != AMUX_TAG || tag_old != AMUX_END);

        len = (uint16_t) uart_rbyte();
        if (len & AMUX_EXT) {
            len &= (~AMUX_EXT);
            len |= (uint16_t) (uart_rbyte() << 7);
        }
        if (len > buf_len) {
            return len;
        }

        uint16_t i = 0;
        cs = 0;
        for (i = 0; i < len; i++) {
            info = uart_rbyte();
            buf[i] = info;
            cs += info;
        }
        cs += uart_rbyte();
        tag = uart_rbyte();
        if (0xff == cs) {
            if (AMUX_END == tag) {
                return len;
            }
        }

    }
}