usb: typec: ucsi: Fix command cancellation

[platform/kernel/linux-starfive.git] / drivers / usb / typec / ucsi / ucsi.c

diff --git a/drivers/usb/typec/ucsi/ucsi.c b/drivers/usb/typec/ucsi/ucsi.c
index 2b472ec..b664ecb 100644 (file)
--- a/drivers/usb/typec/ucsi/ucsi.c
+++ b/drivers/usb/typec/ucsi/ucsi.c
@@ -132,10 +132,8 @@ static int ucsi_exec_command(struct ucsi *ucsi, u64 cmd)
        if (ret)
                return ret;
 
-       if (cci & UCSI_CCI_BUSY) {
-               ucsi->ops->async_write(ucsi, UCSI_CANCEL, NULL, 0);
-               return -EBUSY;
-       }
+       if (cmd != UCSI_CANCEL && cci & UCSI_CCI_BUSY)
+               return ucsi_exec_command(ucsi, UCSI_CANCEL);
 
        if (!(cci & UCSI_CCI_COMMAND_COMPLETE))
                return -EIO;
@@ -149,6 +147,11 @@ static int ucsi_exec_command(struct ucsi *ucsi, u64 cmd)
                return ucsi_read_error(ucsi);
        }
 
+       if (cmd == UCSI_CANCEL && cci & UCSI_CCI_CANCEL_COMPLETE) {
+               ret = ucsi_acknowledge_command(ucsi);
+               return ret ? ret : -EBUSY;
+       }
+
        return UCSI_CCI_LENGTH(cci);
 }