1 files changed, 27 insertions, 0 deletions

diff --git a/src/raspi.c b/src/raspi.c
new file mode 100644
index 0000000..bc76f89
--- /dev/null
+++ b/src/raspi.c
@@ -0,0 +1,27 @@
+#include <raspi.h>
+
+#include <stdint.h>
+
+int raspi_init() {
+  int raspi;
+  uint32_t reg;
+
+  // Read the system register.
+#if __aarch64__
+  asm volatile ("mrs %x0, midr_el1" : "=r" (reg));
+#else
+  asm volatile ("mrc p15,0,%0,c0,c0,0" : "=r" (reg));
+#endif
+
+  // Get the PartNum and detect the board.
+  switch ((reg >> 4) & 0xFFF) {
+      case 0xB76: raspi = 1; break;
+      case 0xC07: raspi = 2; break;
+      case 0xD03: raspi = 3; break;
+      case 0xD08: raspi = 4; break;
+      default:    raspi = 0; break;
+  }
+  
+  return raspi;
+}
+