// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (c) 2019 MediaTek Inc.
*/

#include <linux/clk.h>
#include <linux/delay.h>
#include <linux/debugfs.h>
#include <linux/dma-mapping.h>
#include <linux/mailbox_controller.h>
#include <linux/module.h>
#include <linux/of_address.h>
#include <linux/of_platform.h>
#include <linux/platform_device.h>
#include <linux/sched/clock.h>
#include <linux/timer.h>
#include <linux/uaccess.h>

#include "cmdq-util.h"
#include "cmdq-sec.h"
#include "cmdq-sec-mailbox.h"

#define RIU_PM_OFFSET			(0x1c000000)
#define CMDQ_THR_SPR0(base, id)		((base) + (0x80 * (id)) + 0x160)
#define CMDQ_THR_SPR1(base, id)		((base) + (0x80 * (id)) + 0x164)
#define CMDQ_THR_SPR2(base, id)		((base) + (0x80 * (id)) + 0x168)
#define CMDQ_THR_SPR3(base, id)		((base) + (0x80 * (id)) + 0x16c)
#define CMDQ_GPR_R32(base, id)		((base) + (0x4 * (id)) + 0x80)
#define CMDQ_SYNC_TOKEN_UPD(base)	((base) + 0x68)
#define CMDQ_TPR_MASK(base)		((base) + 0xd0)
#define CMDQ_TPR_TIMEOUT_EN(base)	((base) + 0xdc)

#define	CMDQ_TEST_CNT			8

#define CMDQ_GPR_DEBUG_TIMER		CMDQ_GPR_R14
#define CMDQ_GPR_DEBUG_DUMMY		CMDQ_GPR_R15
#define CMDQ_HIGH_MASK			(0xFFFF0000)
#define CMDQ_STR_LDR_PA			(0x40000000) //FIXME:DRAM 0 for test STR POC

#define GCE_BUS_GCTL	0x40
#define GCE_PC_HIGH		0xd4

#define CMDQ_ALWAYS_ON_SUPPORT  (1)
#define CMDQ_EVENT_ML_TRIG_DISP (23)

#define DS_MAX_INDEX (8)
#define IP_DS_DEPTH  (4)
#define OPF_DS_DEPTH (4)
#define OPB_DS_DEPTH (4)
#define OP2_DS_DEPTH (4)

#define CMDQ_EVENT_DS_TRIG_IP (4)
#define CMDQ_EVENT_DS_TRIG_OPF (5)
#define CMDQ_EVENT_DS_TRIG_OPB (6)
#define CMDQ_EVENT_DS_TRIG_OP2 (7)

#define CMDQ_SAFE_IOUNMAP(x) \
do { \
	if (x) \
		iounmap(x); \
} while (0)

#if CMDQ_ALWAYS_ON_SUPPORT
#define CMDQ_CLK_PREPARE_ENABLE(clk)
#define CMDQ_CLK_DISABLE_UNPREPARE(clk)
#else
#define CMDQ_CLK_PREPARE_ENABLE(clk) \
do { \
	if (clk_prepare_enable(clk)) { \
		cmdq_err("clk fail"); \
	} \
} while (0)
#define CMDQ_CLK_DISABLE_UNPREPARE(clk) clk_disable_unprepare(clk)

#endif

enum {
	CMDQ_TEST_SUBSYS_GCE,
	CMDQ_TEST_SUBSYS_MMSYS,
	CMDQ_TEST_SUBSYS_NR,
	CMDQ_TEST_SUBSYS_ERR = 99
};

enum CMDQ_SECURE_STATE_ENUM {
	CMDQ_MTEE_STATE = -1,
	CMDQ_NORMAL_STATE = 0,
	CMDQ_TEE_STATE = 1,
};

enum CMDQ_POLL_TYPE {
	CMDQ_POLL_TIMEOUT,
	CMDQ_POLL,
	CMDQ_POLL_SLEEP,
};

enum DS_DOMAIN {
	DS_DOMAIN_IP,
	DS_DOMAIN_OPF,
	DS_DOMAIN_OPB,
	DS_DOMAIN_OP2,
	DS_DOMAIN_MAX,
};

struct test_node {
	struct device	*dev;
	void __iomem	*va;
	phys_addr_t	pa;
	struct clk	*clk;
	struct clk	*clk_timer;
};

struct cmdq_test {
	struct device		*dev;
	struct mutex		lock;
	struct test_node	gce;
	struct test_node	mmsys;
	struct cmdq_client	*clt;
	struct cmdq_client	*clt2;
	struct cmdq_client	*loop;
	struct cmdq_client	*sec;
	struct cmdq_client	*cltDsIP;
	struct cmdq_client	*cltDsOPF;
	struct cmdq_client	*cltDsOPB;
	struct cmdq_client	*cltDsOP2;
	u32			iter;
	u32			subsys[CMDQ_TEST_SUBSYS_NR];
	struct dentry		*fs;

	bool			tick;
	struct timer_list	timer;

	u16			token_user0;
	u16			token_gpr_set4;
};

static struct cmdq_test		*gtest;

static void cmdq_test_mbox_cb(struct cmdq_cb_data data)
{
	struct cmdq_flush_completion	*cmplt = data.data;

	if (data.err < 0)
		cmdq_err("pkt:%p err:%d", cmplt->pkt, data.err);
	cmplt->err = !data.err ? false : true;
	complete(&cmplt->cmplt);
}

static void cmdq_test_mbox_cb_destroy(struct cmdq_cb_data data)
{
	struct cmdq_pkt *pkt = (struct cmdq_pkt *)data.data;

	if (data.err < 0)
		cmdq_err("pkt:%p err:%d", pkt, data.err);
	cmdq_dump_pkt(pkt, 0, true);
	cmdq_pkt_destroy(pkt);
	cmdq_msg("%s: pkt:%#lx", __func__, (unsigned long)pkt);
}

static void cmdq_test_mbox_cb_dump(struct cmdq_cb_data data)
{
	cmdq_msg("pkt:0x%p err:%d done", data.data, data.err);
}

static void cmdq_test_mbox_cb_dump_err(struct cmdq_cb_data data)
{
	cmdq_err("pkt:0x%p err:%d during err", data.data, data.err);
}

static int cmdq_test_no_aee(struct cmdq_cb_data data)
{
	struct cmdq_pkt *pkt = (struct cmdq_pkt *)data.data;

	cmdq_err("pkt:0x%p err:%d during err", pkt, data.err);
	return CMDQ_NO_AEE;
}

static void cmdq_test_mbox_err_dump(struct cmdq_test *test, const bool sec, const bool aee)
{
	struct cmdq_pkt *pkt;
	struct cmdq_flush_completion cmplt;
	s32 ret;
	u64 *inst;
	dma_addr_t pc;
	struct cmdq_client *clt = sec ? test->sec : test->clt;
	size_t wfe_offset;

	cmdq_msg("%s sec[%d] aee[%d]", __func__, sec, aee);

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);

	cmdq_clear_event(clt->chan, test->token_user0);
	pkt = cmdq_pkt_create(clt);

	if (!aee)
		pkt->aee_cb = cmdq_test_no_aee;
#ifdef CMDQ_SECURE_SUPPORT
	if (sec)
		cmdq_sec_pkt_set_data(pkt, 0, 0, CMDQ_SEC_DEBUG,
			CMDQ_METAEX_NONE);
#endif

	cmdq_pkt_wfe(pkt, test->token_user0);
	wfe_offset = pkt->cmd_buf_size - CMDQ_INST_SIZE;

	init_completion(&cmplt.cmplt);
	cmplt.pkt = pkt;
	cmdq_pkt_flush_async(pkt, cmdq_test_mbox_cb, &cmplt);

	cmdq_thread_dump(clt->chan, pkt, &inst, &pc);
	cmdq_set_event(clt->chan, test->token_user0);

	ret = cmdq_pkt_wait_complete(pkt);
	cmdq_msg("wait complete pkt:0x%p ret:%d", pkt, ret);

	ret = wait_for_completion_timeout(
		&cmplt.cmplt, msecs_to_jiffies(CMDQ_TIMEOUT_DEFAULT));
	if (!ret)
		cmdq_err(
			"wait_for_completion_timeout pkt:0x%p ret:%d inst:0x%016llx pc:%pa",
			pkt, ret, inst ? *inst : 0, &pc);
	else
		cmdq_msg("%s round 1 done", __func__);

	/* second round, use flush async ex with wait, pre-dump and timeout */
	pkt->err_cb.cb = cmdq_test_mbox_cb_dump_err;
	pkt->err_cb.data = pkt;
	cmdq_clear_event(clt->chan, test->token_user0);
	ret = cmdq_pkt_flush_async(pkt, cmdq_test_mbox_cb_dump, (void *)pkt);
	cmdq_msg("flush pkt:0x%p ret:%d", pkt, ret);
	ret = cmdq_pkt_wait_complete(pkt);
	if (wfe_offset == pkt->err_data.offset)
		cmdq_msg("pkt:0x%p right wfe_offset:%zu = err_offset:%zu",
			pkt, wfe_offset, pkt->err_data.offset);
	else
		cmdq_err("pkt:0x%p wrong wfe_offset:%zu, err_offset:%zu",
			pkt, wfe_offset, pkt->err_data.offset);
	cmdq_msg("wait complete pkt:0x%p ret:%d", pkt, ret);

	cmdq_pkt_destroy(pkt);

	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);
}

static void cmdq_test_mbox_gpr_sleep(struct cmdq_test *test, const bool sleep)
{
	struct cmdq_pkt		*pkt;
	struct cmdq_pkt_buffer	*buf;
	struct cmdq_operand	l_op = {.reg = true, .idx = CMDQ_TPR_ID};
	struct cmdq_operand	r_op = {.reg = false, .idx = 100};
	dma_addr_t		out_pa;
	u32			*out_va, gce_time;
	u64			cpu_time;
	const u16		event =
		(u16)CMDQ_EVENT_GPR_TIMER + CMDQ_GPR_DEBUG_TIMER;

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);
	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk_timer);

	writel(0x80000000, (void *)CMDQ_TPR_MASK(test->gce.va));
	writel(1 << CMDQ_GPR_DEBUG_TIMER,
		(void *)CMDQ_TPR_TIMEOUT_EN(test->gce.va));

	pkt = cmdq_pkt_create(test->clt);
	if (!sleep) {
		cmdq_pkt_write(pkt, NULL, CMDQ_TPR_TIMEOUT_EN(test->gce.pa),
			1 << CMDQ_GPR_DEBUG_TIMER, 1 << CMDQ_GPR_DEBUG_TIMER);
		cmdq_pkt_clear_event(pkt, event);
	} else
		cmdq_pkt_wfe(pkt, test->token_gpr_set4);

	buf = list_last_entry(&pkt->buf, typeof(*buf), list_entry);
	out_pa = CMDQ_BUF_ADDR(buf) + 3096;
	out_va = (u32 *)(buf->va_base + 3096);
	*out_va = 0;
	*(out_va + 1) = 0;
	*(out_va + 2) = 0;

	if (!sleep) {
		cmdq_pkt_write_indriect(pkt, NULL, out_pa, CMDQ_TPR_ID, ~0);
		cmdq_pkt_logic_command(pkt, CMDQ_LOGIC_ADD,
			CMDQ_GPR_CNT_ID + CMDQ_GPR_DEBUG_TIMER, &l_op, &r_op);
		cmdq_pkt_wfe(pkt, event);
		cmdq_pkt_write_indriect(pkt, NULL, out_pa + 4, CMDQ_TPR_ID, ~0);
	} else {
		cmdq_pkt_write_indriect(pkt, NULL, out_pa, CMDQ_TPR_ID, ~0);
		cmdq_pkt_sleep(pkt, 100, CMDQ_GPR_DEBUG_TIMER);
		cmdq_pkt_write_indriect(pkt, NULL, out_pa + 4, CMDQ_TPR_ID, ~0);
		cmdq_pkt_set_event(pkt, test->token_gpr_set4);
		cmdq_pkt_write_indriect(pkt, NULL, out_pa + 8,
			CMDQ_GPR_CNT_ID + CMDQ_GPR_DEBUG_TIMER, ~0);
	}
	cmdq_pkt_dump_buf(pkt, 0);
	cpu_time = sched_clock();
	cmdq_pkt_flush(pkt);

	cpu_time = div_u64(sched_clock() - cpu_time, 1000000);

	if (*out_va <= *(out_va + 1))
		gce_time = *(out_va + 1) - *out_va;
	else
		gce_time = 0xffffffff - *out_va + *(out_va + 1);

	if (cpu_time > 100 || gce_time > 300)
		cmdq_err("sleep:%d cpu:%llu gce:%u out:%u %u %u",
			sleep, cpu_time, gce_time, *out_va, *(out_va + 1),
			*(out_va + 2));
	else
		cmdq_msg("sleep:%d cpu:%llu gce:%u out:%u %u %u",
			sleep, cpu_time, gce_time, *out_va, *(out_va + 1),
			*(out_va + 2));
	cmdq_pkt_destroy(pkt);
	writel(0, (void *)CMDQ_TPR_MASK(test->gce.va));

	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk_timer);
	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);

}

static void cmdq_test_mbox_cpr(struct cmdq_test *test)
{
	unsigned long va = (unsigned long)CMDQ_GPR_R32(test->gce.va,
		CMDQ_GPR_DEBUG_DUMMY);
	unsigned long pa = CMDQ_GPR_R32(test->gce.pa,
		CMDQ_GPR_DEBUG_DUMMY);
	u32 pttn = 0xdeaddead, *buf_va, mark_assign, mark_write;
	s32 i, ret;
	struct cmdq_pkt *pkt;
	struct cmdq_pkt_buffer *buf;

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);

	pkt = cmdq_pkt_create(test->clt);
	mark_assign = pkt->cmd_buf_size >> 2;
	cmdq_pkt_assign_command(pkt, CMDQ_THR_SPR_IDX3, pttn);
	mark_write = (pkt->cmd_buf_size + CMDQ_INST_SIZE) >> 2;
	cmdq_pkt_write_indriect(pkt, NULL, pa, CMDQ_THR_SPR_IDX3, ~0);
	cmdq_pkt_finalize(pkt);
	cmdq_pkt_dump_buf(pkt, 0);

	buf = list_first_entry(&pkt->buf, typeof(*buf), list_entry);
	buf_va = (u32 *)buf->va_base;

	/* (1024 - 256 - 4 * 24 - 16) * 2 = 1312 */
	for (i = 0; i < 1312; i++) { // CPR_CNT
		writel(0, (void *)va);
		buf_va[mark_assign + 1] =
			(buf_va[1] & 0xffff0000) | (CMDQ_CPR_STRAT_ID + i);
		buf_va[mark_write] =
			((CMDQ_CPR_STRAT_ID + i) << 16) | (buf_va[5] & 0xffff);

		ret = cmdq_pkt_flush(pkt);
		if (ret)
			cmdq_err("cmdq_pkt_flush failed:%d i:%d", ret, i);

		ret = readl((void *)va);
		if (ret != pttn)
			cmdq_err("ret:%#x not equal to pttn:%#x for idx:%d",
				ret, pttn, i + CMDQ_CPR_STRAT_ID);
		else
			cmdq_msg("ret:%#x equals to pttn:%#x for idx:%d",
				ret, pttn, i + CMDQ_CPR_STRAT_ID);
	}
	cmdq_pkt_destroy(pkt);
	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);

}

