/*
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
*/
 
/* CAN frames contains at most 8 bytes of data, so in order to transmit CSP
 * packets larger than this, a fragmentation protocol is required. The CAN
 * Fragmentation Protocol (CFP) header is designed to match the 29 bit CAN
 * identifier.
 *
 * The CAN identifier is divided in these fields:
 * src:			 5 bits
 * dst:			 5 bits
 * type:			1 bit
 * remain:		  8 bits
 * identifier:	 10 bits
 *
 * Source and Destination addresses must match the CSP packet. The type field
 * is used to distinguish the first and subsequent frames in a fragmented CSP
 * packet. Type is BEGIN (0) for the first fragment and MORE (1) for all other
 * fragments. Remain indicates number of remaining fragments, and must be
 * decremented by one for each fragment sent. The identifier field serves the
 * same purpose as in the Internet Protocol, and should be an auto incrementing
 * integer to uniquely separate sessions.
 */

#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <inttypes.h>

#include <csp/csp.h>
#include <csp/csp_interface.h>
#include <csp/csp_endian.h>
#include <csp/interfaces/csp_if_can.h>

#include <csp/arch/csp_semaphore.h>
#include <csp/arch/csp_time.h>
#include <csp/arch/csp_queue.h>
#include <csp/arch/csp_thread.h>

#include "../drivers/can/can.h"

/** CAN header macros */
#define CFP_HOST_SIZE 	5
#define CFP_TYPE_SIZE 	1
#define CFP_REMAIN_SIZE 8
#define CFP_ID_SIZE 	10

/** Macros for extracting header fields */
#define CFP_FIELD(id,rsiz,fsiz) ((uint32_t)((uint32_t)((id) >> (rsiz)) & (uint32_t)((1 << (fsiz)) - 1)))
#define CFP_SRC(id) 		CFP_FIELD(id, CFP_HOST_SIZE + CFP_TYPE_SIZE + CFP_REMAIN_SIZE + CFP_ID_SIZE, CFP_HOST_SIZE)
#define CFP_DST(id) 		CFP_FIELD(id, CFP_TYPE_SIZE + CFP_REMAIN_SIZE + CFP_ID_SIZE, CFP_HOST_SIZE)
#define CFP_TYPE(id) 		CFP_FIELD(id, CFP_REMAIN_SIZE + CFP_ID_SIZE, CFP_TYPE_SIZE)
#define CFP_REMAIN(id)		CFP_FIELD(id, CFP_ID_SIZE, CFP_REMAIN_SIZE)
#define CFP_ID(id) 			CFP_FIELD(id, 0, CFP_ID_SIZE)

/** Macros for building CFP headers */
#define CFP_MAKE_FIELD(id,fsiz,rsiz) ((uint32_t)(((id) & (uint32_t)((uint32_t)(1 << (fsiz)) - 1)) << (rsiz)))
#define CFP_MAKE_SRC(id) 	CFP_MAKE_FIELD(id, CFP_HOST_SIZE, CFP_HOST_SIZE + CFP_TYPE_SIZE + CFP_REMAIN_SIZE + CFP_ID_SIZE)
#define CFP_MAKE_DST(id) 	CFP_MAKE_FIELD(id, CFP_HOST_SIZE, CFP_TYPE_SIZE + CFP_REMAIN_SIZE + CFP_ID_SIZE)
#define CFP_MAKE_TYPE(id) 	CFP_MAKE_FIELD(id, CFP_TYPE_SIZE, CFP_REMAIN_SIZE + CFP_ID_SIZE)
#define CFP_MAKE_REMAIN(id)	CFP_MAKE_FIELD(id, CFP_REMAIN_SIZE, CFP_ID_SIZE)
#define CFP_MAKE_ID(id) 	CFP_MAKE_FIELD(id, CFP_ID_SIZE, 0)

/** Mask to uniquely separate connections */
#define CFP_ID_CONN_MASK 	(CFP_MAKE_SRC((uint32_t)(1 << CFP_HOST_SIZE) - 1) \
							| CFP_MAKE_DST((uint32_t)(1 << CFP_HOST_SIZE) - 1) \
							| CFP_MAKE_ID((uint32_t)(1 << CFP_ID_SIZE) - 1))

/** Maximum Transmission Unit for CSP over CAN */
#define CSP_CAN_MTU 256

