diff --git a/drivers/i3c/master/mipi-i3c-hci/core.c b/drivers/i3c/master/mipi-i3c-hci/core.c index c4b249fde764..3b0609cb7da7 100644 --- a/drivers/i3c/master/mipi-i3c-hci/core.c +++ b/drivers/i3c/master/mipi-i3c-hci/core.c @@ -585,6 +585,34 @@ static irqreturn_t i3c_hci_irq_handler(int irq, void *dev_id) return result; } +static int i3c_hci_software_reset(struct i3c_hci *hci) +{ + u32 regval; + int ret; + + /* + * SOFT_RST must be clear before we write to it. + * Then we must wait until it clears again. + */ + ret = readx_poll_timeout(reg_read, RESET_CONTROL, regval, + !(regval & SOFT_RST), 0, 10 * USEC_PER_MSEC); + if (ret) { + dev_err(&hci->master.dev, "%s: Software reset stuck\n", __func__); + return ret; + } + + reg_write(RESET_CONTROL, SOFT_RST); + + ret = readx_poll_timeout(reg_read, RESET_CONTROL, regval, + !(regval & SOFT_RST), 0, 10 * USEC_PER_MSEC); + if (ret) { + dev_err(&hci->master.dev, "%s: Software reset failed\n", __func__); + return ret; + } + + return 0; +} + static int i3c_hci_init(struct i3c_hci *hci) { bool size_in_dwords, mode_selector; @@ -654,18 +682,7 @@ static int i3c_hci_init(struct i3c_hci *hci) if (ret) return ret; - /* - * Now let's reset the hardware. - * SOFT_RST must be clear before we write to it. - * Then we must wait until it clears again. - */ - ret = readx_poll_timeout(reg_read, RESET_CONTROL, regval, - !(regval & SOFT_RST), 1, 10000); - if (ret) - return -ENXIO; - reg_write(RESET_CONTROL, SOFT_RST); - ret = readx_poll_timeout(reg_read, RESET_CONTROL, regval, - !(regval & SOFT_RST), 1, 10000); + ret = i3c_hci_software_reset(hci); if (ret) return -ENXIO;