4 files changed, 72 insertions, 3 deletions
diff --git a/src/soc/mediatek/mt8196/Makefile.mk b/src/soc/mediatek/mt8196/Makefile.mk
index b11778f066..388dad14c4 100644
--- a/src/soc/mediatek/mt8196/Makefile.mk
+++ b/src/soc/mediatek/mt8196/Makefile.mk
@@ -6,7 +6,7 @@ all-y += ../common/flash_controller.c
 all-y += ../common/gpio.c ../common/gpio_op.c gpio.c gpio_eint.c
 all-y += ../common/i2c.c i2c.c
 all-$(CONFIG_SPI_FLASH) += spi.c
-all-y += timer.c
+all-y += timer.c timer_prepare.c
 all-y += ../common/uart.c
 
 bootblock-y += bootblock.c
diff --git a/src/soc/mediatek/mt8196/include/soc/timer.h b/src/soc/mediatek/mt8196/include/soc/timer.h
index d6422c5442..f6841fc250 100644
--- a/src/soc/mediatek/mt8196/include/soc/timer.h
+++ b/src/soc/mediatek/mt8196/include/soc/timer.h
@@ -2,12 +2,44 @@
 
 /*
  * This file is created based on MT8196 Functional Specification
- * Chapter number: 5.13
+ * Chapter number: 1.2 2.2
  */
 
 #ifndef SOC_MEDIATEK_MT8196_TIMER_H
 #define SOC_MEDIATEK_MT8196_TIMER_H
 
 #include <soc/timer_v2.h>
+#include <stdint.h>
+
+DEFINE_BITFIELD(COMP_FEATURE, 12, 10)
+DEFINE_BITFIELD(COMP_FEATURE_TIE, 4, 3)
+DEFINE_BITFIELD(REV_SET, 18, 17)
+DEFINE_BITFIELD(SYST_CON, 4, 0)
+
+#define SYST_CON_EN			BIT(0)
+#define SYST_CON_IRQ_CLR		BIT(4)
+#define REV_CLR_EN			0x3
+#define COMP_FEATURE_20_EN		0x2
+#define COMP_FEATURE_TIE_EN		0x1
+#define COMP_FEATURE_CLR                0
+#define COMP_FEATURE_TIE_CLR		0
+#define SYSTIMER_CNT			8
+#define SYST_CON_CLR			0
+
+struct systimer {
+	u32 cntcr;
+	u32 reserved;
+	u32 cntcv_l;
+	u32 cntcv_h;
+	u32 reserved1[0x30];
+	struct {
+		u32 con;
+		u32 val;
+	} cnttval[SYSTIMER_CNT];
+};
+
+check_member(systimer, cntcr, 0x0);
+check_member(systimer, cntcv_l, 0x0008);
+check_member(systimer, cntcv_h, 0x000c);
 
 #endif
diff --git a/src/soc/mediatek/mt8196/timer.c b/src/soc/mediatek/mt8196/timer.c
index 9d8dd7a9a6..bf3f3cee63 100644
--- a/src/soc/mediatek/mt8196/timer.c
+++ b/src/soc/mediatek/mt8196/timer.c
@@ -3,8 +3,11 @@
 #include <arch/lib_helpers.h>
 #include <commonlib/helpers.h>
 #include <delay.h>
+#include <soc/timer.h>
 
 void init_timer(void)
 {
-	raw_write_cntfrq_el0(13 * MHz);
+	timer_prepare();
+
+	raw_write_cntfrq_el0(GPT_MHZ * MHz);
 }
diff --git a/src/soc/mediatek/mt8196/timer_prepare.c b/src/soc/mediatek/mt8196/timer_prepare.c
new file mode 100644
index 0000000000..08496e83d8
--- /dev/null
+++ b/src/soc/mediatek/mt8196/timer_prepare.c
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+
+/*
+ * This file is created based on MT8196 Functional Specification
+ * Chapter number: 1.2 2.2
+ */
+
+#include <device/mmio.h>
+#include <soc/addressmap.h>
+#include <soc/timer.h>
+
+static void clear_systimer(struct systimer *const mtk_systimer)
+{
+	unsigned int id = 0;
+
+	for (id = 0; id < SYSTIMER_CNT; id++) {
+		u32 *cnttval_con = &mtk_systimer->cnttval[id].con;
+		WRITE32_BITFIELDS(cnttval_con, SYST_CON, SYST_CON_EN);
+		SET32_BITFIELDS(cnttval_con, SYST_CON, SYST_CON_IRQ_CLR);
+		WRITE32_BITFIELDS(cnttval_con, SYST_CON, SYST_CON_CLR);
+	}
+
+	SET32_BITFIELDS(&mtk_systimer->cntcr, REV_SET, REV_CLR_EN);
+}
+
+void timer_prepare(void)
+{
+	struct systimer *mtk_systimer = (void *)SYSTIMER_BASE;
+
+	SET32_BITFIELDS(&mtk_systimer->cntcr,
+			COMP_FEATURE, COMP_FEATURE_CLR | COMP_FEATURE_20_EN,
+			COMP_FEATURE_TIE, COMP_FEATURE_TIE_CLR | COMP_FEATURE_TIE_EN);
+	clear_systimer(mtk_systimer);
+}