u32 *cmdq_test_mbox_polling_timeout_unit(struct cmdq_pkt *pkt,
	const unsigned long pa, const u32 pttn, const u32 mask, const bool aee)
{
	struct cmdq_pkt_buffer	*buf;
	u32			*out_va;
	dma_addr_t		out_pa;

	buf = list_last_entry(&pkt->buf, typeof(*buf), list_entry);
	// last 1k as output buffer
	out_pa = CMDQ_BUF_ADDR(buf) + 3096 + gce_mminfra;
	out_va = (u32 *)(buf->va_base + 3096);
	*out_va = 0;
	*(out_va + 1) = 0;

	cmdq_pkt_write_indriect(pkt, NULL, out_pa, CMDQ_CPR_STRAT_ID, ~0);
	cmdq_pkt_poll_timeout(pkt, pttn & mask,
		SUBSYS_NO_SUPPORT, pa, mask, aee ? U16_MAX : 100,
		CMDQ_GPR_DEBUG_TIMER);
	cmdq_pkt_write_indriect(pkt, NULL, out_pa + 4, CMDQ_CPR_STRAT_ID, ~0);
	return out_va;
}

void cmdq_test_mbox_polling(
	struct cmdq_test *test, const bool secure, enum CMDQ_POLL_TYPE timeout,
	const bool aee)
{
	unsigned long	va = (unsigned long)(secure ?
		CMDQ_THR_SPR3(test->gce.va, 3) :
		CMDQ_GPR_R32(test->gce.va, CMDQ_GPR_DEBUG_DUMMY));
	unsigned long	pa = secure ? CMDQ_THR_SPR3(test->gce.pa, 3) :
		CMDQ_GPR_R32(test->gce.pa, CMDQ_GPR_DEBUG_DUMMY);
	const u32	pttn[CMDQ_TEST_CNT] = {
		0xdada1818, 0xdada1818, 0xdada1818, 0x00001818};
	const u32	mask[CMDQ_TEST_CNT] = {
		0xff00ff00, 0xffffffff, 0x0000ff00, 0xffffffff};

	struct cmdq_client		*clt = secure ? test->sec : test->clt;
	struct cmdq_pkt			*pkt[CMDQ_TEST_CNT];
	u64				cpu_time;
	u32				i, val = 0, *out_va, gce_time;

	cmdq_msg("%s: secure:%d timeout:%d va:%#lx pa:%#lx",
		__func__, secure, timeout, va, pa);

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);
	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk_timer);

	writel(0, (void *)va);

	for (i = 0; i < CMDQ_TEST_CNT && pttn[i]; i++) {
		if (timeout > CMDQ_POLL_TIMEOUT)
			writel(0x80000000, (void *)CMDQ_TPR_MASK(test->gce.va));

		pkt[i] = cmdq_pkt_create(clt);
#ifdef CMDQ_SECURE_SUPPORT
		if (secure)
			cmdq_sec_pkt_set_data(pkt[i], 0, 0, CMDQ_SEC_DEBUG,
				CMDQ_METAEX_NONE);
#endif

		cmdq_pkt_wfe(pkt[i], test->token_gpr_set4);
		switch (timeout) {
		case CMDQ_POLL_TIMEOUT:
			out_va = cmdq_test_mbox_polling_timeout_unit(
				pkt[i], pa, pttn[i], mask[i], aee);
			break;
		case CMDQ_POLL:
			cmdq_pkt_poll(pkt[i], NULL, pttn[i] & mask[i], pa,
				mask[i], CMDQ_GPR_DEBUG_TIMER);
			break;
		case CMDQ_POLL_SLEEP:
			cmdq_pkt_poll_sleep(pkt[i], pttn[i] & mask[i], pa, mask[i]);
			break;
		default:
			cmdq_err("append poll instr fail, timeout:%d", timeout);
			break;
		}

		cmdq_pkt_set_event(pkt[i], test->token_gpr_set4);

		cpu_time = sched_clock();
		cmdq_pkt_flush_async(pkt[i], NULL, NULL);

		if (timeout != CMDQ_POLL_TIMEOUT) {
			writel(pttn[i] & mask[i], (void *)va);
			val = readl((void *)va);
		}
		cmdq_pkt_wait_complete(pkt[i]);
		cpu_time = div_u64(sched_clock() - cpu_time, 1000000);

		if (timeout != CMDQ_POLL_TIMEOUT)
			gce_time = 0;
		else if (*out_va <= *(out_va + 1))
			gce_time = *(out_va + 1) - *out_va;
		else
			gce_time = 0xffffffff - *out_va + *(out_va + 1);

		cmdq_msg("%d: pkt:%p pttn:%#x mask:%#x val:%#x cpu:%llu gce:%u",
			i, pkt[i], pttn[i], mask[i], val, cpu_time, gce_time);
		cmdq_pkt_dump_buf(pkt[i], 0);
		cmdq_pkt_destroy(pkt[i]);

		if (timeout > CMDQ_POLL_TIMEOUT)
			writel(0, (void *)CMDQ_TPR_MASK(test->gce.va));
	}
	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk_timer);
	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);

}

static void cmdq_test_mbox_large_cmd(struct cmdq_test *test, const u32 count)
{
	unsigned long	va = (unsigned long)(CMDQ_GPR_R32(
		test->gce.va, CMDQ_GPR_DEBUG_DUMMY));
	unsigned long	pa = CMDQ_GPR_R32(
		test->gce.pa, CMDQ_GPR_DEBUG_DUMMY);

	struct cmdq_pkt		*pkt, *pkt2;
	s32			i, val;

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);

	writel(0xdeaddead, (void *)va);

	pkt = cmdq_pkt_create(test->clt);
	for (i = 0; i < count; i++)
		cmdq_pkt_write(pkt, NULL, pa, i, ~0);

	pkt2 = cmdq_pkt_create(test->loop);
	cmdq_pkt_copy(pkt2, pkt);
	cmdq_pkt_write(pkt2, NULL, pa, i, ~0);

	cmdq_msg("%s: pkt:%p before flush", __func__, pkt);
	cmdq_dump_pkt(pkt, 0, true);
	cmdq_pkt_flush(pkt);
	cmdq_msg("%s: pkt:%p after flush", __func__, pkt);
	cmdq_dump_pkt(pkt, 0, true);
	cmdq_pkt_destroy(pkt);

	cmdq_msg("%s: pkt2:%p before flush", __func__, pkt2);
	cmdq_dump_pkt(pkt2, 0, true);
	cmdq_pkt_flush(pkt2);
	cmdq_msg("%s: pkt2:%p after flush", __func__, pkt2);
	cmdq_dump_pkt(pkt2, 0, true);
	cmdq_pkt_destroy(pkt2);

	val = readl((void *)va);
	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);

	if (val != i)
		cmdq_err("val:%#x not equal to i:%#x", val, i);
	else
		cmdq_msg("val:%#x equals to i:%#x", val, i);
}

static void cmdq_test_mbox_sync_token_loop_iter(struct timer_list *t)
{
	struct cmdq_test *test = from_timer(test, t, timer);

	if (!test->tick)
		del_timer(&test->timer);
	else {
		mod_timer(&test->timer, jiffies + msecs_to_jiffies(300));
		test->iter += 1;
	}
}

static void cmdq_test_mbox_loop(struct cmdq_test *test)
{
	struct cmdq_pkt		*pkt;
	struct cmdq_thread	*thread =
		(struct cmdq_thread *)test->loop->chan->con_priv;
	s32		ret;

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);

	pkt = cmdq_pkt_create(test->loop);
	cmdq_pkt_wfe(pkt, test->token_user0);
	cmdq_pkt_finalize_loop(pkt);

	cmdq_dump_pkt(pkt, 0, true);

	test->iter = 0;
	test->tick = true;
	timer_setup(&test->timer, cmdq_test_mbox_sync_token_loop_iter,
		0);
	mod_timer(&test->timer, jiffies + msecs_to_jiffies(300));

	writel(test->token_user0,
		(void *)CMDQ_SYNC_TOKEN_UPD(test->gce.va));

	ret = cmdq_pkt_flush_async(pkt, NULL, 0);
	cmdq_msg("flush pkt:0x%p ret:%d", pkt, ret);
	while (test->iter < CMDQ_TEST_CNT) {
		cmdq_msg("loop thrd-idx:%u pkt:%p iter:%u",
			thread->idx, pkt, test->iter);
		msleep_interruptible(1000);
	}

	cmdq_mbox_stop(test->loop);
	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);
	test->tick = false;
	del_timer(&test->timer);
}

static void cmdq_test_mbox_dma_access(struct cmdq_test *test, const bool secure)
{
	unsigned long	va = (unsigned long)(secure ?
		CMDQ_THR_SPR3(test->gce.va, 3) :
		CMDQ_GPR_R32(test->gce.va, CMDQ_GPR_DEBUG_DUMMY));
	unsigned long	pa = secure ? CMDQ_THR_SPR3(test->gce.pa, 3) :
		CMDQ_GPR_R32(test->gce.pa, CMDQ_GPR_DEBUG_DUMMY);
	const u32	ofst = 0xabc, pttn[CMDQ_TEST_CNT] = {
		0xabcdabcd, 0xaabbccdd, 0xdeaddead};

	struct cmdq_client	*clt = secure ? test->sec : test->clt;
	struct cmdq_pkt		*pkt;

	u32		*dma_va;
	dma_addr_t	dma_pa;
	u32		val;

	dma_va = cmdq_mbox_buf_alloc(clt, &dma_pa);
	if (!dma_va || !dma_pa) {
		cmdq_err("cmdq_mbox_buf_alloc failed");
		return;
	}
	dma_va[0] = pttn[0];
	dma_va[1] = pttn[0];
	dma_va[2] = pttn[1];

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);

	writel(pttn[2], (void *)va);

	pkt = cmdq_pkt_create(clt);
#ifdef CMDQ_SECURE_SUPPORT
	if (secure)
		cmdq_sec_pkt_set_data(pkt, 0, 0, CMDQ_SEC_DEBUG,
			CMDQ_METAEX_NONE);
#endif

	cmdq_pkt_mem_move(pkt, NULL, pa, dma_pa, CMDQ_THR_SPR_IDX1);
	cmdq_pkt_mem_move(pkt, NULL, dma_pa, dma_pa + 4, CMDQ_THR_SPR_IDX1);
	cmdq_pkt_mem_move(pkt, NULL, dma_pa + 8, pa, CMDQ_THR_SPR_IDX1);
	cmdq_pkt_flush(pkt);
	cmdq_pkt_dump_buf(pkt, 0);
	cmdq_pkt_destroy(pkt);

	if (dma_va[0] != pttn[2])
		cmdq_err("move pa:%#x to dma-0:%#x dft:%#x",
			pttn[2], dma_va[0], pttn[0]);
	if (dma_va[1] != pttn[2])
		cmdq_err("move dma-0:%#x to dma-1:%#x dft:%#x",
			pttn[2], dma_va[1], pttn[0]);
	val = readl((void *)va);
	if (dma_va[2] != val)
		cmdq_err("move dma-2:%#x to pa:%#x dft:%#x",
			dma_va[2], val, pttn[2]);

	pkt = cmdq_pkt_create(clt);
#ifdef CMDQ_SECURE_SUPPORT
	if (secure)
		cmdq_sec_pkt_set_data(pkt, 0, 0, CMDQ_SEC_DEBUG,
			CMDQ_METAEX_NONE);
#endif

	cmdq_pkt_jump(pkt, 8);
	cmdq_pkt_write_value_addr(pkt, dma_pa + ofst, pttn[2], ~0);
	cmdq_pkt_read_addr(pkt, dma_pa + ofst, CMDQ_THR_SPR_IDX1);
	cmdq_pkt_write_reg_addr(pkt, dma_pa + ofst + 4, CMDQ_THR_SPR_IDX1, ~0);
	cmdq_pkt_flush(pkt);
	cmdq_pkt_dump_buf(pkt, 2);
	cmdq_pkt_destroy(pkt);

	if (dma_va[ofst / 4] != dma_va[ofst / 4 + 1] ||
		dma_va[ofst / 4 + 1] != pttn[2])
		cmdq_err("pa:%pa offset:%#x va:%p val:%#x %#x pttn:%#x",
			&dma_pa, ofst, &dma_va[ofst / 4], dma_va[ofst / 4],
			dma_va[ofst / 4 + 1], pttn[2]);

	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);
	cmdq_mbox_buf_free(clt, dma_va, dma_pa);
}

static void cmdq_test_mbox_write_dma(
	struct cmdq_test *test, const bool secure, u32 cnt)
{
	struct cmdq_client	*clt = secure ? test->sec : test->clt;
	struct cmdq_pkt		*pkt;
	u32		*dma_va;
	dma_addr_t	dma_pa;
	s32 i;

	dma_va = cmdq_mbox_buf_alloc(clt, &dma_pa);
	if (!dma_va) {
		cmdq_err("cmdq_mbox_buf_alloc failed");
		return;
	}
	pkt = cmdq_pkt_create(clt);
	for (i = 0; i < cnt; i++)
		cmdq_pkt_write(pkt, NULL, dma_pa + ((i * 4) % PAGE_SIZE), i, ~0);

