[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/kernel/linux/v4.14/drivers/soc/mediatek/mtk-cmdq-helper.c b/src/kernel/linux/v4.14/drivers/soc/mediatek/mtk-cmdq-helper.c
new file mode 100644
index 0000000..46b48ae
--- /dev/null
+++ b/src/kernel/linux/v4.14/drivers/soc/mediatek/mtk-cmdq-helper.c
@@ -0,0 +1,537 @@
+// SPDX-License-Identifier: GPL-2.0
+//
+// Copyright (c) 2018 MediaTek Inc.
+
+#include <linux/completion.h>
+#include <linux/errno.h>
+#include <linux/dma-mapping.h>
+#include <linux/module.h>
+#include <linux/mailbox_controller.h>
+#include <linux/soc/mediatek/mtk-cmdq.h>
+
+#define CMDQ_GET_ARG_B(arg) (((arg) & GENMASK(31, 16)) >> 16)
+#define CMDQ_GET_ARG_C(arg) ((arg) & GENMASK(15, 0))
+/** conbine the argument b and c to a 32bits balue */
+#define CMDQ_GET_32B_VALUE(arg_b, arg_c) ((u32)((arg_b) << 16) | (arg_c))
+/** get the register index prefix from type */
+#define CMDQ_REG_IDX_PREFIX(type) ((type) ? "" : "Reg Index ")
+/** get operand index or value */
+#define CMDQ_OPERAND_GET_IDX_VALUE(operand) ((operand)->reg ? \
+ (operand)->idx : \
+ (operand)->value)
+#define CMDQ_WRITE_ENABLE_MASK BIT(0)
+#define CMDQ_EOC_IRQ_EN BIT(0)
+#define CMDQ_EOC_CMD ((u64)((CMDQ_CODE_EOC << CMDQ_OP_CODE_SHIFT)) \
+ << 32 | CMDQ_EOC_IRQ_EN)
+#define CMDQ_IMMEDIATE_VALUE 0
+#define CMDQ_REG_TYPE 1
+
+struct cmdq_instruction {
+ s16 arg_c:16;
+ s16 arg_b:16;
+ s16 arg_a:16;
+ u8 s_op:5;
+ u8 arg_c_type:1;
+ u8 arg_b_type:1;
+ u8 arg_a_type:1;
+ u8 op:8;
+};
+
+static void cmdq_pkt_instr_encoder(struct cmdq_pkt *pkt, s16 arg_c, s16 arg_b,
+ s16 arg_a, u8 s_op, u8 arg_c_type,
+ u8 arg_b_type, u8 arg_a_type, u8 op)
+{
+ struct cmdq_instruction *cmdq_inst;
+
+ cmdq_inst = pkt->va_base + pkt->cmd_buf_size;
+ cmdq_inst->op = op;
+ cmdq_inst->arg_a_type = arg_a_type;
+ cmdq_inst->arg_b_type = arg_b_type;
+ cmdq_inst->arg_c_type = arg_c_type;
+ cmdq_inst->s_op = s_op;
+ cmdq_inst->arg_a = arg_a;
+ cmdq_inst->arg_b = arg_b;
+ cmdq_inst->arg_c = arg_c;
+ pkt->cmd_buf_size += CMDQ_INST_SIZE;
+}
+
+static void cmdq_client_timeout(struct timer_list *t)
+{
+ struct cmdq_client *client = from_timer(client, t, timer);
+
+ dev_err(client->client.dev, "cmdq timeout!\n");
+}
+
+struct cmdq_client *cmdq_mbox_create(struct device *dev, int index, u32 timeout)
+{
+ struct cmdq_client *client;
+
+ client = kzalloc(sizeof(*client), GFP_KERNEL);
+ if (!client)
+ return (struct cmdq_client *)-ENOMEM;
+
+ client->timeout_ms = timeout;
+ if (timeout != CMDQ_NO_TIMEOUT) {
+ spin_lock_init(&client->lock);
+ timer_setup(&client->timer, cmdq_client_timeout, 0);
+ }
+ client->pkt_cnt = 0;
+ client->client.dev = dev;
+ client->client.tx_block = false;
+ client->chan = mbox_request_channel(&client->client, index);
+
+ if (IS_ERR(client->chan)) {
+ long err;
+
+ dev_err(dev, "failed to request channel\n");
+ err = PTR_ERR(client->chan);
+ kfree(client);
+
+ return ERR_PTR(err);
+ }
+
+ return client;
+}
+EXPORT_SYMBOL(cmdq_mbox_create);
+
+void cmdq_mbox_destroy(struct cmdq_client *client)
+{
+ if (client->timeout_ms != CMDQ_NO_TIMEOUT) {
+ spin_lock(&client->lock);
+ del_timer_sync(&client->timer);
+ spin_unlock(&client->lock);
+ }
+ mbox_free_channel(client->chan);
+ kfree(client);
+}
+EXPORT_SYMBOL(cmdq_mbox_destroy);
+
+struct cmdq_pkt *cmdq_pkt_create(struct cmdq_client *client, size_t size)
+{
+ struct cmdq_pkt *pkt;
+ struct device *dev;
+ dma_addr_t dma_addr;
+
+ pkt = kzalloc(sizeof(*pkt), GFP_KERNEL);
+ if (!pkt)
+ return ERR_PTR(-ENOMEM);
+ pkt->va_base = kzalloc(size, GFP_KERNEL);
+ if (!pkt->va_base) {
+ kfree(pkt);
+ return ERR_PTR(-ENOMEM);
+ }
+ pkt->buf_size = size;
+ pkt->cl = (void *)client;
+
+ dev = client->chan->mbox->dev;
+ dma_addr = dma_map_single(dev, pkt->va_base, pkt->buf_size,
+ DMA_TO_DEVICE);
+ if (dma_mapping_error(dev, dma_addr)) {
+ dev_err(dev, "dma map failed, size=%u\n", (u32)(u64)size);
+ kfree(pkt->va_base);
+ kfree(pkt);
+ return ERR_PTR(-ENOMEM);
+ }
+
+ pkt->pa_base = dma_addr;
+
+ return pkt;
+}
+EXPORT_SYMBOL(cmdq_pkt_create);
+
+void cmdq_pkt_destroy(struct cmdq_pkt *pkt)
+{
+ struct cmdq_client *client = (struct cmdq_client *)pkt->cl;
+
+ dma_unmap_single(client->chan->mbox->dev, pkt->pa_base, pkt->buf_size,
+ DMA_TO_DEVICE);
+ kfree(pkt->va_base);
+ kfree(pkt);
+}
+EXPORT_SYMBOL(cmdq_pkt_destroy);
+
+static int cmdq_pkt_append_command(struct cmdq_pkt *pkt, s16 arg_c, s16 arg_b,
+ s16 arg_a, u8 s_op, u8 arg_c_type,
+ u8 arg_b_type, u8 arg_a_type,
+ enum cmdq_code code)
+{
+
+ if (unlikely(pkt->cmd_buf_size + CMDQ_INST_SIZE > pkt->buf_size)) {
+ /*
+ * In the case of allocated buffer size (pkt->buf_size) is used
+ * up, the real required size (pkt->cmdq_buf_size) is still
+ * increased, so that the user knows how much memory should be
+ * ultimately allocated after appending all commands and
+ * flushing the command packet. Therefor, the user can call
+ * cmdq_pkt_create() again with the real required buffer size.
+ */
+ pkt->cmd_buf_size += CMDQ_INST_SIZE;
+ WARN_ONCE(1, "%s: buffer size %u is too small !\n",
+ __func__, (u32)pkt->buf_size);
+ return -ENOMEM;
+ }
+ cmdq_pkt_instr_encoder(pkt, arg_c, arg_b, arg_a, s_op, arg_c_type,
+ arg_b_type, arg_a_type, code);
+
+ return 0;
+}
+
+int cmdq_pkt_write(struct cmdq_pkt *pkt, u8 subsys, u16 offset, u32 value)
+{
+ return cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(value),
+ CMDQ_GET_ARG_B(value), offset, subsys,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE, CMDQ_CODE_WRITE);
+}
+EXPORT_SYMBOL(cmdq_pkt_write);
+
+int cmdq_pkt_write_mask(struct cmdq_pkt *pkt, u8 subsys, u16 offset,
+ u32 value, u32 mask)
+{
+ u32 offset_mask = offset;
+ int err = 0;
+
+ if (mask != 0xffffffff) {
+ err = cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(~mask),
+ CMDQ_GET_ARG_B(~mask),
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_CODE_MASK);
+ offset_mask |= CMDQ_WRITE_ENABLE_MASK;
+ }
+ err |= cmdq_pkt_write(pkt, subsys, offset_mask, value);
+
+ return err;
+}
+EXPORT_SYMBOL(cmdq_pkt_write_mask);
+
+int cmdq_pkt_load(struct cmdq_pkt *pkt, u16 dst_reg_idx,
+ u16 indirect_src_reg_idx)
+{
+ return cmdq_pkt_append_command(pkt, 0, indirect_src_reg_idx,
+ dst_reg_idx, 0, CMDQ_IMMEDIATE_VALUE,
+ CMDQ_REG_TYPE, CMDQ_REG_TYPE,
+ CMDQ_CODE_READ_S);
+}
+EXPORT_SYMBOL(cmdq_pkt_load);
+
+int cmdq_pkt_store_reg(struct cmdq_pkt *pkt, u16 indirect_dst_reg_idx,
+ u16 src_reg_idx, u32 mask)
+{
+ int err = 0;
+ enum cmdq_code op = CMDQ_CODE_WRITE_S;
+
+ if (mask != 0xffffffff) {
+ err = cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(~mask),
+ CMDQ_GET_ARG_B(~mask), 0, 0, 0, 0,
+ 0, CMDQ_CODE_MASK);
+ if (err != 0)
+ return err;
+
+ op = CMDQ_CODE_WRITE_S_W_MASK;
+ }
+
+ return cmdq_pkt_append_command(pkt, 0, src_reg_idx,
+ indirect_dst_reg_idx, 0,
+ CMDQ_IMMEDIATE_VALUE, CMDQ_REG_TYPE,
+ CMDQ_REG_TYPE, op);
+}
+EXPORT_SYMBOL(cmdq_pkt_store_reg);
+
+int cmdq_pkt_store_value(struct cmdq_pkt *pkt, u16 indirect_dst_reg_idx,
+ u32 value, u32 mask)
+{
+ int err = 0;
+ enum cmdq_code op = CMDQ_CODE_WRITE_S;
+
+ if (mask != 0xffffffff) {
+ err = cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(~mask),
+ CMDQ_GET_ARG_B(~mask), 0, 0, 0, 0,
+ 0, CMDQ_CODE_MASK);
+ if (err != 0)
+ return err;
+
+ op = CMDQ_CODE_WRITE_S_W_MASK;
+ }
+
+ return cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(value),
+ CMDQ_GET_ARG_B(value),
+ indirect_dst_reg_idx, 0,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE, CMDQ_REG_TYPE, op);
+}
+EXPORT_SYMBOL(cmdq_pkt_store_value);
+
+int cmdq_pkt_assign_command(struct cmdq_pkt *pkt, u16 reg_idx, s32 value)
+{
+ return cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(value),
+ CMDQ_GET_ARG_B(value), reg_idx,
+ CMDQ_LOGIC_ASSIGN, CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE, CMDQ_REG_TYPE,
+ CMDQ_CODE_LOGIC);
+}
+EXPORT_SYMBOL(cmdq_pkt_assign_command);
+
+int cmdq_pkt_logic_command(struct cmdq_pkt *pkt, enum CMDQ_LOGIC_ENUM s_op,
+ u16 result_reg_idx,
+ struct cmdq_operand *left_operand,
+ struct cmdq_operand *right_operand)
+{
+ u32 left_idx_value;
+ u32 right_idx_value;
+
+ if (!left_operand || !right_operand)
+ return -EINVAL;
+
+ left_idx_value = CMDQ_OPERAND_GET_IDX_VALUE(left_operand);
+ right_idx_value = CMDQ_OPERAND_GET_IDX_VALUE(right_operand);
+
+ return cmdq_pkt_append_command(pkt, right_idx_value, left_idx_value,
+ result_reg_idx, s_op, right_operand->reg,
+ left_operand->reg, CMDQ_REG_TYPE,
+ CMDQ_CODE_LOGIC);
+}
+EXPORT_SYMBOL(cmdq_pkt_logic_command);
+
+int cmdq_pkt_jump(struct cmdq_pkt *pkt, s32 addr_offset)
+{
+ return cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(addr_offset),
+ CMDQ_GET_ARG_B(addr_offset), 0, 0, 0, 0,
+ 0, CMDQ_CODE_JUMP);
+}
+EXPORT_SYMBOL(cmdq_pkt_jump);
+
+int cmdq_pkt_cond_jump(struct cmdq_pkt *pkt,
+ u16 offset_reg_idx,
+ struct cmdq_operand *left_operand,
+ struct cmdq_operand *right_operand,
+ enum CMDQ_CONDITION_ENUM condition_operator)
+{
+ u32 left_idx_value;
+ u32 right_idx_value;
+
+ if (!left_operand || !right_operand)
+ return -EINVAL;
+
+ left_idx_value = CMDQ_OPERAND_GET_IDX_VALUE(left_operand);
+ right_idx_value = CMDQ_OPERAND_GET_IDX_VALUE(right_operand);
+
+ return cmdq_pkt_append_command(pkt, right_idx_value, left_idx_value,
+ offset_reg_idx, condition_operator,
+ right_operand->reg, left_operand->reg,
+ CMDQ_REG_TYPE,
+ CMDQ_CODE_JUMP_C_RELATIVE);
+}
+EXPORT_SYMBOL(cmdq_pkt_cond_jump);
+
+struct cmdq_operand *cmdq_operand_immediate(struct cmdq_operand *operand,
+ u16 value)
+{
+ if (!operand)
+ return (struct cmdq_operand *)ERR_PTR(-EINVAL);
+
+ operand->reg = false;
+ operand->value = value;
+
+ return operand;
+}
+EXPORT_SYMBOL(cmdq_operand_immediate);
+
+struct cmdq_operand *cmdq_operand_reg(struct cmdq_operand *operand, u16 idx)
+{
+ if (!operand)
+ return (struct cmdq_operand *)ERR_PTR(-EINVAL);
+
+ operand->reg = true;
+ operand->idx = idx;
+
+ return operand;
+}
+EXPORT_SYMBOL(cmdq_operand_reg);
+
+int cmdq_pkt_wfe(struct cmdq_pkt *pkt, u16 event)
+{
+ if (event >= CMDQ_MAX_EVENT)
+ return -EINVAL;
+
+ return cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(CMDQ_WFE_OPTION),
+ CMDQ_GET_ARG_B(CMDQ_WFE_OPTION), event,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_CODE_WFE);
+}
+EXPORT_SYMBOL(cmdq_pkt_wfe);
+
+int cmdq_pkt_clear_event(struct cmdq_pkt *pkt, u16 event)
+{
+ if (event >= CMDQ_MAX_EVENT)
+ return -EINVAL;
+
+ return cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(CMDQ_WFE_UPDATE),
+ CMDQ_GET_ARG_B(CMDQ_WFE_UPDATE), event,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_CODE_WFE);
+}
+EXPORT_SYMBOL(cmdq_pkt_clear_event);
+
+int cmdq_pkt_poll(struct cmdq_pkt *pkt, u8 subsys,
+ u16 offset, u32 value, u32 mask)
+{
+ int err;
+
+ if (mask != 0xffffffff) {
+ err = cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(~mask),
+ CMDQ_GET_ARG_B(~mask),
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_CODE_MASK);
+
+ if (err != 0)
+ return err;
+ }
+ offset = offset | 0x1;
+
+ return cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(value),
+ CMDQ_GET_ARG_B(value),
+ offset, subsys,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_CODE_POLL);
+}
+EXPORT_SYMBOL(cmdq_pkt_poll);
+
+static int cmdq_pkt_finalize(struct cmdq_pkt *pkt)
+{
+ int err;
+
+ /* insert EOC and generate IRQ for each command iteration */
+ err = cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(CMDQ_EOC_IRQ_EN),
+ CMDQ_GET_ARG_B(CMDQ_EOC_IRQ_EN),
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_CODE_EOC);
+ if (err < 0)
+ return err;
+ /* JUMP to end */
+ err = cmdq_pkt_append_command(pkt, CMDQ_GET_ARG_C(CMDQ_JUMP_PASS),
+ CMDQ_GET_ARG_B(CMDQ_JUMP_PASS),
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_IMMEDIATE_VALUE,
+ CMDQ_CODE_JUMP);
+
+ return err;
+}
+
+static void cmdq_pkt_flush_async_cb(struct cmdq_cb_data data)
+{
+ struct cmdq_pkt *pkt = (struct cmdq_pkt *)data.data;
+ struct cmdq_task_cb *cb = &pkt->cb;
+ struct cmdq_client *client = (struct cmdq_client *)pkt->cl;
+
+ if (client->timeout_ms != CMDQ_NO_TIMEOUT) {
+ unsigned long flags = 0;
+
+ spin_lock_irqsave(&client->lock, flags);
+ if (--client->pkt_cnt == 0)
+ del_timer(&client->timer);
+ else
+ mod_timer(&client->timer, jiffies +
+ msecs_to_jiffies(client->timeout_ms));
+ spin_unlock_irqrestore(&client->lock, flags);
+ }
+
+ dma_sync_single_for_cpu(client->chan->mbox->dev, pkt->pa_base,
+ pkt->cmd_buf_size, DMA_TO_DEVICE);
+ if (cb->cb) {
+ data.data = cb->data;
+ cb->cb(data);
+ }
+}
+
+int cmdq_pkt_flush_async(struct cmdq_pkt *pkt, cmdq_async_flush_cb cb,
+ void *data)
+{
+ int err;
+ unsigned long flags = 0;
+ struct cmdq_client *client = (struct cmdq_client *)pkt->cl;
+
+ err = cmdq_pkt_finalize(pkt);
+ if (err < 0)
+ return err;
+
+ pkt->cb.cb = cb;
+ pkt->cb.data = data;
+ pkt->async_cb.cb = cmdq_pkt_flush_async_cb;
+ pkt->async_cb.data = pkt;
+
+ dma_sync_single_for_device(client->chan->mbox->dev, pkt->pa_base,
+ pkt->cmd_buf_size, DMA_TO_DEVICE);
+
+ if (client->timeout_ms != CMDQ_NO_TIMEOUT) {
+ spin_lock_irqsave(&client->lock, flags);
+ if (client->pkt_cnt++ == 0)
+ mod_timer(&client->timer, jiffies +
+ msecs_to_jiffies(client->timeout_ms));
+ spin_unlock_irqrestore(&client->lock, flags);
+ }
+
+ mbox_send_message(client->chan, pkt);
+ /* We can send next packet immediately, so just call txdone. */
+ mbox_client_txdone(client->chan, 0);
+
+ return 0;
+}
+EXPORT_SYMBOL(cmdq_pkt_flush_async);
+
+struct cmdq_flush_completion {
+ struct completion cmplt;
+ bool err;
+};
+
+static void cmdq_pkt_flush_cb(struct cmdq_cb_data data)
+{
+ struct cmdq_flush_completion *cmplt;
+
+ cmplt = (struct cmdq_flush_completion *)data.data;
+ if (data.sta != CMDQ_CB_NORMAL)
+ cmplt->err = true;
+ else
+ cmplt->err = false;
+ complete(&cmplt->cmplt);
+}
+
+int cmdq_pkt_flush(struct cmdq_pkt *pkt)
+{
+ struct cmdq_flush_completion cmplt;
+ int err;
+
+ init_completion(&cmplt.cmplt);
+ err = cmdq_pkt_flush_async(pkt, cmdq_pkt_flush_cb, &cmplt);
+ if (err < 0)
+ return err;
+ wait_for_completion(&cmplt.cmplt);
+
+ return cmplt.err ? -EFAULT : 0;
+}
+EXPORT_SYMBOL(cmdq_pkt_flush);
+
+MODULE_LICENSE("GPL v2");