[v2,16/28] soc: fsl: cpm1: qmc: Handle timeslot entries at channel start() and stop()
Commit Message
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(-)
Comments
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);
>
@@ -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);