/** Maximum number of frames in RX queue */
#define CSP_CAN_RX_QUEUE_SIZE 100

/** Number of packet buffer elements */
#define PBUF_ELEMENTS CSP_CONN_MAX

/** Buffer element timeout in ms */
#define PBUF_TIMEOUT_MS 10000

/** CFP Frame Types */
enum cfp_frame_t {
	CFP_BEGIN = 0,
	CFP_MORE = 1
};

/** CFP identification number */
static int cfp_id;

/** CFP identification number semaphore */
static csp_bin_sem_handle_t id_sem;

/** Packet buffer semaphore */
CSP_DEFINE_CRITICAL(pbuf_sem);

/** RX task handle */
static csp_thread_handle_t can_rx_task;

/** RX frame queue */
static csp_queue_handle_t can_rx_queue;

/* Identification number */
static int id_init(void) {

	/* Init ID field to random number */
	srand((int)csp_get_ms());
	cfp_id = rand() & ((1 << CFP_ID_SIZE) - 1);
	
	if (csp_bin_sem_create(&id_sem) == CSP_SEMAPHORE_OK) {
		return CSP_ERR_NONE;
	} else {
		csp_log_error("Could not initialize CFP id semaphore");
		return CSP_ERR_NOMEM;
	}

}

static int id_get(void) {

	int id;
	if (csp_bin_sem_wait(&id_sem, 1000) != CSP_SEMAPHORE_OK)
		return CSP_ERR_TIMEDOUT;
	id = cfp_id++;
	cfp_id = cfp_id & ((1 << CFP_ID_SIZE) - 1);
	csp_bin_sem_post(&id_sem);
	return id;

}

/* Packet buffers */
typedef enum {
	BUF_FREE = 0,					/**< Buffer element free */
	BUF_USED = 1,					/**< Buffer element used */
} pbuf_state_t;

typedef struct {
	uint16_t rx_count;				/**< Received bytes */
	uint16_t tx_count;				/**< Transmitted bytes */
	uint32_t remain;				/**< Remaining packets */
	csp_bin_sem_handle_t tx_sem;	/**< Transmit semaphore for blocking mode */
	uint32_t cfpid;					/**< Connection CFP identification number */
	csp_packet_t *packet;			/**< Pointer to packet buffer */
	pbuf_state_t state;				/**< Element state */
	uint32_t last_used;				/**< Timestamp in ms for last use of buffer */
} pbuf_element_t;

static pbuf_element_t pbuf[PBUF_ELEMENTS];

/** pbuf_init
 * Initialize packet buffer.
 * @return 0 on success, -1 on error.
 */
static int pbuf_init(void) {

	/* Initialize packet buffers */
	int i;
	pbuf_element_t *buf;

	for (i = 0; i < PBUF_ELEMENTS; i++) {
		buf = &pbuf[i];
		buf->rx_count = 0;
		buf->tx_count = 0;
		buf->cfpid = 0;
		buf->packet = NULL;
		buf->state = BUF_FREE;
		buf->last_used = 0;
		buf->remain = 0;
		/* Create tx semaphore if blocking mode is enabled */
		if (csp_bin_sem_create(&buf->tx_sem) != CSP_SEMAPHORE_OK) {
			csp_log_error("Failed to allocate TX semaphore");
			return CSP_ERR_NOMEM;
		}
	}

    /* Initialize global lock */
	if (CSP_INIT_CRITICAL(pbuf_sem) != CSP_ERR_NONE) {
		csp_log_error("No more memory for packet buffer semaphore");
		return CSP_ERR_NOMEM;
	}

	return CSP_ERR_NONE;
	
}

/** pbuf_timestamp
 * Update packet buffer timestamp of last use.
 * @param buf Buffer element to update
 * @param task_woken
 * @return
 */
int pbuf_timestamp(pbuf_element_t *buf, CSP_BASE_TYPE *task_woken) {

	if (buf != NULL) {
		buf->last_used = task_woken ? csp_get_ms_isr() : csp_get_ms();
		return CSP_ERR_NONE;
	} else {
		return CSP_ERR_INVAL;
	}

}

/** pbuf_free_locked
 * Free buffer element and associated CSP packet buffer element with pbuf list locked.
 * @param buf Buffer element to free
 * @param free_packet true if the associated packet should be freed as well
 * @return 0 on success, -1 on error.
 */
