From ae2b206700876668b05ebda5b704a25f311d8d00 Mon Sep 17 00:00:00 2001
From: Eemeli Lehtonen <eemeli.o.lehtonen@utu.fi>
Date: Mon, 16 Jan 2023 20:14:23 +0200
Subject: [PATCH] SMP

---
 .cargo/runner.sh                              |  2 +
 build.rs                                      |  6 +-
 src/arch/x86_64/gdt.rs                        |  5 +-
 src/arch/x86_64/limine/mod.rs                 | 37 --------
 src/arch/x86_64/mod.rs                        | 34 ++++---
 src/{arch/x86_64 => boot}/bootboot/mod.rs     |  0
 src/{arch/x86_64 => boot}/limine/cmdline.rs   |  0
 .../x86_64 => boot}/limine/framebuffer.rs     |  0
 src/{arch/x86_64 => boot}/limine/link.ld      |  0
 src/boot/limine/mem.rs                        | 36 ++++++++
 src/boot/limine/mod.rs                        | 33 +++++++
 src/boot/limine/smp.rs                        | 49 ++++++++++
 src/{arch/x86_64 => boot}/limine/term.rs      |  0
 src/boot/mod.rs                               | 14 +++
 src/{arch/x86_64 => boot}/multiboot1/link.ld  |  0
 src/{arch/x86_64 => boot}/multiboot1/mod.rs   |  0
 .../x86_64 => boot}/multiboot1/start.asm      |  0
 src/{arch/x86_64 => boot}/multiboot2/link.ld  |  0
 src/{arch/x86_64 => boot}/multiboot2/mod.rs   |  0
 .../x86_64 => boot}/multiboot2/start.asm      |  0
 src/log.rs                                    | 36 ++++++--
 src/main.rs                                   | 51 +++--------
 src/mem.rs                                    | 89 +++++++++++++++++++
 src/qemu.rs                                   |  5 +-
 src/smp.rs                                    | 52 +++++++++++
 src/term/escape/decode.rs                     |  2 +-
 src/testfw.rs                                 | 24 ++++-
 src/video/color.rs                            | 72 +++++++++++++++
 src/video/framebuffer.rs                      | 84 +++--------------
 src/video/logger.rs                           |  3 +-
 src/video/mod.rs                              |  1 +
 31 files changed, 459 insertions(+), 176 deletions(-)
 delete mode 100644 src/arch/x86_64/limine/mod.rs
 rename src/{arch/x86_64 => boot}/bootboot/mod.rs (100%)
 rename src/{arch/x86_64 => boot}/limine/cmdline.rs (100%)
 rename src/{arch/x86_64 => boot}/limine/framebuffer.rs (100%)
 rename src/{arch/x86_64 => boot}/limine/link.ld (100%)
 create mode 100644 src/boot/limine/mem.rs
 create mode 100644 src/boot/limine/mod.rs
 create mode 100644 src/boot/limine/smp.rs
 rename src/{arch/x86_64 => boot}/limine/term.rs (100%)
 create mode 100644 src/boot/mod.rs
 rename src/{arch/x86_64 => boot}/multiboot1/link.ld (100%)
 rename src/{arch/x86_64 => boot}/multiboot1/mod.rs (100%)
 rename src/{arch/x86_64 => boot}/multiboot1/start.asm (100%)
 rename src/{arch/x86_64 => boot}/multiboot2/link.ld (100%)
 rename src/{arch/x86_64 => boot}/multiboot2/mod.rs (100%)
 rename src/{arch/x86_64 => boot}/multiboot2/start.asm (100%)
 create mode 100644 src/mem.rs
 create mode 100644 src/smp.rs
 create mode 100644 src/video/color.rs

diff --git a/.cargo/runner.sh b/.cargo/runner.sh
index 1f65173..15d0ddd 100755
--- a/.cargo/runner.sh
+++ b/.cargo/runner.sh
@@ -44,6 +44,7 @@ if [ "$(basename $KERNEL)" = "hyperion" ]; then
         -enable-kvm \
         -machine q35 \
         -cpu qemu64 \
+        -smp 8 \
         -M smm=off \
         -d int,guest_errors,cpu_reset \
         -no-reboot \
@@ -59,6 +60,7 @@ else
         -enable-kvm \
         -machine q35 \
         -cpu qemu64 \
+        -smp 8 \
         -M smm=off \
         -d int,guest_errors,cpu_reset \
         -device isa-debug-exit,iobase=0xf4,iosize=0x04 \
