create a simple CPIO parser for initrd's

4 files changed, 123 insertions, 1 deletions

diff --git a/include/common.h b/include/common.h
index 88ee8c4..d26b8ae 100644
--- a/include/common.h
+++ b/include/common.h
@@ -16,6 +16,19 @@ static inline int strcmp(const char *str1, const char* str2) {
     return *(const unsigned char *)str1 - *(const unsigned char *)str2;
 }
 
+static inline int memcmp(const void *s1, const void *s2, size_t n) {
+    const unsigned char *p1 = (const unsigned char *)s1;
+    const unsigned char *p2 = (const unsigned char *)s2;
+
+    for (size_t i = 0; i < n; i++) {
+        if (p1[i] != p2[i]) {
+            return p1[i] - p2[i];
+        }
+    }
+
+    return 0;
+}
+
 static inline void *memcpy(void *restrict dest, const void* restrict src, size_t size) {
     unsigned char *d = (unsigned char *)dest;
     unsigned char *s = (unsigned char *)src;
diff --git a/include/cpio.h b/include/cpio.h
new file mode 100644
index 0000000..3bb40c0
--- /dev/null
+++ b/include/cpio.h
@@ -0,0 +1,23 @@
+#pragma once
+#include <types.h>
+#define CPIO_HEADER_LEN 110
+
+struct cpio_newc_hdr {
+    char magic[6];
+    char ino[8];
+    char mode[8];
+    char uid[8];
+    char gid[8];
+    char nlink[8];
+    char mtime[8];
+    char filesize[8];
+    char devmajor[8];
+    char devminor[8];
+    char rdevmajor[8];
+    char rdevminor[8];
+    char namesize[8];
+    char check[8];
+};
+
+void *cpio_get_file(void *archive, size_t size, const char *filename);
+
diff --git a/kernel/cpio.c b/kernel/cpio.c
new file mode 100644
index 0000000..565b073
--- /dev/null
+++ b/kernel/cpio.c
@@ -0,0 +1,73 @@
+#include <cpio.h>
+#include <types.h>
+#include <common.h>
+#include <log.h>
+
+static uint32_t parse_hex(const char *p)
+{
+    uint32_t value = 0;
+
+    for (int i = 0; i < 8; i++) {
+        char c = p[i];
+        value <<= 4;
+
+        if (c >= '0' && c <= '9')
+            value |= c - '0';
+        else if (c >= 'A' && c <= 'F')
+            value |= c - 'A' + 10;
+        else if (c >= 'a' && c <= 'f')
+            value |= c - 'a' + 10;
+    }
+    return value;
+}
+
+static size_t align4(size_t x)
+{
+    return (x + 3) & ~3;
+}
+
+void *cpio_get_file(void *archive, size_t size, const char *filename) {
+    const uint8_t *p = (uint8_t *)archive;
+    const uint8_t *end = archive + size;
+
+    while (p + CPIO_HEADER_LEN <= end) {
+        const struct cpio_newc_hdr *hdr =
+            (const struct cpio_newc_hdr *)p;
+
+        if (memcmp(hdr->magic, "070701", 6) &&
+            memcmp(hdr->magic, "070702", 6)) {
+            printk("Error parsing cpio: bad magic number!\n");
+            return NULL;
+        }
+
+        uint32_t namesize = parse_hex(hdr->namesize);
+        uint32_t filesize = parse_hex(hdr->filesize);
+
+        p += CPIO_HEADER_LEN;
+
+        if (p + namesize > end) {
+            printk("Error parsing cpio: truncated filename\n");
+            return NULL;
+        }
+
+        const char *name = (const char *)p;
+
+        if (strcmp(name, "TRAILER!!!") == 0)
+            return 0;
+
+        p += align4(namesize);
+
+        if (p + filesize > end) {
+            printk("Error parsing cpio: truncated file\n");
+            return NULL;
+        }
+
+        if (strcmp(name, filename) == 0) {
+            return (void*)p;
+        }
+
+        p += align4(filesize);
+    }
+    return NULL;
+}
+
diff --git a/kernel/kernel.c b/kernel/kernel.c
index e41d239..de556e7 100644
--- a/kernel/kernel.c
+++ b/kernel/kernel.c
@@ -2,6 +2,7 @@
 #include <dtb.h>
 #include <bump.h>
 #include <log.h>
+#include <cpio.h>
 
 #define KERNEL_SIZE 3 * 1024 * 1024
 
@@ -24,7 +25,19 @@ void kernel_main(size_t hart, void *fdt) {
 
     km_init(info);
 
-    printk("kernel init was successful\n");
+    printk("memory blocks initialized\n");
+
+    size_t len = 0;
+    uint32_t *initrd = dt_get_prop(head, "initrd", &len);
+    if (!initrd || len < 8) {
+        printk("failed to find initrd!\n");
+        return;
+    }
+    uint32_t base = btohi(initrd[0]);
+    uint32_t size = btohi(initrd[1]);
+    
+    char *file = cpio_get_file((void*)base, (size_t)size, "/sbin/init");
+    (void)file;
 }
 
 void kernel_trap() {