You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by gn...@apache.org on 2020/06/02 14:09:24 UTC
[incubator-nuttx] 06/31: SocketCAN initial receive working as well
This is an automated email from the ASF dual-hosted git repository.
gnutt pushed a commit to branch SocketCAN
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx.git
commit 72d10458eca8b6a3fcea4c7d3ba45883b367653d
Author: Peter van der Perk <pe...@nxp.com>
AuthorDate: Thu Feb 20 16:06:15 2020 +0100
SocketCAN initial receive working as well
---
arch/arm/src/s32k1xx/s32k1xx_flexcan.c | 462 ++++++++++++++++++++++-----------
net/can/Make.defs | 6 +-
net/can/can.h | 28 ++
net/can/can_input.c | 129 +++++++++
net/can/can_recvfrom.c | 354 +++++++++++++++++++++++++
net/can/can_sockif.c | 55 +---
net/devif/Make.defs | 6 +-
7 files changed, 836 insertions(+), 204 deletions(-)
diff --git a/arch/arm/src/s32k1xx/s32k1xx_flexcan.c b/arch/arm/src/s32k1xx/s32k1xx_flexcan.c
index 7046050..9c7a7d7 100644
--- a/arch/arm/src/s32k1xx/s32k1xx_flexcan.c
+++ b/arch/arm/src/s32k1xx/s32k1xx_flexcan.c
@@ -115,19 +115,23 @@
# error "Need at least one RX buffer"
#endif*/
-#define MaskStdID 0x000007FF;
-#define MaskExtID 0x1FFFFFFF;
+#define MaskStdID 0x000007FF
+#define MaskExtID 0x1FFFFFFF
+#define FlagEFF (1 << 31) /* Extended frame format */
+#define FlagRTR (1 << 30) /* Remote transmission request */
//Fixme nice variables/constants
-#define RxMBCount 10
-#define TxMBCount 6
-#define TotalMBcount RxMBCount + TxMBCount
-#define TXMBMask (((1 << TxMBCount)-1) << RxMBCount)
+#define RxMBCount 6
+#define FilterCount 0
+#define RxandFilterMBCount (RxMBCount + FilterCount)
+#define TxMBCount 12 //???????????? why 12 idk it works
+#define TotalMBcount RxandFilterMBCount + TxMBCount
+#define TXMBMask (((1 << TxMBCount)-1) << RxandFilterMBCount)
#define CAN_FIFO_NE (1 << 5)
#define CAN_FIFO_OV (1 << 6)
#define CAN_FIFO_WARN (1 << 7)
-#define FIFO_IFLAG1 CAN_FIFO_NE | CAN_FIFO_WARN | CAN_FIFO_OV
+#define FIFO_IFLAG1 (CAN_FIFO_NE | CAN_FIFO_WARN | CAN_FIFO_OV)
static int peak_tx_mailbox_index_ = 0;
@@ -345,6 +349,12 @@ static bool s32k1xx_txringfull(FAR struct s32k1xx_driver_s *priv);
static int s32k1xx_transmit(FAR struct s32k1xx_driver_s *priv);
static int s32k1xx_txpoll(struct net_driver_s *dev);
+/* Helper functions */
+static void s32k1xx_setenable(uint32_t enable);
+static void s32k1xx_setfreeze(uint32_t freeze);
+static uint32_t s32k1xx_waitmcr_change(uint32_t mask,
+ uint32_t target_state);
+
/* Interrupt handling */
static void s32k1xx_dispatch(FAR struct s32k1xx_driver_s *priv);
@@ -385,6 +395,7 @@ static int s32k1xx_ioctl(struct net_driver_s *dev, int cmd,
/* Initialization */
static void s32k1xx_initbuffers(struct s32k1xx_driver_s *priv);
+static int s32k1xx_initialize(struct s32k1xx_driver_s *priv);
static void s32k1xx_reset(struct s32k1xx_driver_s *priv);
/****************************************************************************
@@ -462,7 +473,7 @@ static int s32k1xx_transmit(FAR struct s32k1xx_driver_s *priv)
mbi = (getreg32(S32K1XX_CAN0_ESR2) & CAN_ESR2_LPTM_MASK) >> CAN_ESR2_LPTM_SHIFT;
}
- uint32_t mb_bit = 1 << (RxMBCount + mbi);
+ uint32_t mb_bit = 1 << (RxandFilterMBCount + mbi);
while (mbi < TxMBCount)
{
@@ -476,8 +487,9 @@ static int s32k1xx_transmit(FAR struct s32k1xx_driver_s *priv)
mbi++;
}
- if (mbi == TxMBCount)
+ if ((mbi-RxandFilterMBCount) == TxMBCount)
{
+ nwarn("No TX MB available mbi %i\r\n", mbi);
return 0; // No transmission for you!
}
@@ -644,7 +656,96 @@ static inline void s32k1xx_dispatch(FAR struct s32k1xx_driver_s *priv)
static void s32k1xx_receive(FAR struct s32k1xx_driver_s *priv)
{
#warning Missing logic
- ninfo("FLEXCAN: receive\r\n");
+ //ninfo("FLEXCAN: receive\r\n");
+
+ s32k1xx_gpiowrite(PIN_PORTD | PIN31, 1);
+
+ struct can_frame frame;
+ uint32_t flags = getreg32(S32K1XX_CAN0_IFLAG1);
+
+ if ((flags & FIFO_IFLAG1) == 0)
+ {
+ // Weird, IRQ is here but no data to read
+ return;
+ }
+
+ if (flags & CAN_FIFO_OV)
+ {
+ //error_cnt_++;
+ putreg32(CAN_FIFO_OV, S32K1XX_CAN0_IFLAG1);
+ }
+
+ if (flags & CAN_FIFO_WARN)
+ {
+ //fifo_warn_cnt_++;
+ putreg32(CAN_FIFO_WARN, S32K1XX_CAN0_IFLAG1);
+ }
+
+ if (flags & CAN_FIFO_NE)
+ {
+ struct MbRx *rf = priv->rx;
+
+ /*
+ * Read the frame contents
+ */
+
+ if (rf->CS.ide)
+ {
+ frame.can_id = MaskExtID & rf->ID.ext;
+ frame.can_id |= FlagEFF;
+ }
+ else
+ {
+ frame.can_id = MaskStdID & rf->ID.std;
+ }
+
+ if (rf->CS.rtr)
+ {
+ frame.can_id |= FlagRTR;
+ }
+
+ frame.can_dlc = rf->CS.dlc;
+
+ frame.data[0] = rf->data.b0;
+ frame.data[1] = rf->data.b1;
+ frame.data[2] = rf->data.b2;
+ frame.data[3] = rf->data.b3;
+ frame.data[4] = rf->data.b4;
+ frame.data[5] = rf->data.b5;
+ frame.data[6] = rf->data.b6;
+ frame.data[7] = rf->data.b7;
+
+
+ putreg32(CAN_FIFO_NE, S32K1XX_CAN0_IFLAG1);
+
+ /* Copy the buffer pointer to priv->dev.d_buf. Set amount of data
+ * in priv->dev.d_len
+ */
+
+ priv->dev.d_len = sizeof(struct can_frame);
+ priv->dev.d_buf =
+ (uint8_t *)s32k1xx_swap32((uint32_t)&frame); //FIXME
+
+ /* Invalidate the buffer so that the correct packet will be re-read
+ * from memory when the packet content is accessed.
+ */
+
+ up_invalidate_dcache((uintptr_t)priv->dev.d_buf,
+ (uintptr_t)priv->dev.d_buf + priv->dev.d_len);
+
+ /* Send to socket interface */
+ NETDEV_RXPACKETS(&priv->dev);
+
+ can_input(&priv->dev);
+
+
+
+
+ /*
+ * Store with timeout into the FIFO buffer and signal update event
+ */
+
+ }
}
/****************************************************************************
@@ -736,9 +837,6 @@ static void s32k1xx_flexcan_interrupt_work(FAR void *arg)
static int s32k1xx_flexcan_interrupt(int irq, FAR void *context, FAR void *arg)
{
#warning Missing logic
-
- ninfo("FLEXCAN INT %i\r\n", irq);
-
FAR struct s32k1xx_driver_s *priv = &g_flexcan[0];
uint32_t flags;
flags = getreg32(S32K1XX_CAN0_IFLAG1);
@@ -881,6 +979,24 @@ static void s32k1xx_polltimer_expiry(int argc, uint32_t arg, ...)
work_queue(ETHWORK, &priv->pollwork, s32k1xx_poll_work, priv, 0);
}
+static void s32k1xx_setenable(uint32_t enable)
+{
+ uint32_t regval;
+ if(enable)
+ {
+ regval = getreg32(S32K1XX_CAN0_MCR);
+ regval &= ~(CAN_MCR_MDIS);
+ putreg32(regval, S32K1XX_CAN0_MCR);
+ }
+ else
+ {
+ regval = getreg32(S32K1XX_CAN0_MCR);
+ regval |= CAN_MCR_MDIS;
+ putreg32(regval, S32K1XX_CAN0_MCR);
+ }
+ s32k1xx_waitmcr_change(CAN_MCR_LPMACK,1);
+}
+
static void s32k1xx_setfreeze(uint32_t freeze)
{
uint32_t regval;
@@ -945,136 +1061,13 @@ static int s32k1xx_ifup(struct net_driver_s *dev)
uint32_t regval;
#warning Missing logic
- ninfo("FLEXCAN: test ifup\r\n");
-
- /* initialize CAN device */
- //FIXME we only support a single can device for now
-
- //TEST GPIO tming
- s32k1xx_pinconfig(PIN_PORTD | PIN31 | GPIO_OUTPUT);
-
- regval = getreg32(S32K1XX_CAN0_MCR);
- regval |= CAN_MCR_MDIS;
- putreg32(regval, S32K1XX_CAN0_MCR);
-
- /* Set SYS_CLOCK src */
- regval = getreg32(S32K1XX_CAN0_CTRL1);
- regval |= CAN_CTRL1_CLKSRC;
- putreg32(regval, S32K1XX_CAN0_CTRL1);
-
- regval = getreg32(S32K1XX_CAN0_MCR);
- regval &= ~(CAN_MCR_MDIS);
- putreg32(regval, S32K1XX_CAN0_MCR);
-
-
- regval = getreg32(S32K1XX_CAN0_MCR);
- regval |= CAN_MCR_RFEN | CAN_MCR_SLFWAK | CAN_MCR_WRNEN | CAN_MCR_SRXDIS
- | CAN_MCR_IRMQ | CAN_MCR_AEN |
- (((TotalMBcount - 1) << CAN_MCR_MAXMB_SHIFT) & CAN_MCR_MAXMB_MASK);
- putreg32(regval, S32K1XX_CAN0_MCR);
-
- regval = CAN_CTRL2_RRS | CAN_CTRL2_EACEN | CAN_CTRL2_RFFN_16MB; //FIXME TASD
- putreg32(regval, S32K1XX_CAN0_CTRL2);
-
- /* Enter freeze mode */
- s32k1xx_setfreeze(1);
- if(!s32k1xx_waitfreezeack_change(1))
+ if(!s32k1xx_initialize(priv))
{
- ninfo("FLEXCAN: freeze fail\r\n");
+ nerr("initialize failed");
return -1;
}
- /*regval = getreg32(S32K1XX_CAN0_CTRL1);
- regval |= ((0 << CAN_CTRL1_PRESDIV_SHIFT) & CAN_CTRL1_PRESDIV_MASK)
- | ((46 << CAN_CTRL1_ROPSEG_SHIFT) & CAN_CTRL1_ROPSEG_MASK)
- | ((18 << CAN_CTRL1_PSEG1_SHIFT) & CAN_CTRL1_PSEG1_MASK)
- | ((12 << CAN_CTRL1_PSEG2_SHIFT) & CAN_CTRL1_PSEG2_MASK)
- | ((12 << CAN_CTRL1_RJW_SHIFT) & CAN_CTRL1_RJW_MASK)
- | CAN_CTRL1_ERRMSK
- | CAN_CTRL1_TWRNMSK
- | CAN_CTRL1_RWRNMSK;
-
- putreg32(regval, S32K1XX_CAN0_CTRL1);*/
-
- /* CAN Bit Timing (CBT) configuration for a nominal phase of 1 Mbit/s
- * with 80 time quantas,in accordance with Bosch 2012 specification,
- * sample point at 83.75% */
- regval = getreg32(S32K1XX_CAN0_CBT);
- regval |= CAN_CBT_BTF | /* Enable extended bit timing configurations for CAN-FD
- for setting up separetely nominal and data phase */
- CAN_CBT_EPRESDIV(0) | /* Prescaler divisor factor of 1 */
- CAN_CBT_EPROPSEG(46) | /* Propagation segment of 47 time quantas */
- CAN_CBT_EPSEG1(18) | /* Phase buffer segment 1 of 19 time quantas */
- CAN_CBT_EPSEG2(12) | /* Phase buffer segment 2 of 13 time quantas */
- CAN_CBT_ERJW(12); /* Resynchronization jump width same as PSEG2 */
- putreg32(regval, S32K1XX_CAN0_CBT);
-
-#ifdef CAN_FD
-
- /* Enable CAN FD feature */
- regval = getreg32(S32K1XX_CAN0_MCR);
- regval |= CAN_MCR_FDEN;
- putreg32(regval, S32K1XX_CAN0_MCR);
-
- /* CAN-FD Bit Timing (FDCBT) for a data phase of 4 Mbit/s with 20 time quantas,
- in accordance with Bosch 2012 specification, sample point at 75% */
- regval = getreg32(S32K1XX_CAN0_FDCBT);
- regval |= CAN_FDCBT_FPRESDIV(0) | /* Prescaler divisor factor of 1 */
- CAN_FDCBT_FPROPSEG(7) | /* Propagation semgment of 7 time quantas
- (only register that doesn't add 1) */
- CAN_FDCBT_FPSEG1(6) | /* Phase buffer segment 1 of 7 time quantas */
- CAN_FDCBT_FPSEG2(4) | /* Phase buffer segment 2 of 5 time quantas */
- CAN_FDCBT_FRJW(4); /* Resynchorinzation jump width same as PSEG2 */
- putreg32(regval, S32K1XX_CAN0_FDCBT);
-
- /* Additional CAN-FD configurations */
- regval = getreg32(S32K1XX_CAN0_FDCTRL);
- regval |= CAN_FDCTRL_FDRATE | /* Enable bit rate switch in data phase of frame */
- CAN_FDCTRL_TDCEN | /* Enable transceiver delay compensation */
- CAN_FDCTRL_TDCOFF(5) | /* Setup 5 cycles for data phase sampling delay */
- CAN_FDCTRL_MBDSR0(3); /* Setup 64 bytes per message buffer (7 MB's) */
- putreg32(regval, S32K1XX_CAN0_FDCTRL);
-
- regval = getreg32(S32K1XX_CAN0_CTRL2);
- regval |= CAN_CTRL2_ISOCANFDEN;
- putreg32(regval, S32K1XX_CAN0_CTRL2);
-#endif
-
-
-
- /* Iniatilize all MB rx and tx */
- for(int i = 0; i < TotalMBcount; i++)
- {
- ninfo("MB %i %p\r\n", i, &priv->rx[i]);
- ninfo("MB %i %p\r\n", i, &priv->rx[i].ID.w);
- priv->rx[i].CS.cs = 0x0;
- priv->rx[i].ID.w = 0x0;
- priv->rx[i].data.l = 0x0;
- priv->rx[i].data.h = 0x0;
- }
-
- /* Filtering catchall */
- putreg32(0x0, S32K1XX_CAN0_RXFGMASK);
-
- for(int i = 0; i < TotalMBcount; i++)
- {
- putreg32(0,S32K1XX_CAN0_RXIMR(i));
- }
-
- putreg32(FIFO_IFLAG1 | TXMBMask, S32K1XX_CAN0_IFLAG1);
- putreg32(FIFO_IFLAG1, S32K1XX_CAN0_IMASK1);
-
-
- /* Exit freeze mode */
- s32k1xx_setfreeze(0);
- if(!s32k1xx_waitfreezeack_change(0))
- {
- ninfo("FLEXCAN: unfreeze fail\r\n");
- return -1;
- }
-
-
/* Set and activate a timer process */
wd_start(priv->txpoll, S32K1XX_WDDELAY, s32k1xx_polltimer_expiry, 1,
@@ -1190,8 +1183,11 @@ static int s32k1xx_txavail(struct net_driver_s *dev)
if (work_available(&priv->pollwork))
{
/* Schedule to serialize the poll on the worker thread. */
-
+#ifdef WORK_QUEUE_BYPASS
+ s32k1xx_txavail_work(priv);
+#else
work_queue(ETHWORK, &priv->pollwork, s32k1xx_txavail_work, priv, 0);
+#endif
}
return OK;
@@ -1233,6 +1229,138 @@ static int s32k1xx_ioctl(struct net_driver_s *dev, int cmd,
}
#endif /* CONFIG_NETDEV_IOCTL */
+/****************************************************************************
+ * Function: s32k1xx_initalize
+ *
+ * Description:
+ * Initialize FLEXCAN device
+ *
+ * Input Parameters:
+ * priv - Reference to the private FLEXCAN driver state structure
+ *
+ * Returned Value:
+ * None
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+static int s32k1xx_initialize(struct s32k1xx_driver_s *priv)
+{
+ uint32_t regval;
+ uint32_t i;
+
+ /* initialize CAN device */
+ //FIXME we only support a single can device for now
+
+ //TEST GPIO tming
+ s32k1xx_pinconfig(PIN_PORTD | PIN31 | GPIO_OUTPUT);
+
+
+ s32k1xx_setenable(0);
+
+ /* Set SYS_CLOCK src */
+ regval = getreg32(S32K1XX_CAN0_CTRL1);
+ regval |= CAN_CTRL1_CLKSRC;
+ putreg32(regval, S32K1XX_CAN0_CTRL1);
+
+ s32k1xx_setenable(1);
+
+ s32k1xx_reset(priv);
+
+ /* Enter freeze mode */
+ s32k1xx_setfreeze(1);
+ if(!s32k1xx_waitfreezeack_change(1))
+ {
+ ninfo("FLEXCAN: freeze fail\r\n");
+ return -1;
+ }
+
+ /*regval = getreg32(S32K1XX_CAN0_CTRL1);
+ regval |= ((0 << CAN_CTRL1_PRESDIV_SHIFT) & CAN_CTRL1_PRESDIV_MASK)
+ | ((46 << CAN_CTRL1_ROPSEG_SHIFT) & CAN_CTRL1_ROPSEG_MASK)
+ | ((18 << CAN_CTRL1_PSEG1_SHIFT) & CAN_CTRL1_PSEG1_MASK)
+ | ((12 << CAN_CTRL1_PSEG2_SHIFT) & CAN_CTRL1_PSEG2_MASK)
+ | ((12 << CAN_CTRL1_RJW_SHIFT) & CAN_CTRL1_RJW_MASK)
+ | CAN_CTRL1_ERRMSK
+ | CAN_CTRL1_TWRNMSK
+ | CAN_CTRL1_RWRNMSK;
+
+ putreg32(regval, S32K1XX_CAN0_CTRL1);*/
+
+#define BIT_METHOD2
+#ifdef BIT_METHOD2
+ /* CAN Bit Timing (CBT) configuration for a nominal phase of 1 Mbit/s
+ * with 80 time quantas,in accordance with Bosch 2012 specification,
+ * sample point at 83.75% */
+ regval = getreg32(S32K1XX_CAN0_CBT);
+ regval |= CAN_CBT_BTF | /* Enable extended bit timing configurations for CAN-FD
+ for setting up separetely nominal and data phase */
+ CAN_CBT_EPRESDIV(0) | /* Prescaler divisor factor of 1 */
+ CAN_CBT_EPROPSEG(46) | /* Propagation segment of 47 time quantas */
+ CAN_CBT_EPSEG1(18) | /* Phase buffer segment 1 of 19 time quantas */
+ CAN_CBT_EPSEG2(12) | /* Phase buffer segment 2 of 13 time quantas */
+ CAN_CBT_ERJW(12); /* Resynchronization jump width same as PSEG2 */
+ putreg32(regval, S32K1XX_CAN0_CBT);
+#endif
+
+#ifdef CAN_FD
+
+ /* Enable CAN FD feature */
+ regval = getreg32(S32K1XX_CAN0_MCR);
+ regval |= CAN_MCR_FDEN;
+ putreg32(regval, S32K1XX_CAN0_MCR);
+
+ /* CAN-FD Bit Timing (FDCBT) for a data phase of 4 Mbit/s with 20 time quantas,
+ in accordance with Bosch 2012 specification, sample point at 75% */
+ regval = getreg32(S32K1XX_CAN0_FDCBT);
+ regval |= CAN_FDCBT_FPRESDIV(0) | /* Prescaler divisor factor of 1 */
+ CAN_FDCBT_FPROPSEG(7) | /* Propagation semgment of 7 time quantas
+ (only register that doesn't add 1) */
+ CAN_FDCBT_FPSEG1(6) | /* Phase buffer segment 1 of 7 time quantas */
+ CAN_FDCBT_FPSEG2(4) | /* Phase buffer segment 2 of 5 time quantas */
+ CAN_FDCBT_FRJW(4); /* Resynchorinzation jump width same as PSEG2 */
+ putreg32(regval, S32K1XX_CAN0_FDCBT);
+
+ /* Additional CAN-FD configurations */
+ regval = getreg32(S32K1XX_CAN0_FDCTRL);
+ regval |= CAN_FDCTRL_FDRATE | /* Enable bit rate switch in data phase of frame */
+ CAN_FDCTRL_TDCEN | /* Enable transceiver delay compensation */
+ CAN_FDCTRL_TDCOFF(5) | /* Setup 5 cycles for data phase sampling delay */
+ CAN_FDCTRL_MBDSR0(3); /* Setup 64 bytes per message buffer (7 MB's) */
+ putreg32(regval, S32K1XX_CAN0_FDCTRL);
+
+ regval = getreg32(S32K1XX_CAN0_CTRL2);
+ regval |= CAN_CTRL2_ISOCANFDEN;
+ putreg32(regval, S32K1XX_CAN0_CTRL2);
+#endif
+
+ for(i = TxMBCount; i < TotalMBcount; i++)
+ {
+ priv->rx[i].ID.w = 0x0;
+ }
+
+ putreg32(0x0, S32K1XX_CAN0_RXFGMASK);
+
+ for(i = 0; i < TotalMBcount; i++)
+ {
+ putreg32(0,S32K1XX_CAN0_RXIMR(i));
+ }
+
+ putreg32(FIFO_IFLAG1 | TXMBMask, S32K1XX_CAN0_IFLAG1);
+ putreg32(FIFO_IFLAG1, S32K1XX_CAN0_IMASK1);
+
+
+ /* Exit freeze mode */
+ s32k1xx_setfreeze(0);
+ if(!s32k1xx_waitfreezeack_change(0))
+ {
+ ninfo("FLEXCAN: unfreeze fail\r\n");
+ return -1;
+ }
+
+ return 1;
+}
/****************************************************************************
* Function: s32k1xx_initbuffers
@@ -1273,19 +1401,57 @@ static void s32k1xx_initbuffers(struct s32k1xx_driver_s *priv)
static void s32k1xx_reset(struct s32k1xx_driver_s *priv)
{
- unsigned int i;
+ uint32_t regval;
+ uint32_t i;
- /* Set the reset bit and clear the enable bit */
+ regval = getreg32(S32K1XX_CAN0_MCR);
+ regval |= CAN_MCR_SOFTRST;
+ putreg32(regval, S32K1XX_CAN0_MCR);
-
- #warning Missing logic
+ if(!s32k1xx_waitmcr_change(CAN_MCR_SOFTRST, 0))
+ {
+ nerr("Reset failed");
+ return;
+ }
- /* Wait at least 8 clock cycles */
+ /* TODO calculate TASD */
- for (i = 0; i < 10; i++)
- {
- asm volatile ("nop");
- }
+
+ regval = getreg32(S32K1XX_CAN0_MCR);
+ regval &= ~(CAN_MCR_SUPV);
+ putreg32(regval, S32K1XX_CAN0_MCR);
+
+ /* Initialize all MB rx and tx */
+ for(i = 0; i < TotalMBcount; i++)
+ {
+ ninfo("MB %i %p\r\n", i, &priv->rx[i]);
+ ninfo("MB %i %p\r\n", i, &priv->rx[i].ID.w);
+ priv->rx[i].CS.cs = 0x0;
+ priv->rx[i].ID.w = 0x0;
+ priv->rx[i].data.l = 0x0;
+ priv->rx[i].data.h = 0x0;
+ }
+
+ regval = getreg32(S32K1XX_CAN0_MCR);
+ regval |= CAN_MCR_RFEN | CAN_MCR_SLFWAK | CAN_MCR_WRNEN | CAN_MCR_SRXDIS
+ | CAN_MCR_IRMQ | CAN_MCR_AEN |
+ (((TotalMBcount - 1) << CAN_MCR_MAXMB_SHIFT) & CAN_MCR_MAXMB_MASK);
+ putreg32(regval, S32K1XX_CAN0_MCR);
+
+ regval = CAN_CTRL2_RRS | CAN_CTRL2_EACEN | CAN_CTRL2_RFFN_16MB; //FIXME TASD
+ putreg32(regval, S32K1XX_CAN0_CTRL2);
+
+
+ for(i = 0; i < TotalMBcount; i++)
+ {
+ putreg32(0,S32K1XX_CAN0_RXIMR(i));
+ }
+
+ /* Filtering catchall */
+ putreg32(0x3FFFFFFF, S32K1XX_CAN0_RX14MASK);
+ putreg32(0x3FFFFFFF, S32K1XX_CAN0_RX15MASK);
+ putreg32(0x3FFFFFFF, S32K1XX_CAN0_RXMGMASK);
+ putreg32(0x0, S32K1XX_CAN0_RXFGMASK);
}
/****************************************************************************
diff --git a/net/can/Make.defs b/net/can/Make.defs
index f8488c0..d078a05 100644
--- a/net/can/Make.defs
+++ b/net/can/Make.defs
@@ -26,13 +26,15 @@ ifeq ($(CONFIG_NET_CAN),y)
SOCK_CSRCS += can_sockif.c
SOCK_CSRCS += can_send.c
+SOCK_CSRCS += can_recvfrom.c
NET_CSRCS += can_conn.c
-NET_CSRCS += can_poll.c
+NET_CSRCS += can_input.c
NET_CSRCS += can_callback.c
+NET_CSRCS += can_poll.c
# Include can build support
DEPPATH += --dep-path can
VPATH += :can
-endif
+endif # CONFIG_NET_CAN
diff --git a/net/can/can.h b/net/can/can.h
index 3fed49b..c2b6857 100644
--- a/net/can/can.h
+++ b/net/can/can.h
@@ -166,6 +166,34 @@ uint16_t can_callback(FAR struct net_driver_s *dev,
FAR struct can_conn_s *conn, uint16_t flags);
/****************************************************************************
+ * Name: can_recvfrom
+ *
+ * Description:
+ * Implements the socket recvfrom interface pkt_recvfrom() receives messages from
+ * a socket, and may be used to receive data on a socket whether or not it
+ * is connection-oriented.
+ *
+ * Input Parameters:
+ * psock A pointer to a NuttX-specific, internal socket structure
+ * buf Buffer to receive data
+ * len Length of buffer
+ * flags Receive flags
+ * from Address of source (may be NULL)
+ * fromlen The length of the address structure
+ *
+ * Returned Value:
+ * On success, returns the number of characters received. If no data is
+ * available to be received and the peer has performed an orderly shutdown,
+ * recv() will return 0. Otherwise, on errors, a negated errno value is
+ * returned (see recvfrom() for the list of appropriate error values).
+ *
+ ****************************************************************************/
+
+ssize_t can_recvfrom(FAR struct socket *psock, FAR void *buf, size_t len,
+ int flags, FAR struct sockaddr *from,
+ FAR socklen_t *fromlen);
+
+/****************************************************************************
* Name: can_poll
*
* Description:
diff --git a/net/can/can_input.c b/net/can/can_input.c
new file mode 100644
index 0000000..32dcebf
--- /dev/null
+++ b/net/can/can_input.c
@@ -0,0 +1,129 @@
+/****************************************************************************
+ * net/can/can_input.c
+ * Handling incoming packet input
+ *
+ * Copyright (C) 2014, 2020 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <gn...@nuttx.org>
+ *
+ * Adapted for NuttX from logic in uIP which also has a BSD-like license:
+ *
+ * Original author Adam Dunkels <ad...@dunkels.com>
+ * Copyright () 2001-2003, Adam Dunkels.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote
+ * products derived from this software without specific prior
+ * written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS
+ * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
+ * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#if defined(CONFIG_NET) && defined(CONFIG_NET_CAN)
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/net/netdev.h>
+#include <nuttx/net/can.h>
+
+#include "devif/devif.h"
+#include "can/can.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: can_input
+ *
+ * Description:
+ * Handle incoming packet input
+ *
+ * Input Parameters:
+ * dev - The device driver structure containing the received packet
+ *
+ * Returned Value:
+ * OK The packet has been processed and can be deleted
+ * -EAGAIN There is a matching connection, but could not dispatch the packet
+ * yet. Useful when a packet arrives before a recv call is in
+ * place.
+ *
+ * Assumptions:
+ * The network is locked.
+ *
+ ****************************************************************************/
+
+int can_input(struct net_driver_s *dev)
+{
+ FAR struct can_conn_s *conn;
+ int ret = OK;
+
+ conn = can_nextconn(NULL); //FIXME
+ if (conn)
+ {
+ uint16_t flags;
+
+ /* Setup for the application callback */
+
+ dev->d_appdata = dev->d_buf;
+ dev->d_sndlen = 0;
+
+ /* Perform the application callback */
+
+ flags = can_callback(dev, conn, CAN_NEWDATA);
+
+ /* If the operation was successful, the CAN_NEWDATA flag is removed
+ * and thus the packet can be deleted (OK will be returned).
+ */
+
+ if ((flags & CAN_NEWDATA) != 0)
+ {
+ /* No.. the packet was not processed now. Return -EAGAIN so
+ * that the driver may retry again later. We still need to
+ * set d_len to zero so that the driver is aware that there
+ * is nothing to be sent.
+ */
+
+ nwarn("WARNING: Packet not processed\n");
+ ret = -EAGAIN;
+ }
+ }
+ else
+ {
+ ninfo("No CAN listener\n");
+ }
+
+ return ret;
+}
+
+#endif /* CONFIG_NET && CONFIG_NET_CAN */
diff --git a/net/can/can_recvfrom.c b/net/can/can_recvfrom.c
new file mode 100644
index 0000000..4062ed3
--- /dev/null
+++ b/net/can/can_recvfrom.c
@@ -0,0 +1,354 @@
+/****************************************************************************
+ * net/can/can_recvfrom.c
+ *
+ * Copyright (C) 2007-2009, 2011-2017, 2020 Gregory Nutt. All rights
+ * reserved.
+ * Author: Gregory Nutt <gn...@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#ifdef CONFIG_NET_CAN
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <stdint.h>
+#include <string.h>
+#include <errno.h>
+#include <debug.h>
+#include <assert.h>
+
+#include <arch/irq.h>
+
+#include <nuttx/semaphore.h>
+#include <nuttx/net/net.h>
+#include <nuttx/net/netdev.h>
+
+#include "netdev/netdev.h"
+#include "devif/devif.h"
+#include "can/can.h"
+#include "socket/socket.h"
+#include <netpacket/packet.h>
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+struct can_recvfrom_s
+{
+ FAR struct devif_callback_s *pr_cb; /* Reference to callback instance */
+ sem_t pr_sem; /* Semaphore signals recv completion */
+ size_t pr_buflen; /* Length of receive buffer */
+ FAR uint8_t *pr_buffer; /* Pointer to receive buffer */
+ ssize_t pr_recvlen; /* The received length */
+ int pr_result; /* Success:OK, failure:negated errno */
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: can_add_recvlen
+ *
+ * Description:
+ * Update information about space available for new data and update size
+ * of data in buffer, This logic accounts for the case where
+ * recvfrom_udpreadahead() sets state.pr_recvlen == -1 .
+ *
+ * Input Parameters:
+ * pstate recvfrom state structure
+ * recvlen size of new data appended to buffer
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+static inline void can_add_recvlen(FAR struct can_recvfrom_s *pstate,
+ size_t recvlen)
+{
+ if (pstate->pr_recvlen < 0)
+ {
+ pstate->pr_recvlen = 0;
+ }
+
+ pstate->pr_recvlen += recvlen;
+ pstate->pr_buffer += recvlen;
+ pstate->pr_buflen -= recvlen;
+}
+
+/****************************************************************************
+ * Name: can_recvfrom_newdata
+ *
+ * Description:
+ * Copy the read data from the packet
+ *
+ * Input Parameters:
+ * dev The structure of the network driver that caused the event.
+ * pstate recvfrom state structure
+ *
+ * Returned Value:
+ * None.
+ *
+ * Assumptions:
+ * The network is locked.
+ *
+ ****************************************************************************/
+
+static void can_recvfrom_newdata(FAR struct net_driver_s *dev,
+ FAR struct can_recvfrom_s *pstate)
+{
+ size_t recvlen;
+
+ if (dev->d_len > pstate->pr_buflen)
+ {
+ recvlen = pstate->pr_buflen;
+ }
+ else
+ {
+ recvlen = dev->d_len;
+ }
+
+ /* Copy the new packet data into the user buffer */
+
+ memcpy(pstate->pr_buffer, dev->d_buf, recvlen);
+ //ninfo("Received %d bytes (of %d)\n", (int)recvlen, (int)dev->d_len);
+
+ /* Update the accumulated size of the data read */
+
+ can_add_recvlen(pstate, recvlen);
+}
+
+static uint16_t can_recvfrom_eventhandler(FAR struct net_driver_s *dev,
+ FAR void *pvconn,
+ FAR void *pvpriv, uint16_t flags)
+{
+ struct can_recvfrom_s *pstate = (struct can_recvfrom_s *)pvpriv;
+
+ //ninfo("flags: %04x\n", flags);
+
+ /* 'priv' might be null in some race conditions (?) */
+
+ if (pstate)
+ {
+ /* If a new packet is available, then complete the read action. */
+
+ if ((flags & CAN_NEWDATA) != 0)
+ {
+ /* Copy the packet */
+
+ can_recvfrom_newdata(dev, pstate);
+
+ /* We are finished. */
+
+ //ninfo("CAN done\n");
+
+ /* Don't allow any further call backs. */
+
+ pstate->pr_cb->flags = 0;
+ pstate->pr_cb->priv = NULL;
+ pstate->pr_cb->event = NULL;
+
+ /* Save the sender's address in the caller's 'from' location */
+
+ //pkt_recvfrom_sender(dev, pstate);
+
+ /* indicate that the data has been consumed */
+
+ flags &= ~CAN_NEWDATA;
+
+ /* Wake up the waiting thread, returning the number of bytes
+ * actually read.
+ */
+
+ nxsem_post(&pstate->pr_sem);
+ }
+ }
+
+ return flags;
+}
+
+/****************************************************************************
+ * Name: can_recvfrom_result
+ *
+ * Description:
+ * Evaluate the result of the recv operations
+ *
+ * Input Parameters:
+ * result The result of the net_lockedwait operation (may indicate EINTR)
+ * pstate A pointer to the state structure to be initialized
+ *
+ * Returned Value:
+ * The result of the recv operation with errno set appropriately
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+static ssize_t can_recvfrom_result(int result,
+ FAR struct can_recvfrom_s *pstate)
+{
+ /* Check for a error/timeout detected by the event handler. Errors are
+ * signaled by negative errno values for the rcv length
+ */
+
+ if (pstate->pr_result < 0)
+ {
+ /* This might return EAGAIN on a timeout or ENOTCONN on loss of
+ * connection (TCP only)
+ */
+
+ return pstate->pr_result;
+ }
+
+ /* If net_lockedwait failed, then we were probably reawakened by a signal.
+ * In this case, net_lockedwait will have returned negated errno
+ * appropriately.
+ */
+
+ if (result < 0)
+ {
+ return result;
+ }
+
+ return pstate->pr_recvlen;
+}
+
+/****************************************************************************
+ * Name: can_recvfrom
+ *
+ * Description:
+ * recvfrom() receives messages from a socket, and may be used to receive
+ * data on a socket whether or not it is connection-oriented.
+ *
+ * If from is not NULL, and the underlying protocol provides the source
+ * address, this source address is filled in. The argument 'fromlen'
+ * initialized to the size of the buffer associated with from, and modified
+ * on return to indicate the actual size of the address stored there.
+ *
+ * Input Parameters:
+ * psock A pointer to a NuttX-specific, internal socket structure
+ * buf Buffer to receive data
+ * len Length of buffer
+ * flags Receive flags (ignored)
+ * from Address of source (may be NULL)
+ * fromlen The length of the address structure
+ *
+ ****************************************************************************/
+
+ssize_t can_recvfrom(FAR struct socket *psock, FAR void *buf,
+ size_t len, int flags,
+ FAR struct sockaddr *from,
+ FAR socklen_t *fromlen)
+{
+ FAR struct can_conn_s *conn;
+ FAR struct net_driver_s *dev;
+ struct can_recvfrom_s state;
+ int ret;
+
+ DEBUGASSERT(psock != NULL && psock->s_conn != NULL && buf != NULL);
+ DEBUGASSERT(from == NULL ||
+ (fromlen != NULL && *fromlen >= sizeof(struct sockaddr_can)));
+
+ conn = (FAR struct can_conn_s *)psock->s_conn;
+
+ if (psock->s_type != SOCK_RAW)
+ {
+ nerr("ERROR: Unsupported socket type: %d\n", psock->s_type);
+ ret = -ENOSYS;
+ }
+
+
+ net_lock();
+
+ /* Initialize the state structure. */
+
+ memset(&state, 0, sizeof(struct can_recvfrom_s));
+
+ /* This semaphore is used for signaling and, hence, should not have
+ * priority inheritance enabled.
+ */
+
+ nxsem_init(&state.pr_sem, 0, 0); /* Doesn't really fail */
+ nxsem_setprotocol(&state.pr_sem, SEM_PRIO_NONE);
+
+ state.pr_buflen = len;
+ state.pr_buffer = buf;
+
+ /* Get the device driver that will service this transfer */
+
+ dev = conn->dev;
+ if (dev == NULL)
+ {
+ ret = -ENODEV;
+ goto errout_with_state;
+ }
+
+ /* Set up the callback in the connection */
+
+ state.pr_cb = can_callback_alloc(dev, conn);
+ if (state.pr_cb)
+ {
+ state.pr_cb->flags = (CAN_NEWDATA | CAN_POLL);
+ state.pr_cb->priv = (FAR void *)&state;
+ state.pr_cb->event = can_recvfrom_eventhandler;
+
+ /* Wait for either the receive to complete or for an error/timeout to
+ * occur. NOTES: (1) net_lockedwait will also terminate if a signal
+ * is received, (2) the network is locked! It will be un-locked while
+ * the task sleeps and automatically re-locked when the task restarts.
+ */
+
+ ret = net_lockedwait(&state.pr_sem);
+
+ /* Make sure that no further events are processed */
+
+ can_callback_free(dev, conn, state.pr_cb);
+ ret = can_recvfrom_result(ret, &state);
+ }
+ else
+ {
+ ret = -EBUSY;
+ }
+
+
+errout_with_state:
+ net_unlock();
+ nxsem_destroy(&state.pr_sem);
+ return ret;
+}
+
+#endif /* CONFIG_NET_CAN */
diff --git a/net/can/can_sockif.c b/net/can/can_sockif.c
index 1de95d9..0cd389a 100644
--- a/net/can/can_sockif.c
+++ b/net/can/can_sockif.c
@@ -69,9 +69,6 @@ static ssize_t can_send(FAR struct socket *psock,
static ssize_t can_sendto(FAR struct socket *psock, FAR const void *buf,
size_t len, int flags, FAR const struct sockaddr *to,
socklen_t tolen);
-static ssize_t can_recvfrom(FAR struct socket *psock, FAR void *buf,
- size_t len, int flags, FAR struct sockaddr *from,
- FAR socklen_t *fromlen);
static int can_close(FAR struct socket *psock);
/****************************************************************************
@@ -99,12 +96,13 @@ const struct sock_intf_s g_can_sockif =
can_close /* si_close */
};
+
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
- * Name: inet_setup
+ * Name: can_setup
*
* Description:
* Called for socket() to verify that the provided socket type and
@@ -684,55 +682,6 @@ static ssize_t can_sendto(FAR struct socket *psock, FAR const void *buf,
}
/****************************************************************************
- * Name: can_recvfrom
- *
- * Description:
- * recvfrom() receives messages from a socket, and may be used to receive
- * data on a socket whether or not it is connection-oriented.
- *
- * If from is not NULL, and the underlying protocol provides the source
- * address, this source address is filled in. The argument 'fromlen'
- * initialized to the size of the buffer associated with from, and modified
- * on return to indicate the actual size of the address stored there.
- *
- * Input Parameters:
- * psock A pointer to a NuttX-specific, internal socket structure
- * buf Buffer to receive data
- * len Length of buffer
- * flags Receive flags (ignored)
- * from Address of source (may be NULL)
- * fromlen The length of the address structure
- *
- ****************************************************************************/
-
-static ssize_t can_recvfrom(FAR struct socket *psock, FAR void *buf,
- size_t len, int flags,
- FAR struct sockaddr *from,
- FAR socklen_t *fromlen)
-{
- FAR struct can_conn_s *conn;
- int ret;
-
- DEBUGASSERT(psock != NULL && psock->s_conn != NULL && buf != NULL);
- DEBUGASSERT(from == NULL ||
- (fromlen != NULL && *fromlen >= sizeof(struct sockaddr_can)));
-
- conn = (FAR struct can_conn_s *)psock->s_conn;
-#warning Missing logic
-
- switch (conn->protocol)
- {
-#warning Missing logic
-
- default:
- ret = -EOPNOTSUPP;
- break;
- }
-
- return ret;
-}
-
-/****************************************************************************
* Name: can_close
*
* Description:
diff --git a/net/devif/Make.defs b/net/devif/Make.defs
index b2dca4a..57aac89 100644
--- a/net/devif/Make.defs
+++ b/net/devif/Make.defs
@@ -62,7 +62,11 @@ endif
# Raw packet socket support
-ifeq ($(filter y,$(CONFIG_NET_PKT) $(CONFIG_NET_CAN)),)
+ifeq ($(CONFIG_NET_PKT),y)
+NET_CSRCS += devif_pktsend.c
+endif
+
+ifeq ($(CONFIG_NET_CAN),y)
NET_CSRCS += devif_pktsend.c
endif