Initial drop

Add some riscv code

Signed-off-by: Carlos Maiolino <[email protected]>

58 files changed, 4544 insertions, 0 deletions

diff --git a/riscv/riscv-probe/libfemto/README.md b/riscv/riscv-probe/libfemto/README.md
new file mode 100644
index 0000000..86cd934
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/README.md
@@ -0,0 +1,28 @@
+# libfemto
+
+libfemto is a lightweight bare-metal C library for embedded RISC-V development.
+libfemto provides:
+
+- Reduced set of the _POSIX.1-2017 / IEEE 1003.1-2017_ standard
+- Simple lightweight hardware configuration mechanism
+- RISC-V machine mode functions and macros
+- Console and power device drivers
+
+libfemto implements a reduced set of the _POSIX.1-2017 / IEEE 1003.1-2017_
+standard, with the addition of glibc's `getauxval` API to access hardware
+configuration in an auxiliary vector (`__auxv`) that contains tuples
+describing the target environment. The auxiliary vector is intended as a
+lightweight mechanism to pass dynamic configuration information on embedded
+targets, serving as an alternative to compile time constants used during
+hardware initialization. The auxiliary vector API has been repurposed to
+allow retrieval of hardware configuration parameters such as clock
+frequencies and device base addresses for use as a compact alternative to
+(DTB) Device Tree Binary, which is not available on small embedded targets.
+
+libfemto contains the following device drivers:
+
+- HTIF (Host Target Interface)
+- NS16550A UART Console
+- SiFive UART Console
+- SiFive Test Device
+- Semihosting Syscalls
diff --git a/riscv/riscv-probe/libfemto/arch/riscv/auxval.c b/riscv/riscv-probe/libfemto/arch/riscv/auxval.c
new file mode 100644
index 0000000..eff10d3
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/arch/riscv/auxval.c
@@ -0,0 +1,15 @@
+// See LICENSE for license details.
+
+#include "femto.h"
+
+unsigned long getauxval(unsigned long key)
+{
+    auxval_t *auxv = __auxv;
+    while(auxv->key) {
+        if (auxv->key == key) {
+            return auxv->val;
+        }
+        auxv++;
+    }
+    return 0;
+}
diff --git a/riscv/riscv-probe/libfemto/arch/riscv/csr.c b/riscv/riscv-probe/libfemto/arch/riscv/csr.c
new file mode 100644
index 0000000..673e87e
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/arch/riscv/csr.c
@@ -0,0 +1,284 @@
+// See LICENSE for license details.
+
+#include "femto.h"
+#include "arch/riscv/csr.h"
+#include "arch/riscv/encoding.h"
+#include "arch/riscv/machine.h"
+
+static int all_csr_enums[] = {
+    csr_fflags,
+    csr_frm,
+    csr_fcsr,
+    csr_mcycle,
+    csr_minstret,
+    csr_mcycleh,
+    csr_minstreth,
+    csr_cycle,
+    csr_time,
+    csr_instret,
+    csr_cycleh,
+    csr_timeh,
+    csr_instreth,
+    csr_mvendorid,
+    csr_marchid,
+    csr_mimpid,
+    csr_mhartid,
+    csr_mstatus,
+    csr_misa,
+    csr_medeleg,
+    csr_mideleg,
+    csr_mie,
+    csr_mtvec,
+    csr_mcounteren,
+    csr_mscratch,
+    csr_mepc,
+    csr_mcause,
+    csr_mtval,
+    csr_mip,
+    csr_sstatus,
+    csr_sedeleg,
+    csr_sideleg,
+    csr_sie,
+    csr_stvec,
+    csr_scounteren,
+    csr_sscratch,
+    csr_sepc,
+    csr_scause,
+    csr_stval,
+    csr_sip,
+    csr_satp,
+    csr_pmpcfg0,
+    csr_pmpcfg1,
+    csr_pmpcfg2,
+    csr_pmpcfg3,
+    csr_pmpaddr0,
+    csr_pmpaddr1,
+    csr_pmpaddr2,
+    csr_pmpaddr3,
+    csr_pmpaddr4,
+    csr_pmpaddr5,
+    csr_pmpaddr6,
+    csr_pmpaddr7,
+    csr_pmpaddr8,
+    csr_pmpaddr9,
+    csr_pmpaddr10,
+    csr_pmpaddr11,
+    csr_pmpaddr12,
+    csr_pmpaddr13,
+    csr_pmpaddr14,
+    csr_pmpaddr15,
+    csr_none,
+};
+
+static const char* all_csr_names[] = {
+    "none",
+    "fflags",
+    "frm",
+    "fcsr",
+    "mcycle",
+    "minstret",
+    "mcycleh",
+    "minstreth",
+    "cycle",
+    "time",
+    "instret",
+    "cycleh",
+    "timeh",
+    "instreth",
+    "mvendorid",
+    "marchid",
+    "mimpid",
+    "mhartid",
+    "mstatus",
+    "misa",
+    "medeleg",
+    "mideleg",
+    "mie",
+    "mtvec",
+    "mcounteren",
+    "mscratch",
+    "mepc",
+    "mcause",
+    "mtval",
+    "mip",
+    "sstatus",
+    "sedeleg",
+    "sideleg",
+    "sie",
+    "stvec",
+    "scounteren",
+    "sscratch",
+    "sepc",
+    "scause",
+    "stval",
+    "sip",
+    "satp",
+    "pmpcfg0",
+    "pmpcfg1",
+    "pmpcfg2",
+    "pmpcfg3",
+    "pmpaddr0",
+    "pmpaddr1",
+    "pmpaddr2",
+    "pmpaddr3",
+    "pmpaddr4",
+    "pmpaddr5",
+    "pmpaddr6",
+    "pmpaddr7",
+    "pmpaddr8",
+    "pmpaddr9",
+    "pmpaddr10",
+    "pmpaddr11",
+    "pmpaddr12",
+    "pmpaddr13",
+    "pmpaddr14",
+    "pmpaddr15"
+};
+
+int* csr_enum_array()
+{
+    return all_csr_enums;
+}
+
+const char** csr_name_array()
+{
+    return all_csr_names;
+}
+
+long read_csr_enum(int csrenum)
+{
+    long result = -1;
+    switch (csrenum) {
+    case csr_fflags:     result = read_csr(0x001); break;
+    case csr_frm:        result = read_csr(0x002); break;
+    case csr_fcsr:       result = read_csr(0x003); break;
+    case csr_mcycle:     result = read_csr(0xB00); break;
+    case csr_minstret:   result = read_csr(0xB02); break;
+    case csr_mcycleh:    result = read_csr(0xB80); break;
+    case csr_minstreth:  result = read_csr(0xB82); break;
+    case csr_cycle:      result = read_csr(0xC00); break;
+    case csr_time:       result = read_csr(0xC01); break;
+    case csr_instret:    result = read_csr(0xC02); break;
+    case csr_cycleh:     result = read_csr(0xC80); break;
+    case csr_timeh:      result = read_csr(0xC81); break;
+    case csr_instreth:   result = read_csr(0xC82); break;
+    case csr_mvendorid:  result = read_csr(0xF11); break;
+    case csr_marchid:    result = read_csr(0xF12); break;
+    case csr_mimpid:     result = read_csr(0xF13); break;
+    case csr_mhartid:    result = read_csr(0xF14); break;
+    case csr_mstatus:    result = read_csr(0x300); break;
+    case csr_misa:       result = read_csr(0x301); break;
+    case csr_medeleg:    result = read_csr(0x302); break;
+    case csr_mideleg:    result = read_csr(0x303); break;
+    case csr_mie:        result = read_csr(0x304); break;
+    case csr_mtvec:      result = read_csr(0x305); break;
+    case csr_mcounteren: result = read_csr(0x306); break;
+    case csr_mscratch:   result = read_csr(0x340); break;
+    case csr_mepc:       result = read_csr(0x341); break;
+    case csr_mcause:     result = read_csr(0x342); break;
+    case csr_mtval:      result = read_csr(0x343); break;
+    case csr_mip:        result = read_csr(0x344); break;
+    case csr_sstatus:    result = read_csr(0x100); break;
+    case csr_sedeleg:    result = read_csr(0x102); break;
+    case csr_sideleg:    result = read_csr(0x103); break;
+    case csr_sie:        result = read_csr(0x104); break;
+    case csr_stvec:      result = read_csr(0x105); break;
+    case csr_scounteren: result = read_csr(0x106); break;
+    case csr_sscratch:   result = read_csr(0x140); break;
+    case csr_sepc:       result = read_csr(0x141); break;
+    case csr_scause:     result = read_csr(0x142); break;
+    case csr_stval:      result = read_csr(0x143); break;
+    case csr_sip:        result = read_csr(0x144); break;
+    case csr_satp:       result = read_csr(0x180); break;
+    case csr_pmpcfg0:    result = read_csr(0x3A0); break;
+    case csr_pmpcfg1:    result = read_csr(0x3A1); break;
+    case csr_pmpcfg2:    result = read_csr(0x3A2); break;
+    case csr_pmpcfg3:    result = read_csr(0x3A3); break;
+    case csr_pmpaddr0:   result = read_csr(0x3B0); break;
+    case csr_pmpaddr1:   result = read_csr(0x3B1); break;
+    case csr_pmpaddr2:   result = read_csr(0x3B2); break;
+    case csr_pmpaddr3:   result = read_csr(0x3B3); break;
+    case csr_pmpaddr4:   result = read_csr(0x3B4); break;
+    case csr_pmpaddr5:   result = read_csr(0x3B5); break;
+    case csr_pmpaddr6:   result = read_csr(0x3B6); break;
+    case csr_pmpaddr7:   result = read_csr(0x3B7); break;
+    case csr_pmpaddr8:   result = read_csr(0x3B8); break;
+    case csr_pmpaddr9:   result = read_csr(0x3B9); break;
+    case csr_pmpaddr10:  result = read_csr(0x3BA); break;
+    case csr_pmpaddr11:  result = read_csr(0x3BB); break;
+    case csr_pmpaddr12:  result = read_csr(0x3BC); break;
+    case csr_pmpaddr13:  result = read_csr(0x3BD); break;
+    case csr_pmpaddr14:  result = read_csr(0x3BE); break;
+    case csr_pmpaddr15:  result = read_csr(0x3BF); break;
+    default: break;
+    }
+    return result;
+}
+
+void write_csr_enum(int csrenum, long value)
+{
+    switch (csrenum) {
+    case csr_fflags:     write_csr(0x001, value); break;
+    case csr_frm:        write_csr(0x002, value); break;
+    case csr_fcsr:       write_csr(0x003, value); break;
+    case csr_mcycle:     write_csr(0xB00, value); break;
+    case csr_minstret:   write_csr(0xB02, value); break;
+    case csr_mcycleh:    write_csr(0xB80, value); break;
+    case csr_minstreth:  write_csr(0xB82, value); break;
+    case csr_cycle:      write_csr(0xC00, value); break;
+    case csr_time:       write_csr(0xC01, value); break;
+    case csr_instret:    write_csr(0xC02, value); break;
+    case csr_cycleh:     write_csr(0xC80, value); break;
+    case csr_timeh:      write_csr(0xC81, value); break;
+    case csr_instreth:   write_csr(0xC82, value); break;
+    case csr_mvendorid:  write_csr(0xF11, value); break;
+    case csr_marchid:    write_csr(0xF12, value); break;
+    case csr_mimpid:     write_csr(0xF13, value); break;
+    case csr_mhartid:    write_csr(0xF14, value); break;
+    case csr_mstatus:    write_csr(0x300, value); break;
+    case csr_misa:       write_csr(0x301, value); break;
+    case csr_medeleg:    write_csr(0x302, value); break;
+    case csr_mideleg:    write_csr(0x303, value); break;
+    case csr_mie:        write_csr(0x304, value); break;
+    case csr_mtvec:      write_csr(0x305, value); break;
+    case csr_mcounteren: write_csr(0x306, value); break;
+    case csr_mscratch:   write_csr(0x340, value); break;
+    case csr_mepc:       write_csr(0x341, value); break;
+    case csr_mcause:     write_csr(0x342, value); break;
+    case csr_mtval:      write_csr(0x343, value); break;
+    case csr_mip:        write_csr(0x344, value); break;
+    case csr_sstatus:    write_csr(0x100, value); break;
+    case csr_sedeleg:    write_csr(0x102, value); break;
+    case csr_sideleg:    write_csr(0x103, value); break;
+    case csr_sie:        write_csr(0x104, value); break;
+    case csr_stvec:      write_csr(0x105, value); break;
+    case csr_scounteren: write_csr(0x106, value); break;
+    case csr_sscratch:   write_csr(0x140, value); break;
+    case csr_sepc:       write_csr(0x141, value); break;
+    case csr_scause:     write_csr(0x142, value); break;
+    case csr_stval:      write_csr(0x143, value); break;
+    case csr_sip:        write_csr(0x144, value); break;
+    case csr_satp:       write_csr(0x180, value); break;
+    case csr_pmpcfg0:    write_csr(0x3A0, value); break;
+    case csr_pmpcfg1:    write_csr(0x3A1, value); break;
+    case csr_pmpcfg2:    write_csr(0x3A2, value); break;
+    case csr_pmpcfg3:    write_csr(0x3A3, value); break;
+    case csr_pmpaddr0:   write_csr(0x3B0, value); break;
+    case csr_pmpaddr1:   write_csr(0x3B1, value); break;
+    case csr_pmpaddr2:   write_csr(0x3B2, value); break;
+    case csr_pmpaddr3:   write_csr(0x3B3, value); break;
+    case csr_pmpaddr4:   write_csr(0x3B4, value); break;
+    case csr_pmpaddr5:   write_csr(0x3B5, value); break;
+    case csr_pmpaddr6:   write_csr(0x3B6, value); break;
+    case csr_pmpaddr7:   write_csr(0x3B7, value); break;
+    case csr_pmpaddr8:   write_csr(0x3B8, value); break;
+    case csr_pmpaddr9:   write_csr(0x3B9, value); break;
+    case csr_pmpaddr10:  write_csr(0x3BA, value); break;
+    case csr_pmpaddr11:  write_csr(0x3BB, value); break;
+    case csr_pmpaddr12:  write_csr(0x3BC, value); break;
+    case csr_pmpaddr13:  write_csr(0x3BD, value); break;
+    case csr_pmpaddr14:  write_csr(0x3BE, value); break;
+    case csr_pmpaddr15:  write_csr(0x3BF, value); break;
+    default: break;
+    }
+}
diff --git a/riscv/riscv-probe/libfemto/arch/riscv/device.c b/riscv/riscv-probe/libfemto/arch/riscv/device.c
new file mode 100644
index 0000000..9c3a6bd
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/arch/riscv/device.c
@@ -0,0 +1,53 @@
+// See LICENSE for license details.
+
+#include "femto.h"
+
+void register_console(console_device_t *dev)
+{
+    console_dev = dev;
+    if (dev->init) {
+        dev->init();
+    }
+}
+
+void register_poweroff(poweroff_device_t *dev)
+{
+    poweroff_dev = dev;
+    if (dev->init) {
+        dev->init();
+    }
+}
+
+static int default_getchar()
+{
+    asm volatile("ebreak");
+    return 0;
+}
+
+static int default_putchar(int ch)
+{
+    asm volatile("ebreak");
+    return 0;
+}
+
+static void default_poweroff(int status)
+{
+    asm volatile("ebreak");
+    while (1) {
+        asm volatile("" : : : "memory");
+    }
+}
+
+console_device_t console_none = {
+    NULL,
+    default_getchar,
+    default_putchar
+};
+
+poweroff_device_t poweroff_none = {
+    NULL,
+    default_poweroff,
+};
+
+console_device_t *console_dev = &console_none;
+poweroff_device_t *poweroff_dev = &poweroff_none;
diff --git a/riscv/riscv-probe/libfemto/arch/riscv/memory.c b/riscv/riscv-probe/libfemto/arch/riscv/memory.c
new file mode 100644
index 0000000..440e67c
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/arch/riscv/memory.c
@@ -0,0 +1,50 @@
+// See LICENSE for license details.
+
+#include "femto.h"
+#include "arch/riscv/trap.h"
+#include "arch/riscv/encoding.h"
+#include "arch/riscv/machine.h"
+
+extern char _memory_start;
+
+static memory_info_t memory_info = {
+    .start = -1UL,
+    .end = -1UL
+};
+
+static volatile int save_cause = -1;
+
+static void trap_save_cause(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
+{
+    save_cause = mcause;
+    write_csr(mepc, mepc + 4);
+}
+
+uintptr_t memory_probe_range(uintptr_t start, uintptr_t end)
+{
+    trap_fn save = get_trap_fn();
+    set_trap_fn(trap_save_cause);
+    volatile uintptr_t *p = (volatile uintptr_t *)start;
+    for (; p < (uintptr_t *)end; p += RISCV_PGSIZE) {
+        /* trap_save_cause adds 4 to the PC so we
+         * can't emit compressed instructions */
+        asm volatile (".option push");
+        asm volatile (".option norvc");
+        asm volatile ("" : : "r" (*p));
+        asm volatile (".option pop");
+        if (save_cause != -1) {
+            break;
+        }
+    }
+    set_trap_fn(save);
+    return ((uintptr_t)p)-1;
+}
+
+memory_info_t memory_probe()
+{
+    if (memory_info.start == -1UL) {
+        memory_info.start = (uintptr_t)&_memory_start,
+        memory_info.end = memory_probe_range((uintptr_t)&_memory_start, -1UL);
+    }
+    return memory_info;
+}
diff --git a/riscv/riscv-probe/libfemto/arch/riscv/pmp.c b/riscv/riscv-probe/libfemto/arch/riscv/pmp.c
new file mode 100644
index 0000000..a540527
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/arch/riscv/pmp.c
@@ -0,0 +1,191 @@
+// See LICENSE for license details.
+
+#include "femto.h"
+#include "arch/riscv/csr.h"
+#include "arch/riscv/trap.h"
+#include "arch/riscv/encoding.h"
+#include "arch/riscv/machine.h"
+
+static pmp_info_t pmp_info = {
+    .width = -1,
+    .count = -1,
+    .granularity = -1
+};
+
+static void trap_save_cause(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
+{
+    write_csr(mepc, mepc + 4);
+}
+
+pmp_info_t pmp_probe()
+{
+    trap_fn save;
+
+    if (pmp_info.count >= 0) {
+        return pmp_info;
+    }
+
+    /* loop through PMPs checking we can set any bits */
+    save = get_trap_fn();
+    set_trap_fn(trap_save_cause);
+    pmp_info.count = 0;
+    for (size_t i = 0; i < PMPADDR_COUNT; i++) {
+        uintptr_t addr, addrsave = read_csr_enum(csr_pmpaddr0 + i);
+        write_csr_enum(csr_pmpaddr0 + i, -1UL);
+        addr = read_csr_enum(csr_pmpaddr0 + i);
+        write_csr_enum(csr_pmpaddr0 + i, addrsave);
+        if (addr) {
+            pmp_info.count++;
+            if (i == 0) {
+                pmp_info.width = (sizeof(addr) << 3) - clz(addr) + 2;
+                pmp_info.granularity = ctz(addr) + 2;
+            }
+        } else {
+            if (i == 0) {
+                pmp_info.width = 0;
+                pmp_info.granularity = 0;
+            }
+            break;
+        }
+    }
+    set_trap_fn(save);
+
+    return pmp_info;
+}
+
+int pmp_entry_width()
+{
+    if (pmp_info.width < 0) {
+        pmp_probe();
+    }
+    return pmp_info.width;
+}
+
+int pmp_entry_granularity()
+{
+    if (pmp_info.granularity < 0) {
+        pmp_probe();
+    }
+    return pmp_info.granularity;
+}
+
+int pmp_entry_count()
+{
+    if (pmp_info.count < 0) {
+        pmp_probe();
+    }
+    return pmp_info.count;
+}
+
+void pmp_clear_all()
+{
+    trap_fn save = get_trap_fn();
+    set_trap_fn(trap_save_cause);
+    for (size_t i = 0; i < PMPCFG_COUNT; i++) {
+        write_csr_enum(csr_pmpcfg0 + i, 0);
+    }
+    for (size_t i = 0; i < PMPADDR_COUNT; i++) {
+        write_csr_enum(csr_pmpaddr0 + i, 0);
+    }
+    set_trap_fn(save);
+}
+
+void pmp_allow_all()
+{
+    const uintptr_t pmpc = PMP_NAPOT | PMP_R | PMP_W | PMP_X;
+
+    if (pmp_entry_count() == 0) {
+        return;
+    }
+
+    pmp_clear_all();
+
+    /* borrowed from bbl  */
+    asm volatile ("la t0, 1f\n\t"
+                  "csrrw t0, mtvec, t0\n\t"
+                  "csrw pmpaddr0, %1\n\t"
+                  "csrw pmpcfg0, %0\n\t"
+                  ".align 2\n\t"
+                  "1: csrw mtvec, t0"
+                  : : "r" (pmpc), "r" (-1UL) : "t0");
+}
+
+static int pmp_entry_set_pow2(unsigned n, uint8_t prot, uint64_t addr, uint64_t len)
+{
+    /* calculate PMP config register and offset */
+    int pmpcfg_csr = (__riscv_xlen == 32) ? csr_pmpcfg0 + (n >> 2) :
+                     (__riscv_xlen == 64) ? csr_pmpcfg0 + (n >> 2) & ~1 : -1;
+    int pmpcfg_shift = (__riscv_xlen == 32) ? (n & 3) << 3 :
+                       (__riscv_xlen == 64) ? (n & 7) << 3 : -1;
+    if (pmpcfg_csr < 0 || pmpcfg_shift < 0) {
+        return -1;
+    }
+
+    /* encode config */
+    prot |= (len == 4) ? PMP_NA4 : PMP_NAPOT;
+    uintptr_t pmpcfg = ((uintptr_t)prot) << pmpcfg_shift;
+    uintptr_t cfgmask = ~(0xff << pmpcfg_shift);
+
+    /* encode address */
+    int pmpaddr_csr = csr_pmpaddr0 + n;
+    uintptr_t pmpmask = (1UL << (ctz(len) - PMP_SHIFT)) - 1;
+    uintptr_t pmpaddr = ((addr >> PMP_SHIFT) & ~pmpmask) | (pmpmask >> 1);
+
+    /* write csrs */
+    pmpcfg |= (read_csr_enum(pmpcfg_csr) & cfgmask) | pmpcfg;
+    write_csr_enum(pmpcfg_csr, pmpcfg);
+    write_csr_enum(pmpaddr_csr, pmpaddr);
+
+    return 0;
+}
+
+static int pmp_entry_set_range(unsigned n, uint8_t prot, uint64_t addr, uint64_t len)
+{
+    /* calculate PMP config register and offset */
+    int pmpcfg_csr0 = (__riscv_xlen == 32) ? csr_pmpcfg0 + (n >> 2) :
+                      (__riscv_xlen == 64) ? csr_pmpcfg0 + (n >> 2) & ~1 : -1;
+    int pmpcfg_csr1 = (__riscv_xlen == 32) ? csr_pmpcfg0 + ((n+1) >> 2) :
+                      (__riscv_xlen == 64) ? csr_pmpcfg0 + ((n+1) >> 2) & ~1 : -1;
+    int pmpcfg_shift0 = (__riscv_xlen == 32) ? (n & 3) << 3 :
+                        (__riscv_xlen == 64) ? (n & 7) << 3 : -1;
+    int pmpcfg_shift1 = (__riscv_xlen == 32) ? ((n+1) & 3) << 3 :
+                        (__riscv_xlen == 64) ? ((n+1) & 7) << 3 : -1;
+    if (pmpcfg_csr0 < 0 || pmpcfg_csr1 < 0 || pmpcfg_shift0 < 0 || pmpcfg_shift1 < 0) {
+        return -1;
+    }
+
+    /* encode config */
+    uintptr_t pmpcfg0 = ((uintptr_t)prot) << pmpcfg_shift0;
+    uintptr_t cfgmask0 = ~(0xff << pmpcfg_shift0);
+    uintptr_t pmpcfg1 = ((uintptr_t)prot | PMP_TOR) << pmpcfg_shift1;
+    uintptr_t cfgmask1 = ~(0xff << pmpcfg_shift1);
+
+    /* encode address */
+    int pmpaddr_csr0 = csr_pmpaddr0 + n;
+    int pmpaddr_csr1 = csr_pmpaddr1 + n;
+    uintptr_t pmpaddr0 = addr >> PMP_SHIFT;
+    uintptr_t pmpaddr1 = (addr + len) >> PMP_SHIFT;
+
+    /* write csrs */
+    pmpcfg0 |= (read_csr_enum(pmpcfg_csr0) & cfgmask0) | pmpcfg0;
+    write_csr_enum(pmpcfg_csr0, pmpcfg0);
+    write_csr_enum(pmpaddr_csr0, pmpaddr0);
+    pmpcfg1 |= (read_csr_enum(pmpcfg_csr1) & cfgmask1) | pmpcfg1;
+    write_csr_enum(pmpcfg_csr1, pmpcfg1);
+    write_csr_enum(pmpaddr_csr1, pmpaddr1);
+
+    return 0;
+}
+
+int pmp_entry_set(unsigned n, uint8_t prot, uint64_t addr, uint64_t len)
+{
+    /* check parameters */
+    if (n >= PMPADDR_COUNT || len < 4) {
+        return -1;
+    }
+    if (ispow2(len)) {
+        return pmp_entry_set_pow2(n, prot, addr, len);
+    } else {
+        return pmp_entry_set_range(n, prot, addr, len);
+    }
+}
diff --git a/riscv/riscv-probe/libfemto/arch/riscv/spinlock.c b/riscv/riscv-probe/libfemto/arch/riscv/spinlock.c
new file mode 100644
index 0000000..b8bbe72
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/arch/riscv/spinlock.c
@@ -0,0 +1,142 @@
+// See LICENSE for license details.
+
+#include <arch/riscv/spinlock.h>
+
+/**
+ * spinlock lock
+ *
+ * \param lock pointer to lock
+ *
+ *     lui     a2,0x10
+ *  1: lr.w.aq a4,(a0)
+ *     addw    a5,a2,a4
+ *     sc.w.rl a3,a5,(a0)
+ *     bnez    a4,1b
+ *     srliw   a4,a5,0x10
+ *     fence   r,rw
+ * 2:  slliw   a5,a5,0x10
+ *     srliw   a5,a5,0x10
+ *     bne     a5,a4,3f
+ *     ret
+ * 3:  lw      a5,0(a0)
+ *     fence   r,rw
+ *     j       2b
+ */
+
+void spinlock_lock(spinlock_t *lock)
+{
+    ticketdata_t ld;
+    ticket_t ticket_num;
+
+    /* increment tail to acquire a ticket number */
+    do {
+        ld = _lr_aq_w(&lock->data) + ((ticketdata_t)1 << TICKET_SHIFT);
+    } while (_sc_rl_w(&lock->data, ld));
+
+    /* fetch our ticket number */
+    ticket_num = ticketlock_tail(ld);
+
+    /* subsequent modifications are ordered after lock read */
+    _barrier_acquire();
+
+    /* wail until our ticket number is head */
+    while (ticketlock_head(ld) != ticket_num) {
+        ld = _l_aq_w(&lock->data);
+    }
+}
+
+/**
+ * spinlock trylock
+ *
+ * \param lock pointer to lock
+ * \return non zero if lock was successfully taken
+ *
+ *     lui     a5,0x10
+ *     lr.w.aq a4,(a0)
+ *     addw    a5,a5,a4
+ *     slliw   a3,a5,0x10
+ *     srliw   a3,a3,0x10
+ *     srliw   a4,a5,0x10
+ *     beq     a3,a4,2f
+ * 1:  li      a0,0
+ *     ret
+ * 2:  sc.w.rl a4,a5,(a0)
+ *     bnez    a4,1b
+ *     fence   r,rw
+ *     li      a0,1
+ *     ret
+ */
+
+int spinlock_trylock(spinlock_t *lock)
+{
+    ticketdata_t ld;
+    ticket_t ticket_num;
+    int result;
+
+    /* increment tail to the next ticket number */
+    ld = _lr_aq_w(&lock->data) + ((ticketdata_t)1 << TICKET_SHIFT);
+    ticket_num = ticketlock_tail(ld);
+
+    /* return true if our ticket is next and lock update succeeded */
+    result = ticketlock_head(ld) == ticket_num && !_sc_rl_w(&lock->data, ld);
+
+    if (result) {
+        /* subsequent modifications are ordered after lock read */
+        _barrier_acquire();
+    }
+
+    return result;
+}
+
+/**
+ * spinlock unlock
+ *
+ * \param lock pointer to lock
+ *
+ *     fence   rw,w
+ * 1:  lr.w.aq a4,(a0)
+ *     srliw   a5,a4,0x10
+ *     addiw   a4,a4,1
+ *     slliw   a4,a4,0x10
+ *     srliw   a4,a4,0x10
+ *     slliw   a5,a5,0x10
+ *     or      a5,a5,a4
+ *     sc.w.rl a4,a5,(a0)
+ *     bnez    a5,1b
+ *     ret
+ */
+void spinlock_unlock(spinlock_t *lock)
+{
+    ticketdata_t ld;
+
+    /* prior modifications are ordered before lock write */
+    _barrier_release();
+
+    /* increment head to release lock to the next ticket holder */
+    do {
+        ld = _lr_aq_w(&lock->data);
+        ld = ticketlock_data(ticketlock_tail(ld), ticketlock_head(ld) + 1);
+    } while (_sc_rl_w(&lock->data, ld));
+}
+
+/**
+ * spinlock waiters
+ *
+ * \param lock pointer to lock
+ * \return number of waiters
+ *
+ *     lw      a5,0(a0)
+ *     fence   r,rw
+ *     srliw   a0,a5,0x10
+ *     slliw   a5,a5,0x10
+ *     srliw   a5,a5,0x10
+ *     subw    a0,a0,a5
+ *     addiw   a0,a0,1
+ *     ret
+ */
+int spinlock_waiters(spinlock_t *lock)
+{
+    ticketdata_t ld = _l_aq_w(&lock->data);
+
+    return (int)ticketlock_tail(ld) - (int)ticketlock_head(ld) + 1;
+}
diff --git a/riscv/riscv-probe/libfemto/arch/riscv/start.c b/riscv/riscv-probe/libfemto/arch/riscv/start.c
new file mode 100644
index 0000000..60f48a2
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/arch/riscv/start.c
@@ -0,0 +1,21 @@
+// See LICENSE for license details.
+
+#include "femto.h"
+#include "arch/riscv/encoding.h"
+#include "arch/riscv/machine.h"
+
+extern char _bss_start;
+extern char _bss_end;
+extern char _memory_end;
+
+int main(int argc, char **argv);
+
+__attribute__((noreturn)) void libfemto_start_main()
+{
+	char *argv[] = { "femto", NULL };
+	memset(&_bss_start, 0, &_bss_end - &_bss_start);
+	arch_setup();
+	_malloc_addblock(&_bss_end, &_memory_end - &_bss_end);
+	exit(main(1, argv));
+	__builtin_unreachable();
+}
diff --git a/riscv/riscv-probe/libfemto/arch/riscv/trap.c b/riscv/riscv-probe/libfemto/arch/riscv/trap.c
new file mode 100644
index 0000000..6a7d80c
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/arch/riscv/trap.c
@@ -0,0 +1,65 @@
+// See LICENSE for license details.
+
+#include "femto.h"
+#include "arch/riscv/trap.h"
+#include "arch/riscv/encoding.h"
+#include "arch/riscv/machine.h"
+
+static trap_fn tfn = 0;
+
+const char * riscv_excp_names[16] = {
+    "misaligned_fetch",
+    "fault_fetch",
+    "illegal_instruction",
+    "breakpoint",
+    "misaligned_load",
+    "fault_load",
+    "misaligned_store",
+    "fault_store",
+    "user_ecall",
+    "supervisor_ecall",
+    "hypervisor_ecall",
+    "machine_ecall",
+    "exec_page_fault",
+    "load_page_fault",
+    "reserved",
+    "store_page_fault"
+};
+
+const char * riscv_intr_names[16] = {
+    "u_software",
+    "s_software",
+    "h_software",
+    "m_software",
+    "u_timer",
+    "s_timer",
+    "h_timer",
+    "m_timer",
+    "u_external",
+    "s_external",
+    "h_external",
+    "m_external",
+    "reserved",
+    "reserved",
+    "reserved",
+    "reserved"
+};
+
+trap_fn get_trap_fn()
+{
+    return tfn;
+}
+
+void set_trap_fn(trap_fn fn)
+{
+    tfn = fn;
+}
+
+void trap_handler(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
+{
+    if (tfn) {
+        tfn(regs, mcause, mepc);
+    } else {
+        die("machine mode: unhandlable trap %d @ %p", mcause, mepc);
+    }
+}
diff --git a/riscv/riscv-probe/libfemto/drivers/htif.c b/riscv/riscv-probe/libfemto/drivers/htif.c
new file mode 100644
index 0000000..ef6995e
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/drivers/htif.c
@@ -0,0 +1,91 @@
+// See LICENSE for license details.
+
+#ifdef __riscv
+
+#include "femto.h"
+#include "spinlock.h"
+
+struct { uint32_t arr[2]; } volatile tohost __attribute__((section(".htif")));
+struct { uint32_t arr[2]; } volatile fromhost __attribute__((section(".htif")));
+
+static spinlock_t htif_lock = SPINLOCK_INIT;
+
+static inline void htif_send(uint8_t dev, uint8_t cmd, int64_t data)
+{
+    /* endian neutral encoding with ordered 32-bit writes */
+    union { uint32_t arr[2]; uint64_t val; } encode = {
+        .val = (uint64_t)dev << 56 | (uint64_t)cmd << 48 | data
+    };
+    tohost.arr[0] = encode.arr[0];
+    tohost.arr[1] = encode.arr[1];
+}
+
+static inline void htif_recv(uint8_t *dev, uint8_t *cmd, int64_t *data)
+{
+    /* endian neutral decoding with ordered 32-bit reads */
+    union { uint32_t arr[2]; uint64_t val; } decode;
+    decode.arr[0] = fromhost.arr[0];
+    decode.arr[1] = fromhost.arr[1];
+    *dev = decode.val >> 56;
+    *cmd = (decode.val >> 48) & 0xff;
+    *data = decode.val << 16 >> 16;
+}
+
+static int64_t htif_get_fromhost(uint8_t dev, uint8_t cmd)
+{
+    /* receive data for specified device and command */
+    uint8_t rdev, rcmd;
+    int64_t data;
+    htif_recv(&rdev, &rcmd, &data);
+    return rdev == dev && rcmd == cmd ? data : -1;
+}
+
+static void htif_set_tohost(uint8_t dev, uint8_t cmd, int64_t data)
+{
+    /* send data with specified device and command */
+    while (tohost.arr[0]) {
+        asm volatile ("" : : "r" (fromhost.arr[0]));
+        asm volatile ("" : : "r" (fromhost.arr[1]));
+    }
+    htif_send(dev, cmd, data);
+}
+
+static int htif_getchar()
+{
+    int ch;
+    spinlock_lock(&htif_lock);
+    if ((ch = htif_get_fromhost(1, 0) & 0xff)) {
+        htif_set_tohost(1, 0, 0);
+    }
+    spinlock_unlock(&htif_lock);
+    return ch;
+}
+
+static int htif_putchar(int ch)
+{
+    spinlock_lock(&htif_lock);
+    htif_set_tohost(1, 1, ch & 0xff);
+    spinlock_unlock(&htif_lock);
+    return ch & 0xff;
+}
+
+static void htif_poweroff(int status)
+{
+    for (;;) {
+        htif_set_tohost(0, 0, 1);
+    }
+}
+
+console_device_t console_htif = {
+    NULL,
+    htif_getchar,
+    htif_putchar
+};
+
+
+poweroff_device_t poweroff_htif = {
+    NULL,
+    htif_poweroff
+};
+
+#endif
diff --git a/riscv/riscv-probe/libfemto/drivers/ns16550a.c b/riscv/riscv-probe/libfemto/drivers/ns16550a.c
new file mode 100644
index 0000000..cff5605
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/drivers/ns16550a.c
@@ -0,0 +1,65 @@
+// See LICENSE for license details.
+
+#include "femto.h"
+
+enum {
+    UART_RBR      = 0x00,  /* Receive Buffer Register */
+    UART_THR      = 0x00,  /* Transmit Hold Register */
+    UART_IER      = 0x01,  /* Interrupt Enable Register */
+    UART_DLL      = 0x00,  /* Divisor LSB (LCR_DLAB) */
+    UART_DLM      = 0x01,  /* Divisor MSB (LCR_DLAB) */
+    UART_FCR      = 0x02,  /* FIFO Control Register */
+    UART_LCR      = 0x03,  /* Line Control Register */
+    UART_MCR      = 0x04,  /* Modem Control Register */
+    UART_LSR      = 0x05,  /* Line Status Register */
+    UART_MSR      = 0x06,  /* Modem Status Register */
+    UART_SCR      = 0x07,  /* Scratch Register */
+
+    UART_LCR_DLAB = 0x80,  /* Divisor Latch Bit */
+    UART_LCR_8BIT = 0x03,  /* 8-bit */
+    UART_LCR_PODD = 0x08,  /* Parity Odd */
+
+    UART_LSR_DA   = 0x01,  /* Data Available */
+    UART_LSR_OE   = 0x02,  /* Overrun Error */
+    UART_LSR_PE   = 0x04,  /* Parity Error */
+    UART_LSR_FE   = 0x08,  /* Framing Error */
+    UART_LSR_BI   = 0x10,  /* Break indicator */
+    UART_LSR_RE   = 0x20,  /* THR is empty */
+    UART_LSR_RI   = 0x40,  /* THR is empty and line is idle */
+    UART_LSR_EF   = 0x80,  /* Erroneous data in FIFO */
+};
+
+static volatile uint8_t *uart;
+
+static void ns16550a_init()
+{
+	uart = (uint8_t *)(void *)getauxval(NS16550A_UART0_CTRL_ADDR);
+	uint32_t uart_freq = getauxval(UART0_CLOCK_FREQ);
+	uint32_t baud_rate = getauxval(UART0_BAUD_RATE);
+    uint32_t divisor = uart_freq / (16 * baud_rate);
+    uart[UART_LCR] = UART_LCR_DLAB;
+    uart[UART_DLL] = divisor & 0xff;
+    uart[UART_DLM] = (divisor >> 8) & 0xff;
+    uart[UART_LCR] = UART_LCR_PODD | UART_LCR_8BIT;
+}
+
+static int ns16550a_getchar()
+{
+    if (uart[UART_LSR] & UART_LSR_DA) {
+        return uart[UART_RBR];
+    } else {
+        return -1;
+    }
+}
+
+static int ns16550a_putchar(int ch)
+{
+    while ((uart[UART_LSR] & UART_LSR_RI) == 0);
+    return uart[UART_THR] = ch & 0xff;
+}
+
+console_device_t console_ns16550a = {
+    ns16550a_init,
+    ns16550a_getchar,
+    ns16550a_putchar
+};
diff --git a/riscv/riscv-probe/libfemto/drivers/semihost.c b/riscv/riscv-probe/libfemto/drivers/semihost.c
new file mode 100644
index 0000000..9f47834
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/drivers/semihost.c
@@ -0,0 +1,60 @@
+// See LICENSE for license details.
+
+#ifdef __riscv
+
+#include "femto.h"
+
+enum {
+    SBI_CONSOLE_PUTCHAR = 1,
+    SBI_CONSOLE_GETCHAR = 2,
+    SBI_SHUTDOWN = 8
+};
+
+#define __syscall(...) \
+    asm volatile ("ecall\n\t" : "+r"(a0) : __VA_ARGS__ : "memory")
+
+static inline long semihost_call0(long n)
+{
+    register long a7 __asm__("a7") = n;
+    register long a0 __asm__("a0");
+    __syscall("r"(a7));
+    return a0;
+}
+
+static inline long semihost_call1(long n, long a)
+{
+    register long a7 __asm__("a7") = n;
+    register long a0 __asm__("a0") = a;
+    __syscall("r"(a7), "0"(a0));
+    return a0;
+}
+
+static int semihost_getchar()
+{
+    semihost_call0(SBI_CONSOLE_GETCHAR);
+}
+
+static int semihost_putchar(int ch)
+{
+    semihost_call1(SBI_CONSOLE_PUTCHAR, ch);
+}
+
+static void semihost_poweroff(int status)
+{
+    semihost_call0(SBI_SHUTDOWN);
+    __builtin_unreachable();
+}
+
+console_device_t console_semihost = {
+    NULL,
+    semihost_getchar,
+    semihost_putchar
+};
+
+
+poweroff_device_t poweroff_semihost = {
+	NULL,
+	semihost_poweroff
+};
+
+#endif
\ No newline at end of file
diff --git a/riscv/riscv-probe/libfemto/drivers/sifive_test.c b/riscv/riscv-probe/libfemto/drivers/sifive_test.c
new file mode 100644
index 0000000..1bda9e9
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/drivers/sifive_test.c
@@ -0,0 +1,28 @@
+// See LICENSE for license details.
+
+#include "femto.h"
+
+enum {
+	SIFIVE_TEST_FAIL = 0x3333,
+	SIFIVE_TEST_PASS = 0x5555,
+};
+
+static volatile uint32_t *test;
+
+static void sifive_test_init()
+{
+	test = (uint32_t *)(void *)getauxval(SIFIVE_TEST_CTRL_ADDR);
+}
+
+static void sifive_test_poweroff(int status)
+{
+    *test = SIFIVE_TEST_PASS;
+    while (1) {
+        asm volatile("");
+    }
+}
+
+poweroff_device_t poweroff_sifive_test = {
+	sifive_test_init,
+	sifive_test_poweroff
+};
diff --git a/riscv/riscv-probe/libfemto/drivers/sifive_uart.c b/riscv/riscv-probe/libfemto/drivers/sifive_uart.c
new file mode 100644
index 0000000..9be877e
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/drivers/sifive_uart.c
@@ -0,0 +1,58 @@
+// See LICENSE for license details.
+
+#include "femto.h"
+
+enum {
+    /* UART Registers */
+    UART_REG_TXFIFO = 0,
+    UART_REG_RXFIFO = 1,
+    UART_REG_TXCTRL = 2,
+    UART_REG_RXCTRL = 3,
+    UART_REG_IE     = 4,
+    UART_REG_IP     = 5,
+    UART_REG_DIV    = 6,
+
+    /* TXCTRL register */
+    UART_TXEN       = 1,
+    UART_TXSTOP     = 2,
+
+    /* RXCTRL register */
+    UART_RXEN       = 1,
+
+    /* IP register */
+    UART_IP_TXWM    = 1,
+    UART_IP_RXWM    = 2
+};
+
+static volatile int *uart;
+
+static void sifive_uart_init()
+{
+	uart = (int *)(void *)getauxval(SIFIVE_UART0_CTRL_ADDR);
+	uint32_t uart_freq = getauxval(UART0_CLOCK_FREQ);
+	uint32_t baud_rate = getauxval(UART0_BAUD_RATE);
+    uint32_t divisor = uart_freq / baud_rate - 1;
+    uart[UART_REG_DIV] = divisor;
+    uart[UART_REG_TXCTRL] = UART_TXEN;
+    uart[UART_REG_RXCTRL] = UART_RXEN;
+    uart[UART_REG_IE] = 0;
+}
+
+static int sifive_uart_getchar()
+{
+    int ch = uart[UART_REG_RXFIFO];
+    if (ch < 0) return -1;
+    return ch;
+}
+
+static int sifive_uart_putchar(int ch)
+{
+    while (uart[UART_REG_TXFIFO] < 0);
+    return uart[UART_REG_TXFIFO] = ch & 0xff;
+}
+
+console_device_t console_sifive_uart = {
+    sifive_uart_init,
+    sifive_uart_getchar,
+    sifive_uart_putchar
+};
diff --git a/riscv/riscv-probe/libfemto/include/alloca.h b/riscv/riscv-probe/libfemto/include/alloca.h
new file mode 100644
index 0000000..2ae6a3e
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/alloca.h
@@ -0,0 +1,3 @@
+#pragma once
+
+#define alloca(size) __builtin_alloca (size)
diff --git a/riscv/riscv-probe/libfemto/include/arch/riscv/csr.h b/riscv/riscv-probe/libfemto/include/arch/riscv/csr.h
new file mode 100644
index 0000000..8bbc5f4
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/arch/riscv/csr.h
@@ -0,0 +1,81 @@
+// See LICENSE for license details.
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int* csr_enum_array();
+const char** csr_name_array();
+long read_csr_enum(int csrenum);
+void write_csr_enum(int csrenum, long value);
+
+enum {
+    csr_none,
+    csr_fflags,
+    csr_frm,
+    csr_fcsr,
+    csr_mcycle,
+    csr_minstret,
+    csr_mcycleh,
+    csr_minstreth,
+    csr_cycle,
+    csr_time,
+    csr_instret,
+    csr_cycleh,
+    csr_timeh,
+    csr_instreth,
+    csr_mvendorid,
+    csr_marchid,
+    csr_mimpid,
+    csr_mhartid,
+    csr_mstatus,
+    csr_misa,
+    csr_medeleg,
+    csr_mideleg,
+    csr_mie,
+    csr_mtvec,
+    csr_mcounteren,
+    csr_mscratch,
+    csr_mepc,
+    csr_mcause,
+    csr_mtval,
+    csr_mip,
+    csr_sstatus,
+    csr_sedeleg,
+    csr_sideleg,
+    csr_sie,
+    csr_stvec,
+    csr_scounteren,
+    csr_sscratch,
+    csr_sepc,
+    csr_scause,
+    csr_stval,
+    csr_sip,
+    csr_satp,
+    csr_pmpcfg0,
+    csr_pmpcfg1,
+    csr_pmpcfg2,
+    csr_pmpcfg3,
+    csr_pmpaddr0,
+    csr_pmpaddr1,
+    csr_pmpaddr2,
+    csr_pmpaddr3,
+    csr_pmpaddr4,
+    csr_pmpaddr5,
+    csr_pmpaddr6,
+    csr_pmpaddr7,
+    csr_pmpaddr8,
+    csr_pmpaddr9,
+    csr_pmpaddr10,
+    csr_pmpaddr11,
+    csr_pmpaddr12,
+    csr_pmpaddr13,
+    csr_pmpaddr14,
+    csr_pmpaddr15
+};
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/arch/riscv/encoding.h b/riscv/riscv-probe/libfemto/include/arch/riscv/encoding.h
new file mode 100644
index 0000000..d592bbc
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/arch/riscv/encoding.h
@@ -0,0 +1,197 @@
+// See LICENSE for license details.
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define MSTATUS_UIE         0x00000001
+#define MSTATUS_SIE         0x00000002
+#define MSTATUS_HIE         0x00000004
+#define MSTATUS_MIE         0x00000008
+#define MSTATUS_UPIE        0x00000010
+#define MSTATUS_SPIE        0x00000020
+#define MSTATUS_HPIE        0x00000040
+#define MSTATUS_MPIE        0x00000080
+#define MSTATUS_SPP         0x00000100
+#define MSTATUS_HPP         0x00000600
+#define MSTATUS_MPP         0x00001800
+#define MSTATUS_FS          0x00006000
+#define MSTATUS_XS          0x00018000
+#define MSTATUS_MPRV        0x00020000
+#define MSTATUS_SUM         0x00040000
+#define MSTATUS_MXR         0x00080000
+#define MSTATUS_TVM         0x00100000
+#define MSTATUS_TW          0x00200000
+#define MSTATUS_TSR         0x00400000
+#define MSTATUS32_SD        0x80000000
+#define MSTATUS64_SD        0x8000000000000000
+
+#define SSTATUS_UIE         0x00000001
+#define SSTATUS_SIE         0x00000002
+#define SSTATUS_UPIE        0x00000010
+#define SSTATUS_SPIE        0x00000020
+#define SSTATUS_SPP         0x00000100
+#define SSTATUS_FS          0x00006000
+#define SSTATUS_XS          0x00018000
+#define SSTATUS_SUM         0x00040000
+#define SSTATUS_MXR         0x00080000
+#define SSTATUS32_SD        0x80000000
+#define SSTATUS64_SD        0x8000000000000000
+
+#define DCSR_XDEBUGVER      (3U<<30)
+#define DCSR_NDRESET        (1<<29)
+#define DCSR_FULLRESET      (1<<28)
+#define DCSR_EBREAKM        (1<<15)
+#define DCSR_EBREAKH        (1<<14)
+#define DCSR_EBREAKS        (1<<13)
+#define DCSR_EBREAKU        (1<<12)
+#define DCSR_STOPCYCLE      (1<<10)
+#define DCSR_STOPTIME       (1<<9)
+#define DCSR_CAUSE          (7<<6)
+#define DCSR_DEBUGINT       (1<<5)
+#define DCSR_HALT           (1<<3)
+#define DCSR_STEP           (1<<2)
+#define DCSR_PRV            (3<<0)
+
+#define DCSR_CAUSE_NONE     0
+#define DCSR_CAUSE_SWBP     1
+#define DCSR_CAUSE_HWBP     2
+#define DCSR_CAUSE_DEBUGINT 3
+#define DCSR_CAUSE_STEP     4
+#define DCSR_CAUSE_HALT     5
+
+#define MCONTROL_TYPE(xlen)    (0xfULL<<((xlen)-4))
+#define MCONTROL_DMODE(xlen)   (1ULL<<((xlen)-5))
+#define MCONTROL_MASKMAX(xlen) (0x3fULL<<((xlen)-11))
+
+#define MCONTROL_SELECT     (1<<19)
+#define MCONTROL_TIMING     (1<<18)
+#define MCONTROL_ACTION     (0x3f<<12)
+#define MCONTROL_CHAIN      (1<<11)
+#define MCONTROL_MATCH      (0xf<<7)
+#define MCONTROL_M          (1<<6)
+#define MCONTROL_H          (1<<5)
+#define MCONTROL_S          (1<<4)
+#define MCONTROL_U          (1<<3)
+#define MCONTROL_EXECUTE    (1<<2)
+#define MCONTROL_STORE      (1<<1)
+#define MCONTROL_LOAD       (1<<0)
+
+#define MCONTROL_TYPE_NONE      0
+#define MCONTROL_TYPE_MATCH     2
+
+#define MCONTROL_ACTION_DEBUG_EXCEPTION   0
+#define MCONTROL_ACTION_DEBUG_MODE        1
+#define MCONTROL_ACTION_TRACE_START       2
+#define MCONTROL_ACTION_TRACE_STOP        3
+#define MCONTROL_ACTION_TRACE_EMIT        4
+
+#define MCONTROL_MATCH_EQUAL     0
+#define MCONTROL_MATCH_NAPOT     1
+#define MCONTROL_MATCH_GE        2
+#define MCONTROL_MATCH_LT        3
+#define MCONTROL_MATCH_MASK_LOW  4
+#define MCONTROL_MATCH_MASK_HIGH 5
+
+#define MIP_SSIP            (1 << IRQ_S_SOFT)
+#define MIP_HSIP            (1 << IRQ_H_SOFT)
+#define MIP_MSIP            (1 << IRQ_M_SOFT)
+#define MIP_STIP            (1 << IRQ_S_TIMER)
+#define MIP_HTIP            (1 << IRQ_H_TIMER)
+#define MIP_MTIP            (1 << IRQ_M_TIMER)
+#define MIP_SEIP            (1 << IRQ_S_EXT)
+#define MIP_HEIP            (1 << IRQ_H_EXT)
+#define MIP_MEIP            (1 << IRQ_M_EXT)
+
+#define SIP_SSIP MIP_SSIP
+#define SIP_STIP MIP_STIP
+
+#define PRV_U 0
+#define PRV_S 1
+#define PRV_H 2
+#define PRV_M 3
+
+#define SPTBR32_MODE 0x80000000
+#define SPTBR32_ASID 0x7FC00000
+#define SPTBR32_PPN  0x003FFFFF
+#define SPTBR64_MODE 0xF000000000000000
+#define SPTBR64_ASID 0x0FFFF00000000000
+#define SPTBR64_PPN  0x00000FFFFFFFFFFF
+
+#define SPTBR_MODE_OFF  0
+#define SPTBR_MODE_SV32 1
+#define SPTBR_MODE_SV39 8
+#define SPTBR_MODE_SV48 9
+#define SPTBR_MODE_SV57 10
+#define SPTBR_MODE_SV64 11
+
+#define PMP_R     0x01
+#define PMP_W     0x02
+#define PMP_X     0x04
+#define PMP_A     0x18
+#define PMP_L     0x80
+#define PMP_SHIFT     2
+#define PMPCFG_COUNT  4
+#define PMPADDR_COUNT 16
+
+#define PMP_OFF   0x00
+#define PMP_TOR   0x08
+#define PMP_NA4   0x10
+#define PMP_NAPOT 0x18
+
+#define IRQ_S_SOFT   1
+#define IRQ_H_SOFT   2
+#define IRQ_M_SOFT   3
+#define IRQ_S_TIMER  5
+#define IRQ_H_TIMER  6
+#define IRQ_M_TIMER  7
+#define IRQ_S_EXT    9
+#define IRQ_H_EXT    10
+#define IRQ_M_EXT    11
+#define IRQ_COP      12
+#define IRQ_HOST     13
+
+#define DEFAULT_RSTVEC     0x00001000
+#define CLINT_BASE         0x02000000
+#define CLINT_SIZE         0x000c0000
+#define EXT_IO_BASE        0x40000000
+#define DRAM_BASE          0x80000000
+
+// page table entry (PTE) fields
+#define PTE_V     0x001 // Valid
+#define PTE_R     0x002 // Read
+#define PTE_W     0x004 // Write
+#define PTE_X     0x008 // Execute
+#define PTE_U     0x010 // User
+#define PTE_G     0x020 // Global
+#define PTE_A     0x040 // Accessed
+#define PTE_D     0x080 // Dirty
+#define PTE_SOFT  0x300 // Reserved for Software
+
+#define PTE_PPN_SHIFT 10
+
+#define PTE_TABLE(PTE) (((PTE) & (PTE_V | PTE_R | PTE_W | PTE_X)) == PTE_V)
+
+#ifdef __riscv
+
+#if __riscv_xlen == 64
+# define MSTATUS_SD MSTATUS64_SD
+# define SSTATUS_SD SSTATUS64_SD
+# define RISCV_PGLEVEL_BITS 9
+# define SPTBR_MODE SPTBR64_MODE
+#else
+# define MSTATUS_SD MSTATUS32_SD
+# define SSTATUS_SD SSTATUS32_SD
+# define RISCV_PGLEVEL_BITS 10
+# define SPTBR_MODE SPTBR32_MODE
+#endif
+#define RISCV_PGSHIFT 12
+#define RISCV_PGSIZE (1 << RISCV_PGSHIFT)
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/arch/riscv/machine.h b/riscv/riscv-probe/libfemto/include/arch/riscv/machine.h
new file mode 100644
index 0000000..077a6c1
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/arch/riscv/machine.h
@@ -0,0 +1,183 @@
+// See LICENSE for license details.
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+void arch_setup();
+void exit(int status) __attribute__((noreturn));
+
+#define die(str, ...) ({ \
+  printf("%s:%d: " str "\n", __FILE__, __LINE__, ##__VA_ARGS__); exit(-1); })
+
+#define assert(x) ({ if (!(x)) die("assertion failed: %s", #x); })
+
+#define read_const_csr(reg) ({ unsigned long __tmp; \
+  asm ("csrr %0, " #reg : "=r"(__tmp)); __tmp; })
+
+#define read_csr(reg) ({ unsigned long __tmp; \
+  asm volatile ("csrr %0, " #reg : "=r"(__tmp)); __tmp; })
+
+#define write_csr(reg, val) ({ \
+  asm volatile ("csrw " #reg ", %0" :: "rK"(val)); })
+
+#define swap_csr(reg, val) ({ unsigned long __tmp; \
+  asm volatile ("csrrw %0, " #reg ", %1" : "=r"(__tmp) : "rK"(val)); __tmp; })
+
+#define set_csr(reg, bit) ({ unsigned long __tmp; \
+  asm volatile ("csrrs %0, " #reg ", %1" : "=r"(__tmp) : "rK"(bit)); __tmp; })
+
+#define clear_csr(reg, bit) ({ unsigned long __tmp; \
+  asm volatile ("csrrc %0, " #reg ", %1" : "=r"(__tmp) : "rK"(bit)); __tmp; })
+
+static inline uintptr_t get_field(uintptr_t reg, uintptr_t mask)
+{
+    return ((reg & mask) / (mask & ~(mask << 1)));
+}
+
+static inline uintptr_t set_field(uintptr_t reg, uintptr_t mask, uintptr_t val)
+{
+    return ((reg & ~mask) | ((val * (mask & ~(mask << 1))) & mask));
+}
+
+static inline unsigned long rdtime() { return read_csr(time); }
+static inline unsigned long rdcycle() { return read_csr(cycle); }
+static inline unsigned long rdinstret() { return read_csr(instret); }
+static inline int64_t misa() { return read_const_csr(misa); }
+static inline int has_ext(char ext) { return misa() & (1 << (ext - 'a')); }
+static inline int xlen() { return misa() < 0 ? 64 : 32; }
+static inline void wfi() { asm volatile ("wfi" ::: "memory"); }
+
+__attribute__((noreturn)) static inline void mret()
+{
+    asm volatile ("mret");
+    __builtin_unreachable();
+}
+
+
+/*
+ * Memory
+ *
+ * TODO - improve this API to return a list of memory segments
+ */
+
+typedef struct memory_info
+{
+    uintptr_t start;
+    uintptr_t end;
+} memory_info_t;
+
+
+/*
+ * memory_probe - return memory_info
+ */
+memory_info_t memory_probe();
+
+/*
+ * memory_probe_range - probe a memory address range
+ */
+uintptr_t memory_probe_range(uintptr_t start, uintptr_t end);
+
+
+/*
+ * Physical Memory Protection
+ *
+ * PMP is optional but if implememented, enforcement must be enabled by
+ * default, if no PMP entries are set. This means loads, stores or fetches
+ * from any mode besides M mode, will fail unless explicitly allowed.
+ * PMP must be configured irregardless of whether it is implemented.
+ */
+
+typedef struct pmp_info
+{
+    int width;
+    int granularity;
+    int count;
+} pmp_info_t;
+
+/*
+ * pmp_probe - return pmp_info
+ */
+pmp_info_t pmp_probe();
+
+/*
+ * pmp_entry_granularity - return PMP entry width (physical memory width)
+ */
+int pmp_entry_width();
+
+/*
+ * pmp_entry_granularity - return PMP entry granularity (smallest entry size)
+ */
+int pmp_entry_granularity();
+
+/*
+ * pmp_entry_count - return number of PMP entries
+ */
+int pmp_entry_count();
+
+/*
+ * pmp_clear_all - set PMP to disallow mode != PRV_M physical memory accesses
+ */
+void pmp_clear_all();
+
+/*
+ * pmp_allow_all - set PMP to allow mode != PRV_M physical memory accesses
+ */
+void pmp_allow_all();
+
+/*
+ * pmp_entry_set - set one PMP entry
+ *
+ * - n    : pmp entry number
+ * - prot : protection (PMP_R | PMP_W | PMP_X)
+ * - addr : start address
+ * - len  : power of two length
+ */
+int pmp_entry_set(unsigned n, uint8_t prot, uint64_t addr, uint64_t len);
+
+
+/*
+ * Privileged modes
+ */
+
+/*
+ * mode_set_and_jump
+ *
+ * Set mstatus.mpp, sets mepc to passed function pointer and then issues mret
+ * Note: the hart will continue running on the same stack
+ */
+static inline void mode_set_and_jump(unsigned mode, void (*fn)(void))
+{
+    assert(mode <= PRV_U);
+    write_csr(mstatus, set_field(read_csr(mstatus), MSTATUS_MPP, mode));
+    write_csr(mepc, fn);
+    mret();
+}
+
+/*
+ * mode_set_and_continue
+ *
+ * Set mstatus.mpp, sets mepc to instruction after mret and then issues mret
+ * Note: the hart will continue running on the same stack
+ */
+static inline void mode_set_and_continue(unsigned mode)
+{
+    assert(mode <= PRV_U);
+    write_csr(mstatus, set_field(read_csr(mstatus), MSTATUS_MPP, mode));
+    asm volatile (
+        "lla t0, 1f\n"
+        "csrw mepc, t0\n"
+        "mret\n"
+        "1:"
+        ::: "t0"
+    );
+}
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/arch/riscv/pte.h b/riscv/riscv-probe/libfemto/include/arch/riscv/pte.h
new file mode 100644
index 0000000..38d7b8d
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/arch/riscv/pte.h
@@ -0,0 +1,121 @@
+#pragma once
+
+enum {
+    PTE_SHIFT_V   = 0,
+    PTE_SHIFT_R   = 1,
+    PTE_SHIFT_W   = 2,
+    PTE_SHIFT_X   = 3,
+    PTE_SHIFT_U   = 4,
+    PTE_SHIFT_G   = 5,
+    PTE_SHIFT_A   = 6,
+    PTE_SHIFT_D   = 7,
+    PTE_SHIFT_SW1 = 8,
+    PTE_SHIFT_SW2 = 9,
+    PTE_V         = 1 << PTE_SHIFT_V,
+    PTE_R         = 1 << PTE_SHIFT_R,
+    PTE_W         = 1 << PTE_SHIFT_W,
+    PTE_X         = 1 << PTE_SHIFT_X,
+    PTE_U         = 1 << PTE_SHIFT_U,
+    PTE_G         = 1 << PTE_SHIFT_G,
+    PTE_A         = 1 << PTE_SHIFT_A,
+    PTE_D         = 1 << PTE_SHIFT_D,
+    PTE_SW1       = 1 << PTE_SHIFT_SW1,
+    PTE_SW2       = 1 << PTE_SHIFT_SW2
+};
+
+enum {
+    SV32_LEVELS = 2,
+    SV32_LEVEL_BITS = 10,
+    SV32_PTE_SIZE = 4
+};
+
+union sv32_va {
+    uint32_t val;
+    struct {
+        uint32_t pg_off : 12;
+        uint32_t vpn    : 20;
+    } va;
+};
+
+union sv32_pa {
+    uint64_t val;
+    struct {
+        uint64_t pg_off  : 12;
+        uint64_t ppn     : 22;
+        uint64_t rsrv    : 30;
+    } pa;
+};
+
+union sv32_pte {
+    uint32_t val;
+    struct {
+        uint32_t flags   : 10;
+        uint32_t ppn     : 22;
+    } pte;
+};
+
+enum {
+    SV39_LEVELS = 3,
+    SV39_LEVEL_BITS = 9,
+    SV39_PTE_SIZE = 8
+};
+
+union sv39_va {
+    uint64_t val;
+    struct {
+        uint64_t pg_off  : 12;
+        uint64_t vpn     : 27;
+        uint64_t rsrv    : 25;
+    } va;
+};
+
+union sv39_pa {
+    uint64_t val;
+    struct {
+        uint64_t pg_off  : 12;
+        uint64_t ppn     : 44;
+        uint64_t rsrv    : 8;
+    } pa;
+};
+
+union sv39_pte {
+    uint64_t val;
+    struct {
+        uint64_t flags   : 10;
+        uint64_t ppn     : 44;
+        uint64_t rsrv    : 10;
+    } pte;
+};
+
+enum {
+    SV48_LEVELS = 4,
+    SV48_LEVEL_BITS = 9,
+    SV48_PTE_SIZE = 8
+};
+
+union sv48_va {
+    uint64_t val;
+    struct {
+        uint64_t pg_off  : 12;
+        uint64_t vpn     : 36;
+        uint64_t rsrv    : 16;
+    } va;
+};
+
+union sv48_pa {
+    uint64_t val;
+    struct {
+        uint64_t pg_off  : 12;
+        uint64_t ppn     : 44;
+        uint64_t rsrv    : 8;
+    } pa;
+};
+
+union sv48_pte {
+    uint64_t val;
+    struct {
+        uint64_t flags   : 10;
+        uint64_t ppn     : 44;
+        uint64_t rsrv    : 10;
+    } pte;
+};
diff --git a/riscv/riscv-probe/libfemto/include/arch/riscv/spinlock.h b/riscv/riscv-probe/libfemto/include/arch/riscv/spinlock.h
new file mode 100644
index 0000000..d747cef
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/arch/riscv/spinlock.h
@@ -0,0 +1,192 @@
+// See LICENSE for license details.
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Ticket Lock
+ *
+ * Author: Michael Clark <[email protected]>
+ *
+ * Ticket locks are fair spinlocks that order acccess on a first-in,
+ * first-out basis. The lock is composed of a head counter and a tail
+ * counter. The head counter indicates the ticket number of the current
+ * lock owner. The tail counter indicates the last issued ticket number.
+ * To acquire the lock, the acquiring thread atomically increments the
+ * tail counter to assign itself a ticket number. It then waits until
+ * its assigned ticket number is seen in the head counter. If the lock
+ * is not contended, then head will equal the ticket number just assigned.
+ * To release the lock, the lock owner atomically increments head counter
+ * thereby allowing access to the thread holding the next ticket number.
+ *
+ * Initialializtion - the lock is initialized unlocked, so that the
+ * next tail increment returns a ticket that will acquire the lock.
+ *
+ *   tail   | head
+ *   -------|-------
+ *   0x0000 | 0x0001
+ *
+ * Lock - the tail is incremented, and the thread waits unti the tail
+ * is equal to the head before returning control, which is the typical
+ * case if the lock is uncontended.
+ *
+ *   tail   | head
+ *   -------|-------
+ *   0x0001 | 0x0001
+ *
+ * Unlock - the head is incremented, so that it will now equal the value
+ * of the ticket of the next waiter, if any.
+ *
+ *   tail   | head
+ *   -------|-------
+ *   0x0001 | 0x0002
+ *
+ * Waiters - the number of waiters can be calculated by subtracting
+ * head (current ticket) from the tail plus one (next ticket)
+ *
+ *   num_waiters = tail + 1 - head
+ */
+
+/**
+ * Ticket lock types
+ */
+
+typedef unsigned short ticket_t;
+typedef unsigned int ticketdata_t;
+
+typedef struct {
+    volatile ticketdata_t data;
+} spinlock_t;
+
+#define TICKET_SHIFT ((sizeof(ticketdata_t) / sizeof(ticket_t)) * 8)
+
+/**
+ * Ticket lock encode
+ */
+static inline ticketdata_t ticketlock_data(ticket_t tail, ticket_t head)
+{
+    return (((ticketdata_t)tail) << TICKET_SHIFT) | (ticketdata_t)head;
+}
+
+/**
+ * Ticket lock tail
+ */
+static inline ticket_t ticketlock_tail(ticketdata_t data) { return data >> 16; }
+
+/**
+ * Ticket lock head
+ */
+static inline ticket_t ticketlock_head(ticketdata_t data) { return data; }
+
+/**
+ * Ticket lock initializer
+ */
+#define SPINLOCK_INIT (spinlock_t){1};
+
+/**
+ * load reserved acquire
+ *
+ * \param pvalue pointer to word to load from
+ * \return loaded value
+ */
+static inline unsigned int _lr_aq_w(volatile unsigned int *pvalue)
+{
+    unsigned int value;
+    asm volatile (
+          "lr.w.aq %0, %1"
+        : "=&r"(value), "+A"(*pvalue));
+    return value;
+}
+
+/**
+ * store conditional release
+ *
+ * \param pvalue pointer to word to store to
+ * \param value word to write
+ * \return true for failure
+ */
+static inline int _sc_rl_w(volatile unsigned int *pvalue, unsigned int value)
+{
+    int result;
+    asm volatile (
+          "sc.w.rl %0, %2, %1"
+        : "=&r"(result), "+A"(*pvalue)
+        : "r"(value)
+        : "memory"
+    );
+    return result;
+}
+
+/**
+ * load acquire
+ *
+ * \param pvalue pointer to word to load from
+ * \return loaded value
+ */
+static inline unsigned int _l_aq_w(volatile unsigned int *pvalue)
+{
+    unsigned int result;
+    asm volatile (
+          "lw %0, %1\n"
+          "fence r,rw"
+        : "=&r"(result), "+A"(*pvalue)
+    );
+    return result;
+}
+
+/**
+ * barrier acquire
+ *
+ * order subsequent modifications after prior lock reads
+ *
+ *   <read lock>
+ *   _barrier_acquire();
+ */
+static inline void _barrier_acquire() { asm volatile ("fence r,rw"); }
+
+/**
+ * barrier release
+ *
+ * order prior modifications before subsequent lock write
+ *
+ *   _barrier_release();
+ *   <write lock>
+ */
+static inline void _barrier_release() { asm volatile ("fence rw,w"); }
+
+/**
+ * spinlock lock
+ *
+ * \param lock pointer to lock
+ */
+void spinlock_lock(spinlock_t *lock);
+
+/**
+ * spinlock trylock
+ *
+ * \param lock pointer to lock
+ * \return non zero if lock was successfully taken
+ */
+int spinlock_trylock(spinlock_t *lock);
+
+/**
+ * spinlock unlock
+ *
+ * \param lock pointer to lock
+ */
+void spinlock_unlock(spinlock_t *lock);
+
+/**
+ * spinlock waiters
+ *
+ * \param lock pointer to lock
+ * \return number of waiters
+ */
+int spinlock_waiters(spinlock_t *lock);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/arch/riscv/stdatomic.h b/riscv/riscv-probe/libfemto/include/arch/riscv/stdatomic.h
new file mode 100644
index 0000000..8672636
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/arch/riscv/stdatomic.h
@@ -0,0 +1,592 @@
+// See LICENSE for license details.
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * We use the GCC implementation of __atomic_is_lock_free
+ */
+
+#define atomic_is_lock_free(obj) \
+    __atomic_is_lock_free(sizeof(obj), obj)
+
+/*
+ * GCC Built-in Atomics (not enabled by default, because they are buggy)
+ */
+
+#if defined(LIBFEMTO_USE_GCC_BUILTINS)
+
+#define atomic_flag_test_and_set(obj) \
+    __atomic_test_and_set(obj, __ATOMIC_SEQ_CST)
+#define atomic_flag_test_and_set_explicit(obj, order) \
+    __atomic_test_and_set(obj, order)
+
+#define atomic_flag_clear(obj) \
+    __atomic_test_and_set(obj, __ATOMIC_SEQ_CST)
+#define atomic_flag_clear_explicit(obj, order) \
+    __atomic_test_and_set(obj, order)
+
+#define atomic_thread_fence(order) \
+    __atomic_thread_fence(order)
+
+#define atomic_signal_fence(order) \
+    __atomic_signal_fence(order)
+
+#define atomic_store(obj, desired) \
+    __atomic_store_n(obj, desired, __ATOMIC_SEQ_CST)
+#define atomic_store_explicit(obj, desired, order) \
+    __atomic_store_n(obj, desired, order)
+
+#define atomic_load(obj) \
+    __atomic_load_n(obj, __ATOMIC_SEQ_CST)
+#define atomic_load_explicit(obj,order) \
+    __atomic_load_n(obj,order)
+
+#define atomic_exchange(obj, desired) \
+    __atomic_exchange_n(obj, desired, __ATOMIC_SEQ_CST)
+#define atomic_exchange_explicit(obj, desired, order) \
+    __atomic_exchange_n(obj, desired, order)
+
+#define atomic_compare_exchange_strong(obj, expected, desired) \
+    __atomic_compare_exchange_n(obj, expected, desired, 0, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST)
+#define atomic_compare_exchange_strong_explicit(obj, expected, desired, succ, fail) \
+    __atomic_compare_exchange_n(obj, expected, desired, 0, succ, fail)
+
+#define atomic_compare_exchange_weak(obj, expected, desired) \
+    __atomic_compare_exchange_n(obj, expected, desired, 1, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST)
+#define atomic_compare_exchange_weak_explicit(obj, expected, desired, succ, fail) \
+    __atomic_compare_exchange_n(obj, expected, desired, 1, succ, fail)
+
+#define atomic_fetch_add(obj, arg) \
+    __atomic_fetch_add(obj, arg, __ATOMIC_SEQ_CST)
+#define atomic_fetch_add_explicit(obj, arg, order) \
+    __atomic_fetch_add(obj, arg, order)
+
+#define atomic_fetch_sub(obj, arg) \
+    __atomic_fetch_sub(obj, arg, __ATOMIC_SEQ_CST)
+#define atomic_fetch_sub_explicit(obj, arg, order) \
+    __atomic_fetch_sub(obj, arg, order)
+
+#define atomic_fetch_or(obj, arg) \
+    __atomic_fetch_or(obj, arg, __ATOMIC_SEQ_CST)
+#define atomic_fetch_or_explicit(obj, arg, order) \
+    __atomic_fetch_or(obj, arg, order)
+
+#define atomic_fetch_xor(obj, arg) \
+    __atomic_fetch_xor(obj, arg, __ATOMIC_SEQ_CST)
+#define atomic_fetch_xor_explicit(obj, arg, order) \
+    __atomic_fetch_xor(obj, arg, order)
+
+#define atomic_fetch_and(obj, arg) \
+    __atomic_fetch_and(obj, arg, __ATOMIC_SEQ_CST)
+#define atomic_fetch_and_explicit(obj, arg, order) \
+    __atomic_fetch_and(obj, arg, order)
+#endif
+
+#elif defined(__riscv)
+
+/*
+ * atomic_load
+ */
+
+#define __atomic_load_asm(obj, ASM_BARRIER, ASM_ACQUIRE)            \
+__extension__ ({                                                    \
+    __typeof__(obj) __obj = (obj);                                  \
+    __typeof__(*(obj)) __result;                                    \
+    switch (sizeof(__typeof__(*obj))) {                             \
+    case 1:                                                         \
+        __asm__ volatile (                                          \
+              ASM_BARRIER "\n" "lb %0, %1\n" ASM_ACQUIRE            \
+            : "=&r"(__result), "+A"(*__obj)                         \
+        );                                                          \
+        break;                                                      \
+    case 2:                                                         \
+        __asm__ volatile (                                          \
+              ASM_BARRIER "\n" "lh %0, %1\n" ASM_ACQUIRE            \
+            : "=&r"(__result), "+A"(*__obj)                         \
+        );                                                          \
+        break;                                                      \
+    case 4:                                                         \
+        __asm__ volatile (                                          \
+              ASM_BARRIER "\n" "lw %0, %1\n" ASM_ACQUIRE            \
+            : "=&r"(__result), "+A"(*__obj)                         \
+        );                                                          \
+        break;                                                      \
+    case 8:                                                         \
+        if (__riscv_xlen < 64) __builtin_unreachable();             \
+        __asm__ volatile (                                          \
+              ASM_BARRIER "\n" "ld %0, %1\n" ASM_ACQUIRE            \
+            : "=&r"(__result), "+A"(*__obj)                         \
+        );                                                          \
+        break;                                                      \
+    }                                                               \
+    __result;                                                       \
+})
+
+#define __atomic_load_relaxed(obj) \
+    __atomic_load_asm(obj, "", "")
+
+#define __atomic_load_acquire(obj) \
+    __atomic_load_asm(obj, "", "fence r,rw")
+
+#define __atomic_load_seq_cst(obj) \
+    __atomic_load_asm(obj, "fence rw,rw", "fence r,rw")
+
+#define atomic_load_explicit(obj, order)                            \
+__extension__ ({                                                    \
+    __typeof__(*(obj)) __result;                                    \
+    switch (order) {                                                \
+    case __ATOMIC_ACQUIRE:                                          \
+        __result = __atomic_load_acquire(obj) ; break;              \
+    case __ATOMIC_SEQ_CST:                                          \
+        __result = __atomic_load_seq_cst(obj) ; break;              \
+    case __ATOMIC_RELAXED:                                          \
+    default:                                                        \
+        __result = __atomic_load_relaxed(obj) ; break;              \
+    }                                                               \
+    __result;                                                       \
+})
+
+#define atomic_load(obj) \
+    atomic_load_explicit(obj, __ATOMIC_SEQ_CST)
+
+/*
+ * atomic_store
+ */
+
+#define __atomic_store_asm(obj, value, ASM_RELEASE)                 \
+__extension__ ({                                                    \
+    __typeof__(obj) __obj = (obj);                                  \
+    __typeof__(*(obj)) __value = (value);                           \
+    switch (sizeof(__typeof__(*obj))) {                             \
+    case 1:                                                         \
+        __asm__ volatile (                                          \
+            ASM_RELEASE "\n" "sb %1, %0\n"                          \
+            : "+A"(*__obj) : "r"(__value) : "memory"                \
+        );                                                          \
+        break;                                                      \
+    case 2:                                                         \
+        __asm__ volatile (                                          \
+            ASM_RELEASE "\n" "sh %1, %0\n"                          \
+            : "+A"(*__obj) : "r"(__value) : "memory"                \
+        );                                                          \
+        break;                                                      \
+    case 4:                                                         \
+        __asm__ volatile (                                          \
+            ASM_RELEASE "\n" "sw %1, %0\n"                          \
+            : "+A"(*__obj) : "r"(__value) : "memory"                \
+        );                                                          \
+        break;                                                      \
+    case 8:                                                         \
+        if (__riscv_xlen < 64) __builtin_unreachable();             \
+        __asm__ volatile (                                          \
+            ASM_RELEASE "\n" "sd %1, %0\n"                          \
+            : "+A"(*__obj) : "r"(__value) : "memory"                \
+        );                                                          \
+        break;                                                      \
+    }                                                               \
+})
+
+#define __atomic_store_relaxed(obj, value) \
+    __atomic_store_asm(obj, value, "")
+
+#define __atomic_store_release(obj, value) \
+    __atomic_store_asm(obj, value, "fence rw,w")
+
+#define __atomic_store_seq_cst(obj, value) \
+    __atomic_store_asm(obj, value, "fence rw,w")
+
+
+#define atomic_store_explicit(obj, value, order)                    \
+__extension__ ({                                                    \
+    switch (order) {                                                \
+    case __ATOMIC_RELEASE:                                          \
+        __atomic_store_release(obj, value) ; break;                 \
+    case __ATOMIC_SEQ_CST:                                          \
+        __atomic_store_seq_cst(obj, value) ; break;                 \
+    case __ATOMIC_RELAXED:                                          \
+    default:                                                        \
+        __atomic_store_relaxed(obj, value) ; break;                 \
+    }                                                               \
+})
+
+#define atomic_store(obj, value) \
+    atomic_store_explicit(obj, value, __ATOMIC_SEQ_CST)
+
+/*
+ * atomic_compare_exchange
+ */
+
+#define __atomic_cmpxchg_asm(obj, exp, val, ASM_AQ, ASM_RL)         \
+__extension__ ({                                                    \
+    __typeof__(obj) __obj = (obj);                                  \
+    __typeof__(*(obj)) __exp = (exp);                               \
+    __typeof__(*(obj)) __val = (val);                               \
+    __typeof__(*(obj)) __result;                                    \
+    register unsigned int __ret;                                    \
+    switch (sizeof(__typeof__(*obj))) {                             \
+    case 1:                                                         \
+    case 2:                                                         \
+        __builtin_unreachable();                                    \
+        break;                                                      \
+    case 4:                                                         \
+        __asm__ volatile (                                          \
+              "0:  lr.w" ASM_AQ " %0, %2\n"                         \
+              "    bne  %0, %z3, 1f\n"                              \
+              "    sc.w" ASM_RL " %1, %z4, %2\n"                    \
+              "    bnez %1, 0b  \n" /* always strong */             \
+              "1:\n"                                                \
+            : "=&r"(__result), "=&r" (__ret), "+A"(*__obj)          \
+            : "r"(__exp), "r"(__val)                                \
+            : "memory"                                              \
+        );                                                          \
+        break;                                                      \
+    case 8:                                                         \
+        if (__riscv_xlen < 64) __builtin_unreachable();             \
+        __asm__ volatile (                                          \
+              "0:  lr.d" ASM_AQ " %0, %2\n"                         \
+              "    bne  %0, %z3, 1f\n"                              \
+              "    sc.d" ASM_RL " %1, %z4, %2\n"                    \
+              "    bnez %1, 0b  \n" /* always strong */             \
+              "1:\n"                                                \
+            : "=&r"(__result), "=&r" (__ret), "+A"(*__obj)          \
+            : "r"(__exp), "r"(__val)                                \
+            : "memory"                                              \
+        );                                                          \
+        break;                                                      \
+    }                                                               \
+    __result;                                                       \
+})
+
+#define __atomic_cmpxchg_relaxed(obj, exp, val) \
+    __atomic_cmpxchg_asm(obj, exp, val, "", "")
+
+#define __atomic_cmpxchg_acquire(obj, exp, val) \
+    __atomic_cmpxchg_asm(obj, exp, val, ".aq", "")
+
+#define __atomic_cmpxchg_release(obj, exp, val) \
+    __atomic_cmpxchg_asm(obj, exp, val, "", ".rl")
+
+#define __atomic_cmpxchg_acq_rel(obj, exp, val) \
+    __atomic_cmpxchg_asm(obj, exp, val, ".aq", ".rl")
+
+#define __atomic_cmpxchg_seq_cst(obj, exp, val) \
+    __atomic_cmpxchg_asm(obj, exp, val, ".aqrl", ".rl")
+
+#define __atomic_cmpxchg_strong(obj, exp, val, succ, fail)          \
+__extension__ ({                                                    \
+    __typeof__(*(obj)) __result;                                    \
+    switch (succ) {                                                 \
+    case __ATOMIC_ACQUIRE:                                          \
+    case __ATOMIC_CONSUME: /* promote to acquire for now */         \
+        __result = __atomic_cmpxchg_acquire(obj, exp, val); break;  \
+    case __ATOMIC_RELEASE:                                          \
+        __result = __atomic_cmpxchg_release(obj, exp, val); break;  \
+    case __ATOMIC_ACQ_REL:                                          \
+        __result = __atomic_cmpxchg_acq_rel(obj, exp, val); break;  \
+    case __ATOMIC_SEQ_CST:                                          \
+        __result = __atomic_cmpxchg_seq_cst(obj, exp, val); break;  \
+    case __ATOMIC_RELAXED:                                          \
+    default:                                                        \
+        __result = __atomic_cmpxchg_relaxed(obj, exp, val); break;  \
+    }                                                               \
+    __result;                                                       \
+})
+
+#define atomic_compare_exchange_strong(obj, exp, val) \
+    __atomic_cmpxchg_strong(obj, exp, val, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST)
+#define atomic_compare_exchange_strong_explicit(obj, exp, val, succ, fail) \
+    __atomic_cmpxchg_strong(obj, exp, val, succ, fail)
+
+#define atomic_compare_exchange_weak(obj, exp, val) \
+    __atomic_cmpxchg_strong(obj, exp, val, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST)
+#define atomic_compare_exchange_weak_explicit(obj, exp, val, succ, fail) \
+    __atomic_cmpxchg_strong(obj, exp, val, succ, fail)
+
+/*
+ * atomic_op template
+ */
+
+#define __atomic_op_asm(AMO_OP, obj, arg, ASM_AQRL)                 \
+__extension__ ({                                                    \
+    __typeof__(obj) __obj = (obj);                                  \
+    __typeof__(*(obj)) __arg = (arg);                               \
+    __typeof__(*(obj)) __result;                                    \
+    switch (sizeof(__typeof__(*obj))) {                             \
+    case 1:                                                         \
+    case 2:                                                         \
+        __builtin_unreachable();                                    \
+        break;                                                      \
+    case 4:                                                         \
+        __asm__ volatile (                                          \
+              AMO_OP ".w" ASM_AQRL " %0, %2, %1\n"                  \
+            : "=&r"(__result), "+A"(*__obj)                         \
+            : "r"(__arg)                                            \
+            : "memory"                                              \
+        );                                                          \
+        break;                                                      \
+    case 8:                                                         \
+        if (__riscv_xlen < 64) __builtin_unreachable();             \
+        __asm__ volatile (                                          \
+              AMO_OP ".d" ASM_AQRL " %0, %2, %1\n"                  \
+            : "=&r"(__result), "+A"(*__obj)                         \
+            : "r"(__arg)                                            \
+            : "memory"                                              \
+        );                                                          \
+        break;                                                      \
+    }                                                               \
+    __result;                                                       \
+})
+
+#define __atomic_op_relaxed(op, obj, arg) \
+    __atomic_op_asm(op, obj, arg, "")
+
+#define __atomic_op_acquire(op, obj, arg) \
+    __atomic_op_asm(op, obj, arg, ".aq")
+
+#define __atomic_op_release(op, obj, arg) \
+    __atomic_op_asm(op, obj, arg, ".rl")
+
+#define __atomic_op_acq_rel(op, obj, arg) \
+    __atomic_op_asm(op, obj, arg, ".aqrl")
+
+#define __atomic_op_seq_cst(op, obj, arg) \
+    __atomic_op_asm(op, obj, arg, ".aqrl")
+
+#define __atomic_op(op, obj, arg, order)                            \
+__extension__ ({                                                    \
+    __typeof__(*(obj)) __result;                                    \
+    switch (order) {                                                \
+    case __ATOMIC_ACQUIRE:                                          \
+    case __ATOMIC_CONSUME: /* promote to acquire for now */         \
+        __result = __atomic_op_acquire(op, obj, arg); break;        \
+    case __ATOMIC_RELEASE:                                          \
+        __result = __atomic_op_release(op, obj, arg); break;        \
+    case __ATOMIC_ACQ_REL:                                          \
+        __result = __atomic_op_acq_rel(op, obj, arg); break;        \
+    case __ATOMIC_SEQ_CST:                                          \
+        __result = __atomic_op_seq_cst(op, obj, arg); break;        \
+    case __ATOMIC_RELAXED:                                          \
+    default:                                                        \
+        __result = __atomic_op_relaxed(op, obj, arg); break;        \
+    }                                                               \
+    __result;                                                       \
+})
+
+/*
+ * atomic_exchange
+ */
+
+#define atomic_exchange(obj, arg) \
+    __atomic_op("amoswap", obj, arg, __ATOMIC_SEQ_CST)
+#define atomic_exchange_explicit(obj, arg, order) \
+    __atomic_op("amoswap", obj, arg, order)
+
+/*
+ * atomic_fetch_add
+ */
+
+#define atomic_fetch_add(obj, arg) \
+    __atomic_op("amoadd", obj, arg, __ATOMIC_SEQ_CST)
+#define atomic_fetch_add_explicit(obj, arg, order) \
+    __atomic_op("amoadd", obj, arg, order)
+
+/*
+ * atomic_fetch_sub
+ */
+
+#define atomic_fetch_sub(obj, arg) \
+    __atomic_op("amoadd", obj, -arg, __ATOMIC_SEQ_CST)
+#define atomic_fetch_sub_explicit(obj, arg, order) \
+    __atomic_op("amoadd", obj, -arg, order)
+
+/*
+ * atomic_fetch_or
+ */
+
+#define atomic_fetch_or(obj, arg) \
+    __atomic_op("amoor", obj, -arg, __ATOMIC_SEQ_CST)
+#define atomic_fetch_or_explicit(obj, arg, order) \
+    __atomic_op("amoor", obj, -arg, order)
+
+/*
+ * atomic_fetch_xor
+ */
+
+#define atomic_fetch_xor(obj, arg) \
+    __atomic_op("amoxor", obj, -arg, __ATOMIC_SEQ_CST)
+#define atomic_fetch_xor_explicit(obj, arg, order) \
+    __atomic_op("amoxor", obj, -arg, order)
+
+/*
+ * atomic_fetch_and
+ */
+
+#define atomic_fetch_and(obj, arg) \
+    __atomic_op("amoand", obj, -arg, __ATOMIC_SEQ_CST)
+#define atomic_fetch_and_explicit(obj, arg, order) \
+    __atomic_op("amoand", obj, -arg, order)
+
+
+/*
+ * atomic_flag_test_and_set
+ */
+
+#define __atomic_flag_test_and_set_asm(obj, ASM_AQRL)               \
+__extension__ ({                                                    \
+    struct atomic_flag* __obj = (obj);                              \
+    size_t __shift = (((unsigned long)__obj) & 3) << 3;             \
+    uint32_t *__word = (uint32_t *)(((unsigned long)__obj) & ~3);   \
+    uint32_t __mask = 1 << __shift;                                 \
+    uint32_t __result;                                              \
+    __asm__ volatile (                                              \
+          "amoor.w" ASM_AQRL " %0, %2, %1\n"                        \
+        : "=&r"(__result), "+A"(*__word)                            \
+        : "r"(__mask)                                               \
+        : "memory"                                                  \
+    );                                                              \
+    ((__result >> __shift) & 0xff);                                 \
+})
+
+#define __atomic_flag_test_and_set_relaxed(obj) \
+    __atomic_flag_test_and_set_asm(obj, "")
+
+#define __atomic_flag_test_and_set_acquire(obj) \
+    __atomic_flag_test_and_set_asm(obj, ".aq")
+
+#define __atomic_flag_test_and_set_release(obj) \
+    __atomic_flag_test_and_set_asm(obj, ".rl")
+
+#define __atomic_flag_test_and_set_acq_rel(obj) \
+    __atomic_flag_test_and_set_asm(obj, ".aqrl")
+
+#define __atomic_flag_test_and_set_seq_cst(obj) \
+    __atomic_flag_test_and_set_asm(obj, ".aqrl")
+
+
+#define __atomic_flag_test_and_set(obj, order)                      \
+__extension__ ({                                                    \
+    _Bool __result;                                                 \
+    switch (order) {                                                \
+    case __ATOMIC_ACQUIRE:                                          \
+    case __ATOMIC_CONSUME: /* promote to acquire for now */         \
+        __result = __atomic_flag_test_and_set_acquire(obj); break;  \
+    case __ATOMIC_RELEASE:                                          \
+        __result = __atomic_flag_test_and_set_release(obj); break;  \
+    case __ATOMIC_ACQ_REL:                                          \
+        __result = __atomic_flag_test_and_set_acq_rel(obj); break;  \
+    case __ATOMIC_SEQ_CST:                                          \
+        __result = __atomic_flag_test_and_set_seq_cst(obj); break;  \
+    case __ATOMIC_RELAXED:                                          \
+    default:                                                        \
+        __result = __atomic_flag_test_and_set_relaxed(obj); break;  \
+    }                                                               \
+    __result;                                                       \
+})
+
+#define atomic_flag_test_and_set_explicit(obj, order) \
+    __atomic_flag_test_and_set(obj, order)
+#define atomic_flag_test_and_set(obj, order) \
+    __atomic_flag_test_and_set(obj, __ATOMIC_SEQ_CST)
+
+/*
+ * atomic_flag_clear
+ */
+
+#define __atomic_flag_clear_asm(obj, ASM_AQRL)                      \
+__extension__ ({                                                    \
+    struct atomic_flag* __obj = (obj);                              \
+    size_t __shift = (((unsigned long)__obj) & 3) << 3;             \
+    uint32_t *__word = (uint32_t *)(((unsigned long)__obj) & ~3);   \
+    uint32_t __mask = ~(0xff << __shift);                           \
+    uint32_t __result;                                              \
+    __asm__ volatile (                                              \
+          "amoand.w" ASM_AQRL " %0, %2, %1\n"                       \
+        : "=&r"(__result), "+A"(*__word)                            \
+        : "r"(__mask)                                               \
+        : "memory"                                                  \
+    );                                                              \
+    ((__result >> __shift) & 0xff);                                 \
+})
+
+#define __atomic_flag_clear_relaxed(obj) \
+    __atomic_flag_clear_asm(obj, "")
+
+#define __atomic_flag_clear_acquire(obj) \
+    __atomic_flag_clear_asm(obj, ".aq")
+
+#define __atomic_flag_clear_release(obj) \
+    __atomic_flag_clear_asm(obj, ".rl")
+
+#define __atomic_flag_clear_acq_rel(obj) \
+    __atomic_flag_clear_asm(obj, ".aqrl")
+
+#define __atomic_flag_clear_seq_cst(obj) \
+    __atomic_flag_clear_asm(obj, ".aqrl")
+
+
+#define __atomic_flag_clear(obj, order)                             \
+__extension__ ({                                                    \
+    _Bool __result;                                                 \
+    switch (order) {                                                \
+    case __ATOMIC_ACQUIRE:                                          \
+    case __ATOMIC_CONSUME: /* promote to acquire for now */         \
+        __result = __atomic_flag_clear_acquire(obj); break;         \
+    case __ATOMIC_RELEASE:                                          \
+        __result = __atomic_flag_clear_release(obj); break;         \
+    case __ATOMIC_ACQ_REL:                                          \
+        __result = __atomic_flag_clear_acq_rel(obj); break;         \
+    case __ATOMIC_SEQ_CST:                                          \
+        __result = __atomic_flag_clear_seq_cst(obj); break;         \
+    case __ATOMIC_RELAXED:                                          \
+    default:                                                        \
+        __result = __atomic_flag_clear_relaxed(obj); break;         \
+    }                                                               \
+    __result;                                                       \
+})
+
+#define atomic_flag_clear_explicit(obj, order) \
+    __atomic_flag_clear(obj, order)
+#define atomic_flag_clear(obj, order) \
+    __atomic_flag_clear(obj, __ATOMIC_SEQ_CST)
+
+/*
+ * atomic_thread_fence
+ */
+
+#define __atomic_thread_fence_asm(order)                            \
+__extension__ ({                                                    \
+    switch (order) {                                                \
+    case __ATOMIC_ACQUIRE:                                          \
+    case __ATOMIC_CONSUME: /* promote to acquire for now */         \
+        __asm__ volatile ("fence r,rw" ::: "memory"); break;        \
+    case __ATOMIC_RELEASE:                                          \
+        __asm__ volatile ("fence rw,w" ::: "memory"); break;        \
+    case __ATOMIC_ACQ_REL: /* should be fence.tso */                \
+        __asm__ volatile ("fence rw,rw" ::: "memory"); break;       \
+    case __ATOMIC_SEQ_CST:                                          \
+        __asm__ volatile ("fence rw,rw" ::: "memory"); break;       \
+    case __ATOMIC_RELAXED:                                          \
+    default:                                                        \
+        __asm__ volatile ("" ::: "memory"); break;                  \
+    }                                                               \
+})
+
+#define atomic_thread_fence(order) \
+    __atomic_thread_fence_asm(order)
+
+/*
+ * atomic_signal_fence
+ */
+
+#define atomic_signal_fence(order) \
+    __asm__ volatile ("" ::: "memory")
+
+#endif /* __riscv */
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/arch/riscv/trap.h b/riscv/riscv-probe/libfemto/include/arch/riscv/trap.h
new file mode 100644
index 0000000..0b33ec5
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/arch/riscv/trap.h
@@ -0,0 +1,51 @@
+// See LICENSE for license details.
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef void (*trap_fn)(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc);
+trap_fn get_trap_fn();
+void set_trap_fn(trap_fn fn);
+
+const char * riscv_excp_names[16];
+const char * riscv_intr_names[16];
+
+enum {
+  cause_misaligned_fetch     = 0,
+  cause_fault_fetch          = 1,
+  cause_illegal_instruction  = 2,
+  cause_breakpoint           = 3,
+  cause_misaligned_load      = 4,
+  cause_fault_load           = 5,
+  cause_misaligned_store     = 6,
+  cause_fault_store          = 7,
+  cause_user_ecall           = 8,
+  cause_supervisor_ecall     = 9,
+  cause_hypervisor_ecall     = 10,
+  cause_machine_ecall        = 11,
+  cause_exec_page_fault      = 12,
+  cause_load_page_fault      = 13,
+  cause_store_page_fault     = 15
+};
+
+enum {
+  intr_u_software           = 0,
+  intr_s_software           = 1,
+  intr_h_software           = 2,
+  intr_m_software           = 3,
+  intr_u_timer              = 4,
+  intr_s_timer              = 5,
+  intr_h_timer              = 6,
+  intr_m_timer              = 7,
+  intr_u_external           = 8,
+  intr_s_external           = 9,
+  intr_h_external           = 10,
+  intr_m_external           = 11,
+};
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/auxval.h b/riscv/riscv-probe/libfemto/include/auxval.h
new file mode 100644
index 0000000..de10824
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/auxval.h
@@ -0,0 +1,34 @@
+// See LICENSE for license details.
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+enum {
+    HART0_CLOCK_FREQ           = 0x00010000,
+    UART0_CLOCK_FREQ           = 0x00011000,
+    UART0_BAUD_RATE            = 0x00011100,
+    NS16550A_UART0_CTRL_ADDR   = 0x00030000,
+    RISCV_HTIF_BASE_ADDR       = 0x00050000,
+    SIFIVE_CLINT_CTRL_ADDR     = 0x55550000,
+    SIFIVE_CLIC_CRTL_ADDR      = 0x55550001,
+    SIFIVE_TEST_CTRL_ADDR      = 0x55550002,
+    SIFIVE_UART0_CTRL_ADDR     = 0x55550010,
+    SIFIVE_GPIO0_CTRL_ADDR     = 0x55550020,
+    SIFIVE_SPI0_CTRL_ADDR      = 0x55550030,
+};
+
+typedef struct auxval {
+    unsigned long key;
+    unsigned long val;
+} auxval_t;
+
+extern auxval_t __auxv[];
+
+unsigned long getauxval(unsigned long key);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/device.h b/riscv/riscv-probe/libfemto/include/device.h
new file mode 100644
index 0000000..822ab8c
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/device.h
@@ -0,0 +1,39 @@
+// See LICENSE for license details.
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct console_device {
+    void (*init)();
+    int (*getchar)();
+    int (*putchar)(int);
+} console_device_t;
+
+typedef struct poweroff_device {
+    void (*init)();
+    void (*poweroff)(int);
+} poweroff_device_t;
+
+void register_console(console_device_t *dev);
+void register_poweroff(poweroff_device_t *dev);
+
+extern console_device_t *console_dev;
+extern poweroff_device_t *poweroff_dev;
+
+extern console_device_t console_none;
+extern console_device_t console_htif;
+extern console_device_t console_ns16550a;
+extern console_device_t console_sifive_uart;
+extern console_device_t console_semihost;
+
+extern poweroff_device_t poweroff_none;
+extern poweroff_device_t poweroff_htif;
+extern poweroff_device_t poweroff_sifive_test;
+extern poweroff_device_t poweroff_semihost;
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/elf.h b/riscv/riscv-probe/libfemto/include/elf.h
new file mode 100644
index 0000000..abfb892
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/elf.h
@@ -0,0 +1,545 @@
+#pragma once
+
+#include <stdint.h>
+
+typedef uint32_t Elf32_Addr;
+typedef uint16_t Elf32_Half;
+typedef uint32_t Elf32_Off;
+typedef int32_t  Elf32_Sword;
+typedef uint32_t Elf32_Word;
+typedef uint8_t  Elf32_Byte;
+
+typedef uint64_t Elf64_Addr;
+typedef uint16_t Elf64_Half;
+typedef uint64_t Elf64_Off;
+typedef int32_t  Elf64_Sword;
+typedef uint32_t Elf64_Word;
+typedef uint64_t Elf64_Xword;
+typedef int64_t  Elf64_Sxword;
+typedef uint8_t  Elf64_Byte;
+
+enum {
+	AT_NULL = 0,
+	AT_PHDR = 3,
+	AT_PHENT = 4,
+	AT_PHNUM = 5,
+	AT_PAGESZ = 6,
+	AT_BASE = 7,
+	AT_FLAGS = 8,
+	AT_ENTRY = 9,
+	AT_UID = 11,
+	AT_EUID = 12,
+	AT_GID = 13,
+	AT_EGID  = 14,
+	AT_CLKTCK = 17,
+	AT_SECURE = 23,
+	AT_RANDOM = 25
+};
+
+typedef struct {
+	Elf32_Word a_type;
+	Elf32_Word a_val;
+} Elf32_auxv;
+
+typedef struct {
+	Elf64_Word a_type;
+	Elf64_Word a_val;
+} Elf64_auxv;
+
+enum {
+	ET_NONE = 0,
+	ET_REL = 1,
+	ET_EXEC = 2,
+	ET_DYN = 3,
+	ET_CORE = 4,
+	ET_LOPROC = 0xff00,
+	ET_HIPROC = 0xffff
+};
+
+enum {
+	EM_NONE = 0,
+	EM_X86_64 = 62,
+	EM_RISCV = 243
+};
+
+enum {
+	EV_NONE = 0,
+	EV_CURRENT = 1
+};
+
+enum {
+	EI_MAG0 = 0,
+	EI_MAG1 = 1,
+	EI_MAG2 = 2,
+	EI_MAG3 = 3,
+	EI_CLASS = 4,
+	EI_DATA = 5,
+	EI_VERSION = 6,
+	EI_OSABI = 7,
+	EI_ABIVERSION = 8,
+	EI_PAD = 9,
+	EI_NIDENT = 16
+};
+
+enum {
+	ELFMAG0 = 0x7f,
+	ELFMAG1 = 'E',
+	ELFMAG2 = 'L',
+	ELFMAG3 = 'F'
+};
+
+enum {
+	ELFCLASSNONE = 0,
+	ELFCLASS32 = 1,
+	ELFCLASS64 = 2,
+	ELFCLASS128 = 3
+};
+
+enum {
+	ELFDATANONE = 0,
+	ELFDATA2LSB = 1,
+	ELFDATA2MSB = 2
+};
+
+enum {
+	ELFOSABI_SYSV = 0,
+	ELFOSABI_LINUX = 3,
+	ELFOSABI_SOLARIS = 6,
+	ELFOSABI_FREEBSD = 9
+};
+
+enum {
+	ELFABIVERSION_NONE = 0
+};
+
+typedef struct {
+	Elf32_Byte  e_ident[EI_NIDENT];
+	Elf32_Half  e_type;
+	Elf32_Half  e_machine;
+	Elf32_Word  e_version;
+	Elf32_Addr  e_entry;
+	Elf32_Off   e_phoff;
+	Elf32_Off   e_shoff;
+	Elf32_Word  e_flags;
+	Elf32_Half  e_ehsize;
+	Elf32_Half  e_phentsize;
+	Elf32_Half  e_phnum;
+	Elf32_Half  e_shentsize;
+	Elf32_Half  e_shnum;
+	Elf32_Half  e_shstrndx;
+} Elf32_Ehdr;
+
+typedef struct {
+	Elf64_Byte  e_ident[EI_NIDENT];
+	Elf64_Half  e_type;
+	Elf64_Half  e_machine;
+	Elf64_Word  e_version;
+	Elf64_Addr  e_entry;
+	Elf64_Off   e_phoff;
+	Elf64_Off   e_shoff;
+	Elf64_Word  e_flags;
+	Elf64_Half  e_ehsize;
+	Elf64_Half  e_phentsize;
+	Elf64_Half  e_phnum;
+	Elf64_Half  e_shentsize;
+	Elf64_Half  e_shnum;
+	Elf64_Half  e_shstrndx;
+} Elf64_Ehdr;
+
+enum {
+	PT_NULL = 0,
+	PT_LOAD = 1,
+	PT_DYNAMIC = 2,
+	PT_INTERP = 3,
+	PT_NOTE = 4,
+	PT_SHLIB = 5,
+	PT_PHDR = 6,
+	PT_TLS = 7,
+	PT_LOOS = 0x60000000,
+	PT_GNU_EH_FRAME = 0x6474e550,
+	PT_GNU_STACK = 0x6474e551,
+	PT_GNU_RELRO = 0x6474e552,
+	PT_HIOS = 0x6fffffff,
+	PT_LOPROC = 0x70000000,
+	PT_HIPROC = 0x7fffffff
+};
+
+enum {
+	PF_X = 0x1,
+	PF_W = 0x2,
+	PF_R = 0x4,
+	PF_MASKOS = 0x00FF0000,
+	PF_MASKPROC = 0xFF000000,
+};
+
+typedef struct {
+	Elf32_Word  p_type;
+	Elf32_Off   p_offset;
+	Elf32_Addr  p_vaddr;
+	Elf32_Addr  p_paddr;
+	Elf32_Word  p_filesz;
+	Elf32_Word  p_memsz;
+	Elf32_Word  p_flags;
+	Elf32_Word  p_align;
+} Elf32_Phdr;
+
+typedef struct {
+	Elf64_Word  p_type;
+	Elf64_Word  p_flags;
+	Elf64_Off   p_offset;
+	Elf64_Addr  p_vaddr;
+	Elf64_Addr  p_paddr;
+	Elf64_Xword p_filesz;
+	Elf64_Xword p_memsz;
+	Elf64_Xword p_align;
+} Elf64_Phdr;
+
+enum {
+	SHN_UNDEF = 0,
+	SHN_LOPROC = 0xff00,
+	SHN_HIPROC = 0xff1f,
+	SHN_LOOS = 0xff20,
+	SHN_HIOS = 0xff3f,
+	SHN_ABS = 0xfff1,
+	SHN_COMMON = 0xfff2,
+	SHN_XINDEX = 0xffff,
+	SHN_HIRESERVE = 0xffff,
+};
+
+enum {
+	SHT_NULL = 0,
+	SHT_PROGBITS = 1,
+	SHT_SYMTAB = 2,
+	SHT_STRTAB = 3,
+	SHT_RELA = 4,
+	SHT_HASH = 5,
+	SHT_DYNAMIC = 6,
+	SHT_NOTE = 7,
+	SHT_NOBITS = 8,
+	SHT_REL = 9,
+	SHT_SHLIB = 10,
+	SHT_DYNSYM = 11,
+	SHT_INIT_ARRAY = 14,
+	SHT_FINI_ARRAY = 15,
+	SHT_PREINIT_ARRAY = 16,
+	SHT_GROUP = 17,
+	SHT_SYMTAB_SHNDX = 18,
+	SHT_LOOS = 0x60000000,
+	SHT_GNU_VERDEF = 0x6ffffffd,
+	SHT_GNU_VERNEED = 0x6ffffffe,
+	SHT_GNU_VERSYM = 0x6fffffff,
+	SHT_HIOS = 0x6fffffff,
+	SHT_LOPROC = 0x70000000,
+	SHT_HIPROC = 0x7fffffff,
+	SHT_LOUSER = 0x80000000,
+	SHT_HIUSER = 0xffffffff
+};
+
+enum {
+	SHF_WRITE = 0x1,
+	SHF_ALLOC = 0x2,
+	SHF_EXECINSTR = 0x4,
+	SHF_MERGE = 0x10,
+	SHF_STRINGS = 0x20,
+	SHF_INFO_LINK = 0x40,
+	SHF_LINK_ORDER = 0x80,
+	SHF_GROUP = 0x200,
+	SHF_TLS = 0x400,
+	SHF_MASKOS = 0x0F000000,
+	SHF_MASKPROC = 0xf0000000
+};
+
+enum {
+  GRP_COMDAT = 0x1,
+  GRP_MASKOS = 0x0ff00000,
+  GRP_MASKPROC = 0xf0000000
+};
+
+typedef struct {
+	Elf32_Word  sh_name;
+	Elf32_Word  sh_type;
+	Elf32_Word  sh_flags;
+	Elf32_Addr  sh_addr;
+	Elf32_Off   sh_offset;
+	Elf32_Word  sh_size;
+	Elf32_Word  sh_link;
+	Elf32_Word  sh_info;
+	Elf32_Word  sh_addralign;
+	Elf32_Word  sh_entsize;
+} Elf32_Shdr;
+
+typedef struct {
+	Elf64_Word  sh_name;
+	Elf64_Word  sh_type;
+	Elf64_Xword sh_flags;
+	Elf64_Addr  sh_addr;
+	Elf64_Off   sh_offset;
+	Elf64_Xword sh_size;
+	Elf64_Word  sh_link;
+	Elf64_Word  sh_info;
+	Elf64_Xword sh_addralign;
+	Elf64_Xword sh_entsize;
+} Elf64_Shdr;
+
+
+typedef struct {
+	Elf32_Word n_namesz;
+	Elf32_Word n_descsz;
+	Elf32_Word n_type;
+} Elf32_Nhdr;
+
+typedef struct {
+	Elf64_Word n_namesz;
+	Elf64_Word n_descsz;
+	Elf64_Word n_type;
+} Elf64_Nhdr;
+
+static inline const Elf32_Byte ELF32_ST_BIND(Elf32_Word i) { return i >> 4; }
+static inline const Elf32_Byte ELF32_ST_TYPE(Elf32_Word i) { return i & 0xf; }
+static inline const Elf32_Byte ELF32_ST_INFO(Elf32_Word b, Elf32_Word t) { return (b << 4) | (t & 0xf); }
+
+static inline const Elf32_Byte ELF64_ST_BIND(Elf32_Word i) { return i >> 4; }
+static inline const Elf32_Byte ELF64_ST_TYPE(Elf32_Word i) { return i & 0xf; }
+static inline const Elf32_Byte ELF64_ST_INFO(Elf32_Word b, Elf32_Word t) { return (b << 4) | (t & 0xf); }
+
+enum {
+	STB_LOCAL = 0,
+	STB_GLOBAL = 1,
+	STB_WEAK = 2,
+	STB_LOOS = 10,
+	STB_HIOS = 12,
+	STB_LOPROC = 13,
+	STB_HIPROC = 15
+};
+
+enum {
+	STT_NOTYPE = 0,
+	STT_OBJECT = 1,
+	STT_FUNC = 2,
+	STT_SECTION = 3,
+	STT_FILE = 4,
+	STT_LOOS = 10,
+	STT_HIOS = 12,
+	STT_LOPROC = 13,
+	STT_HIPROC = 15
+};
+
+enum {
+	STV_DEFAULT = 0,
+	STV_INTERNAL = 1,
+	STV_HIDDEN = 2,
+	STV_PROTECTED = 3
+};
+
+typedef struct {
+	Elf32_Word  st_name;
+	Elf32_Addr  st_value;
+	Elf32_Word  st_size;
+	Elf32_Byte  st_info;
+	Elf32_Byte  st_other;
+	Elf32_Half  st_shndx;
+} Elf32_Sym;
+
+typedef struct {
+	Elf64_Word  st_name;
+	Elf64_Byte  st_info;
+	Elf64_Byte  st_other;
+	Elf64_Half  st_shndx;
+	Elf64_Addr  st_value;
+	Elf64_Xword st_size;
+} Elf64_Sym;
+
+static inline const Elf32_Word ELF32_R_SYM(Elf32_Word i) { return i >> 8; }
+static inline const Elf32_Word ELF32_R_TYPE(Elf32_Word i) { return i & 0xff; }
+static inline const Elf32_Word ELF32_R_INFO(Elf32_Word s, Elf32_Word t) { return (s << 8) | (t & 0xff); }
+
+static inline const Elf64_Xword ELF64_R_SYM(Elf64_Xword i) { return i >> 32; }
+static inline const Elf64_Xword ELF64_R_TYPE(Elf64_Xword i) { return i & 0xffffffffUL; }
+static inline const Elf64_Xword ELF64_R_INFO(Elf64_Xword s, Elf64_Xword t) { return (s << 32) | (t & 0xffffffffUL); }
+
+typedef struct {
+	Elf32_Addr  r_offset;
+	Elf32_Word  r_info;
+} Elf32_Rel;
+
+typedef struct {
+	Elf32_Addr  r_offset;
+	Elf32_Word  r_info;
+	Elf32_Sword r_addend;
+} Elf32_Rela;
+
+typedef struct
+{
+	Elf64_Addr  r_offset;
+	Elf64_Xword r_info;
+} Elf64_Rel;
+
+typedef struct
+{
+	Elf64_Addr r_offset;
+	Elf64_Xword r_info;
+	Elf64_Sxword r_addend;
+} Elf64_Rela;
+
+enum {
+	R_X86_64_NONE = 0,
+	R_X86_64_64 = 1,
+	R_X86_64_PC32 = 2,
+	R_X86_64_GOT32 = 3,
+	R_X86_64_PLT32 = 4,
+	R_X86_64_COPY = 5,
+	R_X86_64_GLOB_DAT = 6,
+	R_X86_64_JUMP_SLOT = 7,
+	R_X86_64_RELATIVE = 8,
+	R_X86_64_GOTPCREL = 9,
+	R_X86_64_32 = 10,
+	R_X86_64_32S = 11,
+	R_X86_64_16 = 12,
+	R_X86_64_PC16 = 13,
+	R_X86_64_8 = 14,
+	R_X86_64_PC8 = 15,
+	R_X86_64_DTPMOD64 = 16,
+	R_X86_64_DTPOFF64 = 17,
+	R_X86_64_TPOFF64  = 18,
+	R_X86_64_TLSGD = 19,
+	R_X86_64_TLSLD = 20,
+	R_X86_64_DTPOFF32 = 21,
+	R_X86_64_GOTTPOFF = 22,
+	R_X86_64_TPOFF32 = 23,
+	R_X86_64_PC64 = 24,
+	R_X86_64_GOTOFF64 = 25,
+	R_X86_64_GOTPC32 = 26,
+	R_X86_64_SIZE32 = 32,
+	R_X86_64_SIZE64 = 33,
+	R_X86_64_GOTPC32_TLSDESC = 34,
+	R_X86_64_TLSDESC_CALL = 35,
+	R_X86_64_TLSDESC = 36,
+	R_X86_64_IRELATIVE = 37,
+	R_X86_64_RELATIVE64 = 38
+};
+
+enum {
+	R_RISCV_NONE = 0,
+	R_RISCV_32 = 1,
+	R_RISCV_64 = 2,
+	R_RISCV_RELATIVE = 3,
+	R_RISCV_COPY = 4,
+	R_RISCV_JUMP_SLOT = 5,
+	R_RISCV_TLS_DTPMOD32 = 6,
+	R_RISCV_TLS_DTPMOD64 = 7,
+	R_RISCV_TLS_DTPREL32 = 8,
+	R_RISCV_TLS_DTPREL64 = 9,
+	R_RISCV_TLS_TPREL32 = 10,
+	R_RISCV_TLS_TPREL64 = 11,
+	R_RISCV_BRANCH = 16,
+	R_RISCV_JAL = 17,
+	R_RISCV_CALL = 18,
+	R_RISCV_CALL_PLT = 19,
+	R_RISCV_GOT_HI20 = 20,
+	R_RISCV_TLS_GOT_HI20 = 21,
+	R_RISCV_TLS_GD_HI20 = 22,
+	R_RISCV_PCREL_HI20 = 23,
+	R_RISCV_PCREL_LO12_I = 24,
+	R_RISCV_PCREL_LO12_S = 25,
+	R_RISCV_HI20 = 26,
+	R_RISCV_LO12_I = 27,
+	R_RISCV_LO12_S = 28,
+	R_RISCV_TPREL_HI20 = 29,
+	R_RISCV_TPREL_LO12_I = 30,
+	R_RISCV_TPREL_LO12_S = 31,
+	R_RISCV_TPREL_ADD = 32,
+	R_RISCV_ADD8 = 33,
+	R_RISCV_ADD16 = 34,
+	R_RISCV_ADD32 = 35,
+	R_RISCV_ADD64 = 36,
+	R_RISCV_SUB8 = 37,
+	R_RISCV_SUB16 = 38,
+	R_RISCV_SUB32 = 39,
+	R_RISCV_SUB64 = 40,
+	R_RISCV_GNU_VTINHERIT = 41,
+	R_RISCV_GNU_VTENTRY = 42,
+	R_RISCV_ALIGN = 43,
+	R_RISCV_RVC_BRANCH = 44,
+	R_RISCV_RVC_JUMP = 45,
+	R_RISCV_RVC_LUI = 46,
+	R_RISCV_GPREL_I = 47,
+	R_RISCV_GPREL_S = 48,
+	R_RISCV_TPREL_I = 49,
+	R_RISCV_TPREL_S = 50,
+	R_RISCV_RELAX = 51,
+	R_RISCV_SUB6 = 52,
+	R_RISCV_SET6 = 53,
+	R_RISCV_SET8 = 54,
+	R_RISCV_SET16 = 55,
+	R_RISCV_SET32 = 56,
+};
+
+enum {
+	EF_RISCV_RVC = 0x1,
+	EF_RISCV_FLOAT_ABI_SINGLE = 0x2,
+	EF_RISCV_FLOAT_ABI_DOUBLE = 0x4,
+	EF_RISCV_FLOAT_ABI_QUAD = 0x6,
+	EF_RISCV_RVE = 0x8
+};
+
+enum {
+	DF_ORIGIN = 1,
+	DF_SYMBOLIC = 2,
+	DF_TEXTREL = 4,
+	DF_BIND_NOW = 8,
+	DF_STATIC_TLS = 16
+};
+
+enum {
+	DT_NULL = 0,
+	DT_NEEDED = 1,
+	DT_PLTRELSZ = 2,
+	DT_PLTGOT = 3,
+	DT_HASH = 4,
+	DT_STRTAB = 5,
+	DT_SYMTAB = 6,
+	DT_RELA = 7,
+	DT_RELASZ = 8,
+	DT_RELAENT = 9,
+	DT_STRSZ = 10,
+	DT_SYMENT = 11,
+	DT_INIT = 12,
+	DT_FINI = 13,
+	DT_SONAME = 14,
+	DT_RPATH = 15,
+	DT_SYMBOLIC = 16,
+	DT_REL = 17,
+	DT_RELSZ = 18,
+	DT_RELENT = 19,
+	DT_PLTREL = 20,
+	DT_DEBUG = 21,
+	DT_TEXTREL = 22,
+	DT_JMPREL = 23,
+	DT_BIND_NOW = 24,
+	DT_INIT_ARRAY = 25,
+	DT_FINI_ARRAY = 26,
+	DT_INIT_ARRAYSZ = 27,
+	DT_FINI_ARRAYSZ = 28,
+	DT_RUNPATH = 29,
+	DT_FLAGS = 30,
+	DT_LOOS = 0x60000000,
+	DT_HIOS = 0x6fffffff,
+	DT_LOPROC = 0x70000000,
+	DT_HIPROC = 0x7fffffff
+};
+
+typedef struct {
+	Elf32_Sword  d_tag;
+	union {
+		Elf32_Word  d_val;
+		Elf32_Addr  d_ptr;
+	} d_un;
+} Elf32_Dyn;
+
+typedef struct {
+	Elf64_Sxword d_tag;
+	union {
+		Elf64_Xword d_val;
+		Elf64_Addr  d_ptr;
+	} d_un;
+} Elf64_Dyn;
diff --git a/riscv/riscv-probe/libfemto/include/endian.h b/riscv/riscv-probe/libfemto/include/endian.h
new file mode 100644
index 0000000..8510f29
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/endian.h
@@ -0,0 +1,95 @@
+#pragma once
+
+#define bswap16(x) __builtin_bswap16(x)
+#define bswap32(x) __builtin_bswap32(x)
+#define bswap64(x) __builtin_bswap64(x)
+
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+static inline uint16_t htobe16(uint16_t x) { return bswap16(x); }
+static inline uint16_t htole16(uint16_t x) { return x; }
+static inline uint16_t be16toh(uint16_t x) { return bswap16(x); }
+static inline uint16_t le16toh(uint16_t x) { return x; }
+
+static inline uint32_t htobe32(uint32_t x) { return bswap32(x); }
+static inline uint32_t htole32(uint32_t x) { return x; }
+static inline uint32_t be32toh(uint32_t x) { return bswap32(x); }
+static inline uint32_t le32toh(uint32_t x) { return x; }
+
+static inline uint64_t htobe64(uint64_t x) { return bswap64(x); }
+static inline uint64_t htole64(uint64_t x) { return x; }
+static inline uint64_t be64toh(uint64_t x) { return bswap64(x); }
+static inline uint64_t le64toh(uint64_t x) { return x; }
+#elif __BYTE_ORDER == __BIG_ENDIAN
+static inline uint16_t htobe16(uint16_t x) { return x; }
+static inline uint16_t htole16(uint16_t x) { return bswap16(x); }
+static inline uint16_t be16toh(uint16_t x) { return x; }
+static inline uint16_t le16toh(uint16_t x) { return bswap16(x); }
+
+static inline uint32_t htobe32(uint32_t x) { return x; }
+static inline uint32_t htole32(uint32_t x) { return bswap32(x); }
+static inline uint32_t be32toh(uint32_t x) { return x; }
+static inline uint32_t le32toh(uint16_t x) { return bswap64(x); }
+
+static inline uint64_t htobe64(uint64_t x) { return x; }
+static inline uint64_t htole64(uint64_t x) { return bswap64(x); }
+static inline uint64_t be64toh(uint64_t x) { return x; }
+static inline uint64_t le64toh(uint64_t x) { return bswap64(x); }
+#endif
+
+#if __SIZE_WIDTH__ == 32
+#define _htobel htobe32
+#define _beltoh be32toh
+#define _htolel htole32
+#define _leltoh le32toh
+#endif
+
+#if __SIZE_WIDTH__ == 64
+#define _htobel htobe64
+#define _beltoh be64toh
+#define _htolel htole64
+#define _leltoh le64toh
+#endif
+
+#define htobe(X) _Generic((X),                \
+                 short: htobe16,              \
+                 unsigned short: htobe16,     \
+                 int: htobe32,                \
+                 unsigned int: htobe32,       \
+                 long: _htobel,               \
+                 unsigned long: _htobel,      \
+                 long long: htobe64,          \
+                 unsigned long long: htobe64  \
+                 )(X)
+
+#define betoh(X) _Generic((X),                \
+                 short: be16toh,              \
+                 unsigned short: be16toh,     \
+                 int: be32toh,                \
+                 unsigned int: be32toh,       \
+                 long: _beltoh,               \
+                 unsigned long: _beltoh,      \
+                 long long: be64toh,          \
+                 unsigned long long: be64toh  \
+                 )(X)
+
+#define htole(X) _Generic((X),                \
+                 short: htole16,              \
+                 unsigned short: htole16,     \
+                 int: htole32,                \
+                 unsigned int: htole32,       \
+                 long: _htolel,               \
+                 unsigned long: _htolel,      \
+                 long long: htole64,          \
+                 unsigned long long: htole64  \
+                 )(X)
+
+#define letoh(X) _Generic((X),                \
+                 short: le16toh,              \
+                 unsigned short: le16toh,     \
+                 int: le32toh,                \
+                 unsigned int: le32toh,       \
+                 long: _leltoh,               \
+                 unsigned long: _leltoh,      \
+                 long long: le64toh,          \
+                 unsigned long long: le64toh  \
+                 )(X)
diff --git a/riscv/riscv-probe/libfemto/include/femto.h b/riscv/riscv-probe/libfemto/include/femto.h
new file mode 100644
index 0000000..aa8b4cf
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/femto.h
@@ -0,0 +1,14 @@
+// See LICENSE for license details.
+
+#pragma once
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdarg.h>
+#include <stdlib.h>
+#include <stdbits.h>
+#include <string.h>
+
+#include "auxval.h"
+#include "device.h"
diff --git a/riscv/riscv-probe/libfemto/include/list.h b/riscv/riscv-probe/libfemto/include/list.h
new file mode 100644
index 0000000..5f4cc72
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/list.h
@@ -0,0 +1,495 @@
+/*
+ * Minimal Linux-like double-linked list helper functions
+ *
+ * Copyright (c) 2012-2016, Sven Eckelmann <[email protected]>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stddef.h>
+
+/**
+ * container_of() - Calculate address of object that contains address ptr
+ * @ptr: pointer to member variable
+ * @type: type of the structure containing ptr
+ * @member: name of the member variable in struct @type
+ *
+ * Return: @type pointer of object containing ptr
+ */
+
+#define container_of(ptr, type, member) __extension__ ({ \
+	const __typeof__(((type *)0)->member) *__pmember = (ptr); \
+	(type *)((char *)__pmember - offsetof(type, member)); })
+
+/**
+ * struct list_head - Head and node of a double-linked list
+ * @prev: pointer to the previous node in the list
+ * @next: pointer to the next node in the list
+ *
+ * The simple double-linked list consists of a head and nodes attached to
+ * this head. Both node and head share the same struct type. The list_*
+ * functions and macros can be used to access and modify this data structure.
+ *
+ * The @prev pointer of the list head points to the last list node of the
+ * list and @next points to the first list node of the list. For an empty list,
+ * both member variables point to the head.
+ *
+ * The list nodes are usually embedded in a container structure which holds the
+ * actual data. Such an container object is called entry. The helper list_entry
+ * can be used to calculate the object address from the address of the node.
+ */
+struct list_head {
+	struct list_head *prev;
+	struct list_head *next;
+};
+
+/**
+ * LIST_HEAD - Declare list head and initialize it
+ * @head: name of the new object
+ */
+#define LIST_HEAD(head) \
+	struct list_head head = { &(head), &(head) }
+
+/**
+ * INIT_LIST_HEAD() - Initialize empty list head
+ * @head: pointer to list head
+ *
+ * This can also be used to initialize a unlinked list node.
+ *
+ * A node is usually linked inside a list, will be added to a list in
+ * the near future or the entry containing the node will be free'd soon.
+ *
+ * But an unlinked node may be given to a function which uses list_del(_init)
+ * before it ends up in a previously mentioned state. The list_del(_init) on an
+ * initialized node is well defined and safe. But the result of a
+ * list_del(_init) on an uninitialized node is undefined (unrelated memory is
+ * modified, crashes, ...).
+ */
+static __inline__ void INIT_LIST_HEAD(struct list_head *head)
+{
+	head->next = head;
+	head->prev = head;
+}
+
+/**
+ * list_add() - Add a list node to the beginning of the list
+ * @node: pointer to the new node
+ * @head: pointer to the head of the list
+ */
+static __inline__ void list_add(struct list_head *node,
+				struct list_head *head)
+{
+	struct list_head *next = head->next;
+
+	next->prev = node;
+	node->next = next;
+	node->prev = head;
+	head->next = node;
+}
+
+/**
+ * list_add_tail() - Add a list node to the end of the list
+ * @node: pointer to the new node
+ * @head: pointer to the head of the list
+ */
+static __inline__ void list_add_tail(struct list_head *node,
+				     struct list_head *head)
+{
+	struct list_head *prev = head->prev;
+
+	prev->next = node;
+	node->next = head;
+	node->prev = prev;
+	head->prev = node;
+}
+
+/**
+ * list_add_before() - Add a list node before another node to the list
+ * @new_node: pointer to the new node
+ * @node: pointer to the reference node in the list
+ *
+ * WARNING this functionality is not available in the Linux list implementation
+ */
+#define list_add_before(new_node, node) \
+	list_add_tail(new_node, node)
+
+/**
+ * list_add_behind() - Add a list node behind another node to the list
+ * @new_node: pointer to the new node
+ * @node: pointer to the reference node in the list
+ *
+ * WARNING this functionality is not available in the Linux list implementation
+ */
+#define list_add_behind(new_node, node) \
+	list_add(new_node, node)
+
+/**
+ * list_del() - Remove a list node from the list
+ * @node: pointer to the node
+ *
+ * The node is only removed from the list. Neither the memory of the removed
+ * node nor the memory of the entry containing the node is free'd. The node
+ * has to be handled like an uninitialized node. Accessing the next or prev
+ * pointer of the node is not safe.
+ *
+ * Unlinked, initialized nodes are also uninitialized after list_del.
+ *
+ * LIST_POISONING can be enabled during build-time to provoke an invalid memory
+ * access when the memory behind the next/prev pointer is used after a list_del.
+ * This only works on systems which prohibit access to the predefined memory
+ * addresses.
+ */
+static __inline__ void list_del(struct list_head *node)
+{
+	struct list_head *next = node->next;
+	struct list_head *prev = node->prev;
+
+	next->prev = prev;
+	prev->next = next;
+
+#ifdef LIST_POISONING
+	node->prev = (struct list_head *)(0x00100100);
+	node->next = (struct list_head *)(0x00200200);
+#endif
+}
+
+/**
+ * list_del_init() - Remove a list node from the list and reinitialize it
+ * @node: pointer to the node
+ *
+ * The removed node will not end up in an uninitialized state like when using
+ * list_del. Instead the node is initialized again to the unlinked state.
+ */
+static __inline__ void list_del_init(struct list_head *node)
+{
+	list_del(node);
+	INIT_LIST_HEAD(node);
+}
+
+/**
+ * list_empty() - Check if list head has no nodes attached
+ * @head: pointer to the head of the list
+ *
+ * Return: 0 - list is not empty !0 - list is empty
+ */
+static __inline__ int list_empty(const struct list_head *head)
+{
+	return (head->next == head);
+}
+
+/**
+ * list_is_singular() - Check if list head has exactly one node attached
+ * @head: pointer to the head of the list
+ *
+ * Return: 0 - list is not singular !0 -list has exactly one entry
+ */
+static __inline__ int list_is_singular(const struct list_head *head)
+{
+	return (!list_empty(head) && head->prev == head->next);
+}
+
+/**
+ * list_splice() - Add list nodes from a list to beginning of another list
+ * @list: pointer to the head of the list with the node entries
+ * @head: pointer to the head of the list
+ *
+ * All nodes from @list are added to to the beginning of the list of @head.
+ * It is similar to list_add but for multiple nodes. The @list head is not
+ * modified and has to be initialized to be used as a valid list head/node
+ * again.
+ */
+static __inline__ void list_splice(struct list_head *list,
+				   struct list_head *head)
+{
+	struct list_head *head_first = head->next;
+	struct list_head *list_first = list->next;
+	struct list_head *list_last = list->prev;
+
+	if (list_empty(list))
+		return;
+
+	head->next = list_first;
+	list_first->prev = head;
+
+	list_last->next = head_first;
+	head_first->prev = list_last;
+}
+
+/**
+ * list_splice_tail() - Add list nodes from a list to end of another list
+ * @list: pointer to the head of the list with the node entries
+ * @head: pointer to the head of the list
+ *
+ * All nodes from @list are added to to the end of the list of @head.
+ * It is similar to list_add_tail but for multiple nodes. The @list head is not
+ * modified and has to be initialized to be used as a valid list head/node
+ * again.
+ */
+static __inline__ void list_splice_tail(struct list_head *list,
+					struct list_head *head)
+{
+	struct list_head *head_last = head->prev;
+	struct list_head *list_first = list->next;
+	struct list_head *list_last = list->prev;
+
+	if (list_empty(list))
+		return;
+
+	head->prev = list_last;
+	list_last->next = head;
+
+	list_first->prev = head_last;
+	head_last->next = list_first;
+}
+
+/**
+ * list_splice_init() - Move list nodes from a list to beginning of another list
+ * @list: pointer to the head of the list with the node entries
+ * @head: pointer to the head of the list
+ *
+ * All nodes from @list are added to to the beginning of the list of @head.
+ * It is similar to list_add but for multiple nodes.
+ *
+ * The @list head will not end up in an uninitialized state like when using
+ * list_splice. Instead the @list is initialized again to the an empty
+ * list/unlinked state.
+ */
+static __inline__ void list_splice_init(struct list_head *list,
+					struct list_head *head)
+{
+	list_splice(list, head);
+	INIT_LIST_HEAD(list);
+}
+
+/**
+ * list_splice_tail_init() - Move list nodes from a list to end of another list
+ * @list: pointer to the head of the list with the node entries
+ * @head: pointer to the head of the list
+ *
+ * All nodes from @list are added to to the end of the list of @head.
+ * It is similar to list_add_tail but for multiple nodes.
+ *
+ * The @list head will not end up in an uninitialized state like when using
+ * list_splice. Instead the @list is initialized again to the an empty
+ * list/unlinked state.
+ */
+static __inline__ void list_splice_tail_init(struct list_head *list,
+					     struct list_head *head)
+{
+	list_splice_tail(list, head);
+	INIT_LIST_HEAD(list);
+}
+
+/**
+ * list_cut_position() - Move beginning of a list to another list
+ * @head_to: pointer to the head of the list which receives nodes
+ * @head_from: pointer to the head of the list
+ * @node: pointer to the node in which defines the cutting point
+ *
+ * All entries from the beginning of the list @head_from to (including) the
+ * @node is moved to @head_from.
+ *
+ * @head_to is replaced when @head_from is not empty. @node must be a real
+ * list node from @head_from or the behavior is undefined.
+ */
+static __inline__ void list_cut_position(struct list_head *head_to,
+					 struct list_head *head_from,
+					 struct list_head *node)
+{
+	struct list_head *head_from_first = head_from->next;
+
+	if (list_empty(head_from))
+		return;
+
+	if (head_from == node) {
+		INIT_LIST_HEAD(head_to);
+		return;
+	}
+
+	head_from->next = node->next;
+	head_from->next->prev = head_from;
+
+	head_to->prev = node;
+	node->next = head_to;
+	head_to->next = head_from_first;
+	head_to->next->prev = head_to;
+}
+
+/**
+ * list_move() - Move a list node to the beginning of the list
+ * @node: pointer to the node
+ * @head: pointer to the head of the list
+ *
+ * The @node is removed from its old position/node and add to the beginning of
+ * @head
+ */
+static __inline__ void list_move(struct list_head *node, struct list_head *head)
+{
+	list_del(node);
+	list_add(node, head);
+}
+
+/**
+ * list_move_tail() - Move a list node to the end of the list
+ * @node: pointer to the node
+ * @head: pointer to the head of the list
+ *
+ * The @node is removed from its old position/node and add to the end of @head
+ */
+static __inline__ void list_move_tail(struct list_head *node,
+				      struct list_head *head)
+{
+	list_del(node);
+	list_add_tail(node, head);
+}
+
+/**
+ * list_entry() - Calculate address of entry that contains list node
+ * @node: pointer to list node
+ * @type: type of the entry containing the list node
+ * @member: name of the list_head member variable in struct @type
+ *
+ * Return: @type pointer of entry containing node
+ */
+#define list_entry(node, type, member) container_of(node, type, member)
+
+/**
+ * list_first_entry() - get first entry of the list
+ * @head: pointer to the head of the list
+ * @type: type of the entry containing the list node
+ * @member: name of the list_head member variable in struct @type
+ *
+ * Return: @type pointer of first entry in list
+ */
+#define list_first_entry(head, type, member) \
+	list_entry((head)->next, type, member)
+
+/**
+ * list_last_entry() - get last entry of the list
+ * @head: pointer to the head of the list
+ * @type: type of the entry containing the list node
+ * @member: name of the list_head member variable in struct @type
+ *
+ * Return: @type pointer of last entry in list
+ */
+#define list_last_entry(head, type, member) \
+	list_entry((head)->prev, type, member)
+
+/**
+ * list_for_each - iterate over list nodes
+ * @node: list_head pointer used as iterator
+ * @head: pointer to the head of the list
+ *
+ * The nodes and the head of the list must must be kept unmodified while
+ * iterating through it. Any modifications to the the list will cause undefined
+ * behavior.
+ */
+#define list_for_each(node, head) \
+	for (node = (head)->next; \
+	     node != (head); \
+	     node = node->next)
+
+/**
+ * list_for_each_entry_t - iterate over list entries
+ * @entry: @type pointer used as iterator
+ * @head: pointer to the head of the list
+ * @type: type of the entries containing the list nodes
+ * @member: name of the list_head member variable in struct @type
+ *
+ * The nodes and the head of the list must must be kept unmodified while
+ * iterating through it. Any modifications to the the list will cause undefined
+ * behavior.
+ *
+ * WARNING this functionality is not available in the Linux list implementation
+ */
+#define list_for_each_entry_t(entry, head, type, member) \
+	for (entry = list_entry((head)->next, type, member); \
+	     &entry->member != (head); \
+	     entry = list_entry(entry->member.next, type, member))
+
+/**
+ * list_for_each_entry - iterate over list entries
+ * @entry: pointer used as iterator
+ * @head: pointer to the head of the list
+ * @member: name of the list_head member variable in struct type of @entry
+ *
+ * The nodes and the head of the list must must be kept unmodified while
+ * iterating through it. Any modifications to the the list will cause undefined
+ * behavior.
+ */
+#define list_for_each_entry(entry, head, member) \
+	list_for_each_entry_t(entry, head, __typeof__(*entry), member)
+
+/**
+ * list_for_each_safe - iterate over list nodes and allow deletes
+ * @node: list_head pointer used as iterator
+ * @safe: list_head pointer used to store info for next entry in list
+ * @head: pointer to the head of the list
+ *
+ * The current node (iterator) is allowed to be removed from the list. Any
+ * other modifications to the the list will cause undefined behavior.
+ */
+#define list_for_each_safe(node, safe, head) \
+	for (node = (head)->next, safe = node->next; \
+	     node != (head); \
+	     node = safe, safe = node->next)
+
+/**
+ * list_for_each_entry_safe_t - iterate over list entries and allow deletes
+ * @entry: @type pointer used as iterator
+ * @safe: @type pointer used to store info for next entry in list
+ * @head: pointer to the head of the list
+ * @type: type of the entries containing the list nodes
+ * @member: name of the list_head member variable in struct @type
+ *
+ * The current node (iterator) is allowed to be removed from the list. Any
+ * other modifications to the the list will cause undefined behavior.
+ *
+ * WARNING this functionality is not available in the Linux list implementation
+ */
+#define list_for_each_entry_safe_t(entry, safe, head, type, member) \
+	for (entry = list_entry((head)->next, type, member), \
+	     safe = list_entry(entry->member.next, type, member); \
+	     &entry->member != (head); \
+	     entry = safe, \
+	     safe = list_entry(safe->member.next, type, member))
+
+/**
+ * list_for_each_entry_safe - iterate over list entries and allow deletes
+ * @entry: pointer used as iterator
+ * @safe: @type pointer used to store info for next entry in list
+ * @head: pointer to the head of the list
+ * @member: name of the list_head member variable in struct type of @entry
+ *
+ * The current node (iterator) is allowed to be removed from the list. Any
+ * other modifications to the the list will cause undefined behavior.
+ */
+#define list_for_each_entry_safe(entry, safe, head, member) \
+	list_for_each_entry_safe_t(entry, safe, head, __typeof__(*entry), \
+				   member)
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/spinlock.h b/riscv/riscv-probe/libfemto/include/spinlock.h
new file mode 100644
index 0000000..c11200f
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/spinlock.h
@@ -0,0 +1,6 @@
+// See LICENSE for license details.
+#pragma once
+
+#ifdef __riscv
+#include <arch/riscv/spinlock.h>
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/stdarg.h b/riscv/riscv-probe/libfemto/include/stdarg.h
new file mode 100644
index 0000000..7383c4a
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/stdarg.h
@@ -0,0 +1,8 @@
+#pragma once
+
+typedef __builtin_va_list va_list;
+
+#define va_start(v,l)   __builtin_va_start(v,l)
+#define va_end(v)       __builtin_va_end(v)
+#define va_arg(v,l)     __builtin_va_arg(v,l)
+#define va_copy(d,s)    __builtin_va_copy(d,s)
diff --git a/riscv/riscv-probe/libfemto/include/stdatomic.h b/riscv/riscv-probe/libfemto/include/stdatomic.h
new file mode 100644
index 0000000..8073868
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/stdatomic.h
@@ -0,0 +1,61 @@
+// See LICENSE for license details.
+#pragma once
+
+typedef enum memory_order {
+    memory_order_relaxed = __ATOMIC_RELAXED,
+    memory_order_acquire = __ATOMIC_ACQUIRE,
+    memory_order_consume = __ATOMIC_CONSUME,
+    memory_order_release = __ATOMIC_RELEASE,
+    memory_order_acq_rel = __ATOMIC_ACQ_REL,
+    memory_order_seq_cst = __ATOMIC_SEQ_CST
+} memory_order;
+
+#ifdef __cplusplus
+typedef _Atomic(bool)               atomic_bool;
+#else
+typedef _Atomic(_Bool)              atomic_bool;
+#endif
+typedef _Atomic(char)               atomic_char;
+typedef _Atomic(signed char)        atomic_schar;
+typedef _Atomic(unsigned char)      atomic_uchar;
+typedef _Atomic(short)              atomic_short;
+typedef _Atomic(unsigned short)     atomic_ushort;
+typedef _Atomic(int)                atomic_int;
+typedef _Atomic(unsigned int)       atomic_uint;
+typedef _Atomic(long)               atomic_long;
+typedef _Atomic(unsigned long)      atomic_ulong;
+typedef _Atomic(long long)          atomic_llong;
+typedef _Atomic(unsigned long long) atomic_ullong;
+typedef _Atomic(uint_least16_t)     atomic_char16_t;
+typedef _Atomic(uint_least32_t)     atomic_char32_t;
+typedef _Atomic(wchar_t)            atomic_wchar_t;
+typedef _Atomic(int_least8_t)       atomic_int_least8_t;
+typedef _Atomic(uint_least8_t)      atomic_uint_least8_t;
+typedef _Atomic(int_least16_t)      atomic_int_least16_t;
+typedef _Atomic(uint_least16_t)     atomic_uint_least16_t;
+typedef _Atomic(int_least32_t)      atomic_int_least32_t;
+typedef _Atomic(uint_least32_t)     atomic_uint_least32_t;
+typedef _Atomic(int_least64_t)      atomic_int_least64_t;
+typedef _Atomic(uint_least64_t)     atomic_uint_least64_t;
+typedef _Atomic(int_fast8_t)        atomic_int_fast8_t;
+typedef _Atomic(uint_fast8_t)       atomic_uint_fast8_t;
+typedef _Atomic(int_fast16_t)       atomic_int_fast16_t;
+typedef _Atomic(uint_fast16_t)      atomic_uint_fast16_t;
+typedef _Atomic(int_fast32_t)       atomic_int_fast32_t;
+typedef _Atomic(uint_fast32_t)      atomic_uint_fast32_t;
+typedef _Atomic(int_fast64_t)       atomic_int_fast64_t;
+typedef _Atomic(uint_fast64_t)      atomic_uint_fast64_t;
+typedef _Atomic(intptr_t)           atomic_intptr_t;
+typedef _Atomic(uintptr_t)          atomic_uintptr_t;
+typedef _Atomic(size_t)             atomic_size_t;
+typedef _Atomic(ptrdiff_t)          atomic_ptrdiff_t;
+typedef _Atomic(intmax_t)           atomic_intmax_t;
+typedef _Atomic(uintmax_t)          atomic_uintmax_t;
+
+typedef struct atomic_flag { atomic_bool _Value; } atomic_flag;
+ 
+#define ATOMIC_FLAG_INIT { 0 }
+
+#ifdef __riscv
+#include <arch/riscv/stdatomic.h>
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/stdbits.h b/riscv/riscv-probe/libfemto/include/stdbits.h
new file mode 100644
index 0000000..2985381
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/stdbits.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#define clz(val) ({                         \
+    int result;                             \
+    switch(sizeof(val)) {                   \
+    case 1: result = clz8(val); break;      \
+    case 2: result = clz16(val); break;     \
+    case 4: result = clz32(val); break;     \
+    case 8: result = clz64(val); break;     \
+    }                                       \
+    result;                                 \
+})
+
+#define ctz(val) ({                         \
+    int result;                             \
+    switch(sizeof(val)) {                   \
+    case 1: result = ctz8(val); break;      \
+    case 2: result = ctz16(val); break;     \
+    case 4: result = ctz32(val); break;     \
+    case 8: result = ctz64(val); break;     \
+    }                                       \
+    result;                                 \
+})
+
+int clz8(int8_t val);
+int clz16(int16_t val);
+int clz32(int32_t val);
+int clz64(int64_t val);
+
+int ctz8(int8_t val);
+int ctz16(int16_t val);
+int ctz32(int32_t val);
+int ctz64(int64_t val);
+
+static inline int ispow2(uintptr_t val)
+{
+    return val && !(val & (val-1));
+}
+
+static inline uintptr_t roundpow2(uintptr_t val)
+{
+    val--;
+    val |= val >> 1;
+    val |= val >> 2;
+    val |= val >> 4;
+    val |= val >> 8;
+    val |= val >> 16;
+#if __SIZE_WIDTH__ == 64
+    val |= val >> 32;
+#endif
+    val++;
+    return val;
+}
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/stddef.h b/riscv/riscv-probe/libfemto/include/stddef.h
new file mode 100644
index 0000000..3799f2a
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/stddef.h
@@ -0,0 +1,8 @@
+#pragma once
+
+typedef __PTRDIFF_TYPE__ ptrdiff_t;
+typedef __SIZE_TYPE__ size_t;
+typedef __WCHAR_TYPE__ wchar_t;
+
+#define NULL 0L
+#define offsetof(type, member) __builtin_offsetof(type, member)
diff --git a/riscv/riscv-probe/libfemto/include/stdint.h b/riscv/riscv-probe/libfemto/include/stdint.h
new file mode 100644
index 0000000..bef5c2f
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/stdint.h
@@ -0,0 +1,52 @@
+#pragma once
+
+typedef __INT8_TYPE__ int8_t;
+typedef __INT16_TYPE__ int16_t;
+typedef __INT32_TYPE__ int32_t;
+typedef __INT64_TYPE__ int64_t;
+
+typedef __INT_FAST8_TYPE__ int_fast8_t;
+typedef __INT_FAST16_TYPE__ int_fast16_t;
+typedef __INT_FAST32_TYPE__ int_fast32_t;
+typedef __INT_FAST64_TYPE__ int_fast64_t;
+
+typedef __INT_LEAST16_TYPE__ int_least16_t;
+typedef __INT_LEAST32_TYPE__ int_least32_t;
+typedef __INT_LEAST64_TYPE__ int_least64_t;
+typedef __INT_LEAST8_TYPE__ int_least8_t;
+
+typedef __INTMAX_TYPE__ intmax_t;
+typedef __INTPTR_TYPE__ intptr_t;
+
+typedef __UINT8_TYPE__ uint8_t;
+typedef __UINT16_TYPE__ uint16_t;
+typedef __UINT32_TYPE__ uint32_t;
+typedef __UINT64_TYPE__ uint64_t;
+
+typedef __UINT_FAST8_TYPE__ uint_fast8_t;
+typedef __UINT_FAST16_TYPE__ uint_fast16_t;
+typedef __UINT_FAST32_TYPE__ uint_fast32_t;
+typedef __UINT_FAST64_TYPE__ uint_fast64_t;
+
+typedef __UINT_LEAST8_TYPE__ uint_least8_t;
+typedef __UINT_LEAST16_TYPE__ uint_least16_t;
+typedef __UINT_LEAST32_TYPE__ uint_least32_t;
+typedef __UINT_LEAST64_TYPE__ uint_least64_t;
+
+typedef __UINTMAX_TYPE__ uintmax_t;
+typedef __UINTPTR_TYPE__ uintptr_t;
+
+#define INT8_MAX __INT8_MAX__
+#define INT16_MAX __INT16_MAX__
+#define INT32_MAX __INT32_MAX__
+#define INT64_MAX __INT64_MAX__
+
+#define UINT8_MAX  __UINT8_MAX__
+#define UINT16_MAX __UINT16_MAX__
+#define UINT32_MAX __UINT32_MAX__
+#define UINT64_MAX __UINT64_MAX__
+
+#define INTPTR_MAX __INTPTR_MAX__
+#define UINTPTR_MAX __UINTPTR_MAX__
+#define PTRDIFF_MAX __PTRDIFF_MAX__
+#define SIZE_MAX __SIZE_MAX__
diff --git a/riscv/riscv-probe/libfemto/include/stdio.h b/riscv/riscv-probe/libfemto/include/stdio.h
new file mode 100644
index 0000000..f97e6e1
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/stdio.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdarg.h>
+#include <stddef.h>
+
+int getchar(void);
+int printf(const char *, ...);
+int putchar(int);
+int puts(const char *);
+int snprintf(char *, size_t, const char *, ...);
+int vprintf(const char *, va_list);
+int vsnprintf(char *, size_t, const char *, va_list);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/stdlib.h b/riscv/riscv-probe/libfemto/include/stdlib.h
new file mode 100644
index 0000000..7552a1c
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/stdlib.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stddef.h>
+
+__attribute__((noreturn)) void abort(void);
+__attribute__((noreturn)) void exit(int status);
+void* malloc(size_t size);
+void free(void* ptr);
+void _malloc_addblock(void* addr, size_t size);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/include/string.h b/riscv/riscv-probe/libfemto/include/string.h
new file mode 100644
index 0000000..6572d23
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/include/string.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stddef.h>
+
+void *memchr(const void *s, int c, size_t n);
+int memcmp(const void *, const void *, size_t);
+void *memcpy(void *, const void *, size_t);
+void *memset(void *, int, size_t);
+char *strchr(const char *s, int c);
+int strcmp(const char *, const char *);
+size_t strlen(const char *);
+int strncmp(const char *, const char *, size_t);
+char *strncpy(char *, const char *, size_t);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/riscv/riscv-probe/libfemto/std/abort.c b/riscv/riscv-probe/libfemto/std/abort.c
new file mode 100644
index 0000000..d4a7f06
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/abort.c
@@ -0,0 +1,8 @@
+#include <stdio.h>
+#include <stdlib.h>
+
+void abort(void)
+{
+	puts("abort");
+	exit(-1);
+}
diff --git a/riscv/riscv-probe/libfemto/std/clz.c b/riscv/riscv-probe/libfemto/std/clz.c
new file mode 100644
index 0000000..bc94795
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/clz.c
@@ -0,0 +1,22 @@
+// See LICENSE for license details.
+
+#include <stdint.h>
+
+#define GLUE_HELPER(x, y) x##y
+#define GLUE(x, y) GLUE_HELPER(x, y)
+
+#define DEFINE_CLZ(T,bits)         \
+int GLUE(clz,bits)(T val)          \
+{                                  \
+    int n = 0;                     \
+    for (n = 0; n < bits; n++) {   \
+        if (val < 0) break;        \
+        val <<= 1;                 \
+    }                              \
+    return n;                      \
+}
+
+DEFINE_CLZ(int8_t,8)
+DEFINE_CLZ(int16_t,16)
+DEFINE_CLZ(int32_t,32)
+DEFINE_CLZ(int64_t,64)
diff --git a/riscv/riscv-probe/libfemto/std/ctz.c b/riscv/riscv-probe/libfemto/std/ctz.c
new file mode 100644
index 0000000..098687c
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/ctz.c
@@ -0,0 +1,22 @@
+// See LICENSE for license details.
+
+#include <stdint.h>
+
+#define GLUE_HELPER(x, y) x##y
+#define GLUE(x, y) GLUE_HELPER(x, y)
+
+#define DEFINE_CTZ(T,bits)         \
+int GLUE(ctz,bits)(T val)          \
+{                                  \
+    int n = 0;                     \
+    for (n = 0; n < bits; n++) {   \
+        if (val & 1) break;        \
+        val >>= 1;                 \
+    }                              \
+    return n;                      \
+}
+
+DEFINE_CTZ(int8_t,8)
+DEFINE_CTZ(int16_t,16)
+DEFINE_CTZ(int32_t,32)
+DEFINE_CTZ(int64_t,64)
diff --git a/riscv/riscv-probe/libfemto/std/exit.c b/riscv/riscv-probe/libfemto/std/exit.c
new file mode 100644
index 0000000..2489ef5
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/exit.c
@@ -0,0 +1,12 @@
+// See LICENSE for license details.
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <device.h>
+
+void exit(int status)
+{
+    poweroff_dev->poweroff(status);
+    asm volatile("1: j 1b");
+    __builtin_unreachable();
+}
diff --git a/riscv/riscv-probe/libfemto/std/getchar.c b/riscv/riscv-probe/libfemto/std/getchar.c
new file mode 100644
index 0000000..35babff
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/getchar.c
@@ -0,0 +1,9 @@
+// See LICENSE for license details.
+
+#include <stdio.h>
+#include <device.h>
+
+int getchar()
+{
+    return console_dev->getchar();
+}
diff --git a/riscv/riscv-probe/libfemto/std/malloc.c b/riscv/riscv-probe/libfemto/std/malloc.c
new file mode 100644
index 0000000..3df26f3
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/malloc.c
@@ -0,0 +1,146 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2017 Embedded Artistry
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <stddef.h>
+#include <stdint.h>
+
+#include "list.h"
+
+/* align to nearest power of two */
+#define ALIGN_SIZE(sz, align) (((sz) + ((align)-1)) & ~((align)-1))
+
+/* free list node*/
+typedef struct alloc_node
+{
+	struct list_head node;
+	size_t size;
+	char* block;
+} alloc_node_t;
+
+/* allocation metadata size */
+#define ALLOC_HEADER_SZ __builtin_offsetof(alloc_node_t, block)
+
+/* minimum allocation size of 32 bytes */
+#define MIN_ALLOC_SZ ALLOC_HEADER_SZ + 32
+
+/* free list */
+static LIST_HEAD(free_list);
+
+static void coalesce_free_list(void)
+{
+	alloc_node_t *b, *lb = NULL, *t;
+
+	list_for_each_entry_safe(b, t, &free_list, node)
+	{
+		if (lb)
+		{
+			/* coalesce adjacent blocks */
+			if ((((uintptr_t)&lb->block) + lb->size) == (uintptr_t)b)
+			{
+				lb->size += sizeof(*b) + b->size;
+				list_del(&b->node);
+				continue;
+			}
+		}
+		lb = b;
+	}
+}
+
+void* malloc(size_t size)
+{
+	void* ptr = NULL;
+	alloc_node_t* blk = NULL;
+
+	if (size > 0)
+	{
+		/* Align the pointer */
+		size = ALIGN_SIZE(size, sizeof(void*));
+
+		/* try to find a big enough block */
+		list_for_each_entry(blk, &free_list, node)
+		{
+			if (blk->size >= size)
+			{
+				ptr = &blk->block;
+				break;
+			}
+		}
+
+		if (ptr)
+		{
+			/* split block if possible */
+			if ((blk->size - size) >= MIN_ALLOC_SZ)
+			{
+				alloc_node_t* new_blk;
+				new_blk = (alloc_node_t*)((uintptr_t)(&blk->block) + size);
+				new_blk->size = blk->size - size - ALLOC_HEADER_SZ;
+				blk->size = size;
+				list_add(&new_blk->node, &blk->node);
+			}
+
+			list_del(&blk->node);
+		}
+
+	}
+
+	return ptr;
+}
+
+void free(void* ptr)
+{
+	alloc_node_t *blk, *free_blk;
+
+	if (ptr)
+	{
+		blk = container_of(ptr, alloc_node_t, block);
+
+		/* add block to free list in ascending order by pointer */
+		list_for_each_entry(free_blk, &free_list, node)
+		{
+			if (free_blk > blk)
+			{
+				list_add_tail(&blk->node, &free_blk->node);
+				goto blockadded;
+			}
+		}
+		list_add_tail(&blk->node, &free_list);
+
+	blockadded:
+		coalesce_free_list();
+	}
+}
+
+void _malloc_addblock(void* addr, size_t size)
+{
+	alloc_node_t* blk;
+
+	/* pointer align the block */
+	blk = (alloc_node_t*)ALIGN_SIZE((uintptr_t)addr, sizeof(void*));
+
+	/* calculate usable size */
+	blk->size = (uintptr_t)addr + size - (uintptr_t)blk - ALLOC_HEADER_SZ;
+
+	/* add the block to the free list */
+	list_add(&blk->node, &free_list);
+}
diff --git a/riscv/riscv-probe/libfemto/std/memchr.c b/riscv/riscv-probe/libfemto/std/memchr.c
new file mode 100644
index 0000000..b8287ff
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/memchr.c
@@ -0,0 +1,11 @@
+#include <string.h>
+
+void *memchr(const void *s, int c, size_t n)
+{
+    unsigned char *p = (unsigned char *)s;
+    while (n-- > 0) {
+       if (*p == c) return p;
+       p++;
+    }
+    return NULL;
+}
diff --git a/riscv/riscv-probe/libfemto/std/memcmp.c b/riscv/riscv-probe/libfemto/std/memcmp.c
new file mode 100644
index 0000000..73932c9
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/memcmp.c
@@ -0,0 +1,15 @@
+// See LICENSE for license details.
+
+#include <stddef.h>
+
+int memcmp(const void *s1, const void *s2, size_t n)
+{
+    while (n-- > 0) {
+        char c1 = *(const char*)s1++;
+        char c2 = *(const char*)s2++;
+        if (c1 != c2) {
+            return c1 - c2;
+        }
+    }
+    return 0;
+}
diff --git a/riscv/riscv-probe/libfemto/std/memcpy.c b/riscv/riscv-probe/libfemto/std/memcpy.c
new file mode 100644
index 0000000..69209e7
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/memcpy.c
@@ -0,0 +1,12 @@
+// See LICENSE for license details.
+
+#include <stddef.h>
+
+void *memcpy(void *dest, const void *src, size_t n)
+{
+    char *p = dest;
+    while (n-- > 0) {
+        *(char*)dest++ = *(char*)src++;
+    }
+    return p;
+}
diff --git a/riscv/riscv-probe/libfemto/std/memset.c b/riscv/riscv-probe/libfemto/std/memset.c
new file mode 100644
index 0000000..82b1545
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/memset.c
@@ -0,0 +1,12 @@
+// See LICENSE for license details.
+
+#include <stddef.h>
+
+void *memset(void *dest, int c, size_t n)
+{
+    char *p = dest;
+    while (n-- > 0) {
+        *(char*)dest++ = c;
+    }
+    return p;
+}
diff --git a/riscv/riscv-probe/libfemto/std/printf.c b/riscv/riscv-probe/libfemto/std/printf.c
new file mode 100644
index 0000000..5d81212
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/printf.c
@@ -0,0 +1,15 @@
+// See LICENSE for license details.
+
+#include <stdio.h>
+#include <stddef.h>
+#include <stdarg.h>
+
+int printf(const char* s, ...)
+{
+    int res = 0;
+    va_list vl;
+    va_start(vl, s);
+    res = vprintf(s, vl);
+    va_end(vl);
+    return res;
+}
diff --git a/riscv/riscv-probe/libfemto/std/putchar.c b/riscv/riscv-probe/libfemto/std/putchar.c
new file mode 100644
index 0000000..3066a65
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/putchar.c
@@ -0,0 +1,9 @@
+// See LICENSE for license details.
+
+#include <stdio.h>
+#include <device.h>
+
+int putchar(int ch)
+{
+    return console_dev->putchar(ch);
+}
diff --git a/riscv/riscv-probe/libfemto/std/puts.c b/riscv/riscv-probe/libfemto/std/puts.c
new file mode 100644
index 0000000..2dd07f7
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/puts.c
@@ -0,0 +1,9 @@
+// See LICENSE for license details.
+
+#include <stdio.h>
+
+int puts(const char *s)
+{
+    while (*s) putchar(*s++);
+    putchar('\n');
+}
diff --git a/riscv/riscv-probe/libfemto/std/snprintf.c b/riscv/riscv-probe/libfemto/std/snprintf.c
new file mode 100644
index 0000000..0228195
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/snprintf.c
@@ -0,0 +1,14 @@
+// See LICENSE for license details.
+
+#include <stdio.h>
+#include <stddef.h>
+#include <stdarg.h>
+
+int snprintf(char* out, size_t n, const char* s, ...)
+{
+    va_list vl;
+    va_start(vl, s);
+    int res = vsnprintf(out, n, s, vl);
+    va_end(vl);
+    return res;
+}
diff --git a/riscv/riscv-probe/libfemto/std/strchr.c b/riscv/riscv-probe/libfemto/std/strchr.c
new file mode 100644
index 0000000..7eef221
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/strchr.c
@@ -0,0 +1,11 @@
+#include <string.h>
+
+char *strchr(const char *s, int c)
+{
+    char *p = (char *)s;
+    while (*p) {
+       if (*p == c) return p;
+       p++;
+    }
+    return NULL;
+}
diff --git a/riscv/riscv-probe/libfemto/std/strcmp.c b/riscv/riscv-probe/libfemto/std/strcmp.c
new file mode 100644
index 0000000..73d9d8a
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/strcmp.c
@@ -0,0 +1,9 @@
+// See LICENSE for license details.
+
+#include <stddef.h>
+
+int strcmp(const char *s1, const char *s2)
+{
+    while (*s1 && *s2 && *s1 == *s2) { s1++; s2++; }
+    return *s1 - *s2;
+}
diff --git a/riscv/riscv-probe/libfemto/std/strlen.c b/riscv/riscv-probe/libfemto/std/strlen.c
new file mode 100644
index 0000000..74de6c4
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/strlen.c
@@ -0,0 +1,12 @@
+// See LICENSE for license details.
+
+#include <stddef.h>
+
+size_t strlen(const char* s)
+{
+    const char *p = s;
+    for (;; s++) {
+        if (!*s) break;
+    }
+    return s - p;
+}
diff --git a/riscv/riscv-probe/libfemto/std/strncmp.c b/riscv/riscv-probe/libfemto/std/strncmp.c
new file mode 100644
index 0000000..e1b476c
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/strncmp.c
@@ -0,0 +1,9 @@
+// See LICENSE for license details.
+
+#include <stddef.h>
+
+int strncmp(const char *s1, const char *s2, size_t n)
+{
+    while (n-- > 0 && *s1 && *s2 && *s1 == *s2) { s1++; s2++; }
+    return *s1 - *s2;
+}
diff --git a/riscv/riscv-probe/libfemto/std/strncpy.c b/riscv/riscv-probe/libfemto/std/strncpy.c
new file mode 100644
index 0000000..ccb8fc3
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/strncpy.c
@@ -0,0 +1,15 @@
+// See LICENSE for license details.
+
+#include <stddef.h>
+
+char *strncpy(char *dst, const char *src, size_t n)
+{
+    size_t i;
+    for (i = 0; i < n && src[i]; i++) {
+        dst[i] = src[i];
+    }
+    for (; i < n; i++) {
+        dst[i] = 0;
+    }
+    return dst;
+}
diff --git a/riscv/riscv-probe/libfemto/std/vprintf.c b/riscv/riscv-probe/libfemto/std/vprintf.c
new file mode 100644
index 0000000..36c99a3
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/vprintf.c
@@ -0,0 +1,14 @@
+// See LICENSE for license details.
+
+#include <stdio.h>
+#include <alloca.h>
+
+int vprintf(const char* s, va_list vl)
+{
+    char *out;
+    int res = vsnprintf(NULL, -1, s, vl);
+    out = alloca(res + 1);
+    vsnprintf(out, res + 1, s, vl);
+    while (*out) putchar(*out++);
+    return res;
+}
diff --git a/riscv/riscv-probe/libfemto/std/vsnprintf.c b/riscv/riscv-probe/libfemto/std/vsnprintf.c
new file mode 100644
index 0000000..32dcf3f
--- /dev/null
+++ b/riscv/riscv-probe/libfemto/std/vsnprintf.c
@@ -0,0 +1,109 @@
+// See LICENSE for license details.
+
+#include <stddef.h>
+#include <stdarg.h>
+
+int vsnprintf(char * out, size_t n, const char* s, va_list vl)
+{
+    int format = 0;
+    int longarg = 0;
+    size_t pos = 0;
+    for( ; *s; s++) {
+        if (format) {
+            switch(*s) {
+            case 'l': {
+                longarg = 1;
+                break;
+            }
+            case 'p': {
+                longarg = 1;
+                if (out && pos < n) {
+                    out[pos] = '0';
+                }
+                pos++;
+                if (out && pos < n) {
+                    out[pos] = 'x';
+                }
+                pos++;
+            }
+            case 'x': {
+                long num = longarg ? va_arg(vl, long) : va_arg(vl, int);
+                int hexdigits = 2*(longarg ? sizeof(long) : sizeof(int))-1;
+                for(int i = hexdigits; i >= 0; i--) {
+                    int d = (num >> (4*i)) & 0xF;
+                    if (out && pos < n) {
+                        out[pos] = (d < 10 ? '0'+d : 'a'+d-10);
+                    }
+                    pos++;
+                }
+                longarg = 0;
+                format = 0;
+                break;
+            }
+            case 'd': {
+                long num = longarg ? va_arg(vl, long) : va_arg(vl, int);
+                if (num < 0) {
+                    num = -num;
+                    if (out && pos < n) {
+                        out[pos] = '-';
+                    }
+                    pos++;
+                }
+                long digits = 1;
+                for (long nn = num; nn /= 10; digits++)
+                    ;
+                for (int i = digits-1; i >= 0; i--) {
+                    if (out && pos + i < n) {
+                        out[pos + i] = '0' + (num % 10);
+                    }
+                    num /= 10;
+                }
+                pos += digits;
+                longarg = 0;
+                format = 0;
+                break;
+            }
+            case 's': {
+                const char* s2 = va_arg(vl, const char*);
+                while (*s2) {
+                    if (out && pos < n) {
+                        out[pos] = *s2;
+                    }
+                    pos++;
+                    s2++;
+                }
+                longarg = 0;
+                format = 0;
+                break;
+            }
+            case 'c': {
+                if (out && pos < n) {
+                    out[pos] = (char)va_arg(vl,int);
+                }
+                pos++;
+                longarg = 0;
+                format = 0;
+                break;
+            }
+            default:
+                break;
+            }
+        }
+        else if(*s == '%') {
+          format = 1;
+        }
+        else {
+          if (out && pos < n) {
+            out[pos] = *s;
+          }
+          pos++;
+        }
+    }
+    if (out && pos < n) {
+        out[pos] = 0;
+    }
+    else if (out && n) {
+        out[n-1] = 0;
+    }
+    return pos;
+}