Start power stuff

1 files changed, 33 insertions, 3 deletions

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
 }