	cmdq_pkt_flush(pkt);
	cmdq_pkt_destroy(pkt);
}

static void cmdq_test_mbox_write_dma_cpr(
	struct cmdq_test *test, const bool secure, u32 cnt)
{
	struct cmdq_client *clt = secure ? test->sec : test->clt;
	struct cmdq_pkt *pkt;
	u32 *dma_va;
	dma_addr_t dma_pa;
	s32 i;
	const u32 pattern = 0xbeef0000;

	dma_va = cmdq_mbox_buf_alloc(clt, &dma_pa);
	if (!dma_va) {
		cmdq_err("cmdq_mbox_buf_alloc failed");
		return;
	}

	cmdq_msg("dma pa %#lx", (unsigned long)dma_pa);
	pkt = cmdq_pkt_create(clt);
	for (i = 0; i < cnt; i++) {
		cmdq_pkt_assign_command(pkt, CMDQ_THR_SPR_IDX3, pattern + i);
		cmdq_pkt_assign_command(pkt, CMDQ_CPR_STRAT_ID + i * 2, (u32)dma_pa + i * 4);
		cmdq_pkt_assign_command(pkt, CMDQ_CPR_STRAT_ID + i * 2 + 1, (u32)(dma_pa >> 32));
		cmdq_pkt_write_reg_indriect(pkt, CMDQ_CPR_STRAT_ID + CMDQ_CPR64 + i,
			CMDQ_THR_SPR_IDX3, U32_MAX);
		*(dma_va + i) = 0xdead0000 + i;
	}

	cmdq_pkt_flush(pkt);
	cmdq_pkt_destroy(pkt);

	for (i = 0; i < cnt; i++) {
		if (*(dma_va + i) != pattern + i)
			cmdq_err("data %u: %#x fail", i, *(dma_va + i));
		else
			cmdq_msg("data %u: %#x match", i, *(dma_va + i));
	}

	cmdq_mbox_buf_free(test->clt, dma_va, dma_pa);
}

static void cmdq_test_mbox_sync_token_flush(struct timer_list *t)
{
	u32	val;
	struct cmdq_test *test = from_timer(test, t, timer);

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);

	writel((1L << 16) | test->token_user0,
		(void *)CMDQ_SYNC_TOKEN_UPD(test->gce.va));
	val = readl((void *)CMDQ_SYNC_TOKEN_UPD(test->gce.va));
	cmdq_log("data:%#hx event:%#x val:%#x",
		test->token_user0, (1 << 16), val);

	if (!test->tick)
		del_timer(&test->timer);
	else
		mod_timer(&test->timer, jiffies + msecs_to_jiffies(10));

	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);
}

void cmdq_test_mbox_flush(
	struct cmdq_test *test, const s32 secure, const bool threaded)
{
	struct cmdq_client		*clt = secure ? test->sec : test->clt;
	struct cmdq_pkt			*pkt[CMDQ_TEST_CNT] = {0};
	s32				i, err;

	cmdq_msg("%s sec:%d threaded:%d", __func__, secure, threaded);

	test->tick = true;
	timer_setup(&test->timer, cmdq_test_mbox_sync_token_flush,
		0);
	mod_timer(&test->timer, jiffies + msecs_to_jiffies(10));

	for (i = 0; i < CMDQ_TEST_CNT; i++) {
		pkt[i] = cmdq_pkt_create(clt);
#ifdef CMDQ_SECURE_SUPPORT
		if (secure) {
			cmdq_sec_pkt_set_data(pkt[i], 0, 0, CMDQ_SEC_DEBUG,
				CMDQ_METAEX_NONE);
#ifdef CMDQ_SECURE_MTEE_SUPPORT
			if (!~secure)
				cmdq_sec_pkt_set_mtee(pkt[i], true);
#endif
		}
#endif

		cmdq_pkt_wfe(pkt[i], test->token_user0);
		pkt[i]->priority = i;

		if (!threaded)
			cmdq_pkt_flush_async(pkt[i], NULL, NULL);
		else
			cmdq_pkt_flush_threaded(pkt[i],
				cmdq_test_mbox_cb_destroy, (void *)pkt[i]);
	}

	for (i = 0; i < CMDQ_TEST_CNT; i++) {
		if (!pkt[i]) {
			cmdq_err("NULL pkt:%d", i);
			continue;
		}
		msleep_interruptible(100);
		if (!threaded)
			err = cmdq_pkt_wait_complete(pkt[i]);
		else
			err = 0;
		if (err < 0) {
			cmdq_err("wait complete pkt[%d]:%p err:%d",
				i, pkt[i], err);
			continue;
		}
		if (!threaded)
			cmdq_pkt_destroy(pkt[i]);
	}

	test->tick = false;
	del_timer(&test->timer);
}

static void cmdq_test_mbox_write(
	struct cmdq_test *test, const s32 secure, const bool need_mask)
{
	const u32	mask = need_mask ? (1 << 16) : ~0;
	const u32	pttn = (1 << 0) | (1 << 2) | (1 << 16);
	unsigned long	va = (unsigned long)(secure ?
		CMDQ_THR_SPR3(test->gce.va, 3) :
		CMDQ_GPR_R32(test->gce.va, CMDQ_GPR_DEBUG_DUMMY));
	unsigned long	pa = secure ? CMDQ_THR_SPR3(test->gce.pa, 3) :
		CMDQ_GPR_R32(test->gce.pa, CMDQ_GPR_DEBUG_DUMMY);

	struct cmdq_client	*clt = secure ? test->sec : test->clt;
	struct cmdq_pkt		*pkt;
	s32			val;

	cmdq_msg("sec:%d va:%#lx pa:%#lx pttn:%#x mask:%#x clt:%p",
		secure, va, pa, pttn, mask, clt);

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);

	writel(0, (void *)va);

	pkt = cmdq_pkt_create(clt);
#ifdef CMDQ_SECURE_SUPPORT
	if (secure) {
		cmdq_sec_pkt_set_data(pkt, 0, 0, CMDQ_SEC_DEBUG,
			CMDQ_METAEX_NONE);
#ifdef CMDQ_SECURE_MTEE_SUPPORT
		if (!~secure)
			cmdq_sec_pkt_set_mtee(pkt, true);
#endif
	}
#endif
	//pa:0x178000bc pttn:0x10005 mask:0xffffffff
	cmdq_pkt_write(pkt, NULL, pa, pttn, mask);
	cmdq_dump_pkt(pkt, 0, true);
	cmdq_pkt_flush(pkt);

	val = readl((void *)va);
	if (val != (pttn & mask)) {
		cmdq_err("wrong val:%#x ans:%#x", val, pttn & mask);
		cmdq_pkt_dump_buf(pkt, 0);
	} else
		cmdq_msg("right val:%#x ans:%#x", val, pttn & mask);

	cmdq_pkt_destroy(pkt);

	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);

}

static void cmdq_test_menuload_write(
	struct cmdq_test *test, const s32 secure, const bool need_mask)
{
	const u32	mask = need_mask ? (1 << 16) : ~0;
	const u32	pttn = 0xFFFF; //0x1005
	const int BKA3477F = 0x1468FFC;
	unsigned long	va  = 0;
	struct cmdq_client	*clt = secure ? test->sec : test->clt;
	struct cmdq_pkt		*pkt;
	s32			val;
	void __iomem *virt_addr_iomem =
		ioremap(BKA3477F + RIU_PM_OFFSET, sizeof(int));
	if (!virt_addr_iomem) {
		cmdq_err("ioremap fail\n");
		return;
	}
	va = (unsigned long)virt_addr_iomem;

	cmdq_msg("sec:%d va:%#lx pa:0x%x pttn:%#x mask:%#x clt:%p",
		secure, va, BKA3477F, pttn, mask, clt);

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);

	writel(0, (void *)va);

	pkt = cmdq_pkt_create(clt);
#ifdef CMDQ_SECURE_SUPPORT
	if (secure) {
		cmdq_sec_pkt_set_data(pkt, 0, 0, CMDQ_SEC_DEBUG,
			CMDQ_METAEX_NONE);
#ifdef CMDQ_SECURE_MTEE_SUPPORT
		if (!~secure)
			cmdq_sec_pkt_set_mtee(pkt, true);
#endif
	}
#endif

	//Clear ML ${domain} trigger
	cmdq_pkt_clear_event(pkt, CMDQ_EVENT_ML_TRIG_DISP);
	//Wait ML ${domian} trigger
	cmdq_pkt_wfe(pkt, CMDQ_EVENT_ML_TRIG_DISP);
	//Fill in scenario.
	cmdq_pkt_write(pkt, NULL, 0x1468FFC, pttn, ~0);

	cmdq_dump_pkt(pkt, 0, true);
	//Flush packet.
	cmdq_pkt_flush(pkt);

	msleep(30);
	val = readl((void *)va);
	if (val != (pttn & mask)) {
		cmdq_err("wrong val:%#x ans:%#x", val, pttn & mask);
		cmdq_pkt_dump_buf(pkt, 0);
	} else
		cmdq_msg("right val:%#x ans:%#x", val, pttn & mask);

	cmdq_pkt_destroy(pkt);

	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);

}

static void cmdq_test_mbox_handshake_event(struct cmdq_test *test)
{
	struct cmdq_client *clt1 = test->clt, *clt2 = test->loop;
	struct cmdq_pkt *pkt_wait, *pkt_shake;
	int ret;

	if (!test->clt)
		return;

	if (!test->loop)
		return;

	if (cmdq_mbox_get_base_pa(clt1->chan) ==
		cmdq_mbox_get_base_pa(clt2->chan)) {
		cmdq_msg("no handshake for 2 same client");
		return;
	}

	pkt_wait = cmdq_pkt_create(clt1);
	cmdq_pkt_wfe(pkt_wait, CMDQ_EVENT_HANDSHAKE);
	cmdq_pkt_handshake_event(pkt_wait, CMDQ_EVENT_HANDSHAKE + 1);

	pkt_shake = cmdq_pkt_create(clt2);
	cmdq_pkt_handshake_event(pkt_shake, CMDQ_EVENT_HANDSHAKE);
	cmdq_pkt_wfe(pkt_shake, CMDQ_EVENT_HANDSHAKE + 1);

	cmdq_pkt_flush_async(pkt_wait, NULL, NULL);
	ret = cmdq_pkt_flush(pkt_shake);

	cmdq_pkt_wait_complete(pkt_wait);

	if (ret < 0)
		cmdq_err("shake event fail:%d", ret);

}

static void cmdq_access_sub_impl(struct cmdq_test *test,
	struct cmdq_client *clt, const char *tag)
{
	struct cmdq_pkt *pkt;
	u32 *regs, count, *va, i;
	dma_addr_t pa;
	u8 swap_reg = CMDQ_THR_SPR_IDX1;
	u32 pat = 0xadceabce, pat_init = 0xdeaddead, pat_src = 0xbeefbeef;
	void __iomem	*va_base;
	u32 val = 0;
	u32 pa_base;

	count = cmdq_util_test_get_subsys_list(&regs);
	if (count <= 0) {
		cmdq_err("invalid count:%d", count);
		return;
	}

	va = cmdq_mbox_buf_alloc(clt, &pa);
	if (!va) {
		cmdq_err("cmdq_mbox_buf_alloc failed");
		return;
	}

	for (i = 0; i < count; i++) {
		va[0] = pat_init;

		//cmdq_msg("%s idx:%d, addr:%#x = %#x ", __func__, i, regs[i]);
		pa_base = regs[i];
		va_base = ioremap(pa_base, 0x1000);
		writel(pat, va_base);
		val = readl(va_base);
		if (val != pat)
			cmdq_msg("%s AP write, addr:%#x = %#x, pat:%#x",
				__func__, regs[i], val, pat);
		writel(0, va_base);

		pkt = cmdq_pkt_create(test->clt);
		cmdq_pkt_write_value_addr(pkt, regs[i], pat_src, ~0);
		cmdq_pkt_mem_move(pkt, NULL, regs[i], pa, swap_reg);
		cmdq_pkt_flush(pkt);

		if (va[0] != pat_src)
			cmdq_err(
				"%s access reg fail addr:%#x val:%#x should be:%#x",
				tag, regs[i], va[0], pat_src);

		cmdq_pkt_destroy(pkt);
	}

	cmdq_mbox_buf_free(clt, va, pa);
}

static void cmdq_test_mbox_subsys_access(struct cmdq_test *test)
{
	cmdq_access_sub_impl(test, test->clt, "clt");
	cmdq_access_sub_impl(test, test->loop, "loop");
}

static void cmdq_test_err_irq(struct cmdq_test *test)
{
	struct cmdq_pkt *pkt;
	struct cmdq_pkt_buffer *buf;
	u64 *inst;

	pkt = cmdq_pkt_create(test->clt);
	cmdq_pkt_jump(pkt, 0);

	buf = list_first_entry(&pkt->buf, typeof(*buf), list_entry);
	inst = (u64 *)buf->va_base;
	*inst = 0xffffbeefdeadbeef;

	cmdq_pkt_flush(pkt);
	cmdq_pkt_destroy(pkt);
}

static void cmdq_test_devapc_vio(struct cmdq_test *test)
{
	struct cmdq_pkt *pkt;
	int ret;

	pkt = cmdq_pkt_create(test->clt);
	cmdq_pkt_read(pkt, NULL, 0x0, CMDQ_THR_SPR_IDX3);
	ret = cmdq_pkt_flush(pkt);
	cmdq_pkt_destroy(pkt);

	cmdq_msg("%s ret:%d end", __func__, ret);
}

static void cmdq_test_mbox_stop(struct cmdq_test *test)
{
	struct cmdq_pkt *pkt[3];
	int i;

	for (i = 0; i < ARRAY_SIZE(pkt); i++) {
		pkt[i] = cmdq_pkt_create(test->clt);
		cmdq_pkt_wfe(pkt[i], test->token_user0);
		cmdq_pkt_flush_async(pkt[i], NULL, NULL);
	}

	cmdq_msg("%s stop channel", __func__);
	cmdq_mbox_channel_stop(test->clt->chan);

	cmdq_msg("%s still wait it", __func__);
	for (i = 0; i < ARRAY_SIZE(pkt); i++) {
		cmdq_pkt_wait_complete(pkt[i]);
		cmdq_pkt_destroy(pkt[i]);
	}
}

