From ce4e86f3806e599a2abf014b0b0fc85a5e6c4e37 Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Thu, 17 Feb 2022 08:38:56 -0700
Subject: Start power stuff

---
 include/symbols.h   | 14 --------------
 include/sys/power.h | 20 +++++++++++++++++++-
 src/sys/power.c     | 36 +++++++++++++++++++++++++++++++++---
 3 files changed, 52 insertions(+), 18 deletions(-)

diff --git a/include/symbols.h b/include/symbols.h
index 8c41c86..04d1d1b 100644
--- a/include/symbols.h
+++ b/include/symbols.h
@@ -108,20 +108,6 @@ enum
 	SYS_TIMER_SC_M1 = (1 << 1),
 	SYS_TIMER_SC_M2 = (1 << 2),
 	SYS_TIMER_SC_M3 = (1 << 3),
-
-	/* Power Management, Reset controller and Watchdog registers */
-	//BCM2835_PERI_BASE        = 0x3F000000,
-	BCM2835_PERI_BASE        = 0x20000000,
-	PM_BASE                  = (BCM2835_PERI_BASE + 0x100000), 
-	PM_RSTC                  = (PM_BASE+0x1c),
-	PM_WDOG                  = (PM_BASE+0x24),
-	PM_WDOG_RESET            = 0x00000000,
-	PM_PASSWORD              = 0x5a000000,
-	PM_WDOG_TIME_SET         = 0x000fffff,
-	PM_RSTC_WRCFG_CLR        = 0xffffffcf,
-	PM_RSTC_WRCFG_SET        = 0x00000030,
-	PM_RSTC_WRCFG_FULL_RESET = 0x00000020,
-	PM_RSTC_RESET            = 0x00000102,
 };
 
 #endif
diff --git a/include/sys/power.h b/include/sys/power.h
index 711842b..4a6eec9 100644
--- a/include/sys/power.h
+++ b/include/sys/power.h
@@ -1,6 +1,24 @@
 #ifndef SYS_POWER_H
 #define SYS_POWER_H
 
-void reboot(void);
+#define PM_RSTC 0x1c
+#define PM_RSTS 0x20
+#define PM_WDOG 0x24
+
+#define PM_PASSWORD       0x5a000000
+#define BCM2835_PERI_BASE 0x3F000000
+
+#define PM_WDOG_TIME_SET         0x000fffff
+#define PM_RSTC_WRCFG_CLR        0xffffffcf
+#define PM_RSTS_HADWRH_SET       0x00000040
+#define PM_RSTC_WRCFG_SET        0x00000030
+#define PM_RSTC_WRCFG_FULL_RESET 0x00000020
+#define PM_RSTC_RESET            0x00000102
+#define PM_RSTS_PARTITION_CLR    0xfffffaaa
+#define SECS_TO_WDOG_TICS(x) ((x) << 16)
+
+void wdt_start(void);
+void __bcm2835_restart(unsigned char);
+void bcm2835_power_off(void);
 
 #endif
diff --git a/src/sys/power.c b/src/sys/power.c
index 8c2f469..c4f12a9 100644
--- a/src/sys/power.c
+++ b/src/sys/power.c
@@ -2,8 +2,38 @@
 #include <sys/core.h>
 #include <sys/power.h>
 
-void reboot(void)
+//https://github.com/raspberrypi/linux/blob/aeaa2460db088fb2c97ae56dec6d7d0058c68294/drivers/watchdog/bcm2835_wdt.c
+void wdt_start(void)
 {
-	store32(PM_WDOG, PM_PASSWORD | 1);
-	store32(PM_RSTC, PM_PASSWORD | PM_RSTC_WRCFG_FULL_RESET);
+	store32(BCM2835_PERI_BASE + PM_WDOG, PM_PASSWORD | (SECS_TO_WDOG_TICS(100) & PM_WDOG_TIME_SET));
+	unsigned long cur = load32(BCM2835_PERI_BASE + PM_RSTC);
+	store32(BCM2835_PERI_BASE + PM_RSTC, PM_PASSWORD | (cur & PM_RSTC_WRCFG_CLR) | PM_RSTC_WRCFG_FULL_RESET);
+}
+
+void wdt_stop(void)
+{
+	store32(BCM2835_PERI_BASE + PM_RSTC, PM_PASSWORD | PM_RSTC_RESET);
+}
+
+void __bcm2835_restart(unsigned char partition)
+{
+	unsigned long val, rsts;
+	rsts = (partition & 1) | ((partition & 0b10) << 1) |
+		((partition & 0b100) << 2) | ((partition & 0b1000) << 3) |
+		((partition & 0b10000) << 4) | ((partition & 0b100000) << 5);
+	val = load32(BCM2835_PERI_BASE + PM_RSTS);
+	val &= PM_RSTS_PARTITION_CLR;
+	val |= PM_PASSWORD | rsts;
+	store32(BCM2835_PERI_BASE + PM_RSTS, val);
+	store32(BCM2835_PERI_BASE + PM_WDOG, 10 | PM_PASSWORD);
+	val = load32(BCM2835_PERI_BASE + PM_RSTC);
+	val &= PM_RSTC_WRCFG_CLR;
+	val |= PM_PASSWORD | PM_RSTC_WRCFG_FULL_RESET;
+	store32(BCM2835_PERI_BASE + PM_RSTC, val);
+	delay(1);
+}
+
+void bcm2835_power_off(void)
+{
+	__bcm2835_restart(63); // Partition 63 => Halt
 }
-- 
cgit v1.2.1