| Message ID | 20230726150225.483464-17-herve.codina@bootlin.com |
|---|---|
| State | Superseded |
| Headers | show |
| Series | Add support for QMC HDLC, framer infrastruture and PEF2256 framer | expand |
Le 26/07/2023 à 17:02, Herve Codina a écrit : > In order to support runtime timeslot route changes, enable the > channel timeslot entries at channel start() and disable them at > channel stop(). > > Signed-off-by: Herve Codina <herve.codina@bootlin.com> Reviewed-by: Christophe Leroy <christophe.leroy@csgroup.eu> > --- > drivers/soc/fsl/qe/qmc.c | 175 ++++++++++++++++++++++++++++++++++++--- > 1 file changed, 163 insertions(+), 12 deletions(-) > > diff --git a/drivers/soc/fsl/qe/qmc.c b/drivers/soc/fsl/qe/qmc.c > index dc113463fbc7..258a34641551 100644 > --- a/drivers/soc/fsl/qe/qmc.c > +++ b/drivers/soc/fsl/qe/qmc.c > @@ -177,6 +177,7 @@ struct qmc_chan { > struct qmc *qmc; > void __iomem *s_param; > enum qmc_mode mode; > + spinlock_t ts_lock; /* Protect timeslots */ > u64 tx_ts_mask_avail; > u64 tx_ts_mask; > u64 rx_ts_mask_avail; > @@ -265,6 +266,7 @@ static inline void qmc_setbits32(void __iomem *addr, u32 set) > int qmc_chan_get_info(struct qmc_chan *chan, struct qmc_chan_info *info) > { > struct tsa_serial_info tsa_info; > + unsigned long flags; > int ret; > > /* Retrieve info from the TSA related serial */ > @@ -272,6 +274,8 @@ int qmc_chan_get_info(struct qmc_chan *chan, struct qmc_chan_info *info) > if (ret) > return ret; > > + spin_lock_irqsave(&chan->ts_lock, flags); > + > info->mode = chan->mode; > info->rx_fs_rate = tsa_info.rx_fs_rate; > info->rx_bit_rate = tsa_info.rx_bit_rate; > @@ -280,6 +284,8 @@ int qmc_chan_get_info(struct qmc_chan *chan, struct qmc_chan_info *info) > info->tx_bit_rate = tsa_info.tx_bit_rate; > info->nb_rx_ts = hweight64(chan->rx_ts_mask); > > + spin_unlock_irqrestore(&chan->ts_lock, flags); > + > return 0; > } > EXPORT_SYMBOL(qmc_chan_get_info); > @@ -683,6 +689,40 @@ static int qmc_chan_setup_tsa_32tx(struct qmc_chan *chan, const struct tsa_seria > return 0; > } > > +static int qmc_chan_setup_tsa_tx(struct qmc_chan *chan, bool enable) > +{ > + struct tsa_serial_info info; > + int ret; > + > + /* Retrieve info from the TSA related serial */ > + ret = tsa_serial_get_info(chan->qmc->tsa_serial, &info); > + if (ret) > + return ret; > + > + /* Setup entries */ > + if (chan->qmc->is_tsa_64rxtx) > + return qmc_chan_setup_tsa_64rxtx(chan, &info, enable); > + > + return qmc_chan_setup_tsa_32tx(chan, &info, enable); > +} > + > +static int qmc_chan_setup_tsa_rx(struct qmc_chan *chan, bool enable) > +{ > + struct tsa_serial_info info; > + int ret; > + > + /* Retrieve info from the TSA related serial */ > + ret = tsa_serial_get_info(chan->qmc->tsa_serial, &info); > + if (ret) > + return ret; > + > + /* Setup entries */ > + if (chan->qmc->is_tsa_64rxtx) > + return qmc_chan_setup_tsa_64rxtx(chan, &info, enable); > + > + return qmc_chan_setup_tsa_32rx(chan, &info, enable); > +} > + > static int qmc_chan_setup_tsa(struct qmc_chan *chan, bool enable) > { > struct tsa_serial_info info; > @@ -719,6 +759,12 @@ static int qmc_chan_stop_rx(struct qmc_chan *chan) > > spin_lock_irqsave(&chan->rx_lock, flags); > > + if (chan->is_rx_stopped) { > + /* The channel is already stopped -> simply return ok */ > + ret = 0; > + goto end; > + } > + > /* Send STOP RECEIVE command */ > ret = qmc_chan_command(chan, 0x0); > if (ret) { > @@ -729,6 +775,15 @@ static int qmc_chan_stop_rx(struct qmc_chan *chan) > > chan->is_rx_stopped = true; > > + if (!chan->qmc->is_tsa_64rxtx || chan->is_tx_stopped) { > + ret = qmc_chan_setup_tsa_rx(chan, false); > + if (ret) { > + dev_err(chan->qmc->dev, "chan %u: Disable tsa entries failed (%d)\n", > + chan->id, ret); > + goto end; > + } > + } > + > end: > spin_unlock_irqrestore(&chan->rx_lock, flags); > return ret; > @@ -741,6 +796,12 @@ static int qmc_chan_stop_tx(struct qmc_chan *chan) > > spin_lock_irqsave(&chan->tx_lock, flags); > > + if (chan->is_tx_stopped) { > + /* The channel is already stopped -> simply return ok */ > + ret = 0; > + goto end; > + } > + > /* Send STOP TRANSMIT command */ > ret = qmc_chan_command(chan, 0x1); > if (ret) { > @@ -751,37 +812,82 @@ static int qmc_chan_stop_tx(struct qmc_chan *chan) > > chan->is_tx_stopped = true; > > + if (!chan->qmc->is_tsa_64rxtx || chan->is_rx_stopped) { > + ret = qmc_chan_setup_tsa_tx(chan, false); > + if (ret) { > + dev_err(chan->qmc->dev, "chan %u: Disable tsa entries failed (%d)\n", > + chan->id, ret); > + goto end; > + } > + } > + > end: > spin_unlock_irqrestore(&chan->tx_lock, flags); > return ret; > } > > +static int qmc_chan_start_rx(struct qmc_chan *chan); > + > int qmc_chan_stop(struct qmc_chan *chan, int direction) > { > - int ret; > + bool is_rx_rollback_needed = false; > + unsigned long flags; > + int ret = 0; > + > + spin_lock_irqsave(&chan->ts_lock, flags); > > if (direction & QMC_CHAN_READ) { > + is_rx_rollback_needed = !chan->is_rx_stopped; > ret = qmc_chan_stop_rx(chan); > if (ret) > - return ret; > + goto end; > } > > if (direction & QMC_CHAN_WRITE) { > ret = qmc_chan_stop_tx(chan); > - if (ret) > - return ret; > + if (ret) { > + /* Restart rx if needed */ > + if (is_rx_rollback_needed) > + qmc_chan_start_rx(chan); > + goto end; > + } > } > > - return 0; > +end: > + spin_unlock_irqrestore(&chan->ts_lock, flags); > + return ret; > } > EXPORT_SYMBOL(qmc_chan_stop); > > -static void qmc_chan_start_rx(struct qmc_chan *chan) > +static int qmc_setup_chan_trnsync(struct qmc *qmc, struct qmc_chan *chan); > + > +static int qmc_chan_start_rx(struct qmc_chan *chan) > { > unsigned long flags; > + int ret; > > spin_lock_irqsave(&chan->rx_lock, flags); > > + if (!chan->is_rx_stopped) { > + /* The channel is already started -> simply return ok */ > + ret = 0; > + goto end; > + } > + > + ret = qmc_chan_setup_tsa_rx(chan, true); > + if (ret) { > + dev_err(chan->qmc->dev, "chan %u: Enable tsa entries failed (%d)\n", > + chan->id, ret); > + goto end; > + } > + > + ret = qmc_setup_chan_trnsync(chan->qmc, chan); > + if (ret) { > + dev_err(chan->qmc->dev, "chan %u: setup TRNSYNC failed (%d)\n", > + chan->id, ret); > + goto end; > + } > + > /* Restart the receiver */ > if (chan->mode == QMC_TRANSPARENT) > qmc_write32(chan->s_param + QMC_SPE_ZDSTATE, 0x18000080); > @@ -792,15 +898,38 @@ static void qmc_chan_start_rx(struct qmc_chan *chan) > > chan->is_rx_stopped = false; > > +end: > spin_unlock_irqrestore(&chan->rx_lock, flags); > + return ret; > } > > -static void qmc_chan_start_tx(struct qmc_chan *chan) > +static int qmc_chan_start_tx(struct qmc_chan *chan) > { > unsigned long flags; > + int ret; > > spin_lock_irqsave(&chan->tx_lock, flags); > > + if (!chan->is_tx_stopped) { > + /* The channel is already started -> simply return ok */ > + ret = 0; > + goto end; > + } > + > + ret = qmc_chan_setup_tsa_tx(chan, true); > + if (ret) { > + dev_err(chan->qmc->dev, "chan %u: Enable tsa entries failed (%d)\n", > + chan->id, ret); > + goto end; > + } > + > + ret = qmc_setup_chan_trnsync(chan->qmc, chan); > + if (ret) { > + dev_err(chan->qmc->dev, "chan %u: setup TRNSYNC failed (%d)\n", > + chan->id, ret); > + goto end; > + } > + > /* > * Enable channel transmitter as it could be disabled if > * qmc_chan_reset() was called. > @@ -812,18 +941,39 @@ static void qmc_chan_start_tx(struct qmc_chan *chan) > > chan->is_tx_stopped = false; > > +end: > spin_unlock_irqrestore(&chan->tx_lock, flags); > + return ret; > } > > int qmc_chan_start(struct qmc_chan *chan, int direction) > { > - if (direction & QMC_CHAN_READ) > - qmc_chan_start_rx(chan); > + bool is_rx_rollback_needed = false; > + unsigned long flags; > + int ret = 0; > > - if (direction & QMC_CHAN_WRITE) > - qmc_chan_start_tx(chan); > + spin_lock_irqsave(&chan->ts_lock, flags); > > - return 0; > + if (direction & QMC_CHAN_READ) { > + is_rx_rollback_needed = chan->is_rx_stopped; > + ret = qmc_chan_start_rx(chan); > + if (ret) > + goto end; > + } > + > + if (direction & QMC_CHAN_WRITE) { > + ret = qmc_chan_start_tx(chan); > + if (ret) { > + /* Restop rx if needed */ > + if (is_rx_rollback_needed) > + qmc_chan_stop_rx(chan); > + goto end; > + } > + } > + > +end: > + spin_unlock_irqrestore(&chan->ts_lock, flags); > + return ret; > } > EXPORT_SYMBOL(qmc_chan_start); > > @@ -992,6 +1142,7 @@ static int qmc_of_parse_chans(struct qmc *qmc, struct device_node *np) > } > > chan->id = chan_id; > + spin_lock_init(&chan->ts_lock); > spin_lock_init(&chan->rx_lock); > spin_lock_init(&chan->tx_lock); >
diff --git a/drivers/soc/fsl/qe/qmc.c b/drivers/soc/fsl/qe/qmc.c index dc113463fbc7..258a34641551 100644 --- a/drivers/soc/fsl/qe/qmc.c +++ b/drivers/soc/fsl/qe/qmc.c @@ -177,6 +177,7 @@ struct qmc_chan { struct qmc *qmc; void __iomem *s_param; enum qmc_mode mode; + spinlock_t ts_lock; /* Protect timeslots */ u64 tx_ts_mask_avail; u64 tx_ts_mask; u64 rx_ts_mask_avail; @@ -265,6 +266,7 @@ static inline void qmc_setbits32(void __iomem *addr, u32 set) int qmc_chan_get_info(struct qmc_chan *chan, struct qmc_chan_info *info) { struct tsa_serial_info tsa_info; + unsigned long flags; int ret; /* Retrieve info from the TSA related serial */ @@ -272,6 +274,8 @@ int qmc_chan_get_info(struct qmc_chan *chan, struct qmc_chan_info *info) if (ret) return ret; + spin_lock_irqsave(&chan->ts_lock, flags); + info->mode = chan->mode; info->rx_fs_rate = tsa_info.rx_fs_rate; info->rx_bit_rate = tsa_info.rx_bit_rate; @@ -280,6 +284,8 @@ int qmc_chan_get_info(struct qmc_chan *chan, struct qmc_chan_info *info) info->tx_bit_rate = tsa_info.tx_bit_rate; info->nb_rx_ts = hweight64(chan->rx_ts_mask); + spin_unlock_irqrestore(&chan->ts_lock, flags); + return 0; } EXPORT_SYMBOL(qmc_chan_get_info); @@ -683,6 +689,40 @@ static int qmc_chan_setup_tsa_32tx(struct qmc_chan *chan, const struct tsa_seria return 0; } +static int qmc_chan_setup_tsa_tx(struct qmc_chan *chan, bool enable) +{ + struct tsa_serial_info info; + int ret; + + /* Retrieve info from the TSA related serial */ + ret = tsa_serial_get_info(chan->qmc->tsa_serial, &info); + if (ret) + return ret; + + /* Setup entries */ + if (chan->qmc->is_tsa_64rxtx) + return qmc_chan_setup_tsa_64rxtx(chan, &info, enable); + + return qmc_chan_setup_tsa_32tx(chan, &info, enable); +} + +static int qmc_chan_setup_tsa_rx(struct qmc_chan *chan, bool enable) +{ + struct tsa_serial_info info; + int ret; + + /* Retrieve info from the TSA related serial */ + ret = tsa_serial_get_info(chan->qmc->tsa_serial, &info); + if (ret) + return ret; + + /* Setup entries */ + if (chan->qmc->is_tsa_64rxtx) + return qmc_chan_setup_tsa_64rxtx(chan, &info, enable); + + return qmc_chan_setup_tsa_32rx(chan, &info, enable); +} + static int qmc_chan_setup_tsa(struct qmc_chan *chan, bool enable) { struct tsa_serial_info info; @@ -719,6 +759,12 @@ static int qmc_chan_stop_rx(struct qmc_chan *chan) spin_lock_irqsave(&chan->rx_lock, flags); + if (chan->is_rx_stopped) { + /* The channel is already stopped -> simply return ok */ + ret = 0; + goto end; + } + /* Send STOP RECEIVE command */ ret = qmc_chan_command(chan, 0x0); if (ret) { @@ -729,6 +775,15 @@ static int qmc_chan_stop_rx(struct qmc_chan *chan) chan->is_rx_stopped = true; + if (!chan->qmc->is_tsa_64rxtx || chan->is_tx_stopped) { + ret = qmc_chan_setup_tsa_rx(chan, false); + if (ret) { + dev_err(chan->qmc->dev, "chan %u: Disable tsa entries failed (%d)\n", + chan->id, ret); + goto end; + } + } + end: spin_unlock_irqrestore(&chan->rx_lock, flags); return ret; @@ -741,6 +796,12 @@ static int qmc_chan_stop_tx(struct qmc_chan *chan) spin_lock_irqsave(&chan->tx_lock, flags); + if (chan->is_tx_stopped) { + /* The channel is already stopped -> simply return ok */ + ret = 0; + goto end; + } + /* Send STOP TRANSMIT command */ ret = qmc_chan_command(chan, 0x1); if (ret) { @@ -751,37 +812,82 @@ static int qmc_chan_stop_tx(struct qmc_chan *chan) chan->is_tx_stopped = true; + if (!chan->qmc->is_tsa_64rxtx || chan->is_rx_stopped) { + ret = qmc_chan_setup_tsa_tx(chan, false); + if (ret) { + dev_err(chan->qmc->dev, "chan %u: Disable tsa entries failed (%d)\n", + chan->id, ret); + goto end; + } + } + end: spin_unlock_irqrestore(&chan->tx_lock, flags); return ret; } +static int qmc_chan_start_rx(struct qmc_chan *chan); + int qmc_chan_stop(struct qmc_chan *chan, int direction) { - int ret; + bool is_rx_rollback_needed = false; + unsigned long flags; + int ret = 0; + + spin_lock_irqsave(&chan->ts_lock, flags); if (direction & QMC_CHAN_READ) { + is_rx_rollback_needed = !chan->is_rx_stopped; ret = qmc_chan_stop_rx(chan); if (ret) - return ret; + goto end; } if (direction & QMC_CHAN_WRITE) { ret = qmc_chan_stop_tx(chan); - if (ret) - return ret; + if (ret) { + /* Restart rx if needed */ + if (is_rx_rollback_needed) + qmc_chan_start_rx(chan); + goto end; + } } - return 0; +end: + spin_unlock_irqrestore(&chan->ts_lock, flags); + return ret; } EXPORT_SYMBOL(qmc_chan_stop); -static void qmc_chan_start_rx(struct qmc_chan *chan) +static int qmc_setup_chan_trnsync(struct qmc *qmc, struct qmc_chan *chan); + +static int qmc_chan_start_rx(struct qmc_chan *chan) { unsigned long flags; + int ret; spin_lock_irqsave(&chan->rx_lock, flags); + if (!chan->is_rx_stopped) { + /* The channel is already started -> simply return ok */ + ret = 0; + goto end; + } + + ret = qmc_chan_setup_tsa_rx(chan, true); + if (ret) { + dev_err(chan->qmc->dev, "chan %u: Enable tsa entries failed (%d)\n", + chan->id, ret); + goto end; + } + + ret = qmc_setup_chan_trnsync(chan->qmc, chan); + if (ret) { + dev_err(chan->qmc->dev, "chan %u: setup TRNSYNC failed (%d)\n", + chan->id, ret); + goto end; + } + /* Restart the receiver */ if (chan->mode == QMC_TRANSPARENT) qmc_write32(chan->s_param + QMC_SPE_ZDSTATE, 0x18000080); @@ -792,15 +898,38 @@ static void qmc_chan_start_rx(struct qmc_chan *chan) chan->is_rx_stopped = false; +end: spin_unlock_irqrestore(&chan->rx_lock, flags); + return ret; } -static void qmc_chan_start_tx(struct qmc_chan *chan) +static int qmc_chan_start_tx(struct qmc_chan *chan) { unsigned long flags; + int ret; spin_lock_irqsave(&chan->tx_lock, flags); + if (!chan->is_tx_stopped) { + /* The channel is already started -> simply return ok */ + ret = 0; + goto end; + } + + ret = qmc_chan_setup_tsa_tx(chan, true); + if (ret) { + dev_err(chan->qmc->dev, "chan %u: Enable tsa entries failed (%d)\n", + chan->id, ret); + goto end; + } + + ret = qmc_setup_chan_trnsync(chan->qmc, chan); + if (ret) { + dev_err(chan->qmc->dev, "chan %u: setup TRNSYNC failed (%d)\n", + chan->id, ret); + goto end; + } + /* * Enable channel transmitter as it could be disabled if * qmc_chan_reset() was called. @@ -812,18 +941,39 @@ static void qmc_chan_start_tx(struct qmc_chan *chan) chan->is_tx_stopped = false; +end: spin_unlock_irqrestore(&chan->tx_lock, flags); + return ret; } int qmc_chan_start(struct qmc_chan *chan, int direction) { - if (direction & QMC_CHAN_READ) - qmc_chan_start_rx(chan); + bool is_rx_rollback_needed = false; + unsigned long flags; + int ret = 0; - if (direction & QMC_CHAN_WRITE) - qmc_chan_start_tx(chan); + spin_lock_irqsave(&chan->ts_lock, flags); - return 0; + if (direction & QMC_CHAN_READ) { + is_rx_rollback_needed = chan->is_rx_stopped; + ret = qmc_chan_start_rx(chan); + if (ret) + goto end; + } + + if (direction & QMC_CHAN_WRITE) { + ret = qmc_chan_start_tx(chan); + if (ret) { + /* Restop rx if needed */ + if (is_rx_rollback_needed) + qmc_chan_stop_rx(chan); + goto end; + } + } + +end: + spin_unlock_irqrestore(&chan->ts_lock, flags); + return ret; } EXPORT_SYMBOL(qmc_chan_start); @@ -992,6 +1142,7 @@ static int qmc_of_parse_chans(struct qmc *qmc, struct device_node *np) } chan->id = chan_id; + spin_lock_init(&chan->ts_lock); spin_lock_init(&chan->rx_lock); spin_lock_init(&chan->tx_lock);
In order to support runtime timeslot route changes, enable the channel timeslot entries at channel start() and disable them at channel stop(). Signed-off-by: Herve Codina <herve.codina@bootlin.com> --- drivers/soc/fsl/qe/qmc.c | 175 ++++++++++++++++++++++++++++++++++++--- 1 file changed, 163 insertions(+), 12 deletions(-)