[PATCH 13/26] leon, grcanfd: split out GRCANFD specific support in separate file

Daniel Hellstrom daniel at gaisler.com
Mon Jun 29 11:28:00 UTC 2020


---
 bsps/shared/grlib-sources.am           |   1 +
 bsps/shared/grlib/can/grcan.c          | 638 +--------------------------------
 bsps/shared/grlib/can/grcan_internal.h | 140 ++++++++
 bsps/shared/grlib/can/grcanfd.c        | 535 +++++++++++++++++++++++++++
 4 files changed, 687 insertions(+), 627 deletions(-)
 create mode 100644 bsps/shared/grlib/can/grcan_internal.h
 create mode 100644 bsps/shared/grlib/can/grcanfd.c

diff --git a/bsps/shared/grlib-sources.am b/bsps/shared/grlib-sources.am
index f2ad1bf..10820dd 100644
--- a/bsps/shared/grlib-sources.am
+++ b/bsps/shared/grlib-sources.am
@@ -23,6 +23,7 @@ librtemsbsp_a_SOURCES += ../../../../../../bsps/shared/grlib/btimer/tlib_ckinit.
 librtemsbsp_a_SOURCES += ../../../../../../bsps/shared/grlib/can/canbtrs.c
 librtemsbsp_a_SOURCES += ../../../../../../bsps/shared/grlib/can/canmux.c
 librtemsbsp_a_SOURCES += ../../../../../../bsps/shared/grlib/can/grcan.c
+librtemsbsp_a_SOURCES += ../../../../../../bsps/shared/grlib/can/grcanfd.c
 librtemsbsp_a_SOURCES += ../../../../../../bsps/shared/grlib/can/occan.c
 librtemsbsp_a_SOURCES += ../../../../../../bsps/shared/grlib/can/satcan.c
 librtemsbsp_a_SOURCES += ../../../../../../bsps/shared/grlib/drvmgr/ambapp_bus.c
diff --git a/bsps/shared/grlib/can/grcan.c b/bsps/shared/grlib/can/grcan.c
index 959c84d..e2298d3 100644
--- a/bsps/shared/grlib/can/grcan.c
+++ b/bsps/shared/grlib/can/grcan.c
@@ -24,13 +24,11 @@
 #include <grlib/ambapp.h>
 
 #include <grlib/grlib_impl.h>
+#include "grcan_internal.h"
 
 /* Maximum number of GRCAN devices supported by driver */
 #define GRCAN_COUNT_MAX 8
 
-#define WRAP_AROUND_TX_MSGS 1
-#define WRAP_AROUND_RX_MSGS 2
-#define GRCAN_MSG_SIZE sizeof(struct grcan_msg)
 #define BLOCK_SIZE (16*4)
 
 /* grcan needs to have it buffers aligned to 1k boundaries */
@@ -58,15 +56,6 @@
  #define IRQ_MASK(irqno)
 #endif
 
-#ifndef GRCAN_DEFAULT_BAUD
- /* default to 500kbits/s */
- #define GRCAN_DEFAULT_BAUD 500000
-#endif
-
-#ifndef GRCAN_SAMPLING_POINT
- #define GRCAN_SAMPLING_POINT 80
-#endif
-
 /* Uncomment for debug output */
 /****************** DEBUG Definitions ********************/
 #define DBG_TX 2
@@ -89,65 +78,6 @@ int state2err[4] = {
 	/* STATE_AHBERR  */ GRCAN_RET_AHBERR
 };
 
