From a74cf0ca83cbc3ad8e1fce7b656e1dfeb4346ef1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=C5=81ukasz=20Leczkowski?=
 <lukasz.leczkowski@phoenix-rtos.com>
Date: Thu, 6 Jun 2024 15:32:01 +0200
Subject: [PATCH 1/4] main: move `syspage_init` before `_hal_init`

This change is necessary to be able to add hal memory mapper.

JIRA: RTOS-844
---
 main.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/main.c b/main.c
index 4e28e4e66..b35477d41 100644
--- a/main.c
+++ b/main.c
@@ -96,6 +96,7 @@ int main(void)
 {
 	char s[128];
 
+	syspage_init();
 	_hal_init();
 	_usrv_init();
 
@@ -104,7 +105,6 @@ int main(void)
 	lib_printf("hal: %s\n", hal_cpuFeatures(s, sizeof(s)));
 	lib_printf("hal: %s\n", hal_interruptsFeatures(s, sizeof(s)));
 	lib_printf("hal: %s\n", hal_timerFeatures(s, sizeof(s)));
-	syspage_init();
 
 	_vm_init(&main_common.kmap, &main_common.kernel);
 	_proc_init(&main_common.kmap, &main_common.kernel);

From d2e31344a642270664d5d95a7a6429d4b26a651d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=C5=81ukasz=20Leczkowski?=
 <lukasz.leczkowski@phoenix-rtos.com>
Date: Thu, 27 Jun 2024 15:19:11 +0200
Subject: [PATCH 2/4] process/tls: fix tls size on 64-bit arch

JIRA: RTOS-844
---
 proc/process.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/proc/process.c b/proc/process.c
index 6dbb03291..5ab1a4ace 100644
--- a/proc/process.c
+++ b/proc/process.c
@@ -313,7 +313,7 @@ static void process_tlsAssign(hal_tls_t *process_tls, hal_tls_t *tls, ptr_t tbss
 	}
 	process_tls->tdata_sz = tls->tdata_sz;
 	process_tls->tbss_sz = tls->tbss_sz;
-	process_tls->tls_sz = tls->tbss_sz + tls->tdata_sz + sizeof(void *);
+	process_tls->tls_sz = (tls->tbss_sz + tls->tdata_sz + sizeof(void *) + sizeof(void *) - 1) & ~(sizeof(void *) - 1);
 	process_tls->arm_m_tls = tls->arm_m_tls;
 }
 
@@ -1759,7 +1759,7 @@ int process_tlsInit(hal_tls_t *dest, hal_tls_t *source, vm_map_t *map)
 		hal_memcpy((void *)dest->tls_base, (void *)source->tls_base, dest->tdata_sz);
 		hal_memset((char *)dest->tls_base + dest->tdata_sz, 0, dest->tbss_sz);
 		/* At the end of TLS there must be a pointer to itself */
