Add HardFault handler
- expanded kernel memory down into last 4K of main SRAM
- added console flush()
- improve the panic handler a little bit
   
    
         BOOT2 (r)   : ORIGIN = 0x10000000, LENGTH = 0x100
     FLASH (rx)  : ORIGIN = 0x10000100, LENGTH = 1024K - 0x100
     ARAM  (rw)  : ORIGIN = 0x20000000, LENGTH = 256K
-    RAM   (rw)  : ORIGIN = 0x20040000, LENGTH = 4K
+    RAM   (rw)  : ORIGIN = 0x2003F000, LENGTH = 8K
     RAM2  (rw)  : ORIGIN = 0x20041000, LENGTH = 4K
 }
 
    
   
    
                 stdlib::ps();
             Ok(())
         }
+        "panic" => {
+            panic!("Test panic");
+        }
+        "kernelpanic" => {
+            stdlib::kernel_panic();
+            Ok(())
+        }
         _ => {
             Err(CommandError::BadCommand)
         }
    
   
    
         }
 
     pub fn write(&mut self, bytes: impl AsRef<[u8]>) -> Result<usize, ConsoleError> {
-        with_usb(|u| {
-            match u.serial().write(bytes.as_ref()) {
-                Ok(n) => Ok(n),
-                Err(UsbError::WouldBlock) => Err(ConsoleError::Blocked),
-                Err(_) => Err(ConsoleError::Write),
-            }
+        with_usb(|u| match u.serial().write(bytes.as_ref()) {
+            Ok(n) => Ok(n),
+            Err(UsbError::WouldBlock) => Err(ConsoleError::Blocked),
+            Err(_) => Err(ConsoleError::Write),
         })
     }
 
         })
 }
 
