diff --git a/src/apps/shell.rs b/src/apps/shell.rs
line changes: +34/-0
index 1b03314..ff5384f
--- a/src/apps/shell.rs
+++ b/src/apps/shell.rs

@@ -133,6 +133,17 @@ fn parse_cmdline(cmdline: &str) -> Result<(), CommandError> {
             stdlib::ps();
             Ok(())
         }
+        "unaligned" => unsafe {
+            let p = 2 as *const u32;
+            print_hex_u32(*p);
+            Ok(())
+        }
+        "instr" => unsafe {
+            let b: &[u16; 2] = &[0xf7f0, 0xa000]; // Should be an undefined instruction
+            let fp: fn() = core::mem::transmute(b);
+            fp();
+            Ok(())
+        }
         "panic" => {
             panic!("Test panic");
         }

@@ -140,6 +151,29 @@ fn parse_cmdline(cmdline: &str) -> Result<(), CommandError> {
             stdlib::kernel_panic();
             Ok(())
         }
+        "k" => {
+            let mut keybits = [0u32; 3];
+            let mut db = [0u8; 1];
+            stdlib::read_keys(&mut keybits);
+            stdlib::print("   c1 c2 c3 c4 c5 c6 c7 c8 c9\r\n");
+            for r in 0..9 {
+                stdlib::print("r");
+                db[0] = 0x30 + r + 1;
+                stdlib::write(&db);
+                for c in 0..9 {
+                    let i = (r * 9 + c) as usize;
+                    if keybits[i / 32] & 1 << (i % 32) != 0 {
+                        db[0] = 0x31;
+                    } else {
+                        db[0] = 0x30;
+                    }
+                    stdlib::print("  ");
+                    stdlib::write(&db);
+                }
+                stdlib::print("\r\n");
+            }
+            Ok(())
+        }
         _ => Err(CommandError::BadCommand),
     }
 }

diff --git a/src/peripherals.rs b/src/peripherals.rs
line changes: +71/-7
index a04f3e3..06e199e
--- a/src/peripherals.rs
+++ b/src/peripherals.rs

@@ -1,3 +1,4 @@
+mod keyboard;
 mod usbdev;
 
 use core::cell::RefCell;

@@ -15,6 +16,8 @@ use bsp::Pins;
 use cortex_m::peripheral::scb::SystemHandler;
 use critical_section::Mutex;
 
+use embedded_hal::digital::v2::PinState;
+pub use keyboard::Keyboard;
 use usbdev::init_usb;
 pub use usbdev::with_usb;
 

@@ -22,11 +25,13 @@ const EXTERNAL_XTAL_FREQ_HZ: u32 = 12_000_000;
 
 static PERIPHERALS: Mutex<RefCell<Option<Peripherals>>> = Mutex::new(RefCell::new(None));
 
