@@ -63,25 +63,74 @@ static int ucsi_acknowledge(struct ucsi *ucsi, bool conn_ack)
return ucsi->ops->sync_control(ucsi, ctrl);
}
-static int ucsi_exec_command(struct ucsi *ucsi, u64 command);
-
-static int ucsi_read_error(struct ucsi *ucsi)
+static int ucsi_run_command(struct ucsi *ucsi, u64 command, u32 *cci,
+ void *data, size_t size, bool conn_ack)
{
- u16 error;
int ret;
- ret = ucsi_exec_command(ucsi, UCSI_GET_ERROR_STATUS);
- if (ret < 0)
+ *cci = 0;
+
+ ret = ucsi->ops->sync_control(ucsi, command);
+ if (ret)
return ret;
- ret = ucsi_read_message_in(ucsi, &error, sizeof(error));
+ ret = ucsi->ops->read_cci(ucsi, cci);
if (ret)
return ret;
- ret = ucsi_acknowledge(ucsi, false);
+ if (*cci & UCSI_CCI_BUSY)
+ return -EBUSY;
+
+ if (!(*cci & UCSI_CCI_COMMAND_COMPLETE))
+ return -EIO;
+
+ if (*cci & UCSI_CCI_NOT_SUPPORTED) {
+ if (ucsi_acknowledge(ucsi, false) < 0)
+ dev_err(ucsi->dev,
+ "ACK of unsupported command failed\n");
+ return -EOPNOTSUPP;
+ }
+
+ if (*cci & UCSI_CCI_ERROR) {
+ /* Acknowledge the command that failed */
+ ret = ucsi_acknowledge(ucsi, false);
+ return ret ? ret : -EIO;
+ }
+
+ if (data) {
+ ret = ucsi_read_message_in(ucsi, data, size);
+ if (ret)
+ return ret;
+ }
+
+ ret = ucsi_acknowledge(ucsi, conn_ack);
if (ret)
return ret;
+ return 0;
+}
+
+static int ucsi_read_error(struct ucsi *ucsi)
+{
+ u16 error;
+ u32 cci;
+ int ret;
+
+ ret = ucsi_run_command(ucsi, UCSI_GET_ERROR_STATUS, &cci,
+ &error, sizeof(error), false);
+
+ if (cci & UCSI_CCI_BUSY) {
+ ret = ucsi_run_command(ucsi, UCSI_CANCEL, &cci, NULL, 0, false);
+
+ return ret ? ret : -EBUSY;
+ }
+
+ if (ret < 0)
+ return ret;
+
+ if (cci & UCSI_CCI_ERROR)
+ return -EIO;
+
switch (error) {
case UCSI_ERROR_INCOMPATIBLE_PARTNER:
return -EOPNOTSUPP;
@@ -121,78 +170,23 @@ static int ucsi_read_error(struct ucsi *ucsi)
return -EIO;
}
-static int ucsi_exec_command(struct ucsi *ucsi, u64 cmd)
-{
- u32 cci;
- int ret;
-
- ret = ucsi->ops->sync_control(ucsi, cmd);
- if (ret)
- return ret;
-
- ret = ucsi->ops->read_cci(ucsi, &cci);
- if (ret)
- return ret;
-
- if (cmd != UCSI_CANCEL && cci & UCSI_CCI_BUSY)
- return ucsi_exec_command(ucsi, UCSI_CANCEL);
-
- if (!(cci & UCSI_CCI_COMMAND_COMPLETE))
- return -EIO;
-
- if (cci & UCSI_CCI_NOT_SUPPORTED) {
- if (ucsi_acknowledge(ucsi, false) < 0)
- dev_err(ucsi->dev,
- "ACK of unsupported command failed\n");
- return -EOPNOTSUPP;
- }
-
- if (cci & UCSI_CCI_ERROR) {
- /* Acknowledge the command that failed */
- ret = ucsi_acknowledge(ucsi, false);
- if (ret)
- return ret;
-
- if (cmd == UCSI_GET_ERROR_STATUS)
- return -EIO;
-
- return ucsi_read_error(ucsi);
- }
-
- if (cmd == UCSI_CANCEL && cci & UCSI_CCI_CANCEL_COMPLETE) {
- ret = ucsi_acknowledge(ucsi, false);
- return ret ? ret : -EBUSY;
- }
-
- return UCSI_CCI_LENGTH(cci);
-}
-
static int ucsi_send_command_common(struct ucsi *ucsi, u64 command,
void *data, size_t size, bool conn_ack)
{
- u8 length;
+ u32 cci;
int ret;
mutex_lock(&ucsi->ppm_lock);
- ret = ucsi_exec_command(ucsi, command);
- if (ret < 0)
- goto out;
-
- length = ret;
-
- if (data) {
- ret = ucsi_read_message_in(ucsi, data, size);
- if (ret)
- goto out;
+ ret = ucsi_run_command(ucsi, command, &cci, data, size, conn_ack);
+ if (cci & UCSI_CCI_BUSY) {
+ ret = ucsi_run_command(ucsi, UCSI_CANCEL, &cci, NULL, 0, false);
+ return ret ? ret : -EBUSY;
}
- ret = ucsi_acknowledge(ucsi, conn_ack);
- if (ret)
- goto out;
+ if (cci & UCSI_CCI_ERROR)
+ return ucsi_read_error(ucsi);
- ret = length;
-out:
mutex_unlock(&ucsi->ppm_lock);
return ret;
}
Rework command execution code to remove recursive calls of ucsi_exec_command. This also streamlines the sync_control / read(CCI) read (MESSAGE_IN) sequence, allowing further rework of the command code. Signed-off-by: Dmitry Baryshkov <dmitry.baryshkov@linaro.org> --- drivers/usb/typec/ucsi/ucsi.c | 134 ++++++++++++++++++++---------------------- 1 file changed, 64 insertions(+), 70 deletions(-)