static int pbuf_free_locked(pbuf_element_t *buf, CSP_BASE_TYPE *task_woken, bool free_packet) {

	/* Free CSP packet */
	if (buf->packet != NULL && free_packet) {
		if (task_woken == NULL) {
			csp_buffer_free(buf->packet);
		} else {
			csp_buffer_free_isr(buf->packet);
		}
	}

	/* Mark buffer element free */
	buf->packet = NULL;
	buf->state = BUF_FREE;
	buf->rx_count = 0;
	buf->tx_count = 0;
	buf->cfpid = 0;
	buf->last_used = 0;
	buf->remain = 0;

	return CSP_ERR_NONE;

}

/** pbuf_free
 * Free buffer element and associated CSP packet buffer element.
 * @param buf Buffer element to free
 * @param free_packet true if the associated packet should be freed as well
 * @return 0 on success, -1 on error.
 */
static int pbuf_free(pbuf_element_t *buf, CSP_BASE_TYPE *task_woken, bool free_packet) {

	int ret;

	/* Lock packet buffer */
	if (task_woken == NULL)
		CSP_ENTER_CRITICAL(pbuf_sem);

	ret = pbuf_free_locked(buf, task_woken, free_packet);

	/* Unlock packet buffer */
	if (task_woken == NULL)
		CSP_EXIT_CRITICAL(pbuf_sem);

	return ret;

}

/** pbuf_new
 * Get new packet buffer element.
 * @param id CFP identifier
 * @return Pointer to packet buffer element on success, NULL on error.
 */
static pbuf_element_t *pbuf_new(uint32_t id, CSP_BASE_TYPE *task_woken) {

	/* Search for free buffer */
	int i;
	pbuf_element_t *buf, *ret = NULL;

	/* Lock packet buffer */
	if (task_woken == NULL)
		CSP_ENTER_CRITICAL(pbuf_sem);

	/* Loop through buffer elements */
	for (i = 0; i < PBUF_ELEMENTS; i++) {
		buf = &pbuf[i];
		if(buf->state == BUF_FREE) {
			buf->state = BUF_USED;
			buf->cfpid = id;
			buf->remain = 0;
			pbuf_timestamp(buf, task_woken);
			ret = buf;
			break;
		}
	}

	/* Unlock packet buffer */
	if (task_woken == NULL)
		CSP_EXIT_CRITICAL(pbuf_sem);

	/* No free buffer was found */
	return ret;
  
}


/** pbuf_find
 * Find matching packet buffer or create a new one.
 * @param id CFP identifier to match
 * @param mask Match mask
 * @return Pointer to matching or new packet buffer element on success, NULL on error.
 */
static pbuf_element_t *pbuf_find(uint32_t id, uint32_t mask, CSP_BASE_TYPE *task_woken) {
	
	/* Search for matching buffer */
	int i;
	pbuf_element_t *buf, *ret = NULL;

	/* Lock packet buffer */
	if (task_woken == NULL)
		CSP_ENTER_CRITICAL(pbuf_sem);

	/* Loop through buffer elements */
	for (i = 0; i < PBUF_ELEMENTS; i++) {
		buf = &pbuf[i];

		if((buf->state == BUF_USED) && ((buf->cfpid & mask) == (id & mask))) {
			pbuf_timestamp(buf, task_woken);
			ret = buf;
			break;
		}
	}

	/* Unlock packet buffer */
	if (task_woken == NULL)
		CSP_EXIT_CRITICAL(pbuf_sem);

	/* No matching buffer was found */
	return ret;

}

/** pbuf_cleanup
 * Purge all packets buffers that have timed out
 */
static void pbuf_cleanup(void) {

	int i;
	pbuf_element_t *buf;

	/* Lock packet buffer */
	CSP_ENTER_CRITICAL(pbuf_sem);

	/* Loop through buffer elements */
	for (i = 0; i < PBUF_ELEMENTS; i++) {
		buf = &pbuf[i];
		if (buf->state == BUF_USED) {
			/* Check timeout */
			uint32_t now = csp_get_ms();
			if (now - buf->last_used > PBUF_TIMEOUT_MS) {
				csp_log_warn("CAN Buffer element timed out");
				/* Recycle packet buffer */
				pbuf_free_locked(buf, NULL, true);
			}
		}
	}

	/* Unlock packet buffer */
	CSP_EXIT_CRITICAL(pbuf_sem);

}