+pub fn flush() {
+    with_usb(
+        |u| {
+            while let Err(UsbError::WouldBlock) = u.serial().flush() {}
+        },
+    );
+}
+
 pub(crate) fn _printf(s: core::fmt::Arguments) {
     let mut buf: heapless::Vec<u8, 256> = heapless::Vec::new();
     write!(&mut buf, "{}", s).expect("write!");
    
   
    
    +use core::arch::global_asm;
+
+use crate::console::flush;
+use crate::kprintf;
+use crate::task::TaskRegisters;
+
+fn print_regs(regs: &TaskRegisters) {
+    kprintf!(
+        "R0:  {:08X}  R1:  {:08X}  R2:  {:08X}  R3:  {:08X}\r\n",
+        regs.r0,
+        regs.r1,
+        regs.r2,
+        regs.r3
+    );
+    kprintf!(
+        "R4:  {:08X}  R5:  {:08X}  R6:  {:08X}  R7:  {:08X}\r\n",
+        regs.r4,
+        regs.r5,
+        regs.r6,
+        regs.r7
+    );
+    kprintf!(
+        "R8:  {:08X}  R9:  {:08X}  R10: {:08X}  R11: {:08X}\r\n",
+        regs.r8,
+        regs.r9,
+        regs.r10,
+        regs.r11
+    );
+    kprintf!(
+        "R12: {:08X}  SP:  {:08X}  LR:  {:08X}  PC:  {:08X}\r\n",
+        regs.r12,
+        regs.sp,
+        regs.lr,
+        regs.pc
+    );
+    kprintf!("XPSR: {:08X}\r\n", regs.xpsr);
+}
+
+unsafe fn print_stack(regs: &TaskRegisters) {
+    let sp = regs.sp as *const u32;
+
+    kprintf!("SP: {:08X}\r\n", sp as u32);
+
+    for i in 0..4 {
+        kprintf!("{:08X}:", (sp as u32) + i * 32);
+        for j in 0..8 {
+            let d = *sp.offset((i * 8 + j) as isize);
+            kprintf!(" {:08X}", d);
+        }
+        kprintf!("\r\n");
+    }
+}
+
+// This one is slightly different than the usual ExceptionEntry/Exit
+// because we want to know the registers even if we came from handler
+// mode.
+global_asm!(
+    r#"
+    .section .HardFault, "ax"
+    .global HardFault
+    .type HardFault,function
+HardFault:
+
+    // stack the registers
+    push {{r4, r5, r6, r7}} // push r4-r7
+    mov r0, r8
+    mov r1, r9
+    mov r2, r10
+    mov r3, r11
+    push {{r0, r1, r2, r3}} // push r8-r11
+
+    mov r3, lr
+    movs r1, #0x04       // Check which mode we just came from
+    ands r3, r1
+    beq 2f              // if we came from thread mode,
+    mrs r0, PSP         // use the process stack
+    b 3f
+2:  mrs r0, MSP         // otherwise, use main stack
+3:
+    push {{r0}}         // push SP
+    sub sp, #32         // reserve space for the next eight registers
+    mov r1, sp          // copy the stack pointer
+    ldm r0!, {{r4, r5, r6, r7}} // load r0-r3
+    stm r1!, {{r4, r5, r6, r7}} // store r0-r3
+    ldm r0!, {{r4, r5, r6, r7}} // load r12, lr, pc, xpsr
+    stm r1!, {{r4, r5, r6, r7}} // save r12, lr, pc, xpsr
+    mov r0, sp          // get address of TaskRegisters struct
+    mov r1, r3
+    movs r2, 2          // shift the thread mode flag right so it's a
+    rors r1, r2         // bool and it's the second argument
+    push {{lr}}         // save LR
+    bl hardfault_handler   // call the handler
+    // TODO: kill process and continue
+"#
+);
+
+#[no_mangle]
+unsafe fn hardfault_handler(regs: &mut TaskRegisters, thread_mode: bool) -> ! {
+    kprintf!(
+        "\r\nHardFault! ({})\r\n",
+        if thread_mode { "thread" } else { "handler" }
+    );
+    print_regs(regs);
+    kprintf!("\r\n");
+
+    print_stack(regs);
+    kprintf!("\r\n");
+
+    let scb = &(*cortex_m::peripheral::SCB::PTR);
+    kprintf!("ICSR: {:08X}\r\n", scb.icsr.read());
+    flush();
+    cortex_m::interrupt::disable();
+
+    loop {}
+}
    
   
    
     mod asm;
 mod capabilities;
 mod console;
+mod hardfault;
 mod panic;
 mod peripherals;
 mod stdlib;
    
   
    
     use core::fmt::Write;
 use core::panic::PanicInfo;
 
-use crate::console::{write, ConsoleError};
+use crate::console::{flush, write, ConsoleError};
 
 static mut ALREADY_PANICKING: bool = false;
 
                 write(b"could not get payload")?;
         }
     }
-    write(b"\n")?;
+    write(b"\r\n")?;
 
     if let Some(location) = info.location() {
         write!(
             .expect("write!");
         write(&format_buffer)?;
     } else {
-        write(b"Unknown location\n")?;
+        write(b"Unknown location")?;
     }
     write(b"\r\n")?;
 
             }
         ALREADY_PANICKING = true;
     }
-    cortex_m::interrupt::disable();
-
     print_panic(info).expect("rut roh");
 
+    flush();
+
+    cortex_m::interrupt::disable();
+
     loop {}
 }
    
   
    
     
 pub fn ps() {
     unsafe { _svc_call(102, 0, 0, 0); }
+}
+
+pub fn kernel_panic() {
+    unsafe { _svc_call(200, 0, 0, 0); }
 } 
\ No newline at end of file
    
   
    
             102 => {
             with_task_manager(|tm| tm.print_tasks());
         }
+        200 => {
+            let p = 0x4000 as *const u8;
+            let b = *p;
+            regs.r0 = b as u32;
+        }
         _ => panic!("Unknown SVCall"),
     }
 }