static void cmdq_test_show_events(struct cmdq_test *test)
{
	u32 i;

	cmdq_msg("%s scan all active event ...", __func__);
	cmdq_mbox_enable(test->clt->chan);

	for (i = 0; i < CMDQ_EVENT_MAX; i++)
		if (cmdq_get_event(test->clt->chan, i))
			cmdq_msg("event set:%u", i);
	cmdq_mbox_disable(test->clt->chan);
}

static void cmdq_test_mbox_reuse_buf_va(struct cmdq_test *test)
{
	unsigned long	va = (unsigned long)(CMDQ_GPR_R32(
		test->gce.va, CMDQ_GPR_DEBUG_DUMMY));
	unsigned long	pa = CMDQ_GPR_R32(
		test->gce.pa, CMDQ_GPR_DEBUG_DUMMY);

	struct cmdq_pkt		*pkt;
	s32			i, j = 0, val;
	struct cmdq_reuse *reuse;
	const u32 ans = (CMDQ_INST_SIZE * CMDQ_INST_SIZE - 1) * CMDQ_INST_SIZE;

	reuse = kcalloc(CMDQ_INST_SIZE, sizeof(*reuse), GFP_KERNEL);

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);

	writel(0xdeaddead, (void *)va);

	pkt = cmdq_pkt_create(test->clt);
	for (i = 0; i < CMDQ_INST_SIZE * CMDQ_INST_SIZE; i++) {
		if (i % CMDQ_INST_SIZE != CMDQ_INST_SIZE - 1)
			cmdq_pkt_write(pkt, NULL, pa, i, ~0);
		else {
			cmdq_pkt_write_value_addr_reuse(
				pkt, pa, i, ~0, &reuse[j]);
			reuse[j].val = i;
			cmdq_msg("%s: reuse:%d va:%p val:%#x inst:%#llx",
				__func__, j,
				reuse[j].va, reuse[j].val, *reuse[j].va);
			j += 1;
		}
	}
	cmdq_pkt_flush(pkt);

	for (i = 0; i < j; i++)
		reuse[i].val *= CMDQ_INST_SIZE;
	cmdq_pkt_reuse_buf_va(pkt, reuse, CMDQ_INST_SIZE);
	cmdq_pkt_flush(pkt);

	cmdq_pkt_destroy(pkt);

	val = readl((void *)va);
	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);

	if (val != ans)
		cmdq_err("val:%#x not equal to ans:%#x", val, ans);
	else
		cmdq_msg("val:%#x equals to ans:%#x", val, ans);
	kfree(reuse);
}

struct cmdq_test_mbox_routine {
	struct cmdq_pkt *pkt;
	u16 mod;
	u16 pipe;
	u32 pa;
	u32 pas;
	u16 spr;
	const u32 *reg;
	size_t sz;
	const u32 *ext_reg;
	size_t ext_sz;
	u64 *inst;
	s32 mark;
	u32 aid_sel;
	bool secure;
};

static void cmdq_test_mbox_prebuilt_routine(struct cmdq_test_mbox_routine r)
{
	s32 i;

	if (r.inst)
		*r.inst |= CMDQ_REG_SHIFT_ADDR(
			(s32)r.pkt->cmd_buf_size - r.mark - CMDQ_INST_SIZE);

	cmdq_pkt_write_value_addr(r.pkt, r.pa + 0x000, 0x1, 0x00000001);
	if (r.mod == CMDQ_PREBUILT_MML)
		cmdq_pkt_write_value_addr(r.pkt, r.pa + 0x024, 0x1, UINT_MAX);
	for (i = 0; i < r.sz; i++)
		cmdq_pkt_write_reg_addr(r.pkt, r.pa + r.reg[i],
			CMDQ_CPR_PREBUILT(r.mod, r.pipe, i), UINT_MAX);
	for (i = 0; i < r.ext_sz; i++) {
		if (r.ext_reg[i] == 0x000 || r.ext_reg[i] == 0x024)
			continue;
		cmdq_pkt_write_reg_addr(r.pkt, r.pa + r.ext_reg[i],
			CMDQ_CPR_PREBUILT_EXT(r.mod, r.pipe, i), UINT_MAX);
	}
	cmdq_pkt_write_value_addr(r.pkt, r.pa + 0x038,
				  r.secure ? (1 << 18) : 0, 1 << 18);
	cmdq_pkt_write_value_addr(r.pkt, r.pas + 0xfa8,
				  r.secure ? r.aid_sel : 0, r.aid_sel);
}

static void cmdq_test_mbox_prebuilt_instr_ext_table(struct cmdq_test *test,
	struct cmdq_test_mbox_routine r, const u16 event,
	const u32 pa0, const u32 pa1, const u32 pas, const bool secure)
{
	struct cmdq_pkt *pkt;
	struct cmdq_pkt_buffer *buf;
	struct cmdq_operand lop, rop;
	u64 *inst[6] = {NULL};
	s32 mark[6], i = 0;

	memset(mark, 0, sizeof(mark));

	cmdq_msg("%s: mod:%hu event:%hu pa0:%#x pa1:%#x pas:%#x",
		__func__, r.mod, event, pa0, pa1, pas);

	pkt = cmdq_pkt_create(test->clt);
	cmdq_pkt_wfe(pkt, event);

	/* conditional jumps */
	lop.reg = true;
	lop.idx = CMDQ_CPR_PREBUILT_PIPE(r.mod);
	rop.reg = false;
	if (pa0 && pa1) {
		rop.value = 1;
		mark[0] = pkt->cmd_buf_size;
		inst[0] = cmdq_pkt_get_curr_buf_va(pkt);
		cmdq_pkt_assign_command(pkt, r.spr, 0);
		cmdq_pkt_cond_jump(pkt, r.spr, &lop, &rop, CMDQ_EQUAL);
	}
	if (pa0 && secure) {
		rop.value = 2;
		mark[1] = pkt->cmd_buf_size;
		inst[1] = cmdq_pkt_get_curr_buf_va(pkt);
		cmdq_pkt_assign_command(pkt, r.spr, 0);
		cmdq_pkt_cond_jump(pkt, r.spr, &lop, &rop, CMDQ_EQUAL);
	}
	if (pa1 && secure) {
		rop.value = 3;
		mark[2] = pkt->cmd_buf_size;
		inst[2] = cmdq_pkt_get_curr_buf_va(pkt);
		cmdq_pkt_assign_command(pkt, r.spr, 0);
		cmdq_pkt_cond_jump(pkt, r.spr, &lop, &rop, CMDQ_EQUAL);
	}

	/* routines */
	r.pkt = pkt;
	r.pas = pas;
	if (pa0) {
		/* case 0: normal pipe 0 */
		r.pa = pa0;
		r.aid_sel = 0x15;
		cmdq_test_mbox_prebuilt_routine(r);

		if (pa1 || secure) {
			mark[3] = pkt->cmd_buf_size;
			inst[3] = cmdq_pkt_get_curr_buf_va(pkt);
			cmdq_pkt_jump(pkt, 0);
		}
	}
	if (pa1) {
		/* case 1: normal pipe 1 */
		r.pipe = 1;
		r.pa = pa1;
		r.inst = inst[0];
		r.mark = mark[0];
		r.aid_sel = 0x2a;
		cmdq_test_mbox_prebuilt_routine(r);

		if (secure) {
			mark[4] = pkt->cmd_buf_size;
			inst[4] = cmdq_pkt_get_curr_buf_va(pkt);
			cmdq_pkt_jump(pkt, 0);
		}
	}
	if (pa1 && secure) {
		/* case 3: secure pipe 1 */
		r.inst = inst[2];
		r.mark = mark[2];
		r.secure = true;
		cmdq_test_mbox_prebuilt_routine(r);

		if (pa0) {
			mark[5] = pkt->cmd_buf_size;
			inst[5] = cmdq_pkt_get_curr_buf_va(pkt);
			cmdq_pkt_jump(pkt, 0);
		}
	}
	if (pa0 && secure) {
		/* case 2: secure pipe 0 */
		r.pipe = 0;
		r.pa = pa0;
		r.inst = inst[1];
		r.mark = mark[1];
		r.aid_sel = 0x15;
		r.secure = true;
		cmdq_test_mbox_prebuilt_routine(r);
	}

	if (inst[3])
		*inst[3] |= CMDQ_REG_SHIFT_ADDR((s32)pkt->cmd_buf_size - mark[3]);
	if (inst[4])
		*inst[4] |= CMDQ_REG_SHIFT_ADDR((s32)pkt->cmd_buf_size - mark[4]);
	if (inst[5])
		*inst[5] |= CMDQ_REG_SHIFT_ADDR((s32)pkt->cmd_buf_size - mark[5]);
	cmdq_pkt_set_event(pkt, event + 1);
	cmdq_pkt_finalize_loop(pkt);
	cmdq_dump_pkt(pkt, 0, true);

	buf = list_first_entry_or_null(&pkt->buf, typeof(*buf), list_entry);
	if (!buf) {
		cmdq_pkt_destroy(pkt);
		return;
	}

	cmdq_msg("%s: pkt:%p pa:%#lx cmd_buf_size:%#lx pc:%#lx end:%#lx",
		__func__, pkt, (unsigned long)buf->pa_base,
		(unsigned long)pkt->cmd_buf_size,
		(unsigned long)CMDQ_REG_SHIFT_ADDR(buf->pa_base),
		(unsigned long)CMDQ_REG_SHIFT_ADDR(buf->pa_base +
			pkt->cmd_buf_size));

	for (i = 0; i < pkt->cmd_buf_size / CMDQ_INST_SIZE; i++)
		cmdq_msg(",%d,%#llx,", i, *((u64 *)buf->va_base + i));

	cmdq_pkt_destroy(pkt);
}

static void cmdq_test_mbox_prebuilt_instr_ext(struct cmdq_test *test,
	const u16 mod, const u16 event)
{
	/* configure */
	const u16 spr = CMDQ_THR_SPR_IDX3;
	static const u32 reg[] = {
		0x118, 0x120, 0x128, 0x148, 0x150, 0x200,
		0xf00, 0xf08, 0xf10, 0xf20, 0xf28, 0xf30, 0xf34,
		0xf38, 0xf3c, 0xf40, 0xf44, 0xf48, 0xf4c, 0xf50};
	static const u32 mdp_reg[] = {
		0x000, 0x020, 0x024, 0x028, 0x030, 0x038,
		0x060, 0x068, 0x070, 0x078, 0x080, 0x090, 0x098,
		0x240, 0x244, 0x248, 0x250, 0x254, 0x258, 0x260, 0x264,
		0x268, 0x270, 0x274, 0x278, 0x280, 0x284, 0x288, 0x290, 0x2a0};
	static const u32 mml_reg[] = {
		0x020, 0x028, 0x030, 0x038,
		0x060, 0x068, 0x070, 0x078, 0x080, 0x090, 0x098,
		0x240, 0x244, 0x248, 0x250, 0x254, 0x258, 0x260, 0x264,
		0x268, 0x270, 0x274, 0x278, 0x280, 0x284, 0x288, 0x290, 0x2a0};
	struct cmdq_test_mbox_routine r = {0};

	r.mod = mod;
	r.spr = spr;
	r.reg = reg;
	r.sz = ARRAY_SIZE(reg);

	if (mod == CMDQ_PREBUILT_MDP) {
		r.ext_reg = mdp_reg;
		r.ext_sz = ARRAY_SIZE(mdp_reg);

		cmdq_test_mbox_prebuilt_instr_ext_table(test, r, event,
			0x1f003000,
			0x1f004000,
			0x1f000000, true);
		cmdq_test_mbox_prebuilt_instr_ext_table(test, r, event,
			0,
			0x1f004000,
			0x1f000000, true);
		cmdq_test_mbox_prebuilt_instr_ext_table(test, r, event,
			0x1f003000,
			0,
			0x1f000000, false);
	} else if (mod == CMDQ_PREBUILT_MML) {
		r.ext_reg = mml_reg;
		r.ext_sz = ARRAY_SIZE(mml_reg);

		cmdq_test_mbox_prebuilt_instr_ext_table(test, r, event,
			0x1f803000,
			0x1f804000,
			0x1f800000, true);
		cmdq_test_mbox_prebuilt_instr_ext_table(test, r, event,
			0x1f003000,
			0,
			0x1f000000, true);
	}
}

