diff options
| -rw-r--r-- | drivers/soundwire/cadence_master.c | 50 |
1 files changed, 27 insertions, 23 deletions
diff --git a/drivers/soundwire/cadence_master.c b/drivers/soundwire/cadence_master.c index a6635f7f350e..521387322145 100644 --- a/drivers/soundwire/cadence_master.c +++ b/drivers/soundwire/cadence_master.c @@ -555,6 +555,29 @@ cdns_fill_msg_resp(struct sdw_cdns *cdns, return SDW_CMD_OK; } +static void cdns_read_response(struct sdw_cdns *cdns) +{ + u32 num_resp, cmd_base; + int i; + + /* RX_FIFO_AVAIL can be 2 entries more than the FIFO size */ + BUILD_BUG_ON(ARRAY_SIZE(cdns->response_buf) < CDNS_MCP_CMD_LEN + 2); + + num_resp = cdns_readl(cdns, CDNS_MCP_FIFOSTAT); + num_resp &= CDNS_MCP_RX_FIFO_AVAIL; + if (num_resp > ARRAY_SIZE(cdns->response_buf)) { + dev_warn(cdns->dev, "RX AVAIL %d too long\n", num_resp); + num_resp = ARRAY_SIZE(cdns->response_buf); + } + + cmd_base = CDNS_MCP_CMD_BASE; + + for (i = 0; i < num_resp; i++) { + cdns->response_buf[i] = cdns_readl(cdns, cmd_base); + cmd_base += CDNS_MCP_CMD_WORD_LEN; + } +} + static enum sdw_command_response _cdns_xfer_msg(struct sdw_cdns *cdns, struct sdw_msg *msg, int cmd, int offset, int count, bool defer) @@ -596,6 +619,10 @@ _cdns_xfer_msg(struct sdw_cdns *cdns, struct sdw_msg *msg, int cmd, dev_err(cdns->dev, "IO transfer timed out, cmd %d device %d addr %x len %d\n", cmd, msg->dev_num, msg->addr, msg->len); msg->len = 0; + + /* Drain anything in the RX_FIFO */ + cdns_read_response(cdns); + return SDW_CMD_TIMEOUT; } @@ -769,29 +796,6 @@ EXPORT_SYMBOL(cdns_read_ping_status); * IRQ handling */ -static void cdns_read_response(struct sdw_cdns *cdns) -{ - u32 num_resp, cmd_base; - int i; - - /* RX_FIFO_AVAIL can be 2 entries more than the FIFO size */ - BUILD_BUG_ON(ARRAY_SIZE(cdns->response_buf) < CDNS_MCP_CMD_LEN + 2); - - num_resp = cdns_readl(cdns, CDNS_MCP_FIFOSTAT); - num_resp &= CDNS_MCP_RX_FIFO_AVAIL; - if (num_resp > ARRAY_SIZE(cdns->response_buf)) { - dev_warn(cdns->dev, "RX AVAIL %d too long\n", num_resp); - num_resp = ARRAY_SIZE(cdns->response_buf); - } - - cmd_base = CDNS_MCP_CMD_BASE; - - for (i = 0; i < num_resp; i++) { - cdns->response_buf[i] = cdns_readl(cdns, cmd_base); - cmd_base += CDNS_MCP_CMD_WORD_LEN; - } -} - static int cdns_update_slave_status(struct sdw_cdns *cdns, u64 slave_intstat) { |