From ed34174d6891c4b29c30a790b61d09d9ba472905 Mon Sep 17 00:00:00 2001
From: Xnoe <xnoe@xnoe.moe>
Date: Fri, 4 Mar 2022 20:25:17 +0000
Subject: [PATCH] Rename boot_stage2 to bootstage2

---
 src/bootstage2/atapio.c      | 122 +++++++++++++++++++++
 src/bootstage2/atapio.h      |  16 +++
 src/bootstage2/bootstage2.ld |  14 +++
 src/bootstage2/io.c          |  19 ++++
 src/bootstage2/io.h          |  12 +++
 src/bootstage2/main.c        | 199 +++++++++++++++++++++++++++++++++++
 src/bootstage2/paging.c      |  46 ++++++++
 src/bootstage2/paging.h      |  37 +++++++
 src/bootstage2/screenstuff.c | 165 +++++++++++++++++++++++++++++
 src/bootstage2/screenstuff.h |  19 ++++
 src/bootstage2/strings.c     |  69 ++++++++++++
 src/bootstage2/strings.h     |  11 ++
 src/bootstage2/types.h       |  11 ++
 13 files changed, 740 insertions(+)
 create mode 100644 src/bootstage2/atapio.c
 create mode 100644 src/bootstage2/atapio.h
 create mode 100644 src/bootstage2/bootstage2.ld
 create mode 100644 src/bootstage2/io.c
 create mode 100644 src/bootstage2/io.h
 create mode 100644 src/bootstage2/main.c
 create mode 100644 src/bootstage2/paging.c
 create mode 100644 src/bootstage2/paging.h
 create mode 100644 src/bootstage2/screenstuff.c
 create mode 100644 src/bootstage2/screenstuff.h
 create mode 100644 src/bootstage2/strings.c
 create mode 100644 src/bootstage2/strings.h
 create mode 100644 src/bootstage2/types.h

