From 941a8f6fbd6d89c17b2ad8d834237e2bf228e4dd Mon Sep 17 00:00:00 2001
From: NIIBE Yutaka <gniibe@fsij.org>
Date: Wed, 15 Nov 2017 17:09:30 +0900
Subject: [PATCH] Add chx_sleep_mode.

---
 ChangeLog           |  6 +++++
 chopstx-cortex-m.c  | 64 +++++++++++++++++++++++++++++++++++++++++++++
 chopstx-gnu-linux.c |  5 ++++
 chopstx.c           | 12 ++++++++-
 4 files changed, 86 insertions(+), 1 deletion(-)

diff --git a/ChangeLog b/ChangeLog
index e7cf4a1..c08b645 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,3 +1,9 @@
+2017-11-15  NIIBE Yutaka  <gniibe@fsij.org>
+
+	* chopstx.c (chopstx_conf_idle): Call chx_sleep_mode.
+	* chopstx-cortex-m.c (chx_sleep_mode): New.
+	* chopstx-gnu-linux.c (chx_sleep_mode): New.
+
 2017-11-15  NIIBE Yutaka  <gniibe@fsij.org>
 
 	* example-fsm-55/hh.c (main): Add call to chopstx_conf_idle.
diff --git a/chopstx-cortex-m.c b/chopstx-cortex-m.c
index 8795d5e..ffad3ec 100644
--- a/chopstx-cortex-m.c
+++ b/chopstx-cortex-m.c
@@ -704,3 +704,67 @@ svc (void)
 	: /* no output */ : "r" (tp) : "memory");
 }
 #endif
+
+struct SCB
+{
+  volatile uint32_t CPUID;
+  volatile uint32_t ICSR;
+  volatile uint32_t VTOR;
+  volatile uint32_t AIRCR;
+  volatile uint32_t SCR;
+  volatile uint32_t CCR;
+  volatile uint8_t  SHP[12];
+  volatile uint32_t SHCSR;
+  volatile uint32_t CFSR;
+  volatile uint32_t HFSR;
+  volatile uint32_t DFSR;
+  volatile uint32_t MMFAR;
+  volatile uint32_t BFAR;
+  volatile uint32_t AFSR;
+  volatile uint32_t PFR[2];
+  volatile uint32_t DFR;
+  volatile uint32_t ADR;
+  volatile uint32_t MMFR[4];
+  volatile uint32_t ISAR[5];
+  /* Cortex-M3 has more...  */
+};
+static struct SCB *const SCB = ((struct SCB *)0xE000ED00);
+#define SCB_SCR_SLEEPDEEP (1 << 2)
+
+struct PWR
+{
+  volatile uint32_t CR;
+  volatile uint32_t CSR;
+};
+static struct PWR *const PWR = ((struct PWR *)0x40007000);
+#define PWR_CR_LPDS 0x0001
+#define PWR_CR_PDDS 0x0002
+#define PWR_CR_CWUF 0x0004
+
+void
+chx_sleep_mode (int enable_sleep)
+{
+  if (enable_sleep == 0)
+    ;
+  else
+    {
+      if (enable_sleep == 1)
+	/* sleep only */
+	SCB->SCR &= ~SCB_SCR_SLEEPDEEP;
+      else
+	{
+	  PWR->CR |= PWR_CR_CWUF;
+	
+	  if (enable_sleep == 2 || enable_sleep == 3)
+	    {
+	      PWR->CR &= ~PWR_CR_PDDS;
+	      if (enable_sleep == 3)
+		PWR->CR |= PWR_CR_LPDS;
+	    }
+	  else /* enable_sleep == 4 */
+	    PWR->CR |= PWR_CR_PDDS;
+
+	  SCB->SCR |= SCB_SCR_SLEEPDEEP;
+	}
+    }
+}
diff --git a/chopstx-gnu-linux.c b/chopstx-gnu-linux.c
index 078f0db..32329ca 100644
--- a/chopstx-gnu-linux.c
+++ b/chopstx-gnu-linux.c
@@ -342,3 +342,8 @@ chopstx_create_arch (uintptr_t stack_addr, size_t stack_size,
   chx_cpu_sched_unlock ();
   return tp;
 }
+
+void
+chx_sleep_mode (int enable_sleep)
+{
+}
diff --git a/chopstx.c b/chopstx.c
index b6f3e1b..387847b 100644
--- a/chopstx.c
+++ b/chopstx.c
@@ -1481,7 +1481,16 @@ chopstx_setpriority (chopstx_prio_t prio_new)
  * chopstx_conf_idle - Configure IDLE thread
  * @enable_sleep: Enable sleep on idle or not
  *
- * If @enable_sleep is true, allow sleep for the idle thread.
+ * If @enable_sleep is > 0, allow sleep for the idle thread.
+ *
+ * Behavior of @enable_sleep >= 1 depends on MCU.
+ *
+ * For STM32F103, it's like following.
+ *    1: Sleep   (CPU clock OFF only)
+ *    2: Stop    Wakeup by EXTI  (voltage regulator on)
+ *    3: Stop    Wakeup by EXTI  (voltage regulator low-power)
+ *    4: Standby Wakeup by RESET (voltage regulator off)
+ *
  * Return previous value of @enable_sleep.
  */
 int
@@ -1492,6 +1501,7 @@ chopstx_conf_idle (int enable_sleep)
   chx_spin_lock (&chx_enable_sleep_lock);
   r = chx_allow_sleep;
   chx_allow_sleep = enable_sleep;
+  chx_sleep_mode (enable_sleep);
   chx_spin_unlock (&chx_enable_sleep_lock);
 
   return r;