static void cmdq_test_mbox_prebuilt_instr(struct cmdq_test *test,
	const u16 mod, const u16 event)
{
	struct cmdq_pkt *pkt;
	struct cmdq_pkt_buffer *buf;
	struct cmdq_operand lop, rop;
	u64 *inst, *inst2;
	s32 mark, mark2, i;
	/* configure */
	const u16 spr = CMDQ_THR_SPR_IDX3;
	unsigned long pa0, pa1, reg[] = {
		0x118, 0x120, 0x128, 0x148, 0x150, 0x200,
		0xf00, 0xf08, 0xf10, 0xf20, 0xf28, 0xf30, 0xf34,
		0xf38, 0xf3c, 0xf40, 0xf44, 0xf48, 0xf4c, 0xf50};

	if (mod == CMDQ_PREBUILT_MDP) {
		pa0 = 0x1f003000;
		pa1 = 0x1f004000;
	} else if (mod == CMDQ_PREBUILT_MML) {
		pa0 = 0x1f803000;
		pa1 = 0x1f804000;
	} else if (mod == CMDQ_PREBUILT_VFMT) {
		pa0 = 0x16005000;
		pa1 = 0x16007000;
	} else {
		pa0 = CMDQ_GPR_R32(test->gce.pa, CMDQ_GPR_DEBUG_TIMER);
		pa1 = CMDQ_GPR_R32(test->gce.pa, CMDQ_GPR_DEBUG_DUMMY);
		memset(reg, 0, sizeof(reg));
	}
	cmdq_msg("%s: mod:%hu event:%hu pa0:%#lx pa1:%#lx",
		__func__, mod, event, pa0, pa1);

	pkt = cmdq_pkt_create(test->clt);
	cmdq_pkt_wfe(pkt, event);

	/* conditional jump */
	mark = pkt->cmd_buf_size;
	inst = cmdq_pkt_get_curr_buf_va(pkt);
	cmdq_pkt_assign_command(pkt, spr, 0);

	lop.reg = true;
	lop.idx = CMDQ_CPR_PREBUILT_PIPE(mod);
	rop.reg = false;
	rop.value = 1;
	cmdq_pkt_cond_jump(pkt, spr, &lop, &rop, CMDQ_EQUAL);

	/* pipe 0 */
	for (i = 0; i < ARRAY_SIZE(reg); i++)
		cmdq_pkt_write_reg_addr(pkt, pa0 + reg[i],
			CMDQ_CPR_PREBUILT(mod, 0, i), UINT_MAX);

	mark2 = pkt->cmd_buf_size;
	inst2 = cmdq_pkt_get_curr_buf_va(pkt);
	cmdq_pkt_jump(pkt, 0);

	/* pipe 1 */
	*inst |= CMDQ_REG_SHIFT_ADDR(
		(s32)pkt->cmd_buf_size - mark - CMDQ_INST_SIZE);
	for (i = 0; i < ARRAY_SIZE(reg); i++)
		cmdq_pkt_write_reg_addr(pkt, pa1 + reg[i],
			CMDQ_CPR_PREBUILT(mod, 1, i), UINT_MAX);

	*inst2 |= CMDQ_REG_SHIFT_ADDR((s32)pkt->cmd_buf_size - mark2);
	cmdq_pkt_set_event(pkt, event + 1);
	cmdq_pkt_finalize_loop(pkt);
	cmdq_dump_pkt(pkt, 0, true);

	buf = list_first_entry_or_null(&pkt->buf, typeof(*buf), list_entry);
	if (!buf) {
		cmdq_pkt_destroy(pkt);
		return;
	}

	cmdq_msg("%s: pkt:%p pa:%#lx cmd_buf_size:%#lx pc:%#lx end:%#lx",
		__func__, pkt, (unsigned long)buf->pa_base,
		(unsigned long)pkt->cmd_buf_size,
		(unsigned long)CMDQ_REG_SHIFT_ADDR(buf->pa_base),
		(unsigned long)CMDQ_REG_SHIFT_ADDR(buf->pa_base +
			pkt->cmd_buf_size));

	for (i = 0; i < pkt->cmd_buf_size / CMDQ_INST_SIZE; i++)
		cmdq_msg(",%d,%#llx,", i, *((u64 *)buf->va_base + i));

	cmdq_pkt_destroy(pkt);
}

static void cmdq_test_mbox_prebuilt(struct cmdq_test *test, const u16 mod,
	const bool pipe, const bool timeout)
{
	unsigned long	va = (unsigned long)
		CMDQ_GPR_R32(test->gce.va, CMDQ_GPR_DEBUG_DUMMY);
	unsigned long	pa =
		CMDQ_GPR_R32(test->gce.pa, CMDQ_GPR_DEBUG_DUMMY);
	struct cmdq_pkt *pkt;
	const u32 ans = pipe ? 0xbeaf : 0xfeed, dead = 0xdead;
	u16 event = 689, cpr[3];
	u32 init, cpu[2], val;

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);

	event = CMDQ_PREBUILT_MDP ? 680 : 689;
	cpr[0] = CMDQ_PREBUILT_MDP ? 0x8003 : 0x8006;
	cpr[1] = CMDQ_PREBUILT_MDP ? 0x8023 : 0x809b;
	cpr[2] = CMDQ_PREBUILT_MDP ? 0x8037 : 0x80af;

	writel(0x12345678, (void *)va);

	init = cmdq_get_event(test->clt->chan, event);
	cmdq_msg(
		"%s: mod:%hu pipe:%d timeout:%d event:%hu init:%x cpr:%#hx:%#hx:%#hx",
		__func__, mod, pipe, timeout,
		event, init, cpr[0], cpr[1], cpr[2]);

	pkt = cmdq_pkt_create(test->clt);
	cmdq_pkt_assign_command(pkt, cpr[0], pipe ? 0x1 : 0x0);
	cmdq_pkt_assign_command(pkt, cpr[1], !pipe ? ans : dead);
	cmdq_pkt_assign_command(pkt, cpr[2], pipe ? ans : dead);
	if (!timeout)
		cmdq_pkt_set_event(pkt, event);
	cmdq_pkt_wfe(pkt, event + 1);
	cmdq_pkt_flush(pkt);
	cmdq_pkt_destroy(pkt);

	cpu[0] = cmdq_get_event(test->clt->chan, event);
	cpu[1] = cmdq_get_event(test->clt->chan, event + 1);
	val = readl((void *)va);
	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);

	cmdq_msg("%s: event:%hu cpu:%u:%u pa:%#lx val:%#x ans:%#x",
		__func__, event, cpu[0], cpu[1], pa, val, ans);
}

static void cmdq_test_mbox_tzmp(struct cmdq_test *test, const s32 secure,
	const bool timeout)
{
	unsigned long	va = (unsigned long)
		CMDQ_GPR_R32(test->gce.va, CMDQ_GPR_DEBUG_DUMMY);
	unsigned long	pa =
		CMDQ_GPR_R32(test->gce.pa, CMDQ_GPR_DEBUG_DUMMY);
	const u32	ans = 0xbeafdead, event = 657;
	struct cmdq_client	*clt = test->clt, *sec = test->sec;
	struct cmdq_pkt		*pkt, *pkt2;
	s32			val, i;

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);

	/* trigger secure loop */
	pkt2 = cmdq_pkt_create(sec);
#ifdef CMDQ_SECURE_SUPPORT
	if (secure) {
		cmdq_sec_pkt_set_data(pkt2, 0, 0, CMDQ_SEC_DEBUG,
			CMDQ_METAEX_TZMP);
#ifdef CMDQ_SECURE_MTEE_SUPPORT
		if (!~secure)
			cmdq_sec_pkt_set_mtee(pkt2, true);
#endif
	}
#endif
	cmdq_pkt_finalize_loop(pkt2);
	cmdq_pkt_flush_threaded(pkt2, NULL, (void *)pkt2);

	/* trigger normal */
	for (i = 0; i < 3; i++) {
		writel(0x12345678, (void *)va);

		pkt = cmdq_pkt_create(clt);
		cmdq_pkt_write(pkt, NULL, pa, ans + i, UINT_MAX);
		if (!timeout)
			cmdq_pkt_set_event(pkt, event);
		cmdq_pkt_wfe(pkt, event + 1);
		cmdq_pkt_flush(pkt);

		val = readl((void *)va);
		cmdq_msg("%s: val:%#x ans:%#x", __func__, val, ans + i);

		cmdq_pkt_dump_buf(pkt, 0);
		cmdq_pkt_destroy(pkt);
	}
#ifdef CMDQ_SECURE_SUPPORT
	cmdq_sec_mbox_stop(sec);
#endif
	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);
}

static void cmdq_test_mbox_vcp(struct cmdq_test *test, const bool reuse)
{
	struct cmdq_pkt	*pkt1 = cmdq_pkt_create(test->clt);
	struct cmdq_pkt	*pkt2 = cmdq_pkt_create(test->loop);
	struct cmdq_pkt	*pkt1_reuse;
	struct cmdq_pkt	*pkt2_reuse;
	struct cmdq_reuse reuse1, reuse2;
	struct cmdq_poll_reuse poll_reuse1, poll_reuse2;
	dma_addr_t iova;
	void *va = cmdq_get_vcp_buf(CMDQ_VCP_ENG_MDP_HDR0, &iova);
	u32 val[4], i, j;

	for (i = 0; i < 4; i++)
		writel(0xdeaddead, (void *)va + i * 4);

	cmdq_vcp_enable(true);
	for (i = 0; i < 4; i++) {
		if (!i) {
			cmdq_pkt_readback(pkt1, CMDQ_VCP_ENG_MDP_HDR0, 0, 0,
				CMDQ_GPR_DEBUG_DUMMY, &reuse1, &poll_reuse1);
			cmdq_pkt_readback(pkt2, CMDQ_VCP_ENG_MDP_HDR1, 4, 0,
				CMDQ_GPR_DEBUG_TIMER, &reuse2, &poll_reuse2);
		} else {
			pkt1_reuse = cmdq_pkt_create(test->clt);
			cmdq_pkt_copy(pkt1_reuse, pkt1);
			reuse1.va = cmdq_pkt_get_va_by_offset(pkt1_reuse, reuse1.offset);
			reuse1.val = cmdq_pkt_vcp_reuse_val(
				CMDQ_VCP_ENG_MDP_HDR0, 0, i);
			cmdq_pkt_reuse_buf_va(pkt1_reuse, &reuse1, 1);
			cmdq_pkt_reuse_poll(pkt1_reuse, &poll_reuse1);
			pkt2_reuse = cmdq_pkt_create(test->loop);
			cmdq_pkt_copy(pkt2_reuse, pkt2);
			reuse2.va = cmdq_pkt_get_va_by_offset(pkt2_reuse, reuse2.offset);
			reuse2.val = cmdq_pkt_vcp_reuse_val(
				CMDQ_VCP_ENG_MDP_HDR1, 4, i);
			cmdq_pkt_reuse_buf_va(pkt2_reuse, &reuse2, 1);
			cmdq_pkt_reuse_poll(pkt2_reuse, &poll_reuse2);
		}

		if (reuse) {
			if (!i) {
				cmdq_pkt_dump_buf(pkt1, 0);
				cmdq_pkt_dump_buf(pkt2, 0);
				cmdq_pkt_flush(pkt1);
				cmdq_pkt_flush(pkt2);
			} else {
				cmdq_pkt_refinalize(pkt1_reuse);
				cmdq_pkt_dump_buf(pkt1_reuse, 0);
				cmdq_pkt_flush(pkt1_reuse);
				cmdq_pkt_destroy(pkt1_reuse);

				cmdq_pkt_refinalize(pkt2_reuse);
				cmdq_pkt_dump_buf(pkt2_reuse, 0);
				cmdq_pkt_flush(pkt2_reuse);
				cmdq_pkt_destroy(pkt2_reuse);
			}

			for (j = 0; j < 4; j++)
				val[j] = readl(va + j * 4);
			cmdq_msg("%s: i:%d va:%p iova:%pa val:%#x %#x %#x %#x",
				__func__, i, va, &iova,
				val[0], val[1], val[2], val[3]);
		} else {
			cmdq_pkt_flush_threaded(
				pkt1, cmdq_test_mbox_cb_destroy, (void *)pkt1);
			cmdq_pkt_flush_threaded(
				pkt2, cmdq_test_mbox_cb_destroy, (void *)pkt2);
			msleep(1000);
			break;
		}
	}
	cmdq_vcp_enable(false);

	for (i = 0; i < 4; i++)
		val[i] = readl(va + i * 4);
	cmdq_msg("%s: va:%p iova:%pa", __func__, va, &iova);
	cmdq_msg("%s: val:%#x %#x %#x %#x",
		__func__, val[0], val[1], val[2], val[3]);
}

static void cmdq_test_tf(struct cmdq_test *test)
{
	struct cmdq_client	*clt = test->clt;
	struct cmdq_pkt		*pkt;

	pkt = cmdq_pkt_create(clt);
	cmdq_pkt_write(pkt, NULL, gce_mminfra + 0, ~0, ~0);

	cmdq_pkt_flush(pkt);
	cmdq_pkt_destroy(pkt);
}

static void cmdq_test_mbox_access_illegal(struct cmdq_test *test)
{
	struct cmdq_pkt *pkt;
	s32 i, count = 5;

	pkt = cmdq_pkt_create(test->clt);
	for (i = 0; i < count; i++)
		cmdq_pkt_write(pkt, NULL, 0x1f800400, i, ~0);
	cmdq_pkt_flush(pkt);
	cmdq_dump_pkt(pkt, 0, true);
	cmdq_pkt_destroy(pkt);
}

static void cmdq_test_sec_reg(struct cmdq_test *test)
{
	writel(0x1 << 4, (void *)(test->gce.va + GCE_BUS_GCTL));
	cmdq_msg("%s GCE_BUS_GCTL = %#x", __func__,
		readl((void *)(test->gce.va + GCE_BUS_GCTL)));

	writel(0x1, (void *)(test->gce.va + GCE_PC_HIGH));
	cmdq_msg("%s GCE_PC_HIGH = %#x", __func__,
		readl((void *)(test->gce.va + GCE_PC_HIGH)));
}

#define CMDQ_EVENT_STR_IP_TRIG (56)
static void cmdq_test_str_ldr(struct cmdq_test *test)
{
	struct cmdq_pkt *pkt;
	const int BKA54500 = 0x14A8A00;
	const int BKA5457D = 0x14A8BF4;
	const int BKA5457D_OFFSET = BKA5457D - BKA54500;
	void __iomem	*BKA54500_va;
	const s32 add_data = 0x1;
	const s32 origin_data = 0x1234;
	const s32 answer_data = origin_data + add_data;
	struct cmdq_operand lop = {.reg = true, .idx = CMDQ_THR_SPR_IDX1};
	struct cmdq_operand rop = {.reg = false, .value = add_data};
	s32 val = -1;

	BKA54500_va = ioremap(BKA54500 + RIU_PM_OFFSET, 0x200);

	if (!BKA54500_va) {
		cmdq_err("ioremap BKA54500 fail");
		return;
	}

	pkt = cmdq_pkt_create(test->loop);

	writel(origin_data, (void *)(BKA54500_va + BKA5457D_OFFSET));
	//Check write data to dummy register is ok
	val = readl((void *)(BKA54500_va + BKA5457D_OFFSET));
	if (val != origin_data) {
		cmdq_err("Write to dummy bank fail BKA5457D");
		goto EXIT;
	}

	cmdq_pkt_set_event(pkt, CMDQ_EVENT_STR_IP_TRIG);
	cmdq_pkt_wfe(pkt, CMDQ_EVENT_STR_IP_TRIG);
	//Read register CPU to HGCE(BKA5457D to SPR[1])
	cmdq_pkt_read(pkt, NULL, BKA5457D, CMDQ_THR_SPR_IDX1);
	//SPR[2] = SPR[0] + 1
	cmdq_pkt_logic_command(pkt, CMDQ_LOGIC_ADD, CMDQ_THR_SPR_IDX2, &lop, &rop);
	//Write SPR[2] to DRAM
	cmdq_pkt_write_reg_addr(pkt, CMDQ_STR_LDR_PA, CMDQ_THR_SPR_IDX2, ~0);
	//Write SPR[2] to 0xA545_7D
	cmdq_pkt_write_reg_addr(pkt, BKA5457D, CMDQ_THR_SPR_IDX2, ~0);
	cmdq_pkt_flush(pkt);
	//cmdq_dump_pkt(pkt, 0, true);
	msleep(20);

	//Check answer
	val = readl((void *)(BKA54500_va + BKA5457D_OFFSET));
	if (val != (answer_data)) {
		cmdq_err("FAIL val:%#x ans:%#x", val, answer_data);
		cmdq_pkt_dump_buf(pkt, 0);
	} else
		cmdq_log("PASS val:%#x ans:%#x", val, answer_data);

EXIT:
	iounmap(BKA54500_va);
	cmdq_pkt_destroy(pkt);
}