diff --git a/src/bootstage2/atapio.c b/src/bootstage2/atapio.c
new file mode 100644
index 0000000..c6e7745
--- /dev/null
+++ b/src/bootstage2/atapio.c
@@ -0,0 +1,122 @@
+#include "atapio.h"
+
+// Disk code
+
+// Primary ATA Bus: 0x1f0 to 0x1f7
+// Device Control Register: 0x3f6
+// Secondary ATA Bus: 0x170 to 0x177
+// Device Control Register: 0x376
+
+
+uint16_t identify_result[256];
+uint32_t total_28_lbas = 0;
+
+uint8_t* rootDirEntries = 0x1000000;
+uint16_t* FAT1 = 0x1002000;
+
+uint16_t countReserved;
+uint8_t countFATs;
+uint16_t countRDEs;
+uint16_t sectorsPerFAT;
+
+
+void init_atapio() {
+  countReserved = *(uint16_t*)0x7c0e;
+  countFATs = *(uint8_t*)0x7c10;
+  countRDEs = *(uint16_t*)0x7c11;
+  sectorsPerFAT = *(uint16_t*)0x7c16;
+
+  // Select Drive 0 on the Primary Bus
+  outb(0x1f6, 0xa0);
+
+  // Set sector count to 0 for IDENTIFY
+  outb(0x1f2, 0);
+  // Set LBAlo to 0
+  outb(0x1f3, 0);
+  // Set LBAmid to 0
+  outb(0x1f4, 0);
+  // Set LBAhi to 0
+  outb(0x1f5, 0);
+
+  // Send IDENTIFY command
+  outb(0x1f7, 0xec);
+
+  uint8_t status = inb(0x1f7);
+  if (status) {
+    while ((status = inb(0x1f7)) & 0x80);
+
+    if ( !(inb(0x1f4) || inb(0x1f5)) ) {
+      while ( !(status & 8 || status & 1) )
+        status = inb(0x1f7);
+      
+      if (!(status & 1)) {
+        for (int index=0; index<256; index++)
+          identify_result[index] = inw(0x1f0);
+      }
+    }
+  }
+
+  total_28_lbas = *(uint32_t*)(identify_result+60);
+
+  // We've initialised now, let's load the FAT and RootDirEntries.
+  read_sectors(sectorsPerFAT * countFATs + countReserved, countRDEs / 16, rootDirEntries);
+  read_sectors(countReserved, sectorsPerFAT, FAT1);
+}
+
+void read_sector(uint32_t address, uint8_t* buffer) {
+  outb(0x1f6, 0xe0 | ((address>>24)&0x0f));
+  // Read a single sector
+  outb(0x1f2, 1);
+  // Set LBAlo, LBAmid and LBAhi
+  outb(0x1f3, address);
+  outb(0x1f4, address>>8);
+  outb(0x1f5, address>>16);
+
+  // Send read command 
+  outb(0x1f7, 0x20);
+  // Poll
+  uint8_t status = inb(0x1f7);
+  while ( (status & 0x80) && !(status & 8) )
+    status = inb(0x1f7);
+
+  for (int index=0; index<256; index++)
+    ((uint16_t*)buffer)[index] = inw(0x1f0);
+}
+
+void read_sectors(uint32_t address, int count, uint8_t* buffer) {
+  for (int i=0; i<count; i++) {
+    read_sector(address+i, buffer+512*i);
+    for (int i=0; i<15; i++)
+      inb(0x1f7);
+  } 
+}
+
+uint16_t file_exists(char* filename) {
+  for (int i=0; i<countRDEs; i++) {
+    bool found = strcmp(rootDirEntries+(i*32), filename, 11);
+    if (found) {
+      uint16_t* correctEntry = (rootDirEntries+(i*32));
+      return correctEntry[13];
+    }
+  }
+  return 0;
+}
+
+void load_file(char* filename, uint8_t* destination) {
+  uint16_t location = file_exists(filename);
+  if (!location)
+    return;
+
+  int offset = 0;
+
+  bool loaded = false;
+  while (!loaded) {
+    uint16_t fromSector = location + (sectorsPerFAT * countFATs) + (countRDEs / 16) + (countReserved - 1) - 1;
+    read_sector(fromSector, destination+offset);
+    offset += 512;
+
+    location = FAT1[location++];
+    if (location == 0xffff)
+      loaded = true;
+  }
+}
\ No newline at end of file
diff --git a/src/bootstage2/atapio.h b/src/bootstage2/atapio.h
new file mode 100644
index 0000000..48de720
--- /dev/null
+++ b/src/bootstage2/atapio.h
@@ -0,0 +1,16 @@
+#ifndef ATA_PIO
+#define ATA_PIO
+
+#include <stdbool.h>
+
+#include "io.h"
+#include "types.h"
+#include "strings.h"
+
+void init_atapio();
+void read_sector(uint32_t address, uint8_t* buffer);
+void read_sectors(uint32_t address, int count, uint8_t* buffer);
+uint16_t file_exists(char* filename);
+void load_file(char* filename, uint8_t* location);
+
+#endif
\ No newline at end of file
diff --git a/src/bootstage2/bootstage2.ld b/src/bootstage2/bootstage2.ld
new file mode 100644
index 0000000..6205a6b
--- /dev/null
+++ b/src/bootstage2/bootstage2.ld
@@ -0,0 +1,14 @@
+OUTPUT_FORMAT(binary)
+OUTPUT_ARCH(i386:i386)
+
+OUTPUT(build/bootstage2/boot.bin)
+
+SECTIONS {
+  . = 0x7E00;
+
+  .text : {
+    build/c_code_entry.o(.text)
+    build/bootstage2/main.o(.text)
+    build/bootstage2/*(.text)
+  }
+}
\ No newline at end of file
diff --git a/src/bootstage2/io.c b/src/bootstage2/io.c
new file mode 100644
index 0000000..ae78e65
--- /dev/null
+++ b/src/bootstage2/io.c
@@ -0,0 +1,19 @@
+#include "io.h"
+
+void outb(uint16_t portnumber, uint8_t data) {
+  asm volatile("outb %0, %1" : : "a" (data), "Nd" (portnumber));
+}
+uint8_t inb(uint16_t portnumber) {
+  uint8_t result;
+  asm volatile("inb %1, %0" : "=a" (result) : "Nd" (portnumber));
+  return result;
+}
+
+void outw(uint16_t portnumber, uint16_t data) {
+  asm volatile("outw %0, %1" : : "a" (data), "Nd" (portnumber));
+}
+uint16_t inw(uint16_t portnumber) {
+  uint16_t result;
+  asm volatile("inw %1, %0" : "=a" (result) : "Nd" (portnumber));
+  return result;
+}
\ No newline at end of file
diff --git a/src/bootstage2/io.h b/src/bootstage2/io.h
new file mode 100644
index 0000000..8df8ec6
--- /dev/null
+++ b/src/bootstage2/io.h
@@ -0,0 +1,12 @@
+#ifndef IO_H
+#define IO_H
+
+#include "types.h"
+
+void outb(uint16_t portnumber, uint8_t data);
+uint8_t inb(uint16_t portnumber);
+
+void outw(uint16_t portnumber, uint16_t data);
+uint16_t inw(uint16_t portnumber);
+
+#endif
\ No newline at end of file
diff --git a/src/bootstage2/main.c b/src/bootstage2/main.c
new file mode 100644
index 0000000..d2d4231
--- /dev/null
+++ b/src/bootstage2/main.c
@@ -0,0 +1,199 @@
+#include "atapio.h"
+#include "screenstuff.h"
+#include "paging.h"
+
+typedef struct {
+  uint32_t base_low;
+  uint32_t base_high;
+  uint32_t length_low;
+  uint32_t length_high;
+  uint32_t type;
+  uint32_t acpi3_extended;
+}__attribute__((packed)) e820entry;
+
+uint8_t* bitmap = 0x100000;
+
+void set_bit(uint32_t offset, uint8_t* buffer) {
+  uint32_t index = offset / 8;
+  uint32_t bit = offset % 8;
+
+  buffer[index] |= (1<<(7 - bit));
+}
+
+void unset_bit(uint32_t offset, uint8_t* buffer) {
+  uint32_t index = offset / 8;
+  uint32_t bit = offset % 8;
+
+  buffer[index] &= (255 - (1<<(7 - bit)));
+}
+
+void memset(uint8_t* base, uint32_t count, uint8_t to) {
+  for (int i=0; i<count; i++) {
+    base[i] = to;
+  }
+}
+
+void mark_unavailble(uint32_t address, uint32_t size, uint8_t* buffer) {
+  // This function takes an address and length and marks the corresponding pages as unavailable.
+  address -= address % 4096; 
+  if (size % 4096)
+    size += 4096 - (size % 4096);
+
+  address /= 4096;
+  size /= 4096;
+
+  for (int i=0; i<size; i++) {
+    unset_bit(address + i, buffer);
+  }
+}
+
+char* stringify_type(uint32_t type) {
+  switch (type) {
+    case 1:
+      return "Usable";
+    case 3:
+      return "ACPI Reclaimable";
+    case 4:
+      return "ACPI NVS";
+    case 5:
+      return "Bad memory";
+    default:
+      return "Reserved";
+  }
+}
+
+void main() {
+  init_term();
+  init_atapio();
+
+  // e820 memory map exists at 0x20000
+  e820entry* e820_entries = 0x20000;
+
+  // Zero out the bitmap.
+  memset(bitmap, 0x20000, 0);
+  // Ensure the bitmap data is clear
+
+  for (int i=0; i<0x20000; i++)
+    if (bitmap[i])
+      printf("Found data in bitmap at %x!\n", (bitmap+i));
+
+  for (int i=0; e820_entries[i].length_low != 0 || e820_entries[i].length_high != 0; i++) {
+    e820entry entry = e820_entries[i];
+    printf("BIOS-e820: Starting %x%x, length %x%x is %s\n", entry.base_high, entry.base_low, entry.length_high, entry.length_low, stringify_type(entry.type));
+
+    if (entry.type != 1)
+      continue;
+    
+    uint32_t base = entry.base_low;
+    uint32_t length = entry.length_low;
+
+    if (base % 4096) { 
+      // Memory isn't page aligned, we need to fix that.
+      uint32_t add_offset = 4096 - (base % 4096);
+      if (length > add_offset) {
+        base += add_offset;
+        length -= add_offset;
+      }
+    }
+    uint32_t page_index = base / 4096;
+
+    printf("Page Index: %d\nLength (Pages): %d\n", page_index, length / 4096);
+
+    for (int j=0; length > 4096; length -= 4096, j++) {
+      set_bit(page_index + j, bitmap);
+    }
+  }
+
+  mark_unavailble(bitmap, 0x20000, bitmap);
+
+  // Page Directory
+  PDE* kernel_page_directory = bitmap + 0x20000;
+
+  // Clear the PD 
+  memset((uint8_t*)kernel_page_directory, 4096, 0);
+
+  // Clear 4MB of RAM from 0x121000 to 0x521000 for the 1024 page tables
+  memset((uint8_t*)0x121000, 0x400000, 0);
+
+  // Construct a 1024 long PTE** at 0x521000
+  for (int i=0; i<1024; i++) {
+    ((uint32_t*)0x521000)[i] = 0x121000 + 0x1000*i;
+  }
+
+  PTE** kernel_page_tables = 0x521000;
+
+  for (int i = 0; i < 1023; i++) {
+    kernel_page_directory[i] = (PDE){
+      .address = (uint32_t)(kernel_page_tables[i]) >> 12,
+      .available = 0,
+      .page_4mb = 0,
+      .accessed = 0,
+      .disable_cache = 0,
+      .write_through_cache = 0,
+      .privilege = 0,
+      .present = 1,
+      .read_write = 1,
+
+      .ignored = 0,
+      .ignored2 = 0
+    };
+  }
+
+  // Mark unavailable bitmap to 0x522000
+  mark_unavailble(bitmap, 0x4000000, bitmap);
+
+  // Now we want to map some stuff.
+  // But first, we should load the kernel somewhere
+
+  uint8_t* kernel_location = 0x542000; // Just load it at 0x524000 for now
+  mark_unavailble(kernel_location, 0x10000, bitmap); // Just treat the kernel as not growing beyond 32k for now.
+
+  map_many_4k_phys_to_virt(kernel_location, 0xc0000000, kernel_page_directory, kernel_page_tables, 0x10); // Map 16 pages from 0x522000 to 0xc0000000;
+  map_4k_phys_to_virt((uint32_t)kernel_page_directory, 0xc0100000, kernel_page_directory, kernel_page_tables); // Map the page directory to 0xc0100000
+  map_many_4k_phys_to_virt(0x121000, 0xc0101000, kernel_page_directory, kernel_page_tables, 1024); // Map 1024 pages from 0x121000 to 0xc0101000
+  map_4k_phys_to_virt(0xb8000, 0xc0501000, kernel_page_directory, kernel_page_tables); // Map 0xb8000 (video) to 0xc0501000
+  map_4k_phys_to_virt(0x521000, 0xc0502000, kernel_page_directory, kernel_page_tables); // Map the PTE** data, we'll need to convert the pointers to point at kernel space at some point.
+
+  map_many_4k_phys_to_virt(0xa0000, 0xc07a0000, kernel_page_directory, kernel_page_tables, 0x20); // Map 32 pages from 0xa0000 to 0xa0000
+  mark_unavailble(0xa0000, 0x20000, bitmap);
+  
+
+  uint8_t* vm_bitmap = 0x522000;
+  mark_unavailble(vm_bitmap, 0x20000, bitmap);
+
+  memset(vm_bitmap, 0x20000, 0xff);
+  mark_unavailble(0xc07a0000, 0x20000, vm_bitmap);
+  mark_unavailble(0xc0000000, 0x10000, vm_bitmap);
+  mark_unavailble(0xc0100000, 0x1000, vm_bitmap);
+  mark_unavailble(0xc0101000, 0x400000, vm_bitmap);
+  mark_unavailble(0xc0501000, 0x1000, vm_bitmap);
+  mark_unavailble(0xc0502000, 0x1000, vm_bitmap);
+  mark_unavailble(0xc0600000, 0x20000, vm_bitmap);
+  mark_unavailble(0xc0620000, 0x20000, vm_bitmap);
+  mark_unavailble(0xc1000000, 0x6000, vm_bitmap);
+
+  // Map the bitmap 
+  map_many_4k_phys_to_virt(0x100000, 0xc0600000, kernel_page_directory, kernel_page_tables, 32);
+  // Map the virtual memory bitmap
+  map_many_4k_phys_to_virt(0x522000, 0xc0620000, kernel_page_directory, kernel_page_tables, 32);
+
+  map_4k_phys_to_virt(0x8000, 0x8000, kernel_page_directory, kernel_page_tables);
+  // Map the stack
+  map_many_4k_phys_to_virt(0x8a000, 0x8a000, kernel_page_directory, kernel_page_tables, 16);
+  map_many_4k_phys_to_virt(0x8a000, 0xc1000000, kernel_page_directory, kernel_page_tables, 16);
+  mark_unavailble(0x8a000, 0x10000, bitmap);
+
+  load_file("KERNEL  BIN", kernel_location);
+
+  printf("Stage2 success!\n");
+
+  //while (1);
+
+  asm volatile("mov %0, %%eax;" 
+       "mov %%eax, %%cr3;"
+       "mov %%cr0, %%eax;"
+       "or $0x80000000, %%eax;"
+       "mov %%eax, %%cr0" : : "m" (kernel_page_directory));
+
+  ((void(*)(void))0xc0000000)();
+}
\ No newline at end of file
diff --git a/src/bootstage2/paging.c b/src/bootstage2/paging.c
new file mode 100644
index 0000000..63978d3
--- /dev/null
+++ b/src/bootstage2/paging.c
@@ -0,0 +1,46 @@
+#include "paging.h"
+
+typedef struct {
+  uint32_t page_offset : 12;
+  uint32_t pt_index : 10;
+  uint32_t pd_index : 10;
+}__attribute__((packed)) split_addr;
+
+void map_4k_phys_to_virt(uint32_t physical, uint32_t virtual, PDE* page_directory, PTE** page_tables) {
+  split_addr* split = (split_addr*)&virtual;
+
+  page_directory[split->pd_index] = (PDE){
+    .address = (uint32_t)(page_tables[split->pd_index]) >> 12,
+    .available = 0,
+    .page_4mb = 0,
+    .accessed = 0,
+    .disable_cache = 0,
+    .write_through_cache = 0,
+    .privilege = 0,
+    .present = 1,
+    .read_write = 1,
+
+    .ignored = 0,
+    .ignored2 = 0
+  };
+
+  page_tables[split->pd_index][split->pt_index] = (PTE){
+    .address = physical >> 12,
+    .available = 0,
+    .global = 0,
+    .accessed = 0,
+    .disable_cache = 0,
+    .dirty = 0,
+    .write_through_cache = 0,
+    .privilege = 0,
+    .present = 1,
+    .read_write = 1,
+
+    .ignored = 0
+  };
+}
+
+void map_many_4k_phys_to_virt(uint32_t physical, uint32_t virtual, PDE* page_directory, PTE** page_tables, uint32_t count) {
+  for (int i=0; i<count; i++)
+    map_4k_phys_to_virt(physical + 4096*i, virtual + 4096*i, page_directory, page_tables);
+}
\ No newline at end of file
diff --git a/src/bootstage2/paging.h b/src/bootstage2/paging.h
new file mode 100644
index 0000000..1978218
--- /dev/null
+++ b/src/bootstage2/paging.h
@@ -0,0 +1,37 @@
+#ifndef PAGING_H
+#define PAGING_H
+
+#include <stdbool.h>
+#include "types.h"
+
+typedef struct {
+  uint32_t present : 1;
+  uint32_t read_write : 1;
+  uint32_t privilege : 1;
+  uint32_t write_through_cache : 1;
+  uint32_t disable_cache : 1;
+  uint32_t accessed : 1;
+  uint32_t ignored2 : 1;
+  uint32_t page_4mb : 1;
+  uint32_t ignored : 1;
+  uint32_t available : 3;
+  uint32_t address : 20;
+}__attribute__((packed)) PDE;
+
+typedef struct {
+  uint32_t present : 1;
+  uint32_t read_write : 1;
+  uint32_t privilege : 1;
+  uint32_t write_through_cache : 1;
+  uint32_t disable_cache : 1;
+  uint32_t accessed : 1;
+  uint32_t dirty : 1;
+  uint32_t ignored : 1;
+  uint32_t global : 1;
+  uint32_t available : 3;
+  uint32_t address : 20;
+}__attribute__((packed)) PTE;
+
+void map_4k_phys_to_virt(uint32_t physical, uint32_t virtual, PDE* page_directory, PTE** page_tables);
+void map_many_4k_phys_to_virt(uint32_t physical, uint32_t virtual, PDE* page_directory, PTE** page_tables, uint32_t count);
+#endif
\ No newline at end of file
diff --git a/src/bootstage2/screenstuff.c b/src/bootstage2/screenstuff.c
new file mode 100644
index 0000000..1b898e1
--- /dev/null
+++ b/src/bootstage2/screenstuff.c
@@ -0,0 +1,165 @@
+#include "screenstuff.h"
+
+uint16_t* VMEM_ADDR = (uint16_t*)0xb8000;
+const int TERM_WIDTH = 80;
+const int TERM_HEIGHT = 25;
+
+int cursor_x = 0;
+int cursor_y = 0;
+
+uint16_t get_curpos() {
+  uint16_t cursor_position = 0;
+  uint8_t* cursor_position_split = (uint8_t*)&cursor_position;
+  outb(0x3D4, 0x0F);
+  cursor_position_split[0] = inb(0x3D5);
+  outb(0x3D4, 0x0E);
+  cursor_position_split[1] = inb(0x3D5);
+  return cursor_position;
+}
+
+void init_term() {
+  uint16_t cursor_position = get_curpos();
+
+  cursor_y = cursor_position / TERM_WIDTH;
+  cursor_x = cursor_position % TERM_WIDTH;
+}
+
+void clear_screen()  {
+  for (int i=0; i<TERM_WIDTH*TERM_HEIGHT; i++) {
+    VMEM_ADDR[i] = 0x0720;
+  }
+}
+
+void clear_line(int line) {
+  for (int x=0; x<TERM_WIDTH; x++) {
+    VMEM_ADDR[TERM_WIDTH*line + x] = 0x0720;
+  }
+}
+
+void set_curpos_raw(int curpos) {
+  uint8_t* cursor_position_split = (uint8_t*)&curpos;
+  outb(0x3D4, 0x0F);
+  outb(0x3D5, cursor_position_split[0]);
+  outb(0x3D4, 0x0E);
+  outb(0x3D5, cursor_position_split[1]);
+
+  cursor_x = (*(uint16_t*)cursor_position_split) % TERM_WIDTH;
+  cursor_y = (*(uint16_t*)cursor_position_split) / TERM_WIDTH;
+}
+
+void set_curpos(int x, int y) {
+  set_curpos_raw(y * TERM_WIDTH + x);
+  
+  cursor_x = x;
+  cursor_y = y;
+}
+
+int int_to_decimal(unsigned int number, char* string_buffer) {
+  for (int i=0; i<11; i++)
+    string_buffer[i] = 0;
+  
+  int index = 9;
+  unsigned int acc = number;
+  if (acc == 0)
+    string_buffer[index--] = '0';
+  while (acc != 0) {
+    string_buffer[index--] = 0x30+(acc%10);
+    acc /= 10;
+  }
+  return (index+1);
+}
+
+char dec_to_hex[16] = "0123456789abcdef";
+int int_to_hex(unsigned int number, char* string_buffer) { 
+  for (int i=0; i<8; i++)
+    string_buffer[i] = '0';
+  string_buffer[8] = 0;
+  
+  int index = 7;
+  unsigned int acc = number;
+  if (acc == 0)
+    string_buffer[index--] = '0';
+  while (acc != 0) {
+    string_buffer[index--] = dec_to_hex[acc%0x10];
+    acc /= 0x10;
+  }
+  return (index+1);
+}
+
+void printf(const char* string, ...) {
+  va_list ptr;
+  va_start(ptr, string);
+
+  int index = 0;
+  int count = 0;
+  char current;
+  while (current=string[index++]) {
+    count++;
+    if (current == '\n') {
+      cursor_x = 0;
+      cursor_y++;
+    }
+
+    if (cursor_x == TERM_WIDTH) {
+      cursor_x = 0;
+      cursor_y++;
+    }
+
+    if (cursor_y == TERM_HEIGHT) {
+      for (int i=1; i<TERM_HEIGHT; i++) {
+        for (int x=0; x<TERM_WIDTH; x++) {
+          int from = i * TERM_WIDTH + x;
+          int to = (i-1) * TERM_WIDTH + x;
+          VMEM_ADDR[to] = VMEM_ADDR[from];
+        }
+      }
+      clear_line(24);
+      cursor_y--;
+    }
+
+    if (current == '%') {
+      int type = string[index++];
+      int offset;
+      switch (type) {
+        case 'd': {
+          char decimal_buffer[11];
+          offset = int_to_decimal(va_arg(ptr, int), decimal_buffer);
+          printf(decimal_buffer + offset);
+          break;
+        }
+        case 'x': {
+          char hex_buffer[8];
+          offset = int_to_hex(va_arg(ptr, int), hex_buffer);
+          printf(hex_buffer);
+          break;
+        }
+        case 's': {
+          printf(va_arg(ptr, const char*));
+          break;
+        }
+        case 'c': {
+          int mem_pos = cursor_y * TERM_WIDTH + cursor_x++;
+          int promoted = va_arg(ptr, int);
+          char charred = promoted;
+          VMEM_ADDR[mem_pos] = charred + (0x07<<8);
+          break;
+        }
+      }
+      continue;
+    }
+
+    if (current != '\n') {
+      int mem_pos = cursor_y * TERM_WIDTH + cursor_x++;
+      VMEM_ADDR[mem_pos] = current + (0x07<<8);
+    }
+  }
+
+  set_curpos(cursor_x, cursor_y);
+
+  va_end(ptr);
+}
+
+void non_moving_put(char chr) {
+  int mem_pos = cursor_y * TERM_WIDTH + cursor_x;
+  VMEM_ADDR[mem_pos] = chr + (0x07<<8);
+}
\ No newline at end of file
diff --git a/src/bootstage2/screenstuff.h b/src/bootstage2/screenstuff.h
new file mode 100644
index 0000000..15276fc
--- /dev/null
+++ b/src/bootstage2/screenstuff.h
@@ -0,0 +1,19 @@
+#ifndef SCREENSTUFF_H
+#define SCREENSTUFF_H
+
+#include <stdarg.h>
+#include "types.h"
+#include "io.h"
+
+uint16_t get_curpos();
+void init_term();
+void clear_screen();
+void clear_line(int line);
+void set_curpos_raw(int curpos);
+void set_curpos(int x, int y);
+int int_to_decimal(unsigned int number, char* string_buffer);
+int int_to_hex(unsigned int number, char* string_buffer);
+void printf(const char* string, ...);
+void non_moving_put(char chr);
+
+#endif
\ No newline at end of file
diff --git a/src/bootstage2/strings.c b/src/bootstage2/strings.c
new file mode 100644
index 0000000..cc48b9f
--- /dev/null
+++ b/src/bootstage2/strings.c
@@ -0,0 +1,69 @@
+#include "strings.h"
+
+bool strcmp(char* a, char* b, int max) {
+  int index = 0;
+  while (index < max) {
+    if (a[index] != b[index])
+      return false;
+    index++;
+  }
+  return true;
+}
+
+char* split_on_first(char delimeter, char* string) {
+  int index = 0;
+  char current;
+
+  while (current = string[index++]) {
+    if (current == delimeter) {
+      string[index-1] = 0;
+      return string+index;
+    }
+  }
+
+  return 0;
+}
+
+int string_split(char delimeter, char* string, char** pointer_array) {
+  int index = 0;
+  int last_split_index = 0;
+  int split_count = 0;
+
+  char current;
+
+  while (current = string[index]) {
+    if (current == delimeter) {
+      string[index] = 0;
+      pointer_array[split_count++] = (string+last_split_index);
+      last_split_index = (index+1);
+    }
+    index++;
+  }
+
+  // Add remaining part of the string to the pointer_array
+  pointer_array[split_count] = (string+last_split_index);
+
+  return split_count;
+}
+
+void decode_filename(char* nice_name, char* filenamebuffer) {
+  // Clear filenamebuffer
+  for (int i=0; i<11; i++)
+    filenamebuffer[i] = ' ';
+  filenamebuffer[11] = 0;
+  
+  int fbIndex = 0;
+  for (int i=0; i<12; i++) {
+    if (nice_name[i] == 0)
+      return;
+    if (nice_name[i] == '.') {
+      fbIndex = 8;
+      continue;
+    }
+
+    if (nice_name[i] >= 0x61 && nice_name[i] <= 0x7f)
+      filenamebuffer[fbIndex++] = nice_name[i] - 32;
+    else 
+      filenamebuffer[fbIndex++] = nice_name[i];
+  }
+}
\ No newline at end of file
diff --git a/src/bootstage2/strings.h b/src/bootstage2/strings.h
new file mode 100644
index 0000000..baf7c5c
--- /dev/null
+++ b/src/bootstage2/strings.h
@@ -0,0 +1,11 @@
+#ifndef STRINGS_H
+#define STRINGS_H
+
+#include <stdbool.h>
+
+bool strcmp(char* a, char* b, int max);
+char* split_on_first(char delimeter, char* string);
+int string_split(char delimeter, char* string, char** pointer_array);
+void decode_filename(char* nice_name, char* filenamebuffer);
+
+#endif
\ No newline at end of file
diff --git a/src/bootstage2/types.h b/src/bootstage2/types.h
new file mode 100644
index 0000000..1bae039
--- /dev/null
+++ b/src/bootstage2/types.h
@@ -0,0 +1,11 @@
+#ifndef TYPES_H
+#define TYPES_H
+
+typedef char int8_t;
+typedef unsigned char uint8_t;
+typedef short int16_t;
+typedef unsigned short uint16_t;
+typedef long int int32_t;
+typedef unsigned long int uint32_t;
+
+#endif
\ No newline at end of file