From 722d73825414e3253fec4e9e7d33afded4afca6e Mon Sep 17 00:00:00 2001
From: Jeremy Soller <jackpot51@gmail.com>
Date: Thu, 1 Sep 2016 08:12:50 -0600
Subject: [PATCH] Fix mouse position, map display on APs

---
 arch/x86_64/src/context.rs        |  2 --
 arch/x86_64/src/device/display.rs | 38 ++++++++++++++++++++++++++++++-
 arch/x86_64/src/device/mod.rs     |  4 ++++
 arch/x86_64/src/device/ps2.rs     |  4 ++--
 arch/x86_64/src/lib.rs            |  6 ++---
 arch/x86_64/src/start.rs          |  8 +++++++
 6 files changed, 53 insertions(+), 9 deletions(-)

diff --git a/arch/x86_64/src/context.rs b/arch/x86_64/src/context.rs
index 75756a5..ef95e05 100644
--- a/arch/x86_64/src/context.rs
+++ b/arch/x86_64/src/context.rs
@@ -51,8 +51,6 @@ impl Context {
     #[inline(never)]
     #[naked]
     pub unsafe fn switch_to(&mut self, next: &mut Context) {
-        asm!("xchg bx, bx" : : : "memory" : "intel", "volatile");
-
 /*
         asm!("fxsave [$0]" : : "r"(self.fx) : "memory" : "intel", "volatile");
         self.loadable = true;
diff --git a/arch/x86_64/src/device/display.rs b/arch/x86_64/src/device/display.rs
index 8bfc215..1a978d3 100644
--- a/arch/x86_64/src/device/display.rs
+++ b/arch/x86_64/src/device/display.rs
@@ -1,4 +1,4 @@
-use core::slice;
+use core::{cmp, slice};
 use spin::Mutex;
 
 use memory::Frame;
@@ -72,6 +72,26 @@ pub unsafe fn init(active_table: &mut ActivePageTable) {
     }
 }
 
+pub unsafe fn init_ap(active_table: &mut ActivePageTable) {
+    active_table.identity_map(Frame::containing_address(PhysicalAddress::new(0x5200)), entry::PRESENT | entry::NO_EXECUTE);
+
+    let mode_info = &*(0x5200 as *const VBEModeInfo);
+    if mode_info.physbaseptr > 0 {
+        let width = mode_info.xresolution as usize;
+        let height = mode_info.yresolution as usize;
+        let start = mode_info.physbaseptr as usize;
+        let size = width * height;
+
+        {
+            let start_frame = Frame::containing_address(PhysicalAddress::new(start));
+            let end_frame = Frame::containing_address(PhysicalAddress::new(start + size * 4 - 1));
+            for frame in Frame::range_inclusive(start_frame, end_frame) {
+                active_table.identity_map(frame, entry::PRESENT | entry::WRITABLE | entry::NO_EXECUTE);
+            }
+        }
+    }
+}
+
 /// A display
 pub struct Display {
     pub width: usize,
@@ -87,4 +107,20 @@ impl Display {
             data: data,
         }
     }
+
+    pub fn rect(&mut self, x: usize, y: usize, w: usize, h: usize, color: u32) {
+        let start_y = cmp::min(self.height - 1, y);
+        let end_y = cmp::min(self.height, y + h);
+
+        let start_x = cmp::min(self.width - 1, x);
+        let len = cmp::min(self.width, x + w) - start_x;
+
+        for y in start_y..end_y {
+            let offset = y * self.width + start_x;
+            let row = &mut self.data[offset..offset + len];
+            for pixel in row.iter_mut() {
+                *pixel = color;
+            }
+        }
+    }
 }
diff --git a/arch/x86_64/src/device/mod.rs b/arch/x86_64/src/device/mod.rs
index d550e02..bfac385 100644
--- a/arch/x86_64/src/device/mod.rs
+++ b/arch/x86_64/src/device/mod.rs
@@ -7,3 +7,7 @@ pub unsafe fn init(active_table: &mut ActivePageTable){
     display::init(active_table);
     ps2::init();
 }
+
+pub unsafe fn init_ap(active_table: &mut ActivePageTable) {
+    display::init_ap(active_table);
+}
diff --git a/arch/x86_64/src/device/ps2.rs b/arch/x86_64/src/device/ps2.rs
index 79dbd25..3a07fe4 100644
--- a/arch/x86_64/src/device/ps2.rs
+++ b/arch/x86_64/src/device/ps2.rs
@@ -285,8 +285,8 @@ impl Ps2 {
             //print!("MOUSE {:?}, {}, {}, {}\n", flags, dx, dy, extra);
 
             if let Some(ref mut display) = *super::display::DISPLAY.lock() {
-                self.mouse_x = cmp::min(display.width, cmp::max(0, self.mouse_x as isize + dx) as usize);
-                self.mouse_y = cmp::min(display.height, cmp::max(0, self.mouse_y as isize - dy) as usize);
+                self.mouse_x = cmp::max(0, cmp::min(display.width as isize - 1, self.mouse_x as isize + dx)) as usize;
+                self.mouse_y = cmp::max(0, cmp::min(display.height as isize - 1, self.mouse_y as isize - dy)) as usize;
                 let offset = self.mouse_y * display.width + self.mouse_x;
                 display.data[offset as usize] = 0xFF0000;
             }
diff --git a/arch/x86_64/src/lib.rs b/arch/x86_64/src/lib.rs
index 84e7846..856eec8 100644
--- a/arch/x86_64/src/lib.rs
+++ b/arch/x86_64/src/lib.rs
@@ -44,8 +44,7 @@ macro_rules! interrupt {
             }
 
             // Push scratch registers
-            asm!("xchg bx, bx
-                push rax
+            asm!("push rax
                 push rcx
                 push rdx
                 push rdi
@@ -86,8 +85,7 @@ macro_rules! interrupt_error {
             }
 
             // Push scratch registers
-            asm!("xchg bx, bx
-                push rax
+            asm!("push rax
                 push rcx
                 push rdx
                 push rdi
diff --git a/arch/x86_64/src/start.rs b/arch/x86_64/src/start.rs
index a84d2e9..987bb7e 100644
--- a/arch/x86_64/src/start.rs
+++ b/arch/x86_64/src/start.rs
@@ -165,6 +165,9 @@ pub unsafe extern fn kstart_ap(stack_start: usize, stack_end: usize) -> ! {
             assert_eq!(TDATA_TEST_NONZERO, 0xFFFFFFFFFFFFFFFE);
         }
 
+        // Init devices for AP
+        device::init_ap(&mut active_table);
+
         // Map heap
         {
             let heap_start_page = Page::containing_address(VirtualAddress::new(HEAP_START));
@@ -192,5 +195,10 @@ pub unsafe extern fn kstart_ap(stack_start: usize, stack_end: usize) -> ! {
         interrupt::pause();
     }
 
+    if let Some(ref mut display) = *device::display::DISPLAY.lock() {
+        let width = display.width;
+        display.rect(0, ap_number * 32, width, 16, 0xFF00);
+    }
+
     kmain_ap(ap_number);
 }