1 files changed, 23 insertions, 0 deletions

diff --git a/src/mmio.c b/src/mmio.c
new file mode 100644
index 0000000..e545517
--- /dev/null
+++ b/src/mmio.c
@@ -0,0 +1,23 @@
+#include <mmio.h>
+
+void* MMIO_BASE;
+
+void mmio_init(int raspi) {
+  switch (raspi) {
+    case 2:
+    case 3:  MMIO_BASE = (void*)0x3F000000; break; // raspi2 & 3
+    case 4:  MMIO_BASE = (void*)0xFE000000; break; // raspi4
+    default: MMIO_BASE = (void*)0x20000000; break; // raspi1, raspi zero, etc.
+  }
+}
+
+#define REG_ADDR(reg) ((volatile uint32_t*)(MMIO_BASE + reg))
+
+uint32_t mmio_read(uint32_t reg) {
+  return *REG_ADDR(reg);
+}
+
+void mmio_write(uint32_t reg, uint32_t val) {
+  *REG_ADDR(reg) = val;
+}
+