#define CMDQ_EVENT_REG_SW_DUMMY_0 (72)
static void cmdq_test_wait_event(struct cmdq_test *test)
{
	struct cmdq_pkt *pkt;
	const int BKA54500 = 0x14A8A00;
	const int BKA5457D = 0x14A8BF4;
	const int BKA5457F = 0x14A8BFC;
	const int BKA5457D_OFFSET = BKA5457D - BKA54500;
	const int BKA5457F_OFFSET = BKA5457F - BKA54500;
	void __iomem	*BKA54500_va;
	const s32 add_data = 0x1;
	const s32 origin_data = 0x1234;
	const s32 answer_data = origin_data + add_data;
	struct cmdq_operand lop = {.reg = true, .idx = CMDQ_THR_SPR_IDX1};
	struct cmdq_operand rop = {.reg = false, .value = add_data};
	s32 val = -1;

	BKA54500_va = ioremap(BKA54500 + RIU_PM_OFFSET, 0x200);

	if (!BKA54500_va) {
		cmdq_err("ioremap BKA54500 fail");
		return;
	}

	pkt = cmdq_pkt_create(test->clt);

	writel(origin_data, (void *)(BKA54500_va + BKA5457D_OFFSET));
	//Check write data to dummy register is ok
	val = readl((void *)(BKA54500_va + BKA5457D_OFFSET));
	if (val != origin_data) {
		cmdq_err("Write to dummy bank fail BKA5457D");
		goto EXIT;
	}

	//cmdq_pkt_set_event(pkt, CMDQ_EVENT_REG_SW_DUMMY_0);
	//send sw event trigger A545_7F[0] = 1
	writel(0x1, (void *)(BKA54500_va + BKA5457F_OFFSET));
	cmdq_pkt_wfe(pkt, CMDQ_EVENT_REG_SW_DUMMY_0);
	//Read register CPU to HGCE(BKA5457D to SPR[1])
	cmdq_pkt_read(pkt, NULL, BKA5457D, CMDQ_THR_SPR_IDX1);
	//SPR[2] = SPR[0] + 1
	cmdq_pkt_logic_command(pkt, CMDQ_LOGIC_ADD, CMDQ_THR_SPR_IDX2, &lop, &rop);
	//Write SPR[2] to DRAM
	cmdq_pkt_write_reg_addr(pkt, CMDQ_STR_LDR_PA, CMDQ_THR_SPR_IDX2, ~0);
	//Write SPR[2] to 0xA545_7D
	cmdq_pkt_write_reg_addr(pkt, BKA5457D, CMDQ_THR_SPR_IDX2, ~0);
	cmdq_pkt_flush(pkt);
	//cmdq_dump_pkt(pkt, 0, true);
	msleep(20);

	//Check answer
	val = readl((void *)(BKA54500_va + BKA5457D_OFFSET));
	if (val != (answer_data)) {
		cmdq_err("FAIL val:%#x ans:%#x", val, answer_data);
		cmdq_pkt_dump_buf(pkt, 0);
	} else {
		cmdq_log("PASS val:%#x ans:%#x", val, answer_data);
		cmdq_pkt_dump_buf(pkt, 0);
	}

	//Restore dummy sw event
	writel(0x0, (void *)(BKA54500_va + BKA5457F_OFFSET));
EXIT:
	iounmap(BKA54500_va);
	cmdq_pkt_destroy(pkt);
}

#define CMDQ_EVENT_REG_CMDQ_DISP_VSYNC (88)
static void cmdq_test_cmdqhw_wait_event(struct cmdq_test *test)
{
	struct cmdq_pkt *pkt;
	const int BKA54500 = 0x14A8A00;
	const int BKA5457D = 0x14A8BF4;
	const int BKA5457D_OFFSET = BKA5457D - BKA54500;
	void __iomem	*BKA54500_va;
	const s32 add_data = 0x1;
	const s32 origin_data = 0x1234;
	const s32 answer_data = origin_data + add_data;
	struct cmdq_operand lop = {.reg = true, .idx = CMDQ_THR_SPR_IDX1};
	struct cmdq_operand rop = {.reg = false, .value = add_data};
	s32 val = -1;

	BKA54500_va = ioremap(BKA54500 + RIU_PM_OFFSET, 0x200);

	if (!BKA54500_va) {
		cmdq_err("ioremap BKA54500 fail");
		return;
	}

	pkt = cmdq_pkt_create(test->loop);

	writel(origin_data, (void *)(BKA54500_va + BKA5457D_OFFSET));
	//Check write data to dummy register is ok
	val = readl((void *)(BKA54500_va + BKA5457D_OFFSET));
	if (val != origin_data) {
		cmdq_err("Write to dummy bank fail BKA5457D");
		goto EXIT;
	}

	cmdq_pkt_wfe(pkt, CMDQ_EVENT_REG_CMDQ_DISP_VSYNC);
	//Read register CPU to HGCE(BKA5457D to SPR[1])
	cmdq_pkt_read(pkt, NULL, BKA5457D, CMDQ_THR_SPR_IDX1);
	//SPR[2] = SPR[0] + 1
	cmdq_pkt_logic_command(pkt, CMDQ_LOGIC_ADD, CMDQ_THR_SPR_IDX2, &lop, &rop);
	//Write SPR[2] to 0xA545_7D
	cmdq_pkt_write_reg_addr(pkt, BKA5457D, CMDQ_THR_SPR_IDX2, ~0);
	cmdq_pkt_flush(pkt);
	//cmdq_dump_pkt(pkt, 0, true);
	msleep(100);
//bc00 60/64/68 event status
	//Check answer
	val = readl((void *)(BKA54500_va + BKA5457D_OFFSET));
	if (val != (answer_data)) {
		cmdq_err("FAIL val:%#x ans:%#x", val, answer_data);
		cmdq_pkt_dump_buf(pkt, 0);
	} else {
		cmdq_log("PASS val:%#x ans:%#x", val, answer_data);
		cmdq_pkt_dump_buf(pkt, 0);
	}

	//Restore dummy sw event
	//writel(0x0, (void *)(BKA54500_va + BKA5457F_OFFSET));
EXIT:
	iounmap(BKA54500_va);
	cmdq_pkt_destroy(pkt);
}

struct cmdq_test_bk {
	char name[10];
	const int pa;
	void __iomem *va;
};

#define TEST_BK_LEN (3)
static void cmdq_test_sc_frc_register(struct cmdq_test *test)
{
	struct cmdq_pkt *pkt = NULL;
	struct cmdq_test_bk test_bk[TEST_BK_LEN] = {
		{"BKA31F01", ((0xA31F * 0x200) + (0x01 * 4)), 0}, //SC register(0xA3xx)
		{"BKA44E01", ((0xA44E * 0x200) + (0x01 * 4)), 0}, //SC register(0xA4xx)
		{"BK851716", ((0x8517 * 0x200) + (0x16 * 4)), 0}}; //FRC register(0x8xxx)
	const s32 origin_data = 0x0;
	const s32 answer_data = 0x1234;
	s32 val = -1;
	int i = 0;

	for (i = 0; i < TEST_BK_LEN; i++) {
		test_bk[i].va = ioremap(test_bk[i].pa + RIU_PM_OFFSET, 0x100);
		if (!test_bk[i].va) {
			cmdq_err("ioremap %s fail", test_bk[i].name);
			goto EXIT;
		}
	}

	pkt = cmdq_pkt_create(test->clt);

	for (i = 0; i < TEST_BK_LEN; i++) {
		writel(origin_data, (void *)(test_bk[i].va));
		val = readl((void *)(test_bk[i].va));
		if (val != origin_data) {
			cmdq_err("Write %s va=%p,val=%d fail",
				test_bk[i].name, test_bk[i].va, val);
			goto EXIT;
		}
	}

	cmdq_pkt_write(pkt, NULL, test_bk[0].pa, answer_data, ~0);
	cmdq_pkt_write(pkt, NULL, test_bk[1].pa, answer_data, ~0);
	cmdq_pkt_write(pkt, NULL, test_bk[2].pa, answer_data, ~0);
	cmdq_pkt_flush(pkt);
	//cmdq_dump_pkt(pkt, 0, true);
	msleep(20);

	for (i = 0; i < TEST_BK_LEN; i++) {
		val = readl((void *)(test_bk[i].va));
		if (val != (answer_data))
			cmdq_err("FAIL [%d]val:%#x ans:%#x", i, val, answer_data);
		else
			cmdq_log("PASS [%d]val:%#x ans:%#x", i, val, answer_data);
	}

	cmdq_pkt_dump_buf(pkt, 0);

EXIT:
	for (i = 0; i < TEST_BK_LEN; i++)
		CMDQ_SAFE_IOUNMAP(test_bk[i].va);
	cmdq_pkt_destroy(pkt);
}

static void cmdq_test_multi_packet_in_one_thread(struct cmdq_test *test)
{
	struct cmdq_pkt *pktA, *pktB, *pktC;
	s32 ml_trig_disp_event_id = cmdq_dev_get_event(test->dev, "ml_trig_disp");
	u32 test_val = 0;
	s32 val;
	const s32 secure = false;
	unsigned long	va = (unsigned long)(secure ?
		CMDQ_THR_SPR3(test->gce.va, 3) :
		CMDQ_GPR_R32(test->gce.va, CMDQ_GPR_DEBUG_DUMMY));
	unsigned long	pa = secure ? CMDQ_THR_SPR3(test->gce.pa, 3) :
		CMDQ_GPR_R32(test->gce.pa, CMDQ_GPR_DEBUG_DUMMY);
	dma_addr_t pktB_start_pa, pktC_start_pa;
	s32 err;

	pktA = cmdq_pkt_create(test->clt);
	pktB = cmdq_pkt_create(test->clt);
	pktC = cmdq_pkt_create(test->clt);

	pktB_start_pa = cmdq_pkt_get_pa_by_offset(pktB, 0);
	pktC_start_pa = cmdq_pkt_get_pa_by_offset(pktC, 0);

	cmdq_log("ml_trig_disp_event_id:%d, pkt(A,B)=(%pa,%pa)",
		ml_trig_disp_event_id, &pktB_start_pa, &pktC_start_pa);

	// Add one dummy jump command for get right pa address
	err = cmdq_pkt_jump(pktB, CMDQ_JUMP_PASS);
	if (err < 0) {
		cmdq_err("FAIL pktB add jump 8 err = %d", err);
		goto EXIT;
	}
	err = cmdq_pkt_jump(pktC, CMDQ_JUMP_PASS);
	if (err < 0) {
		cmdq_err("FAIL pktC add jump 8 err = %d", err);
		goto EXIT;
	}
	/* JUMP to start of pkt */
	pktB_start_pa = cmdq_pkt_get_pa_by_offset(pktB, 0);
	pktC_start_pa = cmdq_pkt_get_pa_by_offset(pktC, 0);
	/* Expect run pktA(wait,write,jump_pktB)->pktB(write,jump_pktC)->pktC(write,eoc) */

	cmdq_log("ml_trig_disp_event_id:%d, pkt(A,B)=(%pa,%pa)",
		ml_trig_disp_event_id, &pktB_start_pa, &pktC_start_pa);

	//Write pktA
	pktA->no_irq = true;
	//cmdq_pkt_wfe(pktA, ml_trig_disp_event_id);
	cmdq_pkt_write(pktA, NULL, pa, ++test_val, ~0);
	err = cmdq_pkt_jump_addr(pktA, pktB_start_pa);
	if (err < 0) {
		cmdq_err("FAIL pktA add jump pktB err = %d", err);
		goto EXIT;
	}
	cmdq_pkt_flush(pktA);

	//Write pktB
	pktB->no_irq = true;
	cmdq_pkt_write(pktB, NULL, pa, ++test_val, ~0);
	err = cmdq_pkt_jump_addr(pktB, pktC_start_pa);
	if (err < 0) {
		cmdq_err("FAIL pktB add jump pktC, err = %d", err);
		goto EXIT;
	}
	cmdq_pkt_flush(pktB);

	//Write pktC
	cmdq_pkt_write(pktC, NULL, pa, ++test_val, ~0);
	cmdq_pkt_flush(pktC);

	//Dump packet for debug
	cmdq_pkt_dump_buf(pktA, 0);
	cmdq_pkt_dump_buf(pktB, 0);
	cmdq_pkt_dump_buf(pktC, 0);

	msleep(20);

	val = readl((void *)va);
	if (val != (3))
		cmdq_err("FAIL val:%#x ans:3", val);
	else
		cmdq_msg("PASS val:%#x ans:3", val);


EXIT:
	cmdq_pkt_destroy(pktA);
	cmdq_pkt_destroy(pktB);
	cmdq_pkt_destroy(pktC);
}

static void _cmdq_test_read_reg(unsigned int reg, unsigned int *val)
{
	unsigned long va = 0;
	void __iomem *virt_addr_iomem = ioremap(reg + RIU_PM_OFFSET, sizeof(int));

	if (!virt_addr_iomem) {
		cmdq_err("ioremap fail\n");
		return;
	}

	va = (unsigned long)virt_addr_iomem;
	*val = readl((void *)va);

	iounmap(virt_addr_iomem);
}

static void _cmdq_test_write_reg(unsigned int reg, unsigned int val)
{
	unsigned long va = 0;
	void __iomem *virt_addr_iomem = ioremap(reg + RIU_PM_OFFSET, sizeof(int));

	if (!virt_addr_iomem) {
		cmdq_err("ioremap fail\n");
		return;
	}

	va = (unsigned long)virt_addr_iomem;
	writel(val, (void *)va);

	iounmap(virt_addr_iomem);
}

