[PATCH 4/6] net: can: xilinx_can: Add support for CANFD FD frames

[Appana Durga Kedareswara rao]

CANFD IP supports both CAN and CAN FD frames,
Existing driver supports only CAN frames, This patch
adds support for CAN FD frames.

Signed-off-by: Naga Sureshkumar Relli <naga.sureshkumar.relli@xxxxxxxxxx>
Signed-off-by: Appana Durga Kedareswara rao <appana.durga.rao@xxxxxxxxxx>
---
 drivers/net/can/xilinx_can.c | 230 ++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 214 insertions(+), 16 deletions(-)

diff --git a/drivers/net/can/xilinx_can.c b/drivers/net/can/xilinx_can.c
index b45ece9..3d75a28 100644
--- a/drivers/net/can/xilinx_can.c
+++ b/drivers/net/can/xilinx_can.c
@@ -58,6 +58,10 @@ enum xcan_reg {
 	XCAN_AFR_OFFSET		= 0x60, /* Acceptance Filter */
 
 	/* only on CAN FD cores */
+	XCAN_F_BRPR_OFFSET	= 0x088, /* Data Phase Baud Rate
+					  * Prescalar
+					  */
+	XCAN_F_BTR_OFFSET	= 0x08C, /* Data Phase Bit Timing */
 	XCAN_TRR_OFFSET		= 0x0090, /* TX Buffer Ready Request */
 	XCAN_AFR_EXT_OFFSET	= 0x00E0, /* Acceptance Filter */
 	XCAN_FSR_OFFSET		= 0x00E8, /* RX FIFO Status */
@@ -70,6 +74,8 @@ enum xcan_reg {
 #define XCAN_FRAME_DLC_OFFSET(frame_base)	((frame_base) + 0x04)
 #define XCAN_FRAME_DW1_OFFSET(frame_base)	((frame_base) + 0x08)
 #define XCAN_FRAME_DW2_OFFSET(frame_base)	((frame_base) + 0x0C)
+#define XCANFD_FRAME_DW_OFFSET(frame_base, n)	(((frame_base) + 0x08) + \
+						 ((n) * XCAN_CANFD_FRAME_SIZE))
 
 #define XCAN_CANFD_FRAME_SIZE		0x48
 #define XCAN_TXMSG_FRAME_OFFSET(n)	(XCAN_TXMSG_BASE_OFFSET + \
@@ -128,6 +134,8 @@ enum xcan_reg {
 #define XCAN_FSR_FL_MASK		0x00003F00 /* RX Fill Level */
 #define XCAN_FSR_IRI_MASK		0x00000080 /* RX Increment Read Index */
 #define XCAN_FSR_RI_MASK		0x0000001F /* RX Read Index */
+#define XCAN_DLCR_EDL_MASK		0x08000000 /* EDL Mask in DLC */
+#define XCAN_DLCR_BRS_MASK		0x04000000 /* BRS Mask in DLC */
 
 /* CAN register bit shift - XCAN_<REG>_<BIT>_SHIFT */
 #define XCAN_BTR_SJW_SHIFT		7  /* Synchronous jump width */
@@ -141,6 +149,7 @@ enum xcan_reg {
 
 /* CAN frame length constants */
 #define XCAN_FRAME_MAX_DATA_LEN		8
+#define XCANFD_DW_BYTES			4
 #define XCAN_TIMEOUT			(1 * HZ)
 
 /* TX-FIFO-empty interrupt available */
@@ -221,6 +230,7 @@ static const struct can_bittiming_const xcan_bittiming_const = {
 	.brp_inc = 1,
 };
 
+/* AXI CANFD Arbitration Bittiming constants as per AXI CANFD 1.0 spec */
 static const struct can_bittiming_const xcan_bittiming_const_canfd = {
 	.name = DRIVER_NAME,
 	.tseg1_min = 1,
@@ -233,6 +243,20 @@ static const struct can_bittiming_const xcan_bittiming_const_canfd = {
 	.brp_inc = 1,
 };
 
+/* AXI CANFD Data Bittiming constants as per AXI CANFD 1.0 specs */
+static struct can_bittiming_const xcan_data_bittiming_const_canfd = {
+	.name = DRIVER_NAME,
+	.tseg1_min = 1,
+	.tseg1_max = 16,
+	.tseg2_min = 1,
+	.tseg2_max = 8,
+	.sjw_max = 8,
+	.brp_min = 1,
+	.brp_max = 256,
+	.brp_inc = 1,
+};
+
+/* AXI CANFD 2.0 Arbitration Bittiming constants as per AXI CANFD 2.0 spec */
 static const struct can_bittiming_const xcan_bittiming_const_canfd2 = {
 	.name = DRIVER_NAME,
 	.tseg1_min = 1,
@@ -245,6 +269,19 @@ static const struct can_bittiming_const xcan_bittiming_const_canfd2 = {
 	.brp_inc = 1,
 };
 
+/* AXI CANFD 2.0 Data Bittiming constants as per AXI CANFD 2.0 spec */
+static struct can_bittiming_const xcan_data_bittiming_const_canfd2 = {
+	.name = DRIVER_NAME,
+	.tseg1_min = 1,
+	.tseg1_max = 32,
+	.tseg2_min = 1,
+	.tseg2_max = 16,
+	.sjw_max = 16,
+	.brp_min = 1,
+	.brp_max = 256,
+	.brp_inc = 1,
+};
+
 /**
  * xcan_write_reg_le - Write a value to the device register little endian
  * @priv:	Driver private data structure
@@ -359,6 +396,7 @@ static int xcan_set_bittiming(struct net_device *ndev)
 {
 	struct xcan_priv *priv = netdev_priv(ndev);
 	struct can_bittiming *bt = &priv->can.bittiming;
+	struct can_bittiming *dbt = &priv->can.data_bittiming;
 	u32 btr0, btr1;
 	u32 is_config_mode;
 
@@ -388,6 +426,24 @@ static int xcan_set_bittiming(struct net_device *ndev)
 	priv->write_reg(priv, XCAN_BRPR_OFFSET, btr0);
 	priv->write_reg(priv, XCAN_BTR_OFFSET, btr1);
 
+	if (priv->devtype.cantype == XAXI_CANFD ||
+	    priv->devtype.cantype == XAXI_CANFD_2_0) {
+		/* Setting Baud Rate prescalar value in F_BRPR Register */
+		btr0 = dbt->brp - 1;
+
+		/* Setting Time Segment 1 in BTR Register */
+		btr1 = dbt->prop_seg + bt->phase_seg1 - 1;
+
+		/* Setting Time Segment 2 in BTR Register */
+		btr1 |= (dbt->phase_seg2 - 1) << priv->devtype.btr_ts2_shift;
+
+		/* Setting Synchronous jump width in BTR Register */
+		btr1 |= (dbt->sjw - 1) << priv->devtype.btr_sjw_shift;
+
+		priv->write_reg(priv, XCAN_F_BRPR_OFFSET, btr0);
+		priv->write_reg(priv, XCAN_F_BTR_OFFSET, btr1);
+	}
+
 	netdev_dbg(ndev, "BRPR=0x%08x, BTR=0x%08x\n",
 		   priv->read_reg(priv, XCAN_BRPR_OFFSET),
 		   priv->read_reg(priv, XCAN_BTR_OFFSET));
@@ -506,7 +562,8 @@ static void xcan_write_frame(struct xcan_priv *priv, struct sk_buff *skb,
 			     int frame_offset)
 {
 	u32 id, dlc, data[2] = {0, 0};
-	struct can_frame *cf = (struct can_frame *)skb->data;
+	struct canfd_frame *cf = (struct canfd_frame *)skb->data;
+	u32 ramoff, dwindex = 0, i;
 
 	/* Watch carefully on the bit sequence */
 	if (cf->can_id & CAN_EFF_FLAG) {
@@ -535,26 +592,44 @@ static void xcan_write_frame(struct xcan_priv *priv, struct sk_buff *skb,
 			id |= XCAN_IDR_SRR_MASK;
 	}
 
-	dlc = cf->can_dlc << XCAN_DLCR_DLC_SHIFT;
-
-	if (cf->can_dlc > 0)
-		data[0] = be32_to_cpup((__be32 *)(cf->data + 0));
-	if (cf->can_dlc > 4)
-		data[1] = be32_to_cpup((__be32 *)(cf->data + 4));
+	dlc = can_len2dlc(cf->len) << XCAN_DLCR_DLC_SHIFT;
+	if (can_is_canfd_skb(skb)) {
+		if (cf->flags & CANFD_BRS)
+			dlc |= XCAN_DLCR_BRS_MASK;
+		dlc |= XCAN_DLCR_EDL_MASK;
+	}
 
 	priv->write_reg(priv, XCAN_FRAME_ID_OFFSET(frame_offset), id);
 	/* If the CAN frame is RTR frame this write triggers transmission
 	 * (not on CAN FD)
 	 */
 	priv->write_reg(priv, XCAN_FRAME_DLC_OFFSET(frame_offset), dlc);
-	if (!(cf->can_id & CAN_RTR_FLAG)) {
-		priv->write_reg(priv, XCAN_FRAME_DW1_OFFSET(frame_offset),
-				data[0]);
-		/* If the CAN frame is Standard/Extended frame this
-		 * write triggers transmission (not on CAN FD)
-		 */
-		priv->write_reg(priv, XCAN_FRAME_DW2_OFFSET(frame_offset),
-				data[1]);
+	if (priv->devtype.cantype == XAXI_CANFD ||
+	    priv->devtype.cantype == XAXI_CANFD_2_0) {
+		for (i = 0; i < cf->len; i += 4) {
+			ramoff = XCANFD_FRAME_DW_OFFSET(frame_offset, dwindex) +
+					(dwindex * XCANFD_DW_BYTES);
+			priv->write_reg(priv, ramoff,
+					be32_to_cpup((__be32 *)(cf->data + i)));
+			dwindex++;
+		}
+	} else {
+		if (cf->len > 0)
+			data[0] = be32_to_cpup((__be32 *)(cf->data + 0));
+		if (cf->len > 4)
+			data[1] = be32_to_cpup((__be32 *)(cf->data + 4));
+
+		if (!(cf->can_id & CAN_RTR_FLAG)) {
+			priv->write_reg(priv,
+					XCAN_FRAME_DW1_OFFSET(frame_offset),
+					data[0]);
+			/* If the CAN frame is Standard/Extended frame this
+			 * write triggers transmission (not on CAN FD)
+			 */
+			priv->write_reg(priv,
+					XCAN_FRAME_DW2_OFFSET(frame_offset),
+					data[1]);
+		}
 	}
 }
 
@@ -728,6 +803,113 @@ static int xcan_rx(struct net_device *ndev, int frame_base)
 }
 
 /**
+ * xcanfd_rx -  Is called from CAN isr to complete the received
+ *		frame  processing
+ * @ndev:	Pointer to net_device structure
+ * @frame_base:	Register offset to the frame to be read
+ *
+ * This function is invoked from the CAN isr(poll) to process the Rx frames. It
+ * does minimal processing and invokes "netif_receive_skb" to complete further
+ * processing.
+ * Return: 1 on success and 0 on failure.
+ */
+static int xcanfd_rx(struct net_device *ndev, int frame_base)
+{
+	struct xcan_priv *priv = netdev_priv(ndev);
+	struct net_device_stats *stats = &ndev->stats;
+	struct canfd_frame *cf;
+	struct sk_buff *skb;
+	u32 id_xcan, dlc, data[2] = {0, 0}, dwindex = 0, i, fsr, readindex;
+
+	fsr = priv->read_reg(priv, XCAN_FSR_OFFSET);
+	if (fsr & XCAN_FSR_FL_MASK) {
+		readindex = fsr & XCAN_FSR_RI_MASK;
+		id_xcan = priv->read_reg(priv,
+					 XCAN_FRAME_ID_OFFSET(frame_base));
+		dlc = priv->read_reg(priv, XCAN_FRAME_DLC_OFFSET(frame_base));
+		if (dlc & XCAN_DLCR_EDL_MASK)
+			skb = alloc_canfd_skb(ndev, &cf);
+		else
+			skb = alloc_can_skb(ndev, (struct can_frame **)&cf);
+
+		if (unlikely(!skb)) {
+			stats->rx_dropped++;
+			return 0;
+		}
+
+		/* Change Xilinx CANFD data length format to socketCAN data
+		 * format
+		 */
+		if (dlc & XCAN_DLCR_EDL_MASK)
+			cf->len = can_dlc2len((dlc & XCAN_DLCR_DLC_MASK) >>
+					  XCAN_DLCR_DLC_SHIFT);
+		else
+			cf->len = get_can_dlc((dlc & XCAN_DLCR_DLC_MASK) >>
+						  XCAN_DLCR_DLC_SHIFT);
+
+		/* Change Xilinx CAN ID format to socketCAN ID format */
+		if (id_xcan & XCAN_IDR_IDE_MASK) {
+			/* The received frame is an Extended format frame */
+			cf->can_id = (id_xcan & XCAN_IDR_ID1_MASK) >> 3;
+			cf->can_id |= (id_xcan & XCAN_IDR_ID2_MASK) >>
+					XCAN_IDR_ID2_SHIFT;
+			cf->can_id |= CAN_EFF_FLAG;
+			if (id_xcan & XCAN_IDR_RTR_MASK)
+				cf->can_id |= CAN_RTR_FLAG;
+		} else {
+			/* The received frame is a standard format frame */
+			cf->can_id = (id_xcan & XCAN_IDR_ID1_MASK) >>
+					XCAN_IDR_ID1_SHIFT;
+			if (!(dlc & XCAN_DLCR_EDL_MASK) && (id_xcan &
+						XCAN_IDR_SRR_MASK))
+				cf->can_id |= CAN_RTR_FLAG;
+		}
+
+		/* Check the frame received is FD or not*/
+		if (dlc & XCAN_DLCR_EDL_MASK) {
+			for (i = 0; i < cf->len; i += 4) {
+				if (priv->devtype.flags & XCAN_FLAG_CANFD_2)
+					data[0] = priv->read_reg(priv,
+					(XCAN_RXMSG_2_FRAME_OFFSET(readindex) +
+					(dwindex * XCANFD_DW_BYTES)));
+				else
+					data[0] = priv->read_reg(priv,
+					(XCAN_RXMSG_FRAME_OFFSET(readindex) +
+						(dwindex * XCANFD_DW_BYTES)));
+				*(__be32 *)(cf->data + i) =
+						cpu_to_be32(data[0]);
+				dwindex++;
+			}
+		} else {
+			for (i = 0; i < cf->len; i += 4) {
+				if (priv->devtype.flags & XCAN_FLAG_CANFD_2)
+					data[0] = priv->read_reg(priv,
+						XCAN_RXMSG_2_FRAME_OFFSET(readindex) + i);
+				else
+					data[0] = priv->read_reg(priv,
+						XCAN_RXMSG_FRAME_OFFSET(readindex) + i);
+				*(__be32 *)(cf->data + i) =
+						cpu_to_be32(data[0]);
+			}
+		}
+		/* Update FSR Register so that next packet will save to
+		 * buffer
+		 */
+		fsr = priv->read_reg(priv, XCAN_FSR_OFFSET);
+		fsr |= XCAN_FSR_IRI_MASK;
+		priv->write_reg(priv, XCAN_FSR_OFFSET, fsr);
+		fsr = priv->read_reg(priv, XCAN_FSR_OFFSET);
+		stats->rx_bytes += cf->len;
+		stats->rx_packets++;
+		netif_receive_skb(skb);
+
+		return 1;
+	}
+	/* If FSR Register is not updated with fill level */
+	return 0;
+}
+
+/**
  * xcan_current_error_state - Get current error state from HW
  * @ndev:	Pointer to net_device structure
  *
@@ -1037,7 +1219,10 @@ static int xcan_rx_poll(struct napi_struct *napi, int quota)
 
 	while ((frame_offset = xcan_rx_fifo_get_next_frame(priv)) >= 0 &&
 	       (work_done < quota)) {
-		work_done += xcan_rx(ndev, frame_offset);
+		if (xcan_rx_int_mask(priv) & XCAN_IXR_RXOK_MASK)
+			work_done += xcanfd_rx(ndev, frame_offset);
+		else
+			work_done += xcan_rx(ndev, frame_offset);
 
 		if (priv->devtype.flags & XCAN_FLAG_RX_FIFO_MULTI)
 			/* increment read index */
@@ -1579,6 +1764,19 @@ static int xcan_probe(struct platform_device *pdev)
 	priv->can.do_get_berr_counter = xcan_get_berr_counter;
 	priv->can.ctrlmode_supported = CAN_CTRLMODE_LOOPBACK |
 					CAN_CTRLMODE_BERR_REPORTING;
+
+	if (devtype->cantype == XAXI_CANFD)
+		priv->can.data_bittiming_const =
+			&xcan_data_bittiming_const_canfd;
+
+	if (devtype->cantype == XAXI_CANFD_2_0)
+		priv->can.data_bittiming_const =
+			&xcan_data_bittiming_const_canfd2;
+
+	if (devtype->cantype == XAXI_CANFD ||
+	    devtype->cantype == XAXI_CANFD_2_0)
+		priv->can.ctrlmode_supported |= CAN_CTRLMODE_FD;
+
 	priv->reg_base = addr;
 	priv->tx_max = tx_max;
 	priv->devtype = *devtype;
-- 
2.7.4