soc: fsl: cpm1: qmc: Introduce qmc_chan_setup_tsa*

Introduce the qmc_chan_setup_tsa* functions to setup entries related
to the given channel.
Use them during QMC channels setup.

Signed-off-by: Herve Codina <herve.codina@bootlin.com>
Reviewed-by: Christophe Leroy <christophe.leroy@csgroup.eu>
Link: https://lore.kernel.org/r/20231205152116.122512-10-herve.codina@bootlin.com

diff --git a/drivers/soc/fsl/qe/qmc.c b/drivers/soc/fsl/qe/qmc.c
index e3f2afb8fa4d6294ccc679e12781edbd81f7f529..5d7e2ecdd933eb49610d61f2ddd2f59ad4d56c1d 100644 (file)
--- a/drivers/soc/fsl/qe/qmc.c
+++ b/drivers/soc/fsl/qe/qmc.c
@@ -240,6 +240,11 @@ static void qmc_clrbits16(void __iomem *addr, u16 clr)
        qmc_write16(addr, qmc_read16(addr) & ~clr);
 }
 
+static void qmc_clrsetbits16(void __iomem *addr, u16 clr, u16 set)
+{
+       qmc_write16(addr, (qmc_read16(addr) & ~clr) | set);
+}
+
 static void qmc_write32(void __iomem *addr, u32 val)
 {
        iowrite32be(val, addr);
@@ -562,6 +567,122 @@ end:
        spin_unlock_irqrestore(&chan->rx_lock, flags);
 }
 