int csp_tx_callback(can_id_t canid, can_error_t error, CSP_BASE_TYPE *task_woken) {

	int bytes;
	uint8_t dest;

	/* Match buffer element */
	pbuf_element_t *buf = pbuf_find(canid, CFP_ID_CONN_MASK, task_woken);

	if (buf == NULL) {
		csp_log_warn("Failed to match buffer element in tx callback");
		csp_if_can.tx_error++;
		return CSP_ERR_INVAL;
	}

	if (buf->packet == NULL) {
		csp_log_warn("Buffer packet was NULL");
		csp_if_can.tx_error++;
		pbuf_free(buf, task_woken, false);
		return CSP_ERR_INVAL;
	}

	/* Free packet buffer if send failed */
	if (error != CAN_NO_ERROR) {
		csp_log_warn("Error in transmit callback");
		csp_if_can.tx_error++;
		pbuf_free(buf, task_woken, true);
		return CSP_ERR_DRIVER;
	}

	/* Send next frame if not complete */
	if (buf->tx_count < buf->packet->length) {
		/* Calculate frame data bytes */
		bytes = (buf->packet->length - buf->tx_count >= 8) ? 8 : buf->packet->length - buf->tx_count;

		/* Insert destination node mac address into the CFP destination field */
		dest = csp_rtable_find_mac(buf->packet->id.dst);
		if (dest == CSP_NODE_MAC)
			dest = buf->packet->id.dst;

		/* Prepare identifier */
		can_id_t id  = 0;
		id |= CFP_MAKE_SRC(buf->packet->id.src);
		id |= CFP_MAKE_DST(dest);
		id |= CFP_MAKE_ID(CFP_ID(canid));
		id |= CFP_MAKE_TYPE(CFP_MORE);
		id |= CFP_MAKE_REMAIN((buf->packet->length - buf->tx_count - bytes + 7) / 8);

		/* Increment tx counter */
		buf->tx_count += bytes;

		/* Send frame */
		if (can_send(id, buf->packet->data + buf->tx_count - bytes, bytes, task_woken) != 0) {
			csp_log_warn("Failed to send CAN frame in Tx callback");
			csp_if_can.tx_error++;

			/* Post semaphore to wake up sender */
			if (task_woken != NULL) {
				csp_bin_sem_post_isr(&buf->tx_sem, task_woken);
			} else {
				csp_bin_sem_post(&buf->tx_sem);
			}

			pbuf_free(buf, task_woken, true);

			return CSP_ERR_DRIVER;
		}
	} else {
		/* Post semaphore to wake up sender */
		if (task_woken != NULL) {
			csp_bin_sem_post_isr(&buf->tx_sem, task_woken);
		} else {
			csp_bin_sem_post(&buf->tx_sem);
		}

		/* Free packet buffer */
		pbuf_free(buf, task_woken, true);
	}

	return CSP_ERR_NONE;

}

int csp_rx_callback(can_frame_t *frame, CSP_BASE_TYPE *task_woken) {

	int ret = csp_queue_enqueue_isr(can_rx_queue, frame, task_woken);
	return ret == CSP_QUEUE_OK ? CSP_ERR_NONE : CSP_ERR_NOMEM;

}

