diff --git a/drivers/mailbox/mailbox-mpfs.c b/drivers/mailbox/mailbox-mpfs.c index 0d176aba3462..162df49654fb 100644 --- a/drivers/mailbox/mailbox-mpfs.c +++ b/drivers/mailbox/mailbox-mpfs.c @@ -82,8 +82,22 @@ static bool mpfs_mbox_busy(struct mpfs_mbox *mbox) static bool mpfs_mbox_last_tx_done(struct mbox_chan *chan) { struct mpfs_mbox *mbox = (struct mpfs_mbox *)chan->con_priv; + struct mpfs_mss_response *response = mbox->response; + u32 val; - return !mpfs_mbox_busy(mbox); + if (mpfs_mbox_busy(mbox)) + return false; + + /* + * The service status is stored in bits 31:16 of the SERVICES_SR + * register & is only valid when the system controller is not busy. + * Failed services are intended to generated interrupts, but in reality + * this does not happen, so the status must be checked here. + */ + val = readl_relaxed(mbox->ctrl_base + SERVICES_SR_OFFSET); + response->resp_status = (val & SCB_STATUS_MASK) >> SCB_STATUS_POS; + + return true; } static int mpfs_mbox_send_data(struct mbox_chan *chan, void *data) @@ -138,7 +152,7 @@ static void mpfs_mbox_rx_data(struct mbox_chan *chan) struct mpfs_mbox *mbox = (struct mpfs_mbox *)chan->con_priv; struct mpfs_mss_response *response = mbox->response; u16 num_words = ALIGN((response->resp_size), (4)) / 4U; - u32 i, status; + u32 i; if (!response->resp_msg) { dev_err(mbox->dev, "failed to assign memory for response %d\n", -ENOMEM); @@ -146,8 +160,6 @@ static void mpfs_mbox_rx_data(struct mbox_chan *chan) } /* - * The status is stored in bits 31:16 of the SERVICES_SR register. - * It is only valid when BUSY == 0. * We should *never* get an interrupt while the controller is * still in the busy state. If we do, something has gone badly * wrong & the content of the mailbox would not be valid. @@ -158,18 +170,6 @@ static void mpfs_mbox_rx_data(struct mbox_chan *chan) return; } - status = readl_relaxed(mbox->ctrl_base + SERVICES_SR_OFFSET); - - /* - * If the status of the individual servers is non-zero, the service has - * failed. The contents of the mailbox at this point are not be valid, - * so don't bother reading them. Set the status so that the driver - * implementing the service can handle the result. - */ - response->resp_status = (status & SCB_STATUS_MASK) >> SCB_STATUS_POS; - if (response->resp_status) - return; - for (i = 0; i < num_words; i++) { response->resp_msg[i] = readl_relaxed(mbox->mbox_base