diff --git a/build.rs b/build.rs
index fd0ad99..71decec 100644
--- a/build.rs
+++ b/build.rs
@@ -10,8 +10,8 @@ use std::{
 fn main() -> Result<(), Box<dyn Error>> {
     let kernel = var("CARGO_PKG_NAME")?;
     println!("cargo:rerun-if-env-changed=CARGO_PKG_NAME");
-    let arch = var("CARGO_CFG_TARGET_ARCH")?;
-    println!("cargo:rerun-if-env-changed=CARGO_CFG_TARGET_ARCH");
+    //let arch = var("CARGO_CFG_TARGET_ARCH")?;
+    //println!("cargo:rerun-if-env-changed=CARGO_CFG_TARGET_ARCH");
 
     let mut bootloader: Option<&'static str> = None;
     let mut set = |s| {
@@ -32,7 +32,7 @@ fn main() -> Result<(), Box<dyn Error>> {
     set("multiboot2");
 
     if let Some(bootloader) = bootloader {
-        let script = format!("src/arch/{arch}/{bootloader}/link.ld");
+        let script = format!("src/boot/{bootloader}/link.ld");
         println!("cargo:rustc-link-arg-bin={kernel}=--script={script}");
         println!("cargo:rerun-if-changed={script}");
     } else {
diff --git a/src/arch/x86_64/gdt.rs b/src/arch/x86_64/gdt.rs
index 9589fbd..7c3aec0 100644
--- a/src/arch/x86_64/gdt.rs
+++ b/src/arch/x86_64/gdt.rs
@@ -1,6 +1,6 @@
 use super::idt::DOUBLE_FAULT_IST;
 use crate::debug;
-use spin::Lazy;
+use spin::{Lazy, Once};
 use x86_64::{
     instructions::tables::load_tss,
     registers::segmentation::{Segment, CS, SS},
@@ -15,7 +15,7 @@ use x86_64::{
 
 pub fn init() {
     debug!("Initializing GDT");
-    GDT.0.load();
+    GDT_ONCE.call_once(|| GDT.0.load());
 
     unsafe {
         CS::set_reg(GDT.1.kc);
@@ -43,6 +43,7 @@ static GDT: Lazy<(GlobalDescriptorTable, SegmentSelectors)> = Lazy::new(|| {
     // gdt.add_entry(Descriptor::user_data_segment());
     (gdt, sel)
 });
+static GDT_ONCE: Once<()> = Once::new();
 
 static TSS: Lazy<TaskStateSegment> = Lazy::new(|| {
     let mut tss = TaskStateSegment::new();
diff --git a/src/arch/x86_64/limine/mod.rs b/src/arch/x86_64/limine/mod.rs
deleted file mode 100644
index f188493..0000000
--- a/src/arch/x86_64/limine/mod.rs
+++ /dev/null
@@ -1,37 +0,0 @@
-use super::{gdt, idt};
-use crate::debug;
-
-//
-
-pub use term::_print;
-
-//
-
-mod cmdline;
-mod framebuffer;
-mod term;
-
-//
-
-#[no_mangle]
-pub extern "C" fn _start() -> ! {
-    crate::BOOTLOADER.call_once(|| "Limine");
-
-    cmdline::init();
-    framebuffer::init();
-
-    gdt::init();
-    idt::init();
-
-    debug!("Re-enabling x86_64 interrupts");
-    x86_64::instructions::interrupts::enable();
-
-    debug!("Calling general kernel_main");
-    crate::kernel_main()
-}
-
-pub fn done() -> ! {
-    loop {
-        x86_64::instructions::hlt();
-    }
-}
diff --git a/src/arch/x86_64/mod.rs b/src/arch/x86_64/mod.rs
index 34c7127..7a415c3 100644
--- a/src/arch/x86_64/mod.rs
+++ b/src/arch/x86_64/mod.rs
@@ -1,16 +1,24 @@
-#[cfg(feature = "multiboot1")]
-#[path = "multiboot1/mod.rs"]
-pub mod boot;
-#[cfg(feature = "multiboot2")]
-#[path = "multiboot2/mod.rs"]
-pub mod boot;
-#[cfg(feature = "bootboot")]
-#[path = "bootboot/mod.rs"]
-pub mod boot;
-#[cfg(feature = "limine")]
-#[path = "limine/mod.rs"]
-pub mod boot;
-pub use boot::*;
+use crate::debug;
+
+//
 
 pub mod gdt;
 pub mod idt;
+
+//
+
+pub fn early_boot_cpu() {
+    gdt::init();
+    idt::init();
+
+    debug!("Re-enabling x86_64 interrupts");
+    x86_64::instructions::interrupts::enable();
+}
+
+pub fn early_per_cpu() {}
+
+pub fn done() -> ! {
+    loop {
+        x86_64::instructions::hlt();
+    }
+}
diff --git a/src/arch/x86_64/bootboot/mod.rs b/src/boot/bootboot/mod.rs
similarity index 100%
rename from src/arch/x86_64/bootboot/mod.rs
rename to src/boot/bootboot/mod.rs
diff --git a/src/arch/x86_64/limine/cmdline.rs b/src/boot/limine/cmdline.rs
similarity index 100%
rename from src/arch/x86_64/limine/cmdline.rs
rename to src/boot/limine/cmdline.rs
diff --git a/src/arch/x86_64/limine/framebuffer.rs b/src/boot/limine/framebuffer.rs
similarity index 100%
rename from src/arch/x86_64/limine/framebuffer.rs
rename to src/boot/limine/framebuffer.rs
diff --git a/src/arch/x86_64/limine/link.ld b/src/boot/limine/link.ld
similarity index 100%
rename from src/arch/x86_64/limine/link.ld
rename to src/boot/limine/link.ld
diff --git a/src/boot/limine/mem.rs b/src/boot/limine/mem.rs
new file mode 100644
index 0000000..e8e41f5
--- /dev/null
+++ b/src/boot/limine/mem.rs
@@ -0,0 +1,36 @@
+use crate::mem::Memmap;
+use limine::{LimineMemmapRequest, LimineMemoryMapEntryType};
+
+//
+
+pub fn memmap() -> impl Iterator<Item = Memmap> {
+    static REQ: LimineMemmapRequest = LimineMemmapRequest::new(0);
+
+    const DEFAULT_MEMMAP: Memmap = Memmap {
+        base: u64::MAX,
+        len: 0u64,
+    };
+
+    REQ.get_response()
+        .get()
+        .into_iter()
+        .flat_map(|a| a.memmap())
+        .scan(DEFAULT_MEMMAP, |acc, memmap| {
+            // TODO: zero init reclaimable regions
+            if let LimineMemoryMapEntryType::Usable
+            // | LimineMemoryMapEntryType::AcpiReclaimable
+            // | LimineMemoryMapEntryType::BootloaderReclaimable
+            = memmap.typ
+            {
+                acc.base = memmap.base.min(acc.base);
+                acc.len += memmap.len;
+                Some(None)
+            } else if acc.len == 0 {
+                acc.base = u64::MAX;
+                Some(None)
+            } else {
+                Some(Some(core::mem::replace(acc, DEFAULT_MEMMAP)))
+            }
+        })
+        .flatten()
+}
diff --git a/src/boot/limine/mod.rs b/src/boot/limine/mod.rs
new file mode 100644
index 0000000..0f7170a
--- /dev/null
+++ b/src/boot/limine/mod.rs
@@ -0,0 +1,33 @@
+use crate::arch;
+
+//
+
+pub use mem::memmap;
+pub use term::_print;
+
+//
+
+mod cmdline;
+mod framebuffer;
+mod mem;
+mod smp;
+mod term;
+
+//
+
+#[no_mangle]
+pub extern "C" fn _start() -> ! {
+    crate::BOOTLOADER.call_once(|| "Limine");
+
+    framebuffer::init();
+    cmdline::init();
+
+    arch::early_boot_cpu();
+    arch::early_per_cpu();
+
+    crate::kernel_main()
+}
+
+pub fn smp_init() {
+    smp::init();
+}
diff --git a/src/boot/limine/smp.rs b/src/boot/limine/smp.rs
new file mode 100644
index 0000000..74859e1
--- /dev/null
+++ b/src/boot/limine/smp.rs
@@ -0,0 +1,49 @@
+use crate::{
+    arch,
+    smp::{smp_main, Cpu},
+};
+use limine::{LimineSmpInfo, LimineSmpRequest};
+
+//
+
+pub fn init() -> Cpu {
+    static REQ: LimineSmpRequest = LimineSmpRequest::new(0);
+
+    let mut boot = Cpu::new(0, 0);
+
+    for cpu in REQ
+        .get_response()
+        .get_mut()
+        .into_iter()
+        .flat_map(|resp| {
+            let bsp_lapic_id = resp.bsp_lapic_id;
+            resp.cpus().iter_mut().map(move |cpu| (bsp_lapic_id, cpu))
+        })
+        .filter_map(|(bsp_lapic_id, cpu)| {
+            if bsp_lapic_id == cpu.lapic_id {
+                boot = Cpu::from(&**cpu);
+                None
+            } else {
+                Some(cpu)
+            }
+        })
+    {
+        cpu.goto_address = smp_start;
+    }
+
+    boot
+}
+
+extern "C" fn smp_start(info: *const LimineSmpInfo) -> ! {
+    let info = unsafe { &*info };
+    arch::early_per_cpu();
+    smp_main(Cpu::from(info));
+}
+
+//
+
+impl From<&LimineSmpInfo> for Cpu {
+    fn from(value: &LimineSmpInfo) -> Self {
+        Self::new(value.processor_id, value.lapic_id)
+    }
+}
diff --git a/src/arch/x86_64/limine/term.rs b/src/boot/limine/term.rs
similarity index 100%
rename from src/arch/x86_64/limine/term.rs
rename to src/boot/limine/term.rs
diff --git a/src/boot/mod.rs b/src/boot/mod.rs
new file mode 100644
index 0000000..f670b8d
--- /dev/null
+++ b/src/boot/mod.rs
@@ -0,0 +1,14 @@
+#[cfg(feature = "multiboot1")]
+#[path = "multiboot1/mod.rs"]
+mod boot;
+#[cfg(feature = "multiboot2")]
+#[path = "multiboot2/mod.rs"]
+mod boot;
+#[cfg(feature = "bootboot")]
+#[path = "bootboot/mod.rs"]
+mod boot;
+#[cfg(feature = "limine")]
+#[path = "limine/mod.rs"]
+mod boot;
+
+pub use boot::*;
diff --git a/src/arch/x86_64/multiboot1/link.ld b/src/boot/multiboot1/link.ld
similarity index 100%
rename from src/arch/x86_64/multiboot1/link.ld
rename to src/boot/multiboot1/link.ld
diff --git a/src/arch/x86_64/multiboot1/mod.rs b/src/boot/multiboot1/mod.rs
similarity index 100%
rename from src/arch/x86_64/multiboot1/mod.rs
rename to src/boot/multiboot1/mod.rs
diff --git a/src/arch/x86_64/multiboot1/start.asm b/src/boot/multiboot1/start.asm
similarity index 100%
rename from src/arch/x86_64/multiboot1/start.asm
rename to src/boot/multiboot1/start.asm
diff --git a/src/arch/x86_64/multiboot2/link.ld b/src/boot/multiboot2/link.ld
similarity index 100%
rename from src/arch/x86_64/multiboot2/link.ld
rename to src/boot/multiboot2/link.ld
diff --git a/src/arch/x86_64/multiboot2/mod.rs b/src/boot/multiboot2/mod.rs
similarity index 100%
rename from src/arch/x86_64/multiboot2/mod.rs
rename to src/boot/multiboot2/mod.rs
diff --git a/src/arch/x86_64/multiboot2/start.asm b/src/boot/multiboot2/start.asm
similarity index 100%
rename from src/arch/x86_64/multiboot2/start.asm
rename to src/boot/multiboot2/start.asm
diff --git a/src/log.rs b/src/log.rs
index f23f82e..2d7bfa9 100644
--- a/src/log.rs
+++ b/src/log.rs
@@ -21,8 +21,7 @@ macro_rules! println {
 macro_rules! log {
     ($level:expr, $($t:tt)*) => {
         if $crate::log::test_log_level($level) {
-            $crate::log::_print_log_stamp($level, module_path!());
-            $crate::println!($($t)*);
+            $crate::log::_print_log($level, module_path!(), format_args_nl!($($t)*));
         }
     };
 }
@@ -91,7 +90,7 @@ pub fn test_log_level(level: LogLevel) -> bool {
 }
 
 #[doc(hidden)]
-pub fn _print_log_stamp(level: LogLevel, module: &str) {
+pub fn _print_log(level: LogLevel, module: &str, args: Arguments) {
     // if !LOGGER.color.load(Ordering::SeqCst) {
     //     print!("[{level:?}]: ")
     // } else {
@@ -105,7 +104,7 @@ pub fn _print_log_stamp(level: LogLevel, module: &str) {
     };
 
     print!(
-        "{}{level} {} {}: ",
+        "{}{level} {} {}: {args}",
         '['.true_grey(),
         module.true_grey(),
         ']'.true_grey(),
@@ -113,6 +112,11 @@ pub fn _print_log_stamp(level: LogLevel, module: &str) {
     // }
 }
 
+#[doc(hidden)]
+pub fn _print(args: Arguments) {
+    LOGGER.print(args)
+}
+
 //
 
 #[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
@@ -203,7 +207,25 @@ impl Logger {
     }
 }
 
-#[doc(hidden)]
-pub fn _print(args: Arguments) {
-    LOGGER.print(args)
+//
+
+#[cfg(test)]
+mod tests {
+    use super::{set_log_level, LogLevel};
+
+    #[test_case]
+    fn log_levels() {
+        set_log_level(LogLevel::Trace);
+
+        for level in LogLevel::ALL {
+            log!(level, "LOG TEST")
+        }
+    }
+
+    #[test_case]
+    fn log_chars() {
+        for c in 0..=255u8 {
+            print!("{}", c as char);
+        }
+    }
 }
diff --git a/src/main.rs b/src/main.rs
index 660ee75..74becd5 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -5,25 +5,31 @@
 #![feature(custom_test_frameworks)]
 #![feature(type_alias_impl_trait)]
 #![feature(result_option_inspect)]
+#![feature(allocator_api)]
+#![feature(nonnull_slice_from_raw_parts)]
 #![test_runner(crate::testfw::test_runner)]
 #![reexport_test_harness_main = "test_main"]
 
 //
 
-use crate::{
-    term::escape::encode::EscapeEncoder,
-    video::framebuffer::{get_fbo, Color},
-};
+use crate::term::escape::encode::EscapeEncoder;
 use spin::Once;
 
 //
 
+extern crate alloc;
+
+//
+
 #[path = "arch/x86_64/mod.rs"]
 pub mod arch;
+pub mod boot;
 pub mod env;
 pub mod log;
+pub mod mem;
 pub mod panic;
 pub mod qemu;
+pub mod smp;
 pub mod term;
 #[cfg(test)]
 pub mod testfw;
@@ -44,8 +50,11 @@ pub static BOOTLOADER: Once<&'static str> = Once::new();
 //
 
 fn kernel_main() -> ! {
+    debug!("Entering kernel_main");
     debug!("Cmdline: {:?}", env::Arguments::get());
 
+    mem::init();
+
     // ofc. every kernel has to have this cringy ascii name splash
     info!("\n{}\n", include_str!("./splash"));
 
@@ -54,40 +63,8 @@ fn kernel_main() -> ! {
         debug!("{kernel} was booted with {bl}");
     }
 
-    // error handling test
-    // stack_overflow(79999999);
-    // unsafe {
-    //     *(0xFFFFFFFFDEADC0DE as *mut u8) = 42;
-    // }
-
-    // for level in log::LogLevel::ALL {
-    //     log!(level, "LOG TEST")
-    // }
-
-    // for c in 0..=255u8 {
-    //     print!("{}", c as char);
-    // }
-
-    if let Some(mut fbo) = get_fbo() {
-        fbo.fill(240, 340, 40, 40, Color::RED);
-        fbo.fill(250, 350, 60, 40, Color::GREEN);
-        fbo.fill(205, 315, 80, 20, Color::BLUE);
-    }
-
     #[cfg(test)]
     test_main();
 
-    arch::done();
-}
-
-#[allow(unused)]
-fn stack_overflow(n: usize) {
-    if n == 0 {
-        return;
-    } else {
-        stack_overflow(n - 1);
-    }
-    unsafe {
-        core::ptr::read_volatile(&0 as *const i32);
-    }
+    smp::init();
 }
diff --git a/src/mem.rs b/src/mem.rs
new file mode 100644
index 0000000..61bc690
--- /dev/null
+++ b/src/mem.rs
@@ -0,0 +1,89 @@
+use crate::{boot, debug, error};
+use core::{
+    alloc::{GlobalAlloc, Layout},
+    ptr::null_mut,
+    sync::atomic::{AtomicU64, Ordering},
+};
+use spin::Mutex;
+
+//
+
+pub fn init() {
+    let mut usable = 0;
+
+    for Memmap { base, len } in boot::memmap() {
+        usable += len;
+        debug!("base: {base:#X} len: {len:#X}");
+
+        ALLOC.memory.store(base, Ordering::SeqCst);
+        *ALLOC.remaining.lock() = len;
+    }
+    debug!("Usable system memory: {usable}");
+}
+
+//
+
+pub struct Memmap {
+    pub base: u64,
+    pub len: u64,
+}
+
+//
+
+#[global_allocator]
+static ALLOC: BumpAlloc = BumpAlloc {
+    memory: AtomicU64::new(0),
+    remaining: Mutex::new(0),
+};
+
+struct BumpAlloc {
+    memory: AtomicU64,
+    remaining: Mutex<u64>,
+}
+
+unsafe impl GlobalAlloc for BumpAlloc {
+    unsafe fn alloc(&self, layout: Layout) -> *mut u8 {
+        let memory = self.memory.load(Ordering::SeqCst);
+        let mut remaining = self.remaining.lock();
+
+        let top = memory + *remaining;
+        let Some(tmp) = top.checked_sub(layout.size() as u64) else {
+            error!("OUT OF MEMORY");
+            error!(
+                "ALLOC: size: {} align: {} top: {top} memory: {memory} remaining: {remaining}",
+                layout.size(),
+                layout.align()
+            );
+            return null_mut();
+        };
+        let new_top = tmp / layout.align() as u64 * layout.align() as u64;
+        let reservation = top - new_top;
+
+        if let Some(left) = remaining.checked_sub(reservation) {
+            *remaining = left;
+            (memory + left) as _
+        } else {
+            error!("OUT OF MEMORY");
+            error!(
+            "ALLOC: size: {} align: {} top: {top} new: {new_top} memory: {memory} remaining: {remaining}",
+            layout.size(),
+            layout.align()
+        );
+            null_mut()
+        }
+    }
+
+    unsafe fn dealloc(&self, _ptr: *mut u8, _layout: Layout) {
+        // BUMP alloc is stupid and won't free the memory
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use alloc::vec::Vec;
+
+    #[test_case]
+    fn test_alloc() {
+        core::hint::black_box((0..64).map(|i| i * 2).collect::<Vec<_>>());
+    }
+}
diff --git a/src/qemu.rs b/src/qemu.rs
index d16e757..683489e 100644
--- a/src/qemu.rs
+++ b/src/qemu.rs
@@ -6,10 +6,7 @@ use uart_16550::SerialPort;
 
 #[doc(hidden)]
 pub fn _print(args: Arguments) {
-    if let Some(mut writer) = COM1.try_lock() {
-        // COM1_LOCKER.store(crate::THREAD, Ordering::SeqCst);
-        _ = writer.write_fmt(args);
-    }
+    _ = COM1.lock().write_fmt(args);
 }
 
 /// Unlocks the COM1 writer IF it is locked by this exact thread
diff --git a/src/smp.rs b/src/smp.rs
new file mode 100644
index 0000000..c2bd0a6
--- /dev/null
+++ b/src/smp.rs
@@ -0,0 +1,52 @@
+use crate::{arch, boot, debug};
+use core::fmt::{self, Display, Formatter};
+
+//
+
+// pub static STORAGE: Once<Vec<ThreadLocal>> = Once::new();
+
+//
+
+pub fn init() -> ! {
+    debug!("Waking up non-boot CPUs");
+    boot::smp_init();
+    smp_main(Cpu {
+        processor_id: 0,
+        local_apic_id: 0,
+    })
+}
+
+pub fn smp_main(cpu: Cpu) -> ! {
+    debug!("Entering smp_main ({cpu})");
+
+    // x86_64::instructions::interrupts::int3();
+
+    arch::done();
+}
+
+//
+
+#[derive(Debug)]
+pub struct Cpu {
+    pub processor_id: u32,
+    pub local_apic_id: u32,
+}
+
+impl Cpu {
+    pub fn new(processor_id: u32, local_apic_id: u32) -> Self {
+        Self {
+            processor_id,
+            local_apic_id,
+        }
+    }
+}
+
+impl Display for Cpu {
+    fn fmt(&self, f: &mut Formatter) -> fmt::Result {
+        write!(f, "CPU{}", self.processor_id)
+    }
+}
+
+// pub struct ThreadLocal {
+//     id: u64,
+// }
diff --git a/src/term/escape/decode.rs b/src/term/escape/decode.rs
index d359b98..d16c624 100644
--- a/src/term/escape/decode.rs
+++ b/src/term/escape/decode.rs
@@ -1,4 +1,4 @@
-use crate::video::framebuffer::Color;
+use crate::video::color::Color;
 
 /// foreground color can be changed like this: "\x1B[38;2;<r>;<g>;<b>m"
 /// background color can be changed like this: "\x1B[48;2;<r>;<g>;<b>m"
diff --git a/src/testfw.rs b/src/testfw.rs
index 6e22a50..5c28a89 100644
--- a/src/testfw.rs
+++ b/src/testfw.rs
@@ -20,7 +20,6 @@ pub trait TestCase {
 impl<F: Fn()> TestCase for F {
     fn run(&self) {
         let name = type_name::<Self>();
-        name.len();
         print!(" - {name:.<40}");
         self();
         println!("[ok]");
@@ -64,8 +63,31 @@ pub fn test_panic_handler(info: &PanicInfo) {
 
 #[cfg(test)]
 mod tests {
+    #[allow(clippy::eq_op)]
     #[test_case]
     fn trivial() {
         assert_eq!(0, 0);
     }
+
+    // TODO: should_panic / should_fail
+    #[test_case]
+    fn random_tests() {
+        // error handling test
+        // stack_overflow(79999999);
+        // unsafe {
+        //     *(0xFFFFFFFFDEADC0DE as *mut u8) = 42;
+        // }
+
+        #[allow(unused)]
+        fn stack_overflow(n: usize) {
+            if n == 0 {
+                return;
+            } else {
+                stack_overflow(n - 1);
+            }
+            unsafe {
+                core::ptr::read_volatile(&0 as *const i32);
+            }
+        }
+    }
 }
diff --git a/src/video/color.rs b/src/video/color.rs
new file mode 100644
index 0000000..1ab1943
--- /dev/null
+++ b/src/video/color.rs
@@ -0,0 +1,72 @@
+#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
+pub struct Color {
+    r: u8,
+    g: u8,
+    b: u8,
+}
+
+impl Color {
+    pub const WHITE: Color = Color::new(0xff, 0xff, 0xff);
+    pub const BLACK: Color = Color::new(0x00, 0x00, 0x00);
+
+    pub const RED: Color = Color::new(0xff, 0x00, 0x00);
+    pub const GREEN: Color = Color::new(0x00, 0xff, 0x00);
+    pub const BLUE: Color = Color::new(0x00, 0x00, 0xff);
+
+    pub const fn new(r: u8, g: u8, b: u8) -> Self {
+        Self { r, g, b }
+    }
+
+    pub const fn from_u32(code: u32) -> Self {
+        let [r, g, b, _] = code.to_ne_bytes();
+        Self::new(r, g, b)
+    }
+
+    pub const fn from_hex(hex_code: &str) -> Self {
+        Self::from_hex_bytes(hex_code.as_bytes())
+    }
+
+    pub const fn from_hex_bytes(hex_code: &[u8]) -> Self {
+        match hex_code {
+            [r0, r1, g0, g1, b0, b1, _, _]
+            | [r0, r1, g0, g1, b0, b1]
+            | [b'#', r0, r1, g0, g1, b0, b1, _, _]
+            | [b'#', r0, r1, g0, g1, b0, b1] => {
+                Self::from_hex_bytes_2([*r0, *r1, *g0, *g1, *b0, *b1])
+            }
+            _ => {
+                panic!("Invalid color hex code")
+            }
+        }
+    }
+
+    pub const fn from_hex_bytes_2(hex_code: [u8; 6]) -> Self {
+        const fn parse_hex_char(c: u8) -> u8 {
+            match c {
+                b'0'..=b'9' => c - b'0',
+                b'a'..=b'f' => c - b'a' + 0xa,
+                _ => c,
+            }
+        }
+
+        const fn parse_byte(str_byte: [u8; 2]) -> u8 {
+            parse_hex_char(str_byte[0]) | parse_hex_char(str_byte[1]) << 4
+        }
+
+        let r = parse_byte([hex_code[0], hex_code[1]]);
+        let g = parse_byte([hex_code[2], hex_code[3]]);
+        let b = parse_byte([hex_code[4], hex_code[5]]);
+
+        Self::new(r, g, b)
+    }
+
+    pub const fn as_u32(&self) -> u32 {
+        // self.b as u32 | (self.g as u32) << 8 | (self.r as u32) << 16
+        u32::from_le_bytes([self.b, self.g, self.r, 0])
+    }
+
+    pub const fn as_arr(&self) -> [u8; 4] {
+        self.as_u32().to_ne_bytes()
+        // [self.r, self.g, self.b, 0]
+    }
+}
diff --git a/src/video/framebuffer.rs b/src/video/framebuffer.rs
index 5943a03..ae4b93d 100644
--- a/src/video/framebuffer.rs
+++ b/src/video/framebuffer.rs
@@ -1,7 +1,5 @@
-use super::font::FONT;
-use core::{
-    ops::{Deref, DerefMut},
-};
+use super::{color::Color, font::FONT};
+use core::ops::{Deref, DerefMut};
 use spin::{Mutex, MutexGuard, Once};
 
 //
@@ -29,13 +27,6 @@ pub struct FramebufferInfo {
     pub pitch: usize, // pixels to the next row
 }
 
-#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
-pub struct Color {
-    r: u8,
-    g: u8,
-    b: u8,
-}
-
 //
 
 impl Framebuffer {
@@ -100,68 +91,21 @@ impl DerefMut for Framebuffer {
     }
 }
 
-impl Color {
-    pub const WHITE: Color = Color::new(0xff, 0xff, 0xff);
-    pub const BLACK: Color = Color::new(0x00, 0x00, 0x00);
-
-    pub const RED: Color = Color::new(0xff, 0x00, 0x00);
-    pub const GREEN: Color = Color::new(0x00, 0xff, 0x00);
-    pub const BLUE: Color = Color::new(0x00, 0x00, 0xff);
-
-    pub const fn new(r: u8, g: u8, b: u8) -> Self {
-        Self { r, g, b }
-    }
+//
 
-    pub const fn from_u32(code: u32) -> Self {
-        let [r, g, b, _] = code.to_ne_bytes();
-        Self::new(r, g, b)
-    }
+#[cfg(test)]
+mod tests {
+    use super::get_fbo;
+    use crate::video::color::Color;
 
-    pub const fn from_hex(hex_code: &str) -> Self {
-        Self::from_hex_bytes(hex_code.as_bytes())
-    }
+    //
 
-    pub const fn from_hex_bytes(hex_code: &[u8]) -> Self {
-        match hex_code {
-            [r0, r1, g0, g1, b0, b1, _, _]
-            | [r0, r1, g0, g1, b0, b1]
-            | [b'#', r0, r1, g0, g1, b0, b1, _, _]
-            | [b'#', r0, r1, g0, g1, b0, b1] => {
-                Self::from_hex_bytes_2([*r0, *r1, *g0, *g1, *b0, *b1])
-            }
-            _ => {
-                panic!("Invalid color hex code")
-            }
+    #[test_case]
+    fn fbo_draw() {
+        if let Some(mut fbo) = get_fbo() {
+            fbo.fill(440, 340, 40, 40, Color::RED);
+            fbo.fill(450, 350, 60, 40, Color::GREEN);
+            fbo.fill(405, 315, 80, 20, Color::BLUE);
         }
     }
-
-    pub const fn from_hex_bytes_2(hex_code: [u8; 6]) -> Self {
-        const fn parse_hex_char(c: u8) -> u8 {
-            match c {
-                b'0'..=b'9' => c - b'0',
-                b'a'..=b'f' => c - b'a' + 0xa,
-                _ => c,
-            }
-        }
-
-        const fn parse_byte(str_byte: [u8; 2]) -> u8 {
-            parse_hex_char(str_byte[0]) | parse_hex_char(str_byte[1]) << 4
-        }
-
-        let r = parse_byte([hex_code[0], hex_code[1]]);
-        let g = parse_byte([hex_code[2], hex_code[3]]);
-        let b = parse_byte([hex_code[4], hex_code[5]]);
-
-        Self::new(r, g, b)
-    }
-
-    pub const fn as_u32(&self) -> u32 {
-        // self.b as u32 | (self.g as u32) << 8 | (self.r as u32) << 16
-        u32::from_le_bytes([self.b, self.g, self.r, 0])
-    }
-
-    pub const fn as_arr(&self) -> [u8; 4] {
-        self.as_u32().to_ne_bytes()
-        // [self.r, self.g, self.b, 0]
-    }
 }
diff --git a/src/video/logger.rs b/src/video/logger.rs
index 18f2ad4..5ebf9f7 100644
--- a/src/video/logger.rs
+++ b/src/video/logger.rs
@@ -1,6 +1,7 @@
 use super::{
+    color::Color,
     font::FONT,
-    framebuffer::{get_fbo, Color, Framebuffer},
+    framebuffer::{get_fbo, Framebuffer},
 };
 use crate::term::escape::decode::{DecodedPart, EscapeDecoder};
 use core::fmt::{self, Arguments, Write};
diff --git a/src/video/mod.rs b/src/video/mod.rs
index 7e1c0fb..41b624f 100644
--- a/src/video/mod.rs
+++ b/src/video/mod.rs
@@ -1,3 +1,4 @@
+pub mod color;
 pub mod font;
 pub mod framebuffer;
 pub mod logger;
-- 
GitLab