/*
   Cubesat Space Protocol - A small network-layer protocol designed for Cubesats
   Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com)
   Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk)

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.

   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
   */

#include "csp_if_kiss.h"
#include"csp_if_uhf.h"

#include <string.h>
//#include "main.h"
#include "csp_endian.h"
#include "csp_crc32.h"

#define FEND  		0xC0
#define FESC  		0xDB
#define TFEND 		0xDC
#define TFESC 		0xDD
#define TNC_DATA	0x00

int csp_kiss_tx(const csp_route_t * ifroute, csp_packet_t * packet) {

    csp_kiss_interface_data_t * ifdata = ifroute->iface->interface_data;
    void * driver = ifroute->iface->driver_data;
#if TX_PRINT
    DEBUG_CPRINT(("\n\rTX->\n\r"));
#endif
    /* Add CRC32 checksum - the MTU setting ensures there are space */
    csp_crc32_append(packet, false);

    /* Lock */
    if (csp_mutex_lock(&ifdata->lock, 1000) != CSP_MUTEX_OK) {
        csp_tm_global.csp_err_timedout++;
        return CSP_ERR_TIMEDOUT;
    }

    /* Save the outgoing id in the buffer */
    packet->id.ext = csp_hton32(packet->id.ext);
    packet->length += sizeof(packet->id.ext);

    /* Transmit data */
    const unsigned char start[] = {FEND, TNC_DATA};
    const unsigned char esc_end[] = {FESC, TFEND};
    const unsigned char esc_esc[] = {FESC, TFESC};
    const unsigned char * data = (unsigned char *) &packet->id.ext;
    ifdata->tx_func(driver, start, sizeof(start));
    for (unsigned int i = 0; i < packet->length; i++, ++data) {
        if (*data == FEND) {
            ifdata->tx_func(driver, esc_end, sizeof(esc_end));
            continue;
        }
        if (*data == FESC) {
            ifdata->tx_func(driver, esc_esc, sizeof(esc_esc));
            continue;
        }
        ifdata->tx_func(driver, data, 1);
    }
    const unsigned char stop[] = {FEND};
    ifdata->tx_func(driver, stop, sizeof(stop));

    /* Free data */
    csp_buffer_free(packet);

    /* Unlock */
    csp_mutex_unlock(&ifdata->lock);

    return CSP_ERR_NONE;
}

/**
 * Decode received data and eventually route the packet.
 */
