diff options
| -rw-r--r-- | drivers/net/can/flexcan.c | 60 |
1 files changed, 49 insertions, 11 deletions
diff --git a/drivers/net/can/flexcan.c b/drivers/net/can/flexcan.c index 6b0fecd0a6ad..330b5b957a39 100644 --- a/drivers/net/can/flexcan.c +++ b/drivers/net/can/flexcan.c @@ -325,6 +325,44 @@ static int flexcan_chip_disable(struct flexcan_priv *priv) return 0; } +static int flexcan_chip_freeze(struct flexcan_priv *priv) +{ + struct flexcan_regs __iomem *regs = priv->base; + unsigned int timeout = 1000 * 1000 * 10 / priv->can.bittiming.bitrate; + u32 reg; + + reg = flexcan_read(®s->mcr); + reg |= FLEXCAN_MCR_HALT; + flexcan_write(reg, ®s->mcr); + + while (timeout-- && !(flexcan_read(®s->mcr) & FLEXCAN_MCR_FRZ_ACK)) + usleep_range(100, 200); + + if (!(flexcan_read(®s->mcr) & FLEXCAN_MCR_FRZ_ACK)) + return -ETIMEDOUT; + + return 0; +} + +static int flexcan_chip_unfreeze(struct flexcan_priv *priv) +{ + struct flexcan_regs __iomem *regs = priv->base; + unsigned int timeout = FLEXCAN_TIMEOUT_US / 10; + u32 reg; + + reg = flexcan_read(®s->mcr); + reg &= ~FLEXCAN_MCR_HALT; + flexcan_write(reg, ®s->mcr); + + while (timeout-- && (flexcan_read(®s->mcr) & FLEXCAN_MCR_FRZ_ACK)) + usleep_range(10, 20); + + if (flexcan_read(®s->mcr) & FLEXCAN_MCR_FRZ_ACK) + return -ETIMEDOUT; + + return 0; +} + static int flexcan_get_berr_counter(const struct net_device *dev, struct can_berr_counter *bec) { @@ -756,7 +794,7 @@ static int flexcan_chip_start(struct net_device *dev) netdev_err(dev, "Failed to softreset can module (mcr=0x%08x)\n", reg_mcr); err = -ENODEV; - goto out; + goto out_chip_disable; } flexcan_set_bittiming(dev); @@ -826,12 +864,12 @@ static int flexcan_chip_start(struct net_device *dev) err = flexcan_transceiver_enable(priv); if (err) - goto out; + goto out_chip_disable; /* synchronize with the can bus */ - reg_mcr = flexcan_read(®s->mcr); - reg_mcr &= ~FLEXCAN_MCR_HALT; - flexcan_write(reg_mcr, ®s->mcr); + err = flexcan_chip_unfreeze(priv); + if (err) + goto out_transceiver_disable; priv->can.state = CAN_STATE_ERROR_ACTIVE; @@ -844,7 +882,9 @@ static int flexcan_chip_start(struct net_device *dev) return 0; - out: + out_transceiver_disable: + flexcan_transceiver_disable(priv); + out_chip_disable: flexcan_chip_disable(priv); return err; } @@ -859,12 +899,10 @@ static void flexcan_chip_stop(struct net_device *dev) { struct flexcan_priv *priv = netdev_priv(dev); struct flexcan_regs __iomem *regs = priv->base; - u32 reg; - /* Disable + halt module */ - reg = flexcan_read(®s->mcr); - reg |= FLEXCAN_MCR_MDIS | FLEXCAN_MCR_HALT; - flexcan_write(reg, ®s->mcr); + /* freeze + disable module */ + flexcan_chip_freeze(priv); + flexcan_chip_disable(priv); /* Disable all interrupts */ flexcan_write(0, ®s->imask1); |