-type LedPin = gpio::Pin<gpio::bank0::Gpio7, gpio::Output<gpio::PushPull>>;
+type LedPin = gpio::Pin<gpio::bank0::Gpio7, gpio::FunctionSioOutput, gpio::PullNone>;
+
 pub struct Peripherals {
     watchdog: Watchdog,
     timer: Timer,
     led: LedPin,
+    keyboard: Keyboard,
 }
 
 impl Peripherals {

@@ -37,6 +42,10 @@ impl Peripherals {
     pub fn led(&mut self) -> &mut LedPin {
         &mut self.led
     }
+
+    pub fn keyboard(&mut self) -> &mut Keyboard {
+        &mut self.keyboard
+    }
 }
 
 pub fn init_peripherals() {

@@ -68,7 +77,61 @@ pub fn init_peripherals() {
         sio.gpio_bank0,
         &mut platform.RESETS,
     );
-    let led = pins.led.into_push_pull_output();
+    let led = pins.led.into_push_pull_output().into_pull_type();
+    let keyboard = Keyboard::new(
+        [
+            pins.kb_c1.into_pull_up_input().into_dyn_pin(),
+            pins.kb_c2.into_pull_up_input().into_dyn_pin(),
+            pins.kb_c3.into_pull_up_input().into_dyn_pin(),
+            pins.kb_c4.into_pull_up_input().into_dyn_pin(),
+            pins.kb_c5.into_pull_up_input().into_dyn_pin(),
+            pins.kb_c6.into_pull_up_input().into_dyn_pin(),
+            pins.kb_c7.into_pull_up_input().into_dyn_pin(),
+            pins.kb_c8.into_pull_up_input().into_dyn_pin(),
+            pins.kb_c9.into_pull_up_input().into_dyn_pin(),
+        ],
+        [
+            pins.kb_r1
+                .into_push_pull_output_in_state(PinState::High)
+                .into_pull_type()
+                .into_dyn_pin(),
+            pins.kb_r2
+                .into_push_pull_output_in_state(PinState::High)
+                .into_pull_type()
+                .into_dyn_pin(),
+            pins.kb_r3
+                .into_push_pull_output_in_state(PinState::High)
+                .into_pull_type()
+                .into_dyn_pin(),
+            pins.kb_r4
+                .into_push_pull_output_in_state(PinState::High)
+                .into_pull_type()
+                .into_dyn_pin(),
+            pins.kb_r5
+                .into_push_pull_output_in_state(PinState::High)
+                .into_pull_type()
+                .into_dyn_pin(),
+            pins.kb_r6
+                .into_push_pull_output_in_state(PinState::High)
+                .into_pull_type()
+                .into_dyn_pin(),
+            pins.kb_r7
+                .into_push_pull_output_in_state(PinState::High)
+                .into_pull_type()
+                .into_dyn_pin(),
+            pins.kb_r8
+                .into_push_pull_output_in_state(PinState::High)
+                .into_pull_type()
+                .into_dyn_pin(),
+            pins.kb_r9
+                .into_push_pull_output_in_state(PinState::High)
+                .into_pull_type()
+                .into_dyn_pin(),
+        ],
+    );
+
+    init_timer(core.SYST);
+    let timer = Timer::new(platform.TIMER, &mut platform.RESETS, &clocks);
 
     init_usb(
         platform.USBCTRL_REGS,

@@ -80,9 +143,6 @@ pub fn init_peripherals() {
         pac::NVIC::unmask(pac::Interrupt::USBCTRL_IRQ);
     }
 
-    init_timer(core.SYST);
-    let timer = Timer::new(platform.TIMER, &mut platform.RESETS);
-
     init_mpu(core.MPU);
 
     critical_section::with(|cs| {

@@ -91,11 +151,15 @@ pub fn init_peripherals() {
             watchdog,
             timer,
             led,
+            keyboard,
         });
     })
 }
 
-pub fn with_peripherals<F, R>(mut f: F) -> R where F: FnMut(&mut Peripherals) -> R {
+pub fn with_peripherals<F, R>(mut f: F) -> R
+where
+    F: FnMut(&mut Peripherals) -> R,
+{
     critical_section::with(|cs| {
         let mut peripherals = PERIPHERALS.borrow_ref_mut(cs);
         if let Some(ref mut peripherals) = *peripherals {

@@ -104,4 +168,4 @@ pub fn with_peripherals<F, R>(mut f: F) -> R where F: FnMut(&mut Peripherals) ->
             panic!("Peripherals not initialized");
         }
     })
-} 
\ No newline at end of file
+}

diff --git a/src/peripherals/keyboard.rs b/src/peripherals/keyboard.rs
line changes: +38/-0
index 0000000..58d7a26
--- /dev/null
+++ b/src/peripherals/keyboard.rs

@@ -0,0 +1,38 @@
+use core::arch::asm;
+
+use doa_hallonbrod::hal::gpio::{DynPinId, FunctionSioInput, FunctionSioOutput, Pin, PullNone, PullUp};
+use embedded_hal::digital::v2::{InputPin, OutputPin};
+
+type ColPin = Pin<DynPinId, FunctionSioInput, PullUp>;
+type RowPin = Pin<DynPinId, FunctionSioOutput, PullNone>;
+
+pub struct Keyboard {
+    cols: [ColPin; 9],
+    rows: [RowPin; 9],
+}
+
+impl Keyboard
+{
+    pub fn new(cols: [ColPin; 9], rows: [RowPin; 9]) -> Keyboard {
+        Keyboard { cols, rows }
+    }
+
+    pub fn scan(&mut self) -> [u32; 3] {
+        let mut keybits = [0u32; 3];
+
+        for (r, rp) in self.rows.iter_mut().enumerate() {
+            rp.set_low().unwrap(); // Activate column
+            cortex_m::asm::delay(10);
+            for (c, cp) in self.cols.iter().enumerate() {
+                if cp.is_low().unwrap() { // Read row
+                    let i = r * 9 + c;
+                    keybits[i / 32] |= 1 << (i % 32);
+                }
+            }
+            rp.set_high().unwrap(); // Deactivate column
+            cortex_m::asm::delay(2000);
+        }
+
+        keybits
+    }
+}

diff --git a/src/stdlib.rs b/src/stdlib.rs
line changes: +5/-0
index feab363..b541c45
--- a/src/stdlib.rs
+++ b/src/stdlib.rs

@@ -60,4 +60,9 @@ pub fn ps() {
 
 pub fn kernel_panic() {
     unsafe { _svc_call(200, 0, 0, 0); }
+}
+
+pub fn read_keys(buf: &mut [u32; 3]) {
+    let ptr = buf.as_ptr() as u32;
+    unsafe { _svc_call(1000, ptr, 0, 0); }
 } 
\ No newline at end of file

diff --git a/src/svc.rs b/src/svc.rs
line changes: +10/-7
index bbe8a98..13e2e27
--- a/src/svc.rs
+++ b/src/svc.rs

@@ -6,7 +6,7 @@ use crate::bsp::pac::SCB;
 use crate::capabilities::CapType;
 use crate::console::{self, ConsoleError};
 use crate::peripherals::with_peripherals;
-use crate::task::{with_task_manager, TaskRegisters, current_task_has_capability};
+use crate::task::{current_task_has_capability, with_task_manager, TaskRegisters};
 use crate::timer::ticks;
 
 global_asm!(

@@ -34,7 +34,7 @@ unsafe fn svcall_handler(regs: Option<&mut TaskRegisters>) {
             if !current_task_has_capability(CapType::ConsoleWrite) {
                 return;
             }
-        
+
             let bytes = core::slice::from_raw_parts(regs.r1 as *const u8, regs.r2 as usize);
             match console::write(bytes) {
                 Ok(n) => regs.r0 = n as u32,

@@ -58,7 +58,7 @@ unsafe fn svcall_handler(regs: Option<&mut TaskRegisters>) {
                         t.block_current_task(CapType::ConsoleRead);
                         SCB::set_pendsv();
                     });
-                },
+                }
                 Err(e) => panic!("read syscall error: {:?}", e),
             }
         }