void csp_kiss_rx(csp_iface_t * iface, const uint8_t * buf, size_t len, void * pxTaskWoken) {

    csp_kiss_interface_data_t * ifdata = iface->interface_data;

    while (len--) {

        /* Input */
#if TX_PRINT
        DEBUG_CPRINT((" %x",*buf));
#endif
        uint8_t inputbyte = *buf++;


        /* If packet was too long */
        if (ifdata->rx_length > ifdata->max_rx_length) {
            //csp_log_warn("KISS RX overflow");
            iface->rx_error++;
            ifdata->rx_mode = KISS_MODE_NOT_STARTED;
            ifdata->rx_length = 0;
        }

        switch (ifdata->rx_mode) {

            case KISS_MODE_NOT_STARTED:

                /* Skip any characters until End char detected */
                if (inputbyte != FEND) {
                    break;
                }

                /* Try to allocate new buffer */
                if (ifdata->rx_packet == NULL) {
                    ifdata->rx_packet = pxTaskWoken ? csp_buffer_get_isr(0) : csp_buffer_get(0); // CSP only supports one size
                }

                /* If no more memory, skip frame */
                if (ifdata->rx_packet == NULL) {
                    ifdata->rx_mode = KISS_MODE_SKIP_FRAME;
                    iface->drop++;
                    break;
                }

                /* Start transfer */
                ifdata->rx_length = 0;
                ifdata->rx_mode = KISS_MODE_STARTED;
                ifdata->rx_first = true;
                break;

            case KISS_MODE_STARTED:

                /* Escape char */
                if (inputbyte == FESC) {
                    ifdata->rx_mode = KISS_MODE_ESCAPED;
                    break;
                }

                /* End Char */
                if (inputbyte == FEND) {

                    /* Accept message */
                    if (ifdata->rx_length > 0) {

                        /* Check for valid length */
                        if (ifdata->rx_length < CSP_HEADER_LENGTH + sizeof(uint32_t)) {
                            //csp_log_warn("KISS short frame skipped, len: %u", ifdata->rx_length);
                            iface->rx_error++;
                            ifdata->rx_mode = KISS_MODE_NOT_STARTED;
                            break;
                        }

                        /* Count received frame */
                        iface->frame++;

                        /* The CSP packet length is without the header */
                        ifdata->rx_packet->length = ifdata->rx_length - CSP_HEADER_LENGTH;

                        /* Convert the packet from network to host order */
                        ifdata->rx_packet->id.ext = csp_ntoh32(ifdata->rx_packet->id.ext);

                        /* Validate CRC */
                        if (csp_crc32_verify(ifdata->rx_packet, false) != CSP_ERR_NONE) {
                            //csp_log_warn("KISS invalid crc frame skipped, len: %u", ifdata->rx_packet->length);
                            iface->rx_error++;
                            ifdata->rx_mode = KISS_MODE_NOT_STARTED;
                            //DEBUG_CPRINT(("\n\rCRC ERR\n\r"));
                            break;
                        }

                        /* Send back into CSP, notice calling from task so last argument must be NULL! */
                        csp_qfifo_write(ifdata->rx_packet, iface, pxTaskWoken);
                        ifdata->rx_packet = NULL;
                        ifdata->rx_mode = KISS_MODE_NOT_STARTED;
                        break;

                    }

                    /* Break after the end char */
                    break;
                }

                /* Skip the first char after FEND which is TNC_DATA (0x00) */
                if (ifdata->rx_first) {
                    ifdata->rx_first = false;
                    break;
                }

                /* Valid data char */
                ((char *) &ifdata->rx_packet->id.ext)[ifdata->rx_length++] = inputbyte;

                break;

            case KISS_MODE_ESCAPED:

                /* Escaped escape char */
                if (inputbyte == TFESC)
                    ((char *) &ifdata->rx_packet->id.ext)[ifdata->rx_length++] = FESC;

                /* Escaped fend char */
                if (inputbyte == TFEND)
                    ((char *) &ifdata->rx_packet->id.ext)[ifdata->rx_length++] = FEND;

                /* Go back to started mode */
                ifdata->rx_mode = KISS_MODE_STARTED;
                break;

            case KISS_MODE_SKIP_FRAME:

                /* Just wait for end char */
                if (inputbyte == FEND)
                    ifdata->rx_mode = KISS_MODE_NOT_STARTED;

                break;
        }
    }
}

int csp_kiss_add_interface(csp_iface_t * iface) {

    if ((iface == NULL) || (iface->name == NULL) || (iface->interface_data == NULL)) {
        csp_tm_global.csp_err_inval++;
        return CSP_ERR_INVAL;
    }

    csp_kiss_interface_data_t * ifdata = iface->interface_data;
    if (ifdata->tx_func == NULL) {
        csp_tm_global.csp_err_inval++;
        return CSP_ERR_INVAL;
    }

    if (csp_mutex_create(&ifdata->lock) != CSP_MUTEX_OK) {
        csp_tm_global.csp_err_nomem++;
        return CSP_ERR_NOMEM;
    }

    ifdata->max_rx_length = CSP_HEADER_LENGTH + csp_buffer_data_size(); // CSP header + CSP data
    ifdata->rx_length = 0;
    ifdata->rx_mode = KISS_MODE_NOT_STARTED;
    ifdata->rx_first = false;
    ifdata->rx_packet = NULL;

    const unsigned int max_data_size = csp_buffer_data_size() - sizeof(uint32_t); // compensate for the added CRC32
    if ((iface->mtu == 0) || (iface->mtu > max_data_size)) {
        iface->mtu = max_data_size;
    }

    //iface->nexthop = csp_kiss_tx;
    iface->nexthop = csp_uhf_tx;

    return csp_iflist_add(iface);
}