static int _cmdq_create_ds_domain_script(
	struct cmdq_pkt *pkt,
	enum DS_DOMAIN domain)
{
	dma_addr_t pkt_start_pa = 0;
	unsigned int depth_reg = 0;
	unsigned int idx_reg = 0;
	unsigned int dummy_reg = 0;
	unsigned int wait_event = 0;
	unsigned int depth = 0;
	const s32 op2_idx_shift_bit = 8, pc_move_on = 1, ds_idx_mask = 0xFF;
	unsigned int ds_idx = 0, write_idx = 0;
	struct cmdq_operand lop = {.reg = true, .idx = CMDQ_THR_SPR_IDX1};
	struct cmdq_operand rop = {.reg = false, .value = op2_idx_shift_bit};
	s32 err;

	switch (domain) {
	case DS_DOMAIN_IP:
		depth_reg = 0x14A8AC8; // ip depth BKA54532
		idx_reg = 0x14A8BB4; // ip idx report BKA5456D[7:0]
		dummy_reg = 0x146E180; // ip write dummy reg BKA37060
		wait_event = CMDQ_EVENT_DS_TRIG_IP;
		depth = IP_DS_DEPTH;
	break;
	case DS_DOMAIN_OPF:
		depth_reg = 0x14A8AE8; // opf depth BKA5453A
		idx_reg = 0x14A8BB8; // opf idx report BKA5456E[7:0]
		dummy_reg = 0x146E184; // opf write dummy reg BKA37061
		wait_event = CMDQ_EVENT_DS_TRIG_OPF;
		depth = OPF_DS_DEPTH;
	break;
	case DS_DOMAIN_OPB:
		depth_reg = 0x14A8B08; // opb depth BKA54542
		idx_reg = 0x14A8BBC; // opb idx report BKA5456F[7:0]
		dummy_reg = 0x146E188; // opb write dummy reg BKA37062
		wait_event = CMDQ_EVENT_DS_TRIG_OPB;
		depth = OPB_DS_DEPTH;
	break;
	case DS_DOMAIN_OP2:
		depth_reg = 0x14A8B28; // op2 depth BKA5454A
		idx_reg = 0x14A8BBC; // op2 idx report BKA5456F[15:8]
		dummy_reg = 0x146E18C; // op2 write dummy reg BKA37063
		wait_event = CMDQ_EVENT_DS_TRIG_OP2;
		depth = OP2_DS_DEPTH;
	break;
	default:
	return -1;
	}

#ifdef CMDQ_SECURE_SUPPORT
	if (secure) {
		cmdq_sec_pkt_set_data(pkt, 0, 0, CMDQ_SEC_DEBUG,
			CMDQ_METAEX_NONE);
#ifdef CMDQ_SECURE_MTEE_SUPPORT
		if (!~secure)
			cmdq_sec_pkt_set_mtee(pkt, true);
#endif
	}
#endif

	// get pkt start PA for ds script
	pkt_start_pa = cmdq_pkt_get_pa_by_offset(pkt, 0);

	// Add one dummy jump command for get right pa address
	err = cmdq_pkt_jump(pkt, CMDQ_JUMP_PASS);
	if (err < 0) {
		cmdq_err("FAIL pkt add jump 8 err = %d", err);
		return -1;
	}

	pkt_start_pa = cmdq_pkt_get_pa_by_offset(pkt, 0);

	cmdq_log_ex("domain=%u start addr=%pa depth=%u\n", domain, &pkt_start_pa, depth);

	// Fill in scenario.
	// 1. Clear DS ${domain} trigger
	cmdq_pkt_clear_event(pkt, wait_event);
	// 2. Wait DS ${domian} trigger
	cmdq_pkt_wfe(pkt, wait_event);
	// 3. Read DS ${domian} index to SPR[1]
	cmdq_pkt_read(pkt, NULL, idx_reg, CMDQ_THR_SPR_IDX1);

	if (domain == DS_DOMAIN_OP2) {
		lop.reg = true;
		lop.idx = CMDQ_THR_SPR_IDX1;
		rop.reg = false;
		rop.value = op2_idx_shift_bit;
		cmdq_pkt_logic_command(pkt, CMDQ_LOGIC_RIGHT_SHIFT, CMDQ_THR_SPR_IDX1, &lop, &rop);
	} else {
		lop.reg = true;
		lop.idx = CMDQ_THR_SPR_IDX1;
		rop.reg = false;
		rop.value = ds_idx_mask;
		cmdq_pkt_logic_command(pkt, CMDQ_LOGIC_AND, CMDQ_THR_SPR_IDX1, &lop, &rop);
	}
	// 4. Read DS ${domian} depth to SPR[2] (depth == GCE instruction num)
	cmdq_pkt_read(pkt, NULL, depth_reg, CMDQ_THR_SPR_IDX2);
	// 5. Calculate running DS script offset SPR[3] = SPR[1] * SPR[2]
	lop.reg = true;
	lop.idx = CMDQ_THR_SPR_IDX1;
	rop.reg = true;
	rop.idx = CMDQ_THR_SPR_IDX2;
	cmdq_pkt_logic_command(pkt, CMDQ_LOGIC_MULTIPLY, CMDQ_THR_SPR_IDX3, &lop, &rop);
	// 6. Calculate running DS script offset SPR[3] = SPR[3] + 1 PC
	lop.reg = true;
	lop.idx = CMDQ_THR_SPR_IDX3;
	rop.reg = false;
	rop.value = pc_move_on;
	cmdq_pkt_logic_command(pkt, CMDQ_LOGIC_ADD,
		CMDQ_THR_SPR_IDX3, &lop, &rop);
	// 7. Jump to running DS script pc + SPR[3]
	lop.reg = false;
	lop.value = 0;
	rop.reg = false;
	rop.value = 0;
	cmdq_pkt_cond_jump(pkt, CMDQ_THR_SPR_IDX3, &lop, &rop, CMDQ_EQUAL);
	// 8. Write DS ${domain} all index script (write reg and jump to PC)
	for (ds_idx = 0; ds_idx < DS_MAX_INDEX; ds_idx++) {
		for (write_idx = 0; write_idx < depth - 1; write_idx++)
			cmdq_pkt_write(pkt, NULL, dummy_reg, ds_idx, ~0);

		cmdq_pkt_jump_addr(pkt, pkt_start_pa);
	}

	cmdq_dump_pkt(pkt, 0, true);
	//Flush packet.
	pkt->no_irq = true;
//	cmdq_pkt_flush(pkt);
	cmdq_pkt_flush_async(pkt, NULL, NULL);

	return 0;
}

static void cmdq_test_dynamicscaling(
	struct cmdq_test *test, const s32 secure, const bool need_mask)
{
	const int ds_idx_reg = 0x14A8B90; // force ip idx BKA54564
	const int ds_idx_force_en_reg = 0x14A8B94; // force ip idx en BKA54565
	const int ori_ds_idx_sel_reg = 0x1463E7C; // ds idx sel BKA31F1F
	const int ds_idx_sel_reg = 0x14A8A7C; // ds idx sel BKA5451F
	const int ip_depth_reg = 0x14A8AC8; // op2 depth BKA54532
	const int ip_enable_reg = 0x14A8AC0; // op2 enable BKA54530
	const int opf_depth_reg = 0x14A8AE8; // opf depth BKA5453A
	const int opf_enable_reg = 0x14A8AE0; // opf enable BKA54538
	const int opb_depth_reg = 0x14A8B08; // opb depth BKA54542
	const int opb_enable_reg = 0x14A8B00; // opb enable BKA54540
	const int op2_depth_reg = 0x14A8B28; // op2 depth BKA5454A
	const int op2_enable_reg = 0x14A8B20; // op2 enable BKA54548
	unsigned int val = 0;
	unsigned int sel = 0x44;
	unsigned int ds_idx_test_run = (DS_MAX_INDEX << 8);
	struct cmdq_client	*cltIP = secure ? test->sec : test->cltDsIP;
	struct cmdq_client	*cltOPF = secure ? test->sec : test->cltDsOPF;
	struct cmdq_client	*cltOPB = secure ? test->sec : test->cltDsOPB;
	struct cmdq_client	*cltOP2 = secure ? test->sec : test->cltDsOP2;
	struct cmdq_pkt *pktIP = NULL, *pktOPF = NULL, *pktOPB = NULL, *pktOP2 = NULL;

	cmdq_log_ex("sec:%d cltIP:%p cltOPF:%p cltOPB:%p cltOP2:%p",
		secure, cltIP, cltOPF, cltOPB, cltOP2);

	// enable ds
	_cmdq_test_read_reg(ori_ds_idx_sel_reg, &sel);

	_cmdq_test_write_reg(ds_idx_sel_reg, sel);
	_cmdq_test_write_reg(ip_depth_reg, IP_DS_DEPTH);
	_cmdq_test_write_reg(opf_depth_reg, IP_DS_DEPTH);
	_cmdq_test_write_reg(opb_depth_reg, IP_DS_DEPTH);
	_cmdq_test_write_reg(op2_depth_reg, OP2_DS_DEPTH);
	_cmdq_test_write_reg(ds_idx_reg, 0);
	_cmdq_test_write_reg(ds_idx_force_en_reg, 0x5);
	_cmdq_test_write_reg(ip_enable_reg, 0x211);
	_cmdq_test_write_reg(opf_enable_reg, 0x211);
	_cmdq_test_write_reg(opb_enable_reg, 0x211);
	_cmdq_test_write_reg(op2_enable_reg, 0x11);

	cmdq_mbox_enable(test->cltDsIP->chan);
	cmdq_mbox_enable(test->cltDsOPF->chan);
	cmdq_mbox_enable(test->cltDsOPB->chan);
	cmdq_mbox_enable(test->cltDsOP2->chan);

	CMDQ_CLK_PREPARE_ENABLE(test->gce.clk);

	pktIP = cmdq_pkt_create(cltIP);
	if (_cmdq_create_ds_domain_script(pktIP, DS_DOMAIN_IP) == -1) {
		cmdq_log_ex("create DS IP pkt fail\n");
		goto EXIT;
	}

	pktOPF = cmdq_pkt_create(cltOPF);
	if (_cmdq_create_ds_domain_script(pktOPF, DS_DOMAIN_OPF) == -1) {
		cmdq_log_ex("create DS OPF pkt fail\n");
		goto EXIT;
	}

	pktOPB = cmdq_pkt_create(cltOPB);
	if (_cmdq_create_ds_domain_script(pktOPB, DS_DOMAIN_OPB) == -1) {
		cmdq_log_ex("create DS OPB pkt fail\n");
		goto EXIT;
	}

	pktOP2 = cmdq_pkt_create(cltOP2);
	if (_cmdq_create_ds_domain_script(pktOP2, DS_DOMAIN_OP2) == -1) {
		cmdq_log_ex("create DS OP2 pkt fail\n");
		goto EXIT;
	}

	cmdq_log_ex("enable ds end and start waiting\n");

	// wait until user disable dbg dummy registers
	do {
		msleep(500);
		val += 0x100;
		_cmdq_test_write_reg(ds_idx_reg, val);
	} while (val < ds_idx_test_run);

	cmdq_log_ex("waiting done\n");

EXIT:
	cmdq_pkt_destroy(pktIP);
	cmdq_pkt_destroy(pktOPF);
	cmdq_pkt_destroy(pktOPB);
	cmdq_pkt_destroy(pktOP2);

	CMDQ_CLK_DISABLE_UNPREPARE(test->gce.clk);

	cmdq_mbox_disable(test->cltDsIP->chan);
	cmdq_mbox_disable(test->cltDsOPF->chan);
	cmdq_mbox_disable(test->cltDsOPB->chan);
	cmdq_mbox_disable(test->cltDsOP2->chan);
}

