diff options
Diffstat (limited to 'drivers')
-rw-r--r-- | drivers/soc/fsl/qe/qmc.c | 66 |
1 files changed, 45 insertions, 21 deletions
diff --git a/drivers/soc/fsl/qe/qmc.c b/drivers/soc/fsl/qe/qmc.c index eacc7dd2be53..b95227378f97 100644 --- a/drivers/soc/fsl/qe/qmc.c +++ b/drivers/soc/fsl/qe/qmc.c @@ -1621,6 +1621,41 @@ static int qmc_init_resources(struct qmc *qmc, struct platform_device *pdev) return qmc_cpm1_init_resources(qmc, pdev); } +static int qmc_cpm1_init_scc(struct qmc *qmc) +{ + u32 val; + int ret; + + /* Connect the serial (SCC) to TSA */ + ret = tsa_serial_connect(qmc->tsa_serial); + if (ret) + return dev_err_probe(qmc->dev, ret, "Failed to connect TSA serial\n"); + + /* Init GMSR_H and GMSR_L registers */ + val = SCC_GSMRH_CDS | SCC_GSMRH_CTSS | SCC_GSMRH_CDP | SCC_GSMRH_CTSP; + qmc_write32(qmc->scc_regs + SCC_GSMRH, val); + + /* enable QMC mode */ + qmc_write32(qmc->scc_regs + SCC_GSMRL, SCC_GSMRL_MODE_QMC); + + /* Disable and clear interrupts */ + qmc_write16(qmc->scc_regs + SCC_SCCM, 0x0000); + qmc_write16(qmc->scc_regs + SCC_SCCE, 0x000F); + + return 0; +} + +static int qmc_init_xcc(struct qmc *qmc) +{ + return qmc_cpm1_init_scc(qmc); +} + +static void qmc_exit_xcc(struct qmc *qmc) +{ + /* Disconnect the serial from TSA */ + tsa_serial_disconnect(qmc->tsa_serial); +} + static int qmc_probe(struct platform_device *pdev) { struct device_node *np = pdev->dev.of_node; @@ -1711,29 +1746,18 @@ static int qmc_probe(struct platform_device *pdev) if (ret) return ret; - /* Connect the serial (SCC) to TSA */ - ret = tsa_serial_connect(qmc->tsa_serial); - if (ret) { - dev_err(qmc->dev, "Failed to connect TSA serial\n"); + /* Init SCC */ + ret = qmc_init_xcc(qmc); + if (ret) return ret; - } - /* Init GMSR_H and GMSR_L registers */ - qmc_write32(qmc->scc_regs + SCC_GSMRH, - SCC_GSMRH_CDS | SCC_GSMRH_CTSS | SCC_GSMRH_CDP | SCC_GSMRH_CTSP); - - /* enable QMC mode */ - qmc_write32(qmc->scc_regs + SCC_GSMRL, SCC_GSMRL_MODE_QMC); - - /* Disable and clear interrupts, set the irq handler */ - qmc_write16(qmc->scc_regs + SCC_SCCM, 0x0000); - qmc_write16(qmc->scc_regs + SCC_SCCE, 0x000F); + /* Set the irq handler */ irq = platform_get_irq(pdev, 0); if (irq < 0) - goto err_tsa_serial_disconnect; + goto err_exit_xcc; ret = devm_request_irq(qmc->dev, irq, qmc_irq_handler, 0, "qmc", qmc); if (ret < 0) - goto err_tsa_serial_disconnect; + goto err_exit_xcc; /* Enable interrupts */ qmc_write16(qmc->scc_regs + SCC_SCCM, @@ -1761,8 +1785,8 @@ err_disable_txrx: err_disable_intr: qmc_write16(qmc->scc_regs + SCC_SCCM, 0); -err_tsa_serial_disconnect: - tsa_serial_disconnect(qmc->tsa_serial); +err_exit_xcc: + qmc_exit_xcc(qmc); return ret; } @@ -1776,8 +1800,8 @@ static void qmc_remove(struct platform_device *pdev) /* Disable interrupts */ qmc_write16(qmc->scc_regs + SCC_SCCM, 0); - /* Disconnect the serial from TSA */ - tsa_serial_disconnect(qmc->tsa_serial); + /* Exit SCC */ + qmc_exit_xcc(qmc); } static const struct qmc_data qmc_data_cpm1 = { |