From a61201b8047ebe278cfb281723a4bf6c82556472 Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Thu, 6 Jan 2022 15:51:48 -0800
Subject: Scheduling

---
 include/cpu.h | 69 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 69 insertions(+)
 create mode 100644 include/cpu.h

(limited to 'include/cpu.h')

diff --git a/include/cpu.h b/include/cpu.h
new file mode 100644
index 0000000..c93b3b5
--- /dev/null
+++ b/include/cpu.h
@@ -0,0 +1,69 @@
+#ifndef CPU_H
+#define CPU_H
+
+static inline unsigned long getmode(void)
+{
+	unsigned long mode;
+	asm volatile ("mrs %0, cpsr" : "=r"(mode));
+	return mode;
+}
+
+static inline void setsvc(void)
+{
+	unsigned long mode;
+	asm volatile (
+	"mrs %0, cpsr\n"
+	"bic %0, %0, #0x1F\n"
+	"orr %0, %0, #0x13\n"
+	"msr cpsr_c, %0" 
+	: "=r"(mode));
+}
+
+static inline void setmode(unsigned long mode)
+{
+	asm volatile ("msr cpsr_c, %0" :: "r"(mode));
+}
+
+static inline void* getsvcstack(void)
+{
+	void* sp;
+	asm volatile (
+	"mrs r0, cpsr\n"
+	"bic r1, r0, #0x1F\n"
+	"orr r1, r1, #0x13\n"
+	"msr cpsr_c, r1\n"
+	"mov %0, sp\n"
+	"msr cpsr_c, r0"
+	: "=r"(sp));
+	return sp;
+}
+
+static inline void* getfiqstack(void)
+{
+	void* sp;
+	asm volatile (
+	"mrs r0, cpsr\n"
+	"bic r1, r0, #0x1F\n"
+	"orr r1, r1, #0x11\n"
+	"msr cpsr_c, r1\n"
+	"mov %0, sp\n"
+	"msr cpsr_c, r0"
+	: "=r"(sp));
+	return sp;
+}
+
+static inline void* getirqstack(void)
+{
+	void* sp;
+	asm volatile (
+	"mrs r0, cpsr\n"
+	"bic r1, r0, #0x1F\n"
+	"orr r1, r1, #0x12\n"
+	"msr cpsr_c, r1\n"
+	"mov %0, sp\n"
+	"msr cpsr_c, r0"
+	: "=r"(sp));
+	return sp;
+}
+
+#endif
-- 
cgit v1.2.1