261 lines
6.8 KiB
C
261 lines
6.8 KiB
C
/*
|
|
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 <inttypes.h>
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#include <csp/csp.h>
|
|
#include <csp/csp_endian.h>
|
|
#include <csp/csp_platform.h>
|
|
#include <csp/csp_interface.h>
|
|
#include <csp/interfaces/csp_if_kiss.h>
|
|
#include <csp/arch/csp_semaphore.h>
|
|
#include <csp/csp_crc32.h>
|
|
|
|
#define KISS_MTU 256
|
|
|
|
#define FEND 0xC0
|
|
#define FESC 0xDB
|
|
#define TFEND 0xDC
|
|
#define TFESC 0xDD
|
|
|
|
#define TNC_DATA 0x00
|
|
#define TNC_SET_HARDWARE 0x06
|
|
#define TNC_RETURN 0xFF
|
|
|
|
static int kiss_lock_init = 0;
|
|
static csp_bin_sem_handle_t kiss_lock;
|
|
|
|
/* Send a CSP packet over the KISS RS232 protocol */
|
|
static int csp_kiss_tx(csp_iface_t * interface, csp_packet_t * packet, uint32_t timeout) {
|
|
|
|
if (interface == NULL || interface->driver == NULL)
|
|
return CSP_ERR_DRIVER;
|
|
|
|
/* Add CRC32 checksum */
|
|
csp_crc32_append(packet, false);
|
|
|
|
/* Save the outgoing id in the buffer */
|
|
packet->id.ext = csp_hton32(packet->id.ext);
|
|
packet->length += sizeof(packet->id.ext);
|
|
|
|
/* Lock */
|
|
csp_bin_sem_wait(&kiss_lock, 1000);
|
|
|
|
/* Transmit data */
|
|
csp_kiss_handle_t * driver = interface->driver;
|
|
driver->kiss_putc(FEND);
|
|
driver->kiss_putc(TNC_DATA);
|
|
for (unsigned int i = 0; i < packet->length; i++) {
|
|
if (((unsigned char *) &packet->id.ext)[i] == FEND) {
|
|
((unsigned char *) &packet->id.ext)[i] = TFEND;
|
|
driver->kiss_putc(FESC);
|
|
} else if (((unsigned char *) &packet->id.ext)[i] == FESC) {
|
|
((unsigned char *) &packet->id.ext)[i] = TFESC;
|
|
driver->kiss_putc(FESC);
|
|
}
|
|
driver->kiss_putc(((unsigned char *) &packet->id.ext)[i]);
|
|
}
|
|
driver->kiss_putc(FEND);
|
|
|
|
/* Free data */
|
|
csp_buffer_free(packet);
|
|
|
|
/* Unlock */
|
|
csp_bin_sem_post(&kiss_lock);
|
|
|
|
return CSP_ERR_NONE;
|
|
}
|
|
|
|
/**
|
|
* When a frame is received, decode the kiss-stuff
|
|
* and eventually send it directly to the CSP new packet function.
|
|
*/
|
|
void csp_kiss_rx(csp_iface_t * interface, uint8_t * buf, int len, void * pxTaskWoken) {
|
|
|
|
/* Driver handle */
|
|
csp_kiss_handle_t * driver = interface->driver;
|
|
|
|
while (len--) {
|
|
|
|
/* Input */
|
|
unsigned char inputbyte = *buf++;
|
|
|
|
/* If packet was too long */
|
|
if (driver->rx_length > interface->mtu) {
|
|
//csp_log_warn("KISS RX overflow");
|
|
interface->rx_error++;
|
|
driver->rx_mode = KISS_MODE_NOT_STARTED;
|
|
driver->rx_length = 0;
|
|
}
|
|
|
|
switch (driver->rx_mode) {
|
|
|
|
case KISS_MODE_NOT_STARTED:
|
|
|
|
/* Send normal chars back to usart driver */
|
|
if (inputbyte != FEND) {
|
|
if (driver->kiss_discard != NULL)
|
|
driver->kiss_discard(inputbyte, pxTaskWoken);
|
|
break;
|
|
}
|
|
|
|
/* Try to allocate new buffer */
|
|
if (driver->rx_packet == NULL) {
|
|
if (pxTaskWoken == NULL) {
|
|
driver->rx_packet = csp_buffer_get(interface->mtu);
|
|
} else {
|
|
driver->rx_packet = csp_buffer_get_isr(interface->mtu);
|
|
}
|
|
}
|
|
|
|
/* If no more memory, skip frame */
|
|
if (driver->rx_packet == NULL) {
|
|
driver->rx_mode = KISS_MODE_SKIP_FRAME;
|
|
break;
|
|
}
|
|
|
|
/* Start transfer */
|
|
driver->rx_length = 0;
|
|
driver->rx_mode = KISS_MODE_STARTED;
|
|
driver->rx_first = 1;
|
|
break;
|
|
|
|
case KISS_MODE_STARTED:
|
|
|
|
/* Escape char */
|
|
if (inputbyte == FESC) {
|
|
driver->rx_mode = KISS_MODE_ESCAPED;
|
|
break;
|
|
}
|
|
|
|
/* End Char */
|
|
if (inputbyte == FEND) {
|
|
|
|
/* Accept message */
|
|
if (driver->rx_length > 0) {
|
|
|
|
/* Check for valid length */
|
|
if (driver->rx_length < CSP_HEADER_LENGTH + sizeof(uint32_t)) {
|
|
//csp_log_warn("KISS short frame skipped, len: %u", driver->rx_length);
|
|
interface->rx_error++;
|
|
driver->rx_mode = KISS_MODE_NOT_STARTED;
|
|
break;
|
|
}
|
|
|
|
/* Count received frame */
|
|
interface->frame++;
|
|
|
|
/* The CSP packet length is without the header */
|
|
driver->rx_packet->length = driver->rx_length - CSP_HEADER_LENGTH;
|
|
|
|
/* Convert the packet from network to host order */
|
|
driver->rx_packet->id.ext = csp_ntoh32(driver->rx_packet->id.ext);
|
|
|
|
/* Validate CRC */
|
|
if (csp_crc32_verify(driver->rx_packet, false) != CSP_ERR_NONE) {
|
|
//csp_log_warn("KISS invalid crc frame skipped, len: %u", driver->rx_packet->length);
|
|
interface->rx_error++;
|
|
driver->rx_mode = KISS_MODE_NOT_STARTED;
|
|
break;
|
|
}
|
|
|
|
/* Send back into CSP, notice calling from task so last argument must be NULL! */
|
|
csp_qfifo_write(driver->rx_packet, interface, pxTaskWoken);
|
|
driver->rx_packet = NULL;
|
|
driver->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 (driver->rx_first) {
|
|
driver->rx_first = 0;
|
|
break;
|
|
}
|
|
|
|
/* Valid data char */
|
|
((char *) &driver->rx_packet->id.ext)[driver->rx_length++] = inputbyte;
|
|
|
|
break;
|
|
|
|
case KISS_MODE_ESCAPED:
|
|
|
|
/* Escaped escape char */
|
|
if (inputbyte == TFESC)
|
|
((char *) &driver->rx_packet->id.ext)[driver->rx_length++] = FESC;
|
|
|
|
/* Escaped fend char */
|
|
if (inputbyte == TFEND)
|
|
((char *) &driver->rx_packet->id.ext)[driver->rx_length++] = FEND;
|
|
|
|
/* Go back to started mode */
|
|
driver->rx_mode = KISS_MODE_STARTED;
|
|
break;
|
|
|
|
case KISS_MODE_SKIP_FRAME:
|
|
|
|
/* Just wait for end char */
|
|
if (inputbyte == FEND)
|
|
driver->rx_mode = KISS_MODE_NOT_STARTED;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void csp_kiss_init(csp_iface_t * csp_iface, csp_kiss_handle_t * csp_kiss_handle, csp_kiss_putc_f kiss_putc_f, csp_kiss_discard_f kiss_discard_f, const char * name) {
|
|
|
|
/* Init lock only once */
|
|
if (kiss_lock_init == 0) {
|
|
csp_bin_sem_create(&kiss_lock);
|
|
kiss_lock_init = 1;
|
|
}
|
|
|
|
/* Register device handle as member of interface */
|
|
csp_iface->driver = csp_kiss_handle;
|
|
csp_kiss_handle->kiss_discard = kiss_discard_f;
|
|
csp_kiss_handle->kiss_putc = kiss_putc_f;
|
|
csp_kiss_handle->rx_packet = NULL;
|
|
csp_kiss_handle->rx_mode = KISS_MODE_NOT_STARTED;
|
|
|
|
/* Set default MTU if not given */
|
|
if (csp_iface->mtu == 0) {
|
|
csp_iface->mtu = KISS_MTU;
|
|
}
|
|
|
|
/* Setup other mandatories */
|
|
csp_iface->nexthop = csp_kiss_tx;
|
|
csp_iface->name = name;
|
|
|
|
/* Regsiter interface */
|
|
csp_iflist_add(csp_iface);
|
|
|
|
}
|