intel: mailbox: Read mailbox response even there is an error

Mailbox driver should read the response data if the response length
in the response header is non-zero even the response header indicates
error (non-zero).

Signed-off-by: Chee Hong Ang <chee.hong.ang@intel.com>
Change-Id: I928f705f43c0f46ac74b84428b830276cc4c9640

diff --git a/plat/intel/soc/common/soc/socfpga_mailbox.c b/plat/intel/soc/common/soc/socfpga_mailbox.c
index 3bb2561cb6f7cf68d1ff10101623067b4bcc2df2..4bae66efd8af7a00c1e452e3345a3c3d6adef8ae 100644 (file)
--- a/plat/intel/soc/common/soc/socfpga_mailbox.c
+++ b/plat/intel/soc/common/soc/socfpga_mailbox.c
@@ -46,6 +46,7 @@ int mailbox_read_response(uint32_t *job_id, uint32_t *response, int resp_len)
        int rin = 0;
        int rout = 0;
        int resp_data = 0;
+       int ret_resp_len;
 
        if (mmio_read_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM))
                mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0);
@@ -67,13 +68,19 @@ int mailbox_read_response(uint32_t *job_id, uint32_t *response, int resp_len)
 
                *job_id = MBOX_RESP_JOB_ID(resp_data);
 
+               ret_resp_len = MBOX_RESP_LEN(resp_data);
+
+               if (ret_resp_len != 0) {
+                       ret_resp_len = iterate_resp(ret_resp_len, response,
+                                                   resp_len);
+               }
+
                if (MBOX_RESP_ERR(resp_data) > 0) {
                        INFO("Error in response: %x\n", resp_data);
                        return -resp_data;
                }
 
-               return iterate_resp(MBOX_RESP_LEN(resp_data),
-                                       response, resp_len);
+               return ret_resp_len;
        }
        return MBOX_NO_RESPONSE;
 }
@@ -86,6 +93,7 @@ int mailbox_poll_response(uint32_t job_id, int urgent, uint32_t *response,
        int rin = 0;
        int rout = 0;
        int resp_data = 0;
+       int ret_resp_len;
 
        while (1) {
 
@@ -130,13 +138,20 @@ int mailbox_poll_response(uint32_t job_id, int urgent, uint32_t *response,
                                || MBOX_RESP_JOB_ID(resp_data) != job_id)
                                continue;
 
+                       ret_resp_len = MBOX_RESP_LEN(resp_data);
+
+                       if (ret_resp_len != 0) {
+                               ret_resp_len = iterate_resp(ret_resp_len,
+                                                           response,
+                                                           resp_len);
+                       }
+
                        if (MBOX_RESP_ERR(resp_data) > 0) {
                                INFO("Error in response: %x\n", resp_data);
                                return -MBOX_RESP_ERR(resp_data);
                        }
 
-                       return iterate_resp(MBOX_RESP_LEN(resp_data),
-                                               response, resp_len);
+                       return ret_resp_len;
                }
        }
 }