static void
cmdq_test_trigger(struct cmdq_test *test, enum CMDQ_SECURE_STATE_ENUM sec, const s32 id)
{
	bool bsec = false;
	if (sec < CMDQ_MTEE_STATE || sec > CMDQ_TEE_STATE) {
		cmdq_err("invalid input");
		return;
	}

	if (sec == CMDQ_TEE_STATE || sec == CMDQ_MTEE_STATE)
		bsec = true;

#ifndef CMDQ_SECURE_SUPPORT
	if (bsec) {
		cmdq_err("CMDQ_SECURE not support");
		return;
	}
#endif
#ifndef CMDQ_GP_SUPPORT
	if (sec == CMDQ_TEE_STATE) {
		cmdq_err("%s sec:%d, don't support cmdq tee driver", __func__, sec);
		return;
	}
#endif

	cmdq_mbox_enable(test->clt->chan);
	if (test->loop)
		cmdq_mbox_enable(test->loop->chan);
#ifdef CMDQ_SECURE_SUPPORT
	if (test->sec)
		cmdq_sec_mbox_enable(test->sec->chan);
#endif

	switch (id) {
	case 0:
		cmdq_test_mbox_write(test, sec, false);
		cmdq_test_mbox_write(test, sec, true);
		cmdq_test_mbox_flush(test, sec, false);
		cmdq_test_mbox_flush(test, sec, true);
		cmdq_test_mbox_polling(test, bsec, CMDQ_POLL, false);
		cmdq_test_mbox_dma_access(test, bsec);
		cmdq_test_mbox_gpr_sleep(test, false);
		cmdq_test_mbox_gpr_sleep(test, true);
		cmdq_test_mbox_loop(test);
		cmdq_test_mbox_large_cmd(test, 239);
		cmdq_test_mbox_cpr(test);
		break;
	case 1:
		cmdq_log_ex("start cmdq_test_mbox_write");
		cmdq_test_mbox_write(test, sec, false);
		cmdq_test_mbox_write(test, sec, true);
		break;
	case 2:
		cmdq_test_mbox_flush(test, bsec, false);
		cmdq_test_mbox_flush(test, bsec, true);
		break;
	case 3:
		cmdq_msg("%s start pkt_poll", __func__);
		cmdq_test_mbox_polling(test, bsec, CMDQ_POLL, false);
		cmdq_msg("%s start pkt_poll_timeout", __func__);
		cmdq_test_mbox_polling(test, bsec, CMDQ_POLL_TIMEOUT, false);
		cmdq_msg("%s start pkt_poll_sleep", __func__);
		cmdq_test_mbox_polling(test, bsec, CMDQ_POLL_SLEEP, false);
		break;
	case 4:
		cmdq_test_mbox_dma_access(test, bsec);
		break;
	case 5:
		cmdq_log_ex("start cmdq_test_mbox_gpr_sleep(test, false)");
		cmdq_test_mbox_gpr_sleep(test, false);
		cmdq_log_ex("start cmdq_test_mbox_gpr_sleep(test, true)");
		cmdq_test_mbox_gpr_sleep(test, true);
		break;
	case 6:
		cmdq_test_mbox_loop(test);
		break;
	case 7:
		cmdq_test_mbox_large_cmd(test, 237);
		break;
	case 8:
		cmdq_test_mbox_cpr(test);
		break;
	case 9:
		cmdq_test_mbox_err_dump(test, bsec, true);
		cmdq_test_mbox_err_dump(test, bsec, false);
		break;
	case 10:
		cmdq_test_mbox_handshake_event(test);
		break;
	case 11:
		cmdq_test_mbox_subsys_access(test);
		break;
	case 12:
		cmdq_test_err_irq(test);
		break;
	case 13:
		cmdq_test_devapc_vio(test);
		break;
	case 14:
		cmdq_test_mbox_polling(test, bsec, CMDQ_POLL_TIMEOUT, true);
		break;
	case 15:
		cmdq_test_mbox_stop(test);
		break;
	case 16:
		cmdq_test_show_events(test);
		break;
	case 17:
		cmdq_test_mbox_reuse_buf_va(test);
		break;
	case 18:
		cmdq_test_mbox_prebuilt_instr_ext(test,
			CMDQ_PREBUILT_MDP, CMDQ_TOKEN_PREBUILT_MDP_WAIT);
		cmdq_test_mbox_prebuilt_instr_ext(test,
			CMDQ_PREBUILT_MML, CMDQ_TOKEN_PREBUILT_MML_WAIT);
		cmdq_test_mbox_prebuilt_instr(test,
			CMDQ_PREBUILT_VFMT, CMDQ_TOKEN_PREBUILT_VFMT_WAIT);
		break;
	case 19:
		cmdq_test_mbox_prebuilt(test, CMDQ_PREBUILT_DISP, 0, false);
		cmdq_test_mbox_prebuilt(test, CMDQ_PREBUILT_DISP, 1, false);
		cmdq_test_mbox_prebuilt(test, CMDQ_PREBUILT_DISP, 1, true);
		break;
	case 20:
		cmdq_test_mbox_tzmp(test, sec, false);
		break;
	case 21:
		cmdq_util_test_set_ostd();
		cmdq_test_mbox_write_dma(test, bsec, 10);
		break;
	case 22:
		cmdq_test_mbox_vcp(test, false);
		cmdq_test_mbox_vcp(test, true);
		break;
	case 23:
		cmdq_test_mbox_write_dma_cpr(test, bsec, 3);
		break;
	case 24:
		cmdq_log_ex("start cmdq_test_tf");
		cmdq_test_tf(test);
		break;
	case 25:
		cmdq_log_ex("start cmdq_test_mbox_access_illegal");
		cmdq_test_mbox_access_illegal(test);
		break;
	case 26:
		cmdq_log_ex("start cmdq_test_sec_reg");
		cmdq_test_sec_reg(test);
		break;
	case 27:
		//STR POC
		cmdq_log_ex("start cmdq_test_str_ldr");
		cmdq_test_str_ldr(test);
		break;
	case 28:
		cmdq_log_ex("start cmdq_test_wait_event CMDQ_EVENT_REG_SW_DUMMY_0");
		cmdq_test_wait_event(test);
		break;
	case 29:
		cmdq_log_ex("start cmdq_test_mbox_write true only");
		cmdq_test_mbox_write(test, sec, true);
		break;
	case 30:
		cmdq_log_ex("start cmdq_test_menuload");
		cmdq_test_menuload_write(test, sec, false);
	break;
	case 31:
		cmdq_log_ex("start cmdq_test_sc_frc_register");
		cmdq_test_sc_frc_register(test);
		break;
	case 32:
		cmdq_log_ex("start cmdq_hw wait event test");
		cmdq_test_cmdqhw_wait_event(test);
		break;
	case 33:
		cmdq_log_ex("start multi_packet_in_one_thread");
		cmdq_test_multi_packet_in_one_thread(test);
		break;
	case 34:
		cmdq_log_ex("start cmdq_test_dynamicscaling");
		cmdq_test_dynamicscaling(test, sec, false);
		break;
	default:
		break;
	}

#ifdef CMDQ_SECURE_SUPPORT
	if (test->sec)
		cmdq_sec_mbox_disable(test->sec->chan);
#endif
	cmdq_mbox_disable(test->clt->chan);
	if (test->loop) {
		struct cmdq_thread	*thread =
			(struct cmdq_thread *)test->loop->chan->con_priv;
		s32 backup = cmdq_thread_timeout_backup(thread, CMDQ_NO_TIMEOUT);

		cmdq_mbox_disable(test->loop->chan);
		cmdq_thread_timeout_restore(thread, backup);
	}
}

#define MAX_SCAN 30

static ssize_t
cmdq_test_write(struct file *filp, const char *buf, size_t count, loff_t *offp)
{
	struct cmdq_test *test = (struct cmdq_test *)filp->f_inode->i_private;
	char		str[MAX_SCAN] = {0};
	s32		sec, id = 0;
	u32		len;

	len = (count < MAX_SCAN - 1) ? count : (MAX_SCAN - 1);
	if (copy_from_user(str, buf, len)) {
		cmdq_err("copy_from_user failed len:%d", len);
		return count;
	}
	str[len] = '\0';

	if (sscanf(str, "%d %d", &sec, &id) != 2) {
		cmdq_err("sscanf failed str:%s sec:%d id:%d", str, sec, id);
		return count;
	}
	cmdq_msg("test:%p len:%d sec:%d id:%d str:%s", test, len, sec, id, str);

	mutex_lock(&test->lock);
	cmdq_test_trigger(test, sec, id);
	mutex_unlock(&test->lock);
	return count;
}

static const struct file_operations cmdq_test_fops = {
	.write = cmdq_test_write,
};

static int cmdq_test_probe(struct platform_device *pdev)
{
	struct cmdq_test	*test;
	struct device_node	*np;
	struct platform_device	*np_pdev;
	struct resource		res;
	struct dentry		*dir;
	s32			i, ret;

	test = devm_kzalloc(&pdev->dev, sizeof(*test), GFP_KERNEL);
	if (!test)
		return -ENOMEM;
	test->dev = &pdev->dev;
	mutex_init(&test->lock);
	test->tick = false;
	gtest = test;

	// gce
	np = of_parse_phandle(pdev->dev.of_node, "mediatek,gce", 0);
	if (!np) {
		cmdq_err("of_parse_phandle mediatek,gce failed");
		return -EINVAL;
	}

	np_pdev = of_find_device_by_node(np);
	of_node_put(np);
	if (!np_pdev)
		return -EINVAL;
	test->gce.dev = &np_pdev->dev;

	test->gce.va = of_iomap(np_pdev->dev.of_node, 0);
	if (!test->gce.va)
		return -EINVAL;

	ret = of_address_to_resource(np_pdev->dev.of_node, 0, &res);
	if (ret)
		return ret;
	test->gce.pa = res.start - RIU_PM_OFFSET;
#if CMDQ_ALWAYS_ON_SUPPORT
	test->gce.clk = devm_clk_get(&np_pdev->dev, "gce");
	if (IS_ERR(test->gce.clk)) {
		cmdq_err("devm_clk_get gce clk failed:%ld",
			PTR_ERR(test->gce.clk));
		test->gce.clk = NULL;
	}

	test->gce.clk_timer = devm_clk_get(&np_pdev->dev, "gce-timer");
	if (IS_ERR(test->gce.clk_timer)) {
		cmdq_err("devm_clk_get gce clk_timer failed:%ld",
			PTR_ERR(test->gce.clk_timer));
		test->gce.clk_timer = NULL;
	}
#else
	test->gce.clk = NULL;
	test->gce.clk_timer = NULL;
#endif
	cmdq_msg("gce dev:%p va:%p pa:%pa",
		test->gce.dev, test->gce.va, &test->gce.pa);

	// clt
	test->clt = cmdq_mbox_create(&pdev->dev, 0);
	if (IS_ERR(test->clt) || !test->clt) {
		if (!test->clt)
			return -ENXIO;
	}

	test->loop = cmdq_mbox_create(&pdev->dev, 1);
	/*
	 *if (IS_ERR(test->loop) || !test->loop) {
	 *	if (!test->loop)
	 *		return -ENXIO;
	 *}
	 */
	// clt2
	test->clt2 = cmdq_mbox_create(&pdev->dev, 2);
	if (IS_ERR(test->clt2) || !test->clt2) {
		if (!test->clt2)
			cmdq_err("no test->clt2");
	}

#ifdef CMDQ_SECURE_SUPPORT
	test->sec = cmdq_mbox_create(&pdev->dev, 3);
	if (IS_ERR(test->sec) || !test->sec) {
		if (!test->sec)
			cmdq_err("no test->sec");
	}
#endif

	// cltDsIP
	test->cltDsIP = cmdq_mbox_create(&pdev->dev, 4);
	if (IS_ERR(test->cltDsIP) || !test->cltDsIP) {
		if (!test->cltDsIP)
			cmdq_err("no test->cltDsIP");
	}

	// cltDsOPF
	test->cltDsOPF = cmdq_mbox_create(&pdev->dev, 5);
	if (IS_ERR(test->cltDsOPF) || !test->cltDsOPF) {
		if (!test->cltDsOPF)
			cmdq_err("no test->cltDsOPF");
	}

	// cltDsOPB
	test->cltDsOPB = cmdq_mbox_create(&pdev->dev, 6);
	if (IS_ERR(test->cltDsOPB) || !test->cltDsOPB) {
		if (!test->cltDsOPB)
			cmdq_err("no test->cltDsOPB");
	}

	// cltDsOP2
	test->cltDsOP2 = cmdq_mbox_create(&pdev->dev, 7);
	if (IS_ERR(test->cltDsOP2) || !test->cltDsOP2) {
		if (!test->cltDsOP2)
			cmdq_err("no test->cltDsOP2");
	}

	cmdq_msg("%s %s:%p %s:%p %s:%p %s:%p %s:%p %s:%p %s:%p %s:%p %s:%p %s:%p",
		__func__, "test", test, "dev", test->dev, "clt", test->clt, "clt2",
		test->clt2, "loop", test->loop, "sec", test->sec, "cltDsIP", test->cltDsIP,
		"cltDsOPF", test->cltDsOPF, "cltDsOPB", test->cltDsOPB, "cltDsOP2",
		test->cltDsOP2);

	// subsys
	i = of_property_count_u32_elems(
		pdev->dev.of_node, "mediatek,gce-subsys");
	if (i < 0) {
		cmdq_err("of_property_count_u32_elems gce-subsys failed:%d", i);
		for (i = 0; i < CMDQ_TEST_SUBSYS_NR; i++)
			test->subsys[i] = CMDQ_TEST_SUBSYS_ERR;
	} else {
		ret = of_property_read_u32_array(pdev->dev.of_node,
			"mediatek,gce-subsys", test->subsys, i);
		if (ret) {
			cmdq_err("of_property_read_u32_array failed:%d", ret);
			for (i = 0; i < CMDQ_TEST_SUBSYS_NR; i++)
				test->subsys[i] = CMDQ_TEST_SUBSYS_ERR;
		}
	}
	for (i = 0; i < CMDQ_TEST_SUBSYS_NR; i++)
		cmdq_msg("subsys[%d]:%u", i, test->subsys[i]);

	ret = of_property_read_u16(pdev->dev.of_node, "token-user0",
		&test->token_user0);
	if (ret < 0) {
		cmdq_err("no token_user0 err:%d", ret);
		test->token_user0 = CMDQ_EVENT_MAX;
	}

	ret = of_property_read_u16(pdev->dev.of_node, "token-gpr-set4",
		&test->token_gpr_set4);
	if (ret < 0) {
		cmdq_err("no token-gpr-set4 err:%d", ret);
		test->token_gpr_set4 = CMDQ_EVENT_MAX;
	}

	// fs
	dir = debugfs_lookup("cmdq", NULL);
	if (!dir) {
		dir = debugfs_create_dir("cmdq", NULL);
		if (IS_ERR(dir) && PTR_ERR(dir) != -EEXIST) {
			cmdq_err("debugfs_create_dir cmdq failed:%ld", PTR_ERR(dir));
			return PTR_ERR(dir);
		}
	}

	test->fs = debugfs_create_file(
		"cmdq-test", 0444, dir, test, &cmdq_test_fops);
	if (IS_ERR(test->fs)) {
		cmdq_err("debugfs_create_file cmdq-test failed:%ld",
			PTR_ERR(test->fs));
		return PTR_ERR(test->fs);
	}

	platform_set_drvdata(pdev, test);

	return 0;
}

static int cmdq_test_remove(struct platform_device *pdev)
{
	struct cmdq_test *test = (struct cmdq_test *)platform_get_drvdata(pdev);

	debugfs_remove(test->fs);

	cmdq_mbox_destroy(test->clt);
	cmdq_mbox_destroy(test->clt2);
	cmdq_mbox_destroy(test->loop);
#ifdef CMDQ_SECURE_SUPPORT
	cmdq_mbox_destroy(test->sec);
#endif
	cmdq_mbox_destroy(test->cltDsIP);
	cmdq_mbox_destroy(test->cltDsOPF);
	cmdq_mbox_destroy(test->cltDsOPB);
	cmdq_mbox_destroy(test->cltDsOP2);

	return 0;
}

static const struct of_device_id cmdq_test_of_ids[] = {
	{
		.compatible = "mediatek,cmdq-test",
	},
	{}
};
MODULE_DEVICE_TABLE(of, cmdq_test_of_ids);

static struct platform_driver cmdq_test_drv = {
	.probe = cmdq_test_probe,
	.remove = cmdq_test_remove,
	.driver = {
		.name = "cmdq-test",
		.of_match_table = cmdq_test_of_ids,
	},
};
module_platform_driver(cmdq_test_drv);

MODULE_LICENSE("GPL v2");
