diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 833965b2e..023eeef17 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -14,9 +14,13 @@ on:
       - 'bpaczek/*'
 
 
+
 jobs:
   call-ci:
-    uses: bartekpaczek/phoenix-rtos-project-fork/.github/workflows/ci-submodule.yml@bpaczek/misra-ci
+    uses: bartekpaczek/phoenix-rtos-project-fork/.github/workflows/ci-submodule.yml@master
     secrets: inherit
     with:
       build_params: all tests
+
+
+
diff --git a/hal/aarch64/zynqmp/zynqmp.c b/hal/aarch64/zynqmp/zynqmp.c
index f96c8a9fd..02e0eebad 100644
--- a/hal/aarch64/zynqmp/zynqmp.c
+++ b/hal/aarch64/zynqmp/zynqmp.c
@@ -63,17 +63,6 @@ int _interrupts_gicv2_classify(unsigned int irqn)
 }
 
 
-static int _zynqmp_getActiveBitShift(int dev)
-{
-	if ((dev >= pctl_devclock_lpd_usb3_dual) && (dev <= pctl_devclock_lpd_usb1_bus)) {
-		return 25;
-	}
-	else {
-		return 24;
-	}
-}
-
-
 static int _zynqmp_setBasicGenerator(volatile u32 *reg, int dev, char src, char div0, char div1, char active)
 {
 	u32 val = src;
@@ -84,7 +73,7 @@ static int _zynqmp_setBasicGenerator(volatile u32 *reg, int dev, char src, char
 		val &= 0x3;
 	}
 
-	val |= ((div0 & 0x3f) << 8) | ((div1 & 0x3f) << 16) | (active << _zynqmp_getActiveBitShift(dev));
+	val |= ((div0 & 0x3f) << 8) | ((div1 & 0x3f) << 16) | (active << 24);
 	if (dev == pctl_devclock_lpd_cpu_r5) {
 		/* According to docs turning this bit off could lead to system hang - ensure it is on */
 		val |= (1 << 24);
@@ -111,13 +100,13 @@ int _zynqmp_setDevClock(int dev, char src, char div0, char div1, char active)
 }
 
 
-static int _zynqmp_getBasicGenerator(int dev, volatile u32 *reg, char *src, char *div0, char *div1, char *active)
+static int _zynqmp_getBasicGenerator(volatile u32 *reg, char *src, char *div0, char *div1, char *active)
 {
 	u32 val = *reg;
 	*src = val & 0x7;
 	*div0 = (val >> 8) & 0x3f;
 	*div1 = (val >> 16) & 0x3f;
-	*active = val >> _zynqmp_getActiveBitShift(dev);
+	*active = val >> 24;
 	return 0;
 }
 
@@ -126,11 +115,11 @@ int _zynqmp_getDevClock(int dev, char *src, char *div0, char *div1, char *active
 {
 	if ((dev >= pctl_devclock_lpd_usb3_dual) && (dev <= pctl_devclock_lpd_timestamp)) {
 		unsigned regOffset = (dev - pctl_devclock_lpd_usb3_dual) + crl_apb_usb3_dual_ref_ctrl;
-		return _zynqmp_getBasicGenerator(dev, zynq_common.crl_apb + regOffset, src, div0, div1, active);
+		return _zynqmp_getBasicGenerator(zynq_common.crl_apb + regOffset, src, div0, div1, active);
 	}
 	else if ((dev >= pctl_devclock_fpd_acpu) && (dev <= pctl_devclock_fpd_dbg_tstmp)) {
 		unsigned regOffset = (dev - pctl_devclock_fpd_acpu) + crf_apb_acpu_ctrl;
-		return _zynqmp_getBasicGenerator(dev, zynq_common.crf_apb + regOffset, src, div0, div1, active);
+		return _zynqmp_getBasicGenerator(zynq_common.crf_apb + regOffset, src, div0, div1, active);
 	}
 
 	return -1;
@@ -413,7 +402,7 @@ int hal_platformctl(void *ptr)
 		case pctl_devclock:
 			if (data->action == pctl_set)
 				ret = _zynqmp_setDevClock(data->devclock.dev, data->devclock.src, data->devclock.div0, data->devclock.div1, data->devclock.active);
-			else if (data->action == pctl_get)
+			else if (data->action == pctl_set)
 				ret = _zynqmp_getDevClock(data->devclock.dev, &data->devclock.src, &data->devclock.div0, &data->devclock.div1, &data->devclock.active);
 			break;
 
diff --git a/hal/arm/scs.c b/hal/arm/scs.c
index 1a92afe8e..2ca8c7e00 100644
--- a/hal/arm/scs.c
+++ b/hal/arm/scs.c
@@ -95,18 +95,18 @@ static struct {
 } scs_common;
 
 
-void _hal_scsIRQSet(u32 irqn, u8 state)
+void _hal_scsIRQSet(s8 irqn, u8 state)
 {
 	volatile u32 *ptr = (state != 0) ? scs_common.scs->iser : scs_common.scs->icer;
 
-	*(ptr + (irqn / 32)) = 1u << (irqn % 32);
+	*(ptr + ((u8)irqn >> 5)) = 1u << (irqn & 0x1f);
 
 	hal_cpuDataSyncBarrier();
 	hal_cpuInstrBarrier();
 }
 
 
-void _hal_scsIRQPrioritySet(u32 irqn, u32 priority)
+void _hal_scsIRQPrioritySet(s8 irqn, u32 priority)
 {
 	volatile u8 *ptr = (volatile u8 *)scs_common.scs->ip;
 
@@ -117,28 +117,28 @@ void _hal_scsIRQPrioritySet(u32 irqn, u32 priority)
 }
 
 
-void _hal_scsIRQPendingSet(u32 irqn)
+void _hal_scsIRQPendingSet(s8 irqn)
 {
 	volatile u32 *ptr = scs_common.scs->ispr;
 
-	*(ptr + (irqn / 32)) = 1u << (irqn % 32);
+	*(ptr + ((u8)irqn >> 5)) = 1u << (irqn & 0x1f);
 
 	hal_cpuDataSyncBarrier();
 	hal_cpuInstrBarrier();
 }
 
 
-int _hal_scsIRQPendingGet(u32 irqn)
+int _hal_scsIRQPendingGet(s8 irqn)
 {
-	volatile u32 *ptr = &scs_common.scs->ispr[irqn / 32];
-	return ((*ptr & (1 << (irqn % 32))) != 0) ? 1 : 0;
+	volatile u32 *ptr = &scs_common.scs->ispr[(u8)irqn >> 5];
+	return ((*ptr & (1 << (irqn & 0x1f))) != 0) ? 1 : 0;
 }
 
 
-int _hal_scsIRQActiveGet(u32 irqn)
+int _hal_scsIRQActiveGet(s8 irqn)
 {
-	volatile u32 *ptr = &scs_common.scs->iabr[irqn / 32];
-	return ((*ptr & (1 << (irqn % 32))) != 0) ? 1 : 0;
+	volatile u32 *ptr = &scs_common.scs->iabr[(u8)irqn >> 5];
+	return ((*ptr & (1 << (irqn & 0x1f))) != 0) ? 1 : 0;
 }
 
 
@@ -162,7 +162,7 @@ u32 _hal_scsPriorityGroupingGet(void)
 }
 
 
-void _hal_scsExceptionPrioritySet(u32 excpn, u32 priority)
+void _hal_scsExceptionPrioritySet(s8 excpn, u32 priority)
 {
 	volatile u8 *ptr = (u8 *)&scs_common.scs->shpr1 + excpn - 4;
 
@@ -171,7 +171,7 @@ void _hal_scsExceptionPrioritySet(u32 excpn, u32 priority)
 }
 
 
-u32 _imxrt_scsExceptionPriorityGet(u32 excpn)
+u32 _imxrt_scsExceptionPriorityGet(s8 excpn)
 {
 	volatile u8 *ptr = (u8 *)&scs_common.scs->shpr1 + excpn - 4;
 
@@ -409,12 +409,6 @@ u32 _hal_scsSystickGetCount(u8 *overflow_out)
 }
 
 
-u32 _hal_scsGetDefaultFPSCR(void)
-{
-	return scs_common.scs->fpdscr;
-}
-
-
 void _hal_scsInit(void)
 {
 	scs_common.scs = (void *)0xe000e000;
diff --git a/hal/arm/scs.h b/hal/arm/scs.h
index 1aea81d3e..b30a2a0ac 100644
--- a/hal/arm/scs.h
+++ b/hal/arm/scs.h
@@ -21,19 +21,19 @@
 #include "hal/types.h"
 
 
-void _hal_scsIRQSet(u32 irqn, u8 state);
+void _hal_scsIRQSet(s8 irqn, u8 state);
 
 
-void _hal_scsIRQPrioritySet(u32 irqn, u32 priority);
+void _hal_scsIRQPrioritySet(s8 irqn, u32 priority);
 
 
-void _hal_scsIRQPendingSet(u32 irqn);
+void _hal_scsIRQPendingSet(s8 irqn);
 
 
-int _hal_scsIRQPendingGet(u32 irqn);
+int _hal_scsIRQPendingGet(s8 irqn);
 
 
-int _hal_scsIRQActiveGet(u32 irqn);
+int _hal_scsIRQActiveGet(s8 irqn);
 
 
 void _hal_scsPriorityGroupingSet(u32 group);
@@ -42,10 +42,10 @@ void _hal_scsPriorityGroupingSet(u32 group);
 u32 _hal_scsPriorityGroupingGet(void);
 
 
-void _hal_scsExceptionPrioritySet(u32 excpn, u32 priority);
+void _hal_scsExceptionPrioritySet(s8 excpn, u32 priority);
 
 
-u32 _imxrt_scsExceptionPriorityGet(u32 excpn);
+u32 _imxrt_scsExceptionPriorityGet(s8 excpn);
 
 
 void _hal_scsSystemReset(void);
@@ -91,11 +91,6 @@ void _hal_scsSystickInit(u32 load);
 u32 _hal_scsSystickGetCount(u8 *overflow_out);
 
 
-/* Get the value to use for FPSCR when creating a new context.
- * It is stored in FPDSCR register. */
-u32 _hal_scsGetDefaultFPSCR(void);
-
-
 void _hal_scsInit(void);
 
 
diff --git a/hal/armv8m/arch/cpu.h b/hal/armv8m/arch/cpu.h
index e7d749b79..8feabe2fa 100644
--- a/hal/armv8m/arch/cpu.h
+++ b/hal/armv8m/arch/cpu.h
@@ -28,31 +28,12 @@
 #define SIZE_KSTACK (4U * SIZE_PAGE)
 #endif
 
-/* If KERNEL_FPU_SUPPORT == 0, FPU/MVE context handling in the kernel will be disabled.
- * This flag must be set externally to 1 by the build system to enable FPU handling. */
-#ifndef KERNEL_FPU_SUPPORT
-#define KERNEL_FPU_SUPPORT 0
-#endif
-
 /* values based on EXC_RETURN requirements */
-#define EXC_RETURN_SPSEL (1u << 2) /* 1 - was using process SP, 0 - was using main SP */
-#define EXC_RETURN_FTYPE (1u << 4) /* 1 - standard frame, 0 - frame with FPU state */
-
-#define DEFAULT_PSR 0x01000000
-
-#if KERNEL_FPU_SUPPORT
-#define RET_HANDLER_MSP 0xffffffe1u
-#define RET_THREAD_MSP  0xffffffe9u
-#define RET_THREAD_PSP  0xffffffedu
-#define HWCTXSIZE       (8 + 18)
-#define USERCONTROL     0x7u
-#else
 #define RET_HANDLER_MSP 0xfffffff1u
 #define RET_THREAD_MSP  0xfffffff9u
 #define RET_THREAD_PSP  0xfffffffdu
 #define HWCTXSIZE       8
 #define USERCONTROL     0x3u
-#endif
 
 #ifndef __ASSEMBLY__
 
@@ -87,7 +68,7 @@ typedef struct {
 
 typedef struct _cpu_context_t {
 	u32 savesp_s;
-	u32 fpuctx; /* If KERNEL_FPU_SUPPORT == 0 fpuctx is unused, otherwise it is the value of FPCAR at exception entry. */
+	u32 padding;
 
 	/* Saved by ISR */
 	u32 psp;
@@ -104,48 +85,9 @@ typedef struct _cpu_context_t {
 	u32 msp;
 	u32 pad0;
 
-#if KERNEL_FPU_SUPPORT
-	u32 s16;
-	u32 s17;
-	u32 s18;
-	u32 s19;
-	u32 s20;
-	u32 s21;
-	u32 s22;
-	u32 s23;
-	u32 s24;
-	u32 s25;
-	u32 s26;
-	u32 s27;
-	u32 s28;
-	u32 s29;
-	u32 s30;
-	u32 s31;
-#endif
-
 	/* Saved by hardware */
 	cpu_hwContext_t hwctx;
 
-#if KERNEL_FPU_SUPPORT
-	u32 s0;
-	u32 s1;
-	u32 s2;
-	u32 s3;
-	u32 s4;
-	u32 s5;
-	u32 s6;
-	u32 s7;
-	u32 s8;
-	u32 s9;
-	u32 s10;
-	u32 s11;
-	u32 s12;
-	u32 s13;
-	u32 s14;
-	u32 s15;
-	u32 fpscr;
-	u32 vpr;
-#endif
 } cpu_context_t;
 
 
diff --git a/hal/armv8m/cpu.c b/hal/armv8m/cpu.c
index 5c4fefc0b..5b620fe9c 100644
--- a/hal/armv8m/cpu.c
+++ b/hal/armv8m/cpu.c
@@ -111,19 +111,11 @@ int hal_cpuCreateContext(cpu_context_t **nctx, void (*start)(void *harg), void *
 	ctx->hwctx.r12 = 0xcccccccc;
 	ctx->hwctx.lr = 0xeeeeeeee;
 	ctx->hwctx.pc = (u32)start;
-	ctx->hwctx.psr = DEFAULT_PSR;
+	ctx->hwctx.psr = 0x01000000;
 	if (ustack != NULL) {
-#if KERNEL_FPU_SUPPORT
-		ctx->fpuctx = ctx->psp + (8 * sizeof(u32)); /* Must point to s0 in hw-saved context */
-		ctx->fpscr = _hal_scsGetDefaultFPSCR();
-#endif
 		ctx->irq_ret = RET_THREAD_PSP;
 	}
 	else {
-#if KERNEL_FPU_SUPPORT
-		ctx->fpuctx = (u32)(&ctx->hwctx) + (8 * sizeof(u32)); /* Must point to s0 in hw-saved context */
-		ctx->fpscr = _hal_scsGetDefaultFPSCR();
-#endif
 		ctx->irq_ret = RET_THREAD_MSP;
 	}
 
@@ -152,7 +144,7 @@ int hal_cpuPushSignal(void *kstack, void (*handler)(void), cpu_context_t *signal
 	signalCtx->hwctx.pc = (u32)handler;
 
 	/* Set default PSR, clear potential ICI/IT flags */
-	signalCtx->hwctx.psr = DEFAULT_PSR;
+	signalCtx->hwctx.psr = 0x01000000;
 
 	if (src == SIG_SRC_SCHED) {
 		/* We'll be returning through interrupt dispatcher,
@@ -225,12 +217,7 @@ char *hal_cpuInfo(char *info)
 char *hal_cpuFeatures(char *features, unsigned int len)
 {
 	unsigned int n = 0;
-#if KERNEL_FPU_SUPPORT
-	if ((len - n) > 5) {
-		hal_strcpy(features + n, "FPU, ");
-		n += 5;
-	}
-#else
+#ifdef CPU_NRF91
 	if ((len - n) > 8) {
 		hal_strcpy(features + n, "softfp, ");
 		n += 8;
@@ -270,9 +257,9 @@ void hal_cpuReboot(void)
 }
 
 
+/* TODO: add implementation */
 void hal_cleanDCache(ptr_t start, size_t len)
 {
-	_hal_scsDCacheCleanAddr((void *)start, len);
 }
 
 
diff --git a/hal/armv8m/exceptions.c b/hal/armv8m/exceptions.c
index 7e125501c..73fd76d83 100644
--- a/hal/armv8m/exceptions.c
+++ b/hal/armv8m/exceptions.c
@@ -19,8 +19,6 @@
 #include "hal/string.h"
 #include "config.h"
 
-#define SIZE_FPUCTX (18 * sizeof(u32))
-
 #define CFSR  ((volatile u32 *)0xe000ed28)
 #define MMFAR ((volatile u32 *)0xe000ed34)
 #define BFAR  ((volatile u32 *)0xe000ed38)
@@ -50,20 +48,18 @@ void hal_exceptionsDumpContext(char *buff, exc_context_t *ctx, unsigned int n)
 	};
 	size_t i = 0;
 	u32 msp = (u32)ctx + sizeof(*ctx);
-	const u32 fpu_hwctx_size = ((ctx->excret & EXC_RETURN_FTYPE) == 0) ? SIZE_FPUCTX : 0;
 	u32 psp = ctx->psp;
 	u32 cfsr, far;
 	cpu_hwContext_t *hwctx;
 
 	/* If we came from userspace HW ctx in on psp stack (according to EXC_RETURN) */
-	if ((ctx->excret & EXC_RETURN_SPSEL) != 0) {
+	if ((ctx->excret & (1u << 2)) != 0) {
 		hwctx = (void *)ctx->psp;
 		msp -= sizeof(cpu_hwContext_t);
-		psp += sizeof(cpu_hwContext_t) + fpu_hwctx_size;
+		psp += sizeof(cpu_hwContext_t);
 	}
 	else {
 		hwctx = &ctx->mspctx;
-		msp += fpu_hwctx_size;
 	}
 
 	n &= 0xf;
@@ -147,7 +143,7 @@ ptr_t hal_exceptionsPC(exc_context_t *ctx)
 {
 	cpu_hwContext_t *hwctx;
 
-	if ((ctx->excret & EXC_RETURN_SPSEL) != 0) {
+	if ((ctx->excret & (1u << 2)) != 0) {
 		hwctx = (void *)ctx->psp;
 	}
 	else {
diff --git a/hal/armv8m/pmap.c b/hal/armv8m/pmap.c
index e66221bef..504fcb8c4 100644
--- a/hal/armv8m/pmap.c
+++ b/hal/armv8m/pmap.c
@@ -53,7 +53,6 @@ static struct {
 	volatile u32 *mpu;
 	unsigned int kernelCodeRegion;
 	spinlock_t lock;
-	int mpu_enabled;
 } pmap_common;
 
 
@@ -76,10 +75,6 @@ static unsigned int pmap_map2region(unsigned int map)
 	int i;
 	unsigned int mask = 0;
 
-	if (pmap_common.mpu_enabled == 0) {
-		return 1;
-	}
-
 	for (i = 0; i < sizeof(syspage->hs.mpu.map) / sizeof(*syspage->hs.mpu.map); ++i) {
 		if (map == syspage->hs.mpu.map[i]) {
 			mask |= (1 << i);
@@ -92,12 +87,7 @@ static unsigned int pmap_map2region(unsigned int map)
 
 int pmap_addMap(pmap_t *pmap, unsigned int map)
 {
-	unsigned int rmask;
-	if (pmap_common.mpu_enabled == 0) {
-		return 0;
-	}
-
-	rmask = pmap_map2region(map);
+	unsigned int rmask = pmap_map2region(map);
 	if (rmask == 0) {
 		return -1;
 	}
@@ -112,9 +102,6 @@ void pmap_switch(pmap_t *pmap)
 {
 	unsigned int i, cnt = syspage->hs.mpu.allocCnt;
 	spinlock_ctx_t sc;
-	if (pmap_common.mpu_enabled == 0) {
-		return;
-	}
 
 	if (pmap != NULL) {
 		hal_spinlockSet(&pmap_common.lock, &sc);
@@ -166,11 +153,6 @@ int pmap_isAllowed(pmap_t *pmap, const void *vaddr, size_t size)
 	if ((map == NULL) || (addr_end > map->end) || addr_overflowed) {
 		return 0;
 	}
-
-	if (pmap_common.mpu_enabled == 0) {
-		return 1;
-	}
-
 	rmask = pmap_map2region(map->id);
 
 	return ((pmap->regions & rmask) != 0) ? 1 : 0;
@@ -223,21 +205,9 @@ void _pmap_init(pmap_t *pmap, void **vstart, void **vend)
 	/* Initial size of kernel map */
 	pmap->end = (void *)((addr_t)&__bss_start + 32 * 1024);
 
-	/* Enable all regions for kernel */
-	pmap->regions = (1 << cnt) - 1;
-
 	/* Configure MPU */
 	pmap_common.mpu = MPU_BASE;
 
-	hal_spinlockCreate(&pmap_common.lock, "pmap");
-	if (cnt == 0) {
-		pmap_common.mpu_enabled = 0;
-		pmap_common.kernelCodeRegion = 0;
-		return;
-	}
-
-	pmap_common.mpu_enabled = 1;
-
 	/* Disable MPU just in case */
 	*(pmap_common.mpu + mpu_ctrl) &= ~1;
 	hal_cpuDataMemoryBarrier();
@@ -288,4 +258,6 @@ void _pmap_init(pmap_t *pmap, void **vstart, void **vend)
 	}
 
 	pmap_common.kernelCodeRegion = ikregion;
+
+	hal_spinlockCreate(&pmap_common.lock, "pmap");
 }
diff --git a/hal/armv8m/stm32/_init.S b/hal/armv8m/stm32/_init.S
index 817796424..b11f3e7da 100644
--- a/hal/armv8m/stm32/_init.S
+++ b/hal/armv8m/stm32/_init.S
@@ -17,24 +17,7 @@
 
 #include <arch/cpu.h>
 
-#define SCS_BASE  0xe000e000u
-#define SCS_CPACR 0xd88u
-#define SCS_ICSR  0xd04u
-#define SCS_VTOR  0xd08u
-#define SCS_FPCCR 0xf34u
-#define SCS_FPCAR 0xf38u
-
-
-#if KERNEL_FPU_SUPPORT
-#define HWCTX_SIZE (((8 + 18) * 4))
-#define ISRCTX_SIZE ((14 + 16) * 4)
-/* If CPU is running in secure mode and using FPU we need to set SFPA bit in CONTROL */
-#define CONTROL_SFPA (1 << 3)
-#else
-#define HWCTX_SIZE (8 * 4)
-#define ISRCTX_SIZE (14 * 4)
-#define CONTROL_SFPA 0
-#endif
+#define SCS_BASE 0xe000e000u
 
 .syntax unified
 
@@ -86,51 +69,27 @@ _start:
 	str r9, [r8]
 
 	/* Init vector table pointer */
-	ldr r7, =SCS_BASE
-	adr r1, _init_vectors
-	str r1, [r7, #SCS_VTOR]
+	ldr r0, =SCS_BASE
+	ldr r1, =_init_vectors
+	str r1, [r0, #0xd08] /* VTOR */
 	isb
 	dmb
 
 	/* Init MSP to a first value in _init_vectors (.bss end + 1024 + 256) */
 	ldr r0, [r1]
-	bic r0, #7
+	bic r0, 7
 	msr msp, r0
 	/* These registers are used for hardware checking of stack limits.
 	 * This feature is currently not implemented in our kernel for ARMv8-M,
 	 * setting them to 0 essentially disables this feature. */
-	movs r0, #0
+	mov r0, #0
 	msr msplim, r0
 	msr psplim, r0
 	isb
 
-#if KERNEL_FPU_SUPPORT
-	/* Enable FPU and MVE. We have a function _hal_scsFPUSet, but we can't use it yet
-	 * and we have to untrap FPU early - _stm32_init may get compiled to code with MVE. */
-	mov r0, #(0x3 << 20)
-	str r0, [r7, #SCS_CPACR]
-	isb
-
-	/* Enable lazy floating-point state preservation */
-	ldr r0, [r7, #SCS_FPCCR]
-	orr r0, #(1 << 30)
-	str r0, [r7, #SCS_FPCCR]
-	isb
-#else
-	/* Trap FPU and MVE, in the chosen setup kernel doesn't support them. */
-	movs r0, #0
-	str r0, [r7, #SCS_CPACR]
-	isb
-#endif
-
-	movs r0, #0 /* Switch to main stack, privileged mode */
-	msr control, r0
-
 	bl _stm32_init
 
-	bl main
-	/* If main() returns, wait here infinitely for thread to be rescheduled */
-	b .
+	b main
 .size _start, .-_start
 .ltorg
 
@@ -139,109 +98,73 @@ _start:
 .type _syscallend, %function
 
 _syscall_dispatch:
-	mov r3, sp              /* Store MSP for later */
-	sub sp, sp, #HWCTX_SIZE /* Reserve space for copying hardware-stacked context */
-	mov r1, sp              /* Address to copy hardware-stacked context */
-	mrs r0, psp
+	mov r0, #0
+	msr control, r0
+	isb
+
+	mov r0, sp
+
+	/* Prepare context on kernel stack */
+	sub sp, sp, #((14 * 4) + (8 * 4))
+	str sp, [sp]
 
-#if KERNEL_FPU_SUPPORT
-	vpush {s16-s31}
-#endif
-	str r3, [sp, #-8]! /* store msp, skip pad0 */
+	add r1, sp, #12
 	mov r12, #0
-	push {r0, r4-r12} /* store psp, r4~r11, irq_ret */
+	stmia r1!, {r4-r12}
+	str r0, [r1], #8 /* msp */
+	mov r10, r1 /* addr to store hw-saved ctx */
 
-	mov r10, r1
-	/* Registers:
-	 * r0    - hw-stacked context on process stack
-	 * r1~r8 - userspace r0~r3, r12, lr, pc, psr
-	 * r10   - hw-stacked context on kernel stack
-	 */
+	mrs r0, psp
+	str r0, [sp, #8]
 
+	/* Load hardware saved registers from user stack */
 	ldmia r0!, {r1-r8}
-	orr r7, r7, #1 /* fix PC LSB not being set */
-	stmia r10!, {r1-r8}
-	/* r5~r8 free for use */
-
-#if KERNEL_FPU_SUPPORT
-	/* copy s0-s15, fpscr, vpr */
-	vldmia r0!, {s0-s17}
-	vstmia r10!, {s0-s17}
-#endif
-	/* r10 free for use */
-
-	sub sp, sp, #8
-	str sp, [sp] /* savesp */
-
-#if KERNEL_FPU_SUPPORT
-	ldr r6, =SCS_BASE
-	ldr r5, [r6, #SCS_FPCAR]
-	str r5, [sp, #4] /* fpuctx */
-
-	/* At this point full context is saved on kernel stack.
-	 * Point FPCAR to the s0 register in saved context. */
-	add r5, sp, #(ISRCTX_SIZE + (8 * 4))
-	str r5, [r6, #SCS_FPCAR]
-	dmb
-	isb
-#endif
 
-	/* syscalls_dispatch expects arguments to be on the user stack.
-	 * Push userspace r0~r3 onto ustack. */
-	stmdb r0!, {r1-r4}
+	/* Fix PC LSB not being set */
+	orr r7, r7, #1
+
+	/* Store hardware saved registers on kernel stack */
+	add r10, r10, #(5 * 4)
+	stm r10, {r6-r8} /* lr, pc, psr */
 
-	/* Prepare arguments for function call:
-	 * void *syscalls_dispatch(int n, char *ustack, cpu_context_t *ctx) */
+	/* Copy arguments back to the user stack */
+	stmdb r0!, {r1-r4}
 	mov r1, r0
 	mov r2, sp
-	ldrb r0, [r7, #-3] /* get syscall number by examining SVC instruction */
+	ldrb r0, [r7, #-3]
 
-	/* Prepare pseudo context that will call syscalls_dispatch */
-	adr r5, _syscallend
+	/* Prepare pseudo context */
+	mov r7, #0x01000000
 	ldr r6, =syscalls_dispatch
-	ldr r7, =DEFAULT_PSR
-	push {r0-r7} /* r0~r3, r12, lr, pc, psr */
+	ldr r5, =_syscallend
+	stmdb sp!, {r0-r7} /* PSR, PC, LR, R12, R3, R2, R1, R0 */
 
-	/* Set Thread mode to privileged execution */
-	mrs r0, control
-	bic r0, r0, #0x3 /* Switch to main stack, privileged mode */
-	msr control, r0
-	isb
-
-	/* Exit Handler mode to kernel Thread mode, set FType because our pseudo-context doesn't have FPU */
-	ldr r0, =(RET_THREAD_MSP | EXC_RETURN_FTYPE)
-	bx r0
+	/* Exit handler mode to kernel thread mode */
+	ldr lr,=RET_THREAD_MSP
+	bx lr
 
 _syscallend:
 	/* return value in r0 */
-	add sp, sp, #(2 * 4)  /* Skip over savesp, fpuctx */
-	pop {r1, r4-r11, lr}  /* load psp, restore r4~r11, lr (ignored) */
-	adds r1, r1, #HWCTX_SIZE
-	msr psp, r1
+	ldr lr, [sp, #8] /* psp */
+	add lr, lr, #(8 * 4)
+	msr psp, lr
 	isb
 
-	/* Get lr, pc from the context on stack */
-	ldrd lr, r3, [sp, #(ISRCTX_SIZE - (12 * 4) + (5 * 4))]
-
-#if KERNEL_FPU_SUPPORT
-	add sp, sp, #(2 * 4) /* Skip over irq_ret, msp, pad0 */
-	vpop {s16-s31}
-	add sp, sp, #(8 * 4) /* Skip over hwctx (only the integer part) */
-	vpop {s0-s15}
-	pop {r1-r2}
-	vmsr fpscr, r1
-	vmsr vpr, r2
-#else
-	add sp, sp, #((2 * 4) + HWCTX_SIZE) /* Skip over irq_ret, msp, pad0 and hwctx */
-#endif
+	add r10, sp, #((14 * 4) + (5 * 4))
+	ldm r10, {r2,r3} /* lr, pc */
+
+	add lr, sp, #12
+	ldmia lr!, {r4-r11}
+	add lr, lr, #(11 * 4)
+	mov sp, lr
 
 	/* Switch stacks */
-	mrs r1, control
-	orr r1, r1, #0x3 /* Switch to process stack, unprivileged mode */
+	mov r1, #3
 	msr control, r1
 	isb
 
-	bx r3
+	mov lr, r2
+	mov pc, r3
 .size _syscall_dispatch, .-_syscall_dispatch
 .ltorg
 
@@ -253,13 +176,11 @@ _exceptions_dispatch:
 	isb
 
 	mrs r0, psp
-	/* push psp, r4~r11, excret */
 	stmdb sp!, {r0, r4-r11, lr}
 
 	mrs r0, ipsr
 	mov r1, sp
 
-	/* void exceptions_dispatch(unsigned int n, exc_context_t *ctx) */
 	b exceptions_dispatch
 .size _exceptions_dispatch, .-_exceptions_dispatch
 .ltorg
@@ -267,94 +188,63 @@ _exceptions_dispatch:
 .globl _interrupts_dispatch
 .type _interrupts_dispatch, %function
 _interrupts_dispatch:
-	mov r0, sp /* save MSP */
-	tst lr, #EXC_RETURN_SPSEL
-	it ne
-	subne sp, sp, #HWCTX_SIZE /* space corresponding to hw-saved ctx */
+	mov r0, #0
+	msr control, r0
+	isb
 
-#if KERNEL_FPU_SUPPORT
-	vpush {s16-s31}
-#endif
-	str r0, [sp, #-8]! /* store msp, skip pad0 */
+	mov r0, sp
+	tst  lr, #(1 << 2)
+	it ne
+	subne sp, sp, #(8 * 4) /* space for hw-saved ctx */
+	str r0, [sp, #-8]!
 
 	mrs r0, ipsr
-	sub r1, sp, #(12 * 4) /* pointer to cpu_context_t on stack */
-#if KERNEL_FPU_SUPPORT
-	ldr r2, =SCS_BASE
-	ldr r2, [r2, #SCS_FPCAR]
-#endif
 	mrs r3, psp
-	push {r1-r11, lr} /* store savesp, fpuctx, psp, r4~r11, irq_ret */
+	sub r1, sp, #48
+	stmdb sp!, {r1-r11, lr}
+
+	/* if we came from userspace, copy hw-saved regs to kstack
+	 * in case of signal handling
+	 */
 
-	/* if we came from userspace, copy hw-saved ctx to kstack in case of signal handling */
-	beq .Lskip_hwctx_copy /* tst lr, #EXC_RETURN_SPSEL */
+	beq _intd0 /* tst  lr, #(1 << 2) */
 
-	/* psp in r3 - load hw-saved ctx */
-	add r12, sp, #ISRCTX_SIZE
-#if KERNEL_FPU_SUPPORT
-	vldm r3, {s0-s25}
-	vstm r12, {s0-s25}
-#else
+	/* psp in r3 - load hw-saved regs */
 	ldm r3, {r4-r11}
+	add r12, sp, #(14 * 4)
 	stm r12, {r4-r11}
-#endif
 
-.Lskip_hwctx_copy:
-	/* void interrupts_dispatch(unsigned int n, cpu_context_t *ctx) */
+_intd0:
 	bl interrupts_dispatch
-
-	/* Switch stack to savesp to restore context from there */
-	ldr sp, [sp]
-	isb
-#if KERNEL_FPU_SUPPORT
-	/* If we have FPU, we can load r4~r11 right away, because we will use FPU registers to copy over hw-saved ctx */
-	pop {r1-r11, lr}          /* load savesp (unused), fpuctx, psp, r4~r11, lr */
-	ldr r0, [sp], #8          /* load msp, skip pad0 */
-	tst lr, #EXC_RETURN_SPSEL /* check if we are returning to userspace */
-	beq .Lskip_hwctx_restore
-
-	add r12, sp, #(16 * 4) /* skip s16~s31 */
-	vldm r12, {s0-s25}     /* load hwcontext, s0~s15, fpscr and vpr */
-	vstm r3, {s0-s25}      /* store them at psp */
-
-.Lskip_hwctx_restore:
-	vpop {s16-s31}
+	ldr r1, [sp]
+	mov sp, r1
 	isb
+_intd1:
+	ldr lr, [sp, #(11 * 4)]
 
-	mov sp, r0 /* restore msp */
-
-	ldr r1, =SCS_BASE
-	str r2, [r1, #SCS_FPCAR]
-	dsb
-	isb
-#else
-	/* If we don't have FPU, we need 8 registers to copy over hw-saved ctx, so load r4~r11 later */
-	ldr lr, [sp, #(11 * 4)]   /* load lr */
-	tst lr, #EXC_RETURN_SPSEL /* check if we are returning to userspace */
-	beq .Lskip_hwctx_restore
+	tst  lr, #(1 << 2)
+	beq _intd2
 
 	/* userspace return - restore registers in case of signal handling */
-	ldr r3, [sp, #(2 * 4)] /* load psp */
-	add r12, sp, #ISRCTX_SIZE
-	ldm r12, {r4-r11}
+	ldr r3, [sp, #(2 * 4)] /* psp */
+	add r1, sp, #(14 * 4)
+	ldm r1, {r4-r11}
 	stm r3, {r4-r11}
 
-.Lskip_hwctx_restore:
-	add sp, sp, #(2 * 4) /* skip over savesp, fpuctx - both unused  */
-	pop {r3-r11}         /* load psp, r4~r11 */
-	ldr sp, [sp, #4]     /* restore msp */
-#endif
+_intd2:
+	ldmia sp!, {r1-r11}
+	ldr r0, [sp, #4]
+	mov sp, r0
+
 	msr psp, r3
 	isb
 
-	/* Check if we're returning to userspace and pick the right CONTROL value */
-	mrs r1, control
-	ite ne /* tst lr, #EXC_RETURN_SPSEL */
-	orrne r1, r1, #0x3 /* Switch to process stack, unprivileged mode */
-	biceq r1, r1, #0x3 /* Switch to main stack, privileged mode */
+	/* Check if we're returning to userspace */
+	and r1, lr, #4
+	ror r1, r1, #2
 	msr control, r1
-	dsb
 	isb
+	dsb
 
 	bx lr
 .size _interrupts_dispatch, .-_interrupts_dispatch
@@ -369,9 +259,9 @@ hal_cpuReschedule:
 
 	/* Invoke PendSV to perform a reschedule */
 	ldr r2, =SCS_BASE
-	ldr r1, [r2, #SCS_ICSR]
+	ldr r1, [r2, 0xd04] /* ICSR */
 	orr r1, #(1 << 28)  /* ICSR.PENDSVSET */
-	str r1, [r2, #SCS_ICSR]
+	str r1, [r2, 0xd04] /* ICSR */
 
 	/* If spinlock not NULL, clear it */
 	cbz r0, .Lhal_cpuReschedule0
@@ -400,7 +290,8 @@ hal_jmp:
 	cpsid if
 	isb
 
-	cbnz r2, hal_jmp_user
+	cmp r2, #0
+	bne hal_jmp_user
 
 	/* kargs has been passed on the stack */
 	ldr r5, [sp]
@@ -433,7 +324,7 @@ hal_jmp_user:
 	msr psp, r2
 	cpsie if
 	isb
-	movs r1, #(USERCONTROL | CONTROL_SFPA)
+	mov r1, USERCONTROL
 	msr control, r1
 	isb
 	dmb
@@ -444,27 +335,24 @@ hal_jmp_user:
 
 .globl hal_exceptionJump /* void hal_exceptionJump(unsigned int n, exc_context_t *ctx, void (*handler)(unsigned int, exc_context_t *)) */
 .type hal_exceptionJump, %function
-.type .L_hal_exceptionJump_end, %function
 hal_exceptionJump:
 	push {r4-r11, lr}
-	/* Prepare pseudo context that will call handler */
-	ldr r7, =DEFAULT_PSR
+	ldr r7, =0x01000000
 	mov r6, r2
-	adr r5, .L_hal_exceptionJump_end
-	push {r0-r7} /* r0~r3, r12, lr, pc, psr */
+	ldr r5, =1f
+	orr r5, #1
+	push {r0-r7}
 
-	mrs r0, control
-	bic r0, r0, #0x3 /* Switch to main stack, privileged mode */
-	msr control, r0
+	mov lr, #0
+	msr control, lr
 	isb
-	/* Exit Handler mode to kernel Thread mode, set FType because our pseudo-context doesn't have FPU */
-	ldr r0, =(RET_THREAD_MSP | EXC_RETURN_FTYPE)
+	mov lr, 0xfffffff9
 	cpsie if
 	dsb
-	bx r0
+	bx lr
 
-.L_hal_exceptionJump_end:
-	pop {r4-r11, pc}
+1:	pop {r4-r11, lr}
+	bx lr
 
 .size hal_exceptionJump, .-hal_exceptionJump
 .ltorg
diff --git a/hal/armv8m/stm32/n6/config.h b/hal/armv8m/stm32/n6/config.h
index 3241514c0..a61850a64 100644
--- a/hal/armv8m/stm32/n6/config.h
+++ b/hal/armv8m/stm32/n6/config.h
@@ -26,12 +26,6 @@
 
 #define SIZE_INTERRUPTS 211
 
-/* Constants for configuring which TIM peripheral is used as system timer */
-#define TIM_SYSTEM_BASE ((void *)0x52003C00) /* TIM18 base address */
-#define TIM_SYSTEM_PCTL pctl_tim18
-#define TIM_SYSTEM_IRQ  tim18_irq
-#define TIM_SYSTEM_FREQ (400UL * 1000000UL) /* Frequency in Hz */
-
 #define HAL_NAME_PLATFORM "STM32N6 "
 
 #endif
diff --git a/hal/armv8m/stm32/n6/stm32n6.c b/hal/armv8m/stm32/n6/stm32n6.c
index 2de8496ae..2314cfd57 100644
--- a/hal/armv8m/stm32/n6/stm32n6.c
+++ b/hal/armv8m/stm32/n6/stm32n6.c
@@ -58,7 +58,6 @@
 #define RIFSC_BASE  ((void *)0x54024000)
 #define GPDMA1_BASE ((void *)0x50021000)
 #define HPDMA1_BASE ((void *)0x58020000)
-#define DBGMCU_BASE ((void *)0x54001000)
 
 #define EXTI_LINES   78
 #define DMA_CHANNELS 16
@@ -513,41 +512,6 @@ void _stm32_rccClearResetFlags(void)
 }
 
 
-/* DBGMCU */
-
-
-int _stm32_dbgmcuStopTimerInDebug(unsigned int dev, u32 stop)
-{
-	u32 reg;
-	volatile u32 *base = DBGMCU_BASE;
-	if ((pctl_tim2 <= dev) && (dev <= pctl_tim11)) {
-		reg = dbgmcu_apb1lfz1;
-	}
-	else if (((pctl_tim1 <= dev) && (dev <= pctl_tim8)) || ((pctl_tim18 <= dev) && (dev <= pctl_tim9))) {
-		reg = dbgmcu_apb2fz1;
-	}
-	else if (((pctl_lptim2 <= dev) && (dev <= pctl_lptim5)) || (dev == pctl_rtc) || (dev == pctl_iwdg)) {
-		reg = dbgmcu_apb4fz1;
-	}
-	else if (dev == pctl_gfxtim) {
-		reg = dbgmcu_apb5fz1;
-	}
-	else {
-		return -EINVAL;
-	}
-
-	if (stop) {
-		*(base + reg) |= 1 << (dev % 32);
-	}
-	else {
-		*(base + reg) &= ~(1 << (dev % 32));
-	}
-
-	hal_cpuDataSyncBarrier();
-	return EOK;
-}
-
-
 /* RTC */
 
 
@@ -873,8 +837,6 @@ void _stm32_init(void)
 	_stm32_rccSetDevClock(pctl_risaf, 1, 1);
 	_stm32_risaf_init();
 
-	_stm32_rccSetDevClock(pctl_dbg, 1, 1);
-
 #if defined(WATCHDOG)
 	/* Init watchdog */
 	/* Enable write access to IWDG */
diff --git a/hal/armv8m/stm32/n6/stm32n6_regs.h b/hal/armv8m/stm32/n6/stm32n6_regs.h
index b25762425..28e7e5bd7 100644
--- a/hal/armv8m/stm32/n6/stm32n6_regs.h
+++ b/hal/armv8m/stm32/n6/stm32n6_regs.h
@@ -489,20 +489,4 @@ enum risaf_regs {
 	risaf_reg1_bnestr,
 };
 
-enum dbgmcu_regs {
-	dbgmcu_idcode = 0x0u,
-	dbgmcu_cr,
-	dbgmcu_apb1lfz1 = 0x4u,
-	dbgmcu_apb1hfz1,
-	dbgmcu_apb2fz1,
-	dbgmcu_apb4fz1,
-	dbgmcu_apb5fz1,
-	dbgmcu_ahb1fz1,
-	dbgmcu_ahb5fz1,
-	dbgmcu_sr = 0x3fu,
-	dbgmcu_dbg_auth_host,
-	dbgmcu_dbg_auth_dev,
-	dbgmcu_dbg_auth_ack,
-};
-
 #endif /* _STM32N6_REGS_H_ */
diff --git a/hal/armv8m/stm32/n6/timer.c b/hal/armv8m/stm32/n6/timer.c
index 246a60b9f..334962f4d 100644
--- a/hal/armv8m/stm32/n6/timer.c
+++ b/hal/armv8m/stm32/n6/timer.c
@@ -3,7 +3,7 @@
  *
  * Operating system kernel
  *
- * System timer driver based on STM32 TIM peripheral.
+ * System timer driver based on SysTick
  *
  * Copyright 2012, 2017, 2021, 2025 Phoenix Systems
  * Author: Jakub Sejdak, Aleksander Kaminski, Jacek Maksymowicz
@@ -20,28 +20,10 @@
 #include "hal/string.h"
 #include "lib/assert.h"
 
-/* This implementation can use timers described as "basic" in the documentation.
- * A more advanced timer can be used (like "general-purpose" or "advanced-control" timers),
- * but a "basic" has enough functionality for our needs.
- */
-
-/* List of registers cut down to only those available on basic timers */
-enum tim_regs {
-	tim_cr1 = 0u,
-	tim_cr2,
-	tim_dier = 3u,
-	tim_sr,
-	tim_egr,
-	tim_cnt = 9u,
-	tim_psc,
-	tim_arr,
-};
-
 static struct {
 	intr_handler_t handler;
 	volatile u64 ticks;
 	spinlock_t sp;
-	volatile u32 *base;
 
 	u32 frequency;        /* Timer ticks per second */
 	u32 ticksPerInterval; /* Timer ticks per interval (i.e. between timer interrupts) */
@@ -53,12 +35,11 @@ int _timer_irqHandler(unsigned int n, cpu_context_t *ctx, void *arg)
 	(void)n;
 	(void)arg;
 	(void)ctx;
+	u8 hasOverflowed;
 
-	/* Note: hal_getTicks() may have cleared the interrupt flag and added to the tick count,
-	 * but after clearing the flag the interrupt remains pending. That's why we need to check
-	 * SR to make sure we don't add twice for the same update event. */
-	if ((*(timer_common.base + tim_sr) & 1u) != 0) {
-		*(timer_common.base + tim_sr) = ~1u; /* Flags are write 0 to clear */
+	/* We need to check the overflow flag, because hal_getTicks() may have cleared it */
+	_hal_scsSystickGetCount(&hasOverflowed);
+	if (hasOverflowed) {
 		timer_common.ticks += timer_common.ticksPerInterval;
 	}
 
@@ -71,19 +52,14 @@ void timer_jiffiesAdd(time_t t)
 	spinlock_ctx_t sc;
 
 	hal_spinlockSet(&timer_common.sp, &sc);
-	if (timer_common.frequency == 1000000UL) {
-		timer_common.ticks += t;
-	}
-	else {
-		timer_common.ticks += (t * timer_common.frequency) / 1000000UL;
-	}
+	timer_common.ticks += (t * timer_common.frequency) / 1000000;
 	hal_spinlockClear(&timer_common.sp, &sc);
 }
 
 
 char *hal_timerFeatures(char *features, unsigned int len)
 {
-	hal_strncpy(features, "Using STM32 TIM timer", len);
+	hal_strncpy(features, "Using SysTick timer", len);
 	features[len - 1] = '\0';
 	return features;
 }
@@ -92,19 +68,18 @@ char *hal_timerFeatures(char *features, unsigned int len)
 static u64 hal_getTicks(void)
 {
 	spinlock_ctx_t sc;
-	u32 cntval;
+	u8 hasOverflowed;
+	u32 elapsed;
 	u64 ret;
 
 	hal_spinlockSet(&timer_common.sp, &sc);
-	ret = timer_common.ticks;
-	cntval = *(timer_common.base + tim_cnt);
-	if ((cntval >> 31) != 0) {
-		*(timer_common.base + tim_sr) = ~1u;
-		ret += timer_common.ticksPerInterval;
-		timer_common.ticks = ret;
+	elapsed = timer_common.ticksPerInterval - _hal_scsSystickGetCount(&hasOverflowed);
+	if (hasOverflowed != 0) {
+		/* Reading the flag cleared the overflow, so we need to add the value to the counter */
+		timer_common.ticks += timer_common.ticksPerInterval;
 	}
 
-	ret += cntval & 0xffffu;
+	ret = timer_common.ticks + elapsed;
 	hal_spinlockClear(&timer_common.sp, &sc);
 
 	return ret;
@@ -113,20 +88,14 @@ static u64 hal_getTicks(void)
 
 time_t hal_timerGetUs(void)
 {
-	u64 ticks = hal_getTicks();
-	if (timer_common.frequency == 1000000UL) {
-		return (time_t)ticks;
-	}
-	else {
-		return ((time_t)ticks * 1000000) / timer_common.frequency;
-	}
+	return (hal_getTicks() * 1000000) / timer_common.frequency;
 }
 
 
 int hal_timerRegister(int (*f)(unsigned int, cpu_context_t *, void *), void *data, intr_handler_t *h)
 {
 	h->f = f;
-	h->n = TIM_SYSTEM_IRQ;
+	h->n = SYSTICK_IRQ;
 	h->data = data;
 
 	return hal_interruptsSetHandler(h);
@@ -142,48 +111,14 @@ void hal_timerSetWakeup(u32 waitUs)
 /* interval: microseconds between timer interrupts */
 void _hal_timerInit(u32 interval)
 {
-	u32 prescaler;
 	timer_common.ticks = 0;
-	timer_common.frequency = TIM_SYSTEM_FREQ;
-	if ((timer_common.frequency % 1000000UL) == 0) {
-		/* If frequency divisible by 1 MHz, set the prescaler to tick once per microsecond.
-		 * Timer APIs work on microseconds, so in this mode we avoid having to do 64-bit division
-		 * in hal_timerGetUs (a very frequently called function). */
-		prescaler = timer_common.frequency / 1000000UL;
-		timer_common.frequency = 1000000UL;
-		timer_common.ticksPerInterval = interval;
-		LIB_ASSERT((prescaler >= 1) && (prescaler <= 65535), "Selected timer interval is not achievable");
-	}
-	else {
-		timer_common.ticksPerInterval = ((u64)timer_common.frequency * interval) / 1000000UL;
-		/* TODO: For optimal precision prescaler should be a factor of timer_common.ticksPerInterval,
-		 * but the difference in precision isn't big enough to matter, so I don't want to add a whole lot
-		 * of extra code to handle this. */
-		prescaler = (timer_common.ticksPerInterval + 65535) / 65536;
-		LIB_ASSERT((prescaler >= 1) && (prescaler <= 65535), "Selected timer interval is not achievable");
-		timer_common.frequency /= prescaler;
-		timer_common.ticksPerInterval = ((u64)timer_common.frequency * interval) / 1000000UL;
-	}
-
-	LIB_ASSERT((timer_common.ticksPerInterval >= 1) && (timer_common.ticksPerInterval <= 65535),
-			"Selected timer interval is not achievable");
-	_stm32_rccSetDevClock(TIM_SYSTEM_PCTL, 1, 1);
-	_stm32_dbgmcuStopTimerInDebug(TIM_SYSTEM_PCTL, 1);
-	timer_common.base = TIM_SYSTEM_BASE;
-	/* set UIF status bit remapping, so we can get UIF by just reading the counter */
-	*(timer_common.base + tim_cr1) = (1 << 11);
-	*(timer_common.base + tim_cr2) = 0;
-	*(timer_common.base + tim_cnt) = 0;
-	*(timer_common.base + tim_psc) = prescaler - 1;
-	*(timer_common.base + tim_arr) = timer_common.ticksPerInterval - 1;
-	*(timer_common.base + tim_dier) = 1; /* Activate interrupt */
-
+	timer_common.frequency = _stm32_rccGetCPUClock();
+	timer_common.ticksPerInterval = (((u64)timer_common.frequency) * interval) / 1000000;
+	LIB_ASSERT(timer_common.ticksPerInterval <= 0x01000000, "Selected timer interval is not achievable");
+	_hal_scsSystickInit(timer_common.ticksPerInterval - 1);
 	hal_spinlockCreate(&timer_common.sp, "timer");
 	timer_common.handler.f = _timer_irqHandler;
-	timer_common.handler.n = TIM_SYSTEM_IRQ;
+	timer_common.handler.n = SYSTICK_IRQ;
 	timer_common.handler.data = NULL;
 	hal_interruptsSetHandler(&timer_common.handler);
-
-	hal_cpuDataMemoryBarrier();
-	*(timer_common.base + tim_cr1) |= 1; /* Start counting */
 }
diff --git a/hal/armv8m/stm32/stm32.h b/hal/armv8m/stm32/stm32.h
index eeafdf3ef..0c302444a 100644
--- a/hal/armv8m/stm32/stm32.h
+++ b/hal/armv8m/stm32/stm32.h
@@ -82,10 +82,6 @@ extern u32 _stm32_rccGetPerClock(void);
 extern void _stm32_rccClearResetFlags(void);
 
 
-/* If `stop` != 0, selected timer will be stopped when CPU is halted in debug. */
-extern int _stm32_dbgmcuStopTimerInDebug(unsigned int dev, u32 stop);
-
-
 extern int _stm32_gpioConfig(unsigned int d, u8 pin, u8 mode, u8 af, u8 otype, u8 ospeed, u8 pupd);
 
 
diff --git a/include/arch/aarch64/zynqmp/zynqmp.h b/include/arch/aarch64/zynqmp/zynqmp.h
index 250a539c4..d5e4f7b4e 100644
--- a/include/arch/aarch64/zynqmp/zynqmp.h
+++ b/include/arch/aarch64/zynqmp/zynqmp.h
@@ -32,7 +32,7 @@
 /* clang-format off */
 /* Devices' clocks controllers */
 enum {
-	pctl_devclock_lpd_usb3_dual = 0x13,
+	pctl_devclock_lpd_usb3_dual = 0x12,
 	pctl_devclock_lpd_gem0,
 	pctl_devclock_lpd_gem1,
 	pctl_devclock_lpd_gem2,
diff --git a/include/arch/armv8m/stm32/n6/stm32n6.h b/include/arch/armv8m/stm32/n6/stm32n6.h
index 0188d420a..14b680b94 100644
--- a/include/arch/armv8m/stm32/n6/stm32n6.h
+++ b/include/arch/armv8m/stm32/n6/stm32n6.h
@@ -164,7 +164,6 @@ enum {
 	pctl_vrefbuf = 0x18f,
 	pctl_rtc,
 	pctl_rtcapb,
-	pctl_iwdg,
 	pctl_r2gret = 0x196,
 	pctl_r2gnpu,
 	pctl_serf = 0x19f,
diff --git a/include/posix-fcntl.h b/include/posix-fcntl.h
index 6bf3c2207..d4b36c3ab 100644
--- a/include/posix-fcntl.h
+++ b/include/posix-fcntl.h
@@ -17,24 +17,20 @@
 #define _PHOENIX_POSIX_FCNTL_H_
 
 
-#define FD_CLOEXEC 1
-
-#define O_RDONLY   0x00001
-#define O_WRONLY   0x00002
-#define O_RDWR     0x00004
-#define O_APPEND   0x00008
-#define O_CREAT    0x00100
-#define O_TRUNC    0x00200
-#define O_EXCL     0x00400
-#define O_SYNC     0x00800
-#define O_NONBLOCK 0x01000
+#define FD_CLOEXEC 0x1U
+
+#define O_RDONLY   0x0001U
+#define O_WRONLY   0x0002U
+#define O_RDWR     0x0004U
+#define O_APPEND   0x0008U
+#define O_CREAT    0x0100U
+#define O_TRUNC    0x0200U
+#define O_EXCL     0x0400U
+#define O_SYNC     0x0800U
+#define O_NONBLOCK 0x1000U
 #define O_NDELAY   O_NONBLOCK
-#define O_NOCTTY   0x02000
-#define O_CLOEXEC  0x04000
-#define O_RSYNC    0x08000
-#define O_DSYNC    0x10000
-
-#define O_ACCMODE (O_RDONLY | O_WRONLY | O_RDWR)
+#define O_NOCTTY   0x2000U
+#define O_CLOEXEC  0x4000U
 
 /* clang-format off */
 
diff --git a/include/time.h b/include/time.h
index 95145af50..d467155fc 100644
--- a/include/time.h
+++ b/include/time.h
@@ -21,7 +21,5 @@
 #define PH_CLOCK_REALTIME  1
 #define PH_CLOCK_MONOTONIC 2
 
-#define TIMER_ABSTIME 1
-
 
 #endif
diff --git a/lib/assert.h b/lib/assert.h
index 3456ab0df..3af76c801 100644
--- a/lib/assert.h
+++ b/lib/assert.h
@@ -25,8 +25,6 @@ void lib_assertPanic(const char *func, int line, const char *fmt, ...);
 		lib_assertPanic(__FUNCTION__, __LINE__, (fmt), ##__VA_ARGS__); \
 	}
 
-#define LIB_STATIC_ASSERT_SAME_TYPE(t1, t2) _Static_assert(__builtin_types_compatible_p(typeof(t1), typeof(t2)), "type mismatch")
-
 #ifndef NDEBUG
 
 #define LIB_ASSERT(condition, fmt, ...) LIB_ASSERT_ALWAYS((condition), (fmt), ##__VA_ARGS__)
diff --git a/lib/lib.h b/lib/lib.h
index 37e04f72f..472070685 100644
--- a/lib/lib.h
+++ b/lib/lib.h
@@ -61,7 +61,4 @@
 #define round_page(x) (((x) + SIZE_PAGE - 1U) & ~(SIZE_PAGE - 1U))
 
 
-#define offsetof(st, m) __builtin_offsetof(st, m)
-
-
 #endif
diff --git a/lib/list.h b/lib/list.h
index c56ce4be0..f87a8889e 100644
--- a/lib/list.h
+++ b/lib/list.h
@@ -12,11 +12,11 @@
  *
  * %LICENSE%
  */
+
 #ifndef _LIB_LIST_H_
 #define _LIB_LIST_H_
 
 #include "hal/hal.h"
-#include "lib/lib.h"
 
 
 extern void lib_listAdd(void **list, void *t, size_t noff, size_t poff);
@@ -29,30 +29,21 @@ extern int lib_listBelongs(void **list, void *t, size_t noff, size_t poff);
 
 
 #define LIST_ADD_EX(list, t, next, prev) \
-	do { \
-		LIB_STATIC_ASSERT_SAME_TYPE(*(list), t); \
-		lib_listAdd((void **)(list), (void *)(t), offsetof(typeof(*(t)), next), offsetof(typeof(*(t)), prev)); \
-	} while (0)
+	lib_listAdd((void **)(list), (void *)(t), (size_t) & (((typeof(t))0)->next), (size_t) & (((typeof(t))0)->prev))
 
 
 #define LIST_ADD(list, t) LIST_ADD_EX((list), (t), next, prev)
 
 
 #define LIST_REMOVE_EX(list, t, next, prev) \
-	do { \
-		LIB_STATIC_ASSERT_SAME_TYPE(*(list), t); \
-		lib_listRemove((void **)(list), (void *)(t), offsetof(typeof(*(t)), next), offsetof(typeof(*(t)), prev)); \
-	} while (0)
+	lib_listRemove((void **)(list), (void *)(t), (size_t) & (((typeof(t))0)->next), (size_t) & (((typeof(t))0)->prev))
 
 
 #define LIST_REMOVE(list, t) LIST_REMOVE_EX((list), (t), next, prev)
 
 
 #define LIST_BELONGS_EX(list, t, next, prev) \
-	({ \
-		LIB_STATIC_ASSERT_SAME_TYPE(*(list), t); \
-		lib_listBelongs((void **)(list), (void *)(t), offsetof(typeof(*(t)), next), offsetof(typeof(*(t)), prev)); \
-	})
+	lib_listBelongs((void **)(list), (void *)(t), (size_t) & (((typeof(t))0)->next), (size_t) & (((typeof(t))0)->prev))
 
 
 #define LIST_BELONGS(list, t) LIST_BELONGS_EX((list), (t), next, prev)
diff --git a/proc/process.h b/proc/process.h
index b16b4ae56..c52e49caf 100644
--- a/proc/process.h
+++ b/proc/process.h
@@ -60,7 +60,7 @@ typedef struct _process_t {
 	u32 umask;
 #endif
 
-	struct _port_t *ports;
+	void *ports;
 
 	idtree_t resources;
 
diff --git a/proc/threads.c b/proc/threads.c
index 14e85ffd6..2270b5def 100644
--- a/proc/threads.c
+++ b/proc/threads.c
@@ -1165,15 +1165,23 @@ static int _proc_threadWait(thread_t **queue, time_t timeout, spinlock_ctx_t *sc
 }
 
 
-static int _proc_threadSleepAbs(time_t abs, time_t now, spinlock_ctx_t *sc)
+int proc_threadSleep(time_t us)
 {
+	thread_t *current;
+	int err;
+	time_t now;
+	spinlock_ctx_t sc;
+
+	hal_spinlockSet(&threads_common.spinlock, &sc);
+
 	/* Handle usleep(0) (yield) */
-	if (abs > now) {
-		thread_t *current = _proc_current();
+	if (us != 0) {
+		now = _proc_gettimeRaw();
 
+		current = _proc_current();
 		current->state = SLEEP;
 		current->wait = NULL;
-		current->wakeup = abs;
+		current->wakeup = now + us;
 		current->interruptible = 1;
 
 		(void)lib_rbInsert(&threads_common.sleeping, &current->sleeplinkage);
@@ -1182,62 +1190,12 @@ static int _proc_threadSleepAbs(time_t abs, time_t now, spinlock_ctx_t *sc)
 		_threads_updateWakeup(now, NULL);
 	}
 
-	return hal_cpuReschedule(&threads_common.spinlock, sc);
-}
-
-
-static int _proc_threadSleep(time_t us, time_t now, spinlock_ctx_t *sc)
-{
-	return _proc_threadSleepAbs(now + us, now, sc);
-}
-
-
-int proc_threadSleep(time_t us)
-{
-	spinlock_ctx_t sc;
-
-	hal_spinlockSet(&threads_common.spinlock, &sc);
-	return _proc_threadSleep(us, _proc_gettimeRaw(), &sc);
-}
-
-
-int proc_threadNanoSleep(time_t *sec, long int *nsec, int absolute)
-{
-	time_t us, start, stop, elapsed, unslept;
-	int err;
-	spinlock_ctx_t sc;
-
-	if ((*sec < 0) || ((*nsec) < 0) || ((*nsec) >= (1000 * 1000 * 1000))) {
-		return -EINVAL;
-	}
-
-	us = ((*sec) * 1000 * 1000) + (((*nsec) + 999) / 1000);
-
-	hal_spinlockSet(&threads_common.spinlock, &sc);
-
-	start = _proc_gettimeRaw();
-
-	if (absolute != 0) {
-		err = _proc_threadSleepAbs(us, start, &sc);
-	}
-	else {
-		err = _proc_threadSleep(us, start, &sc);
-		if (err == -EINTR) {
-			proc_gettime(&stop, NULL);
-			elapsed = stop - start;
-			if (us > elapsed) {
-				unslept = us - elapsed;
-				*sec = unslept / (1000 * 1000);
-				*nsec = (unslept % (1000 * 1000)) * 1000;
-			}
-			else {
-				*sec = 0;
-				*nsec = 0;
-			}
-		}
+	err = hal_cpuReschedule(&threads_common.spinlock, &sc);
+	if (err == -ETIME) {
+		err = EOK;
 	}
 
-	return (err == -ETIME) ? EOK : err;
+	return err;
 }
 
 
diff --git a/proc/threads.h b/proc/threads.h
index 96898976e..dd65db561 100644
--- a/proc/threads.h
+++ b/proc/threads.h
@@ -153,9 +153,6 @@ extern int proc_threadsOther(thread_t *t);
 extern int proc_threadSleep(time_t us);
 
 
-extern int proc_threadNanoSleep(time_t *sec, long int *nsec, int absolute);
-
-
 extern int proc_threadWait(thread_t **queue, spinlock_t *spinlock, time_t timeout, spinlock_ctx_t *scp);
 
 
diff --git a/syscalls.c b/syscalls.c
index 924480eff..63310f04a 100644
--- a/syscalls.c
+++ b/syscalls.c
@@ -24,7 +24,6 @@
 #include "include/syscalls.h"
 #include "include/threads.h"
 #include "include/utsname.h"
-#include "include/time.h"
 #include "lib/lib.h"
 #include "proc/proc.h"
 #include "vm/object.h"
@@ -338,16 +337,11 @@ int syscalls_nsleep(void *ustack)
 	process_t *proc = proc_current()->process;
 	time_t *sec;
 	long int *nsec;
-	int clockid;
-	int flags;
+	time_t start, us, stop, elapsed, unslept;
+	int ret;
 
 	GETFROMSTACK(ustack, time_t *, sec, 0);
 	GETFROMSTACK(ustack, long int *, nsec, 1);
-	GETFROMSTACK(ustack, int, clockid, 2);
-	GETFROMSTACK(ustack, int, flags, 3);
-
-	/* Not used right now, future-proofing */
-	(void)clockid;
 
 	if (vm_mapBelongs(proc, sec, sizeof(*sec)) < 0) {
 		return -EFAULT;
@@ -357,7 +351,30 @@ int syscalls_nsleep(void *ustack)
 		return -EFAULT;
 	}
 
-	return proc_threadNanoSleep(sec, nsec, ((flags & TIMER_ABSTIME) != 0) ? 1 : 0);
+	if ((*sec < 0) || ((*nsec) < 0) || ((*nsec) >= (1000 * 1000 * 1000))) {
+		return -EINVAL;
+	}
+
+	proc_gettime(&start, NULL);
+
+	us = ((*sec) * 1000LL * 1000LL) + (((*nsec) + 999LL) / 1000LL);
+
+	ret = proc_threadSleep(us);
+
+	*sec = 0;
+	*nsec = 0;
+
+	if (ret == -EINTR) {
+		proc_gettime(&stop, NULL);
+		elapsed = stop - start;
+		if (us > elapsed) {
+			unslept = us - elapsed;
+			*sec = unslept / (1000LL * 1000LL);
+			*nsec = ((long int)unslept % (1000L * 1000L)) * 1000L;
+		}
+	}
+
+	return ret;
 }
 
 
diff --git a/vm/kmalloc.c b/vm/kmalloc.c
index 6089d3b54..d37a44364 100644
--- a/vm/kmalloc.c
+++ b/vm/kmalloc.c
@@ -22,8 +22,8 @@
 #include "proc/proc.h"
 
 
-struct {
-	vm_zone_t *sizes[24];
+static struct {
+	vm_zone_t *sizes[17];
 	vm_zone_t *used;
 	vm_zone_t firstzone;