static int csp_can_process_frame(can_frame_t *frame) {

	pbuf_element_t *buf;
	uint8_t offset;
	
	can_id_t id = frame->id;

	/* Bind incoming frame to a packet buffer */
	buf = pbuf_find(id, CFP_ID_CONN_MASK, NULL);

	/* Check returned buffer */
	if (buf == NULL) {
		if (CFP_TYPE(id) == CFP_BEGIN) {
			buf = pbuf_new(id, NULL);
			if (buf == NULL) {
				csp_log_warn("No available packet buffer for CAN");
				csp_if_can.rx_error++;
				return CSP_ERR_NOMEM;
			}
		} else {
			csp_log_warn("Out of order MORE frame received");
			csp_if_can.frame++;
			return CSP_ERR_INVAL;
		}
	}

	/* Reset frame data offset */
	offset = 0;

	switch (CFP_TYPE(id)) {

		case CFP_BEGIN:
		
			/* Discard packet if DLC is less than CSP id + CSP length fields */
			if (frame->dlc < sizeof(csp_id_t) + sizeof(uint16_t)) {
				csp_log_warn("Short BEGIN frame received");
				csp_if_can.frame++;
				pbuf_free(buf, NULL, true);
				break;
			}
						
			/* Check for incomplete frame */
			if (buf->packet != NULL) {
				/* Reuse the buffer */
				csp_log_warn("Incomplete frame");
				csp_if_can.frame++;
			} else {
				/* Allocate memory for frame */
				buf->packet = csp_buffer_get(csp_buffer_size() - CSP_BUFFER_PACKET_OVERHEAD);
				if (buf->packet == NULL) {
					csp_log_error("Failed to get buffer for CSP_BEGIN packet");
					csp_if_can.frame++;
					pbuf_free(buf, NULL, true);
					break;
				}
			}

			/* Copy CSP identifier and length*/
			memcpy(&(buf->packet->id), frame->data, sizeof(csp_id_t));
			buf->packet->id.ext = csp_ntoh32(buf->packet->id.ext);
			memcpy(&(buf->packet->length), frame->data + sizeof(csp_id_t), sizeof(uint16_t));
			buf->packet->length = csp_ntoh16(buf->packet->length);
			
			/* Reset RX count */
			buf->rx_count = 0;
			
			/* Set offset to prevent CSP header from being copied to CSP data */
			offset = sizeof(csp_id_t) + sizeof(uint16_t);

			/* Set remain field - increment to include begin packet */
			buf->remain = CFP_REMAIN(id) + 1;

			/* Note fall through! */
			
		case CFP_MORE:

			/* Check 'remain' field match */
			if (CFP_REMAIN(id) != buf->remain - 1) {
				csp_log_error("CAN frame lost in CSP packet");
				pbuf_free(buf, NULL, true);
				csp_if_can.frame++;
				break;
			}

			/* Decrement remaining frames */
			buf->remain--;

			/* Check for overflow */
			if ((buf->rx_count + frame->dlc - offset) > buf->packet->length) {
				csp_log_error("RX buffer overflow");
				csp_if_can.frame++;
				pbuf_free(buf, NULL, true);
				break;
			}

			/* Copy dlc bytes into buffer */
			memcpy(&buf->packet->data[buf->rx_count], frame->data + offset, frame->dlc - offset);
			buf->rx_count += frame->dlc - offset;

			/* Check if more data is expected */
			if (buf->rx_count != buf->packet->length)
				break;

			/* Data is available */
			csp_new_packet(buf->packet, &csp_if_can, NULL);

			/* Drop packet buffer reference */
			buf->packet = NULL;

			/* Free packet buffer */
			pbuf_free(buf, NULL, true);

			break;

		default:
			csp_log_warn("Received unknown CFP message type");
			pbuf_free(buf, NULL, true);
			break;

	}

	return CSP_ERR_NONE;

}

CSP_DEFINE_TASK(csp_can_rx_task) {

	int ret;
	can_frame_t frame;

	while (1) {
		ret = csp_queue_dequeue(can_rx_queue, &frame, 1000);
		if (ret != CSP_QUEUE_OK) {
			pbuf_cleanup();
			continue;
		}

		csp_can_process_frame(&frame);
	}

	csp_thread_exit();

}