@@ -83,9 +83,7 @@ unsafe fn svcall_handler(regs: Option<&mut TaskRegisters>) {
         101 => {
             let entry = regs.r1 as *const ();
             let entry = core::mem::transmute(entry);
-            let r = with_task_manager(|tm| {
-                tm.add_task("TODO", entry, &[], 10)
-            });
+            let r = with_task_manager(|tm| tm.add_task("TODO", entry, &[], 10));
             match r {
                 Ok(tid) => regs.r0 = tid,
                 Err(_) => regs.r0 = 0xFFFFFFFF,

@@ -99,6 +97,11 @@ unsafe fn svcall_handler(regs: Option<&mut TaskRegisters>) {
             let b = *p;
             regs.r0 = b as u32;
         }
+        1000 => {
+            let keys = core::slice::from_raw_parts_mut(regs.r1 as *mut u32, 3);
+            let keybits = with_peripherals(|p| p.keyboard().scan());
+            keys.copy_from_slice(&keybits);
+        }
         _ => panic!("Unknown SVCall"),
     }
 }

@@ -127,4 +130,4 @@ unsafe fn pendsv_handler(regs: Option<&mut TaskRegisters>) {
         tm.task_swap(regs);
     });
     SCB::clear_pendsv();
-} 
\ No newline at end of file
+}

diff --git a/src/task.rs b/src/task.rs
line changes: +12/-1
index 5257601..258d682
--- a/src/task.rs
+++ b/src/task.rs

@@ -15,10 +15,10 @@ pub use error::TaskError;
 
 use crate::bsp::pac::SCB;
 use crate::capabilities::{with_cap_registry, CapToken, CapType, MAX_CAPS};
-use crate::kprintf;
 use crate::mpu::{MemoryMap, MemorySegment, N_PAGES};
 use crate::peripherals::with_peripherals;
 use crate::timer::{ticks, Ticks};
+use crate::{apps, kprint, kprintf};
 
 const MAX_TASKS: usize = 10;
 

@@ -114,6 +114,17 @@ impl TaskManager {
             panic!("Scheduling with no tasks!");
         }
 
+        if self.tasks.len() == 1 && self.tasks[0].id == 1 {
+            kprint!("Only idle task remains - relaunching shell\r\n");
+            self.add_task(
+                "shell",
+                apps::shell::shell,
+                &[CapType::ConsoleRead, CapType::ConsoleWrite],
+                0,
+            )
+            .expect("cannot relaunch shell");
+        }
+
         // Check if any tasks are done sleeping
         for t in &mut self.tasks {
             if let TaskState::Sleeping(t1) = t.state {