Merge pull request #7 from redox-os/user_irq

Userspace Schemes and Drivers

Cargo.toml

@@ -10,6 +10,7 @@ crate-type = ["staticlib"]
 [dependencies]
 bitflags = "*"
 spin = "*"
+syscall = { path = "syscall/" }
 
 [dependencies.goblin]
 git = "https://github.com/m4b/goblin.git"

Makefile

@@ -26,7 +26,9 @@ clean:
 	cargo clean --manifest-path libstd/Cargo.toml
 	cargo clean --manifest-path init/Cargo.toml
 	cargo clean --manifest-path ion/Cargo.toml
+	cargo clean --manifest-path drivers/ps2d/Cargo.toml
 	cargo clean --manifest-path drivers/pcid/Cargo.toml
+	cargo clean --manifest-path schemes/example/Cargo.toml
 	rm -rf build
 
 FORCE:
@@ -133,4 +135,12 @@ $(BUILD)/pcid: drivers/pcid/Cargo.toml drivers/pcid/src/** $(BUILD)/libstd.rlib
 	$(CARGO) rustc --manifest-path $< $(CARGOFLAGS) -o $@
 	strip $@
 
-$(BUILD)/initfs.rs: $(BUILD)/init $(BUILD)/ion $(BUILD)/pcid
+$(BUILD)/ps2d: drivers/ps2d/Cargo.toml drivers/ps2d/src/** $(BUILD)/libstd.rlib
+	$(CARGO) rustc --manifest-path $< $(CARGOFLAGS) -o $@
+	strip $@
+
+$(BUILD)/example: schemes/example/Cargo.toml schemes/example/src/** $(BUILD)/libstd.rlib
+	$(CARGO) rustc --manifest-path $< $(CARGOFLAGS) -o $@
+	strip $@
+
+$(BUILD)/initfs.rs: $(BUILD)/init $(BUILD)/ion $(BUILD)/pcid $(BUILD)/ps2d $(BUILD)/example

arch/x86_64/Cargo.toml

@@ -4,7 +4,8 @@ version = "0.1.0"
 
 [dependencies]
 bitflags = "*"
-hole_list_allocator = { path = "../../alloc/hole_list_allocator"}
+hole_list_allocator = { path = "../../alloc/hole_list_allocator" }
+io = { path = "../../drivers/io" }
 ransid = { git = "https://github.com/redox-os/ransid.git", branch = "new_api" }
 spin = "*"
 

arch/x86_64/src/acpi/mod.rs

@@ -59,11 +59,11 @@ pub fn init_sdt(sdt: &'static Sdt, active_table: &mut ActivePageTable) {
                     if ap_local_apic.flags & 1 == 1 {
                         // Allocate a stack
                         // TODO: Allocate contiguous
-                        let stack_start = allocate_frame().expect("no more frames in acpi stack_start").start_address().get();
+                        let stack_start = allocate_frame().expect("no more frames in acpi stack_start").start_address().get() + ::KERNEL_OFFSET;
                         for _i in 0..62 {
                             allocate_frame().expect("no more frames in acpi stack");
                         }
-                        let stack_end = allocate_frame().expect("no more frames in acpi stack_end").start_address().get() + 4096;
+                        let stack_end = allocate_frame().expect("no more frames in acpi stack_end").start_address().get() + 4096 + ::KERNEL_OFFSET;
 
                         let ap_ready = TRAMPOLINE as *mut u64;
                         let ap_cpu_id = unsafe { ap_ready.offset(1) };

arch/x86_64/src/context.rs

@@ -43,6 +43,10 @@ impl Context {
         }
     }
 
+    pub fn get_page_table(&self) -> usize {
+        self.cr3
+    }
+
     pub fn set_page_table(&mut self, address: usize) {
         self.cr3 = address;
     }
@@ -52,6 +56,7 @@ impl Context {
     }
 
     /// Switch to the next context by restoring its stack and registers
+    #[cold]
     #[inline(never)]
     #[naked]
     pub unsafe fn switch_to(&mut self, next: &mut Context) {
@@ -94,7 +99,12 @@ impl Context {
         asm!("mov $0, rbp" : "=r"(self.rbp) : : "memory" : "intel", "volatile");
         asm!("mov rbp, $0" : : "r"(next.rbp) : "memory" : "intel", "volatile");
 
-        // Unset global lock, set inside of kernel
-        CONTEXT_SWITCH_LOCK.store(false, Ordering::SeqCst);
+        asm!("call context_switch_unlock" : : : "memory" : "intel", "volatile");
     }
 }
+
+/// Unset global lock, set inside of kernel
+#[no_mangle]
+extern fn context_switch_unlock(){
+    CONTEXT_SWITCH_LOCK.store(false, Ordering::SeqCst);
+}

arch/x86_64/src/device/mod.rs

@@ -1,11 +1,9 @@
 use paging::ActivePageTable;
 
 pub mod display;
-pub mod ps2;
 pub mod serial;
 
 pub unsafe fn init(active_table: &mut ActivePageTable){
     serial::init();
     display::init(active_table);
-    ps2::init();
 }

arch/x86_64/src/interrupt/irq.rs

@@ -1,9 +1,9 @@
 use spin::Mutex;
 use x86::io;
 
-use device::ps2::{PS2_KEYBOARD, PS2_MOUSE};
 use device::serial::{COM1, COM2};
 
+pub static ACKS: Mutex<[usize; 16]> = Mutex::new([0; 16]);
 pub static COUNTS: Mutex<[usize; 16]> = Mutex::new([0; 16]);
 
 #[inline(always)]
@@ -17,6 +17,14 @@ unsafe fn slave_ack() {
     master_ack();
 }
 
+pub unsafe fn acknowledge(irq: usize) {
+    if irq >= 8 {
+        slave_ack();
+    } else {
+        master_ack();
+    }
+}
+
 interrupt!(pit, {
     COUNTS.lock()[0] += 1;
     master_ack();
@@ -24,10 +32,6 @@ interrupt!(pit, {
 
 interrupt!(keyboard, {
     COUNTS.lock()[1] += 1;
-    if let Some(ref mut keyboard) = *PS2_KEYBOARD.lock(){
-        keyboard.on_irq();
-    }
-    master_ack();
 });
 
 interrupt!(cascade, {
@@ -84,10 +88,6 @@ interrupt!(pci3, {
 
 interrupt!(mouse, {
     COUNTS.lock()[12] += 1;
-    if let Some(ref mut mouse) = *PS2_MOUSE.lock() {
-        mouse.on_irq();
-    }
-    slave_ack();
 });
 
 interrupt!(fpu, {

arch/x86_64/src/io/mod.rs

@@ -1,9 +0,0 @@
-/// I/O functions
-
-pub use self::io::*;
-pub use self::mmio::*;
-pub use self::pio::*;
-
-mod io;
-mod mmio;
-mod pio;

arch/x86_64/src/lib.rs

@@ -15,6 +15,7 @@ extern crate hole_list_allocator as allocator;
 
 #[macro_use]
 extern crate bitflags;
+extern crate io;
 extern crate ransid;
 extern crate spin;
 pub extern crate x86;
@@ -58,6 +59,9 @@ pub extern crate x86;
     /// Size of user stack
     pub const USER_STACK_SIZE: usize = 1024 * 1024; // 1 MB
 
+    /// Offset to user grants
+    pub const USER_GRANT_OFFSET: usize = USER_STACK_OFFSET + PML4_SIZE;
+
     /// Offset to user temporary image (used when cloning)
     pub const USER_TMP_OFFSET: usize = USER_STACK_OFFSET + PML4_SIZE;
 
@@ -67,14 +71,16 @@ pub extern crate x86;
     /// Offset to user temporary stack (used when cloning)
     pub const USER_TMP_STACK_OFFSET: usize = USER_TMP_HEAP_OFFSET + PML4_SIZE;
 
+    /// Offset to user temporary page for grants
+    pub const USER_TMP_GRANT_OFFSET: usize = USER_TMP_STACK_OFFSET + PML4_SIZE;
+
 
 /// Print to console
 #[macro_export]
 macro_rules! print {
     ($($arg:tt)*) => ({
         use core::fmt::Write;
-        let mut console = $crate::console::CONSOLE.lock();
-        let _ = write!(console, $($arg)*);
+        let _ = write!($crate::console::CONSOLE.lock(), $($arg)*);
     });
 }
 
@@ -199,9 +205,6 @@ pub mod gdt;
 /// Interrupt descriptor table
 pub mod idt;
 
-/// IO Handling
-pub mod io;
-
 /// Interrupt instructions
 pub mod interrupt;
 

arch/x86_64/src/paging/mapper.rs

@@ -93,6 +93,13 @@ impl Mapper {
             .and_then(|p1| p1[page.p1_index()].pointed_frame())
     }
 
+    pub fn translate_page_flags(&self, page: Page) -> Option<EntryFlags> {
+        self.p4().next_table(page.p4_index())
+            .and_then(|p3| p3.next_table(page.p3_index()))
+            .and_then(|p2| p2.next_table(page.p2_index()))
+            .and_then(|p1| Some(p1[page.p1_index()].flags()))
+    }
+
     /// Translate a virtual address to a physical one
     pub fn translate(&self, virtual_address: VirtualAddress) -> Option<PhysicalAddress> {
         let offset = virtual_address.get() % PAGE_SIZE;

arch/x86_64/src/paging/mod.rs

@@ -133,23 +133,23 @@ pub unsafe fn init(cpu_id: usize, stack_start: usize, stack_end: usize) -> (Acti
                 }
             }
 
-            let mut remap = |start: usize, end: usize, flags: EntryFlags, offset: usize| {
+            let mut remap = |start: usize, end: usize, flags: EntryFlags| {
                 if end > start {
                     let start_frame = Frame::containing_address(PhysicalAddress::new(start));
                     let end_frame = Frame::containing_address(PhysicalAddress::new(end - 1));
                     for frame in Frame::range_inclusive(start_frame, end_frame) {
-                        let page = Page::containing_address(VirtualAddress::new(frame.start_address().get() + offset));
+                        let page = Page::containing_address(VirtualAddress::new(frame.start_address().get() + ::KERNEL_OFFSET));
                         mapper.map_to(page, frame, flags);
                     }
                 }
             };
 
             // Remap stack writable, no execute
-            remap(stack_start, stack_end, PRESENT | NO_EXECUTE | WRITABLE, 0);
+            remap(stack_start - ::KERNEL_OFFSET, stack_end - ::KERNEL_OFFSET, PRESENT | NO_EXECUTE | WRITABLE);
 
             // Remap a section with `flags`
             let mut remap_section = |start: &u8, end: &u8, flags: EntryFlags| {
-                remap(start as *const _ as usize - ::KERNEL_OFFSET, end as *const _ as usize - ::KERNEL_OFFSET, flags, ::KERNEL_OFFSET);
+                remap(start as *const _ as usize - ::KERNEL_OFFSET, end as *const _ as usize - ::KERNEL_OFFSET, flags);
             };
             // Remap text read-only
             remap_section(& __text_start, & __text_end, PRESENT);
@@ -211,19 +211,19 @@ pub unsafe fn init_ap(cpu_id: usize, stack_start: usize, stack_end: usize, kerne
             }
         }
 
-        let mut remap = |start: usize, end: usize, flags: EntryFlags, offset: usize| {
+        let mut remap = |start: usize, end: usize, flags: EntryFlags| {
             if end > start {
                 let start_frame = Frame::containing_address(PhysicalAddress::new(start));
                 let end_frame = Frame::containing_address(PhysicalAddress::new(end - 1));
                 for frame in Frame::range_inclusive(start_frame, end_frame) {
-                    let page = Page::containing_address(VirtualAddress::new(frame.start_address().get() + offset));
+                    let page = Page::containing_address(VirtualAddress::new(frame.start_address().get() + ::KERNEL_OFFSET));
                     mapper.map_to(page, frame, flags);
                 }
             }
         };
 
         // Remap stack writable, no execute
-        remap(stack_start, stack_end, PRESENT | NO_EXECUTE | WRITABLE, 0);
+        remap(stack_start - ::KERNEL_OFFSET, stack_end - ::KERNEL_OFFSET, PRESENT | NO_EXECUTE | WRITABLE);
     });
 
     active_table.switch(new_table);
@@ -328,6 +328,10 @@ impl InactivePageTable {
         InactivePageTable { p4_frame: frame }
     }
 
+    pub unsafe fn from_address(cr3: usize) -> InactivePageTable {
+        InactivePageTable { p4_frame: Frame::containing_address(PhysicalAddress::new(cr3)) }
+    }
+
     pub unsafe fn address(&self) -> usize {
         self.p4_frame.start_address().get()
     }

arch/x86_64/src/start.rs

@@ -12,8 +12,8 @@ use externs::memset;
 use gdt;
 use idt;
 use interrupt;
-use memory::{self, Frame};
-use paging::{self, entry, Page, PhysicalAddress, VirtualAddress};
+use memory;
+use paging::{self, entry, Page, VirtualAddress};
 
 /// Test of zero values in BSS.
 static BSS_TEST_ZERO: usize = 0;
@@ -68,8 +68,8 @@ pub unsafe extern fn kstart() -> ! {
         memory::init(0, &__end as *const u8 as usize - ::KERNEL_OFFSET);
 
         // TODO: allocate a stack
-        let stack_start = 0x00080000;
-        let stack_end = 0x0009F000;
+        let stack_start = 0x00080000 + ::KERNEL_OFFSET;
+        let stack_end = 0x0009F000 + ::KERNEL_OFFSET;
 
         // Initialize paging
         let (mut active_table, tcb_offset) = paging::init(0, stack_start, stack_end);
@@ -148,7 +148,7 @@ pub unsafe extern fn kstart_ap(cpu_id: usize, page_table: usize, stack_start: us
         let kernel_table = KERNEL_TABLE.load(Ordering::SeqCst);
 
         // Initialize paging
-        let (mut active_table, tcb_offset) = paging::init_ap(cpu_id, stack_start, stack_end, kernel_table);
+        let (active_table, tcb_offset) = paging::init_ap(cpu_id, stack_start, stack_end, kernel_table);
 
         // Set up GDT for AP
         gdt::init(tcb_offset, stack_end);

bootloader/x86_64/startup-x86_64.asm

@@ -122,10 +122,11 @@ long_mode:
     mov gs, rax
     mov ss, rax
 
-    mov rsp, 0x0009F000
+    mov rsp, 0xFFFFFF000009F000
 
     ;rust init
     mov rax, [kernel_base + 0x18]
+    xchg bx, bx
     jmp rax
 
 long_mode_ap:

drivers/io/Cargo.toml

@@ -0,0 +1,3 @@
+[package]
+name = "io"
+version = "0.1.0"

drivers/io/src/io.rs

@@ -0,0 +1,67 @@
+use core::cmp::PartialEq;
+use core::ops::{BitAnd, BitOr, Not};
+
+pub trait Io {
+    type Value: Copy + PartialEq + BitAnd<Output = Self::Value> + BitOr<Output = Self::Value> + Not<Output = Self::Value>;
+
+    fn read(&self) -> Self::Value;
+    fn write(&mut self, value: Self::Value);
+
+    #[inline(always)]
+    fn readf(&self, flags: Self::Value) -> bool  {
+        (self.read() & flags) as Self::Value == flags
+    }
+
+    #[inline(always)]
+    fn writef(&mut self, flags: Self::Value, value: bool) {
+        let tmp: Self::Value = match value {
+            true => self.read() | flags,
+            false => self.read() & !flags,
+        };
+        self.write(tmp);
+    }
+}
+
+pub struct ReadOnly<I: Io> {
+    inner: I
+}
+
+impl<I: Io> ReadOnly<I> {
+    pub const fn new(inner: I) -> ReadOnly<I> {
+        ReadOnly {
+            inner: inner
+        }
+    }
+
+    #[inline(always)]
+    pub fn read(&self) -> I::Value {
+        self.inner.read()
+    }
+
+    #[inline(always)]
+    pub fn readf(&self, flags: I::Value) -> bool {
+        self.inner.readf(flags)
+    }
+}
+
+pub struct WriteOnly<I: Io> {
+    inner: I
+}
+
+impl<I: Io> WriteOnly<I> {
+    pub const fn new(inner: I) -> WriteOnly<I> {
+        WriteOnly {
+            inner: inner
+        }
+    }
+
+    #[inline(always)]
+    pub fn write(&mut self, value: I::Value) {
+        self.inner.write(value)
+    }
+
+    #[inline(always)]
+    pub fn writef(&mut self, flags: I::Value, value: bool) {
+        self.inner.writef(flags, value)
+    }
+}

drivers/io/src/lib.rs

@@ -0,0 +1,14 @@
+//! I/O functions
+
+#![feature(asm)]
+#![feature(const_fn)]
+#![feature(core_intrinsics)]
+#![no_std]
+
+pub use self::io::*;
+pub use self::mmio::*;
+pub use self::pio::*;
+
+mod io;
+mod mmio;
+mod pio;

drivers/io/src/mmio.rs

@@ -0,0 +1,31 @@
+use core::intrinsics::{volatile_load, volatile_store};
+use core::mem::uninitialized;
+use core::ops::{BitAnd, BitOr, Not};
+
+use super::io::Io;
+
+#[repr(packed)]
+pub struct Mmio<T> {
+    value: T,
+}
+
+impl<T> Mmio<T> {
+    /// Create a new Mmio without initializing
+    pub fn new() -> Self {
+        Mmio {
+            value: unsafe { uninitialized() }
+        }
+    }
+}
+
+impl<T> Io for Mmio<T> where T: Copy + PartialEq + BitAnd<Output = T> + BitOr<Output = T> + Not<Output = T> {
+    type Value = T;
+
+    fn read(&self) -> T {
+        unsafe { volatile_load(&self.value) }
+    }
+
+    fn write(&mut self, value: T) {
+        unsafe { volatile_store(&mut self.value, value) };
+    }
+}

drivers/io/src/pio.rs

@@ -0,0 +1,89 @@
+use core::marker::PhantomData;
+
+use super::io::Io;
+
+/// Generic PIO
+#[derive(Copy, Clone)]
+pub struct Pio<T> {
+    port: u16,
+    value: PhantomData<T>,
+}
+
+impl<T> Pio<T> {
+    /// Create a PIO from a given port
+    pub const fn new(port: u16) -> Self {
+        Pio::<T> {
+            port: port,
+            value: PhantomData,
+        }
+    }
+}
+
+/// Read/Write for byte PIO
+impl Io for Pio<u8> {
+    type Value = u8;
+
+    /// Read
+    #[inline(always)]
+    fn read(&self) -> u8 {
+        let value: u8;
+        unsafe {
+            asm!("in $0, $1" : "={al}"(value) : "{dx}"(self.port) : "memory" : "intel", "volatile");
+        }
+        value
+    }
+
+    /// Write
+    #[inline(always)]
+    fn write(&mut self, value: u8) {
+        unsafe {
+            asm!("out $1, $0" : : "{al}"(value), "{dx}"(self.port) : "memory" : "intel", "volatile");
+        }
+    }
+}
+
+/// Read/Write for word PIO
+impl Io for Pio<u16> {
+    type Value = u16;
+
+    /// Read
+    #[inline(always)]
+    fn read(&self) -> u16 {
+        let value: u16;
+        unsafe {
+            asm!("in $0, $1" : "={ax}"(value) : "{dx}"(self.port) : "memory" : "intel", "volatile");
+        }
+        value
+    }
+
+    /// Write
+    #[inline(always)]
+    fn write(&mut self, value: u16) {
+        unsafe {
+            asm!("out $1, $0" : : "{ax}"(value), "{dx}"(self.port) : "memory" : "intel", "volatile");
+        }
+    }
+}
+
+/// Read/Write for doubleword PIO
+impl Io for Pio<u32> {
+    type Value = u32;
+
+    /// Read
+    #[inline(always)]
+    fn read(&self) -> u32 {
+        let value: u32;
+        unsafe {
+            asm!("in $0, $1" : "={eax}"(value) : "{dx}"(self.port) : "memory" : "intel", "volatile");
+        }
+        value
+    }
+
+    /// Write
+    #[inline(always)]
+    fn write(&mut self, value: u32) {
+        unsafe {
+            asm!("out $1, $0" : : "{eax}"(value), "{dx}"(self.port) : "memory" : "intel", "volatile");
+        }
+    }
+}

drivers/ps2d/Cargo.toml

@@ -0,0 +1,9 @@
+[package]
+name = "ps2d"
+version = "0.1.0"
+
+[dependencies]
+bitflags = "*"
+io = { path = "../io/" }
+spin = "*"
+syscall = { path = "../../syscall/" }

drivers/ps2d/src/controller.rs

@@ -0,0 +1,233 @@
+use io::{Io, Pio, ReadOnly, WriteOnly};
+
+bitflags! {
+    flags StatusFlags: u8 {
+        const OUTPUT_FULL = 1,
+        const INPUT_FULL = 1 << 1,
+        const SYSTEM = 1 << 2,
+        const COMMAND = 1 << 3,
+        // Chipset specific
+        const KEYBOARD_LOCK = 1 << 4,
+        // Chipset specific
+        const SECOND_OUTPUT_FULL = 1 << 5,
+        const TIME_OUT = 1 << 6,
+        const PARITY = 1 << 7
+    }
+}
+
+bitflags! {
+    flags ConfigFlags: u8 {
+        const FIRST_INTERRUPT = 1,
+        const SECOND_INTERRUPT = 1 << 1,
+        const POST_PASSED = 1 << 2,
+        // 1 << 3 should be zero
+        const CONFIG_RESERVED_3 = 1 << 3,
+        const FIRST_DISABLED = 1 << 4,
+        const SECOND_DISABLED = 1 << 5,
+        const FIRST_TRANSLATE = 1 << 6,
+        // 1 << 7 should be zero
+        const CONFIG_RESERVED_7 = 1 << 7,
+    }
+}
+
+#[repr(u8)]
+#[allow(dead_code)]
+enum Command {
+    ReadConfig = 0x20,
+    WriteConfig = 0x60,
+    DisableSecond = 0xA7,
+    EnableSecond = 0xA8,
+    TestSecond = 0xA9,
+    TestController = 0xAA,
+    TestFirst = 0xAB,
+    Diagnostic = 0xAC,
+    DisableFirst = 0xAD,
+    EnableFirst = 0xAE,
+    WriteSecond = 0xD4
+}
+
+#[repr(u8)]
+#[allow(dead_code)]
+enum KeyboardCommand {
+    EnableReporting = 0xF4,
+    SetDefaults = 0xF6,
+    Reset = 0xFF
+}
+
+#[repr(u8)]
+enum KeyboardCommandData {
+    ScancodeSet = 0xF0
+}
+
+#[repr(u8)]
+#[allow(dead_code)]
+enum MouseCommand {
+    GetDeviceId = 0xF2,
+    EnableReporting = 0xF4,
+    SetDefaults = 0xF6,
+    Reset = 0xFF
+}
+
+#[repr(u8)]
+enum MouseCommandData {
+    SetSampleRate = 0xF3,
+}
+
+pub struct Ps2 {
+    data: Pio<u8>,
+    status: ReadOnly<Pio<u8>>,
+    command: WriteOnly<Pio<u8>>
+}
+
+impl Ps2 {
+    pub fn new() -> Self {
+        Ps2 {
+            data: Pio::new(0x60),
+            status: ReadOnly::new(Pio::new(0x64)),
+            command: WriteOnly::new(Pio::new(0x64)),
+        }
+    }
+
+    fn status(&mut self) -> StatusFlags {
+        StatusFlags::from_bits_truncate(self.status.read())
+    }
+
+    fn wait_write(&mut self) {
+        while self.status().contains(INPUT_FULL) {}
+    }
+
+    fn wait_read(&mut self) {
+        while ! self.status().contains(OUTPUT_FULL) {}
+    }
+
+    fn flush_read(&mut self) {
+        while self.status().contains(OUTPUT_FULL) {
+            print!("FLUSH: {:X}\n", self.data.read());
+        }
+    }
+
+    fn command(&mut self, command: Command) {
+        self.wait_write();
+        self.command.write(command as u8);
+    }
+
+    fn read(&mut self) -> u8 {
+        self.wait_read();
+        self.data.read()
+    }
+
+    fn write(&mut self, data: u8) {
+        self.wait_write();
+        self.data.write(data);
+    }
+
+    fn config(&mut self) -> ConfigFlags {
+        self.command(Command::ReadConfig);
+        ConfigFlags::from_bits_truncate(self.read())
+    }
+
+    fn set_config(&mut self, config: ConfigFlags) {
+        self.command(Command::WriteConfig);
+        self.write(config.bits());
+    }
+
+    fn keyboard_command(&mut self, command: KeyboardCommand) -> u8 {
+        self.write(command as u8);
+        self.read()
+    }
+
+    fn keyboard_command_data(&mut self, command: KeyboardCommandData, data: u8) -> u8 {
+        self.write(command as u8);
+        assert_eq!(self.read(), 0xFA);
+        self.write(data as u8);
+        self.read()
+    }
+
+    fn mouse_command(&mut self, command: MouseCommand) -> u8 {
+        self.command(Command::WriteSecond);
+        self.write(command as u8);
+        self.read()
+    }
+
+    fn mouse_command_data(&mut self, command: MouseCommandData, data: u8) -> u8 {
+        self.command(Command::WriteSecond);
+        self.write(command as u8);
+        assert_eq!(self.read(), 0xFA);
+        self.command(Command::WriteSecond);
+        self.write(data as u8);
+        self.read()
+    }
+
+    pub fn init(&mut self) -> bool {
+        // Disable devices
+        self.command(Command::DisableFirst);
+        self.command(Command::DisableSecond);
+
+        // Clear remaining data
+        self.flush_read();
+
+        // Disable clocks, disable interrupts, and disable translate
+        {
+            let mut config = self.config();
+            config.insert(FIRST_DISABLED);
+            config.insert(SECOND_DISABLED);
+            config.remove(FIRST_TRANSLATE);
+            config.remove(FIRST_INTERRUPT);
+            config.remove(SECOND_INTERRUPT);
+            self.set_config(config);
+        }
+
+        // Perform the self test
+        self.command(Command::TestController);
+        assert_eq!(self.read(), 0x55);
+
+        // Enable devices
+        self.command(Command::EnableFirst);
+        self.command(Command::EnableSecond);
+
+        // Reset keyboard
+        assert_eq!(self.keyboard_command(KeyboardCommand::Reset), 0xFA);
+        assert_eq!(self.read(), 0xAA);
+        self.flush_read();
+
+        // Set scancode set to 2
+        assert_eq!(self.keyboard_command_data(KeyboardCommandData::ScancodeSet, 2), 0xFA);
+
+        // Reset mouse and set up scroll
+        // TODO: Check for ack
+        assert_eq!(self.mouse_command(MouseCommand::Reset), 0xFA);
+        assert_eq!(self.read(), 0xAA);
+        assert_eq!(self.read(), 0x00);
+        self.flush_read();
+
+        // Enable extra packet on mouse
+        assert_eq!(self.mouse_command_data(MouseCommandData::SetSampleRate, 200), 0xFA);
+        assert_eq!(self.mouse_command_data(MouseCommandData::SetSampleRate, 100), 0xFA);
+        assert_eq!(self.mouse_command_data(MouseCommandData::SetSampleRate, 80), 0xFA);
+        assert_eq!(self.mouse_command(MouseCommand::GetDeviceId), 0xFA);
+        let mouse_id = self.read();
+        let mouse_extra = mouse_id == 3;
+
+        // Set sample rate to maximum
+        assert_eq!(self.mouse_command_data(MouseCommandData::SetSampleRate, 200), 0xFA);
+
+        // Enable data reporting
+        assert_eq!(self.keyboard_command(KeyboardCommand::EnableReporting), 0xFA);
+        assert_eq!(self.mouse_command(MouseCommand::EnableReporting), 0xFA);
+
+        // Enable clocks and interrupts
+        {
+            let mut config = self.config();
+            config.remove(FIRST_DISABLED);
+            config.remove(SECOND_DISABLED);
+            config.insert(FIRST_TRANSLATE);
+            config.insert(FIRST_INTERRUPT);
+            config.insert(SECOND_INTERRUPT);
+            self.set_config(config);
+        }
+
+        self.flush_read();
+
+        mouse_extra
+    }
+}