From 41c9b71450fae46ea414d6ec5a479600276dda8e Mon Sep 17 00:00:00 2001
From: Tim Lin <tim2.lin@ite.corp-partner.google.com>
Date: Fri, 8 Oct 2021 08:40:37 +0800
Subject: [PATCH] ITE: soc: add cpu idle task

Implement the CPU idle task. The system should enter this task when
there is no any task to ensure power saving.

Tested on it8xxx2_evb board. It will reduce 12.5mA when system enters
the CPU idle task.

Signed-off-by: Tim Lin <tim2.lin@ite.corp-partner.google.com>
---
 .../interrupt_controller/intc_ite_it8xxx2.c   |  2 +
 .../interrupt_controller/intc_ite_it8xxx2.h   |  3 ++
 .../it8xxx2/Kconfig.defconfig.series          |  3 ++
 soc/riscv/riscv-ite/it8xxx2/soc.c             | 53 ++++++++++++++++++-
 4 files changed, 59 insertions(+), 2 deletions(-)

diff --git a/drivers/interrupt_controller/intc_ite_it8xxx2.c b/drivers/interrupt_controller/intc_ite_it8xxx2.c
index 71f463e99d5..90b93451455 100644
--- a/drivers/interrupt_controller/intc_ite_it8xxx2.c
+++ b/drivers/interrupt_controller/intc_ite_it8xxx2.c
@@ -198,6 +198,8 @@ uint8_t get_irq(void *arg)
 	intc_irq -= IVECT_OFFSET_WITH_IRQ;
 	/* clear interrupt status */
 	ite_intc_isr_clear(intc_irq);
+	/* Clear flag on each interrupt. */
+	wait_interrupt_fired = 0;
 	/* return interrupt number */
 	return intc_irq;
 }
diff --git a/drivers/interrupt_controller/intc_ite_it8xxx2.h b/drivers/interrupt_controller/intc_ite_it8xxx2.h
index 8cef428b9e2..d64ed93fc86 100644
--- a/drivers/interrupt_controller/intc_ite_it8xxx2.h
+++ b/drivers/interrupt_controller/intc_ite_it8xxx2.h
@@ -9,4 +9,7 @@
 #include <dt-bindings/interrupt-controller/ite-intc.h>
 #include <soc.h>
 
+/* use data type int here not bool to get better instruction number. */
+volatile int wait_interrupt_fired;
+
 #endif /* ZEPHYR_DRIVERS_INTERRUPT_CONTROLLER_INTC_ITE_IT8XXX2_H_ */
diff --git a/soc/riscv/riscv-ite/it8xxx2/Kconfig.defconfig.series b/soc/riscv/riscv-ite/it8xxx2/Kconfig.defconfig.series
index 0c75b528fb0..c5147ad1d54 100644
--- a/soc/riscv/riscv-ite/it8xxx2/Kconfig.defconfig.series
+++ b/soc/riscv/riscv-ite/it8xxx2/Kconfig.defconfig.series
@@ -23,6 +23,9 @@ config UART_NS16550_WA_ISR_REENABLE_INTERRUPT
 	default y
 	depends on UART_NS16550
 
+config RISCV_HAS_CPU_IDLE
+	default y
+
 if ITE_IT8XXX2_INTC
 config NUM_IRQS
 	default 185
diff --git a/soc/riscv/riscv-ite/it8xxx2/soc.c b/soc/riscv/riscv-ite/it8xxx2/soc.c
index c30250a2ee5..a6d784caa13 100644
--- a/soc/riscv/riscv-ite/it8xxx2/soc.c
+++ b/soc/riscv/riscv-ite/it8xxx2/soc.c
@@ -5,6 +5,7 @@
  *
  */
 
+#include <arch/riscv/csr.h>
 #include <kernel.h>
 #include <device.h>
 #include <init.h>
@@ -51,9 +52,15 @@ static const struct pll_config_t pll_configuration[] = {
 
 void __intc_ram_code chip_pll_ctrl(enum chip_pll_mode mode)
 {
+	volatile uint8_t _pll_ctrl __unused;
+
 	IT8XXX2_ECPM_PLLCTRL = mode;
-	/* for deep doze / sleep mode */
-	IT8XXX2_ECPM_PLLCTRL = mode;
+	/*
+	 * for deep doze / sleep mode
+	 * This load operation will ensure PLL setting is taken into
+	 * control register before wait for interrupt instruction.
+	 */
+	_pll_ctrl = IT8XXX2_ECPM_PLLCTRL;
 }
 
 void __intc_ram_code chip_run_pll_sequence(const struct pll_config_t *pll)
@@ -132,6 +139,48 @@ static int chip_change_pll(const struct device *dev)
 SYS_INIT(chip_change_pll, POST_KERNEL, 0);
 #endif /* CONFIG_SOC_IT8XXX2_PLL_FLASH_48M */
 
+extern volatile int wait_interrupt_fired;
+
+static ALWAYS_INLINE void riscv_idle(unsigned int key)
+{
+	/* Disable M-mode external interrupt */
+	csr_clear(mie, MIP_MEIP);
+
+	sys_trace_idle();
+	/* Chip doze after wfi instruction */
+	chip_pll_ctrl(CHIP_PLL_DOZE);
+	/* Set flag before entering low power mode. */
+	wait_interrupt_fired = 1;
+	/* unlock interrupts */
+	irq_unlock(key);
+	/* Wait for interrupt */
+	__asm__ volatile ("wfi");
+
+	/* Enable M-mode external interrupt */
+	csr_set(mie, MIP_MEIP);
+	/*
+	 * Sometimes wfi instruction may fail due to CPU's MTIP@mip
+	 * register is non-zero.
+	 * If the wait_interrupt_fired flag is true at this point,
+	 * it means that EC waked-up by the above issue not an
+	 * interrupt. Hence we loop running wfi instruction here until
+	 * wfi success.
+	 */
+	while (wait_interrupt_fired) {
+		__asm__ volatile ("wfi");
+	}
+}
+
+void arch_cpu_idle(void)
+{
+	riscv_idle(MSTATUS_IEN);
+}
+
+void arch_cpu_atomic_idle(unsigned int key)
+{
+	riscv_idle(key);
+}
+
 static int ite_it8xxx2_init(const struct device *arg)
 {
 	ARG_UNUSED(arg);