int csp_can_tx(csp_iface_t * interface, csp_packet_t *packet, uint32_t timeout) {

	uint8_t bytes, overhead, avail, dest;
	uint8_t frame_buf[8];

	/* Get CFP identification number */
	int ident = id_get();
	if (ident < 0) {
		csp_log_warn("Failed to get CFP identification number");
		return CSP_ERR_INVAL;
	}
	
	/* Calculate overhead */
	overhead = sizeof(csp_id_t) + sizeof(uint16_t);

	/* Insert destination node mac address into the CFP destination field */
	dest = csp_rtable_find_mac(packet->id.dst);
	if (dest == CSP_NODE_MAC)
		dest = packet->id.dst;

	/* Create CAN identifier */
	can_id_t id = 0;
	id |= CFP_MAKE_SRC(packet->id.src);
	id |= CFP_MAKE_DST(dest);
	id |= CFP_MAKE_ID(ident);
	id |= CFP_MAKE_TYPE(CFP_BEGIN);
	id |= CFP_MAKE_REMAIN((packet->length + overhead - 1) / 8);

	/* Get packet buffer */
	pbuf_element_t *buf = pbuf_new(id, NULL);

	if (buf == NULL) {
		csp_log_warn("Failed to get packet buffer for CAN");
		return CSP_ERR_NOMEM;
	}

	/* Set packet */
	buf->packet = packet;

	/* Calculate first frame data bytes */
	avail = 8 - overhead;
	bytes = (packet->length <= avail) ? packet->length : avail;

	/* Copy CSP headers and data */
	uint32_t csp_id_be = csp_hton32(packet->id.ext);
	uint16_t csp_length_be = csp_hton16(packet->length);

	memcpy(frame_buf, &csp_id_be, sizeof(csp_id_be));
	memcpy(frame_buf + sizeof(csp_id_be), &csp_length_be, sizeof(csp_length_be));
	memcpy(frame_buf + overhead, packet->data, bytes);

	/* Increment tx counter */
	buf->tx_count += bytes;

	/* Take semaphore so driver can post it later */
	if (csp_bin_sem_wait(&buf->tx_sem, 0) != CSP_SEMAPHORE_OK) {
		csp_log_error("Failed to take CAN pbuf TX sem!");
		pbuf_free(buf, NULL, false);
		return CSP_ERR_DRIVER;
	}

	/* Send frame. We must free packet buffer is this fails,
	 * but the packet itself should be freed by the caller */
	if (can_send(id, frame_buf, overhead + bytes, NULL) != 0) {
		csp_log_warn("Failed to send CAN frame in csp_tx_can");
		csp_bin_sem_post(&buf->tx_sem);
		pbuf_free(buf, NULL, false);
		return CSP_ERR_DRIVER;
	}

	/* NOTE: The transmit packet is now owned by the transmission MOB and
	 * must NOT be freed by the calling thread. */

	/* Non blocking mode */
	if (timeout == 0)
		return CSP_ERR_NONE;

	/* Blocking mode */
	if (csp_bin_sem_wait(&buf->tx_sem, timeout) != CSP_SEMAPHORE_OK) {
		/* tx_sem is posted by transmission callback. The packet
		 * could still be in use by the transmission MOB, so
		 * we can not return CSP_ERR_TIMEOUT and risk that the
		 * calling thread frees the packet. */
		return CSP_ERR_NONE;
	} else {
		csp_bin_sem_post(&buf->tx_sem);
		return CSP_ERR_NONE;
	}

}

int csp_can_init(uint8_t mode, struct csp_can_config *conf) {

	int ret;
	uint32_t mask;

	/* Initialize packet buffer */
	if (pbuf_init() != 0) {
		csp_log_error("Failed to initialize CAN packet buffers");
		return CSP_ERR_NOMEM;
	}

	/* Initialize CFP identifier */
	if (id_init() != 0) {
		csp_log_error("Failed to initialize CAN identification number");
		return CSP_ERR_NOMEM;
	}
	
	if (mode == CSP_CAN_MASKED) {
		mask = CFP_MAKE_DST((1 << CFP_HOST_SIZE) - 1);
	} else if (mode == CSP_CAN_PROMISC) {
		mask = 0;
		csp_if_can.promisc = 1;
	} else {
		csp_log_error("Unknown CAN mode");
		return CSP_ERR_INVAL;
	}

	can_rx_queue = csp_queue_create(CSP_CAN_RX_QUEUE_SIZE, sizeof(can_frame_t));
	if (can_rx_queue == NULL) {
		csp_log_error("Failed to create CAN RX queue");
		return CSP_ERR_NOMEM;
	}
	
	ret = csp_thread_create(csp_can_rx_task, (signed char *) "CAN", 6000/sizeof(int), NULL, 3, &can_rx_task);
	if (ret != 0) {
		csp_log_error("Failed to init CAN RX task");
		return CSP_ERR_NOMEM;
	}

	/* Initialize CAN driver */
	if (can_init(CFP_MAKE_DST(my_address), mask, csp_tx_callback, csp_rx_callback, conf) != 0) {
		csp_log_error("Failed to initialize CAN driver");
		return CSP_ERR_DRIVER;
	}

	/* Regsiter interface */
	csp_iflist_add(&csp_if_can);

	return CSP_ERR_NONE;

}

/** Interface definition */
csp_iface_t csp_if_can = {
	.name = "CAN",
	.nexthop = csp_can_tx,
	.mtu = CSP_CAN_MTU,
};
