use cortex_m_rt::ExceptionFrame;
use crate::console::get_global_console;
use crate::util::{print_dec, print_hex_padded};

global_asm!(r#"
    .global SVCall
    .type SVCall,function
SVCall:
    // Store a pointer to registers in r1
    mrs r1, PSP
    // find the svc number from the app stack, store it in r0
    ldr r0, [r1, #24]
    ldr r0, [r0, #-2]
    and r0, 0xFF
    // call service handler
    b svcall_handler
"#);

#[no_mangle]
unsafe extern "C" fn svcall_handler(svc: u8, regs: &mut ExceptionFrame) {
    match svc {
        0 => {
            regs.r0 = &crate::C_DRIVER_INTERFACES as *const crate::CDriverInterfaces as u32
        },
        1 => {
            regs.r0 = &crate::RUST_DRIVER_INTERFACES as *const crate::RustDriverInterfaces as u32
        },
        _ => {
            let console = get_global_console();
            console.write("unknown svcall ");
            print_dec(svc);
            console.write(": ");

            print_hex_padded(regs.r0, 8);
            console.write(" ");
            print_hex_padded(regs.r1, 8);
            console.write(" ");
            print_hex_padded(regs.r2, 8);
            console.write(" ");
            print_hex_padded(regs.r3, 8);

            console.write("\r\n");
        },
    }
}