-struct grcan_msg {
-	unsigned int head[2];
-	unsigned char data[8];
-};
-
-struct grcanfd_bd0 {
-	uint32_t head[2];
-	uint64_t data0; /* variable size, from 1 to 8 dwords */
-};
-
-struct grcanfd_bd1 {
-	unsigned long long data[2];
-};
-
-struct grcan_config {
-	struct grcan_timing timing;
-	struct grcanfd_timing timing_fd;
-	struct grcan_selection selection;
-	int abort;
-	int silent;
-};
-
-struct grcan_priv {
-	struct drvmgr_dev *dev;	/* Driver manager device */
-	char devName[32];	/* Device Name */
-	unsigned int baseaddr, ram_base;
-	struct grcan_regs *regs;
-	int irq;
-	int minor;
-	int open;
-	int started;
-	unsigned int channel;
-	int flushing;
-	unsigned int corefreq_hz;
-	int fd_capable;
-
-	/* Circular DMA buffers */
-	void *_rx, *_rx_hw;
-	void *_tx, *_tx_hw;
-	void *txbuf_adr;
-	void *rxbuf_adr;
-	struct grcan_msg *rx;
-	struct grcan_msg *tx;
-	unsigned int rxbuf_size;    /* requested RX buf size in bytes */
-	unsigned int txbuf_size;    /* requested TX buf size in bytes */
-
-	int txblock, rxblock;
-	int txcomplete, rxcomplete;
-
-	struct grcan_filter sfilter;
-	struct grcan_filter afilter;
-	int config_changed; /* 0=no changes, 1=changes ==> a Core reset is needed */
-	struct grcan_config config;
-	struct grcan_stats stats;
-
-	rtems_id rx_sem, tx_sem, txempty_sem, dev_sem;
-	SPIN_DECLARE(devlock);
-};
-
 static void __inline__ grcan_hw_reset(struct grcan_regs *regs);
 
 static int grcan_hw_read_try(
@@ -162,12 +92,6 @@ static int grcan_hw_write_try(
 	CANMsg *buffer,
 	int count);
 
-static int grcan_hw_write_try_fd(
-	struct grcan_priv *pDev,
-	struct grcan_regs *regs,
-	CANFDMsg *buffer,
-	int count);
-
 static void grcan_hw_config(
 	struct grcan_priv *pDev,
 	struct grcan_config *conf);
@@ -182,26 +106,10 @@ static void grcan_hw_sync(
 
 static void grcan_interrupt(void *arg);
 
-#ifdef GRCAN_REG_BYPASS_CACHE
-#define READ_REG(address) grlib_read_uncached32((unsigned int)(address))
-#else
-#define READ_REG(address) (*(volatile unsigned int *)(address))
-#endif
-
-#ifdef GRCAN_DMA_BYPASS_CACHE
-#define READ_DMA_DOUBLE(address) grlib_read_uncached64((uint64_t *)(address))
-#define READ_DMA_WORD(address) grlib_read_uncached32((unsigned int)(address))
-#define READ_DMA_BYTE(address) grlib_read_uncached8((unsigned int)(address))
-#else
-#define READ_DMA_DOUBLE(address) (*(volatile uint64_t *)(address))
-#define READ_DMA_WORD(address) (*(volatile unsigned int *)(address))
-#define READ_DMA_BYTE(address) (*(volatile unsigned char *)(address))
-#endif
-
 #define NELEM(a) ((int) (sizeof (a) / sizeof (a[0])))
 
 /* GRCAN nominal boundaries for baud-rate paramters */
-static struct grlib_canbtrs_ranges grcan_btrs_ranges = {
+struct grlib_canbtrs_ranges grcan_btrs_ranges = {
 	.max_scaler = 256*8, /* scaler is multiplied by BPR in steps 1,2,4,8 */
 	.has_bpr = 1,
 	.divfactor = 2,
@@ -593,50 +501,6 @@ static void grcan_hw_sync(struct grcan_regs *regs, struct grcan_filter *sfilter)
 	regs->smask = sfilter->mask;
 }
 
-static unsigned int grcan_hw_rxavail(
-	unsigned int rp,
-	unsigned int wp, unsigned int size
-)
-{
-	if (rp == wp) {
-		/* read pointer and write pointer is equal only
-		 * when RX buffer is empty.
-		 */
-		return 0;
-	}
-
-	if (wp > rp) {
-		return (wp - rp) / GRCAN_MSG_SIZE;
-	} else {
-		return (size - (rp - wp)) / GRCAN_MSG_SIZE;
-	}
-}
-
-static unsigned int grcan_hw_txspace(
-	unsigned int rp,
-	unsigned int wp,
-	unsigned int size
-)
-{
-	unsigned int left;
-
-	if (rp == wp) {
-		/* read pointer and write pointer is equal only
-		 * when TX buffer is empty.
-		 */
-		return size / GRCAN_MSG_SIZE - WRAP_AROUND_TX_MSGS;
-	}
-
-	/* size - 4 - abs(read-write) */
-	if (wp > rp) {
-		left = size - (wp - rp);
-	} else {
-		left = rp - wp;
-	}
-
-	return left / GRCAN_MSG_SIZE - WRAP_AROUND_TX_MSGS;
-}
-
 static int grcan_hw_read_try(
 	struct grcan_priv *pDev,
 	struct grcan_regs *regs,
@@ -813,241 +677,7 @@ static int grcan_hw_write_try(
 	return ret;
 }
 
-static uint8_t dlc2len[16] = {
-	0, 1, 2, 3,
-	4, 5, 6, 7,
-	8, 12, 16, 20,
-	24, 32, 48, 64
-};
-
-static uint8_t len2fddlc[14] = {
- /* 12,13 */	0x9,
- /* 16,17 */	0xA,
- /* 20,21 */	0xB,
- /* 24,25 */	0xC,
- /* 28,29 */	-1,
- /* 32,33 */	0xD,
- /* 36,37 */	-1,
- /* 40,41 */	-1,
- /* 44,45 */	-1,
- /* 48,49 */	0xE,
- /* 52,53 */	-1,
- /* 56,57 */	-1,
- /* 60,61 */	-1,
- /* 64,65 */	0xF,
-};
-
-/* Convert length in bytes to descriptor length field */
-static inline uint8_t grcan_len2dlc(int len)
-{
-	if (len <= 8)
-		return len;
-	if (len > 64)
-		return -1;
-	if (len & 0x3)
-		return -1;
-	return len2fddlc[(len - 12) >> 2];
-}
-
-static inline int grcan_numbds(int len)
-{
-	return 1 + ((len + 7) >> 4);
-}
-
-static int grcan_hw_read_try_fd(
-	struct grcan_priv *pDev,
-	struct grcan_regs *regs,
-	CANFDMsg * buffer,
-	int max)
-{
-	int j;
-	CANFDMsg *dest;
-	struct grcanfd_bd0 *source, tmp, *rxmax;
-	unsigned int wp, rp, size, addr;
-	int bds_hw_avail, bds_tot, bds, ret, dlc;
-	uint64_t *dp;
-	SPIN_IRQFLAGS(oldLevel);
-
-	FUNCDBG();
-
-	wp = READ_REG(&regs->rx0wr);
-	rp = READ_REG(&regs->rx0rd);
-
-	/*
-	 * Due to hardware wrap around simplification write pointer will
-	 * never reach the read pointer, at least a gap of 8 bytes.
-	 * The only time they are equal is when the read pointer has
-	 * reached the write pointer (empty buffer)
-	 *
-	 */
-	if (wp != rp) {
-		/* Not empty, we have received chars...
-		 * Read as much as possible from DMA buffer
-		 */
-		size = READ_REG(&regs->rx0size);
-
-		/* Get number of bytes available in RX buffer */
-		bds_hw_avail = grcan_hw_rxavail(rp, wp, size);
-
-		addr = (unsigned int)pDev->rx;
-		source = (struct grcanfd_bd0 *)(addr + rp);
-		dest = buffer;
-		rxmax = (struct grcanfd_bd0 *)(addr + size);
-		ret = bds_tot = 0;
-
-		/* Read as many can messages as possible */
-		while ((ret < max) && (bds_tot < bds_hw_avail)) {
-			/* Read CAN message from DMA buffer */
-			*(uint64_t *)&tmp = READ_DMA_DOUBLE(source);
-			if (tmp.head[1] & 0x4) {
-				DBGC(DBG_RX, "overrun\n");
-			}
-			if (tmp.head[1] & 0x2) {
-				DBGC(DBG_RX, "bus-off mode\n");
-			}
-			if (tmp.head[1] & 0x1) {
-				DBGC(DBG_RX, "error-passive mode\n");
-			}
-			/* Convert one grcan CAN message to one "software" CAN message */
-			dest->extended = tmp.head[0] >> 31;
-			dest->rtr = (tmp.head[0] >> 30) & 0x1;
-			if (dest->extended) {
-				dest->id = tmp.head[0] & 0x3fffffff;
-			} else {
-				dest->id = (tmp.head[0] >> 18) & 0xfff;
-			}
-			dest->fdopts = (tmp.head[1] >> 25) & GRCAN_FDMASK;
-			dlc = tmp.head[1] >> 28;
-			if (dest->fdopts & GRCAN_FDOPT_FDFRM) {
-				dest->len = dlc2len[dlc];
-			} else {
-				dest->len = dlc;
-				if (dlc > 8)
-					dest->len = 8;
-			}
-
-			dp = (uint64_t *)&source->data0;
-			for (j = 0; j < ((dest->len + 7) / 8); j++) {
-				dest->data.dwords[j] = READ_DMA_DOUBLE(dp);
-				if (++dp >= (uint64_t *)rxmax)
-					dp = (uint64_t *)addr; /* wrap around */
-			}
-
-			/* wrap around if neccessary */
-			bds = grcan_numbds(dest->len);
-			source += bds;
-			if (source >= rxmax) {
-				source = (struct grcanfd_bd0 *)
-					 ((void *)source - size);
-			}
-			dest++;	/* straight user buffer */
-			ret++;
-			bds_tot += bds;
-		}
-
-		/* A bus off interrupt may have occured after checking pDev->started */
-		SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
-		if (pDev->started == STATE_STARTED) {
-			regs->rx0rd = (unsigned int) source - addr;
-			regs->rx0ctrl = GRCAN_RXCTRL_ENABLE;
-		} else {
-			DBGC(DBG_STATE, "cancelled due to a BUS OFF error\n");
-			ret = state2err[pDev->started];
-		}
-		SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
-
-		return ret;
-	}
-	return 0;
-}
-
-static int grcan_hw_write_try_fd(
-	struct grcan_priv *pDev,
-	struct grcan_regs *regs,
-	CANFDMsg *buffer,
-	int count)
-{
-	unsigned int rp, wp, size, addr;
-	int ret;
-	struct grcanfd_bd0 *dest, *txmax;
-	CANFDMsg *source = (CANFDMsg *) buffer;
-	int space_left;
-	unsigned int tmp;
-	int i, bds;
-	uint64_t *dp;
-	uint8_t dlc;
-	SPIN_IRQFLAGS(oldLevel);
-
-	DBGC(DBG_TX, "\n");
-
-	rp = READ_REG(&regs->tx0rd);
-	wp = READ_REG(&regs->tx0wr);
-	size = READ_REG(&regs->tx0size);
-	space_left = grcan_hw_txspace(rp, wp, size);
-
-	addr = (unsigned int)pDev->tx;
-	dest = (struct grcanfd_bd0 *)(addr + wp);
-	txmax = (struct grcanfd_bd0 *)(addr + size);
-	ret = 0;
-
-	while (source < &buffer[count]) {
-		/* Get the number of descriptors to wait for */
-		if (source->fdopts & GRCAN_FDOPT_FDFRM)
-			bds = grcan_numbds(source->len); /* next msg's buffers */
-		else
-			bds = 1;
-		if (space_left < bds)
-			break;
-
-		/* Convert and write CAN message to DMA buffer */
-		dlc = grcan_len2dlc(source->len);
-		if (dlc < 0) {
-			/* Bad user input. Report the number of written messages
-			 * or an error when non sent.
-			 */
-			if (ret <= 0)
-				return GRCAN_RET_INVARG;
-			break;
-		}
-		dest->head[1] = (dlc << 28) |
-				((source->fdopts & GRCAN_FDMASK) << 25);
-		dp = &dest->data0;
-		for (i = 0; i < ((source->len + 7) / 8); i++) {
-			*dp++ = source->data.dwords[i];
-			if (dp >= (uint64_t *)txmax)
-				dp = (uint64_t *)addr; /* wrap around */
-		}
-		if (source->extended) {
-			tmp = (1 << 31) | (source->id & 0x3fffffff);
-		} else {
-			tmp = (source->id & 0xfff) << 18;
-		}
-		if (source->rtr)
-			tmp |= (1 << 30);
-		dest->head[0] = tmp;
-		source++;	/* straight user buffer */
-		dest += bds;
-		if (dest >= txmax)
-			dest = (struct grcanfd_bd0 *)((void *)dest - size);
-		space_left -= bds;
-		ret++;
-	}
-
-	/* A bus off interrupt may have occured after checking pDev->started */
-	SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
-	if (pDev->started == STATE_STARTED) {
-		regs->tx0wr = (unsigned int) dest - addr;
-		regs->tx0ctrl = GRCAN_TXCTRL_ENABLE;
-	} else {
-		DBGC(DBG_STATE, "cancelled due to a BUS OFF error\n");
-		ret = state2err[pDev->started];
-	}
-	SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
-
-	return ret;
-}
-
-static int grcan_wait_rxdata(struct grcan_priv *pDev, int min)
+int grcan_wait_rxdata(struct grcan_priv *pDev, int min)
 {
 	unsigned int wp, rp, size, irq;
 	unsigned int irq_trunk, dataavail;
@@ -1130,7 +760,7 @@ static int grcan_wait_rxdata(struct grcan_priv *pDev, int min)
  * min must be at least WRAP_AROUND_TX_MSGS less than max buffer capacity
  * (pDev->txbuf_size/GRCAN_MSG_SIZE) for this algo to work.
  */
-static int grcan_wait_txspace(struct grcan_priv *pDev, int min)
+int grcan_wait_txspace(struct grcan_priv *pDev, int min)
 {
 	int wait, state;
 	unsigned int irq, rp, wp, size, space_left;
@@ -1407,6 +1037,7 @@ void *grcan_open(int dev_no)
 	struct grcan_priv *pDev;
 	void *ret;
 	union drvmgr_key_value *value;
+	struct grlib_canbtrs_ranges *br;
 
 	FUNCDBG();
 
@@ -1478,10 +1109,13 @@ void *grcan_open(int dev_no)
 	pDev->sfilter.code = 0x00000000;
 
 	/* Calculate default timing register values */
+	if (pDev->fd_capable)
+		br = &grcanfd_nom_btrs_ranges;
+	else
+		br = &grcan_btrs_ranges;
 	grlib_canbtrs_calc_timing(
-		GRCAN_DEFAULT_BAUD, pDev->corefreq_hz,
-		GRCAN_SAMPLING_POINT, &grcan_btrs_ranges,
-		(struct grlib_canbtrs_timing *)&pDev->config.timing);
+		GRCAN_DEFAULT_BAUD, pDev->corefreq_hz, GRCAN_SAMPLING_POINT,
+		br, (struct grlib_canbtrs_timing *)&pDev->config.timing);
 
 	if ( grcan_alloc_buffers(pDev,1,1) ) {
 		ret = NULL;
@@ -1604,88 +1238,6 @@ int grcan_read(void *d, CANMsg *msg, size_t ucount)
 	return count;
 }
 
-int grcanfd_read(void *d, CANFDMsg *msg, size_t ucount)
-{
-	struct grcan_priv *pDev = d;
-	CANFDMsg *dest;
-	unsigned int count, left;
-	int nread;
-	int req_cnt;
-
-	FUNCDBG();
-
-	dest = msg;
-	req_cnt = ucount;
-
-	if ( (!dest) || (req_cnt<1) )
-		return GRCAN_RET_INVARG;
-
-	if (pDev->started != STATE_STARTED) {
-		return GRCAN_RET_NOTSTARTED;
-	}
-
-	DBGC(DBG_RX, "grcan_read [%p]: buf: %p len: %u\n", d, msg, (unsigned int) ucount);
-
-	nread = grcan_hw_read_try_fd(pDev,pDev->regs,dest,req_cnt);
-	if (nread < 0) {
-		return nread;
-	}
-	count = nread;
-	if ( !( pDev->rxblock && pDev->rxcomplete && (count!=req_cnt) ) ){
-		if ( count > 0 ) {
-			/* Successfully received messages (at least one) */
-			return count;
-		}
-
-		/* nothing read, shall we block? */
-		if ( !pDev->rxblock ) {
-			/* non-blocking mode */
-			return GRCAN_RET_TIMEOUT;
-		}
-	}
-
-	while (count == 0 || (pDev->rxcomplete && (count!=req_cnt))) {
-		if (!pDev->rxcomplete) {
-			left = 1; /* return as soon as there is one message available */
-		} else {
-			left = req_cnt - count;     /* return as soon as all data are available */
-
-			/* never wait for more than the half the maximum size of the receive buffer 
-			 * Why? We need some time to copy buffer before to catch up with hw,
-			 * otherwise we would have to copy everything when the data has been
-			 * received.
-			 */
-			if (left > ((pDev->rxbuf_size/GRCAN_MSG_SIZE) / 2)){
-				left = (pDev->rxbuf_size/GRCAN_MSG_SIZE) / 2;
-			}
-		}
-
-		nread = grcan_wait_rxdata(pDev, left);
-		if (nread) {
-			/* The wait has been aborted, probably due to
-			 * the device driver has been closed by another
-			 * thread or a bus-off. Return error code.
-			 */
-			return nread;
-		}
-
-		/* Try read bytes from circular buffer */
-		nread = grcan_hw_read_try_fd(
-				pDev,
-				pDev->regs,
-				dest+count,
-				req_cnt-count);
-
-		if (nread < 0) {
-			/* The read was aborted by bus-off. */
-			return nread;
-		}
-		count += nread;
-	}
-	/* no need to unmask IRQ as IRQ Handler do that for us. */
-	return count;
-}
-
 int grcan_write(void *d, CANMsg *msg, size_t ucount)
 {
 	struct grcan_priv *pDev = d;
@@ -1774,103 +1326,6 @@ int grcan_write(void *d, CANMsg *msg, size_t ucount)
 	return count;
 }
 
-int grcanfd_write(
-	void *d,
-	CANFDMsg *msg,
-	size_t ucount)
-{
-	struct grcan_priv *pDev = d;
-	CANFDMsg *source, *curr;
-	unsigned int count, left;
-	int nwritten;
-	int req_cnt;
-
-	DBGC(DBG_TX,"\n");
-
-	if ((pDev->started != STATE_STARTED) || pDev->config.silent || pDev->flushing)
-		return GRCAN_RET_NOTSTARTED;
-
-	req_cnt = ucount;
-	curr = source = (CANFDMsg *) msg;
-
-	/* check proper length and buffer pointer */
-	if (( req_cnt < 1) || (source == NULL) ){
-		return GRCAN_RET_INVARG;
-	}
-
-	nwritten = grcan_hw_write_try_fd(pDev,pDev->regs,source,req_cnt);
-	if (nwritten < 0) {
-		return nwritten;
-	}
-	count = nwritten;
-	if ( !(pDev->txblock && pDev->txcomplete && (count!=req_cnt)) ) {
-		if ( count > 0 ) {
-			/* Successfully transmitted chars (at least one char) */
-			return count;
-		}
-
-		/* nothing written, shall we block? */
-		if ( !pDev->txblock ) {
-			/* non-blocking mode */
-			return GRCAN_RET_TIMEOUT;
-		}
-	}
-
-	/* if in txcomplete mode we need to transmit all chars */
-	while((count == 0) || (pDev->txcomplete && (count!=req_cnt)) ){
-		/*** block until room to fit all or as much of transmit buffer
-		 * as possible before IRQ comes. Set up a valid IRQ point so
-		 * that an IRQ is triggered when we can put a chunk of data
-		 * into transmit fifo.
-		 */
-
-		/* Get the number of descriptors to wait for */
-		curr = &source[count];
-		if (curr->fdopts & GRCAN_FDOPT_FDFRM)
-			left = grcan_numbds(curr->len); /* next msg's buffers */
-		else
-			left = 1;
-
-		if (pDev->txcomplete) {
-			/* Wait for all messages to fit into descriptor table.
-			 * Assume all following msgs are single descriptors.
-			 */
-			left += req_cnt - count - 1;
-			if (left > ((pDev->txbuf_size/GRCAN_MSG_SIZE)/2)) {
-				left = (pDev->txbuf_size/GRCAN_MSG_SIZE)/2;
-			}
-
-		}
-
-		nwritten = grcan_wait_txspace(pDev,left);
-		/* Wait until more room in transmit buffer */
-		if ( nwritten ) {
-			/* The wait has been aborted, probably due to 
-			 * the device driver has been closed by another
-			 * thread. To avoid deadlock we return directly
-			 * with error status.
-			 */
-			return nwritten;
-		}
-
-		/* Try read bytes from circular buffer */
-		nwritten = grcan_hw_write_try_fd(
-			pDev,
-			pDev->regs,
-			source+count,
-			req_cnt-count);
-
-		if (nwritten < 0) {
-			/* Write was aborted by bus-off. */
-			return nwritten;
-		}
-		count += nwritten;
-	}
-	/* no need to unmask IRQ as IRQ Handler do that for us. */
-
-	return count;
-}
-
 int grcan_start(void *d)
 {
 	struct grcan_priv *pDev = d;
@@ -2144,77 +1599,6 @@ int grcan_set_btrs(void *d, const struct grcan_timing *timing)
 	return 0;
 }
 
-int grcanfd_set_speed(void *d, unsigned int nom_hz, unsigned int fd_hz)
-{
-	struct grcan_priv *pDev = d;
-	struct grlib_canbtrs_timing nom, fd;
-	int ret;
-
-	FUNCDBG();
-
-	/* cannot change speed during run mode */
-	if ((pDev->started == STATE_STARTED) || !pDev->fd_capable)
-		return -1;
-
-	/* get speed rate from argument */
-	ret = grlib_canbtrs_calc_timing(
-		nom_hz, pDev->corefreq_hz, GRCAN_SAMPLING_POINT,
-		&grcanfd_nom_btrs_ranges, &nom);
-	if ( ret )
-		return -2;
-	ret = grlib_canbtrs_calc_timing(
-		fd_hz, pDev->corefreq_hz, GRCAN_SAMPLING_POINT,
-		&grcanfd_fd_btrs_ranges, &fd);
-	if ( ret )
-		return -2;
-
-	/* save timing/speed */
-	pDev->config.timing = *(struct grcan_timing *)&nom;
-	pDev->config.timing_fd.scaler = fd.scaler;
-	pDev->config.timing_fd.ps1 = fd.ps1;
-	pDev->config.timing_fd.ps2 = fd.ps2;
-	pDev->config.timing_fd.sjw = fd.rsj;
-	pDev->config.timing_fd.resv_zero = 0;
-	pDev->config_changed = 1;
-
-	return 0;
-
-}
-
-int grcanfd_set_btrs(
-	void *d,
-	const struct grcanfd_timing *nominal,
-	const struct grcanfd_timing *fd)
-{
-	struct grcan_priv *pDev = d;
-
-	FUNCDBG();
-
-	/* Set BTR registers manually
-	 * Read GRCAN/HurriCANe Manual.
-	 */
-	if ((pDev->started == STATE_STARTED) || !pDev->fd_capable)
-		return -1;
-
-	if (!nominal)
-		return -2;
-
-	pDev->config.timing.scaler = nominal->scaler;
-	pDev->config.timing.ps1 = nominal->ps1;
-	pDev->config.timing.ps2 = nominal->ps2;
-	pDev->config.timing.rsj = nominal->sjw;
-	pDev->config.timing.bpr = 0;
-	if (fd) {
-		pDev->config.timing_fd = *fd;
-	} else {
-		memset(&pDev->config.timing_fd, 0,
-			sizeof(struct grcanfd_timing));
-	}
-	pDev->config_changed = 1;
-
-	return 0;
-}
-
 int grcan_set_afilter(void *d, const struct grcan_filter *filter)
 {
 	struct grcan_priv *pDev = d;
diff --git a/bsps/shared/grlib/can/grcan_internal.h b/bsps/shared/grlib/can/grcan_internal.h
new file mode 100644
index 0000000..86ccda1
--- /dev/null
+++ b/bsps/shared/grlib/can/grcan_internal.h
@@ -0,0 +1,140 @@
+/*
+ *  GRCAN driver
+ *
+ *  COPYRIGHT (c) 2007-2019.
+ *  Cobham Gaisler AB.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#ifndef GRCAN_DEFAULT_BAUD
+ /* default to 500kbits/s */
+ #define GRCAN_DEFAULT_BAUD 500000
+#endif
+
+#ifndef GRCAN_SAMPLING_POINT
+ #define GRCAN_SAMPLING_POINT 80
+#endif
+
+#define WRAP_AROUND_TX_MSGS 1
+#define WRAP_AROUND_RX_MSGS 2
+#define GRCAN_MSG_SIZE sizeof(struct grcan_msg)
+
+struct grcan_msg {
+	unsigned int head[2];
+	unsigned char data[8];
+};
+
+struct grcan_config {
+	struct grcan_timing timing;
+	struct grcanfd_timing timing_fd;
+	struct grcan_selection selection;
+	int abort;
+	int silent;
+};
+
+struct grcan_priv {
+	struct drvmgr_dev *dev;	/* Driver manager device */
+	char devName[32];	/* Device Name */
+	unsigned int baseaddr, ram_base;
+	struct grcan_regs *regs;
+	int irq;
+	int minor;
+	int open;
+	int started;
+	unsigned int channel;
+	int flushing;
+	unsigned int corefreq_hz;
+	int fd_capable;
+
+	/* Circular DMA buffers */
+	void *_rx, *_rx_hw;
+	void *_tx, *_tx_hw;
+	void *txbuf_adr;
+	void *rxbuf_adr;
+	struct grcan_msg *rx;
+	struct grcan_msg *tx;
+	unsigned int rxbuf_size;    /* requested RX buf size in bytes */
+	unsigned int txbuf_size;    /* requested TX buf size in bytes */
+
+	int txblock, rxblock;
+	int txcomplete, rxcomplete;
+
+	struct grcan_filter sfilter;
+	struct grcan_filter afilter;
+	int config_changed; /* 0=no changes, 1=changes ==> a Core reset is needed */
+	struct grcan_config config;
+	struct grcan_stats stats;
+
+	rtems_id rx_sem, tx_sem, txempty_sem, dev_sem;
+	SPIN_DECLARE(devlock);
+};
+
+#ifdef GRCAN_REG_BYPASS_CACHE
+#define READ_REG(address) grlib_read_uncached32((unsigned int)(address))
+#else
+#define READ_REG(address) (*(volatile unsigned int *)(address))
+#endif
+
+#ifdef GRCAN_DMA_BYPASS_CACHE
+#define READ_DMA_DOUBLE(address) grlib_read_uncached64((uint64_t *)(address))
+#define READ_DMA_WORD(address) grlib_read_uncached32((unsigned int)(address))
+#define READ_DMA_BYTE(address) grlib_read_uncached8((unsigned int)(address))
+#else
+#define READ_DMA_DOUBLE(address) (*(volatile uint64_t *)(address))
+#define READ_DMA_WORD(address) (*(volatile unsigned int *)(address))
+#define READ_DMA_BYTE(address) (*(volatile unsigned char *)(address))
+#endif
+
+extern int state2err[4];
+extern struct grlib_canbtrs_ranges grcan_btrs_ranges;
+extern struct grlib_canbtrs_ranges grcanfd_nom_btrs_ranges;
+extern struct grlib_canbtrs_ranges grcanfd_fd_btrs_ranges;
+
+int grcan_wait_rxdata(struct grcan_priv *pDev, int min);
+int grcan_wait_txspace(struct grcan_priv *pDev, int min);
+
+static inline unsigned int grcan_hw_rxavail(
+	unsigned int rp,
+	unsigned int wp,
+	unsigned int size)
+{
+	if (rp == wp) {
+		/* read pointer and write pointer is equal only
+		 * when RX buffer is empty.
+		 */
+		return 0;
+	}
+
+	if (wp > rp) {
+		return (wp - rp) / GRCAN_MSG_SIZE;
+	} else {
+		return (size - (rp - wp)) / GRCAN_MSG_SIZE;
+	}
+}
+
+static inline unsigned int grcan_hw_txspace(
+	unsigned int rp,
+	unsigned int wp,
+	unsigned int size)
+{
+	unsigned int left;
+
+	if (rp == wp) {
+		/* read pointer and write pointer is equal only
+		 * when TX buffer is empty.
+		 */
+		return size / GRCAN_MSG_SIZE - WRAP_AROUND_TX_MSGS;
+	}
+
+	/* size - 4 - abs(read-write) */
+	if (wp > rp) {
+		left = size - (wp - rp);
+	} else {
+		left = rp - wp;
+	}
+
+	return left / GRCAN_MSG_SIZE - WRAP_AROUND_TX_MSGS;
+}
diff --git a/bsps/shared/grlib/can/grcanfd.c b/bsps/shared/grlib/can/grcanfd.c
new file mode 100644
index 0000000..00eb4b6
--- /dev/null
+++ b/bsps/shared/grlib/can/grcanfd.c
@@ -0,0 +1,535 @@
+/*
+ *  FD extenstions to the GRCAN driver
+ *
+ *  COPYRIGHT (c) 2007-2019.
+ *  Cobham Gaisler AB.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <bsp.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <ctype.h>
+#include <rtems/bspIo.h>
+
+#include <grlib/grcan.h>
+#include <grlib/canbtrs.h>
+#include <drvmgr/drvmgr.h>
+#include <grlib/ambapp_bus.h>
+#include <grlib/ambapp.h>
+
+#include <grlib/grlib_impl.h>
+#include "grcan_internal.h"
+
+/* Uncomment for debug output */
+/****************** DEBUG Definitions ********************/
+#define DBG_TX 2
+#define DBG_RX 4
+#define DBG_STATE 8
+
+#define DEBUG_FLAGS (DBG_STATE | DBG_RX | DBG_TX )
+/*
+#define DEBUG
+#define DEBUGFUNCS
+*/
+#include <grlib/debug_defs.h>
+
+/*********************************************************/
+
+struct grcanfd_bd0 {
+	uint32_t head[2];
+	uint64_t data0; /* variable size, from 1 to 8 dwords */
+};
+
+struct grcanfd_bd1 {
+	unsigned long long data[2];
+};
+
+static uint8_t dlc2len[16] = {
+	0, 1, 2, 3,
+	4, 5, 6, 7,
+	8, 12, 16, 20,
+	24, 32, 48, 64
+};
+
+static uint8_t len2fddlc[14] = {
+ /* 12,13 */	0x9,
+ /* 16,17 */	0xA,
+ /* 20,21 */	0xB,
+ /* 24,25 */	0xC,
+ /* 28,29 */	-1,
+ /* 32,33 */	0xD,
+ /* 36,37 */	-1,
+ /* 40,41 */	-1,
+ /* 44,45 */	-1,
+ /* 48,49 */	0xE,
+ /* 52,53 */	-1,
+ /* 56,57 */	-1,
+ /* 60,61 */	-1,
+ /* 64,65 */	0xF,
+};
+
+/* Convert length in bytes to descriptor length field */
+static inline uint8_t grcan_len2dlc(int len)
+{
+	if (len <= 8)
+		return len;
+	if (len > 64)
+		return -1;
+	if (len & 0x3)
+		return -1;
+	return len2fddlc[(len - 12) >> 2];
+}
+
+static inline int grcan_numbds(int len)
+{
+	return 1 + ((len + 7) >> 4);
+}
+
+static int grcan_hw_read_try_fd(
+	struct grcan_priv *pDev,
+	struct grcan_regs *regs,
+	CANFDMsg * buffer,
+	int max)
+{
+	int j;
+	CANFDMsg *dest;
+	struct grcanfd_bd0 *source, tmp, *rxmax;
+	unsigned int wp, rp, size, addr;
+	int bds_hw_avail, bds_tot, bds, ret, dlc;
+	uint64_t *dp;
+	SPIN_IRQFLAGS(oldLevel);
+
+	FUNCDBG();
+
+	wp = READ_REG(&regs->rx0wr);
+	rp = READ_REG(&regs->rx0rd);
+
+	/*
+	 * Due to hardware wrap around simplification write pointer will
+	 * never reach the read pointer, at least a gap of 8 bytes.
+	 * The only time they are equal is when the read pointer has
+	 * reached the write pointer (empty buffer)
+	 *
+	 */
+	if (wp != rp) {
+		/* Not empty, we have received chars...
+		 * Read as much as possible from DMA buffer
+		 */
+		size = READ_REG(&regs->rx0size);
+
+		/* Get number of bytes available in RX buffer */
+		bds_hw_avail = grcan_hw_rxavail(rp, wp, size);
+
+		addr = (unsigned int)pDev->rx;
+		source = (struct grcanfd_bd0 *)(addr + rp);
+		dest = buffer;
+		rxmax = (struct grcanfd_bd0 *)(addr + size);
+		ret = bds_tot = 0;
+
+		/* Read as many can messages as possible */
+		while ((ret < max) && (bds_tot < bds_hw_avail)) {
+			/* Read CAN message from DMA buffer */
+			*(uint64_t *)&tmp = READ_DMA_DOUBLE(source);
+			if (tmp.head[1] & 0x4) {
+				DBGC(DBG_RX, "overrun\n");
+			}
+			if (tmp.head[1] & 0x2) {
+				DBGC(DBG_RX, "bus-off mode\n");
+			}
+			if (tmp.head[1] & 0x1) {
+				DBGC(DBG_RX, "error-passive mode\n");
+			}
+			/* Convert one grcan CAN message to one "software" CAN message */
+			dest->extended = tmp.head[0] >> 31;
+			dest->rtr = (tmp.head[0] >> 30) & 0x1;
+			if (dest->extended) {
+				dest->id = tmp.head[0] & 0x3fffffff;
+			} else {
+				dest->id = (tmp.head[0] >> 18) & 0xfff;
+			}
+			dest->fdopts = (tmp.head[1] >> 25) & GRCAN_FDMASK;
+			dlc = tmp.head[1] >> 28;
+			if (dest->fdopts & GRCAN_FDOPT_FDFRM) {
+				dest->len = dlc2len[dlc];
+			} else {
+				dest->len = dlc;
+				if (dlc > 8)
+					dest->len = 8;
+			}
+
+			dp = (uint64_t *)&source->data0;
+			for (j = 0; j < ((dest->len + 7) / 8); j++) {
+				dest->data.dwords[j] = READ_DMA_DOUBLE(dp);
+				if (++dp >= (uint64_t *)rxmax)
+					dp = (uint64_t *)addr; /* wrap around */
+			}
+
+			/* wrap around if neccessary */
+			bds = grcan_numbds(dest->len);
+			source += bds;
+			if (source >= rxmax) {
+				source = (struct grcanfd_bd0 *)
+					 ((void *)source - size);
+			}
+			dest++;	/* straight user buffer */
+			ret++;
+			bds_tot += bds;
+		}
+
+		/* A bus off interrupt may have occured after checking pDev->started */
+		SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+		if (pDev->started == STATE_STARTED) {
+			regs->rx0rd = (unsigned int) source - addr;
+			regs->rx0ctrl = GRCAN_RXCTRL_ENABLE;
+		} else {
+			DBGC(DBG_STATE, "cancelled due to a BUS OFF error\n");
+			ret = state2err[pDev->started];
+		}
+		SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+
+		return ret;
+	}
+	return 0;
+}
+
+int grcanfd_read(void *d, CANFDMsg *msg, size_t ucount)
+{
+	struct grcan_priv *pDev = d;
+	CANFDMsg *dest;
+	unsigned int count, left;
+	int nread;
+	int req_cnt;
+
+	FUNCDBG();
+
+	dest = msg;
+	req_cnt = ucount;
+
+	if ( (!dest) || (req_cnt<1) )
+		return GRCAN_RET_INVARG;
+
+	if (pDev->started != STATE_STARTED) {
+		return GRCAN_RET_NOTSTARTED;
+	}
+
+	DBGC(DBG_RX, "grcan_read [%p]: buf: %p len: %u\n", d, msg, (unsigned int) ucount);
+
+	nread = grcan_hw_read_try_fd(pDev,pDev->regs,dest,req_cnt);
+	if (nread < 0) {
+		return nread;
+	}
+	count = nread;
+	if ( !( pDev->rxblock && pDev->rxcomplete && (count!=req_cnt) ) ){
+		if ( count > 0 ) {
+			/* Successfully received messages (at least one) */
+			return count;
+		}
+
+		/* nothing read, shall we block? */
+		if ( !pDev->rxblock ) {
+			/* non-blocking mode */
+			return GRCAN_RET_TIMEOUT;
+		}
+	}
+
+	while (count == 0 || (pDev->rxcomplete && (count!=req_cnt))) {
+		if (!pDev->rxcomplete) {
+			left = 1; /* return as soon as there is one message available */
+		} else {
+			left = req_cnt - count;     /* return as soon as all data are available */
+
+			/* never wait for more than the half the maximum size of the receive buffer 
+			 * Why? We need some time to copy buffer before to catch up with hw,
+			 * otherwise we would have to copy everything when the data has been
+			 * received.
+			 */
+			if (left > ((pDev->rxbuf_size/GRCAN_MSG_SIZE) / 2)){
+				left = (pDev->rxbuf_size/GRCAN_MSG_SIZE) / 2;
+			}
+		}
+
+		nread = grcan_wait_rxdata(pDev, left);
+		if (nread) {
+			/* The wait has been aborted, probably due to
+			 * the device driver has been closed by another
+			 * thread or a bus-off. Return error code.
+			 */
+			return nread;
+		}
+
+		/* Try read bytes from circular buffer */
+		nread = grcan_hw_read_try_fd(
+				pDev,
+				pDev->regs,
+				dest+count,
+				req_cnt-count);
+
+		if (nread < 0) {
+			/* The read was aborted by bus-off. */
+			return nread;
+		}
+		count += nread;
+	}
+	/* no need to unmask IRQ as IRQ Handler do that for us. */
+	return count;
+}
+
+static int grcan_hw_write_try_fd(
+	struct grcan_priv *pDev,
+	struct grcan_regs *regs,
+	CANFDMsg *buffer,
+	int count)
+{
+	unsigned int rp, wp, size, addr;
+	int ret;
+	struct grcanfd_bd0 *dest, *txmax;
+	CANFDMsg *source = (CANFDMsg *) buffer;
+	int space_left;
+	unsigned int tmp;
+	int i, bds;
+	uint64_t *dp;
+	uint8_t dlc;
+	SPIN_IRQFLAGS(oldLevel);
+
+	DBGC(DBG_TX, "\n");
+
+	rp = READ_REG(&regs->tx0rd);
+	wp = READ_REG(&regs->tx0wr);
+	size = READ_REG(&regs->tx0size);
+	space_left = grcan_hw_txspace(rp, wp, size);
+
+	addr = (unsigned int)pDev->tx;
+	dest = (struct grcanfd_bd0 *)(addr + wp);
+	txmax = (struct grcanfd_bd0 *)(addr + size);
+	ret = 0;
+
+	while (source < &buffer[count]) {
+		/* Get the number of descriptors to wait for */
+		if (source->fdopts & GRCAN_FDOPT_FDFRM)
+			bds = grcan_numbds(source->len); /* next msg's buffers */
+		else
+			bds = 1;
+		if (space_left < bds)
+			break;
+
+		/* Convert and write CAN message to DMA buffer */
+		dlc = grcan_len2dlc(source->len);
+		if (dlc < 0) {
+			/* Bad user input. Report the number of written messages
+			 * or an error when non sent.
+			 */
+			if (ret <= 0)
+				return GRCAN_RET_INVARG;
+			break;
+		}
+		dest->head[1] = (dlc << 28) |
+				((source->fdopts & GRCAN_FDMASK) << 25);
+		dp = &dest->data0;
+		for (i = 0; i < ((source->len + 7) / 8); i++) {
+			*dp++ = source->data.dwords[i];
+			if (dp >= (uint64_t *)txmax)
+				dp = (uint64_t *)addr; /* wrap around */
+		}
+		if (source->extended) {
+			tmp = (1 << 31) | (source->id & 0x3fffffff);
+		} else {
+			tmp = (source->id & 0xfff) << 18;
+		}
+		if (source->rtr)
+			tmp |= (1 << 30);
+		dest->head[0] = tmp;
+		source++;	/* straight user buffer */
+		dest += bds;
+		if (dest >= txmax)
+			dest = (struct grcanfd_bd0 *)((void *)dest - size);
+		space_left -= bds;
+		ret++;
+	}
+
+	/* A bus off interrupt may have occured after checking pDev->started */
+	SPIN_LOCK_IRQ(&pDev->devlock, oldLevel);
+	if (pDev->started == STATE_STARTED) {
+		regs->tx0wr = (unsigned int) dest - addr;
+		regs->tx0ctrl = GRCAN_TXCTRL_ENABLE;
+	} else {
+		DBGC(DBG_STATE, "cancelled due to a BUS OFF error\n");
+		ret = state2err[pDev->started];
+	}
+	SPIN_UNLOCK_IRQ(&pDev->devlock, oldLevel);
+
+	return ret;
+}
+
+int grcanfd_write(
+	void *d,
+	CANFDMsg *msg,
+	size_t ucount)
+{
+	struct grcan_priv *pDev = d;
+	CANFDMsg *source, *curr;
+	unsigned int count, left;
+	int nwritten;
+	int req_cnt;
+
+	DBGC(DBG_TX,"\n");
+
+	if ((pDev->started != STATE_STARTED) || pDev->config.silent || pDev->flushing)
+		return GRCAN_RET_NOTSTARTED;
+
+	req_cnt = ucount;
+	curr = source = (CANFDMsg *) msg;
+
+	/* check proper length and buffer pointer */
+	if (( req_cnt < 1) || (source == NULL) ){
+		return GRCAN_RET_INVARG;
+	}
+
+	nwritten = grcan_hw_write_try_fd(pDev,pDev->regs,source,req_cnt);
+	if (nwritten < 0) {
+		return nwritten;
+	}
+	count = nwritten;
+	if ( !(pDev->txblock && pDev->txcomplete && (count!=req_cnt)) ) {
+		if ( count > 0 ) {
+			/* Successfully transmitted chars (at least one char) */
+			return count;
+		}
+
+		/* nothing written, shall we block? */
+		if ( !pDev->txblock ) {
+			/* non-blocking mode */
+			return GRCAN_RET_TIMEOUT;
+		}
+	}
+
+	/* if in txcomplete mode we need to transmit all chars */
+	while((count == 0) || (pDev->txcomplete && (count!=req_cnt)) ){
+		/*** block until room to fit all or as much of transmit buffer
+		 * as possible before IRQ comes. Set up a valid IRQ point so
+		 * that an IRQ is triggered when we can put a chunk of data
+		 * into transmit fifo.
+		 */
+
+		/* Get the number of descriptors to wait for */
+		curr = &source[count];
+		if (curr->fdopts & GRCAN_FDOPT_FDFRM)
+			left = grcan_numbds(curr->len); /* next msg's buffers */
+		else
+			left = 1;
+
+		if (pDev->txcomplete) {
+			/* Wait for all messages to fit into descriptor table.
+			 * Assume all following msgs are single descriptors.
+			 */
+			left += req_cnt - count - 1;
+			if (left > ((pDev->txbuf_size/GRCAN_MSG_SIZE)/2)) {
+				left = (pDev->txbuf_size/GRCAN_MSG_SIZE)/2;
+			}
+
+		}
+
+		nwritten = grcan_wait_txspace(pDev,left);
+		/* Wait until more room in transmit buffer */
+		if ( nwritten ) {
+			/* The wait has been aborted, probably due to 
+			 * the device driver has been closed by another
+			 * thread. To avoid deadlock we return directly
+			 * with error status.
+			 */
+			return nwritten;
+		}
+
+		/* Try read bytes from circular buffer */
+		nwritten = grcan_hw_write_try_fd(
+			pDev,
+			pDev->regs,
+			source+count,
+			req_cnt-count);
+
+		if (nwritten < 0) {
+			/* Write was aborted by bus-off. */
+			return nwritten;
+		}
+		count += nwritten;
+	}
+	/* no need to unmask IRQ as IRQ Handler do that for us. */
+
+	return count;
+}
+
+int grcanfd_set_speed(void *d, unsigned int nom_hz, unsigned int fd_hz)
+{
+	struct grcan_priv *pDev = d;
+	struct grlib_canbtrs_timing nom, fd;
+	int ret;
+
+	FUNCDBG();
+
+	/* cannot change speed during run mode */
+	if ((pDev->started == STATE_STARTED) || !pDev->fd_capable)
+		return -1;
+
+	/* get speed rate from argument */
+	ret = grlib_canbtrs_calc_timing(
+		nom_hz, pDev->corefreq_hz, GRCAN_SAMPLING_POINT,
+		&grcanfd_nom_btrs_ranges, &nom);
+	if ( ret )
+		return -2;
+	ret = grlib_canbtrs_calc_timing(
+		fd_hz, pDev->corefreq_hz, GRCAN_SAMPLING_POINT,
+		&grcanfd_fd_btrs_ranges, &fd);
+	if ( ret )
+		return -2;
+
+	/* save timing/speed */
+	pDev->config.timing = *(struct grcan_timing *)&nom;
+	pDev->config.timing_fd.scaler = fd.scaler;
+	pDev->config.timing_fd.ps1 = fd.ps1;
+	pDev->config.timing_fd.ps2 = fd.ps2;
+	pDev->config.timing_fd.sjw = fd.rsj;
+	pDev->config.timing_fd.resv_zero = 0;
+	pDev->config_changed = 1;
+
+	return 0;
+
+}
+
+int grcanfd_set_btrs(
+	void *d,
+	const struct grcanfd_timing *nominal,
+	const struct grcanfd_timing *fd)
+{
+	struct grcan_priv *pDev = d;
+
+	FUNCDBG();
+
+	/* Set BTR registers manually
+	 * Read GRCAN/HurriCANe Manual.
+	 */
+	if ((pDev->started == STATE_STARTED) || !pDev->fd_capable)
+		return -1;
+
+	if (!nominal)
+		return -2;
+
+	pDev->config.timing.scaler = nominal->scaler;
+	pDev->config.timing.ps1 = nominal->ps1;
+	pDev->config.timing.ps2 = nominal->ps2;
+	pDev->config.timing.rsj = nominal->sjw;
+	pDev->config.timing.bpr = 0;
+	if (fd) {
+		pDev->config.timing_fd = *fd;
+	} else {
+		memset(&pDev->config.timing_fd, 0,
+			sizeof(struct grcanfd_timing));
+	}
+	pDev->config_changed = 1;
+
+	return 0;
+}
-- 
2.7.4



More information about the devel mailing list