diff --git a/drivers/net/can/flexcan.c b/drivers/net/can/flexcan.c index f16668b8dd61..b5b9045b86ba 100644 --- a/drivers/net/can/flexcan.c +++ b/drivers/net/can/flexcan.c @@ -1188,6 +1188,7 @@ static void flexcan_chip_stop(struct net_device *dev) static int flexcan_open(struct net_device *dev) { struct flexcan_priv *priv = netdev_priv(dev); + struct flexcan_regs __iomem *regs = priv->regs; int err; err = clk_prepare_enable(priv->clk_ipg); @@ -1206,10 +1207,41 @@ static int flexcan_open(struct net_device *dev) if (err) goto out_close; + if (priv->devtype_data->quirks & FLEXCAN_QUIRK_USE_OFF_TIMESTAMP) + priv->tx_mb_reserved = ®s->mb[FLEXCAN_TX_MB_RESERVED_OFF_TIMESTAMP]; + else + priv->tx_mb_reserved = ®s->mb[FLEXCAN_TX_MB_RESERVED_OFF_FIFO]; + + priv->reg_imask1_default = 0; + priv->reg_imask2_default = FLEXCAN_IFLAG_MB(FLEXCAN_TX_MB); + + priv->offload.mailbox_read = flexcan_mailbox_read; + + if (priv->devtype_data->quirks & FLEXCAN_QUIRK_USE_OFF_TIMESTAMP) { + u64 imask; + + priv->offload.mb_first = FLEXCAN_RX_MB_OFF_TIMESTAMP_FIRST; + priv->offload.mb_last = FLEXCAN_RX_MB_OFF_TIMESTAMP_LAST; + + imask = GENMASK_ULL(priv->offload.mb_last, + priv->offload.mb_first); + priv->reg_imask1_default |= imask; + priv->reg_imask2_default |= imask >> 32; + + err = can_rx_offload_add_timestamp(dev, &priv->offload); + } else { + priv->reg_imask1_default |= FLEXCAN_IFLAG_RX_FIFO_OVERFLOW | + FLEXCAN_IFLAG_RX_FIFO_AVAILABLE; + err = can_rx_offload_add_fifo(dev, &priv->offload, + FLEXCAN_NAPI_WEIGHT); + } + if (err) + goto out_free_irq; + /* start chip and queuing */ err = flexcan_chip_start(dev); if (err) - goto out_free_irq; + goto out_offload_del; can_led_event(dev, CAN_LED_EVENT_OPEN); @@ -1218,6 +1250,8 @@ static int flexcan_open(struct net_device *dev) return 0; + out_offload_del: + can_rx_offload_del(&priv->offload); out_free_irq: free_irq(dev->irq, dev); out_close: @@ -1238,6 +1272,7 @@ static int flexcan_close(struct net_device *dev) can_rx_offload_disable(&priv->offload); flexcan_chip_stop(dev); + can_rx_offload_del(&priv->offload); free_irq(dev->irq, dev); clk_disable_unprepare(priv->clk_per); clk_disable_unprepare(priv->clk_ipg); @@ -1502,35 +1537,6 @@ static int flexcan_probe(struct platform_device *pdev) priv->devtype_data = devtype_data; priv->reg_xceiver = reg_xceiver; - if (priv->devtype_data->quirks & FLEXCAN_QUIRK_USE_OFF_TIMESTAMP) - priv->tx_mb_reserved = ®s->mb[FLEXCAN_TX_MB_RESERVED_OFF_TIMESTAMP]; - else - priv->tx_mb_reserved = ®s->mb[FLEXCAN_TX_MB_RESERVED_OFF_FIFO]; - - priv->reg_imask1_default = 0; - priv->reg_imask2_default = FLEXCAN_IFLAG_MB(FLEXCAN_TX_MB); - - priv->offload.mailbox_read = flexcan_mailbox_read; - - if (priv->devtype_data->quirks & FLEXCAN_QUIRK_USE_OFF_TIMESTAMP) { - u64 imask; - - priv->offload.mb_first = FLEXCAN_RX_MB_OFF_TIMESTAMP_FIRST; - priv->offload.mb_last = FLEXCAN_RX_MB_OFF_TIMESTAMP_LAST; - - imask = GENMASK_ULL(priv->offload.mb_last, priv->offload.mb_first); - priv->reg_imask1_default |= imask; - priv->reg_imask2_default |= imask >> 32; - - err = can_rx_offload_add_timestamp(dev, &priv->offload); - } else { - priv->reg_imask1_default |= FLEXCAN_IFLAG_RX_FIFO_OVERFLOW | - FLEXCAN_IFLAG_RX_FIFO_AVAILABLE; - err = can_rx_offload_add_fifo(dev, &priv->offload, FLEXCAN_NAPI_WEIGHT); - } - if (err) - goto failed_offload; - err = register_flexcandev(dev); if (err) { dev_err(&pdev->dev, "registering netdev failed\n"); @@ -1550,7 +1556,6 @@ static int flexcan_probe(struct platform_device *pdev) return 0; - failed_offload: failed_register: free_candev(dev); return err; @@ -1559,10 +1564,8 @@ static int flexcan_probe(struct platform_device *pdev) static int flexcan_remove(struct platform_device *pdev) { struct net_device *dev = platform_get_drvdata(pdev); - struct flexcan_priv *priv = netdev_priv(dev); unregister_flexcandev(dev); - can_rx_offload_del(&priv->offload); free_candev(dev); return 0;