diff --git a/.gitignore b/.gitignore index ada0a32..5de0eeb 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,4 @@ build/ firmware/ app/ root/ +.venv/ diff --git a/Makefile b/Makefile index 56bf09f..cf688c4 100644 --- a/Makefile +++ b/Makefile @@ -3,13 +3,14 @@ MAKEFLAGS += --silent SHELL := /usr/bin/env bash -OUTPUT ?= $(shell pwd)/root .PHONY: all prepare openocd gdb flash monitor clean-all clean build clang-format-check clang-tidy-check all: build prepare: + python -m venv .venv + .venv/bin/pip install esptool git submodule update --init build: diff --git a/README.md b/README.md index 8e46044..ad765e9 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,8 @@ # BadgerOS -BadgerOS is the operating system currently in development for the upcoming MCH2025(1) badge. +BadgerOS is the operating system currently in development for the upcoming [WHY2025](https://why2025.org/) badge. The goal is the allow future badge users to get both the performance that native apps can afford as well as the portability made possible by this OS. -_(1) MCH2025 is a preliminary name, MCH2025 is an event that will be organised by IFCAT in 2025._ - ## Index - [Contributing](#contributing) - [Prerequisites](#prerequisites) @@ -28,40 +26,34 @@ After that, see [Project structure](./docs/project_structure.md) for reference a # Prerequisites - -To build BadgerOS: - +- `git` - `build-essential` - `cmake` -- `riscv32-unknown-elf-gcc` ([RISC-V collab toolchain](https://github.com/riscv-collab/riscv-gnu-toolchain)) +- `gcc-riscv64-linux-gnu` - `python3` - -To flash to an ESP: - -- `esptool.py` from [ESP-IDF](https://docs.espressif.com/projects/esp-idf/en/latest/esp32/get-started/#installation) - -Optional recommends: - - `picocom` # Build system - The build system is based on Makefiles and CMake. The following commands can be used to perform relevant actions: +To select target chip, choose one of: +- `export BADGEROS_PORT=esp32p4` +- `export BADGEROS_PORT=esp32c6` (default) + To build: `make build` To remove build files: `make clean` -To flash to an ESP: `source /export.sh` (once) `make flash` (every time you flash) +To flash to an ESP: `make flash` To open picocom: `make monitor` To build, flash and open picocom: `make` or `make all` -To check cody style: `make clang-format-check` (code formatting) and `make clang-tidy-check` (programming guidelines) +To check code style: `make clang-format-check` (code formatting) and `make clang-tidy-check` (programming guidelines) Build artifacts will be put into the `kernel/firmware` folder once the project was successfully built. diff --git a/files/init/src/main.c b/files/init/src/main.c index b128475..2ebcdb1 100644 --- a/files/init/src/main.c +++ b/files/init/src/main.c @@ -32,6 +32,7 @@ int main(int argc, char **argv) { else print("No read :c\n"); int volatile *mem = syscall_mem_alloc(0, 32, 0, MEMFLAGS_RW); + *mem = 3; syscall_mem_dealloc(mem); return 0; } diff --git a/kernel/CMakeLists.txt b/kernel/CMakeLists.txt index c6844fd..b87a61b 100644 --- a/kernel/CMakeLists.txt +++ b/kernel/CMakeLists.txt @@ -19,7 +19,7 @@ endif() # Set the C compiler if(NOT DEFINED CMAKE_C_COMPILER) - find_program(CMAKE_C_COMPILER NAMES riscv32-unknown-linux-gnu-gcc riscv32-linux-gnu-gcc riscv64-unknown-linux-gnu-gcc riscv64-linux-gnu-gcc REQUIRED) + find_program(CMAKE_C_COMPILER NAMES riscv32-badgeros-linux-gnu-gcc riscv32-unknown-linux-gnu-gcc riscv32-linux-gnu-gcc riscv64-unknown-linux-gnu-gcc riscv64-linux-gnu-gcc REQUIRED) message("Detected RISC-V C compiler as '${CMAKE_C_COMPILER}'") else() message("Using compiler '${CMAKE_C_COMPILER}' from environment") @@ -27,8 +27,8 @@ endif() # Determine the compiler prefix get_filename_component(compiler_name "${CMAKE_C_COMPILER}" NAME) -string(REGEX MATCH "^([A-Za-z0-9_]+\-)*" BADGER_COMPILER_PREFIX "${compiler_name}") -find_program(BADGER_OBJCOPY NAMES "${BADGER_COMPILER_PREFIX}objcopy" REQUIRED) +string(REGEX MATCH "^([A-Za-z0-9_]+\-)*" BADGER_COMPILER_PREFIX "${compiler_name}") +find_program(BADGER_OBJCOPY NAMES "${BADGER_COMPILER_PREFIX}objcopy" REQUIRED) find_program(BADGER_OBJDUMP NAMES "${BADGER_COMPILER_PREFIX}objdump" REQUIRED) # Include port-specific. @@ -38,8 +38,10 @@ include(cpu/riscv/CMakeLists.txt) # LTO is disabled due to GCC bugs inserting calls to memcpy everywhere set(common_compiler_flags -ffreestanding # We do not compile against an OS. + -static -fno-pic # Disable PIC + -fno-exceptions -march=${target_arch} # Selects the target CPU. - -mabi=${target_abi} # Selects target ABI + -mabi=${target_abi} # Selects target ABI -nodefaultlibs -nostdlib # Do not link any default libraries like libgcc or libc. -O2 # Optimize the code. -ggdb # Generate debug information in default extended format. @@ -48,10 +50,12 @@ set(common_compiler_flags -std=gnu11 # We use the C11 standard -DBADGEROS_KERNEL # Tell the code we're building for the kernel -DBADGEROS_PORT_${BADGEROS_PORT} + -DBADGEROS_PORT="${BADGEROS_PORT}" -DBADGEROS_MALLOC_DEBUG_LEVEL=3 # Malloc debug level set to WARN -DSOFTBIT # Turn on our emulated bit operations -fno-omit-frame-pointer # Always use frame pointer -ffunction-sections + -fno-stack-protector -fno-exceptions ) if("${CMAKE_BUILD_TYPE}" MATCHES "Release") @@ -60,7 +64,7 @@ else() message("Building in debug mode") set(common_compiler_flags ${common_compiler_flags} -fsanitize=undefined # Adds sanitizer for undefined behaviour. - -fsanitize-undefined-trap-on-error # Invoke a trap instruction instead of calling into the UBsan runtime. + -fsanitize-undefined-trap-on-error ) endif() @@ -141,7 +145,7 @@ add_custom_command( # Proxy target to allow installation of the bin file add_custom_target(badger-os.bin.target ALL DEPENDS badger-os.bin) -# Also build a dump file that is put into the +# Also build a dump file that is put into the add_custom_target( badger-os.elf.disasm ALL diff --git a/kernel/cpu/riscv/include/cpu/interrupt.h b/kernel/cpu/riscv/include/cpu/interrupt.h index be7750f..897794d 100644 --- a/kernel/cpu/riscv/include/cpu/interrupt.h +++ b/kernel/cpu/riscv/include/cpu/interrupt.h @@ -7,25 +7,6 @@ #include -// Enable/disable an internal interrupt. -// Returns whether the interrupt was enabled. -static inline bool irq_ch_enable(int int_irq, bool enable) { - long mask = 1 << int_irq; - if (enable) { - asm volatile("csrrs %0, mie, %0" : "+r"(mask)); - } else { - asm volatile("csrrc %0, mie, %0" : "+r"(mask)); - } - return ((mask) >> int_irq) & 1; -} - -// Query whether an internal interrupt is enabled. -static inline bool irq_ch_enabled(int int_irq) { - long mask; - asm("csrr %0, mie" : "=r"(mask)); - return ((mask) >> int_irq) & 1; -} - // Globally enable/disable interrupts in this CPU. // Returns whether interrupts were enabled. static inline bool irq_enable(bool enable) { @@ -37,6 +18,7 @@ static inline bool irq_enable(bool enable) { } return (mask >> RISCV_STATUS_MIE_BIT) & 1; } + // Query whether interrupts are enabled in this CPU. static inline bool irq_enabled() { long mask; diff --git a/kernel/cpu/riscv/include/cpu/isr_ctx.h b/kernel/cpu/riscv/include/cpu/isr_ctx.h index 12a8125..a3523ab 100644 --- a/kernel/cpu/riscv/include/cpu/isr_ctx.h +++ b/kernel/cpu/riscv/include/cpu/isr_ctx.h @@ -64,6 +64,8 @@ STRUCT_FIELD_PTR(isr_ctx_t, sched_thread_t, thread, 164) // Thread is a kernel thread. // If true, the thread is run in M-mode, otherwise it is run in U-mode. STRUCT_FIELD_WORD(isr_ctx_t, is_kernel_thread, 168) +// Use SP as the stack, not the ISR stack. +STRUCT_FIELD_WORD(isr_ctx_t, use_sp, 172) STRUCT_END(isr_ctx_t) @@ -75,7 +77,7 @@ enum { STACK_ALIGNMENT = 16, }; -// Get the current kernel context. +// Get the current ISR context. static inline isr_ctx_t *isr_ctx_get() { isr_ctx_t *kctx; asm("csrr %0, mscratch" : "=r"(kctx)); @@ -93,6 +95,11 @@ static inline void isr_ctx_switch_set(isr_ctx_t *switch_to) { asm("csrr %0, mscratch" : "=r"(kctx)); kctx->ctxswitch = switch_to; } +// Immediately swap the ISR context handle. +static inline isr_ctx_t *isr_ctx_swap(isr_ctx_t *kctx) { + asm("csrrw %0, mscratch, %0" : "+r"(kctx)); + return kctx; +} // Print a register dump given isr_ctx_t. void isr_ctx_dump(isr_ctx_t const *ctx); // Print a register dump of the current registers. diff --git a/kernel/cpu/riscv/include/cpu/riscv_pmp.h b/kernel/cpu/riscv/include/cpu/riscv_pmp.h index 6afa282..7022ee2 100644 --- a/kernel/cpu/riscv/include/cpu/riscv_pmp.h +++ b/kernel/cpu/riscv/include/cpu/riscv_pmp.h @@ -13,6 +13,8 @@ #include #include +typedef struct proc_memmap_t proc_memmap_t; + static_assert( RISCV_PMP_REGION_COUNT == 16 || RISCV_PMP_REGION_COUNT == 64, "RISC-V PMP region count must be either 16 or 64." ); @@ -20,7 +22,7 @@ static_assert( #if __riscv_xlen == 32 // 2^XLEN-byte NAPOT range starting at address 0. -#define RISCV_PMPADDR_NAPOT_GLOBAL 0x1fffffff +#define RISCV_PMPADDR_NAPOT_GLOBAL __LONG_MAX__ #define __RISCV_PMP_CFG_BITPOS_0 0 #define __RISCV_PMP_CFG_BITPOS_1 8 @@ -354,15 +356,26 @@ void riscv_pmpaddr_write_all(size_t const addr_in[RISCV_PMP_REGION_COUNT]); // Add a memory protection region. // For kernels using PMP as memory protection. -bool riscv_pmp_memprotect(riscv_pmp_ctx_t *ctx, size_t paddr, size_t length, uint32_t flags); +bool riscv_pmp_memprotect(proc_memmap_t *new_mm, riscv_pmp_ctx_t *ctx, size_t paddr, size_t length, uint32_t flags); // Swap in the set of PMP entries. void riscv_pmp_memprotect_swap(riscv_pmp_ctx_t *ctx); +// Determine whether an address is a valid NAPOT address. +static inline bool riscv_pmpaddr_is_napot(size_t base, size_t size) CONST; +static inline bool riscv_pmpaddr_is_napot(size_t base, size_t size) { + // Size must be a power of 2 >= 8. + if (size < 8 || (size & (size - 1))) { + return false; + } + // Base must be aligned to size. + return (base & (size - 1)) == 0; +} + // Compute NAPOT address value. // Assumes `base` is aligned to `size` bytes and that `size` is a power of two >= 8. -static inline size_t riscv_pmpaddr_calc_napot(size_t base, size_t size) PURE; +static inline size_t riscv_pmpaddr_calc_napot(size_t base, size_t size) CONST; static inline size_t riscv_pmpaddr_calc_napot(size_t base, size_t size) { return (base >> 2) | ((size >> 3) - 1); } diff --git a/kernel/cpu/riscv/src/entrypoint.S b/kernel/cpu/riscv/src/entrypoint.S index f08bc63..88d2f21 100644 --- a/kernel/cpu/riscv/src/entrypoint.S +++ b/kernel/cpu/riscv/src/entrypoint.S @@ -23,6 +23,7 @@ .global __stack_bottom .global __stack_size .equ __stack_size, 8192 + # .equ __stack_size, 1500*16 .lcomm __stack_bottom, __stack_size .equ __stack_top, __stack_bottom + __stack_size @@ -37,33 +38,34 @@ _start: # Set up registers. .option push .option norelax - la gp, __global_pointer$ - mv tp, x0 - la sp, __stack_top + la gp, __global_pointer$ + mv tp, x0 + la sp, __stack_top .option pop # Zero out .bss section. - la a0, __start_bss - la a1, __stop_bss - beq a0, a1, .bssinit_skip + la a0, __start_bss + la a1, __stop_bss + beq a0, a1, .bssinit_skip .bssinit_loop: - sw x0, 0(a0) + sw x0, 0(a0) addi a0, a0, 4 - bne a0, a1, .bssinit_loop + bne a0, a1, .bssinit_loop .bssinit_skip: # Run init functions. - la s0, __start_init_array - la s1, __stop_init_array - beq s0, s1, .initfun_skip + li s0, 0 + la s1, __start_init_array + la s2, __stop_init_array + beq s1, s2, .initfun_skip .initfun_loop: - lw ra, 0(s0) + lw ra, 0(s1) jalr ra, ra - addi s0, s0, 4 - bne s0, s1, .initfun_loop + addi s1, s1, 4 + bne s1, s2, .initfun_loop .initfun_skip: # Jump to the C entrypoint. - jal basic_runtime_init + jal basic_runtime_init # This function isn't allowed to return. ebreak diff --git a/kernel/cpu/riscv/src/isr.S b/kernel/cpu/riscv/src/isr.S index 15f3bcd..7395b8d 100644 --- a/kernel/cpu/riscv/src/isr.S +++ b/kernel/cpu/riscv/src/isr.S @@ -3,6 +3,7 @@ #include "cpu/isr.h" #include "cpu/riscv.h" +#include "port/hardware.h" .global __global_pointer$ .global memprotect_swap_from_isr @@ -56,10 +57,13 @@ # Set up special regs. li tp, 0 .option push - .option norvc + .option norelax la gp, __global_pointer$ .option pop + lw t1, isr_ctx_t_use_sp(t0) + bnez t1, .isr_entry_\@ la sp, __interrupt_stack_hi +.isr_entry_\@: #pragma endregion .endm @@ -388,11 +392,11 @@ riscv_interrupt_vector_table: .option norvc # Trap handler. j __trap_asm - # Interrupt handler. - .rept 31 + # Interrupt handlers. + .rept RISCV_VT_INT_COUNT j __isr_asm .endr - # Padding to 256-byte size. - .skip 128 + # Padding. + .skip RISCV_VT_PADDING*4 .option pop #pragma endregion diff --git a/kernel/cpu/riscv/src/isr.c b/kernel/cpu/riscv/src/isr.c index 3d3d6c8..a756e34 100644 --- a/kernel/cpu/riscv/src/isr.c +++ b/kernel/cpu/riscv/src/isr.c @@ -7,6 +7,7 @@ #include "cpu/isr_ctx.h" #include "cpu/panic.h" #include "log.h" +#include "port/hardware.h" #include "process/internal.h" #include "process/types.h" #include "rawprint.h" @@ -53,14 +54,26 @@ void riscv_trap_handler() { // TODO: Per-CPU double trap detection. static int trap_depth = 0; - uint32_t mcause, mstatus, mtval, mepc; - asm volatile("csrr %0, mstatus" : "=r"(mstatus)); + long mcause, mstatus, mtval, mepc; asm volatile("csrr %0, mcause" : "=r"(mcause)); + if (mcause < 0) { + riscv_interrupt_handler(); + return; + } + asm volatile("csrr %0, mstatus" : "=r"(mstatus)); trap_depth++; - if ((mcause & 31) == RISCV_TRAP_U_ECALL) { + isr_ctx_t recurse_ctx; + recurse_ctx.mpu_ctx = NULL; + recurse_ctx.is_kernel_thread = true; + recurse_ctx.use_sp = true; + isr_ctx_t *kctx = isr_ctx_swap(&recurse_ctx); + recurse_ctx.thread = kctx->thread; + + if ((mcause & RISCV_VT_ICAUSE_MASK) == RISCV_TRAP_U_ECALL) { // ECALL from U-mode goes to system call handler instead of trap handler. sched_raise_from_isr(true, syscall_handler); + isr_ctx_swap(kctx); trap_depth--; return; } @@ -100,9 +113,6 @@ void riscv_trap_handler() { rawputc('\r'); rawputc('\n'); - isr_ctx_t *kctx; - asm volatile("csrr %0, mscratch" : "=r"(kctx)); - // Print privilige mode. if (trap_depth == 1) { if (mstatus & (3 << RISCV_STATUS_MPP_BASE_BIT)) { @@ -124,9 +134,7 @@ void riscv_trap_handler() { rawprintdec(kctx->thread->process->pid, 1); } rawprint("\n"); - if (trap_depth == 1) { - backtrace_from_ptr(kctx->frameptr); - } + backtrace_from_ptr(kctx->frameptr); isr_ctx_dump(kctx); @@ -137,6 +145,7 @@ void riscv_trap_handler() { // When the user traps just stop the process. sched_raise_from_isr(false, kill_proc_on_trap); } + isr_ctx_swap(kctx); trap_depth--; } diff --git a/kernel/cpu/riscv/src/isr_noexc.S b/kernel/cpu/riscv/src/isr_noexc.S index 060c74c..7765300 100644 --- a/kernel/cpu/riscv/src/isr_noexc.S +++ b/kernel/cpu/riscv/src/isr_noexc.S @@ -1,6 +1,8 @@ # SPDX-License-Identifier: MIT +#include "port/hardware.h" + # Run a restricted function and catch exceptions. @@ -100,12 +102,6 @@ isr_noexc_tvec_none: - # Temporary interrupt handler for noexc runtime. -isr_noexc_ivec: - ebreak - - - # Interrupt vector table for the CPU. # This must be aligned to a 256-byte boundary, so it is in a special section. .section ".interrupt_vector_table" @@ -114,10 +110,10 @@ isr_noexc_vec: .option norvc # Trap handler. j isr_noexc_tvec - # Interrupt handler. - .rept 31 - .word isr_noexc_ivec + # Interrupt handlers. + .rept RISCV_VT_INT_COUNT + ebreak .endr - # Padding to 256-byte size. - .skip 128 + # Padding. + .skip RISCV_VT_PADDING*4 .option pop \ No newline at end of file diff --git a/kernel/cpu/riscv/src/riscv_pmp.c b/kernel/cpu/riscv/src/riscv_pmp.c index 9862bf0..c62cf5d 100644 --- a/kernel/cpu/riscv/src/riscv_pmp.c +++ b/kernel/cpu/riscv/src/riscv_pmp.c @@ -8,6 +8,9 @@ #include "memprotect.h" #include "port/hardware_allocation.h" +// PMP granularity. +static size_t grain; + // Initialise memory protection driver. void riscv_pmp_init() { // Make sure we're the first to touch PMP so all of it is in our control; @@ -60,6 +63,16 @@ void riscv_pmp_init() { asm volatile("csrw pmpcfg12, x0"); asm volatile("csrw pmpcfg14, x0"); #endif + + // Determine PMP granularity. + long addr = __LONG_MAX__; + asm volatile("csrw pmpaddr0, %0; csrr %0, pmpaddr0" : "+r"(addr)); + assert_dev_drop(addr != 0); + grain = 4; + while (!(addr & 1)) { + addr >>= 1; + grain <<= 1; + } } @@ -286,50 +299,87 @@ void riscv_pmpaddr_write_all(size_t const addr_in[RISCV_PMP_REGION_COUNT]) { // Add a memory protection region. // For kernels using PMP as memory protection. -bool riscv_pmp_memprotect(riscv_pmp_ctx_t *ctx, size_t paddr, size_t length, uint32_t flags) { - if (paddr % MEMMAP_PAGE_SIZE || length % MEMMAP_PAGE_SIZE) { - return false; - } +bool riscv_pmp_memprotect(proc_memmap_t *new_mm, riscv_pmp_ctx_t *ctx, size_t addr, size_t length, uint32_t flags) { + (void)addr; + (void)length; + (void)flags; - // Corresponding NAPOT pmpaddr value. - if (paddr % length || (length & (length - 1)) || length < 8) { - return false; - } - size_t napot_addr = riscv_pmpaddr_calc_napot(paddr, length); - - // Look for a matching range. - int empty = -1; - for (int i = 0; i < PROC_RISCV_PMP_COUNT; i++) { - if (ctx->pmpaddr[i] == napot_addr && ctx->pmpcfg[i].addr_match_mode) { - // Matching region found; update permissions. - ctx->pmpcfg[i] = (riscv_pmpcfg_t){ + // Last address, used for TOR entries. + size_t prev_addr = PROC_RISCV_PMP_START ? __SIZE_MAX__ : 0; + size_t pmp = 0; + + // Temporary PMP data. + riscv_pmp_ctx_t tmp; + + for (size_t i = 0; i < new_mm->regions_len; i++) { + proc_memmap_ent_t *region = &new_mm->regions[i]; + + if ((region->base | region->size) & (grain - 1)) { + // Misaligned region. + return false; + } + if (pmp >= PROC_RISCV_PMP_COUNT) { + // Out of PMPs. + return false; + } + + // Count amount of PMPs required. + if (region->base != prev_addr && region->size == 4) { + // NA4 region. + tmp.pmpcfg[pmp] = (riscv_pmpcfg_t){ + .read = true, + .write = region->write, + .exec = region->exec, + .addr_match_mode = RISCV_PMPCFG_NA4, + .lock = false, + }; + tmp.pmpaddr[pmp] = region->base >> 2; + + } else if (region->base != prev_addr && riscv_pmpaddr_is_napot(region->base, region->size)) { + // NAPOT region. + tmp.pmpcfg[pmp] = (riscv_pmpcfg_t){ + .read = true, + .write = region->write, + .exec = region->exec, .addr_match_mode = RISCV_PMPCFG_NAPOT, - .read = !!(flags & MEMPROTECT_FLAG_R), - .write = !!(flags & MEMPROTECT_FLAG_W), - .exec = !!(flags & MEMPROTECT_FLAG_X), + .lock = false, }; - return true; - } else if ((ctx->pmpcfg[i].value & 7) == 0) { - // Empty region found. - empty = i; + tmp.pmpaddr[pmp] = riscv_pmpaddr_calc_napot(region->base, region->size); + + } else { + if (region->base != prev_addr) { + // Base address. + if (pmp > PROC_RISCV_PMP_COUNT - 2) { + // Out of PMPs. + return false; + } + tmp.pmpcfg[pmp].value = 0; + tmp.pmpaddr[pmp] = region->base >> 2; + pmp++; + } + + // TOR region. + tmp.pmpcfg[pmp] = (riscv_pmpcfg_t){ + .read = true, + .write = region->write, + .exec = region->exec, + .addr_match_mode = RISCV_PMPCFG_TOR, + .lock = false, + }; + tmp.pmpaddr[pmp] = (region->base + region->size) >> 2; } + prev_addr = region->base + region->size; + pmp++; } - if (empty >= 0 && (flags & 7)) { - // Empty region found to apply flags to. - ctx->pmpcfg[empty] = (riscv_pmpcfg_t){ - .addr_match_mode = RISCV_PMPCFG_NAPOT, - .read = !!(flags & MEMPROTECT_FLAG_R), - .write = !!(flags & MEMPROTECT_FLAG_W), - .exec = !!(flags & MEMPROTECT_FLAG_X), - }; - ctx->pmpaddr[empty] = napot_addr; - return true; - - } else { - // No region found. - return false; + // Clear remaining PMP entries. + for (; pmp < PROC_RISCV_PMP_COUNT; pmp++) { + tmp.pmpcfg[pmp].value = 0; } + + // Commit generated PMPs. + *ctx = tmp; + return true; } diff --git a/kernel/include/hal/spi.h b/kernel/include/hal/spi.h new file mode 100644 index 0000000..8b34b51 --- /dev/null +++ b/kernel/include/hal/spi.h @@ -0,0 +1,31 @@ + +// SPDX-License-Identifier: MIT + +#pragma once + +#include "badge_err.h" + +#include +#include +#include + +// Returns the amount of SPI peripherals present. +// Cannot produce an error. +#define spi_count() (1) + +// Initialises SPI peripheral spi_num in controller mode with SCLK pin `sclk_pin`, MOSI pin `mosi_pin`, MISO pin `miso_pin`, SS pin `ss_pin` and clock speed/bitrate +// bitrate. The modes of the SCLK, MOSI, MISO and SS pins are changed automatically. This +// function may be used again to change the settings on an initialised SPI peripheral in controller mode. +void spi_controller_init(badge_err_t *ec, int spi_num, int sclk_pin, int mosi_pin, int miso_pin, int ss_pin, int32_t bitrate); +// De-initialises SPI peripheral. +void spi_deinit(badge_err_t *ec, int spi_num); +// Reads len bytes into buffer buf from SPI device. +// This function blocks until the entire transaction is completed. +void spi_controller_read(badge_err_t *ec, int spi_num, void *buf, size_t len); +// Writes len bytes from buffer buf to SPI device. +// This function blocks until the entire transaction is completed. +void spi_controller_write(badge_err_t *ec, int spi_num, void const *buf, size_t len); +// Writes len bytes from buffer buf to SPI device, then reads len bytes into buffer buf from SPI device. +// Write and read happen simultaneously if the full-duplex flag fdx is set. +// This function blocks until the entire transaction is completed. +void spi_controller_transfer(badge_err_t *ec, int spi_num, void *buf, size_t len, bool fdx); diff --git a/kernel/include/interrupt.h b/kernel/include/interrupt.h index c867b97..8ea38d8 100644 --- a/kernel/include/interrupt.h +++ b/kernel/include/interrupt.h @@ -20,10 +20,11 @@ void irq_ch_prio(int int_irq, int prio); void irq_ch_ack(int int_irq); // Enable/disable an internal interrupt on this CPU. -// Returns whether the interrupt was enabled on this CPU. -static inline bool irq_ch_enable(int int_irq, bool enable); +void irq_ch_enable(int int_irq, bool enable); // Query whether an internal interrupt is enabled on this CPU. -static inline bool irq_ch_enabled(int int_irq); +bool irq_ch_enabled(int int_irq); +// Query whether an internal interrupt is pending. +bool irq_ch_pending(int int_irq); // Set the interrupt service routine for an interrupt on this CPU. void irq_ch_set_isr(int int_irq, isr_t isr); diff --git a/kernel/include/memprotect.h b/kernel/include/memprotect.h index b25bb3e..3c3ffba 100644 --- a/kernel/include/memprotect.h +++ b/kernel/include/memprotect.h @@ -16,7 +16,8 @@ #define MEMPROTECT_FLAG_RWX 0x00000007 #define MEMPROTECT_FLAG_KERNEL 0x80000000 -#include +#include "port/memprotect.h" +#include "process/types.h" @@ -30,7 +31,7 @@ void memprotect_create(mpu_ctx_t *ctx); // Clean up a memory protection context. void memprotect_destroy(mpu_ctx_t *ctx); // Add a memory protection region. -bool memprotect(mpu_ctx_t *ctx, size_t vaddr, size_t paddr, size_t length, uint32_t flags); +bool memprotect(proc_memmap_t *new_mm, mpu_ctx_t *ctx, size_t vaddr, size_t paddr, size_t length, uint32_t flags); // Commit pending memory protections, if any. void memprotect_commit(mpu_ctx_t *ctx); // Swap in memory protections for the current thread. diff --git a/kernel/port/esp32c6/CMakeLists.txt b/kernel/port/esp32c6/CMakeLists.txt index cc84428..1c35b51 100644 --- a/kernel/port/esp32c6/CMakeLists.txt +++ b/kernel/port/esp32c6/CMakeLists.txt @@ -9,6 +9,7 @@ set(target_abi "ilp32") set(port_src ${CMAKE_CURRENT_LIST_DIR}/src/hal/gpio.c ${CMAKE_CURRENT_LIST_DIR}/src/hal/i2c.c + ${CMAKE_CURRENT_LIST_DIR}/src/hal/spi.c ${CMAKE_CURRENT_LIST_DIR}/src/clkconfig.c ${CMAKE_CURRENT_LIST_DIR}/src/interrupt.c diff --git a/kernel/port/esp32c6/flash.sh b/kernel/port/esp32c6/flash.sh index 4033ad2..b06454f 100755 --- a/kernel/port/esp32c6/flash.sh +++ b/kernel/port/esp32c6/flash.sh @@ -2,7 +2,7 @@ PORT="$1" OUTPUT="$2" -esptool.py -b 921600 --port "$PORT" \ +../.venv/bin/python -m esptool -b 921600 --port "$PORT" \ write_flash --flash_mode dio --flash_freq 80m --flash_size 2MB \ 0x0 port/esp32c6/bootloader.bin \ 0x10000 "$OUTPUT/badger-os.bin" \ diff --git a/kernel/port/esp32c6/include/port/clkconfig.h b/kernel/port/esp32c6/include/port/clkconfig.h index 48cde7e..6145d03 100644 --- a/kernel/port/esp32c6/include/port/clkconfig.h +++ b/kernel/port/esp32c6/include/port/clkconfig.h @@ -8,3 +8,4 @@ // Configure I2C0 clock. void clkconfig_i2c0(uint32_t freq_hz, bool enable, bool reset); +void clkconfig_spi2(uint32_t freq_hz, bool enable, bool reset); diff --git a/kernel/port/esp32c6/include/port/hardware.h b/kernel/port/esp32c6/include/port/hardware.h index 2e0b0ef..9df4a16 100644 --- a/kernel/port/esp32c6/include/port/hardware.h +++ b/kernel/port/esp32c6/include/port/hardware.h @@ -3,8 +3,10 @@ #pragma once +#ifndef __ASSEMBLER__ #include #include +#endif @@ -13,6 +15,15 @@ // Number of PMP regions supported by the CPU. #define RISCV_PMP_REGION_COUNT 16 +// Number of interrupt channels (excluding trap handler) in the vector table. +#define RISCV_VT_INT_COUNT 47 +// Number of padding words in the vector table. +#define RISCV_VT_PADDING 16 +// Bitmask for interrupt cause. +#define RISCV_VT_ICAUSE_MASK 63 +// Bitmask for trap cause. +#define RISCV_VT_TCAUSE_MASK 31 + /* ==== SOC INFO ==== */ @@ -159,8 +170,10 @@ +#ifndef __ASSEMBLER__ #define WRITE_REG(addr, val) \ do { \ *(volatile uint32_t *)(addr) = (val); \ } while (0) #define READ_REG(addr) (*(volatile uint32_t *)(addr)) +#endif diff --git a/kernel/port/esp32c6/linker.ld b/kernel/port/esp32c6/linker.ld index d23c43b..595bef6 100644 --- a/kernel/port/esp32c6/linker.ld +++ b/kernel/port/esp32c6/linker.ld @@ -110,6 +110,7 @@ PROVIDE(CLINT_U = 0x20001C00); __start_init_array = .; KEEP(*(.init_array)) __stop_init_array = .; + . = ALIGN(__section_alignment); } :rodataseg .rodata : AT(LOADADDR(.init_array) + SIZEOF(.init_array)) { *(.rodata) *(.rodata*) diff --git a/kernel/port/esp32c6/src/clkconfig.c b/kernel/port/esp32c6/src/clkconfig.c index f8ad58b..46643e0 100644 --- a/kernel/port/esp32c6/src/clkconfig.c +++ b/kernel/port/esp32c6/src/clkconfig.c @@ -6,6 +6,8 @@ #include "attributes.h" #include "log.h" #include "port/hardware.h" +#include "soc/spi_struct.h" +#include "soc/spi_reg.h" // UART0 configuration register (Access: R/W) #define PCR_UART0_CONF_REG (PCR_BASE + 0x0000) @@ -178,6 +180,19 @@ // Numerator of the clock divider for PCR_*_SCLK_CONF_REG (bit position). #define PCR_CONF_SCLK_DIV_NUM_POS 0 +// Enable bit for PCR_*_CLKM_CONF_REG. +#define PCR_CONF_CLKM_ENABLE_BIT 0x00400000 +// Clock source select mask for PCR_*_CLKM_CONF_REG. +#define PCR_CONF_CLKM_SEL_MASK 0x00300000 +// Clock source select mask for PCR_*_CLKM_CONF_REG (bit position). +#define PCR_CONF_CLKM_SEL_POS 20 +// Clock source select XTAL_CLK for PCR_*_CLKM_CONF_REG +#define PCR_CONF_CLKM_SEL_XTAL_CLK 0 +// Clock source select PLL_F80M_CLK for PCR_*_CLKM_CONF_REG +#define PCR_CONF_CLKM_SEL_PLL_F80M_CLK 1 +// Clock source select RC_FAST_CLK for PCR_*_CLKM_CONF_REG +#define PCR_CONF_CLKM_SEL_RC_FAST_CLK 2 + // Nominal frequency of PLL_CLK. #define FREQ_PLL_CLK 480000000 // Nominal frequency of PLL_F160M_CLK. @@ -197,9 +212,10 @@ // Compute frequency dividers for a certain target frequency and source // frequency. -static uint32_t clk_compute_div(uint32_t source_hz, uint32_t target_hz) PURE; +static uint32_t i2c_clk_compute_div(uint32_t source_hz, uint32_t target_hz) PURE; +static spi_clock_reg_t spi_clk_compute_div(uint32_t source_hz, uint32_t target_hz, uint8_t duty) PURE; -static uint32_t clk_compute_div(uint32_t source_hz, uint32_t target_hz) { +static uint32_t i2c_clk_compute_div(uint32_t source_hz, uint32_t target_hz) { // Divider integral part. uint32_t integral = source_hz / target_hz; // Divider fractional part. @@ -231,11 +247,82 @@ static uint32_t clk_compute_div(uint32_t source_hz, uint32_t target_hz) { (closest_den << PCR_CONF_SCLK_DIV_DEN_POS); } +static spi_clock_reg_t spi_clk_compute_div(uint32_t source_hz, uint32_t target_hz, uint8_t duty) { + const uint32_t pre_max = SPI_CLKDIV_PRE_V + 1; + const uint32_t n_max = SPI_CLKCNT_N_V + 1; + + spi_clock_reg_t spi_clock_reg = {.val = 0}; + + uint32_t best_n = 1, best_pre = 1; + + if ((target_hz < source_hz )) { + if ((source_hz/ (pre_max * n_max)) > target_hz) { + logkf(LOG_DEBUG, "Maximum SPI clock pre-scaling"); + best_pre = pre_max; + best_n = n_max; + } + else if (target_hz < source_hz ) { + logkf(LOG_DEBUG, "Finding optimal SPI clock pre-scaling"); + int32_t best_err = (int32_t) target_hz; // assume worst possible + + // Try to get the most counter resolution and lowest pre-divider + for (uint32_t n = n_max; n >= 2; n--) { + uint32_t pre = (source_hz/n)/target_hz; + + if (pre < 1) pre = 1; + // Prefer to err on the side of too low clock frequency to not damage peripherals + if ((source_hz/n/pre) > target_hz){ + pre += 1; + } + if (pre > pre_max) continue; + + int32_t err = (int32_t) target_hz - (int32_t) (source_hz/n/pre); + if (err < best_err) { + best_n = n; + best_pre = pre; + best_err = err; + } + } + } + + // Map duty from [0..255] to [1..n-1] + uint32_t h = 1 + ((duty * ((best_n-1)-1))/255); + + spi_clock_reg.clkdiv_pre = best_pre - 1; + spi_clock_reg.clkcnt_n = best_n - 1; + spi_clock_reg.clkcnt_l = best_n - 1; + spi_clock_reg.clkcnt_h = h - 1; + } + else { + logkf(LOG_DEBUG, "No SPI clock pre-scaling"); + spi_clock_reg.clk_equ_sysclk = 1; + } + + return spi_clock_reg; +} + // Configure I2C0 clock. void clkconfig_i2c0(uint32_t freq_hz, bool enable, bool reset) { // I2C0 is configured on XTAL_CLK. WRITE_REG(PCR_I2C_CONF_REG, PCR_CONF_ENABLE_BIT + reset * PCR_CONF_RESET_BIT); - WRITE_REG(PCR_I2C_SCLK_CONF_REG, enable * PCR_CONF_SCLK_ENABLE_BIT + clk_compute_div(FREQ_XTAL_CLK, freq_hz)); + WRITE_REG(PCR_I2C_SCLK_CONF_REG, enable * PCR_CONF_SCLK_ENABLE_BIT + i2c_clk_compute_div(FREQ_XTAL_CLK, freq_hz)); logkf(LOG_DEBUG, "PCR_I2C_CONF_REG: %{u32;x}", READ_REG(PCR_I2C_CONF_REG)); logkf(LOG_DEBUG, "PCR_I2C_SCLK_CONF_REG: %{u32;x}", READ_REG(PCR_I2C_SCLK_CONF_REG)); } + +void clkconfig_spi2(uint32_t freq_hz, bool enable, bool reset) { + // SPI2 is configured on PLL_F80M_CLK. + WRITE_REG(PCR_SPI2_CONF_REG, PCR_CONF_ENABLE_BIT + reset * PCR_CONF_RESET_BIT); + WRITE_REG(PCR_SPI2_CLKM_CONF_REG, enable * PCR_CONF_CLKM_ENABLE_BIT + (PCR_CONF_CLKM_SEL_PLL_F80M_CLK << PCR_CONF_CLKM_SEL_POS)); + GPSPI2.clock = spi_clk_compute_div(FREQ_PLL_F80M_CLK, freq_hz, 256/2); + // GPSPI2.clk_gate.mst_clk_active = true; + // GPSPI2.clk_gate.mst_clk_sel = 1; + logkf(LOG_DEBUG, "PCR_SPI2_CONF_REG: %{u32;x}", READ_REG(PCR_SPI2_CONF_REG)); + logkf(LOG_DEBUG, "PCR_SPI2_CLKM_CONF_REG: %{u32;x}", READ_REG(PCR_SPI2_CLKM_CONF_REG)); + logkf(LOG_DEBUG, "CLKDIV_PRE %{u32;d}, CLKCNT_N: %{u32;d}, CLKCNT_L: %{u32;d}, CLKCNT_H: %{u32;d}", + GPSPI2.clock.clkdiv_pre, + GPSPI2.clock.clkcnt_n, + GPSPI2.clock.clkcnt_l, + GPSPI2.clock.clkcnt_h + ); +} diff --git a/kernel/port/esp32c6/src/hal/spi.c b/kernel/port/esp32c6/src/hal/spi.c new file mode 100644 index 0000000..a7843c8 --- /dev/null +++ b/kernel/port/esp32c6/src/hal/spi.c @@ -0,0 +1,184 @@ + +// SPDX-License-Identifier: MIT + +#include "hal/spi.h" + +#include + +#include "interrupt.h" +#include "hal/gpio.h" +#include "port/clkconfig.h" +#include "port/hardware_allocation.h" +#include "soc/gpio_sig_map.h" +#include "soc/gpio_struct.h" +#include "soc/spi_struct.h" +#include "soc/io_mux_struct.h" + +static void spi_clear_fifo(bool clear_rxfifo, bool clear_txfifo) { + GPSPI2.dma_conf = (spi_dma_conf_reg_t){ + .buf_afifo_rst = clear_txfifo, + .rx_afifo_rst = clear_rxfifo, + }; + GPSPI2.dma_conf = (spi_dma_conf_reg_t){ + .buf_afifo_rst = false, + .rx_afifo_rst = false, + }; +} + +static void spi_config_apply(void) { + // Apply the register configurations and wait until it's done. + GPSPI2.cmd.update = 1; + while(GPSPI2.cmd.update); +} + +static void spi_isr(void) { + logk(LOG_DEBUG, "SPI ISR"); + if (GPSPI2.dma_int_raw.trans_done) { + GPSPI2.dma_int_clr.trans_done = 1; + } +} + +static void spi_int_config(bool enable, int channel) { + // Disable interrupts before changing interrupt settings. + bool mie = irq_enable(false); + + if (enable) { + irq_ch_route(EXT_IRQ_GPSPI2_INTR, channel); + irq_ch_set_isr(channel, spi_isr); + irq_ch_prio(channel, INT_PRIO_SPI); + } + irq_ch_enable(channel, enable); + GPSPI2.dma_int_ena.trans_done = enable; + + // Re-enable interrupts. + asm volatile("fence"); + irq_enable(mie); +} + +void spi_controller_init(badge_err_t *ec, int spi_num, int sclk_pin, int mosi_pin, int miso_pin, int ss_pin, int32_t bitrate) { + // Bounds check. + if (spi_num != 0 + || sclk_pin < 0 || sclk_pin >= io_count() + || mosi_pin < 0 || mosi_pin >= io_count() + || miso_pin < 0 || miso_pin >= io_count() + || ss_pin < 0 || ss_pin >= io_count() + ) { + badge_err_set(ec, ELOC_SPI, ECAUSE_RANGE); + return; + } + + // Pin availability check. + if (io_is_peripheral(ec, sclk_pin)) { + logkf(LOG_ERROR, "SCLK pin (%{d}) already in use", sclk_pin); + return; + } else if (io_is_peripheral(ec, mosi_pin)) { + logkf(LOG_ERROR, "MOSI pin (%{d}) already in use", mosi_pin); + return; + } else if (io_is_peripheral(ec, miso_pin)) { + logkf(LOG_ERROR, "MISO pin (%{d}) already in use", miso_pin); + return; + } else if (io_is_peripheral(ec, ss_pin)) { + logkf(LOG_ERROR, "SS pin (%{d}) already in use", ss_pin); + return; + } + + // Clock configuration. + clkconfig_spi2(bitrate, true, false); + + spi_int_config(true, INT_CHANNEL_SPI); + + spi_config_apply(); + + // Set pins to GPIO function and enable input for MISO + IO_MUX.gpio[sclk_pin] = (io_mux_gpio_t){.mcu_sel = 1,}; + IO_MUX.gpio[miso_pin] = (io_mux_gpio_t){.mcu_sel = 1, .fun_ie = true}; + IO_MUX.gpio[mosi_pin] = (io_mux_gpio_t){.mcu_sel = 1}; + IO_MUX.gpio[ss_pin] = (io_mux_gpio_t){.mcu_sel = 1}; + + // GPIO matrix configuration. + GPIO.func_out_sel_cfg[sclk_pin] = (gpio_func_out_sel_cfg_reg_t){ + .oen_inv_sel = false, + .oen_sel = false, + .out_inv_sel = false, + .out_sel = FSPICLK_OUT_IDX, + }; + GPIO.func_out_sel_cfg[mosi_pin] = (gpio_func_out_sel_cfg_reg_t){ + .oen_inv_sel = false, + .oen_sel = false, + .out_inv_sel = false, + .out_sel = FSPID_OUT_IDX, + }; + GPIO.func_out_sel_cfg[ss_pin] = (gpio_func_out_sel_cfg_reg_t){ + .oen_inv_sel = false, + .oen_sel = false, + .out_inv_sel = false, + .out_sel = FSPICS0_OUT_IDX, + }; + + GPIO.func_in_sel_cfg[FSPIQ_IN_IDX] = (gpio_func_in_sel_cfg_reg_t){ + .in_sel = miso_pin, + .in_inv_sel = false, + .sig_in_sel = true, + }; +} + +static void spi_controller_transfer_generic(badge_err_t *ec, int spi_num, void *buf, size_t len) { + const int data_buf_len = sizeof(GPSPI2.data_buf); + uint32_t words [data_buf_len/sizeof(GPSPI2.data_buf[0])]; + + // Bounds check. + if (spi_num != 0) { + badge_err_set(ec, ELOC_SPI, ECAUSE_RANGE); + return; + } + + while (len > 0) { + size_t copy_len = (len > data_buf_len) ? data_buf_len : len; + + if (GPSPI2.user.usr_mosi) { + // Access to SPI data buffer must be in full 32 bit words + // This assumes optimization for alignment in mem_copy + mem_copy(words, buf, copy_len); + mem_copy((void *)GPSPI2.data_buf, words, data_buf_len); // TODO: optimize copy length + } + + // prepare for transfer + GPSPI2.ms_dlen.ms_data_bitlen = copy_len * 8 - 1; + spi_clear_fifo(true, true); + spi_config_apply(); + + // Start transfer and wait for completion + GPSPI2.cmd.usr = 1; + while(GPSPI2.cmd.usr); // TODO: yield? + + if (GPSPI2.user.usr_miso) { + // copy back received data + mem_copy(buf, (void *)GPSPI2.data_buf, copy_len); + } + + len -= copy_len; + buf += copy_len; + } +} + +void spi_controller_read(badge_err_t *ec, int spi_num, void *buf, size_t len) { + GPSPI2.user.usr_mosi = 0; + GPSPI2.user.usr_miso = 1; + + spi_controller_transfer_generic(ec, spi_num, buf, len); +} + +void spi_controller_write(badge_err_t *ec, int spi_num, void const *buf, size_t len) { + GPSPI2.user.usr_mosi = 1; + GPSPI2.user.usr_miso = 0; + + spi_controller_transfer_generic(ec, spi_num, (void*) buf, len); +} + +void spi_controller_transfer(badge_err_t *ec, int spi_num, void *buf, size_t len, bool fdx) { + GPSPI2.user.usr_mosi = 1; + GPSPI2.user.usr_miso = 1; + GPSPI2.user.doutdin = fdx; + + spi_controller_transfer_generic(ec, spi_num, buf, len); +} diff --git a/kernel/port/esp32c6/src/interrupt.c b/kernel/port/esp32c6/src/interrupt.c index b23a32f..d527571 100644 --- a/kernel/port/esp32c6/src/interrupt.c +++ b/kernel/port/esp32c6/src/interrupt.c @@ -110,3 +110,23 @@ void riscv_interrupt_handler() { // Acknowledge interrupt. irq_ch_ack(mcause); } + + + +// Enable/disable an internal interrupt. +void irq_ch_enable(int int_irq, bool enable) { + assert_dev_drop(int_irq > 0 && int_irq < 32); + long mask = 1 << int_irq; + if (enable) { + asm volatile("csrs mie, %0" ::"r"(mask)); + } else { + asm volatile("csrc mie, %0" ::"r"(mask)); + } +} + +// Query whether an internal interrupt is enabled. +bool irq_ch_enabled(int int_irq) { + long mask; + asm("csrr %0, mie" : "=r"(mask)); + return ((mask) >> int_irq) & 1; +} diff --git a/kernel/port/esp32c6/src/memprotect.c b/kernel/port/esp32c6/src/memprotect.c index 7651f01..56ddb95 100644 --- a/kernel/port/esp32c6/src/memprotect.c +++ b/kernel/port/esp32c6/src/memprotect.c @@ -94,8 +94,8 @@ void memprotect_destroy(mpu_ctx_t *ctx) { } // Add a memory protection region. -bool memprotect(mpu_ctx_t *ctx, size_t vaddr, size_t paddr, size_t length, uint32_t flags) { - return vaddr == paddr && riscv_pmp_memprotect(ctx, vaddr, length, flags); +bool memprotect(proc_memmap_t *new_mm, mpu_ctx_t *ctx, size_t vaddr, size_t paddr, size_t length, uint32_t flags) { + return vaddr == paddr && riscv_pmp_memprotect(new_mm, ctx, vaddr, length, flags); } // Commit pending memory protections, if any. diff --git a/kernel/port/esp32p4/CMakeLists.txt b/kernel/port/esp32p4/CMakeLists.txt index 7c9f6cf..3a99903 100644 --- a/kernel/port/esp32p4/CMakeLists.txt +++ b/kernel/port/esp32p4/CMakeLists.txt @@ -17,10 +17,10 @@ set(port_src ${CMAKE_CURRENT_LIST_DIR}/src/port.c ) set(port_include - ${CMAKE_CURRENT_LIST_DIR}/include - ${CMAKE_CURRENT_LIST_DIR}/../esp_common/include - ${CMAKE_CURRENT_LIST_DIR}/../esp_common/esp-idf/components/soc/esp32p4/include - ${CMAKE_CURRENT_LIST_DIR}/../esp_common/esp-idf/components/soc/include + ${CMAKE_CURRENT_LIST_DIR}/include/ + ${CMAKE_CURRENT_LIST_DIR}/../esp_common/include/ + ${CMAKE_CURRENT_LIST_DIR}/../esp_common/esp-idf/components/soc/esp32p4/include/ + ${CMAKE_CURRENT_LIST_DIR}/../esp_common/esp-idf/components/soc/include/ ${CMAKE_CURRENT_LIST_DIR}/../esp_common/esp-idf/components/esp_rom/include/esp32p4/ ) set(port_link diff --git a/kernel/port/esp32p4/flash.sh b/kernel/port/esp32p4/flash.sh index 755e063..467d023 100755 --- a/kernel/port/esp32p4/flash.sh +++ b/kernel/port/esp32p4/flash.sh @@ -2,7 +2,7 @@ PORT="$1" OUTPUT="$2" -esptool.py -b 921600 --port "$PORT" \ +../.venv/bin/python -m esptool -b 921600 --port "$PORT" \ write_flash --flash_mode dio --flash_freq 80m --flash_size 2MB \ 0x2000 port/esp32p4/bootloader.bin \ 0x10000 "$OUTPUT/badger-os.bin" \ diff --git a/kernel/port/esp32p4/include/port/hardware.h b/kernel/port/esp32p4/include/port/hardware.h index 96c2e6d..f70c60a 100644 --- a/kernel/port/esp32p4/include/port/hardware.h +++ b/kernel/port/esp32p4/include/port/hardware.h @@ -3,15 +3,34 @@ #pragma once -#include -#include - /* ==== CPU INFO ==== */ // Number of PMP regions supported by the CPU. #define RISCV_PMP_REGION_COUNT 16 +// Enable use of CLIC interrupt controller. +#define RISCV_USE_CLIC +// Number of CLIC interrupt priority levels. +#define RISCV_CLIC_MAX_PRIO 7 +// MTVT CSR index. +#define CSR_MTVT 0x307 +// MTVT CSR name. +#define CSR_MTVT_STR "0x307" +// MINTSTATUS CSR index. +// As per CLIC specs, mintstatus CSR should be at 0xFB1, however esp32p4 implements it at 0x346 +#define CSR_MINTSTATUS 0x346 +// MINTSTATUS CSR name. +#define CSR_MINTSTATUS_STR "0x346" + +// Number of interrupt channels (excluding trap handler) in the vector table. +#define RISCV_VT_INT_COUNT 47 +// Number of padding words in the vector table. +#define RISCV_VT_PADDING 16 +// Bitmask for interrupt cause. +#define RISCV_VT_ICAUSE_MASK 63 +// Bitmask for trap cause. +#define RISCV_VT_TCAUSE_MASK 31 /* ==== SOC INFO ==== */ @@ -19,4 +38,4 @@ // Number of timer groups. #define ESP_TIMG_COUNT 2 // Number of timers per timer group. -#define ESP_TIMG_TIMER_COUNT 2 +#define ESP_TIMG_TIMER_COUNT 1 diff --git a/kernel/port/esp32p4/include/port/interrupt.h b/kernel/port/esp32p4/include/port/interrupt.h index e8684a8..3983eb2 100644 --- a/kernel/port/esp32p4/include/port/interrupt.h +++ b/kernel/port/esp32p4/include/port/interrupt.h @@ -4,6 +4,6 @@ #pragma once #include "cpu/interrupt.h" -// #include "soc/ext_irq.h" +#include "soc/interrupts.h" -#define EXT_IRQ_COUNT 77 +#define EXT_IRQ_COUNT ETS_MAX_INTR_SOURCE diff --git a/kernel/port/esp32p4/include/soc/clic_struct.h b/kernel/port/esp32p4/include/soc/clic_struct.h new file mode 100644 index 0000000..159fe0d --- /dev/null +++ b/kernel/port/esp32p4/include/soc/clic_struct.h @@ -0,0 +1,76 @@ + +// SPDX-License-Identifier: MIT + +#pragma once + +typedef union { + struct { + // Whether `Smclicshv` is supported. + uint32_t nvbits : 1; + // Number of bits used to represent interrupt priority. + uint32_t nlbits : 4; + // Number of privilege modes supported. + // When 0, only M-mode has interrupts. + uint32_t nmbits : 2; + uint32_t : 25; + }; + // Packed value. + uint32_t val; +} clic_dev_int_config_reg_t; + +typedef union { + struct { + // Actual number of interrupt channels. + uint32_t num_int : 13; + // CLIC hardware version. + uint32_t version : 8; + // Number of bits used to represent interrupt priority. + uint32_t ctlbits : 4; + uint32_t : 7; + }; + // Packed value. + uint32_t val; +} clic_dev_int_info_reg_t; + +typedef union { + struct { + uint32_t thresh : 8; + uint32_t : 24; + }; + // Packed value. + uint32_t val; +} clic_dev_int_thresh_reg_t; + +typedef struct { + clic_dev_int_config_reg_t volatile int_config; + clic_dev_int_info_reg_t volatile int_info; + clic_dev_int_thresh_reg_t volatile int_thresh; +} clic_dev_t; + + + +typedef union { + struct { + // Interrupt is pending. + uint32_t pending : 1; + uint32_t : 7; + // Interrupt is enabled. + uint32_t enable : 1; + uint32_t : 7; + // TODO: What is this? + uint32_t attr_shv : 1; + // Whether the interrupt is edge-triggered. + uint32_t attr_trig : 1; + uint32_t : 4; + // Which privilege mode receives this interrupt. + uint32_t attr_mode : 2; + // Interrupt priority. + uint32_t ctl : 8; + }; + // Packed value. + uint32_t val; +} clic_int_ctl_reg_t; + +typedef union { + clic_int_ctl_reg_t volatile irq_ctl[48]; +} clic_ctl_dev_t; diff --git a/kernel/port/esp32p4/include/soc/soc_types.h b/kernel/port/esp32p4/include/soc/soc_types.h index 231a1a8..f2be04d 100644 --- a/kernel/port/esp32p4/include/soc/soc_types.h +++ b/kernel/port/esp32p4/include/soc/soc_types.h @@ -2,3 +2,9 @@ // SPDX-License-Identifier: MIT #pragma once + +typedef enum { + PMU_HP_MODE_ACTIVE, + PMU_HP_MODE_MODEM, + PMU_HP_MODE_SLEEP, +} pmu_hp_mode_t; diff --git a/kernel/port/esp32p4/linker.ld b/kernel/port/esp32p4/linker.ld index 88e676e..ddcbc41 100644 --- a/kernel/port/esp32p4/linker.ld +++ b/kernel/port/esp32p4/linker.ld @@ -1,22 +1,30 @@ /* SPDX-License-Identifier: MIT */ -PHDRS +OUTPUT_FORMAT("elf32-littleriscv") +OUTPUT_ARCH(riscv) +ENTRY(_start) + +MEMORY { - hdrseg PT_LOAD; - codeseg PT_LOAD; - rodataseg PT_LOAD; - dataseg PT_LOAD; + tcm (RWX) : ORIGIN = 0x30100000, LENGTH = 2K + xip (RX) : ORIGIN = 0x40000000, LENGTH = 64M + sram (RWX) : ORIGIN = 0x4FF00000, LENGTH = 640K + lpsram (RWX) : ORIGIN = 0x50108000, LENGTH = 32K } SECTIONS { - /DISCARD/ : { *(.note.gnu.build-id) } + /DISCARD/ : { *(.note.gnu.build-id) *(.eh_frame*) } /* Import hardware addresses. */ INCLUDE esp32p4.peripherals.ld INCLUDE esp32p4.rom.ld PROVIDE(CACHE = 0x3FF10000); +PROVIDE(INTMTX0 = 0x500D6000); +PROVIDE(INTMTX1 = 0x500D6800); +PROVIDE(CLIC = 0x20800000); +PROVIDE(CLIC_CTL = 0x20801000); /* Physical memory. */ __start_tcm = 0x30100000; @@ -25,38 +33,13 @@ PROVIDE(CACHE = 0x3FF10000); __stop_xip = 0x44000000; __start_sram = 0x4FF00000; __stop_sram = 0x4FFA0000; - /* __start_sram = 0x4FF00000; - __stop_sram = 0x4FF2CBD0; - __start_sram2 = 0x4FF40000; - __stop_sram2 = 0x4FFA0000; */ __start_lpsram = 0x50108000; __stop_lpsram = 0x5010FFE8; __section_alignment = 16; - /* RAM sections. */ - . = __start_sram; - __start_data = .; - .data : AT(SIZEOF(.esphdr) + SIZEOF(.espseg.0)) { - __global_pointer$ = .; - *(.data) *(.data*) - *(.sdata) *(.sdata*) - . = ALIGN(__section_alignment); - } :dataseg - __stop_data = .; - - __start_bss = .; - .bss : { - *(.bss) *(.bss*) - *(.sbss) *(.sbss*) - . = ALIGN(__section_alignment); - } :NONE - __stop_bss = .; - - /* FLASH sections. */ - . = __start_xip; - /* ESP image header. */ + . = __start_xip; .esphdr : AT(0) { BYTE(0xE9); /* Magic byte. */ BYTE(3); /* Segment count. */ @@ -73,64 +56,97 @@ PROVIDE(CACHE = 0x3FF10000); SHORT(0xffff); /* Max chip rev. */ LONG(0x00000000); /* (reserved) */ BYTE(0x00); /* SHA256 appended (not appended). */ - } :hdrseg + } > xip - /* ESP image segment 0. */ + /* ESP image segment 0: code. */ .espseg.0 : AT(LOADADDR(.esphdr) + SIZEOF(.esphdr)) { - LONG(__start_data); - LONG(__stop_data - __start_data); - } :hdrseg - - /* Defined above: Initialised data segment. */ - . = . + SIZEOF(.data); - - /* ESP image segment 1. */ - .espseg.1 : AT(LOADADDR(.data) + SIZEOF(.data)) { LONG(__start_text); LONG(__stop_text - __start_text); - } :codeseg + } >xip - /* Code sections. */ __start_text = .; - .text : AT(LOADADDR(.espseg.1) + SIZEOF(.espseg.1)) { + .init_array : AT(LOADADDR(.espseg.0) + SIZEOF(.espseg.0)) { + . = ALIGN(4); + __start_init_array = .; + *(.init_array) + __stop_init_array = .; + } >xip + .text : AT(LOADADDR(.init_array) + SIZEOF(.init_array)) { . = ALIGN(256); *(.interrupt_vector_table) *(.text) *(.text*) - . = ALIGN(__section_alignment); - } :codeseg + . = ALIGN(4); + } >xip __stop_text = .; - /* ESP image segment 2. */ - .espseg.2 : AT(LOADADDR(.text) + SIZEOF(.text)) { + /* ESP image segment 1: rodata. */ + .espseg.1 : AT(LOADADDR(.text) + SIZEOF(.text)) { LONG(__start_rodata); LONG(__stop_rodata - __start_rodata); - } :rodataseg + } >xip - /* Read-only data sections. */ __start_rodata = .; - .init_array : AT(LOADADDR(.espseg.2) + SIZEOF(.espseg.2)) { - /* Create symbols to keep track of `.init_array`. */ - __start_init_array = .; - KEEP(*(.init_array)) - __stop_init_array = .; - } :rodataseg - .rodata : AT(LOADADDR(.init_array) + SIZEOF(.init_array)) { + .got : AT(LOADADDR(.espseg.1) + SIZEOF(.espseg.1)) { + *(.got.plt) *(.igot.plt) *(.got) *(.igot) + } + .plt : AT(LOADADDR(.got) + SIZEOF(.got)) { + *(.plt) *(.iplt) + } + .rodata : AT(LOADADDR(.plt) + SIZEOF(.plt)) { + . = ALIGN(__section_alignment); *(.rodata) *(.rodata*) + . = ALIGN(4); + } >xip + __stop_rodata = .; + + /* ESP image segment 2: data. */ + .espseg.2 : AT(LOADADDR(.rodata) + SIZEOF(.rodata)) { + LONG(__start_data); + LONG(__stop_data - __start_data); + } >xip + + . = __start_sram; + __start_data = .; + .data : AT(LOADADDR(.espseg.2) + SIZEOF(.espseg.2)) { + . = ALIGN(__section_alignment); + *(.data) *(.data*) + . = ALIGN(__section_alignment); + } >sram + __start_ss = .; + .sdata : AT(LOADADDR(.data) + SIZEOF(.data)) { + *(.sdata) *(.sdata*) *(.srodata) *(.srodata*) - *(.eh_frame) *(.eh_frame_hdr*) + . = ALIGN(4); + } >sram + __stop_data = .; + + /* Zero-initialized */ + __start_bss = .; + .sbss : { . = ALIGN(__section_alignment); - } :rodataseg - __stop_rodata = .; + *(.sbss) *(.sbss.*) + } >sram :NONE + __stop_ss = .; + .bss : { + . = ALIGN(__section_alignment); + *(.bss) *(.bss.*) + } >sram :NONE + __stop_bss = .; + PROVIDE( __global_pointer$ = (__stop_ss + __start_ss) / 2 ); + + /* Uninitialized */ + . = ALIGN(__section_alignment); + __start_noinit = .; + .noinit : { + *(.noinit) *(.noinit.*) + } >sram :NONE + __stop_noinit = .; /* Unallocated physical memory. */ __start_free_xip = (__stop_rodata + 0xffff) & 0xffff0000; __stop_free_xip = __stop_xip; __start_free_sram = (__stop_bss + 0xf) & 0xfffffff0; __stop_free_sram = __stop_sram; - /* __start_free_sram2 = __start_sram2; - __stop_free_sram2 = __stop_sram2; */ __start_free_lpsram = __start_lpsram; __stop_free_lpsram = __stop_lpsram; } - -ENTRY(_start) diff --git a/kernel/port/esp32p4/src/interrupt.c b/kernel/port/esp32p4/src/interrupt.c index 47291a7..50b5dae 100644 --- a/kernel/port/esp32p4/src/interrupt.c +++ b/kernel/port/esp32p4/src/interrupt.c @@ -5,50 +5,101 @@ #include "cpu/isr.h" #include "cpu/panic.h" +#include "esp_intmtx.h" #include "isr_ctx.h" #include "log.h" #include "port/hardware_allocation.h" +#include "soc/clic_reg.h" +#include "soc/clic_struct.h" +#include "soc/hp_sys_clkrst_struct.h" +#include "soc/interrupt_core0_reg.h" +#include "soc/pmu_struct.h" // Temporary interrupt context before scheduler. static isr_ctx_t tmp_ctx = {.is_kernel_thread = true}; // Interrupt service routine table. -static isr_t isr_table[32]; +static isr_t isr_table[48]; + +// NOLINTNEXTLINE +extern intmtx_t INTMTX0; +// NOLINTNEXTLINE +extern intmtx_t INTMTX1; +// NOLINTNEXTLINE +extern clic_dev_t CLIC; +// NOLINTNEXTLINE +extern clic_ctl_dev_t CLIC_CTL; + +// Get INTMTX for this CPU. +static inline intmtx_t *intmtx_local() CONST; +static inline intmtx_t *intmtx_local() { + long mhartid; + asm("csrr %0, mhartid" : "=r"(mhartid)); + return mhartid ? &INTMTX1 : &INTMTX0; +} + +// Get INTMTX by CPU number. +static inline intmtx_t *intmtx_cpu(int cpu) CONST; +static inline intmtx_t *intmtx_cpu(int cpu) { + return cpu ? &INTMTX1 : &INTMTX0; +} // Initialise interrupt drivers for this CPU. void irq_init() { + HP_SYS_CLKRST.soc_clk_ctrl2.reg_intrmtx_apb_clk_en = true; + HP_SYS_CLKRST.soc_clk_ctrl0.reg_core0_clic_clk_en = true; + // Install interrupt handler. asm volatile("csrw mstatus, 0"); - asm volatile("csrw mtvec, %0" ::"r"(riscv_interrupt_vector_table)); + asm volatile("csrw mtvec, %0" ::"r"((size_t)&riscv_interrupt_vector_table | 1)); asm volatile("csrw mscratch, %0" ::"r"(&tmp_ctx)); // Disable all internal interrupts. asm volatile("csrw mie, 0"); asm volatile("csrw mideleg, 0"); + + // Enable interrupt matrix. + // intmtx_local()->clock.clk_en = true; + CLIC.int_thresh.val = 0; + + // Set defaults for CLIC. + uint32_t num_int = CLIC.int_info.num_int; + for (uint32_t i = 0; i < num_int; i++) { + CLIC_CTL.irq_ctl[i] = (clic_int_ctl_reg_t){ + .pending = false, + .enable = false, + .attr_shv = false, + .attr_mode = 3, + .attr_trig = false, + .ctl = 127, + }; + } } // Route an external interrupt to an internal interrupt. void irq_ch_route(int ext_irq, int int_irq) { - assert_dev_drop(int_irq > 0 && int_irq < 32); - assert_dev_drop(ext_irq >= 0 && ext_irq < EXT_IRQ_COUNT); - logkf_from_isr(LOG_WARN, "TODO: IRQ route %{d} -> %{d}", ext_irq, int_irq); + assert_dev_drop(int_irq >= 0 && int_irq < 48); + assert_dev_drop(ext_irq >= 0 && ext_irq < ETS_MAX_INTR_SOURCE); + intmtx_local()->map[ext_irq].val = int_irq; } // Set the priority of an internal interrupt, if possible. // 0 is least priority, 255 is most priority. void irq_ch_prio(int int_irq, int raw_prio) { - assert_dev_drop(int_irq > 0 && int_irq < 32); - if (raw_prio < 0 || raw_prio > 255) + assert_dev_drop(int_irq > 0 && int_irq < 48); + if (raw_prio < 0 || raw_prio > 255) { + logkf_from_isr(LOG_WARN, "Invalid IRQ priority %{d}, using 127", raw_prio); raw_prio = 127; - logkf_from_isr(LOG_WARN, "TODO: IRQ prio %{d} = %{d}", int_irq, raw_prio); + } + CLIC_CTL.irq_ctl[int_irq].ctl = raw_prio; } // Acknowledge an interrupt. void irq_ch_ack(int int_irq) { - logkf_from_isr(LOG_WARN, "TODO: IRQ ack %{d}", int_irq); + CLIC_CTL.irq_ctl[int_irq].pending = false; } // Set the interrupt service routine for an interrupt on this CPU. @@ -76,3 +127,23 @@ void riscv_interrupt_handler() { // Acknowledge interrupt. irq_ch_ack(mcause); } + + + +// Enable/disable an internal interrupt. +void irq_ch_enable(int int_irq, bool enable) { + // Set new enable. + CLIC_CTL.irq_ctl[int_irq].enable = enable; + // Dummy read to force update. + CLIC_CTL.irq_ctl[int_irq].val; +} + +// Query whether an internal interrupt is enabled. +bool irq_ch_enabled(int int_irq) { + return CLIC_CTL.irq_ctl[int_irq].enable; +} + +// Query whether an internal interrupt is pending. +bool irq_ch_pending(int int_irq) { + return CLIC_CTL.irq_ctl[int_irq].pending; +} diff --git a/kernel/port/esp32p4/src/memprotect.c b/kernel/port/esp32p4/src/memprotect.c index 7651f01..56ddb95 100644 --- a/kernel/port/esp32p4/src/memprotect.c +++ b/kernel/port/esp32p4/src/memprotect.c @@ -94,8 +94,8 @@ void memprotect_destroy(mpu_ctx_t *ctx) { } // Add a memory protection region. -bool memprotect(mpu_ctx_t *ctx, size_t vaddr, size_t paddr, size_t length, uint32_t flags) { - return vaddr == paddr && riscv_pmp_memprotect(ctx, vaddr, length, flags); +bool memprotect(proc_memmap_t *new_mm, mpu_ctx_t *ctx, size_t vaddr, size_t paddr, size_t length, uint32_t flags) { + return vaddr == paddr && riscv_pmp_memprotect(new_mm, ctx, vaddr, length, flags); } // Commit pending memory protections, if any. diff --git a/kernel/port/esp32p4/src/pmu_init.c b/kernel/port/esp32p4/src/pmu_init.c index 949edde..474038c 100644 --- a/kernel/port/esp32p4/src/pmu_init.c +++ b/kernel/port/esp32p4/src/pmu_init.c @@ -3,13 +3,131 @@ #include "port/pmu_init.h" +#include "badge_strings.h" #include "soc/pmu_struct.h" #include "soc/soc_types.h" +// TODO: More proper method than copying ESP-IDF's values. +static uint32_t const pmu_rom[] = { + 0x00000000, 0xffffffff, 0xffffffff, 0x00000000, 0x00000000, 0x7f800000, 0x02ec0000, 0x010000a0, 0x07801bc0, + 0x08000000, 0xc0007180, 0x00000000, 0x80000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00200000, + 0x00000000, 0x00000000, 0x00000000, 0x31000000, 0x00e00000, 0xc0000000, 0x12800200, 0x07801bc0, 0x30000000, + 0xc0040000, 0x00000000, 0x00000000, 0xe8400000, 0x00000000, 0x00000000, 0x00000000, 0x40000000, 0x00000000, + 0xc0400000, 0x00000000, 0x00000000, 0x40000000, 0x00000000, 0xc0000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x7fbfdfe0, 0x7fbfdfe0, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x01000100, + 0x00000000, 0x00000000, 0x00000000, 0x00020000, 0x00000000, 0x01000080, 0x00000080, 0x00010000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000032, 0x00000a0a, 0x80000000, 0x09000000, 0x80000000, 0x00028000, + 0x00000000, 0x00000000, 0x00000000, 0x00028000, 0x00000000, 0x00000000, 0x00000000, 0x1ff00001, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x08100801, 0x00802000, 0xf8f8407f, + 0xffffffff, 0xffffffff, 0x40200180, 0xf3800000, 0x40200000, 0xa0000000, 0x40200000, 0xa0000000, 0x40200180, + 0x92000000, 0x40200000, 0xa0000000, 0x40200000, 0xa0000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x000003ff, 0x000f0000, 0x00000000, 0x00100000, 0x004b0205, 0x00000000, 0x00190140, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x02303140, 0x00000000, 0xffffffff, 0xffffffff, 0x00000000, 0x00000000, + 0x7f800000, 0x02ec0000, 0x010000a0, 0x07801bc0, 0x08000000, 0xc0007180, 0x00000000, 0x80000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00200000, 0x00000000, 0x00000000, 0x00000000, 0x31000000, 0x00e00000, + 0xc0000000, 0x12800200, 0x07801bc0, 0x30000000, 0xc0040000, 0x00000000, 0x00000000, 0xe8400000, 0x00000000, + 0x00000000, 0x00000000, 0x40000000, 0x00000000, 0xc0400000, 0x00000000, 0x00000000, 0x40000000, 0x00000000, + 0xc0000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x7fbfdfe0, 0x7fbfdfe0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x01000100, 0x00000000, 0x00000000, 0x00000000, 0x00020000, 0x00000000, + 0x01000080, 0x00000080, 0x00010000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000032, 0x00000a0a, + 0x80000000, 0x09000000, 0x80000000, 0x00028000, 0x00000000, 0x00000000, 0x00000000, 0x00028000, 0x00000000, + 0x00000000, 0x00000000, 0x1ff00001, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x08100801, 0x00802000, 0xf8f8407f, 0xffffffff, 0xffffffff, 0x40200180, 0xf3800000, 0x40200000, + 0xa0000000, 0x40200000, 0xa0000000, 0x40200180, 0x92000000, 0x40200000, 0xa0000000, 0x40200000, 0xa0000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x000003ff, 0x000f0000, 0x00000000, 0x00100000, 0x004b0205, + 0x00000000, 0x00190140, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x02303140, 0x00000000, + 0xffffffff, 0xffffffff, 0x00000000, 0x00000000, 0x7f800000, 0x02ec0000, 0x010000a0, 0x07801bc0, 0x08000000, + 0xc0007180, 0x00000000, 0x80000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00200000, 0x00000000, + 0x00000000, 0x00000000, 0x31000000, 0x00e00000, 0xc0000000, 0x12800200, 0x07801bc0, 0x30000000, 0xc0040000, + 0x00000000, 0x00000000, 0xe8400000, 0x00000000, 0x00000000, 0x00000000, 0x40000000, 0x00000000, 0xc0400000, + 0x00000000, 0x00000000, 0x40000000, 0x00000000, 0xc0000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x7fbfdfe0, 0x7fbfdfe0, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x01000100, 0x00000000, + 0x00000000, 0x00000000, 0x00020000, 0x00000000, 0x01000080, 0x00000080, 0x00010000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000032, 0x00000a0a, 0x80000000, 0x09000000, 0x80000000, 0x00028000, 0x00000000, + 0x00000000, 0x00000000, 0x00028000, 0x00000000, 0x00000000, 0x00000000, 0x1ff00001, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x08100801, 0x00802000, 0xf8f8407f, 0xffffffff, + 0xffffffff, 0x40200180, 0xf3800000, 0x40200000, 0xa0000000, 0x40200000, 0xa0000000, 0x40200180, 0x92000000, + 0x40200000, 0xa0000000, 0x40200000, 0xa0000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x000003ff, + 0x000f0000, 0x00000000, 0x00100000, 0x004b0205, 0x00000000, 0x00190140, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x02303140, 0x00000000, 0xffffffff, 0xffffffff, 0x00000000, 0x00000000, 0x7f800000, + 0x02ec0000, 0x010000a0, 0x07801bc0, 0x08000000, 0xc0007180, 0x00000000, 0x80000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00200000, 0x00000000, 0x00000000, 0x00000000, 0x31000000, 0x00e00000, 0xc0000000, + 0x12800200, 0x07801bc0, 0x30000000, 0xc0040000, 0x00000000, 0x00000000, 0xe8400000, 0x00000000, 0x00000000, + 0x00000000, 0x40000000, 0x00000000, 0xc0400000, 0x00000000, 0x00000000, 0x40000000, 0x00000000, 0xc0000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x7fbfdfe0, + 0x7fbfdfe0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x01000100, 0x00000000, 0x00000000, 0x00000000, 0x00020000, 0x00000000, 0x01000080, + 0x00000080, 0x00010000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000032, 0x00000a0a, 0x80000000, + 0x09000000, 0x80000000, 0x00028000, 0x00000000, 0x00000000, 0x00000000, 0x00028000, 0x00000000, 0x00000000, + 0x00000000, 0x1ff00001, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x08100801, 0x00802000, 0xf8f8407f, 0xffffffff, 0xffffffff, 0x40200180, 0xf3800000, 0x40200000, 0xa0000000, + 0x40200000, 0xa0000000, 0x40200180, 0x92000000, 0x40200000, 0xa0000000, 0x40200000, 0xa0000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x000003ff, 0x000f0000, 0x00000000, 0x00100000, 0x004b0205, 0x00000000, + 0x00190140, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x02303140, +}; + // Initialise the power management unit. void pmu_init() { - /* ==== HP system default ==== */ - // TODO. + mem_copy(&PMU, pmu_rom, sizeof(pmu_rom)); } diff --git a/kernel/port/esp32p4/src/port.c b/kernel/port/esp32p4/src/port.c index f56260a..92d5144 100644 --- a/kernel/port/esp32p4/src/port.c +++ b/kernel/port/esp32p4/src/port.c @@ -6,6 +6,7 @@ #include "log.h" #include "port/pmu_init.h" #include "rom/cache.h" +#include "soc/hp_sys_clkrst_struct.h" #include "soc/uart_struct.h" diff --git a/kernel/port/esp_common/include/esp_intmtx.h b/kernel/port/esp_common/include/esp_intmtx.h new file mode 100644 index 0000000..a5e1e90 --- /dev/null +++ b/kernel/port/esp_common/include/esp_intmtx.h @@ -0,0 +1,42 @@ + +// SPDX-License-Identifier: MIT + +#pragma once + +#include "soc/interrupts.h" + +#include + +// Interrupt map register. +typedef union { + struct { + // Mapped interrupt signal. + uint32_t map : 6; + // Reserved. + uint32_t : 26; + }; + // Packed value. + uint32_t val; +} intmtx_map_t; + +// Clock gate register. +typedef union { + struct { + // Clock enabled. + uint32_t clk_en : 1; + // Reserved. + uint32_t : 31; + }; + // Packed value. + uint32_t val; +} intmtx_clk_t; + +// Interrupt matrix. +typedef struct { + // Interrupt map. + intmtx_map_t volatile map[ETS_MAX_INTR_SOURCE]; + // Interrupts pending. + uint32_t volatile pending[(ETS_MAX_INTR_SOURCE + 31) / 32]; + // Clock gate register. + intmtx_clk_t volatile clock; +} intmtx_t; \ No newline at end of file diff --git a/kernel/port/esp_common/src/time.c b/kernel/port/esp_common/src/time.c index 62719a9..7d4c6fc 100644 --- a/kernel/port/esp_common/src/time.c +++ b/kernel/port/esp_common/src/time.c @@ -38,16 +38,18 @@ void time_init() { LP_WDT.wprotect.val = 0x50D83AA1; LP_WDT.config0.val = 0; + TIMERG0.regclk.clk_en = true; + TIMERG1.regclk.clk_en = true; + // Configure interrupts. #ifdef BADGEROS_PORT_esp32c6 -#if TIMER_SYSTICK_NUM / ESP_TIMG_TIMER_COUNT - irq_ch_route(EXT_IRQ_TG1_T0_INTR, INT_CHANNEL_TIMER_ALARM); -#else irq_ch_route(EXT_IRQ_TG0_T0_INTR, INT_CHANNEL_TIMER_ALARM); +#endif +#ifdef BADGEROS_PORT_esp32p4 + irq_ch_route(ETS_TG0_T0_INTR_SOURCE, INT_CHANNEL_TIMER_ALARM); #endif irq_ch_set_isr(INT_CHANNEL_TIMER_ALARM, timer_isr_timer_alarm); irq_ch_enable(INT_CHANNEL_TIMER_ALARM, true); -#endif // Configure SYSTICK timer. timer_set_freq(TIMER_SYSTICK_NUM, TIMER_SYSTICK_RATE); @@ -71,7 +73,7 @@ void timer_set_freq(int timerno, frequency_hz_t freq) { frequency_hz_t base_freq; #ifdef BADGEROS_PORT_esp32p4 // TODO: Determine what selects timer clock source. - base_freq = 80000000; + base_freq = 40000000; #endif #ifdef BADGEROS_PORT_esp32c6 uint32_t clksrc; @@ -102,7 +104,9 @@ void timer_set_freq(int timerno, frequency_hz_t freq) { // Start timer. void timer_start(int timerno) { GET_TIMER_INFO(timerno) - timg->hw_timer[timer].config.tx_en = true; + timg->hw_timer[timer].config.tx_divcnt_rst = false; + timg->hw_timer[timer].config.tx_increase = true; + timg->hw_timer[timer].config.tx_en = true; } // Stop timer. @@ -114,12 +118,12 @@ void timer_stop(int timerno) { // Configure timer alarm. void timer_alarm_config(int timerno, int64_t threshold, bool reset_on_alarm) { GET_TIMER_INFO(timerno) + timg->hw_timer[timer].alarmlo.val = threshold; + timg->hw_timer[timer].alarmhi.val = threshold >> 32; timg_txconfig_reg_t tmp = timg->hw_timer[timer].config; tmp.tx_autoreload = reset_on_alarm; tmp.tx_alarm_en = true; timg->hw_timer[timer].config = tmp; - timg->hw_timer[timer].alarmlo.val = threshold; - timg->hw_timer[timer].alarmhi.val = threshold >> 32; } // Disable timer alarm. diff --git a/kernel/src/housekeeping.c b/kernel/src/housekeeping.c index 06530d7..a7e5c72 100644 --- a/kernel/src/housekeeping.c +++ b/kernel/src/housekeeping.c @@ -49,7 +49,7 @@ int hk_task_time_cmp(void const *a, void const *b) { // Stack for the housekeeping thread. -static uint8_t hk_stack[8192]; +static uint8_t hk_stack[8192] ALIGNED_TO(16); // The housekeeping thread handle. static sched_thread_t *hk_thread; // Task mutex. @@ -74,7 +74,7 @@ void hk_thread_func(void *ignored) { if (task.interval > 0 && task.next_time <= TIMESTAMP_US_MAX - task.interval) { // Repeated tasks get put back into the queue. task.next_time += task.interval; - array_sorted_insert(queue, sizeof(taskent_t), queue_len, &task, hk_task_time_cmp); + array_sorted_insert(queue, sizeof(taskent_t), queue_len - 1, &task, hk_task_time_cmp); } else { // One-time tasks are removed. queue_len--; diff --git a/kernel/src/loading/kbelfx.c b/kernel/src/loading/kbelfx.c index 1039792..6a50b9f 100644 --- a/kernel/src/loading/kbelfx.c +++ b/kernel/src/loading/kbelfx.c @@ -88,7 +88,7 @@ bool kbelfx_seg_alloc(kbelf_inst inst, size_t segs_len, kbelf_segment *segs) { size_t end = segs[i].vaddr_req + segs[i].size; if (end > max_addr) max_addr = end; - logkf(LOG_DEBUG, "Segment %{size;d}: %{size;x} - %{size;x}", i, start, end); + // logkf(LOG_DEBUG, "Segment %{size;d}: %{size;x} - %{size;x}", i, start, end); } size_t vaddr_real = proc_map_raw(NULL, proc, min_addr, max_addr - min_addr, min_align, MEMPROTECT_FLAG_RWX); @@ -100,7 +100,7 @@ bool kbelfx_seg_alloc(kbelf_inst inst, size_t segs_len, kbelf_segment *segs) { segs[i].paddr = segs[i].vaddr_real; segs[i].laddr = segs[i].vaddr_real; segs[i].alloc_cookie = NULL; - logkf(LOG_DEBUG, "Segment %{size;x} mapped to %{size;x}", i, segs[i].vaddr_real); + // logkf(LOG_DEBUG, "Segment %{size;x} mapped to %{size;x}", i, segs[i].vaddr_real); } segs[0].alloc_cookie = (void *)vaddr_real; diff --git a/kernel/src/main.c b/kernel/src/main.c index b599e38..f7d2444 100644 --- a/kernel/src/main.c +++ b/kernel/src/main.c @@ -3,9 +3,11 @@ #include "backtrace.h" #include "badge_err.h" +#include "esp_intmtx.h" #include "filesystem.h" #include "hal/gpio.h" #include "hal/i2c.h" +#include "hal/spi.h" #include "housekeeping.h" #include "interrupt.h" #include "isr_ctx.h" @@ -15,6 +17,7 @@ #include "port/port.h" #include "process/process.h" #include "scheduler/scheduler.h" +#include "soc/timer_group_struct.h" #include "time.h" #include @@ -93,7 +96,7 @@ void basic_runtime_init() { time_init(); // Announce that we're alive. - logk(LOG_INFO, "BadgerOS starting..."); + logk(LOG_INFO, "BadgerOS " BADGEROS_PORT " starting..."); // Kernel memory allocator initialization. kernel_heap_init(); @@ -145,14 +148,13 @@ static void kernel_init() { static void userland_init() { badge_err_t ec = {0}; logk(LOG_INFO, "Kernel initialized"); - logk(LOG_INFO, "Staring init process"); + logk(LOG_INFO, "Starting init process"); pid_t pid = proc_create(&ec); badge_err_assert_always(&ec); assert_dev_drop(pid == 1); proc_start(&ec, pid, "/sbin/init"); badge_err_assert_always(&ec); - while (1) continue; } diff --git a/kernel/src/process/process.c b/kernel/src/process/process.c index 7aef446..d606e8e 100644 --- a/kernel/src/process/process.c +++ b/kernel/src/process/process.c @@ -44,7 +44,6 @@ static bool allow_proc1_death() { // Clean up: the housekeeping task. static void clean_up_from_housekeeping(int taskno, void *arg) { (void)taskno; - logk(LOG_DEBUG, "kaboom"); proc_delete((pid_t)arg); } @@ -261,7 +260,7 @@ sched_thread_t *proc_create_thread_raw_unsafe( // Add the thread to the list. array_insert(process->threads, sizeof(sched_thread_t *), process->threads_len, &thread, process->threads_len); process->threads_len++; - logkf(LOG_DEBUG, "Creating user thread, PC: 0x%{size;x}", entry_point); + // logkf(LOG_DEBUG, "Creating user thread, PC: 0x%{size;x}", entry_point); return thread; } @@ -298,6 +297,7 @@ size_t proc_map_raw(badge_err_t *ec, process_t *proc, size_t vaddr_req, size_t m proc_memmap_t *map = &proc->memmap; if (map->regions_len >= PROC_MEMMAP_MAX_REGIONS) { + logk(LOG_WARN, "Out of regions"); badge_err_set(ec, ELOC_PROCESS, ECAUSE_NOMEM); return 0; } @@ -305,19 +305,13 @@ size_t proc_map_raw(badge_err_t *ec, process_t *proc, size_t vaddr_req, size_t m // Allocate memory to the process. void *base = buddy_allocate(min_size, BLOCK_TYPE_USER, 0); if (!base) { + logk(LOG_WARN, "Out of vmem"); badge_err_set(ec, ELOC_PROCESS, ECAUSE_NOMEM); return 0; } size_t size = buddy_get_size(base); mem_set(base, 0, size); - // Update memory protection. - if (!memprotect(&map->mpu_ctx, (size_t)base, (size_t)base, size, flags & MEMPROTECT_FLAG_RWX)) { - buddy_deallocate(base); - badge_err_set(ec, ELOC_PROCESS, ECAUSE_NOMEM); - return 0; - } - // Account the process's memory. map->regions[map->regions_len] = (proc_memmap_ent_t){ .base = (size_t)base, @@ -327,6 +321,20 @@ size_t proc_map_raw(badge_err_t *ec, process_t *proc, size_t vaddr_req, size_t m }; map->regions_len++; proc_memmap_sort_raw(map); + + // Update memory protection. + if (!memprotect(map, &map->mpu_ctx, (size_t)base, (size_t)base, size, flags & MEMPROTECT_FLAG_RWX)) { + for (size_t i = 0; i < map->regions_len; i++) { + if (map->regions[i].base == (size_t)base) { + array_remove(&map->regions[0], sizeof(map->regions[0]), map->regions_len, NULL, i); + break; + } + } + map->regions_len--; + buddy_deallocate(base); + badge_err_set(ec, ELOC_PROCESS, ECAUSE_NOMEM); + return 0; + } memprotect_commit(&map->mpu_ctx); logkf(LOG_INFO, "Mapped %{size;d} bytes at %{size;x} to process %{d}", size, base, proc->pid); @@ -339,12 +347,14 @@ void proc_unmap_raw(badge_err_t *ec, process_t *proc, size_t base) { proc_memmap_t *map = &proc->memmap; for (size_t i = 0; i < map->regions_len; i++) { if (map->regions[i].base == base) { - assert_dev_drop(memprotect(&map->mpu_ctx, base, base, map->regions[i].size, 0)); - memprotect_commit(&map->mpu_ctx); - buddy_deallocate((void *)base); + proc_memmap_ent_t region = map->regions[i]; array_remove(&map->regions[0], sizeof(map->regions[0]), map->regions_len, NULL, i); map->regions_len--; + assert_dev_keep(memprotect(map, &map->mpu_ctx, base, base, region.size, 0)); + memprotect_commit(&map->mpu_ctx); + buddy_deallocate((void *)base); badge_err_set_ok(ec); + logkf(LOG_INFO, "Unmapped %{size;d} bytes at %{size;x} from process %{d}", region.size, base, proc->pid); return; } } diff --git a/kernel/src/scheduler.c b/kernel/src/scheduler.c index 836b3e3..bdde37f 100644 --- a/kernel/src/scheduler.c +++ b/kernel/src/scheduler.c @@ -328,7 +328,6 @@ void sched_request_switch_from_isr(void) { isr_ctx_switch_set(&next_thread->kernel_isr_ctx); } else { if (proc_getflags_raw(next_thread->process) & PROC_EXITING) { - logkf_from_isr(LOG_WARN, "Removing thread %{zx}", next_thread); // If a thread's process is exiting, suspend it and get the next one instead. reset_flag(next_thread->flags, THREAD_RUNNING); goto pop_thread; diff --git a/lib/kbelf b/lib/kbelf index d058e5f..be39c38 160000 --- a/lib/kbelf +++ b/lib/kbelf @@ -1 +1 @@ -Subproject commit d058e5f739210b570fb6c0d00c1d794582ee4c6a +Subproject commit be39c38fadd8b50b7407515101b015c68d8377fa diff --git a/tools/pack-image.py b/tools/pack-image.py index b61d514..14da0c1 100755 --- a/tools/pack-image.py +++ b/tools/pack-image.py @@ -19,7 +19,7 @@ def patchElf(fd): # Initialise checksum. xsum_state = 0xEF - fd.seek(1, 0) + fd.seek(1, os.SEEK_SET) seg_num = fd.read(1)[0] @@ -29,7 +29,7 @@ def readword(): # Compute checksum. - fd.seek(24) + fd.seek(24, os.SEEK_SET) for _ in range(seg_num): seg_laddr = readword() seg_len = readword() @@ -41,7 +41,6 @@ def readword(): fd.write(bytes([xsum_state])) def main(): - parser = argparse.ArgumentParser() parser.add_argument("input", type=Path) diff --git a/tools/ramfs-gen.py b/tools/ramfs-gen.py index ce5abf3..ae15342 100755 --- a/tools/ramfs-gen.py +++ b/tools/ramfs-gen.py @@ -36,15 +36,16 @@ def add_rom(path, virtpath, name): global roms, files infd = open(path, "rb") data = infd.read() - roms += "static uint8_t const {}[] = {{\n ".format(name) + roms += "uint8_t const {}[] = {{\n ".format(name) for byte in data: roms += "0x{:02x},".format(byte) roms += "\n};\n" - roms += "static size_t const {}_len = {};\n".format(name, len(data)) + roms += "size_t const {}_len = {};\n".format(name, len(data)) files += " fd = fs_open(&ec, \"{}\", OFLAGS_CREATE | OFLAGS_WRITEONLY);\n".format(escape(virtpath)) files += " badge_err_assert_dev(&ec);\n" - files += " fs_write(&ec, fd, {}, {}_len);\n".format(name, name) + files += " len = fs_write(&ec, fd, {}, {}_len);\n".format(name, name) files += " badge_err_assert_dev(&ec);\n" + files += " assert_dev_drop(len == {}_len);\n".format(name) files += " fs_close(&ec, fd);\n" files += " badge_err_assert_dev(&ec);\n" infd.close() @@ -67,6 +68,7 @@ def add_dir(path, virtpath): outfd.write("void {}() {{\n".format(sys.argv[3])) outfd.write(" badge_err_t ec = {0};\n") outfd.write(" file_t fd;\n") +outfd.write(" fileoff_t len;\n") outfd.write(dirs) outfd.write(files) outfd.write("}\n")