From f6dcf407598411186b0d55b148a23d8e514a0557 Mon Sep 17 00:00:00 2001 From: Chee Hong Ang Date: Wed, 12 Aug 2020 09:56:23 +0800 Subject: [PATCH] arm: socfpga: mailbox: Support sending large mailbox command Mailbox command which is too large to fit into the mailbox FIFO command buffer can be sent to SDM in multiple parts. Signed-off-by: Chee Hong Ang Reviewed-by: Ley Foon Tan --- arch/arm/mach-socfpga/mailbox_s10.c | 113 +++++++++++++++++++--------- 1 file changed, 78 insertions(+), 35 deletions(-) diff --git a/arch/arm/mach-socfpga/mailbox_s10.c b/arch/arm/mach-socfpga/mailbox_s10.c index e8a587f007..a9ec818492 100644 --- a/arch/arm/mach-socfpga/mailbox_s10.c +++ b/arch/arm/mach-socfpga/mailbox_s10.c @@ -43,41 +43,93 @@ static __always_inline int mbox_polling_resp(u32 rout) return -ETIMEDOUT; } +static __always_inline int mbox_is_cmdbuf_full(u32 cin) +{ + return (((cin + 1) % MBOX_CMD_BUFFER_SIZE) == MBOX_READL(MBOX_COUT)); +} + +static __always_inline int mbox_is_cmdbuf_empty(u32 cin) +{ + return (((MBOX_READL(MBOX_COUT) + 1) % MBOX_CMD_BUFFER_SIZE) == cin); +} + +static __always_inline int mbox_wait_for_cmdbuf_empty(u32 cin) +{ + int timeout = 2000; + + while (timeout) { + if (mbox_is_cmdbuf_empty(cin)) + return 0; + udelay(1000); + timeout--; + } + + return -ETIMEDOUT; +} + +static __always_inline int mbox_write_cmd_buffer(u32 *cin, u32 data, + int *is_cmdbuf_overflow) +{ + int timeout = 1000; + + while (timeout) { + if (mbox_is_cmdbuf_full(*cin)) { + if (is_cmdbuf_overflow && + *is_cmdbuf_overflow == 0) { + /* Trigger SDM doorbell */ + MBOX_WRITEL(1, MBOX_DOORBELL_TO_SDM); + *is_cmdbuf_overflow = 1; + } + udelay(1000); + } else { + /* write header to circular buffer */ + MBOX_WRITE_CMD_BUF(data, (*cin)++); + *cin %= MBOX_CMD_BUFFER_SIZE; + MBOX_WRITEL(*cin, MBOX_CIN); + break; + } + timeout--; + } + + if (!timeout) + return -ETIMEDOUT; + + /* Wait for the SDM to drain the FIFO command buffer */ + if (is_cmdbuf_overflow && *is_cmdbuf_overflow) + return mbox_wait_for_cmdbuf_empty(*cin); + + return 0; +} + /* Check for available slot and write to circular buffer. * It also update command valid offset (cin) register. */ static __always_inline int mbox_fill_cmd_circular_buff(u32 header, u32 len, u32 *arg) { - u32 cin; - u32 cout; - u32 i; - - cin = MBOX_READL(MBOX_CIN) % MBOX_CMD_BUFFER_SIZE; - cout = MBOX_READL(MBOX_COUT) % MBOX_CMD_BUFFER_SIZE; + int i, ret; + int is_cmdbuf_overflow = 0; + u32 cin = MBOX_READL(MBOX_CIN) % MBOX_CMD_BUFFER_SIZE; - /* if command buffer is full or not enough free space - * to fit the data. Note, len is in u32 unit. - */ - if (((cin + 1) % MBOX_CMD_BUFFER_SIZE) == cout || - ((MBOX_CMD_BUFFER_SIZE - cin + cout - 1) % - MBOX_CMD_BUFFER_SIZE) < (len + 1)) - return -ENOMEM; - - /* write header to circular buffer */ - MBOX_WRITE_CMD_BUF(header, cin++); - /* wrapping around when it reach the buffer size */ - cin %= MBOX_CMD_BUFFER_SIZE; + ret = mbox_write_cmd_buffer(&cin, header, &is_cmdbuf_overflow); + if (ret) + return ret; /* write arguments */ for (i = 0; i < len; i++) { - MBOX_WRITE_CMD_BUF(arg[i], cin++); - /* wrapping around when it reach the buffer size */ - cin %= MBOX_CMD_BUFFER_SIZE; + is_cmdbuf_overflow = 0; + ret = mbox_write_cmd_buffer(&cin, arg[i], &is_cmdbuf_overflow); + if (ret) + return ret; } - /* write command valid offset */ - MBOX_WRITEL(cin, MBOX_CIN); + /* If SDM doorbell is not triggered after the last data is + * written into mailbox FIFO command buffer, trigger the + * SDM doorbell again to ensure SDM able to read the remaining + * data. + */ + if (!is_cmdbuf_overflow) + MBOX_WRITEL(1, MBOX_DOORBELL_TO_SDM); return 0; } @@ -90,10 +142,6 @@ static __always_inline int mbox_prepare_cmd_only(u8 id, u32 cmd, u32 header; int ret; - /* Total length is command + argument length */ - if ((len + 1) > MBOX_CMD_BUFFER_SIZE) - return -EINVAL; - if (cmd > MBOX_MAX_CMD_INDEX) return -EINVAL; @@ -110,11 +158,7 @@ static __always_inline int mbox_send_cmd_only_common(u8 id, u32 cmd, u8 is_indirect, u32 len, u32 *arg) { - int ret = mbox_prepare_cmd_only(id, cmd, is_indirect, len, arg); - /* write doorbell */ - MBOX_WRITEL(1, MBOX_DOORBELL_TO_SDM); - - return ret; + return mbox_prepare_cmd_only(id, cmd, is_indirect, len, arg); } /* Return number of responses received in buffer */ @@ -167,15 +211,14 @@ static __always_inline int mbox_send_cmd_common(u8 id, u32 cmd, u8 is_indirect, status = MBOX_READL(MBOX_STATUS) & MBOX_STATUS_UA_MSK; /* Write urgent command to urgent register */ MBOX_WRITEL(cmd, MBOX_URG); + /* write doorbell */ + MBOX_WRITEL(1, MBOX_DOORBELL_TO_SDM); } else { ret = mbox_prepare_cmd_only(id, cmd, is_indirect, len, arg); if (ret) return ret; } - /* write doorbell */ - MBOX_WRITEL(1, MBOX_DOORBELL_TO_SDM); - while (1) { ret = 1000; -- 2.39.5