diff mbox series

[v4,01/12] can: m_can: Write transmit header and data in one transaction

Message ID 20230621092350.3130866-2-msp@baylibre.com
State New
Headers
Series can: m_can: Optimizations for m_can/tcan part 2 |

Commit Message

Markus Schneider-Pargmann June 21, 2023, 9:23 a.m. UTC 
  Combine header and data before writing to the transmit fifo to reduce
the overhead for peripheral chips.

Signed-off-by: Markus Schneider-Pargmann <msp@baylibre.com>
---
 drivers/net/can/m_can/m_can.c | 34 +++++++++++++++++++++-------------
 1 file changed, 21 insertions(+), 13 deletions(-)

Comments

Simon Horman June 21, 2023, 2:19 p.m. UTC | #1 
On Wed, Jun 21, 2023 at 11:23:39AM +0200, Markus Schneider-Pargmann wrote:
> Combine header and data before writing to the transmit fifo to reduce
> the overhead for peripheral chips.
> 
> Signed-off-by: Markus Schneider-Pargmann <msp@baylibre.com>
> ---
>  drivers/net/can/m_can/m_can.c | 34 +++++++++++++++++++++-------------
>  1 file changed, 21 insertions(+), 13 deletions(-)
> 
> diff --git a/drivers/net/can/m_can/m_can.c b/drivers/net/can/m_can/m_can.c
> index a5003435802b..5251073987ee 100644
> --- a/drivers/net/can/m_can/m_can.c
> +++ b/drivers/net/can/m_can/m_can.c
> @@ -317,6 +317,12 @@ struct id_and_dlc {
>  	u32 dlc;
>  };
>  
> +struct m_can_fifo_element {
> +	u32 id;
> +	u32 dlc;
> +	u8 data[CANFD_MAX_DLEN];
> +};
> +
>  static inline u32 m_can_read(struct m_can_classdev *cdev, enum m_can_reg reg)
>  {
>  	return cdev->ops->read_reg(cdev, reg);
> @@ -1622,6 +1628,8 @@ static int m_can_next_echo_skb_occupied(struct net_device *dev, int putidx)
>  static netdev_tx_t m_can_tx_handler(struct m_can_classdev *cdev)
>  {
>  	struct canfd_frame *cf = (struct canfd_frame *)cdev->tx_skb->data;
> +	u8 len_padded = DIV_ROUND_UP(cf->len, 4);
> +	struct m_can_fifo_element fifo_element;
>  	struct net_device *dev = cdev->net;
>  	struct sk_buff *skb = cdev->tx_skb;
>  	struct id_and_dlc fifo_header;

Hi Markus,

GCC 12.3.0 complains that fifo_header is not (no longer) used.

 drivers/net/can/m_can/m_can.c:1635:20: warning: unused variable 'fifo_header' [-Wunused-variable]
         struct id_and_dlc fifo_header;
kernel test robot June 21, 2023, 5:49 p.m. UTC | #2 
Hi Markus,

kernel test robot noticed the following build warnings:

[auto build test WARNING on ac9a78681b921877518763ba0e89202254349d1b]

url:    https://github.com/intel-lab-lkp/linux/commits/Markus-Schneider-Pargmann/can-m_can-Write-transmit-header-and-data-in-one-transaction/20230621-173848
base:   ac9a78681b921877518763ba0e89202254349d1b
patch link:    https://lore.kernel.org/r/20230621092350.3130866-2-msp%40baylibre.com
patch subject: [PATCH v4 01/12] can: m_can: Write transmit header and data in one transaction
config: x86_64-allyesconfig (https://download.01.org/0day-ci/archive/20230622/202306220154.20bzOw90-lkp@intel.com/config)
compiler: gcc-12 (Debian 12.2.0-14) 12.2.0
reproduce: (https://download.01.org/0day-ci/archive/20230622/202306220154.20bzOw90-lkp@intel.com/reproduce)

If you fix the issue in a separate patch/commit (i.e. not just a new version of
the same patch/commit), kindly add following tags
| Reported-by: kernel test robot <lkp@intel.com>
| Closes: https://lore.kernel.org/oe-kbuild-all/202306220154.20bzOw90-lkp@intel.com/

All warnings (new ones prefixed by >>):

   drivers/net/can/m_can/m_can.c: In function 'm_can_tx_handler':
>> drivers/net/can/m_can/m_can.c:1635:27: warning: unused variable 'fifo_header' [-Wunused-variable]
    1635 |         struct id_and_dlc fifo_header;
         |                           ^~~~~~~~~~~


vim +/fifo_header +1635 drivers/net/can/m_can/m_can.c

10c1c3975a6663 Mario Huettel             2017-04-08  1627  
441ac340169b79 Dan Murphy                2019-05-09  1628  static netdev_tx_t m_can_tx_handler(struct m_can_classdev *cdev)
e0d1f4816f2a7e Dong Aisheng              2014-07-16  1629  {
441ac340169b79 Dan Murphy                2019-05-09  1630  	struct canfd_frame *cf = (struct canfd_frame *)cdev->tx_skb->data;
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1631  	u8 len_padded = DIV_ROUND_UP(cf->len, 4);
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1632  	struct m_can_fifo_element fifo_element;
441ac340169b79 Dan Murphy                2019-05-09  1633  	struct net_device *dev = cdev->net;
441ac340169b79 Dan Murphy                2019-05-09  1634  	struct sk_buff *skb = cdev->tx_skb;
812270e5445bd1 Matt Kline                2021-08-16 @1635  	struct id_and_dlc fifo_header;
812270e5445bd1 Matt Kline                2021-08-16  1636  	u32 cccr, fdflags;
c1eaf8b9bd3145 Markus Schneider-Pargmann 2022-12-06  1637  	u32 txfqs;
812270e5445bd1 Matt Kline                2021-08-16  1638  	int err;
10c1c3975a6663 Mario Huettel             2017-04-08  1639  	int putidx;
e0d1f4816f2a7e Dong Aisheng              2014-07-16  1640  
e04b2cfe61072c Marc Kleine-Budde         2021-05-05  1641  	cdev->tx_skb = NULL;
e04b2cfe61072c Marc Kleine-Budde         2021-05-05  1642  
10c1c3975a6663 Mario Huettel             2017-04-08  1643  	/* Generate ID field for TX buffer Element */
10c1c3975a6663 Mario Huettel             2017-04-08  1644  	/* Common to all supported M_CAN versions */
e0d1f4816f2a7e Dong Aisheng              2014-07-16  1645  	if (cf->can_id & CAN_EFF_FLAG) {
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1646  		fifo_element.id = cf->can_id & CAN_EFF_MASK;
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1647  		fifo_element.id |= TX_BUF_XTD;
e0d1f4816f2a7e Dong Aisheng              2014-07-16  1648  	} else {
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1649  		fifo_element.id = ((cf->can_id & CAN_SFF_MASK) << 18);
e0d1f4816f2a7e Dong Aisheng              2014-07-16  1650  	}
e0d1f4816f2a7e Dong Aisheng              2014-07-16  1651  
e0d1f4816f2a7e Dong Aisheng              2014-07-16  1652  	if (cf->can_id & CAN_RTR_FLAG)
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1653  		fifo_element.id |= TX_BUF_RTR;
e0d1f4816f2a7e Dong Aisheng              2014-07-16  1654  
441ac340169b79 Dan Murphy                2019-05-09  1655  	if (cdev->version == 30) {
10c1c3975a6663 Mario Huettel             2017-04-08  1656  		netif_stop_queue(dev);
10c1c3975a6663 Mario Huettel             2017-04-08  1657  
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1658  		fifo_element.dlc = can_fd_len2dlc(cf->len) << 16;
80646733f11c2e Dong Aisheng              2014-11-18  1659  
812270e5445bd1 Matt Kline                2021-08-16  1660  		/* Write the frame ID, DLC, and payload to the FIFO element. */
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1661  		err = m_can_fifo_write(cdev, 0, M_CAN_FIFO_ID, &fifo_element, 2);
e39381770ec9ca Matt Kline                2021-08-16  1662  		if (err)
e39381770ec9ca Matt Kline                2021-08-16  1663  			goto out_fail;
e39381770ec9ca Matt Kline                2021-08-16  1664  
812270e5445bd1 Matt Kline                2021-08-16  1665  		err = m_can_fifo_write(cdev, 0, M_CAN_FIFO_DATA,
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1666  				       cf->data, len_padded);
e39381770ec9ca Matt Kline                2021-08-16  1667  		if (err)
e39381770ec9ca Matt Kline                2021-08-16  1668  			goto out_fail;
80646733f11c2e Dong Aisheng              2014-11-18  1669  
441ac340169b79 Dan Murphy                2019-05-09  1670  		if (cdev->can.ctrlmode & CAN_CTRLMODE_FD) {
441ac340169b79 Dan Murphy                2019-05-09  1671  			cccr = m_can_read(cdev, M_CAN_CCCR);
20779943a080c5 Torin Cooper-Bennun       2021-05-04  1672  			cccr &= ~CCCR_CMR_MASK;
80646733f11c2e Dong Aisheng              2014-11-18  1673  			if (can_is_canfd_skb(skb)) {
80646733f11c2e Dong Aisheng              2014-11-18  1674  				if (cf->flags & CANFD_BRS)
20779943a080c5 Torin Cooper-Bennun       2021-05-04  1675  					cccr |= FIELD_PREP(CCCR_CMR_MASK,
20779943a080c5 Torin Cooper-Bennun       2021-05-04  1676  							   CCCR_CMR_CANFD_BRS);
80646733f11c2e Dong Aisheng              2014-11-18  1677  				else
20779943a080c5 Torin Cooper-Bennun       2021-05-04  1678  					cccr |= FIELD_PREP(CCCR_CMR_MASK,
20779943a080c5 Torin Cooper-Bennun       2021-05-04  1679  							   CCCR_CMR_CANFD);
80646733f11c2e Dong Aisheng              2014-11-18  1680  			} else {
20779943a080c5 Torin Cooper-Bennun       2021-05-04  1681  				cccr |= FIELD_PREP(CCCR_CMR_MASK, CCCR_CMR_CAN);
80646733f11c2e Dong Aisheng              2014-11-18  1682  			}
441ac340169b79 Dan Murphy                2019-05-09  1683  			m_can_write(cdev, M_CAN_CCCR, cccr);
80646733f11c2e Dong Aisheng              2014-11-18  1684  		}
441ac340169b79 Dan Murphy                2019-05-09  1685  		m_can_write(cdev, M_CAN_TXBTIE, 0x1);
2e8e79c416aae1 Marc Kleine-Budde         2022-03-17  1686  
2e8e79c416aae1 Marc Kleine-Budde         2022-03-17  1687  		can_put_echo_skb(skb, dev, 0, 0);
2e8e79c416aae1 Marc Kleine-Budde         2022-03-17  1688  
441ac340169b79 Dan Murphy                2019-05-09  1689  		m_can_write(cdev, M_CAN_TXBAR, 0x1);
10c1c3975a6663 Mario Huettel             2017-04-08  1690  		/* End of xmit function for version 3.0.x */
10c1c3975a6663 Mario Huettel             2017-04-08  1691  	} else {
10c1c3975a6663 Mario Huettel             2017-04-08  1692  		/* Transmit routine for version >= v3.1.x */
10c1c3975a6663 Mario Huettel             2017-04-08  1693  
c1eaf8b9bd3145 Markus Schneider-Pargmann 2022-12-06  1694  		txfqs = m_can_read(cdev, M_CAN_TXFQS);
c1eaf8b9bd3145 Markus Schneider-Pargmann 2022-12-06  1695  
10c1c3975a6663 Mario Huettel             2017-04-08  1696  		/* Check if FIFO full */
c1eaf8b9bd3145 Markus Schneider-Pargmann 2022-12-06  1697  		if (_m_can_tx_fifo_full(txfqs)) {
10c1c3975a6663 Mario Huettel             2017-04-08  1698  			/* This shouldn't happen */
10c1c3975a6663 Mario Huettel             2017-04-08  1699  			netif_stop_queue(dev);
10c1c3975a6663 Mario Huettel             2017-04-08  1700  			netdev_warn(dev,
10c1c3975a6663 Mario Huettel             2017-04-08  1701  				    "TX queue active although FIFO is full.");
441ac340169b79 Dan Murphy                2019-05-09  1702  
441ac340169b79 Dan Murphy                2019-05-09  1703  			if (cdev->is_peripheral) {
f524f829b75a7d Dan Murphy                2019-05-09  1704  				kfree_skb(skb);
f524f829b75a7d Dan Murphy                2019-05-09  1705  				dev->stats.tx_dropped++;
f524f829b75a7d Dan Murphy                2019-05-09  1706  				return NETDEV_TX_OK;
f524f829b75a7d Dan Murphy                2019-05-09  1707  			} else {
10c1c3975a6663 Mario Huettel             2017-04-08  1708  				return NETDEV_TX_BUSY;
10c1c3975a6663 Mario Huettel             2017-04-08  1709  			}
f524f829b75a7d Dan Murphy                2019-05-09  1710  		}
10c1c3975a6663 Mario Huettel             2017-04-08  1711  
10c1c3975a6663 Mario Huettel             2017-04-08  1712  		/* get put index for frame */
c1eaf8b9bd3145 Markus Schneider-Pargmann 2022-12-06  1713  		putidx = FIELD_GET(TXFQS_TFQPI_MASK, txfqs);
812270e5445bd1 Matt Kline                2021-08-16  1714  
812270e5445bd1 Matt Kline                2021-08-16  1715  		/* Construct DLC Field, with CAN-FD configuration.
812270e5445bd1 Matt Kline                2021-08-16  1716  		 * Use the put index of the fifo as the message marker,
812270e5445bd1 Matt Kline                2021-08-16  1717  		 * used in the TX interrupt for sending the correct echo frame.
812270e5445bd1 Matt Kline                2021-08-16  1718  		 */
10c1c3975a6663 Mario Huettel             2017-04-08  1719  
10c1c3975a6663 Mario Huettel             2017-04-08  1720  		/* get CAN FD configuration of frame */
10c1c3975a6663 Mario Huettel             2017-04-08  1721  		fdflags = 0;
10c1c3975a6663 Mario Huettel             2017-04-08  1722  		if (can_is_canfd_skb(skb)) {
10c1c3975a6663 Mario Huettel             2017-04-08  1723  			fdflags |= TX_BUF_FDF;
10c1c3975a6663 Mario Huettel             2017-04-08  1724  			if (cf->flags & CANFD_BRS)
10c1c3975a6663 Mario Huettel             2017-04-08  1725  				fdflags |= TX_BUF_BRS;
10c1c3975a6663 Mario Huettel             2017-04-08  1726  		}
10c1c3975a6663 Mario Huettel             2017-04-08  1727  
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1728  		fifo_element.dlc = FIELD_PREP(TX_BUF_MM_MASK, putidx) |
e39381770ec9ca Matt Kline                2021-08-16  1729  			FIELD_PREP(TX_BUF_DLC_MASK, can_fd_len2dlc(cf->len)) |
e39381770ec9ca Matt Kline                2021-08-16  1730  			fdflags | TX_BUF_EFC;
10c1c3975a6663 Mario Huettel             2017-04-08  1731  
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1732  		memcpy_and_pad(fifo_element.data, CANFD_MAX_DLEN, &cf->data,
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1733  			       cf->len, 0);
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1734  
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1735  		err = m_can_fifo_write(cdev, putidx, M_CAN_FIFO_ID,
9a10b9d9f7e946 Markus Schneider-Pargmann 2023-06-21  1736  				       &fifo_element, 2 + len_padded);
e39381770ec9ca Matt Kline                2021-08-16  1737  		if (err)
e39381770ec9ca Matt Kline                2021-08-16  1738  			goto out_fail;
10c1c3975a6663 Mario Huettel             2017-04-08  1739  
10c1c3975a6663 Mario Huettel             2017-04-08  1740  		/* Push loopback echo.
10c1c3975a6663 Mario Huettel             2017-04-08  1741  		 * Will be looped back on TX interrupt based on message marker
10c1c3975a6663 Mario Huettel             2017-04-08  1742  		 */
1dcb6e57db8334 Vincent Mailhol           2021-01-11  1743  		can_put_echo_skb(skb, dev, putidx, 0);
10c1c3975a6663 Mario Huettel             2017-04-08  1744  
10c1c3975a6663 Mario Huettel             2017-04-08  1745  		/* Enable TX FIFO element to start transfer  */
441ac340169b79 Dan Murphy                2019-05-09  1746  		m_can_write(cdev, M_CAN_TXBAR, (1 << putidx));
10c1c3975a6663 Mario Huettel             2017-04-08  1747  
10c1c3975a6663 Mario Huettel             2017-04-08  1748  		/* stop network queue if fifo full */
441ac340169b79 Dan Murphy                2019-05-09  1749  		if (m_can_tx_fifo_full(cdev) ||
10c1c3975a6663 Mario Huettel             2017-04-08  1750  		    m_can_next_echo_skb_occupied(dev, putidx))
10c1c3975a6663 Mario Huettel             2017-04-08  1751  			netif_stop_queue(dev);
10c1c3975a6663 Mario Huettel             2017-04-08  1752  	}
e0d1f4816f2a7e Dong Aisheng              2014-07-16  1753  
e0d1f4816f2a7e Dong Aisheng              2014-07-16  1754  	return NETDEV_TX_OK;
e39381770ec9ca Matt Kline                2021-08-16  1755  
e39381770ec9ca Matt Kline                2021-08-16  1756  out_fail:
e39381770ec9ca Matt Kline                2021-08-16  1757  	netdev_err(dev, "FIFO write returned %d\n", err);
e39381770ec9ca Matt Kline                2021-08-16  1758  	m_can_disable_all_interrupts(cdev);
e39381770ec9ca Matt Kline                2021-08-16  1759  	return NETDEV_TX_BUSY;
e0d1f4816f2a7e Dong Aisheng              2014-07-16  1760  }
e0d1f4816f2a7e Dong Aisheng              2014-07-16  1761
Markus Schneider-Pargmann June 22, 2023, 8:56 a.m. UTC | #3 
Hi Simon,

On Wed, Jun 21, 2023 at 04:19:06PM +0200, Simon Horman wrote:
> On Wed, Jun 21, 2023 at 11:23:39AM +0200, Markus Schneider-Pargmann wrote:
> > Combine header and data before writing to the transmit fifo to reduce
> > the overhead for peripheral chips.
> > 
> > Signed-off-by: Markus Schneider-Pargmann <msp@baylibre.com>
> > ---
> >  drivers/net/can/m_can/m_can.c | 34 +++++++++++++++++++++-------------
> >  1 file changed, 21 insertions(+), 13 deletions(-)
> > 
> > diff --git a/drivers/net/can/m_can/m_can.c b/drivers/net/can/m_can/m_can.c
> > index a5003435802b..5251073987ee 100644
> > --- a/drivers/net/can/m_can/m_can.c
> > +++ b/drivers/net/can/m_can/m_can.c
> > @@ -317,6 +317,12 @@ struct id_and_dlc {
> >  	u32 dlc;
> >  };
> >  
> > +struct m_can_fifo_element {
> > +	u32 id;
> > +	u32 dlc;
> > +	u8 data[CANFD_MAX_DLEN];
> > +};
> > +
> >  static inline u32 m_can_read(struct m_can_classdev *cdev, enum m_can_reg reg)
> >  {
> >  	return cdev->ops->read_reg(cdev, reg);
> > @@ -1622,6 +1628,8 @@ static int m_can_next_echo_skb_occupied(struct net_device *dev, int putidx)
> >  static netdev_tx_t m_can_tx_handler(struct m_can_classdev *cdev)
> >  {
> >  	struct canfd_frame *cf = (struct canfd_frame *)cdev->tx_skb->data;
> > +	u8 len_padded = DIV_ROUND_UP(cf->len, 4);
> > +	struct m_can_fifo_element fifo_element;
> >  	struct net_device *dev = cdev->net;
> >  	struct sk_buff *skb = cdev->tx_skb;
> >  	struct id_and_dlc fifo_header;
> 
> Hi Markus,
> 
> GCC 12.3.0 complains that fifo_header is not (no longer) used.
> 
>  drivers/net/can/m_can/m_can.c:1635:20: warning: unused variable 'fifo_header' [-Wunused-variable]
>          struct id_and_dlc fifo_header;

Yes, I moved everything to fifo_element on purpose and then forgot to
remove fifo_header. Removing it now.

Thanks,
Markus

> 
> -- 
> pw-bot: changes-requested
>
diff mbox series

Patch

diff --git a/drivers/net/can/m_can/m_can.c b/drivers/net/can/m_can/m_can.c
index a5003435802b..5251073987ee 100644
--- a/drivers/net/can/m_can/m_can.c
+++ b/drivers/net/can/m_can/m_can.c
@@ -317,6 +317,12 @@  struct id_and_dlc {
 	u32 dlc;
 };
 
+struct m_can_fifo_element {
+	u32 id;
+	u32 dlc;
+	u8 data[CANFD_MAX_DLEN];
+};
+
 static inline u32 m_can_read(struct m_can_classdev *cdev, enum m_can_reg reg)
 {
 	return cdev->ops->read_reg(cdev, reg);
@@ -1622,6 +1628,8 @@  static int m_can_next_echo_skb_occupied(struct net_device *dev, int putidx)
 static netdev_tx_t m_can_tx_handler(struct m_can_classdev *cdev)
 {
 	struct canfd_frame *cf = (struct canfd_frame *)cdev->tx_skb->data;
+	u8 len_padded = DIV_ROUND_UP(cf->len, 4);
+	struct m_can_fifo_element fifo_element;
 	struct net_device *dev = cdev->net;
 	struct sk_buff *skb = cdev->tx_skb;
 	struct id_and_dlc fifo_header;
@@ -1635,27 +1643,27 @@  static netdev_tx_t m_can_tx_handler(struct m_can_classdev *cdev)
 	/* Generate ID field for TX buffer Element */
 	/* Common to all supported M_CAN versions */
 	if (cf->can_id & CAN_EFF_FLAG) {
-		fifo_header.id = cf->can_id & CAN_EFF_MASK;
-		fifo_header.id |= TX_BUF_XTD;
+		fifo_element.id = cf->can_id & CAN_EFF_MASK;
+		fifo_element.id |= TX_BUF_XTD;
 	} else {
-		fifo_header.id = ((cf->can_id & CAN_SFF_MASK) << 18);
+		fifo_element.id = ((cf->can_id & CAN_SFF_MASK) << 18);
 	}
 
 	if (cf->can_id & CAN_RTR_FLAG)
-		fifo_header.id |= TX_BUF_RTR;
+		fifo_element.id |= TX_BUF_RTR;
 
 	if (cdev->version == 30) {
 		netif_stop_queue(dev);
 
-		fifo_header.dlc = can_fd_len2dlc(cf->len) << 16;
+		fifo_element.dlc = can_fd_len2dlc(cf->len) << 16;
 
 		/* Write the frame ID, DLC, and payload to the FIFO element. */
-		err = m_can_fifo_write(cdev, 0, M_CAN_FIFO_ID, &fifo_header, 2);
+		err = m_can_fifo_write(cdev, 0, M_CAN_FIFO_ID, &fifo_element, 2);
 		if (err)
 			goto out_fail;
 
 		err = m_can_fifo_write(cdev, 0, M_CAN_FIFO_DATA,
-				       cf->data, DIV_ROUND_UP(cf->len, 4));
+				       cf->data, len_padded);
 		if (err)
 			goto out_fail;
 
@@ -1717,15 +1725,15 @@  static netdev_tx_t m_can_tx_handler(struct m_can_classdev *cdev)
 				fdflags |= TX_BUF_BRS;
 		}
 
-		fifo_header.dlc = FIELD_PREP(TX_BUF_MM_MASK, putidx) |
+		fifo_element.dlc = FIELD_PREP(TX_BUF_MM_MASK, putidx) |
 			FIELD_PREP(TX_BUF_DLC_MASK, can_fd_len2dlc(cf->len)) |
 			fdflags | TX_BUF_EFC;
-		err = m_can_fifo_write(cdev, putidx, M_CAN_FIFO_ID, &fifo_header, 2);
-		if (err)
-			goto out_fail;
 
-		err = m_can_fifo_write(cdev, putidx, M_CAN_FIFO_DATA,
-				       cf->data, DIV_ROUND_UP(cf->len, 4));
+		memcpy_and_pad(fifo_element.data, CANFD_MAX_DLEN, &cf->data,
+			       cf->len, 0);
+
+		err = m_can_fifo_write(cdev, putidx, M_CAN_FIFO_ID,
+				       &fifo_element, 2 + len_padded);
 		if (err)
 			goto out_fail;