+static int qmc_chan_setup_tsa_64rxtx(struct qmc_chan *chan, const struct tsa_serial_info *info)
+{
+       unsigned int i;
+       u16 curr;
+       u16 val;
+
+       /*
+        * Use a common Tx/Rx 64 entries table.
+        * Tx and Rx related stuffs must be identical
+        */
+       if (chan->tx_ts_mask != chan->rx_ts_mask) {
+               dev_err(chan->qmc->dev, "chan %u uses different Rx and Tx TS\n", chan->id);
+               return -EINVAL;
+       }
+
+       val = QMC_TSA_VALID | QMC_TSA_MASK | QMC_TSA_CHANNEL(chan->id);
+
+       /* Check entries based on Rx stuff*/
+       for (i = 0; i < info->nb_rx_ts; i++) {
+               if (!(chan->rx_ts_mask & (((u64)1) << i)))
+                       continue;
+
+               curr = qmc_read16(chan->qmc->scc_pram + QMC_GBL_TSATRX + (i * 2));
+               if (curr & QMC_TSA_VALID && (curr & ~QMC_TSA_WRAP) != val) {
+                       dev_err(chan->qmc->dev, "chan %u TxRx entry %d already used\n",
+                               chan->id, i);
+                       return -EBUSY;
+               }
+       }
+
+       /* Set entries based on Rx stuff*/
+       for (i = 0; i < info->nb_rx_ts; i++) {
+               if (!(chan->rx_ts_mask & (((u64)1) << i)))
+                       continue;
+
+               qmc_clrsetbits16(chan->qmc->scc_pram + QMC_GBL_TSATRX + (i * 2),
+                                ~QMC_TSA_WRAP, val);
+       }
+
+       return 0;
+}
+
+static int qmc_chan_setup_tsa_32rx_32tx(struct qmc_chan *chan, const struct tsa_serial_info *info)
+{
+       unsigned int i;
+       u16 curr;
+       u16 val;
+
+       /* Use a Tx 32 entries table and a Rx 32 entries table */
+
+       val = QMC_TSA_VALID | QMC_TSA_MASK | QMC_TSA_CHANNEL(chan->id);
+
+       /* Check entries based on Rx stuff */
+       for (i = 0; i < info->nb_rx_ts; i++) {
+               if (!(chan->rx_ts_mask & (((u64)1) << i)))
+                       continue;
+
+               curr = qmc_read16(chan->qmc->scc_pram + QMC_GBL_TSATRX + (i * 2));
+               if (curr & QMC_TSA_VALID && (curr & ~QMC_TSA_WRAP) != val) {
+                       dev_err(chan->qmc->dev, "chan %u Rx entry %d already used\n",
+                               chan->id, i);
+                       return -EBUSY;
+               }
+       }
+       /* Check entries based on Tx stuff */
+       for (i = 0; i < info->nb_tx_ts; i++) {
+               if (!(chan->tx_ts_mask & (((u64)1) << i)))
+                       continue;
+
+               curr = qmc_read16(chan->qmc->scc_pram + QMC_GBL_TSATTX + (i * 2));
+               if (curr & QMC_TSA_VALID && (curr & ~QMC_TSA_WRAP) != val) {
+                       dev_err(chan->qmc->dev, "chan %u Tx entry %d already used\n",
+                               chan->id, i);
+                       return -EBUSY;
+               }
+       }
+
+       /* Set entries based on Rx stuff */
+       for (i = 0; i < info->nb_rx_ts; i++) {
+               if (!(chan->rx_ts_mask & (((u64)1) << i)))
+                       continue;
+
+               qmc_clrsetbits16(chan->qmc->scc_pram + QMC_GBL_TSATRX + (i * 2),
+                                ~QMC_TSA_WRAP, val);
+       }
+       /* Set entries based on Tx stuff */
+       for (i = 0; i < info->nb_tx_ts; i++) {
+               if (!(chan->tx_ts_mask & (((u64)1) << i)))
+                       continue;
+
+               qmc_clrsetbits16(chan->qmc->scc_pram + QMC_GBL_TSATTX + (i * 2),
+                                ~QMC_TSA_WRAP, val);
+       }
+
+       return 0;
+}
+
+static int qmc_chan_setup_tsa(struct qmc_chan *chan)
+{
+       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 one common 64 entries table or two 32 entries (one for Tx
+        * and one for Tx) according to assigned TS numbers.
+        */
+       return ((info.nb_tx_ts > 32) || (info.nb_rx_ts > 32)) ?
+               qmc_chan_setup_tsa_64rxtx(chan, &info) :
+               qmc_chan_setup_tsa_32rx_32tx(chan, &info);
+}
+
 static int qmc_chan_command(struct qmc_chan *chan, u8 qmc_opcode)
 {
        return cpm_command(chan->id << 2, (qmc_opcode << 4) | 0x0E);
@@ -921,7 +1042,6 @@ static int qmc_of_parse_chans(struct qmc *qmc, struct device_node *np)
 
 static int qmc_init_tsa_64rxtx(struct qmc *qmc, const struct tsa_serial_info *info)
 {
-       struct qmc_chan *chan;
        unsigned int i;
        u16 val;
 
@@ -935,18 +1055,6 @@ static int qmc_init_tsa_64rxtx(struct qmc *qmc, const struct tsa_serial_info *in
        for (i = 0; i < 64; i++)
                qmc_write16(qmc->scc_pram + QMC_GBL_TSATRX + (i * 2), 0x0000);
 
-       /* Set entries based on Rx stuff*/
-       list_for_each_entry(chan, &qmc->chan_head, list) {
-               for (i = 0; i < info->nb_rx_ts; i++) {
-                       if (!(chan->rx_ts_mask & (((u64)1) << i)))
-                               continue;
-
-                       val = QMC_TSA_VALID | QMC_TSA_MASK |
-                             QMC_TSA_CHANNEL(chan->id);
-                       qmc_write16(qmc->scc_pram + QMC_GBL_TSATRX + (i * 2), val);
-               }
-       }
-
        /* Set Wrap bit on last entry */
        qmc_setbits16(qmc->scc_pram + QMC_GBL_TSATRX + ((info->nb_rx_ts - 1) * 2),
                      QMC_TSA_WRAP);
@@ -963,7 +1071,6 @@ static int qmc_init_tsa_64rxtx(struct qmc *qmc, const struct tsa_serial_info *in
 
 static int qmc_init_tsa_32rx_32tx(struct qmc *qmc, const struct tsa_serial_info *info)
 {
-       struct qmc_chan *chan;
        unsigned int i;
        u16 val;
 
@@ -978,28 +1085,6 @@ static int qmc_init_tsa_32rx_32tx(struct qmc *qmc, const struct tsa_serial_info
                qmc_write16(qmc->scc_pram + QMC_GBL_TSATTX + (i * 2), 0x0000);
        }
 
-       /* Set entries based on Rx and Tx stuff*/
-       list_for_each_entry(chan, &qmc->chan_head, list) {
-               /* Rx part */
-               for (i = 0; i < info->nb_rx_ts; i++) {
-                       if (!(chan->rx_ts_mask & (((u64)1) << i)))
-                               continue;
-
-                       val = QMC_TSA_VALID | QMC_TSA_MASK |
-                             QMC_TSA_CHANNEL(chan->id);
-                       qmc_write16(qmc->scc_pram + QMC_GBL_TSATRX + (i * 2), val);
-               }
-               /* Tx part */
-               for (i = 0; i < info->nb_tx_ts; i++) {
-                       if (!(chan->tx_ts_mask & (((u64)1) << i)))
-                               continue;
-
-                       val = QMC_TSA_VALID | QMC_TSA_MASK |
-                             QMC_TSA_CHANNEL(chan->id);
-                       qmc_write16(qmc->scc_pram + QMC_GBL_TSATTX + (i * 2), val);
-               }
-       }
-
        /* Set Wrap bit on last entries */
        qmc_setbits16(qmc->scc_pram + QMC_GBL_TSATRX + ((info->nb_rx_ts - 1) * 2),
                      QMC_TSA_WRAP);
@@ -1081,6 +1166,10 @@ static int qmc_setup_chan(struct qmc *qmc, struct qmc_chan *chan)
 
        chan->qmc = qmc;
 
+       ret = qmc_chan_setup_tsa(chan);
+       if (ret)
+               return ret;
+
        /* Set channel specific parameter base address */
        chan->s_param = qmc->dpram + (chan->id * 64);
        /* 16 bd per channel (8 rx and 8 tx) */