i3c: mipi-i3c-hci: Factor out IO mode setting into helper

Prepare for future reuse.  Move the IO mode setting logic from
i3c_hci_init() into a dedicated helper function, i3c_hci_set_io_mode().

Signed-off-by: Adrian Hunter <adrian.hunter@intel.com>
Reviewed-by: Frank Li <Frank.Li@nxp.com>
Link: https://patch.msgid.link/20260113072702.16268-16-adrian.hunter@intel.com
Signed-off-by: Alexandre Belloni <alexandre.belloni@bootlin.com>

diff --git a/drivers/i3c/master/mipi-i3c-hci/core.c b/drivers/i3c/master/mipi-i3c-hci/core.c
index 3b0609cb7da7c7a5e241fcb249938c6e15fc59ae..569c8584045a29192507f74e4d591498bf5b7fbf 100644 (file)
--- a/drivers/i3c/master/mipi-i3c-hci/core.c
+++ b/drivers/i3c/master/mipi-i3c-hci/core.c
@@ -613,9 +613,35 @@ static int i3c_hci_software_reset(struct i3c_hci *hci)
        return 0;
 }
 
+static inline bool is_version_1_1_or_newer(struct i3c_hci *hci)
+{
+       return hci->version_major > 1 || (hci->version_major == 1 && hci->version_minor > 0);
+}
+
+static int i3c_hci_set_io_mode(struct i3c_hci *hci, bool dma)
+{
+       bool pio_mode;
+
+       if (dma)
+               reg_clear(HC_CONTROL, HC_CONTROL_PIO_MODE);
+       else
+               reg_set(HC_CONTROL, HC_CONTROL_PIO_MODE);
+
+       if (!is_version_1_1_or_newer(hci))
+               return 0;
+
+       pio_mode = reg_read(HC_CONTROL) & HC_CONTROL_PIO_MODE;
+       if ((dma && pio_mode) || (!dma && !pio_mode)) {
+               dev_err(&hci->master.dev, "%s mode is stuck\n", pio_mode ? "PIO" : "DMA");
+               return -EIO;
+       }
+
+       return 0;
+}
+
 static int i3c_hci_init(struct i3c_hci *hci)
 {
-       bool size_in_dwords, mode_selector;
+       bool size_in_dwords;
        u32 regval, offset;
        int ret;
 
@@ -732,20 +758,14 @@ static int i3c_hci_init(struct i3c_hci *hci)
                return -EINVAL;
        }
 
-       mode_selector = hci->version_major > 1 ||
-                               (hci->version_major == 1 && hci->version_minor > 0);
-
        /* Quirk for HCI_QUIRK_PIO_MODE on AMD platforms */
        if (hci->quirks & HCI_QUIRK_PIO_MODE)
                hci->RHS_regs = NULL;
 
        /* Try activating DMA operations first */
        if (hci->RHS_regs) {
-               reg_clear(HC_CONTROL, HC_CONTROL_PIO_MODE);
-               if (mode_selector && (reg_read(HC_CONTROL) & HC_CONTROL_PIO_MODE)) {
-                       dev_err(&hci->master.dev, "PIO mode is stuck\n");
-                       ret = -EIO;
-               } else {
+               ret = i3c_hci_set_io_mode(hci, true);
+               if (!ret) {
                        hci->io = &mipi_i3c_hci_dma;
                        dev_dbg(&hci->master.dev, "Using DMA\n");
                }
@@ -753,11 +773,8 @@ static int i3c_hci_init(struct i3c_hci *hci)
 
        /* If no DMA, try PIO */
        if (!hci->io && hci->PIO_regs) {
-               reg_set(HC_CONTROL, HC_CONTROL_PIO_MODE);
-               if (mode_selector && !(reg_read(HC_CONTROL) & HC_CONTROL_PIO_MODE)) {
-                       dev_err(&hci->master.dev, "DMA mode is stuck\n");
-                       ret = -EIO;
-               } else {
+               ret = i3c_hci_set_io_mode(hci, false);
+               if (!ret) {
                        hci->io = &mipi_i3c_hci_pio;
                        dev_dbg(&hci->master.dev, "Using PIO\n");
                }