-		*(ptr_t *)(dest->tls_base + dest->tdata_sz + dest->tbss_sz) = dest->tls_base + dest->tdata_sz + dest->tbss_sz;
+		*(ptr_t *)((dest->tls_base + dest->tdata_sz + dest->tbss_sz + sizeof(ptr_t) - 1) & ~(sizeof(ptr_t) - 1)) = dest->tls_base + dest->tdata_sz + dest->tbss_sz;
 		err = EOK;
 	}
 	else {

From 5e40a3425b826d18ab305311e5bbc30adb277ca3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=C5=81ukasz=20Leczkowski?=
 <lukasz.leczkowski@phoenix-rtos.com>
Date: Thu, 6 Jun 2024 16:57:47 +0200
Subject: [PATCH 3/4] riscv64: add noelv target

JIRA: RTOS-844
---
 hal/riscv64/Makefile                     |   8 +-
 hal/riscv64/_cache.S                     |  78 +++++
 hal/riscv64/_init.S                      |   6 +-
 hal/riscv64/_interrupts.S                |   2 -
 hal/riscv64/arch/cpu.h                   |   4 +-
 hal/riscv64/arch/pmap.h                  |  10 +
 hal/riscv64/asm-macros.h                 |  75 +++++
 hal/riscv64/cpu.c                        |  13 +-
 hal/riscv64/dtb.c                        | 211 ++++++-------
 hal/riscv64/dtb.h                        |  24 +-
 hal/riscv64/generic/Makefile             |   7 +
 hal/riscv64/{ => generic}/config.h       |   4 +-
 hal/riscv64/{ => generic}/console.c      |   2 +-
 hal/riscv64/generic/generic.c            |  56 ++++
 hal/riscv64/hal.c                        |  44 +--
 hal/riscv64/interrupts.c                 |   1 +
 hal/riscv64/noelv/Makefile               |   7 +
 hal/riscv64/noelv/ambapp.c               | 235 ++++++++++++++
 hal/riscv64/noelv/ambapp.h               |  32 ++
 hal/riscv64/noelv/config.h               |  29 ++
 hal/riscv64/noelv/console.c              |  99 ++++++
 hal/riscv64/noelv/noelv.c                |  64 ++++
 hal/riscv64/plic.c                       |  13 +-
 hal/riscv64/pmap.c                       | 378 +++++++++++++++++------
 hal/riscv64/riscv64.h                    |   6 +
 hal/sparcv8leon3/gaisler/ambapp.h        |   1 +
 include/arch/riscv64/riscv64.h           |  14 +-
 include/arch/sparcv8leon3/sparcv8leon3.h | 184 -----------
 include/gaisler/ambapp.h                 | 204 ++++++++++++
 29 files changed, 1351 insertions(+), 460 deletions(-)
 create mode 100644 hal/riscv64/_cache.S
 create mode 100644 hal/riscv64/asm-macros.h
 create mode 100644 hal/riscv64/generic/Makefile
 rename hal/riscv64/{ => generic}/config.h (82%)
 rename hal/riscv64/{ => generic}/console.c (97%)
 create mode 100644 hal/riscv64/generic/generic.c
 create mode 100644 hal/riscv64/noelv/Makefile
 create mode 100644 hal/riscv64/noelv/ambapp.c
 create mode 100644 hal/riscv64/noelv/ambapp.h
 create mode 100644 hal/riscv64/noelv/config.h
 create mode 100644 hal/riscv64/noelv/console.c
 create mode 100644 hal/riscv64/noelv/noelv.c
 create mode 100644 include/gaisler/ambapp.h

diff --git a/hal/riscv64/Makefile b/hal/riscv64/Makefile
index bdf459beb..143f3998b 100644
--- a/hal/riscv64/Makefile
+++ b/hal/riscv64/Makefile
@@ -4,10 +4,10 @@
 # Copyright 2018, 2020, 2024 Phoenix Systems
 #
 
-OBJS += $(addprefix $(PREFIX_O)hal/riscv64/, _init.o _string.o _interrupts.o hal.o spinlock.o interrupts.o cpu.o pmap.o \
-dtb.o timer.o string.o exceptions.o plic.o sbi.o)
+OBJS += $(addprefix $(PREFIX_O)hal/$(TARGET_SUFF)/, _init.o _cache.o _string.o _interrupts.o hal.o spinlock.o interrupts.o \
+	cpu.o pmap.o dtb.o timer.o string.o exceptions.o plic.o sbi.o)
 
-CFLAGS += -Ihal/riscv64
+include hal/$(TARGET_SUFF)/$(TARGET_SUBFAMILY)/Makefile
 
-OBJS += $(PREFIX_O)hal/riscv64/console.o
+CFLAGS += -Ihal/$(TARGET_SUFF) -Ihal/$(TARGET_SUFF)/$(TARGET_SUBFAMILY)
 
diff --git a/hal/riscv64/_cache.S b/hal/riscv64/_cache.S
new file mode 100644
index 000000000..f7dbe42b9
--- /dev/null
+++ b/hal/riscv64/_cache.S
@@ -0,0 +1,78 @@
+/*
+ * Phoenix-RTOS
+ *
+ * Operating system kernel
+ *
+ * riscv64 cache management
+ *
+ * Copyright 2024 Phoenix Systems
+ * Author: Lukasz Leczkowski
+ *
+ * This file is part of Phoenix-RTOS.
+ *
+ * %LICENSE%
+ */
+
+#define __ASSEMBLY__
+
+#include "asm-macros.h"
+#include <board_config.h>
+
+.text
+
+/* void hal_cpuDCacheInval(void *va, size_t size)
+ * va must have write access
+ */
+.global hal_cpuDCacheInval
+.type hal_cpuDCacheInval, @function
+hal_cpuDCacheInval:
+#ifdef DCACHE_BLOCK_SIZE
+	/* Align size to cache block size */
+	li a4, DCACHE_BLOCK_SIZE
+	addi a5, a4, -1
+	add a1, a1, a5
+	li a5, -DCACHE_BLOCK_SIZE
+	and a1, a1, a5
+
+	/* End address */
+	add a5, a1, a0
+	beq a0, a5, 2f
+
+1:
+	CBO_INVAL(REG_A0)
+	add a0, a0, a4
+	bgtu a5, a0, 1b
+
+2:
+#endif
+	ret
+.size hal_cpuDCacheInval, . - hal_cpuDCacheInval
+
+
+/* void hal_cpuDCacheFlush(void *va, size_t size)
+ * va must have write access
+ */
+.global hal_cpuDCacheFlush
+.type hal_cpuDCacheFlush, @function
+hal_cpuDCacheFlush:
+#ifdef DCACHE_BLOCK_SIZE
+	/* Align size to cache block size */
+	li a4, DCACHE_BLOCK_SIZE
+	addi a5, a4, -1
+	add a1, a1, a5
+	li a5, -DCACHE_BLOCK_SIZE
+	and a1, a1, a5
+
+	/* End address */
+	add a5, a1, a0
+	beq a0, a5, 2f
+
+1:
+	CBO_FLUSH(REG_A0)
+	add a0, a0, a4
+	bgtu a5, a0, 1b
+
+2:
+#endif
+	ret
+.size hal_cpuDCacheFlush, . - hal_cpuDCacheFlush
diff --git a/hal/riscv64/_init.S b/hal/riscv64/_init.S
index 3c07078cf..64f96ce2c 100644
--- a/hal/riscv64/_init.S
+++ b/hal/riscv64/_init.S
@@ -103,8 +103,10 @@ syspage_cpy:
 	bltu t1, t2, syspage_cpy
 
 dtb:
-	mv a1, a2
-	call dtb_parse
+	mv s4, a2
+	mv a0, a2
+	call dtb_save
+	mv a0, s4
 	call _pmap_preinit
 
 _init_core:
diff --git a/hal/riscv64/_interrupts.S b/hal/riscv64/_interrupts.S
index df77589da..ece5ff08c 100644
--- a/hal/riscv64/_interrupts.S
+++ b/hal/riscv64/_interrupts.S
@@ -247,8 +247,6 @@
 
 	/* Restore task's stack pointer */
 	ld sp, 288(sp)
-
-	fence.i
 .endm
 
 
diff --git a/hal/riscv64/arch/cpu.h b/hal/riscv64/arch/cpu.h
index 33f0c7159..2757b7f4e 100644
--- a/hal/riscv64/arch/cpu.h
+++ b/hal/riscv64/arch/cpu.h
@@ -285,10 +285,10 @@ static inline int hal_cpuSupervisorMode(cpu_context_t *ctx)
 void hal_cpuRfenceI(void);
 
 
-void hal_cpuLocalFlushTLB(const struct _pmap_t *pmap, const void *vaddr);
+void hal_cpuLocalFlushTLB(u32 asid, const void *vaddr);
 
 
-void hal_cpuRemoteFlushTLB(const struct _pmap_t *pmap, const void *vaddr, size_t size);
+void hal_cpuRemoteFlushTLB(u32 asid, const void *vaddr, size_t size);
 
 
 /* Code used in disabled code vm/object.c - map_pageFault */
diff --git a/hal/riscv64/arch/pmap.h b/hal/riscv64/arch/pmap.h
index 5c551dca0..cd18aeb51 100644
--- a/hal/riscv64/arch/pmap.h
+++ b/hal/riscv64/arch/pmap.h
@@ -22,6 +22,7 @@
 #define VADDR_MAX     0xffffffffffffffffL
 #define VADDR_USR_MAX VADDR_KERNEL
 
+#define VADDR_DTB 0xffffffffc0000000UL
 
 /* Architecure dependent page attributes */
 #define PGHD_PRESENT    0x01
@@ -82,6 +83,15 @@ typedef struct _pmap_t {
 addr_t pmap_getKernelStart(void);
 
 
+void *_pmap_halMap(addr_t paddr, void *va, size_t size, int attr);
+
+
+void *pmap_halMap(addr_t paddr, void *va, size_t size, int attr);
+
+
+void _pmap_halInit(void);
+
+
 #endif
 
 
diff --git a/hal/riscv64/asm-macros.h b/hal/riscv64/asm-macros.h
new file mode 100644
index 000000000..bc2551c61
--- /dev/null
+++ b/hal/riscv64/asm-macros.h
@@ -0,0 +1,75 @@
+/*
+ * Phoenix-RTOS
+ *
+ * Operating system kernel
+ *
+ * Assembly macros
+ *
+ * Copyright 2024 Phoenix Systems
+ * Author: Lukasz Leczkowski
+ *
+ * This file is part of Phoenix-RTOS.
+ *
+ * %LICENSE%
+ */
+
+#ifndef _ASM_MACROS_H_
+#define _ASM_MACROS_H_
+
+#define STR(x)  #x
+#define XSTR(x) STR(x)
+
+/* RISC-V registers */
+#define REG_ZERO 0
+#define REG_RA   1
+#define REG_SP   2
+#define REG_GP   3
+#define REG_TP   4
+#define REG_T0   5
+#define REG_T1   6
+#define REG_T2   7
+#define REG_S0   8
+#define REG_S1   9
+#define REG_A0   10
+#define REG_A1   11
+#define REG_A2   12
+#define REG_A3   13
+#define REG_A4   14
+#define REG_A5   15
+#define REG_A6   16
+#define REG_A7   17
+#define REG_S2   18
+#define REG_S3   19
+#define REG_S4   20
+#define REG_S5   21
+#define REG_S6   22
+#define REG_S7   23
+#define REG_S8   24
+#define REG_S9   25
+#define REG_S10  26
+#define REG_S11  27
+#define REG_T3   28
+#define REG_T4   29
+#define REG_T5   30
+#define REG_T6   31
+
+
+#define OPCODE_MISC_MEM 0xf
+#define FUNCT3_CBO      (0x2 << 12)
+
+/* clang-format off */
+
+/* Instructions supported from GCC 14 */
+#define CBO_INVAL(REG) \
+	.word ((REG & 0x1f) << 15) | FUNCT3_CBO | OPCODE_MISC_MEM
+
+#define CBO_CLEAN(REG) \
+	.word (1 << 20) | ((REG & 0x1f) << 15) | FUNCT3_CBO | OPCODE_MISC_MEM
+
+#define CBO_FLUSH(REG) \
+	.word (1 << 21) | ((REG & 0x1f) << 15) | FUNCT3_CBO | OPCODE_MISC_MEM
+
+
+/* clang-format on */
+
+#endif
diff --git a/hal/riscv64/cpu.c b/hal/riscv64/cpu.c
index 6bf7787aa..4e08f90df 100644
--- a/hal/riscv64/cpu.c
+++ b/hal/riscv64/cpu.c
@@ -373,9 +373,9 @@ void hal_cpuRfenceI(void)
 }
 
 
-void hal_cpuLocalFlushTLB(const struct _pmap_t *pmap, const void *vaddr)
+void hal_cpuLocalFlushTLB(u32 asid, const void *vaddr)
 {
-	(void)pmap; /* TODO: ASID support */
+	(void)asid; /* TODO: ASID support */
 
 	/* clang-format off */
 	__asm__ volatile (
@@ -388,10 +388,9 @@ void hal_cpuLocalFlushTLB(const struct _pmap_t *pmap, const void *vaddr)
 }
 
 
-void hal_cpuRemoteFlushTLB(const struct _pmap_t *pmap, const void *vaddr, size_t size)
+void hal_cpuRemoteFlushTLB(u32 asid, const void *vaddr, size_t size)
 {
-	(void)pmap; /* TODO: ASID support */
-
+	size_t i;
 	unsigned long hart_mask;
 
 	if (hal_cpuGetCount() > 1) {
@@ -399,7 +398,9 @@ void hal_cpuRemoteFlushTLB(const struct _pmap_t *pmap, const void *vaddr, size_t
 		hal_sbiSfenceVma(hart_mask, 0, (unsigned long)vaddr, size);
 	}
 	else {
-		hal_cpuLocalFlushTLB(pmap, vaddr);
+		for (i = 0; i < size; i += SIZE_PAGE) {
+			hal_cpuLocalFlushTLB(asid, (void *)((unsigned long)vaddr + i));
+		}
 	}
 }
 
diff --git a/hal/riscv64/dtb.c b/hal/riscv64/dtb.c
index e163d568b..2816d0874 100644
--- a/hal/riscv64/dtb.c
+++ b/hal/riscv64/dtb.c
@@ -5,8 +5,8 @@
  *
  * DTB parser
  *
- * Copyright 2018, 2020 Phoenix Systems
- * Author: Pawel Pisarczyk
+ * Copyright 2018, 2020, 2024 Phoenix Systems
+ * Author: Pawel Pisarczyk, Lukasz Leczkowski
  *
  * This file is part of Phoenix-RTOS.
  *
@@ -15,7 +15,8 @@
 
 #include "dtb.h"
 #include "hal/string.h"
-#include "hal/pmap.h"
+
+#include <arch/pmap.h>
 
 #include "include/errno.h"
 
@@ -39,8 +40,6 @@ struct _fdt_header_t {
 static struct {
 	struct _fdt_header_t *fdth;
 
-	void *start;
-
 	char *model;
 	char *compatible;
 
@@ -59,7 +58,7 @@ static struct {
 
 	struct {
 		size_t nreg;
-		u64 *reg;
+		u8 *reg;
 	} memory;
 
 	struct {
@@ -80,40 +79,40 @@ char *dtb_getString(u32 i)
 
 void dtb_parseSystem(void *dtb, u32 si, u32 l)
 {
-	if (!hal_strcmp(dtb_getString(si), "model"))
+	if (!hal_strcmp(dtb_getString(si), "model")) {
 		dtb_common.model = dtb;
-	else if (!hal_strcmp(dtb_getString(si), "compatible"))
+	}
+	else if (!hal_strcmp(dtb_getString(si), "compatible")) {
 		dtb_common.compatible = dtb;
-
-	return;
+	}
 }
 
 
 void dtb_parseCPU(void *dtb, u32 si, u32 l)
 {
-	if (!hal_strcmp(dtb_getString(si), "compatible"))
+	if (!hal_strcmp(dtb_getString(si), "compatible")) {
 		dtb_common.cpus[dtb_common.ncpus].compatible = dtb;
-	else if (!hal_strcmp(dtb_getString(si), "riscv,isa"))
+	}
+	else if (!hal_strcmp(dtb_getString(si), "riscv,isa")) {
 		dtb_common.cpus[dtb_common.ncpus].isa = dtb;
-	else if (!hal_strcmp(dtb_getString(si), "mmu-type"))
+	}
+	else if (!hal_strcmp(dtb_getString(si), "mmu-type")) {
 		dtb_common.cpus[dtb_common.ncpus].mmu = dtb;
-	else if (!hal_strcmp(dtb_getString(si), "clock-frequency"))
+	}
+	else if (!hal_strcmp(dtb_getString(si), "clock-frequency")) {
 		dtb_common.cpus[dtb_common.ncpus].clock = ntoh32(*(u32 *)dtb);
-
-	return;
+	}
 }
 
 
 void dtb_parseInterruptController(void *dtb, u32 si, u32 l)
 {
-	return;
 }
 
 
 void dtb_parseSOCInterruptController(void *dtb, u32 si, u32 l)
 {
 	dtb_common.soc.intctl.exist = 1;
-	return;
 }
 
 
@@ -127,23 +126,15 @@ int dtb_parseMemory(void *dtb, u32 si, u32 l)
 }
 
 
-#if 0 /* Debug function */
-static void dtb_print(char *s) {
-	while (*s != 0) {
-		__asm__ (
-			"li t0, 0x10000000;" \
-			"ld t1, (%0);" \
-			"sd t1, (t0);" \
-			:: "r" (s) : "t0", "t1", "memory"
-		);
-		s++;
-	}
+void dtb_save(void *dtb)
+{
+	dtb_common.fdth = (struct _fdt_header_t *)dtb;
 }
-#endif
 
-void dtb_parse(void *arg, void *dtb)
+
+void dtb_parse(void)
 {
-	extern char _start;
+	void *dtb;
 	unsigned int d = 0;
 	u32 token, si;
 	size_t l;
@@ -157,11 +148,9 @@ void dtb_parse(void *arg, void *dtb)
 		stateSOCInterruptController
 	} state = stateIdle;
 
-	/* Copy DTB into BSS */
-	dtb_common.fdth = (struct _fdt_header_t *)dtb;
-
-	if (dtb_common.fdth->magic != ntoh32(0xd00dfeed))
+	if (dtb_common.fdth->magic != ntoh32(0xd00dfeed)) {
 		return;
+	}
 
 	dtb = (void *)dtb_common.fdth + ntoh32(dtb_common.fdth->off_dt_struct);
 	dtb_common.soc.intctl.exist = 0;
@@ -173,30 +162,24 @@ void dtb_parse(void *arg, void *dtb)
 
 		/* FDT_NODE_BEGIN */
 		if (token == 1) {
-#if 0 /* Debug */
-			char buff[2] = " ";
-			dtb_print(dtb);
-			dtb_print(" ");
-			buff[0] = '0' + d;
-			dtb_print(buff);
-			dtb_print(" ");
-			buff[0] = '0' + state;
-			dtb_print(buff);
-			dtb_print("\n");
-#endif
-
-			if (!d && (*(char *)dtb == 0))
+			if ((d == 0) && (*(char *)dtb == 0)) {
 				state = stateSystem;
-			else if ((d == 1) && !hal_strncmp(dtb, "memory@", 7))
+			}
+			else if ((d == 1) && (hal_strncmp(dtb, "memory@", 7) == 0)) {
 				state = stateMemory;
-			else if ((d == 2) && !hal_strncmp(dtb, "cpu@", 4))
+			}
+			else if ((d == 2) && (hal_strncmp(dtb, "cpu@", 4) == 0)) {
 				state = stateCPU;
-			else if ((state == stateCPU) && !hal_strncmp(dtb, "interrupt-controller", 20))
+			}
+			else if ((state == stateCPU) && (hal_strncmp(dtb, "interrupt-controller", 20) == 0)) {
 				state = stateCPUInterruptController;
-			else if ((d == 1) && !hal_strncmp(dtb, "soc", 3))
+			}
+			else if ((d == 1) && hal_strncmp(dtb, "soc", 3)) {
 				state = stateSOC;
-			else if ((state == stateSOC) && (!hal_strncmp(dtb, "interrupt-controller@", 21) || !hal_strncmp(dtb, "plic@", 5)))
+			}
+			else if ((state == stateSOC) && ((hal_strncmp(dtb, "interrupt-controller@", 21) == 0) || (hal_strncmp(dtb, "plic@", 5) == 0))) {
 				state = stateSOCInterruptController;
+			}
 
 			dtb += ((hal_strlen(dtb) + 3) & ~3);
 			d++;
@@ -212,28 +195,28 @@ void dtb_parse(void *arg, void *dtb)
 			dtb += 4;
 
 			switch (state) {
-			case stateSystem:
-				dtb_parseSystem(dtb, si, l);
-				break;
+				case stateSystem:
+					dtb_parseSystem(dtb, si, l);
+					break;
 
-			case stateMemory:
-				dtb_parseMemory(dtb, si, l);
-				break;
+				case stateMemory:
+					dtb_parseMemory(dtb, si, l);
+					break;
 
-			case stateCPU:
-				dtb_parseCPU(dtb, si, l);
-				break;
+				case stateCPU:
+					dtb_parseCPU(dtb, si, l);
+					break;
 
-			case stateCPUInterruptController:
-				dtb_parseInterruptController(dtb, si, l);
-				break;
+				case stateCPUInterruptController:
+					dtb_parseInterruptController(dtb, si, l);
+					break;
 
-			case stateSOCInterruptController:
-				dtb_parseSOCInterruptController(dtb, si, l);
-				break;
+				case stateSOCInterruptController:
+					dtb_parseSOCInterruptController(dtb, si, l);
+					break;
 
-			default:
-				break;
+				default:
+					break;
 			}
 
 			dtb += l;
@@ -242,75 +225,63 @@ void dtb_parse(void *arg, void *dtb)
 		/* FDT_NODE_END */
 		else if (token == 2) {
 			switch (state) {
-			case stateCPU:
-				dtb_common.ncpus++;
-			case stateMemory:
-				state = stateSystem;
-				break;
-
-			case stateCPUInterruptController:
-				state = stateCPU;
-				break;
-
-			case stateSOCInterruptController:
-				state = stateSOC;
-				break;
-
-			default:
-				break;
+				case stateCPU:
+					dtb_common.ncpus++;
+				case stateMemory:
+					state = stateSystem;
+					break;
+
+				case stateCPUInterruptController:
+					state = stateCPU;
+					break;
+
+				case stateSOCInterruptController:
+					state = stateSOC;
+					break;
+
+				default:
+					break;
 			}
 			d--;
 		}
-		else if (token == 9)
+		else if (token == 9) {
 			break;
+		}
 	}
-
-//	lib_printf("model: %s (%s)\n", dtb_common.model, dtb_common.compatible);
-//	lib_printf("cpu: %s@%dMHz(%s+%s)\n", dtb_common.cpus[0].compatible, dtb_common.cpus[0].clock / 1000000, dtb_common.cpus[0].isa, dtb_common.cpus[0].mmu);
-	dtb_common.start = &_start;
-}
-
-
-static void *dtb_relocate(void *addr)
-{
-	return (void *)((u64)((1L << 39) - (u64)2 * 1024 * 1024 * 1024 + addr - 0x80000000L) | (u64)0xffffff8000000000);
 }
 
 
-const void dtb_getSystem(char **model, char **compatible)
+void dtb_getSystem(char **model, char **compatible)
 {
-	*model = dtb_relocate(dtb_common.model);
-	*compatible = dtb_relocate(dtb_common.compatible);
-
-	return;
+	*model = dtb_common.model;
+	*compatible = dtb_common.compatible;
 }
 
 
 int dtb_getCPU(unsigned int n, char **compatible, u32 *clock, char **isa, char **mmu)
 {
-	if (n >= dtb_common.ncpus)
+	if (n >= dtb_common.ncpus) {
 		return -EINVAL;
+	}
 
-	*compatible = dtb_relocate(dtb_common.cpus[n].compatible);
+	*compatible = dtb_common.cpus[n].compatible;
 	*clock = dtb_common.cpus[n].clock;
-	*isa = dtb_relocate(dtb_common.cpus[n].isa);
-	*mmu = dtb_relocate(dtb_common.cpus[n].mmu);
+	*isa = dtb_common.cpus[n].isa;
+	*mmu = dtb_common.cpus[n].mmu;
 
 	return EOK;
 }
 
 
-void dtb_getMemory(u64 **reg, size_t *nreg)
+void dtb_getMemory(u8 **reg, size_t *nreg)
 {
-	*reg = dtb_relocate(dtb_common.memory.reg);
+	*reg = dtb_common.memory.reg;
 	*nreg = dtb_common.memory.nreg;
-	return;
 }
 
 
 int dtb_getPLIC(void)
 {
-//	*reg = dtb_relocate(dtb_common.memory.reg);
 	return dtb_common.soc.intctl.exist;
 }
 
@@ -319,7 +290,7 @@ void dtb_getReservedMemory(u64 **reg)
 {
 	struct _fdt_header_t *fdth;
 
-	fdth = dtb_relocate(dtb_common.fdth);
+	fdth = dtb_common.fdth;
 
 	*reg = (u64 *)((void *)fdth + ntoh32(fdth->off_mem_rsvmap));
 
@@ -329,12 +300,18 @@ void dtb_getReservedMemory(u64 **reg)
 
 void dtb_getDTBArea(u64 *dtb, u32 *dtbsz)
 {
-	struct _fdt_header_t *fdth;
+	*dtb = (u64)dtb_common.fdth - VADDR_DTB;
+	*dtbsz = ntoh32(dtb_common.fdth->totalsize);
+}
 
-	*dtb = (u64)dtb_common.fdth;
 
-	fdth = dtb_relocate(dtb_common.fdth);
-	*dtbsz = ntoh32(fdth->totalsize);
+void _dtb_init(void)
+{
+	void *dtb = dtb_common.fdth;
+	hal_memset(&dtb_common, 0, sizeof(dtb_common));
 
-	return;
+	/* DTB is mapped on giga-page */
+	dtb_common.fdth = (void *)(((ptr_t)dtb & 0x3fffffffUL) + VADDR_DTB);
+
+	dtb_parse();
 }
diff --git a/hal/riscv64/dtb.h b/hal/riscv64/dtb.h
index 7d4e3fe66..bcd284d7a 100644
--- a/hal/riscv64/dtb.h
+++ b/hal/riscv64/dtb.h
@@ -5,8 +5,8 @@
  *
  * DTB parser
  *
- * Copyright 2018 Phoenix Systems
- * Author: Pawel Pisarczyk
+ * Copyright 2018, 2024 Phoenix Systems
+ * Author: Pawel Pisarczyk, Lukasz Leczkowski
  *
  * This file is part of Phoenix-RTOS.
  *
@@ -22,25 +22,31 @@
 #define ntoh64(x) ((ntoh32(x) << 32) | ntoh32(x >> 32))
 
 
-extern void dtb_parse(void *arg, void *dtb);
+void dtb_save(void *dtb);
 
 
-extern const void dtb_getSystem(char **model, char **compatible);
+void dtb_parse(void);
 
 
-extern int dtb_getCPU(unsigned int n, char **compatible, u32 *clock, char **isa, char **mmu);
+void dtb_getSystem(char **model, char **compatible);
 
 
-extern void dtb_getMemory(u64 **reg, size_t *nreg);
+int dtb_getCPU(unsigned int n, char **compatible, u32 *clock, char **isa, char **mmu);
 
 
-extern int dtb_getPLIC(void);
+void dtb_getMemory(u8 **reg, size_t *nreg);
 
 
-extern void dtb_getReservedMemory(u64 **reg);
+int dtb_getPLIC(void);
 
 
-extern void dtb_getDTBArea(u64 *dtb, u32 *dtbsz);
+void dtb_getReservedMemory(u64 **reg);
+
+
+void dtb_getDTBArea(u64 *dtb, u32 *dtbsz);
+
+
+void _dtb_init(void);
 
 
 #endif
diff --git a/hal/riscv64/generic/Makefile b/hal/riscv64/generic/Makefile
new file mode 100644
index 000000000..3a6395801
--- /dev/null
+++ b/hal/riscv64/generic/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for riscv64-generic
+#
+# Copyright 2024 Phoenix Systems
+#
+
+OBJS += $(addprefix $(PREFIX_O)hal/riscv64/generic/, console.o generic.o)
diff --git a/hal/riscv64/config.h b/hal/riscv64/generic/config.h
similarity index 82%
rename from hal/riscv64/config.h
rename to hal/riscv64/generic/config.h
index b52c8e2b9..efaa692f2 100644
--- a/hal/riscv64/config.h
+++ b/hal/riscv64/generic/config.h
@@ -5,8 +5,8 @@
  *
  * Configuration file for riscv64
  *
- * Copyright 2021 Phoenix Systems
- * Author: Hubert Buczynski
+ * Copyright 2024 Phoenix Systems
+ * Author: Lukasz Leczkowski
  *
  * This file is part of Phoenix-RTOS.
  *
diff --git a/hal/riscv64/console.c b/hal/riscv64/generic/console.c
similarity index 97%
rename from hal/riscv64/console.c
rename to hal/riscv64/generic/console.c
index 547c20e2c..22b1f05b1 100644
--- a/hal/riscv64/console.c
+++ b/hal/riscv64/generic/console.c
@@ -15,7 +15,7 @@
 
 #include "hal/console.h"
 #include "hal/spinlock.h"
-#include "sbi.h"
+#include "hal/riscv64/sbi.h"
 
 
 static struct {
diff --git a/hal/riscv64/generic/generic.c b/hal/riscv64/generic/generic.c
new file mode 100644
index 000000000..b45f30975
--- /dev/null
+++ b/hal/riscv64/generic/generic.c
@@ -0,0 +1,56 @@
+/*
+ * Phoenix-RTOS
+ *
+ * Operating system kernel
+ *
+ * HAL internal functions for riscv64-generic
+ *
+ * Copyright 2024 Phoenix Systems
+ * Author: Lukasz Leczkowski
+ *
+ * This file is part of Phoenix-RTOS.
+ *
+ * %LICENSE%
+ */
+
+
+#include "hal/hal.h"
+#include "include/arch/riscv64/riscv64.h"
+
+static struct {
+	spinlock_t lock;
+} generic_common;
+
+
+int hal_platformctl(void *ptr)
+{
+	platformctl_t *pctl = ptr;
+	spinlock_ctx_t sc;
+	int ret = -1;
+
+	hal_spinlockSet(&generic_common.lock, &sc);
+
+	switch (pctl->type) {
+		case pctl_reboot:
+			if ((pctl->action == pctl_set) && (pctl->task.reboot.magic == PCTL_REBOOT_MAGIC)) {
+				hal_cpuReboot();
+			}
+			break;
+
+		case pctl_iomux:
+			ret = 0;
+			break;
+
+		default:
+			break;
+	}
+	hal_spinlockClear(&generic_common.lock, &sc);
+
+	return ret;
+}
+
+
+void _hal_platformInit(void)
+{
+	hal_spinlockCreate(&generic_common.lock, "generic_common.lock");
+}
diff --git a/hal/riscv64/hal.c b/hal/riscv64/hal.c
index d49684b90..41ca554d2 100644
--- a/hal/riscv64/hal.c
+++ b/hal/riscv64/hal.c
@@ -13,13 +13,12 @@
  * %LICENSE%
  */
 
+#include "dtb.h"
 #include "hal/hal.h"
-#include "include/arch/riscv64/riscv64.h"
 
 
 static struct {
 	volatile u32 started;
-	spinlock_t pltctlSp;
 } hal_common;
 
 
@@ -31,6 +30,9 @@ volatile u32 hal_multilock;
 extern void _hal_cpuInit(void);
 
 
+extern void _hal_platformInit(void);
+
+
 void *hal_syspageRelocate(void *data)
 {
 	return ((u8 *)data + hal_relOffs);
@@ -77,44 +79,22 @@ void hal_lockScheduler(void)
 }
 
 
-int hal_platformctl(void *ptr)
-{
-	platformctl_t *pctl = ptr;
-	spinlock_ctx_t sc;
-	int ret = -1;
-
-	hal_spinlockSet(&hal_common.pltctlSp, &sc);
-
-	switch (pctl->type) {
-		case pctl_reboot:
-			if ((pctl->action == pctl_set) && (pctl->task.reboot.magic == PCTL_REBOOT_MAGIC)) {
-				hal_cpuReboot();
-			}
-			break;
-
-		default:
-			break;
-	}
-	hal_spinlockClear(&hal_common.pltctlSp, &sc);
-
-	return ret;
-}
-
-
 __attribute__((section(".init"))) void _hal_init(void)
 {
-	_hal_sbiInit();
+	hal_common.started = 0;
+	hal_multilock = 0u;
+
 	_hal_spinlockInit();
-	_hal_consoleInit();
+	_dtb_init();
+	_pmap_halInit();
+	_hal_sbiInit();
 
 	_hal_exceptionsInit();
 	_hal_interruptsInit();
 
+	_hal_consoleInit();
 	_hal_timerInit(SYSTICK_INTERVAL);
 
-	hal_spinlockCreate(&hal_common.pltctlSp, "pltctl");
-	hal_common.started = 0;
-	hal_multilock = 0u;
-
+	_hal_platformInit();
 	_hal_cpuInit();
 }
diff --git a/hal/riscv64/interrupts.c b/hal/riscv64/interrupts.c
index 7bb1e1822..c1a686a25 100644
--- a/hal/riscv64/interrupts.c
+++ b/hal/riscv64/interrupts.c
@@ -252,6 +252,7 @@ __attribute__((section(".init"))) void _hal_interruptsInit(void)
 {
 	unsigned int i;
 
+	csr_write(stvec, _interrupts_dispatch);
 	for (i = 0; i < CLINT_IRQ_SIZE; i++) {
 		interrupts_common.clint.handlers[i] = NULL;
 		interrupts_common.clint.counters[i] = 0;
diff --git a/hal/riscv64/noelv/Makefile b/hal/riscv64/noelv/Makefile
new file mode 100644
index 000000000..46b3a11d4
--- /dev/null
+++ b/hal/riscv64/noelv/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for riscv64-noelv
+#
+# Copyright 2024 Phoenix Systems
+#
+
+OBJS += $(addprefix $(PREFIX_O)hal/$(TARGET_SUFF)/$(TARGET_SUBFAMILY)/, ambapp.o console.o noelv.o)
diff --git a/hal/riscv64/noelv/ambapp.c b/hal/riscv64/noelv/ambapp.c
new file mode 100644
index 000000000..14bc28d69
--- /dev/null
+++ b/hal/riscv64/noelv/ambapp.c
@@ -0,0 +1,235 @@
+/*
+ * Phoenix-RTOS
+ *
+ * Operating system kernel
+ *
+ * AMBA Plug'n'Play
+ *
+ * Copyright 2023, 2024 Phoenix Systems
+ * Author: Lukasz Leczkowski
+ *
+ * This file is part of Phoenix-RTOS.
+ *
+ * %LICENSE%
+ */
+
+#include <arch/cpu.h>
+#include <config.h>
+
+#include "ambapp.h"
+
+#include "arch/pmap.h"
+#include "hal/spinlock.h"
+#include "hal/string.h"
+#include "include/errno.h"
+
+
+#define AMBAPP_AHB_MSTR      ((addr_t)0xfffff000)
+#define AMBAPP_AHB_MSTR_OFFS ((ptr_t)0x0)
+#define AMBAPP_AHB_SLV_OFFS  ((ptr_t)0x800)
+#define AMBAPP_APB_OFFS      ((ptr_t)0xff000) /* PnP offset relative to APB bridge base address */
+
+#define AMBAPP_AHB_NMASTERS 16
+#define AMBAPP_AHB_NSLAVES  16
+#define AMBAPP_APB_NSLAVES  16
+
+#define AMBAPP_MAX_APBBRIDGES 16
+
+#define AMBAPP_VEN(id)  ((id) >> 24)
+#define AMBAPP_DEV(id)  (((id) >> 12) & 0xfff)
+#define AMBAPP_VER(id)  (((id) >> 5) & 0x1f)
+#define AMBAPP_IRQN(id) ((id) & 0x1f)
+
+#define AMBAPP_AHB_ADDR(bar)           (ptr_t)(((bar) & 0xfff00000u) & (((bar) & 0xfff0u) << 16))
+#define AMBAPP_AHBIO_ADDR(ioarea, bar) (ptr_t)((ioarea) | ((bar) >> 12))
+#define AMBAPP_APB_ADDR(base, bar)     (ptr_t)((base) | ((((bar) & 0xfff00000u) >> 12) & (((bar) & 0xfff0u) << 4)))
+#define AMBAPP_TYPE(bar)               ((bar) & 0xfu)
+
+
+typedef struct {
+	u32 id;
+	u32 bar;
+} ambapp_apb_dev_t;
+
+
+typedef struct {
+	u32 id;
+	u32 reserved[3];
+	u32 bar[4];
+} ambapp_ahb_dev_t;
+
+
+static struct {
+	spinlock_t lock;
+	ptr_t ahbpnp;
+	ptr_t apbpnp;
+} ambapp_common;
+
+
+static void ambapp_fillApbDev(addr_t apb, ambapp_dev_t *dev, ambapp_apb_dev_t *apbdev)
+{
+	u32 info = apbdev->id;
+
+	dev->vendor = AMBAPP_VEN(info);
+	dev->irqn = AMBAPP_IRQN(info);
+	dev->bus = BUS_AMBA_APB;
+
+	info = apbdev->bar;
+	dev->info.apb.base = (u32 *)AMBAPP_APB_ADDR(apb, info);
+	dev->info.apb.type = AMBAPP_TYPE(info);
+}
+
+
+static int ambapp_apbFind(addr_t apb, ambapp_dev_t *dev, unsigned int *instance)
+{
+	u32 i, id;
+	spinlock_ctx_t sc;
+	ambapp_apb_dev_t *apbdev = (void *)(apb + AMBAPP_APB_OFFS);
+
+	hal_spinlockSet(&ambapp_common.lock, &sc);
+
+	/* Map bridge PnP */
+	apbdev = pmap_halMap((addr_t)apbdev, (void *)ambapp_common.apbpnp, SIZE_PAGE, PGHD_READ | PGHD_PRESENT);
+
+	for (i = 0; i < AMBAPP_APB_NSLAVES; i++) {
+		id = apbdev[i].id;
+		if (AMBAPP_DEV(id) == dev->devId) {
+			(*instance)--;
+			if (*instance == -1) {
+				/* Found desired device, fill struct and return */
+				ambapp_fillApbDev(apb, dev, &apbdev[i]);
+				hal_spinlockClear(&ambapp_common.lock, &sc);
+				return 0;
+			}
+		}
+	}
+	hal_spinlockClear(&ambapp_common.lock, &sc);
+
+	return -1;
+}
+
+
+static void ambapp_fillAhbDev(ambapp_dev_t *dev, ambapp_ahb_dev_t *ahbdev)
+{
+	u8 bar;
+	addr_t addr;
+	u32 info = ahbdev->id;
+
+	dev->vendor = AMBAPP_VEN(info);
+	dev->irqn = AMBAPP_IRQN(info);
+	dev->bus = BUS_AMBA_AHB;
+
+	for (bar = 0; bar < 4; bar++) {
+		info = ahbdev->bar[bar];
+
+		if (info == 0) {
+			dev->info.ahb.base[bar] = 0;
+			dev->info.ahb.type[bar] = 0;
+		}
+		else {
+			addr = AMBAPP_AHB_ADDR(info);
+			if (AMBAPP_TYPE(info) == AMBA_TYPE_AHBIO) {
+				dev->info.ahb.base[bar] = (u32 *)AMBAPP_AHBIO_ADDR(AHB_IOAREA, addr);
+			}
+			else {
+				dev->info.ahb.base[bar] = (u32 *)AMBAPP_AHB_ADDR(info);
+			}
+			dev->info.ahb.type[bar] = AMBAPP_TYPE(info);
+		}
+	}
+}
+
+
+static void ambapp_addBridge(addr_t *bridges, size_t len, addr_t addr)
+{
+	size_t i, j;
+	addr_t curr;
+	for (i = 0; i < len; i++) {
+		curr = bridges[i];
+		if (curr == 0xffffffffu) {
+			bridges[i] = addr;
+			return;
+		}
+		if (addr < curr) {
+			for (j = len - 1; j > i; j--) {
+				bridges[j] = bridges[j - 1];
+			}
+			bridges[i] = addr;
+			return;
+		}
+	}
+}
+
+
+static int ambapp_ahbFind(ptr_t pnpOff, u32 ndevs, ambapp_dev_t *dev, unsigned int *instance)
+{
+	u32 i, id, bar, val;
+	addr_t apb;
+	ambapp_ahb_dev_t *ahbdev = (ambapp_ahb_dev_t *)(ambapp_common.ahbpnp + pnpOff);
+	addr_t apbBridges[AMBAPP_AHB_NSLAVES];
+	hal_memset(apbBridges, 0xff, sizeof(apbBridges));
+
+	for (i = 0; i < ndevs; i++) {
+		/* Scan AHB PnP */
+		id = ahbdev[i].id;
+
+		if (AMBAPP_DEV(id) == dev->devId) {
+			/* Found desired device in AHB */
+			(*instance)--;
+			if (*instance == -1) {
+				ambapp_fillAhbDev(dev, &ahbdev[i]);
+				return 0;
+			}
+		}
+
+		else if (AMBAPP_DEV(id) == CORE_ID_APBCTRL) {
+			/* Found APB Bridge */
+			for (bar = 0; bar < 4; bar++) {
+				val = ahbdev[i].bar[bar];
+				if (AMBAPP_TYPE(val) == AMBA_TYPE_AHBMEM) {
+					apb = AMBAPP_AHB_ADDR(val);
+					ambapp_addBridge(apbBridges, AMBAPP_AHB_NSLAVES, apb);
+				}
+			}
+		}
+	}
+
+	for (i = 0; i < AMBAPP_AHB_NSLAVES; i++) {
+		/* Scan APB PnP */
+		if (apbBridges[i] == 0xffffffffu) {
+			break;
+		}
+		if (ambapp_apbFind(apbBridges[i], dev, instance) == 0) {
+			return 0;
+		}
+	}
+
+	return -1;
+}
+
+
+int ambapp_findMaster(ambapp_dev_t *dev, unsigned int *instance)
+{
+	if (ambapp_ahbFind(AMBAPP_AHB_MSTR_OFFS, AMBAPP_AHB_NMASTERS, dev, instance) < 0) {
+		return -ENODEV;
+	}
+	return EOK;
+}
+
+
+int ambapp_findSlave(ambapp_dev_t *dev, unsigned int *instance)
+{
+	if (ambapp_ahbFind(AMBAPP_AHB_SLV_OFFS, AMBAPP_AHB_NSLAVES, dev, instance) < 0) {
+		return -ENODEV;
+	}
+	return EOK;
+}
+
+
+void ambapp_init(void)
+{
+	hal_spinlockCreate(&ambapp_common.lock, "ambapp_common.lock");
+
+	ambapp_common.ahbpnp = (ptr_t)_pmap_halMap(AMBAPP_AHB_MSTR, NULL, SIZE_PAGE, PGHD_READ | PGHD_PRESENT);
+	ambapp_common.apbpnp = (ptr_t)_pmap_halMap(NULL, NULL, SIZE_PAGE, PGHD_READ | PGHD_PRESENT);
+}
diff --git a/hal/riscv64/noelv/ambapp.h b/hal/riscv64/noelv/ambapp.h
new file mode 100644
index 000000000..18cfc3c25
--- /dev/null
+++ b/hal/riscv64/noelv/ambapp.h
@@ -0,0 +1,32 @@
+/*
+ * Phoenix-RTOS
+ *
+ * Operating system kernel
+ *
+ * AMBA Plug'n'Play
+ *
+ * Copyright 2023, 2024 Phoenix Systems
+ * Author: Lukasz Leczkowski
+ *
+ * This file is part of Phoenix-RTOS.
+ *
+ * %LICENSE%
+ */
+
+#ifndef _HAL_AMBAPP_H_
+#define _HAL_AMBAPP_H_
+
+
+#include "include/gaisler/ambapp.h"
+
+
+int ambapp_findMaster(ambapp_dev_t *dev, unsigned int *instance);
+
+
+int ambapp_findSlave(ambapp_dev_t *dev, unsigned int *instance);
+
+
+void ambapp_init(void);
+
+
+#endif
diff --git a/hal/riscv64/noelv/config.h b/hal/riscv64/noelv/config.h
new file mode 100644
index 000000000..4592539e5
--- /dev/null
+++ b/hal/riscv64/noelv/config.h
@@ -0,0 +1,29 @@
+/*
+ * Phoenix-RTOS
+ *
+ * Operating system kernel
+ *
+ * Configuration file for riscv64
+ *
+ * Copyright 2024 Phoenix Systems
+ * Author: Lukasz Leczkowski
+ *
+ * This file is part of Phoenix-RTOS.
+ *
+ * %LICENSE%
+ */
+
+#ifndef _HAL_CONFIG_H_
+#define _HAL_CONFIG_H_
+
+#ifndef __ASSEMBLY__
+
+#include "include/arch/riscv64/syspage.h"
+#include "include/syspage.h"
+
+
+#define AHB_IOAREA 0xfff00000u
+
+#endif
+
+#endif
diff --git a/hal/riscv64/noelv/console.c b/hal/riscv64/noelv/console.c
new file mode 100644
index 000000000..a93b40ede
--- /dev/null
+++ b/hal/riscv64/noelv/console.c
@@ -0,0 +1,99 @@
+/*
+ * Phoenix-RTOS
+ *
+ * Operating system kernel
+ *
+ * HAL console - GRLIB UART
+ *
+ * Copyright 2022, 2024 Phoenix Systems
+ * Author: Lukasz Leczkowski
+ *
+ * This file is part of Phoenix-RTOS.
+ *
+ * %LICENSE%
+ */
+
+#include <arch/cpu.h>
+#include <arch/pmap.h>
+
+#include "hal/console.h"
+#include "hal/types.h"
+#include "hal/riscv64/riscv64.h"
+
+#include <board_config.h>
+
+/* UART control bits */
+#define TX_EN (1 << 1)
+
+/* UART status bits */
+#define TX_FIFO_FULL (1 << 9)
+
+
+enum {
+	uart_data = 0, /* Data register           : 0x00 */
+	uart_status,   /* Status register         : 0x04 */
+	uart_ctrl,     /* Control register        : 0x08 */
+	uart_scaler,   /* Scaler reload register  : 0x0C */
+	uart_dbg       /* FIFO debug register     : 0x10 */
+};
+
+
+struct {
+	volatile u32 *uart;
+} halconsole_common;
+
+
+static void _hal_consolePrint(const char *s)
+{
+	for (; *s; s++) {
+		hal_consolePutch(*s);
+	}
+
+	/* Wait until TX fifo is empty */
+	while ((*(halconsole_common.uart + uart_status) & TX_FIFO_FULL) != 0) {
+	}
+}
+
+
+static u32 _hal_consoleCalcScaler(u32 baud)
+{
+	return (SYSCLK_FREQ / (baud * 8 + 7));
+}
+
+
+void hal_consolePutch(char c)
+{
+	/* Wait until TX fifo is empty */
+	while ((*(halconsole_common.uart + uart_status) & TX_FIFO_FULL) != 0) {
+	}
+	*(halconsole_common.uart + uart_data) = c;
+}
+
+
+void hal_consolePrint(int attr, const char *s)
+{
+	if (attr == ATTR_BOLD) {
+		_hal_consolePrint(CONSOLE_BOLD);
+	}
+	else if (attr != ATTR_USER) {
+		_hal_consolePrint(CONSOLE_CYAN);
+	}
+
+	_hal_consolePrint(s);
+	_hal_consolePrint(CONSOLE_NORMAL);
+}
+
+
+void _hal_consoleInit(void)
+{
+	halconsole_common.uart = _pmap_halMap((addr_t)UART0_BASE, NULL, SIZE_PAGE, PGHD_READ | PGHD_WRITE | PGHD_PRESENT);
+	*(halconsole_common.uart + uart_ctrl) = 0;
+
+	/* Clear UART FIFO */
+	while ((*(halconsole_common.uart + uart_status) & (1 << 0)) != 0) {
+		(void)*(halconsole_common.uart + uart_data);
+	}
+	*(halconsole_common.uart + uart_scaler) = _hal_consoleCalcScaler(UART_BAUDRATE);
+	RISCV_FENCE(w, o);
+	*(halconsole_common.uart + uart_ctrl) = TX_EN;
+}
diff --git a/hal/riscv64/noelv/noelv.c b/hal/riscv64/noelv/noelv.c
new file mode 100644
index 000000000..0a7cfd1ce
--- /dev/null
+++ b/hal/riscv64/noelv/noelv.c
@@ -0,0 +1,64 @@
+/*
+ * Phoenix-RTOS
+ *
+ * Operating system kernel
+ *
+ * HAL internal functions for riscv64-noelv
+ *
+ * Copyright 2024 Phoenix Systems
+ * Author: Lukasz Leczkowski
+ *
+ * This file is part of Phoenix-RTOS.
+ *
+ * %LICENSE%
+ */
+
+#include "ambapp.h"
+
+#include "hal/hal.h"
+#include "include/arch/riscv64/riscv64.h"
+
+static struct {
+	spinlock_t lock;
+} noelv_common;
+
+
+int hal_platformctl(void *ptr)
+{
+	platformctl_t *pctl = ptr;
+	spinlock_ctx_t sc;
+	int ret = -1;
+
+	hal_spinlockSet(&noelv_common.lock, &sc);
+
+	switch (pctl->type) {
+		case pctl_reboot:
+			if ((pctl->action == pctl_set) && (pctl->task.reboot.magic == PCTL_REBOOT_MAGIC)) {
+				hal_cpuReboot();
+			}
+			break;
+
+		case pctl_iomux:
+			ret = 0;
+			break;
+
+		case pctl_ambapp:
+			if (pctl->action == pctl_get) {
+				ret = ambapp_findSlave(pctl->task.ambapp.dev, pctl->task.ambapp.instance);
+			}
+			break;
+
+		default:
+			break;
+	}
+	hal_spinlockClear(&noelv_common.lock, &sc);
+
+	return ret;
+}
+
+
+void _hal_platformInit(void)
+{
+	hal_spinlockCreate(&noelv_common.lock, "noelv_common.lock");
+	ambapp_init();
+}
diff --git a/hal/riscv64/plic.c b/hal/riscv64/plic.c
index e714829c4..512193200 100644
--- a/hal/riscv64/plic.c
+++ b/hal/riscv64/plic.c
@@ -13,8 +13,11 @@
  * %LICENSE%
  */
 
+#include <arch/pmap.h>
+
 #include "plic.h"
 #include "hal/cpu.h"
+#include "riscv64.h"
 
 #include <board_config.h>
 
@@ -29,6 +32,9 @@
 
 /* clang-format on */
 
+/* Value calculated from MAX_CPU_COUNT (2 contexts/cpu), TODO(?): get from DTB */
+#define PLIC_SIZE PLIC_REG_THRESHOLD(2 * MAX_CPU_COUNT)
+
 
 static struct {
 	volatile u8 *regw;
@@ -37,12 +43,15 @@ static struct {
 
 u32 plic_read(unsigned int reg)
 {
-	return (u32)(*(volatile u32 *)(plic_common.regw + reg));
+	u32 ret = *(volatile u32 *)(plic_common.regw + reg);
+	RISCV_FENCE(i, r);
+	return ret;
 }
 
 
 void plic_write(unsigned int reg, u32 v)
 {
+	RISCV_FENCE(w, o);
 	*(volatile u32 *)(plic_common.regw + reg) = v;
 }
 
@@ -141,7 +150,7 @@ void plic_init(void)
 {
 	unsigned int i;
 
-	plic_common.regw = (void *)(0xffffffffc0000000 + PLIC_BASE);
+	plic_common.regw = _pmap_halMap((addr_t)PLIC_BASE, NULL, PLIC_SIZE, PGHD_READ | PGHD_WRITE | PGHD_PRESENT);
 
 	/* Disable and mask external interrupts */
 	for (i = 1; i < PLIC_IRQ_SIZE; i++) {
diff --git a/hal/riscv64/pmap.c b/hal/riscv64/pmap.c
index cd12288e8..b0c58cfb3 100644
--- a/hal/riscv64/pmap.c
+++ b/hal/riscv64/pmap.c
@@ -15,6 +15,7 @@
 
 
 #include "hal/pmap.h"
+#include "hal/hal.h"
 #include "hal/spinlock.h"
 #include "hal/string.h"
 #include "halsyspage.h"
@@ -25,6 +26,8 @@
 #include "include/errno.h"
 #include "include/mman.h"
 
+#include <board_config.h>
+
 
 #define PDIR2_IDX(va) (((ptr_t)(va) >> 30) & 0x1ffUL)
 #define PDIR1_IDX(va) (((ptr_t)(va) >> 21) & 0x1ffUL)
@@ -36,7 +39,7 @@
 /* PTE attributes */
 #define PTE_V (1UL << 0)
 
-#define CEIL_PAGE(x) (((x) + SIZE_PAGE - 1) & ~(SIZE_PAGE - 1))
+#define CEIL_PAGE(x) ((((addr_t)x) + SIZE_PAGE - 1) & ~(SIZE_PAGE - 1))
 
 /* Linker symbols */
 extern unsigned int _start;
@@ -44,6 +47,16 @@ extern unsigned int _end;
 extern unsigned int _etext;
 
 
+#define PMAP_MEM_ENTRIES 64
+
+
+typedef struct {
+	addr_t start;
+	size_t pageCount;
+	int flags;
+} pmap_memEntry_t;
+
+
 struct {
 	/* The order of below fields should be stricly preserved */
 	u64 pdir2[512];
@@ -68,6 +81,13 @@ struct {
 
 	addr_t kernel;
 	size_t kernelsz;
+	ptr_t vkernelEnd;
+
+	struct {
+		pmap_memEntry_t entries[PMAP_MEM_ENTRIES];
+		size_t count;
+	} memMap;
+	addr_t pageIterator;
 } __attribute__((aligned(SIZE_PAGE))) pmap_common;
 
 
@@ -128,34 +148,40 @@ void pmap_switch(pmap_t *pmap)
 }
 
 
-static int _pmap_enter(pmap_t *pmap, addr_t pa, void *va, int attr, page_t *alloc, int tlbInval)
+static int _pmap_map(u64 *pdir2, addr_t pa, void *vaddr, int attr, page_t *alloc)
 {
 	addr_t addr;
 
-	unsigned int pdi2 = PDIR2_IDX(va);
-	unsigned int pdi1 = PDIR1_IDX(va);
-	unsigned int pti = PDIR0_IDX(va);
+	unsigned int pdi2 = PDIR2_IDX(vaddr);
+	unsigned int pdi1 = PDIR1_IDX(vaddr);
+	unsigned int pti = PDIR0_IDX(vaddr);
+
+	if ((attr & PGHD_WRITE) != 0) {
+		/* RISC-V ISA: w/wx mapping reserved for future use */
+		attr |= PGHD_READ;
+	}
 
 	/* If no page table is allocated add new one */
-	if ((pmap->pdir2[pdi2] & PTE_V) == 0) {
+	if ((pdir2[pdi2] & PTE_V) == 0) {
 		if (alloc == NULL) {
 			return -EFAULT;
 		}
 
-		pmap->pdir2[pdi2] = PTE(alloc->addr, PTE_V);
+		pdir2[pdi2] = PTE(alloc->addr, PTE_V);
 
 		/* Initialize pdir (MOD) - because of reentrancy */
 		pmap_common.pdir0[PDIR0_IDX(pmap_common.ptable)] = PTE(alloc->addr, 0xc7);
-		hal_cpuLocalFlushTLB(pmap, pmap_common.ptable);
+		hal_cpuLocalFlushTLB(0, pmap_common.ptable);
 		hal_memset(pmap_common.ptable, 0, sizeof(pmap_common.ptable));
 
 		alloc = NULL;
 	}
 	else {
 		/* Map next level pdir */
-		addr = PTE_TO_ADDR(pmap->pdir2[pdi2]);
+		addr = PTE_TO_ADDR(pdir2[pdi2]);
 		pmap_common.pdir0[PDIR0_IDX(pmap_common.ptable)] = PTE(addr, 0xc7);
-		hal_cpuLocalFlushTLB(pmap, pmap_common.ptable);
+		hal_cpuLocalFlushTLB(0, pmap_common.ptable);
+		hal_cpuDCacheInval(pmap_common.ptable, sizeof(pmap_common.ptable));
 	}
 
 	if ((pmap_common.ptable[pdi1] & PTE_V) == 0) {
@@ -163,37 +189,55 @@ static int _pmap_enter(pmap_t *pmap, addr_t pa, void *va, int attr, page_t *allo
 			return -EFAULT;
 		}
 		pmap_common.pdir0[PDIR0_IDX(pmap_common.ptable)] = PTE(alloc->addr, 0xc7);
-		hal_cpuLocalFlushTLB(pmap, pmap_common.ptable);
+		hal_cpuLocalFlushTLB(0, pmap_common.ptable);
 		hal_memset(pmap_common.ptable, 0, sizeof(pmap_common.ptable));
 
-		addr = PTE_TO_ADDR(pmap->pdir2[pdi2]);
+		addr = PTE_TO_ADDR(pdir2[pdi2]);
 		pmap_common.pdir0[PDIR0_IDX(pmap_common.ptable)] = PTE(addr, 0xc7);
-		hal_cpuLocalFlushTLB(pmap, pmap_common.ptable);
+		hal_cpuLocalFlushTLB(0, pmap_common.ptable);
 
 		pmap_common.ptable[pdi1] = PTE(alloc->addr, PTE_V);
-
-		alloc = NULL;
 	}
 
 	/* Map next level pdir */
 	addr = PTE_TO_ADDR(pmap_common.ptable[pdi1]);
 	pmap_common.pdir0[PDIR0_IDX(pmap_common.ptable)] = PTE(addr, 0xc7);
-	hal_cpuLocalFlushTLB(pmap, pmap_common.ptable);
+	hal_cpuLocalFlushTLB(0, pmap_common.ptable);
+
+	if ((pmap_common.ptable[pti] & (PGHD_WRITE | PTE_V)) == (PGHD_WRITE | PTE_V)) {
+		hal_cpuDCacheFlush(vaddr, SIZE_PAGE);
+		hal_cpuDCacheInval(vaddr, SIZE_PAGE);
+	}
 
 	/* And at last map page or only changle attributes of map entry */
 	pmap_common.ptable[pti] = PTE(pa, 0xc1 | (attr & 0x3f));
 
-	RISCV_FENCE(rw, rw);
-	hal_cpuInstrBarrier();
-	if ((attr & PGHD_EXEC) != 0) {
-		hal_cpuRfenceI();
+	RISCV_FENCE(w, rw);
+
+	return EOK;
+}
+
+
+static int _pmap_enter(u64 *pdir2, addr_t pa, void *vaddr, int attr, page_t *alloc, int tlbInval)
+{
+	int ret = _pmap_map(pdir2, pa, vaddr, attr, alloc);
+	if (ret < 0) {
+		return ret;
 	}
 
 	if (tlbInval != 0) {
-		hal_cpuRemoteFlushTLB(pmap, va, SIZE_PAGE);
+		hal_cpuRemoteFlushTLB(0, vaddr, SIZE_PAGE);
 	}
 	else {
-		hal_cpuLocalFlushTLB(pmap, va);
+		hal_cpuLocalFlushTLB(0, vaddr);
+	}
+
+	if ((attr & PGHD_WRITE) != 0) {
+		hal_cpuDCacheInval(vaddr, SIZE_PAGE);
+	}
+	hal_cpuInstrBarrier();
+	if ((attr & PGHD_EXEC) != 0) {
+		hal_cpuRfenceI();
 	}
 
 	return EOK;
@@ -201,20 +245,20 @@ static int _pmap_enter(pmap_t *pmap, addr_t pa, void *va, int attr, page_t *allo
 
 
 /* Functions maps page at specified address (Sv39) */
-int pmap_enter(pmap_t *pmap, addr_t pa, void *va, int attr, page_t *alloc)
+int pmap_enter(pmap_t *pmap, addr_t pa, void *vaddr, int attr, page_t *alloc)
 {
 	int ret;
 	spinlock_ctx_t sc;
 
 	hal_spinlockSet(&pmap_common.lock, &sc);
-	ret = _pmap_enter(pmap, pa, va, attr, alloc, 1);
+	ret = _pmap_enter(pmap->pdir2, pa, vaddr, attr, alloc, 1);
 	hal_spinlockClear(&pmap_common.lock, &sc);
 
 	return ret;
 }
 
 
-static void _pmap_remove(pmap_t *pmap, void *vaddr)
+static u8 _pmap_remove(pmap_t *pmap, void *vaddr)
 {
 	addr_t addr, entry;
 
@@ -226,44 +270,56 @@ static void _pmap_remove(pmap_t *pmap, void *vaddr)
 
 	entry = pmap->pdir2[pdi2];
 	if ((entry & PTE_V) == 0) {
-		return;
+		return 0;
 	}
 
 	addr = PTE_TO_ADDR(entry);
 
 	pmap_common.pdir0[PDIR0_IDX(pmap_common.ptable)] = PTE(addr, 0xc7);
-	hal_cpuLocalFlushTLB(pmap, pmap_common.ptable);
+	hal_cpuLocalFlushTLB(0, pmap_common.ptable);
+	hal_cpuDCacheInval(pmap_common.ptable, sizeof(pmap_common.ptable));
 
 	entry = pmap_common.ptable[pdi1];
 
 	if ((entry & PTE_V) != 0) {
 		addr = PTE_TO_ADDR(entry);
 		pmap_common.pdir0[PDIR0_IDX(pmap_common.ptable)] = PTE(addr, 0xc7);
-		hal_cpuLocalFlushTLB(pmap, pmap_common.ptable);
+		hal_cpuLocalFlushTLB(0, pmap_common.ptable);
+		hal_cpuDCacheInval(pmap_common.ptable, sizeof(pmap_common.ptable));
 
-		if ((pmap_common.ptable[pti] & PGHD_EXEC) != 0) {
+		entry = pmap_common.ptable[pti];
+
+		if ((entry & PGHD_EXEC) != 0) {
 			isync = 1;
 		}
 
+		if ((entry & (PGHD_WRITE | PTE_V)) == (PGHD_WRITE | PTE_V)) {
+			hal_cpuDCacheFlush(vaddr, SIZE_PAGE);
+			hal_cpuDCacheInval(vaddr, SIZE_PAGE);
+		}
+
 		pmap_common.ptable[pti] = 0;
 	}
 
-	RISCV_FENCE(rw, rw);
-	hal_cpuInstrBarrier();
+	RISCV_FENCE(w, rw);
 
-	if (isync != 0) {
-		hal_cpuRfenceI();
-	}
+	return isync;
 }
 
 
 int pmap_remove(pmap_t *pmap, void *vaddr)
 {
 	spinlock_ctx_t sc;
+	u8 isync;
 
 	hal_spinlockSet(&pmap_common.lock, &sc);
-	_pmap_remove(pmap, vaddr);
-	hal_cpuRemoteFlushTLB(pmap, vaddr, SIZE_PAGE);
+	isync = _pmap_remove(pmap, vaddr);
+	hal_cpuRemoteFlushTLB(0, vaddr, SIZE_PAGE);
+	hal_cpuInstrBarrier();
+
+	if (isync != 0) {
+		hal_cpuRfenceI();
+	}
 	hal_spinlockClear(&pmap_common.lock, &sc);
 
 	return EOK;
@@ -290,12 +346,14 @@ addr_t pmap_resolve(pmap_t *pmap, void *vaddr)
 	addr = PTE_TO_ADDR(pmap->pdir2[pdi2]);
 
 	pmap_common.pdir0[PDIR0_IDX(pmap_common.ptable)] = PTE(addr, 0xc7);
-	hal_cpuLocalFlushTLB(pmap, pmap_common.ptable);
+	hal_cpuLocalFlushTLB(0, pmap_common.ptable);
+	hal_cpuDCacheInval(pmap_common.ptable, sizeof(pmap_common.ptable));
 
 	addr = PTE_TO_ADDR(pmap_common.ptable[pdi1]);
 
 	pmap_common.pdir0[PDIR0_IDX(pmap_common.ptable)] = PTE(addr, 0xc7);
-	hal_cpuLocalFlushTLB(pmap, pmap_common.ptable);
+	hal_cpuLocalFlushTLB(0, pmap_common.ptable);
+	hal_cpuDCacheInval(pmap_common.ptable, sizeof(pmap_common.ptable));
 
 	addr = PTE_TO_ADDR(pmap_common.ptable[pti]);
 	hal_spinlockClear(&pmap_common.lock, &sc);
@@ -307,9 +365,11 @@ addr_t pmap_resolve(pmap_t *pmap, void *vaddr)
 /* Function fills page_t structure for frame given by addr */
 int pmap_getPage(page_t *page, addr_t *addr)
 {
+	size_t i;
 	addr_t a;
 	spinlock_ctx_t sc;
 	const syspage_prog_t *prog;
+	const pmap_memEntry_t *entry;
 
 	a = *addr & ~(SIZE_PAGE - 1);
 	page->flags = 0;
@@ -318,8 +378,8 @@ int pmap_getPage(page_t *page, addr_t *addr)
 	hal_spinlockSet(&pmap_common.lock, &sc);
 
 	/* Ignore SBI area */
-	if (((a >= 0x80000000UL) && (a < 0x80200000UL)) || (a < pmap_common.minAddr)) {
-		a = 0x80200000;
+	if (((a >= SBI_AREA_START) && (a < SBI_AREA_END)) || (a < pmap_common.minAddr)) {
+		a = SBI_AREA_END;
 	}
 
 	if (a >= pmap_common.maxAddr) {
@@ -333,6 +393,14 @@ int pmap_getPage(page_t *page, addr_t *addr)
 	page->flags = 0;
 	*addr = a + SIZE_PAGE;
 
+	for (i = 0; i < pmap_common.memMap.count; i++) {
+		entry = &pmap_common.memMap.entries[i];
+		if ((a >= entry->start) && ((a - entry->start) < (entry->pageCount * SIZE_PAGE))) {
+			page->flags = entry->flags;
+			return EOK;
+		}
+	}
+
 	if (hal_syspage->progs != NULL) {
 		prog = hal_syspage->progs;
 		do {
@@ -387,15 +455,15 @@ int _pmap_kernelSpaceExpand(pmap_t *pmap, void **start, void *end, page_t *dp)
 
 
 	for (; vaddr < end; vaddr += (1ULL << 30)) {
-		if (_pmap_enter(pmap, 0, vaddr, ~PGHD_PRESENT, NULL, 0) < 0) {
-			if (_pmap_enter(pmap, 0, vaddr, ~PGHD_PRESENT, dp, 0) < 0) {
+		if (_pmap_enter(pmap->pdir2, 0, vaddr, ~PGHD_PRESENT, NULL, 0) < 0) {
+			if (_pmap_enter(pmap->pdir2, 0, vaddr, ~PGHD_PRESENT, dp, 0) < 0) {
 				return -ENOMEM;
 			}
 			dp = NULL;
 		}
 		*start = vaddr;
 	}
-	hal_cpuLocalFlushTLB(NULL, NULL);
+	hal_cpuLocalFlushTLB(0, NULL);
 
 	pmap->start = (void *)VADDR_KERNEL;
 	pmap->end = end;
@@ -417,39 +485,146 @@ char pmap_marker(page_t *p)
 }
 
 
-/* Function initializes low-level page mapping interface */
-void _pmap_init(pmap_t *pmap, void **vstart, void **vend)
+int pmap_segment(unsigned int i, void **vaddr, size_t *size, int *prot, void **top)
 {
-	void *v;
-	struct {
-		u64 addr;
-		u64 limit;
-	} *m, *r;
-	size_t n, i;
-	u64 a, l;
+	switch (i) {
+		case 0:
+			*vaddr = (void *)VADDR_KERNEL;
+			*size = (ptr_t)&_etext - VADDR_KERNEL;
+			*prot = (PROT_EXEC | PROT_READ);
+			break;
+		case 1:
+			*vaddr = &_etext;
+			*size = (ptr_t)(*top) - (ptr_t)&_etext;
+			*prot = (PROT_WRITE | PROT_READ);
+			break;
+		default:
+			return -EINVAL;
+	}
 
-	dtb_getMemory((ptr_t **)&m, &n);
-	dtb_getReservedMemory((ptr_t **)&r);
-	dtb_getDTBArea(&pmap_common.dtb, &pmap_common.dtbsz);
+	return EOK;
+}
 
-	hal_spinlockCreate(&pmap_common.lock, "pmap_common.lock");
 
-	/* Calculate physical address space range */
-	pmap_common.minAddr = (u64)-1;
-	pmap_common.maxAddr = 0;
+static int _pmap_addMemEntry(addr_t start, size_t length, int flags)
+{
+	addr_t end;
+	size_t pageCount;
 
-	for (i = 0; i < n; i++) {
-		a = ntoh64(m[i].addr);
-		l = ntoh64(m[i].limit);
+	if (pmap_common.memMap.count >= PMAP_MEM_ENTRIES) {
+		return -ENOMEM;
+	}
 
-		if ((a + l) > pmap_common.maxAddr) {
-			pmap_common.maxAddr = a + l;
+	end = CEIL_PAGE(start + length);
+	pageCount = (end - start) / SIZE_PAGE;
+
+	pmap_common.memMap.entries[pmap_common.memMap.count].start = start;
+	pmap_common.memMap.entries[pmap_common.memMap.count].pageCount = pageCount;
+	pmap_common.memMap.entries[pmap_common.memMap.count].flags = flags;
+
+	pmap_common.memMap.count++;
+	return EOK;
+}
+
+
+static int _pmap_findFreePage(page_t *page)
+{
+	int ret = -ENOMEM;
+
+	while (pmap_common.pageIterator < pmap_common.maxAddr) {
+		ret = pmap_getPage(page, &pmap_common.pageIterator);
+		if (((page->flags & PAGE_FREE) != 0) || (ret != EOK)) {
+			break;
 		}
-		if (a < pmap_common.minAddr) {
-			pmap_common.minAddr = a;
+	}
+
+	return ret;
+}
+
+
+static void *_pmap_halMapInternal(addr_t paddr, void *va, size_t size, int attr, int remoteFlush)
+{
+	void *ret;
+	void **pva;
+	addr_t end;
+	page_t *alloc = NULL;
+	page_t page;
+
+	if ((hal_started() != 0) && (va == NULL)) {
+		return NULL;
+	}
+
+	paddr &= ~(SIZE_PAGE - 1);
+	end = CEIL_PAGE(paddr + size);
+
+	if (va == NULL) {
+		pva = (void **)&pmap_common.vkernelEnd;
+		va = *pva;
+	}
+	else {
+		va = (void *)((ptr_t)va & ~(SIZE_PAGE - 1));
+		pva = &va;
+	}
+
+	ret = va;
+
+	while (paddr < end) {
+		while (_pmap_map(pmap_common.pdir2, paddr, *pva, attr, alloc) < 0) {
+			if (_pmap_findFreePage(&page) < 0) {
+				if (remoteFlush != 0) {
+					hal_cpuRemoteFlushTLB(0, va, (ptr_t)*pva - (ptr_t)va);
+				}
+				return NULL;
+			}
+			if (_pmap_addMemEntry(page.addr, SIZE_PAGE, PAGE_OWNER_KERNEL | PAGE_KERNEL_PTABLE) != EOK) {
+				if (remoteFlush != 0) {
+					hal_cpuRemoteFlushTLB(0, va, (ptr_t)*pva - (ptr_t)va);
+				}
+				return NULL;
+			}
+			alloc = &page;
+		}
+
+		if (remoteFlush == 0) {
+			hal_cpuLocalFlushTLB(0, *pva);
 		}
+		alloc = NULL;
+		*pva += SIZE_PAGE;
+		paddr += SIZE_PAGE;
+	}
+
+	if (remoteFlush != 0) {
+		hal_cpuRemoteFlushTLB(0, va, size);
 	}
 
+	return ret;
+}
+
+
+void *_pmap_halMap(addr_t paddr, void *va, size_t size, int attr)
+{
+	return _pmap_halMapInternal(paddr, va, size, attr, 0);
+}
+
+
+void *pmap_halMap(addr_t paddr, void *va, size_t size, int attr)
+{
+	void *ret;
+	spinlock_ctx_t sc;
+
+	hal_spinlockSet(&pmap_common.lock, &sc);
+	ret = _pmap_halMapInternal(paddr, va, size, attr, 1);
+	hal_spinlockClear(&pmap_common.lock, &sc);
+
+	return ret;
+}
+
+
+/* Function initializes low-level page mapping interface */
+void _pmap_init(pmap_t *pmap, void **vstart, void **vend)
+{
+	void *v;
+
 	/* Initialize kernel page table - remove first 4 MB mapping */
 	pmap->pdir2 = pmap_common.pdir2;
 
@@ -457,8 +632,7 @@ void _pmap_init(pmap_t *pmap, void **vstart, void **vend)
 	pmap->end = (void *)VADDR_MAX;
 
 	/* Initialize kernel heap start address */
-	(*vstart) = (void *)(((ptr_t)&_end + SIZE_PAGE - 1) & ~(SIZE_PAGE - 1));
-	(*vstart) += SIZE_PAGE; /* Reserve space for syspage */
+	(*vstart) = (void *)pmap_common.vkernelEnd;
 
 	(*vend) = (*vstart) + SIZE_PAGE;
 
@@ -466,7 +640,7 @@ void _pmap_init(pmap_t *pmap, void **vstart, void **vend)
 	pmap_common.end = pmap_common.start + SIZE_PAGE;
 
 	/* Create initial heap */
-	_pmap_enter(pmap, pmap_common.start, (*vstart), PGHD_READ | PGHD_WRITE | PGHD_PRESENT, NULL, 0);
+	_pmap_enter(pmap->pdir2, pmap_common.start, (*vstart), PGHD_READ | PGHD_WRITE | PGHD_PRESENT, NULL, 0);
 
 	for (v = *vend; v < (void *)((char *)VADDR_KERNEL + (2 << 20)); v = (char *)v + SIZE_PAGE) {
 		_pmap_remove(pmap, v);
@@ -474,49 +648,66 @@ void _pmap_init(pmap_t *pmap, void **vstart, void **vend)
 
 	/* Map kernel text as RX */
 	for (v = (void *)VADDR_KERNEL; v < (void *)CEIL_PAGE((ptr_t)(&_etext)); v = (char *)v + SIZE_PAGE) {
-		_pmap_enter(pmap, pmap_resolve(pmap, v), v, PGHD_READ | PGHD_EXEC | PGHD_PRESENT, NULL, 0);
+		_pmap_enter(pmap->pdir2, pmap_resolve(pmap, v), v, PGHD_READ | PGHD_EXEC | PGHD_PRESENT, NULL, 0);
 	}
 
 	/* Map everything else as RW */
-	for (v = (void *)CEIL_PAGE((ptr_t)(&_etext)); v < (void *)(CEIL_PAGE((ptr_t)&_end) + SIZE_PAGE); v = (char *)v + SIZE_PAGE) {
-		_pmap_enter(pmap, pmap_resolve(pmap, v), v, PGHD_READ | PGHD_WRITE | PGHD_PRESENT, NULL, 0);
+	for (v = (void *)CEIL_PAGE((ptr_t)(&_etext)); v < (void *)(CEIL_PAGE((ptr_t)&_end)); v = (char *)v + SIZE_PAGE) {
+		_pmap_enter(pmap->pdir2, pmap_resolve(pmap, v), v, PGHD_READ | PGHD_WRITE | PGHD_PRESENT, NULL, 0);
 	}
 
 	pmap->satp = ((pmap_resolve(pmap, (char *)pmap_common.pdir2) >> 12) | SATP_MODE_SV39);
 
-	hal_cpuLocalFlushTLB(NULL, NULL);
+	hal_cpuLocalFlushTLB(0, NULL);
 }
 
 
-int pmap_segment(unsigned int i, void **vaddr, size_t *size, int *prot, void **top)
+void _pmap_halInit(void)
 {
-	switch (i) {
-		case 0:
-			*vaddr = (void *)VADDR_KERNEL;
-			*size = (ptr_t)&_etext - VADDR_KERNEL;
-			*prot = (PROT_EXEC | PROT_READ);
-			break;
-		case 1:
-			*vaddr = &_etext;
-			*size = (ptr_t)(*top) - (ptr_t)&_etext;
-			*prot = (PROT_WRITE | PROT_READ);
-			break;
-		default:
-			return -EINVAL;
+	struct {
+		u8 addr[8];
+		u8 limit[8];
+	} *m;
+	size_t n, i;
+	u64 a, l;
+
+	dtb_getMemory((u8 **)&m, &n);
+	dtb_getDTBArea(&pmap_common.dtb, &pmap_common.dtbsz);
+
+	hal_spinlockCreate(&pmap_common.lock, "pmap_common.lock");
+
+	/* Calculate physical address space range */
+	pmap_common.minAddr = (u64)-1;
+	pmap_common.maxAddr = 0;
+
+	for (i = 0; i < n; i++) {
+		hal_memcpy(&a, &m[i].addr, sizeof(a));
+		hal_memcpy(&l, &m[i].limit, sizeof(l));
+		a = ntoh64(a);
+		l = ntoh64(l);
+
+		if ((a + l) > pmap_common.maxAddr) {
+			pmap_common.maxAddr = a + l;
+		}
+		if (a < pmap_common.minAddr) {
+			pmap_common.minAddr = a;
+		}
 	}
 
-	return EOK;
+	pmap_common.pageIterator = pmap_common.minAddr;
+	pmap_common.memMap.count = 0;
+	pmap_common.vkernelEnd = VADDR_KERNEL + pmap_common.kernelsz;
 }
 
 
-void _pmap_preinit(void)
+void _pmap_preinit(addr_t dtb)
 {
 	unsigned int i;
 
 	/* Get physical kernel address */
 	pmap_common.kernel = (addr_t)&_start;
 	/* Add SIZE_PAGE to kernel size for syspage */
-	pmap_common.kernelsz = (((addr_t)&_end + SIZE_PAGE - 1) & ~(SIZE_PAGE - 1)) - (addr_t)&_start + SIZE_PAGE;
+	pmap_common.kernelsz = CEIL_PAGE(&_end) - (addr_t)&_start + SIZE_PAGE;
 
 	hal_memset(pmap_common.pdir0, 0, SIZE_PAGE);
 	hal_memset(pmap_common.pdir1, 0, SIZE_PAGE);
@@ -530,11 +721,6 @@ void _pmap_preinit(void)
 		pmap_common.pdir0[((VADDR_KERNEL >> 12) % 512) + i] = PTE((addr_t)&_start + i * SIZE_PAGE, 0xcf);
 	}
 
-	/* Map PLIC (MOD) */
-	pmap_common.pdir2[511] = 0xc7;
-
 	/* Map physical memory to reach DTB before vm subsystem initialization (MOD) */
-	pmap_common.pdir2[510] = ((0x80000000 >> 2) | 0xc7);
-
-	return;
+	pmap_common.pdir2[511] = (((dtb >> 30) << 28) | 0xc3);
 }
diff --git a/hal/riscv64/riscv64.h b/hal/riscv64/riscv64.h
index 12557de99..7b43a4088 100644
--- a/hal/riscv64/riscv64.h
+++ b/hal/riscv64/riscv64.h
@@ -98,4 +98,10 @@ static inline void hal_cpuInstrBarrier(void)
 }
 
 
+void hal_cpuDCacheInval(void *va, size_t size);
+
+
+void hal_cpuDCacheFlush(void *va, size_t size);
+
+
 #endif
diff --git a/hal/sparcv8leon3/gaisler/ambapp.h b/hal/sparcv8leon3/gaisler/ambapp.h
index edf93b051..bfffcd656 100644
--- a/hal/sparcv8leon3/gaisler/ambapp.h
+++ b/hal/sparcv8leon3/gaisler/ambapp.h
@@ -17,6 +17,7 @@
 #define _HAL_SPARCV8LEON3_AMBAPP_H_
 
 
+#include "include/gaisler/ambapp.h"
 #include "include/arch/sparcv8leon3/sparcv8leon3.h"
 
 
diff --git a/include/arch/riscv64/riscv64.h b/include/arch/riscv64/riscv64.h
index 2e90d9c5d..8ad62fbdd 100644
--- a/include/arch/riscv64/riscv64.h
+++ b/include/arch/riscv64/riscv64.h
@@ -22,12 +22,24 @@
 
 typedef struct {
 	enum { pctl_set = 0, pctl_get } action;
-	enum { pctl_reboot = 0 } type;
+	enum { pctl_reboot = 0, pctl_iomux, pctl_ambapp } type;
 
 	union {
 		struct {
 			unsigned int magic;
 		} reboot;
+
+		struct {
+			unsigned char pin;
+			unsigned char opt;
+			unsigned char pullup;
+			unsigned char pulldn;
+		} iocfg;
+
+		struct {
+			struct _ambapp_dev_t *dev;
+			unsigned int *instance;
+		} ambapp;
 	} task;
 } __attribute__((packed)) platformctl_t;
 
diff --git a/include/arch/sparcv8leon3/sparcv8leon3.h b/include/arch/sparcv8leon3/sparcv8leon3.h
index b97bf4de4..979272802 100644
--- a/include/arch/sparcv8leon3/sparcv8leon3.h
+++ b/include/arch/sparcv8leon3/sparcv8leon3.h
@@ -26,188 +26,4 @@
 #endif
 
 
-/* GRLIB Cores' IDs - pages 10-18 GRLIB IP CORE Manual
- * https://www.gaisler.com/products/grlib/grip.pdf#page=10
- */
-
-/* Processor license functions */
-#define CORE_ID_LEON3    0x003u
-#define CORE_ID_LEON3FT  0x053u
-#define CORE_ID_DSU3     0x004u
-#define CORE_ID_L3STAT   0x098u
-#define CORE_ID_LEON4    0x048u
-#define CORE_ID_LEON4FT  0x048u
-#define CORE_ID_L4STAT   0x047u
-#define CORE_ID_DSU4     0x049u
-#define CORE_ID_LEON5    0x0bau
-#define CORE_ID_LEON5FT  0x0bau
-#define CORE_ID_DSU5     0x0bbu
-#define CORE_ID_NOEL_V   0x0bdu
-#define CORE_ID_NOEL_VFT 0x0bdu
-#define CORE_ID_RVDM     0x0beu
-#define CORE_ID_L2CACHE  0x04bu
-#define CORE_ID_L2C_LITE 0x0d0u
-#define CORE_ID_GRIOMMU  0x04fu
-#define CORE_ID_GRIOMMU2 0x0d3u
-
-/* Processor support functions */
-#define CORE_ID_GPTIMER    0x011u
-#define CORE_ID_GRCLKGATE  0x02cu
-#define CORE_ID_GRDMAC     0x095u
-#define CORE_ID_GRDMAC2    0x0c0u
-#define CORE_ID_GRTIMER    0x038u
-#define CORE_ID_GRWATCHDOG 0x0c9u
-#define CORE_ID_IRQMP      0x00du
-#define CORE_ID_IRQAMP     0x00du
-
-/* Memory controllers and supporting cores */
-#define CORE_ID_DDRSPA     0x025u
-#define CORE_ID_DDR2SPA    0x02eu
-#define CORE_ID_MCTRL      0x00fu
-#define CORE_ID_SDCTRL     0x009u
-#define CORE_ID_SRCTRL     0x008u
-#define CORE_ID_SSRCTRL    0x00au
-#define CORE_ID_FTADDR     0x0aeu
-#define CORE_ID_FTMCTRL    0x054u
-#define CORE_ID_FTSDCTRL   0x055u
-#define CORE_ID_FTSDCTRL64 0x058u
-#define CORE_ID_FTSRCTRL   0x051u
-#define CORE_ID_FTSRCTRL8  0x056u
-#define CORE_ID_NANDFCTRL  0x059u
-#define CORE_ID_NANDFCTRL2 0x0c5u
-#define CORE_ID_SPIMCTRL   0x045u
-#define CORE_ID_AHBSTAT    0x052u
-#define CORE_ID_MEMSCRUB   0x057u
-
-/* AMBA Bus control */
-#define CORE_ID_AHB2AHB   0x020u
-#define CORE_ID_AHB2AVLA  0x096u
-#define CORE_ID_AHB2AXIB  0x09fu
-#define CORE_ID_AHBBRIDGE 0x020u
-#define CORE_ID_APBCTRL   0x006u
-#define CORE_ID_AHBTRACE  0x017u
-#define CORE_ID_MMA       0x07fu
-
-/* PCI interface */
-#define CORE_ID_GRPCI2       0x07cu
-#define CORE_ID_PCITARGET    0x012u
-#define CORE_ID_PCIMTF_GRPCI 0x014u
-#define CORE_ID_PCITRACE     0x015u
-#define CORE_ID_PCIDMA       0x016u
-#define CORE_ID_PCIARB       0x010u
-
-/* On-chip memory functions */
-#define CORE_ID_AHBRAM   0x00eu
-#define CORE_ID_AHBDPRAM 0x00fu
-#define CORE_ID_AHBROM   0x01bu
-#define CORE_ID_FTAHBRAM 0x050u
-
-/* Serial communication */
-#define CORE_ID_AHBUART     0x007u
-#define CORE_ID_AHBJTAG     0x01cu
-#define CORE_ID_APBPS2      0x060u
-#define CORE_ID_APBUART     0x00cu
-#define CORE_ID_CAN_OC      0x019u
-#define CORE_ID_CANMUX      0x081u
-#define CORE_ID_GRCAN       0x03du
-#define CORE_ID_GRCANFD     0x0b5u
-#define CORE_ID_GRHSSL      0x0c8u
-#define CORE_ID_GRSPFI      0x0bcu
-#define CORE_ID_GRSPW2      0x029u
-#define CORE_ID_GRSPWROUTER 0x03eu
-#define CORE_ID_GRSPWTDP    0x097u
-#define CORE_ID_GRSRIO      0x0a8u
-#define CORE_ID_GRWIZL      0x0c7u
-#define CORE_ID_I2C2AHB     0x00bu
-#define CORE_ID_I2CMST      0x028u
-#define CORE_ID_I2CSLV      0x03eu
-#define CORE_ID_SOCBRIDGE   0x0c4u
-#define CORE_ID_SPI2AHB     0x05cu
-#define CORE_ID_SPICTRL     0x02du
-#define CORE_ID_SPIMASTER   0x0a6u
-#define CORE_ID_SPISLAVE    0x0a7u
-
-/* Ethernet interface */
-#define CORE_ID_GRETH      0x01du
-#define CORE_ID_GRETH_GBIT 0x01du
-#define CORE_ID_RGMII      0x093u
-
-/* USB interface */
-#define CORE_ID_GRUSBHC 0x027u
-#define CORE_ID_GRUSBDC 0x022u
-
-/* MIL-STD-1553B interface */
-#define CORE_ID_GR1553B    0x04du
-#define CORE_ID_GRB1553BRM 0x072u
-
-/* Encryption and compression */
-#define CORE_ID_GRAES     0x073u
-#define CORE_ID_GRAES_DMA 0x07bu
-#define CORE_ID_GRECC     0x074u
-#define CORE_ID_GRSHYLOC  0x0b7u
-
-/* FPGA control and error mitigation */
-#define CORE_ID_GRSCRUB 0x0c1u
-
-/* Graphics functions */
-#define CORE_ID_APBVGA   0x061u
-#define CORE_ID_SVGACTRL 0x063u
-
-/* Auxiliary functions */
-#define CORE_ID_GRACECTRL 0x067u
-#define CORE_ID_GRADCDAC  0x036u
-#define CORE_ID_GRFIFO    0x035u
-#define CORE_ID_GRGPIO    0x01au
-#define CORE_ID_GRGPREG   0x087u
-#define CORE_ID_GRGPRBANK 0x08fu
-#define CORE_ID_GRPULSE   0x037u
-#define CORE_ID_GRPWM     0x04au
-#define CORE_ID_GRSYSMON  0x066u
-#define CORE_ID_GRVERSION 0x03au
-
-/* Spacecraft data handling functions */
-#define CORE_ID_GRTM      0x030u
-#define CORE_ID_GRTM_DESC 0x084u
-#define CORE_ID_GRTM_VC   0x085u
-#define CORE_ID_GRTM_PAHB 0x088u
-#define CORE_ID_GRGEFFE   0x086u
-#define CORE_ID_GRTMRX    0x082u
-#define CORE_ID_GRTC      0x031u
-#define CORE_ID_GRTCTX    0x083u
-#define CORE_ID_GRCTM     0x033u
-#define CORE_ID_SPWCUC    0x089u
-#define CORE_ID_GRPW      0x032u
-#define CORE_ID_GRPWRX    0x08eu
-#define CORE_ID_GRPWTX    0x08du
-#define CORE_ID_APB2PW    0x03bu
-#define CORE_ID_PW2APB    0x03cu
-#define CORE_ID_AHB2PP    0x039u
-#define CORE_ID_GRRM      0x09au
-
-#define AMBA_TYPE_APBIO  0x1u
-#define AMBA_TYPE_AHBMEM 0x2u
-#define AMBA_TYPE_AHBIO  0x3u
-
-#define BUS_AMBA_AHB 0x0u
-#define BUS_AMBA_APB 0x1u
-
-
-typedef struct _ambapp_dev_t {
-	unsigned short devId;
-	unsigned char vendor;
-	unsigned char irqn;
-	unsigned int bus;
-	union {
-		struct {
-			unsigned int *base[4];
-			unsigned int type[4];
-		} ahb;
-		struct {
-			unsigned int *base;
-			unsigned int type;
-		} apb;
-	} info;
-} __attribute__((packed)) ambapp_dev_t;
-
-
 #endif
diff --git a/include/gaisler/ambapp.h b/include/gaisler/ambapp.h
new file mode 100644
index 000000000..9a676635f
--- /dev/null
+++ b/include/gaisler/ambapp.h
@@ -0,0 +1,204 @@
+/*
+ * Phoenix-RTOS
+ *
+ * Operating system kernel
+ *
+ * GRLIB AMBA Plug&Play definitions
+ *
+ * Copyright 2023, 2024 Phoenix Systems
+ * Author: Lukasz Leczkowski
+ *
+ * This file is part of Phoenix-RTOS.
+ *
+ * %LICENSE%
+ */
+
+
+#ifndef _PHOENIX_GAISLER_AMBAPP_H_
+#define _PHOENIX_GAISLER_AMBAPP_H_
+
+/* GRLIB Cores' IDs - pages 10-18 GRLIB IP CORE Manual
+ * https://www.gaisler.com/products/grlib/grip.pdf#page=10
+ */
+
+/* Processor license functions */
+#define CORE_ID_LEON3    0x003u
+#define CORE_ID_LEON3FT  0x053u
+#define CORE_ID_DSU3     0x004u
+#define CORE_ID_L3STAT   0x098u
+#define CORE_ID_LEON4    0x048u
+#define CORE_ID_LEON4FT  0x048u
+#define CORE_ID_L4STAT   0x047u
+#define CORE_ID_DSU4     0x049u
+#define CORE_ID_LEON5    0x0bau
+#define CORE_ID_LEON5FT  0x0bau
+#define CORE_ID_DSU5     0x0bbu
+#define CORE_ID_NOEL_V   0x0bdu
+#define CORE_ID_NOEL_VFT 0x0bdu
+#define CORE_ID_RVDM     0x0beu
+#define CORE_ID_L2CACHE  0x04bu
+#define CORE_ID_L2C_LITE 0x0d0u
+#define CORE_ID_GRIOMMU  0x04fu
+#define CORE_ID_GRIOMMU2 0x0d3u
+
+/* Processor support functions */
+#define CORE_ID_GPTIMER    0x011u
+#define CORE_ID_GRCLKGATE  0x02cu
+#define CORE_ID_GRDMAC     0x095u
+#define CORE_ID_GRDMAC2    0x0c0u
+#define CORE_ID_GRTIMER    0x038u
+#define CORE_ID_GRWATCHDOG 0x0c9u
+#define CORE_ID_IRQMP      0x00du
+#define CORE_ID_IRQAMP     0x00du
+
+/* Memory controllers and supporting cores */
+#define CORE_ID_DDRSPA     0x025u
+#define CORE_ID_DDR2SPA    0x02eu
+#define CORE_ID_MCTRL      0x00fu
+#define CORE_ID_SDCTRL     0x009u
+#define CORE_ID_SRCTRL     0x008u
+#define CORE_ID_SSRCTRL    0x00au
+#define CORE_ID_FTADDR     0x0aeu
+#define CORE_ID_FTMCTRL    0x054u
+#define CORE_ID_FTSDCTRL   0x055u
+#define CORE_ID_FTSDCTRL64 0x058u
+#define CORE_ID_FTSRCTRL   0x051u
+#define CORE_ID_FTSRCTRL8  0x056u
+#define CORE_ID_NANDFCTRL  0x059u
+#define CORE_ID_NANDFCTRL2 0x0c5u
+#define CORE_ID_SPIMCTRL   0x045u
+#define CORE_ID_AHBSTAT    0x052u
+#define CORE_ID_MEMSCRUB   0x057u
+
+/* AMBA Bus control */
+#define CORE_ID_AHB2AHB   0x020u
+#define CORE_ID_AHB2AVLA  0x096u
+#define CORE_ID_AHB2AXIB  0x09fu
+#define CORE_ID_AHBBRIDGE 0x020u
+#define CORE_ID_APBCTRL   0x006u
+#define CORE_ID_AHBTRACE  0x017u
+#define CORE_ID_MMA       0x07fu
+
+/* PCI interface */
+#define CORE_ID_GRPCI2       0x07cu
+#define CORE_ID_PCITARGET    0x012u
+#define CORE_ID_PCIMTF_GRPCI 0x014u
+#define CORE_ID_PCITRACE     0x015u
+#define CORE_ID_PCIDMA       0x016u
+#define CORE_ID_PCIARB       0x010u
+
+/* On-chip memory functions */
+#define CORE_ID_AHBRAM   0x00eu
+#define CORE_ID_AHBDPRAM 0x00fu
+#define CORE_ID_AHBROM   0x01bu
+#define CORE_ID_FTAHBRAM 0x050u
+
+/* Serial communication */
+#define CORE_ID_AHBUART     0x007u
+#define CORE_ID_AHBJTAG     0x01cu
+#define CORE_ID_APBPS2      0x060u
+#define CORE_ID_APBUART     0x00cu
+#define CORE_ID_CAN_OC      0x019u
+#define CORE_ID_CANMUX      0x081u
+#define CORE_ID_GRCAN       0x03du
+#define CORE_ID_GRCANFD     0x0b5u
+#define CORE_ID_GRHSSL      0x0c8u
+#define CORE_ID_GRSPFI      0x0bcu
+#define CORE_ID_GRSPW2      0x029u
+#define CORE_ID_GRSPWROUTER 0x03eu
+#define CORE_ID_GRSPWTDP    0x097u
+#define CORE_ID_GRSRIO      0x0a8u
+#define CORE_ID_GRWIZL      0x0c7u
+#define CORE_ID_I2C2AHB     0x00bu
+#define CORE_ID_I2CMST      0x028u
+#define CORE_ID_I2CSLV      0x03eu
+#define CORE_ID_SOCBRIDGE   0x0c4u
+#define CORE_ID_SPI2AHB     0x05cu
+#define CORE_ID_SPICTRL     0x02du
+#define CORE_ID_SPIMASTER   0x0a6u
+#define CORE_ID_SPISLAVE    0x0a7u
+
+/* Ethernet interface */
+#define CORE_ID_GRETH      0x01du
+#define CORE_ID_GRETH_GBIT 0x01du
+#define CORE_ID_RGMII      0x093u
+
+/* USB interface */
+#define CORE_ID_GRUSBHC 0x027u
+#define CORE_ID_GRUSBDC 0x022u
+
+/* MIL-STD-1553B interface */
+#define CORE_ID_GR1553B    0x04du
+#define CORE_ID_GRB1553BRM 0x072u
+
+/* Encryption and compression */
+#define CORE_ID_GRAES     0x073u
+#define CORE_ID_GRAES_DMA 0x07bu
+#define CORE_ID_GRECC     0x074u
+#define CORE_ID_GRSHYLOC  0x0b7u
+
+/* FPGA control and error mitigation */
+#define CORE_ID_GRSCRUB 0x0c1u
+
+/* Graphics functions */
+#define CORE_ID_APBVGA   0x061u
+#define CORE_ID_SVGACTRL 0x063u
+
+/* Auxiliary functions */
+#define CORE_ID_GRACECTRL 0x067u
+#define CORE_ID_GRADCDAC  0x036u
+#define CORE_ID_GRFIFO    0x035u
+#define CORE_ID_GRGPIO    0x01au
+#define CORE_ID_GRGPREG   0x087u
+#define CORE_ID_GRGPRBANK 0x08fu
+#define CORE_ID_GRPULSE   0x037u
+#define CORE_ID_GRPWM     0x04au
+#define CORE_ID_GRSYSMON  0x066u
+#define CORE_ID_GRVERSION 0x03au
+
+/* Spacecraft data handling functions */
+#define CORE_ID_GRTM      0x030u
+#define CORE_ID_GRTM_DESC 0x084u
+#define CORE_ID_GRTM_VC   0x085u
+#define CORE_ID_GRTM_PAHB 0x088u
+#define CORE_ID_GRGEFFE   0x086u
+#define CORE_ID_GRTMRX    0x082u
+#define CORE_ID_GRTC      0x031u
+#define CORE_ID_GRTCTX    0x083u
+#define CORE_ID_GRCTM     0x033u
+#define CORE_ID_SPWCUC    0x089u
+#define CORE_ID_GRPW      0x032u
+#define CORE_ID_GRPWRX    0x08eu
+#define CORE_ID_GRPWTX    0x08du
+#define CORE_ID_APB2PW    0x03bu
+#define CORE_ID_PW2APB    0x03cu
+#define CORE_ID_AHB2PP    0x039u
+#define CORE_ID_GRRM      0x09au
+
+#define AMBA_TYPE_APBIO  0x1u
+#define AMBA_TYPE_AHBMEM 0x2u
+#define AMBA_TYPE_AHBIO  0x3u
+
+#define BUS_AMBA_AHB 0x0u
+#define BUS_AMBA_APB 0x1u
+
+
+typedef struct _ambapp_dev_t {
+	unsigned short devId;
+	unsigned char vendor;
+	unsigned char irqn;
+	unsigned int bus;
+	union {
+		struct {
+			unsigned int *base[4];
+			unsigned int type[4];
+		} ahb;
+		struct {
+			unsigned int *base;
+			unsigned int type;
+		} apb;
+	} info;
+} __attribute__((packed)) ambapp_dev_t;
+
+
+#endif

From 1d5ceae0d177b0f7105e0cb9d311b75d8d8a23c3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=C5=81ukasz=20Leczkowski?=
 <lukasz.leczkowski@phoenix-rtos.com>
Date: Thu, 13 Jun 2024 11:05:02 +0200
Subject: [PATCH 4/4] riscv64: interrupt handling optimizations

JIRA: RTOS-844
---
 hal/riscv64/_interrupts.S | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/hal/riscv64/_interrupts.S b/hal/riscv64/_interrupts.S
index ece5ff08c..7b3537780 100644
--- a/hal/riscv64/_interrupts.S
+++ b/hal/riscv64/_interrupts.S
@@ -99,8 +99,8 @@
 	sd sp, 232(sp)       /* ksp */
 
 	/* Check FPU status */
-	csrr t0, sstatus
-	srli t0, t0, 13
+	csrr s1, sstatus
+	srli t0, s1, 13
 
 	/* If FPU is clean or dirty, save context */
 	andi t0, t0, 2
@@ -148,15 +148,13 @@
 	csrc sstatus, t0
 	li t0, (2 << 13)
 	csrs sstatus, t0
-3:
 	csrr s1, sstatus
+3:
 	csrr s2, sepc
-	csrr s3, sbadaddr
 	csrr s4, scause
 
 	sd s1, 240(sp)   /* sstatus */
 	sd s2, 248(sp)   /* sepc */
-	sd s3, 256(sp)   /* sbadaddr */
 	sd s4, 264(sp)   /* scause */
 	sd tp, 280(sp)   /* tp */
 .endm
@@ -308,8 +306,7 @@ _interrupts_exception:
 	bnez t2, _interrupts_exceptionNotFpu
 
 	/* Get failing instruction */
-	csrr t0, sepc
-	lw t2, (t0)
+	csrr t2, stval
 	andi t0, t2, 0x7f
 
 	/* Check opcode:
@@ -425,8 +422,10 @@ _interrupts_exceptionFpu:
 	tail _interrupts_returnUnlocked
 
 _interrupts_exceptionNotFpu:
+	csrr s3, sbadaddr
 	/* Save sscratch to be able to get hart ID */
 	csrr s5, sscratch
+	sd s3, 256(sp)   /* sbadaddr */
 	sd s5, 272(sp)
 
 	mv a1, sp