From 03a0f9565d7a58362a8e073221618a849310821c Mon Sep 17 00:00:00 2001
From: lukileczo <lukasz.leczkowski@phoenix-rtos.com>
Date: Fri, 3 Nov 2023 10:50:24 +0100
Subject: [PATCH 1/3] hal: rename cpu_broadcastIPI -> hal_cpuBroadcastIPI

JIRA: RTOS-615
---
 hal/armv7a/cpu.c       | 2 +-
 hal/armv7m/cpu.c       | 2 +-
 hal/armv8m/cpu.c       | 2 +-
 hal/cpu.h              | 2 +-
 hal/ia32/cpu.c         | 2 +-
 hal/ia32/tlb.c         | 4 ++--
 hal/riscv64/cpu.c      | 2 +-
 hal/sparcv8leon3/cpu.c | 2 +-
 8 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/hal/armv7a/cpu.c b/hal/armv7a/cpu.c
index 2905e5971..54192541b 100644
--- a/hal/armv7a/cpu.c
+++ b/hal/armv7a/cpu.c
@@ -240,7 +240,7 @@ void hal_cpuLowPower(time_t us, spinlock_t *spinlock, spinlock_ctx_t *sc)
 }
 
 
-void cpu_broadcastIPI(unsigned int intr)
+void hal_cpuBroadcastIPI(unsigned int intr)
 {
 }
 
diff --git a/hal/armv7m/cpu.c b/hal/armv7m/cpu.c
index 92af87c48..d5d0b794e 100644
--- a/hal/armv7m/cpu.c
+++ b/hal/armv7m/cpu.c
@@ -338,7 +338,7 @@ void _hal_cpuInit(void)
 }
 
 
-void cpu_broadcastIPI(unsigned int intr)
+void hal_cpuBroadcastIPI(unsigned int intr)
 {
 }
 
diff --git a/hal/armv8m/cpu.c b/hal/armv8m/cpu.c
index 53686a815..865986a5e 100644
--- a/hal/armv8m/cpu.c
+++ b/hal/armv8m/cpu.c
@@ -250,7 +250,7 @@ void _hal_cpuInit(void)
 }
 
 
-void cpu_broadcastIPI(unsigned int intr)
+void hal_cpuBroadcastIPI(unsigned int intr)
 {
 }
 
diff --git a/hal/cpu.h b/hal/cpu.h
index a736b863b..7f68d000d 100644
--- a/hal/cpu.h
+++ b/hal/cpu.h
@@ -124,7 +124,7 @@ extern char *hal_cpuInfo(char *info);
 extern char *hal_cpuFeatures(char *features, unsigned int len);
 
 
-extern void cpu_broadcastIPI(unsigned int intr);
+extern void hal_cpuBroadcastIPI(unsigned int intr);
 
 
 extern void hal_cpuReboot(void);
diff --git a/hal/ia32/cpu.c b/hal/ia32/cpu.c
index c770cef69..f30cba392 100644
--- a/hal/ia32/cpu.c
+++ b/hal/ia32/cpu.c
@@ -326,7 +326,7 @@ unsigned int hal_cpuGetID(void)
 }
 
 /* Sends IPI to everyone but self */
-void cpu_broadcastIPI(unsigned int intr)
+void hal_cpuBroadcastIPI(unsigned int intr)
 {
 	if (hal_isLapicPresent() == 1) {
 		_hal_lapicWrite(LAPIC_ICR_REG_0_31, intr | 0xc4000);
diff --git a/hal/ia32/tlb.c b/hal/ia32/tlb.c
index 782db0e4f..cdf869424 100644
--- a/hal/ia32/tlb.c
+++ b/hal/ia32/tlb.c
@@ -21,7 +21,7 @@
 /* Maximum number of TLB operations */
 #define MAX_CPU_TASK_COUNT 2
 
-extern void cpu_broadcastIPI(unsigned int intr);
+extern void hal_cpuBroadcastIPI(unsigned int intr);
 
 struct task_tlb {
 	void (*func)(void *);
@@ -143,7 +143,7 @@ void hal_tlbCommit(spinlock_t *spinlock, spinlock_ctx_t *ctx)
 	spinlock_ctx_t sc;
 	const unsigned int id = hal_cpuGetID();
 	size_t i, confirmations;
-	cpu_broadcastIPI(TLB_IRQ);
+	hal_cpuBroadcastIPI(TLB_IRQ);
 	hal_spinlockSet(&tlb_common.tlbs[id].core_spinlock, &sc);
 	hal_spinlockClear(spinlock, &sc);
 
diff --git a/hal/riscv64/cpu.c b/hal/riscv64/cpu.c
index 14ce27db4..8da2640a8 100644
--- a/hal/riscv64/cpu.c
+++ b/hal/riscv64/cpu.c
@@ -319,7 +319,7 @@ void _hal_cpuInit(void)
 }
 
 
-void cpu_broadcastIPI(unsigned int intr)
+void hal_cpuBroadcastIPI(unsigned int intr)
 {
 }
 
diff --git a/hal/sparcv8leon3/cpu.c b/hal/sparcv8leon3/cpu.c
index 90601e4f1..1585e6fa1 100644
--- a/hal/sparcv8leon3/cpu.c
+++ b/hal/sparcv8leon3/cpu.c
@@ -302,7 +302,7 @@ unsigned int hal_cpuGetFirstBit(unsigned long v)
 }
 
 
-void cpu_broadcastIPI(unsigned int intr)
+void hal_cpuBroadcastIPI(unsigned int intr)
 {
 }
 

From 13372eba830250040d7aaadb4c4a45e0d8f206c7 Mon Sep 17 00:00:00 2001
From: lukileczo <lukasz.leczkowski@phoenix-rtos.com>
Date: Wed, 15 Nov 2023 16:54:03 +0100
Subject: [PATCH 2/3] hal/tlb: extract common interface, adapt ia32

JIRA: RTOS-615
---
 hal/ia32/Makefile         |  4 ++-
 hal/ia32/{ => arch}/tlb.h | 34 ++++++++----------
 hal/ia32/cpu.c            | 12 ++++---
 hal/ia32/init.c           |  5 +--
 hal/ia32/interrupts.c     |  3 +-
 hal/ia32/pmap.c           | 24 +++++++------
 hal/tlb/Makefile          |  7 ++++
 hal/{ia32 => tlb}/tlb.c   | 74 +++++++++++----------------------------
 hal/tlb/tlb.h             | 38 ++++++++++++++++++++
 9 files changed, 109 insertions(+), 92 deletions(-)
 rename hal/ia32/{ => arch}/tlb.h (51%)
 create mode 100644 hal/tlb/Makefile
 rename hal/{ia32 => tlb}/tlb.c (67%)
 create mode 100644 hal/tlb/tlb.h

diff --git a/hal/ia32/Makefile b/hal/ia32/Makefile
index 457b98c23..a9ab82518 100644
--- a/hal/ia32/Makefile
+++ b/hal/ia32/Makefile
@@ -5,7 +5,9 @@
 # Author: Pawel Pisarczyk
 #
 
-OBJS += $(addprefix $(PREFIX_O)hal/ia32/, _init.o _exceptions.o _interrupts.o multiboot.o spinlock.o exceptions.o interrupts.o cpu.o pmap.o timer.o hal.o string.o pci.o tlb.o init.o)
+include hal/tlb/Makefile
+
+OBJS += $(addprefix $(PREFIX_O)hal/ia32/, _init.o _exceptions.o _interrupts.o multiboot.o spinlock.o exceptions.o interrupts.o cpu.o pmap.o timer.o hal.o string.o pci.o init.o)
 CFLAGS += -Ihal/ia32
 
 ifeq ($(CONSOLE), vga)
diff --git a/hal/ia32/tlb.h b/hal/ia32/arch/tlb.h
similarity index 51%
rename from hal/ia32/tlb.h
rename to hal/ia32/arch/tlb.h
index 25a276707..0d24b6f05 100644
--- a/hal/ia32/tlb.h
+++ b/hal/ia32/arch/tlb.h
@@ -6,22 +6,27 @@
  * TLB handling
  *
  * Copyright 2023 Phoenix Systems
- * Author; Andrzej Stalke
+ * Author: Andrzej Stalke, Lukasz Leczkowski
  *
  * This file is part of Phoenix-RTOS.
  *
  * %LICENSE%
  */
 
-#ifndef _HAL_TLB_H_
-#define _HAL_TLB_H_
 
-#include <arch/types.h>
-#include "hal/spinlock.h"
+#ifndef _HAL_IA32_TLB_H_
+#define _HAL_IA32_TLB_H_
 
-static inline void hal_tlbFlushLocal(void)
+
+#include "pmap.h"
+#include "hal/tlb/tlb.h"
+
+
+static inline void hal_tlbFlushLocal(const pmap_t *pmap)
 {
 	u32 tmpreg;
+	(void)pmap;
+
 	/* clang-format off */
 	__asm__ volatile (
 		"movl %%cr3, %0\n\t"
@@ -35,8 +40,10 @@ static inline void hal_tlbFlushLocal(void)
 }
 
 
-static inline void hal_tlbInvalidateLocalEntry(void const *vaddr)
+static inline void hal_tlbInvalidateLocalEntry(const pmap_t *pmap, const void *vaddr)
 {
+	(void)pmap;
+
 	/* clang-format off */
 	__asm__ volatile (
 		"invlpg (%0)"
@@ -48,18 +55,5 @@ static inline void hal_tlbInvalidateLocalEntry(void const *vaddr)
 	return;
 }
 
-void hal_tlbFlush(void);
-
-
-void hal_tlbInvalidateEntry(const void *vaddr, size_t count);
-
-
-void hal_tlbCommit(spinlock_t *spinlock, spinlock_ctx_t *ctx);
-
-
-void hal_tlbShootdown(void);
-
-
-void hal_tlbInitCore(const unsigned int id);
 
 #endif
diff --git a/hal/ia32/cpu.c b/hal/ia32/cpu.c
index f30cba392..5c04166fe 100644
--- a/hal/ia32/cpu.c
+++ b/hal/ia32/cpu.c
@@ -21,14 +21,18 @@
 #include "hal/string.h"
 #include "hal/pmap.h"
 #include "hal/hal.h"
+#include "hal/tlb/tlb.h"
 #include "pci.h"
 #include "ia32.h"
 #include "halsyspage.h"
-#include "tlb.h"
 #include "init.h"
 
+#include <arch/tlb.h>
+
+
 extern void hal_timerInitCore(const unsigned int id);
 
+
 struct cpu_feature_t {
 	const char *name;
 	s32 eax;
@@ -203,7 +207,7 @@ void hal_cpuSigreturn(void *kstack, void *ustack, cpu_context_t **ctx)
 
 void hal_longjmp(cpu_context_t *ctx)
 {
-	hal_tlbFlushLocal();
+	hal_tlbFlushLocal(NULL);
 	/* clang-format off */
 	__asm__ volatile (
 		"cli\n\t"
@@ -253,7 +257,7 @@ void hal_longjmp(cpu_context_t *ctx)
 
 void hal_jmp(void *f, void *kstack, void *stack, int argc)
 {
-	hal_tlbFlushLocal();
+	hal_tlbFlushLocal(NULL);
 	if (stack == NULL) {
 		/* clang-format off */
 		__asm__ volatile (
@@ -625,7 +629,7 @@ void _hal_cpuInit(void)
 void hal_cpuTlsSet(hal_tls_t *tls, cpu_context_t *ctx)
 {
 	(void)ctx;
-	hal_tlbFlushLocal();
+	hal_tlbFlushLocal(NULL);
 	_cpu_gdtInsert(hal_cpuGetTlsIndex(), tls->tls_base + tls->tbss_sz + tls->tdata_sz, VADDR_KERNEL - tls->tls_base + tls->tbss_sz + tls->tdata_sz, DESCR_TLS);
 	/* Reload the hidden gs register*/
 	hal_cpuReloadTlsSegment();
diff --git a/hal/ia32/init.c b/hal/ia32/init.c
index b1be13752..0584db868 100644
--- a/hal/ia32/init.c
+++ b/hal/ia32/init.c
@@ -18,7 +18,8 @@
 #include "init.h"
 #include "ia32.h"
 #include "include/errno.h"
-#include "tlb.h"
+
+#include <arch/tlb.h>
 
 extern unsigned int _end;
 extern syspage_t *syspage;
@@ -79,7 +80,7 @@ static inline int _hal_configMapPage(u32 *pdir, addr_t pa, void *va, int attr)
 			if (ret == 0) {
 				ptable = (addr_t *)(syspage->hs.ptable + VADDR_KERNEL);
 				ptable[((u32)hal_config.ptable >> 12) & 0x000003ffu] = (page.addr & ~(SIZE_PAGE - 1)) | (PGHD_WRITE | PGHD_PRESENT);
-				hal_tlbInvalidateLocalEntry(hal_config.ptable);
+				hal_tlbInvalidateLocalEntry(NULL, hal_config.ptable);
 				hal_memset(hal_config.ptable, 0, SIZE_PAGE);
 				ret = _pmap_enter(pdir, hal_config.ptable, pa, va, attr, &page, 0);
 			}
diff --git a/hal/ia32/interrupts.c b/hal/ia32/interrupts.c
index e53c848ab..175c60f1e 100644
--- a/hal/ia32/interrupts.c
+++ b/hal/ia32/interrupts.c
@@ -23,9 +23,10 @@
 
 #include "proc/userintr.h"
 #include "include/errno.h"
-#include "tlb.h"
 #include "init.h"
 
+#include <arch/tlb.h>
+
 
 /* Hardware interrupt stubs */
 extern void _interrupts_irq0(void);
diff --git a/hal/ia32/pmap.c b/hal/ia32/pmap.c
index 3d76fa7ae..74cd4e91e 100644
--- a/hal/ia32/pmap.c
+++ b/hal/ia32/pmap.c
@@ -14,13 +14,15 @@
  * %LICENSE%
  */
 
+#include <arch/tlb.h>
+
 #include "halsyspage.h"
 #include "ia32.h"
 #include "hal/pmap.h"
 #include "hal/spinlock.h"
 #include "hal/string.h"
 #include "hal/console.h"
-#include "tlb.h"
+#include "hal/tlb/tlb.h"
 #include "init.h"
 
 #include "include/errno.h"
@@ -103,16 +105,16 @@ int _pmap_enter(u32 *pdir, addr_t *pt, addr_t pa, void *va, int attr, page_t *al
 	ptable = (addr_t *)(syspage->hs.ptable + VADDR_KERNEL);
 	ptable[((u32)pt >> 12) & 0x000003ffu] = (addr & ~(SIZE_PAGE - 1)) | (PGHD_WRITE | PGHD_PRESENT | PGHD_USER);
 
-	hal_tlbInvalidateLocalEntry(pt);
+	hal_tlbInvalidateLocalEntry(NULL, pt);
 
 	/* And at last map page or only changle attributes of map entry */
 	pt[pti] = ((pa & ~(SIZE_PAGE - 1)) | (attr & 0xfffu) | PGHD_PRESENT);
 
 	if (tlbInval != 0) {
-		hal_tlbInvalidateEntry(va, 1);
+		hal_tlbInvalidateEntry(NULL, va, 1);
 	}
 	else {
-		hal_tlbInvalidateLocalEntry(va);
+		hal_tlbInvalidateLocalEntry(NULL, va);
 	}
 
 	return EOK;
@@ -165,18 +167,18 @@ int _pmap_removeMany(u32 *pdir, addr_t *pt, void *vaddr, size_t count, int tlbIn
 		/* Map selected page table */
 		ptable[((u32)pt >> 12) & 0x000003ffu] = (addr & ~(SIZE_PAGE - 1)) | (PGHD_WRITE | PGHD_PRESENT);
 
-		hal_tlbInvalidateLocalEntry(pt);
+		hal_tlbInvalidateLocalEntry(NULL, pt);
 
 		/* Unmap page */
 		pt[pti] = 0;
 	}
 
 	if (tlbInval != 0) {
-		hal_tlbInvalidateEntry(vaddr, count);
+		hal_tlbInvalidateEntry(NULL, vaddr, count);
 	}
 	else {
 		for (i = 0; i < count; ++i, vaddr += SIZE_PAGE) {
-			hal_tlbInvalidateLocalEntry(vaddr);
+			hal_tlbInvalidateLocalEntry(NULL, vaddr);
 		}
 	}
 	return EOK;
@@ -226,7 +228,7 @@ addr_t pmap_resolve(pmap_t *pmap, void *vaddr)
 
 	ptable = (addr_t *)(syspage->hs.ptable + VADDR_KERNEL);
 	ptable[((u32)hal_config.ptable >> 12) & 0x000003ffu] = (addr & ~(SIZE_PAGE - 1)) | (PGHD_WRITE | PGHD_PRESENT);
-	hal_tlbInvalidateLocalEntry(hal_config.ptable);
+	hal_tlbInvalidateLocalEntry(NULL, hal_config.ptable);
 
 	addr = (addr_t)hal_config.ptable[pti];
 	hal_tlbCommit(&pmap_common.lock, &sc);
@@ -350,7 +352,7 @@ int _pmap_kernelSpaceExpand(pmap_t *pmap, void **start, void *end, page_t *dp)
 		}
 		*start = vaddr;
 	}
-	hal_tlbFlushLocal();
+	hal_tlbFlushLocal(NULL);
 
 	pmap->start = (void *)VADDR_KERNEL;
 	pmap->end = end;
@@ -411,7 +413,7 @@ void _pmap_init(pmap_t *pmap, void **vstart, void **vend)
 	pmap->start = (void *)VADDR_KERNEL;
 	pmap->end = (void *)VADDR_MAX;
 
-	hal_tlbFlushLocal();
+	hal_tlbFlushLocal(NULL);
 
 	/* Initialize kernel heap start address */
 	(*vstart) = hal_config.heapStart;
@@ -424,7 +426,7 @@ void _pmap_init(pmap_t *pmap, void **vstart, void **vend)
 	(*vstart) += 0x500;
 
 	pmap_removeMany(pmap, *vend, ((void *)VADDR_KERNEL + (4 << 20) - *vend) / SIZE_PAGE);
-	hal_tlbFlushLocal();
+	hal_tlbFlushLocal(NULL);
 
 	return;
 }
diff --git a/hal/tlb/Makefile b/hal/tlb/Makefile
new file mode 100644
index 000000000..ff5c884c1
--- /dev/null
+++ b/hal/tlb/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for HAL TLB functions
+#
+# Copyright 2023 Phoenix Systems
+#
+
+OBJS += $(addprefix $(PREFIX_O)hal/tlb/, tlb.o)
diff --git a/hal/ia32/tlb.c b/hal/tlb/tlb.c
similarity index 67%
rename from hal/ia32/tlb.c
rename to hal/tlb/tlb.c
index cdf869424..92c1c9172 100644
--- a/hal/ia32/tlb.c
+++ b/hal/tlb/tlb.c
@@ -6,31 +6,37 @@
  * TLB handling
  *
  * Copyright 2023 Phoenix Systems
- * Author; Andrzej Stalke
+ * Author: Andrzej Stalke, Lukasz Leczkowski
  *
  * This file is part of Phoenix-RTOS.
  *
  * %LICENSE%
  */
 
-#include "tlb.h"
+#include <arch/pmap.h>
+#include <arch/tlb.h>
+
 #include "hal/string.h"
 #include "hal/cpu.h"
 #include "hal/interrupts.h"
 
+#include "tlb.h"
+
+
 /* Maximum number of TLB operations */
 #define MAX_CPU_TASK_COUNT 2
 
-extern void hal_cpuBroadcastIPI(unsigned int intr);
 
 struct task_tlb {
 	void (*func)(void *);
 	const void *entry;
+	const pmap_t *pmap;
 	size_t count;
 	volatile size_t confirmations;
 	spinlock_t *spinlock;
 };
 
+
 struct cpu_tlb {
 	struct task_tlb *todo[MAX_CPU_TASK_COUNT * MAX_CPU_COUNT];
 	volatile size_t todo_size;
@@ -47,25 +53,20 @@ static struct {
 } tlb_common;
 
 
-static void tlb_flush(void *arg)
-{
-	struct task_tlb *task = arg;
-	spinlock_ctx_t sc;
-	hal_tlbFlushLocal();
-	hal_spinlockSet(task->spinlock, &sc);
-	--task->confirmations;
-	hal_spinlockClear(task->spinlock, &sc);
-}
-
-
 static void tlb_invalidate(void *arg)
 {
 	struct task_tlb *task = arg;
 	spinlock_ctx_t sc;
 	size_t i;
 	const void *entry;
-	for (i = 0, entry = task->entry; i < task->count; ++i, entry += SIZE_PAGE) {
-		hal_tlbInvalidateLocalEntry(entry);
+
+	if ((task->entry == NULL) && (task->count == 0)) {
+		hal_tlbFlushLocal(task->pmap);
+	}
+	else {
+		for (i = 0, entry = task->entry; i < task->count; ++i, entry += SIZE_PAGE) {
+			hal_tlbInvalidateLocalEntry(task->pmap, entry);
+		}
 	}
 	hal_spinlockSet(task->spinlock, &sc);
 	--task->confirmations;
@@ -73,40 +74,7 @@ static void tlb_invalidate(void *arg)
 }
 
 
-/* Must be protected by pmap_common.lock */
-void hal_tlbFlush(void)
-{
-	/* TODO: Make sure that we aren't pushing into full queue */
-	unsigned int i;
-	const unsigned int n = hal_cpuGetCount(), id = hal_cpuGetID();
-	size_t tasks_size;
-	spinlock_ctx_t sc;
-
-	hal_spinlockSet(&tlb_common.tlbs[id].task_spinlock, &sc);
-	tasks_size = tlb_common.tlbs[id].tasks_size;
-
-	tlb_common.tlbs[id].tasks[tasks_size].func = tlb_flush;
-	tlb_common.tlbs[id].tasks[tasks_size].entry = NULL;
-	tlb_common.tlbs[id].tasks[tasks_size].count = 0;
-	tlb_common.tlbs[id].tasks[tasks_size].confirmations = n - 1;
-	tlb_common.tlbs[id].tasks[tasks_size].spinlock = &tlb_common.tlbs[id].task_spinlock;
-
-	++tlb_common.tlbs[id].tasks_size;
-	hal_spinlockClear(&tlb_common.tlbs[id].task_spinlock, &sc);
-
-	for (i = 0; i < n; ++i) {
-		if (i != id) {
-			hal_spinlockSet(&tlb_common.tlbs[i].todo_spinlock, &sc);
-			tlb_common.tlbs[i].todo[tlb_common.tlbs[i].todo_size++] = &tlb_common.tlbs[id].tasks[tasks_size];
-			hal_spinlockClear(&tlb_common.tlbs[i].todo_spinlock, &sc);
-		}
-	}
-	hal_tlbFlushLocal();
-}
-
-
-/* Must be protected by pmap_common.lock */
-void hal_tlbInvalidateEntry(const void *vaddr, size_t count)
+void hal_tlbInvalidateEntry(const pmap_t *pmap, const void *vaddr, size_t count)
 {
 	/* TODO: Make sure that we aren't pushing into full queue */
 	unsigned int i;
@@ -118,6 +86,7 @@ void hal_tlbInvalidateEntry(const void *vaddr, size_t count)
 	tasks_size = tlb_common.tlbs[id].tasks_size;
 
 	tlb_common.tlbs[id].tasks[tasks_size].func = tlb_invalidate;
+	tlb_common.tlbs[id].tasks[tasks_size].pmap = pmap;
 	tlb_common.tlbs[id].tasks[tasks_size].entry = vaddr;
 	tlb_common.tlbs[id].tasks[tasks_size].count = count;
 	tlb_common.tlbs[id].tasks[tasks_size].confirmations = n - 1;
@@ -133,11 +102,10 @@ void hal_tlbInvalidateEntry(const void *vaddr, size_t count)
 			hal_spinlockClear(&tlb_common.tlbs[i].todo_spinlock, &sc);
 		}
 	}
-	hal_tlbInvalidateLocalEntry(vaddr);
+	hal_tlbInvalidateLocalEntry(pmap, vaddr);
 }
 
 
-/* Must be protected by pmap_common.lock */
 void hal_tlbCommit(spinlock_t *spinlock, spinlock_ctx_t *ctx)
 {
 	spinlock_ctx_t sc;
@@ -185,5 +153,5 @@ void hal_tlbInitCore(const unsigned int id)
 	hal_spinlockCreate(&tlb_common.tlbs[id].core_spinlock, "tlb_common.tlbs.core_spinlock");
 	tlb_common.tlbs[id].tasks_size = 0;
 	tlb_common.tlbs[id].todo_size = 0;
-	hal_tlbFlushLocal();
+	hal_tlbFlushLocal(NULL);
 }
diff --git a/hal/tlb/tlb.h b/hal/tlb/tlb.h
new file mode 100644
index 000000000..7031cc6f1
--- /dev/null
+++ b/hal/tlb/tlb.h
@@ -0,0 +1,38 @@
+/*
+ * Phoenix-RTOS
+ *
+ * Operating system kernel
+ *
+ * TLB handling
+ *
+ * Copyright 2023 Phoenix Systems
+ * Author: Andrzej Stalke, Lukasz Leczkowski
+ *
+ * This file is part of Phoenix-RTOS.
+ *
+ * %LICENSE%
+ */
+
+#ifndef _HAL_TLB_H_
+#define _HAL_TLB_H_
+
+
+#include <arch/spinlock.h>
+
+
+/* To invalidate entire TLB set vaddr = NULL & count = 0.
+ * Must be protected by pmap_common.lock */
+void hal_tlbInvalidateEntry(const pmap_t *pmap, const void *vaddr, size_t count);
+
+
+/* Must be protected by pmap_common.lock */
+void hal_tlbCommit(spinlock_t *spinlock, spinlock_ctx_t *ctx);
+
+
+void hal_tlbShootdown(void);
+
+
+void hal_tlbInitCore(const unsigned int id);
+
+
+#endif

From 2ad291b20a0b09ce7ab736e0e77ef6d8fdf549c5 Mon Sep 17 00:00:00 2001
From: lukileczo <lukasz.leczkowski@phoenix-rtos.com>
Date: Wed, 15 Nov 2023 17:00:13 +0100
Subject: [PATCH 3/3] hal/sparcv8leon3:  enable SMP

JIRA: RTOS-615
---
 hal/sparcv8leon3/Makefile                  |  2 +-
 hal/sparcv8leon3/_interrupts-nommu.S       |  2 +
 hal/sparcv8leon3/_interrupts.S             |  2 +
 hal/sparcv8leon3/_traps.S                  |  8 ++-
 hal/sparcv8leon3/arch/cpu.h                | 40 ++++++++++++
 hal/sparcv8leon3/arch/tlb.h                | 34 ++++++++++
 hal/sparcv8leon3/cpu.c                     | 11 ----
 hal/sparcv8leon3/gaisler/gaisler.h         |  3 +
 hal/sparcv8leon3/gaisler/gr712rc/Makefile  |  2 +
 hal/sparcv8leon3/gaisler/gr712rc/_init.S   | 75 ++++++++++++++++++++++
 hal/sparcv8leon3/gaisler/gr712rc/gr712rc.c | 32 +++++++++
 hal/sparcv8leon3/gaisler/gr716/gr716.c     |  5 ++
 hal/sparcv8leon3/gaisler/irqamp.c          | 10 +++
 hal/sparcv8leon3/gaisler/irqmp.c           | 46 ++++++++++++-
 hal/sparcv8leon3/hal.c                     | 33 +++++++++-
 hal/sparcv8leon3/pmap.c                    | 36 ++++-------
 hal/sparcv8leon3/srmmu.c                   |  2 +-
 hal/sparcv8leon3/srmmu.h                   |  2 +-
 hal/sparcv8leon3/tlb.c                     | 58 +++++++++++++++++
 19 files changed, 359 insertions(+), 44 deletions(-)
 create mode 100644 hal/sparcv8leon3/arch/tlb.h
 create mode 100644 hal/sparcv8leon3/tlb.c

diff --git a/hal/sparcv8leon3/Makefile b/hal/sparcv8leon3/Makefile
index 9ad707cdd..52740441f 100644
--- a/hal/sparcv8leon3/Makefile
+++ b/hal/sparcv8leon3/Makefile
@@ -13,5 +13,5 @@ OBJS += $(addprefix $(PREFIX_O)hal/$(TARGET_SUFF)/, cpu.o hal.o spinlock.o strin
 ifeq ($(findstring -DNOMMU,$(CPPFLAGS)),-DNOMMU)
   OBJS += $(addprefix $(PREFIX_O)hal/$(TARGET_SUFF)/, exceptions-nommu.o pmap-nommu.o _interrupts-nommu.o)
 else
-  OBJS += $(addprefix $(PREFIX_O)hal/$(TARGET_SUFF)/, exceptions.o pmap.o srmmu.o _interrupts.o)
+  OBJS += $(addprefix $(PREFIX_O)hal/$(TARGET_SUFF)/, exceptions.o pmap.o srmmu.o tlb.o _interrupts.o)
 endif
diff --git a/hal/sparcv8leon3/_interrupts-nommu.S b/hal/sparcv8leon3/_interrupts-nommu.S
index b1712592d..8a91da301 100644
--- a/hal/sparcv8leon3/_interrupts-nommu.S
+++ b/hal/sparcv8leon3/_interrupts-nommu.S
@@ -136,6 +136,8 @@ irq_kstack_set:
 	nop
 	nop
 
+	MULTILOCK_CLEAR
+
 	/* l7 still points to bottom of context */
 	mov %l7, %sp
 
diff --git a/hal/sparcv8leon3/_interrupts.S b/hal/sparcv8leon3/_interrupts.S
index a86a2c974..27bc4bcd3 100644
--- a/hal/sparcv8leon3/_interrupts.S
+++ b/hal/sparcv8leon3/_interrupts.S
@@ -160,6 +160,8 @@ irq_cwp_done0:
 	ta 0xb
 #endif
 
+	MULTILOCK_CLEAR
+
 	/* l7 still points to bottom of context */
 	mov %l7, %sp
 
diff --git a/hal/sparcv8leon3/_traps.S b/hal/sparcv8leon3/_traps.S
index aec69e8a0..de5cdc553 100644
--- a/hal/sparcv8leon3/_traps.S
+++ b/hal/sparcv8leon3/_traps.S
@@ -398,11 +398,11 @@ exc_wovfl_done:
 	ld [%l6 + %l7], %l7
 	ba exc_kstack_set
 	sub %l7, 0x60, %sp
- 
+
  exc_no_kstack_switch:
 	/* we came from kernel, make space for context */
 	sub %fp, 0x60, %sp
- 
+
  exc_kstack_set:
 	/* Save context on kernel stack */
 
@@ -450,7 +450,7 @@ exc_wovfl_done:
 	/* trap type */
 	set TBR_TT_MSK, %g2
 	and %g1, %g2, %o0
-	
+
 	/* void exceptions_dispatch(unsigned int n, exc_context_t *ctx) */
 	call exceptions_dispatch
 	srl %o0, TBR_TT_SHIFT, %o0
@@ -954,6 +954,8 @@ r_return:
 	ta 0xb
 #endif
 
+	MULTILOCK_CLEAR
+
 	ld [%l1], %g2 /* ctx pointer */
 
 	/* Set %psr of the new task.
diff --git a/hal/sparcv8leon3/arch/cpu.h b/hal/sparcv8leon3/arch/cpu.h
index 36314a7bb..70801fcab 100644
--- a/hal/sparcv8leon3/arch/cpu.h
+++ b/hal/sparcv8leon3/arch/cpu.h
@@ -69,6 +69,21 @@
 #define CCR_FD  (1 << 22) /* Flush DCache */
 #define CCR_DS  (1 << 23) /* DCache snooping */
 
+/* Basic address space identifiers */
+#define ASI_USER_INSTR  0x08
+#define ASI_SUPER_INSTR 0x09
+#define ASI_USER_DATA   0x0a
+#define ASI_SUPER_DATA  0x0b
+
+/* Interrupts multilock */
+
+/* clang-format off */
+#define MULTILOCK_CLEAR \
+	stbar; \
+	set hal_multilock, %g1; \
+	stub %g0, [%g1]
+/* clang-format on */
+
 
 #ifndef __ASSEMBLY__
 
@@ -77,6 +92,8 @@
 #include "gaisler/gaisler.h"
 
 
+#define MAX_CPU_COUNT NUM_CPUS
+
 #define SYSTICK_INTERVAL 1000
 
 
@@ -286,6 +303,29 @@ static inline void hal_cpuEnableInterrupts(void)
 }
 
 
+static inline void hal_cpuAtomicInc(volatile u32 *dst)
+{
+	/* clang-format off */
+
+	__asm__ volatile (
+		"ld [%0], %%g1\n\t"
+	"1: \n\t"
+		"mov %%g1, %%g2\n\t"
+		"inc %%g1\n\t"
+	".align 16\n\t" /* GRLIB TN-0011 errata */
+		"casa [%0] %c1, %%g2, %%g1\n\t"
+		"cmp %%g1, %%g2\n\t"
+		"bne 1b\n\t"
+		"nop\n\t"
+		:
+		: "r"(dst), "i"(ASI_SUPER_DATA)
+		: "g1", "g2", "memory"
+	);
+
+	/* clang-format on */
+}
+
+
 #endif /* __ASSEMBLY__ */
 
 
diff --git a/hal/sparcv8leon3/arch/tlb.h b/hal/sparcv8leon3/arch/tlb.h
new file mode 100644
index 000000000..1d30a51f0
--- /dev/null
+++ b/hal/sparcv8leon3/arch/tlb.h
@@ -0,0 +1,34 @@
+/*
+ * Phoenix-RTOS
+ *
+ * Operating system kernel
+ *
+ * TLB handling
+ *
+ * Copyright 2023 Phoenix Systems
+ * Author: Andrzej Stalke, Lukasz Leczkowski
+ *
+ * This file is part of Phoenix-RTOS.
+ *
+ * %LICENSE%
+ */
+
+
+#ifndef _HAL_LEON3_TLB_H_
+#define _HAL_LEON3_TLB_H_
+
+#include "cpu.h"
+#include "pmap.h"
+#include "hal/tlb/tlb.h"
+
+
+void hal_tlbFlushLocal(const pmap_t *pmap);
+
+
+void hal_tlbInvalidateLocalEntry(const pmap_t *pmap, const void *vaddr);
+
+
+int hal_tlbIrqHandler(unsigned int n, cpu_context_t *ctx, void *arg);
+
+
+#endif
diff --git a/hal/sparcv8leon3/cpu.c b/hal/sparcv8leon3/cpu.c
index 1585e6fa1..8ec7bfb0b 100644
--- a/hal/sparcv8leon3/cpu.c
+++ b/hal/sparcv8leon3/cpu.c
@@ -234,12 +234,6 @@ void hal_cpuLowPower(time_t us, spinlock_t *spinlock, spinlock_ctx_t *sc)
 }
 
 
-void _hal_cpuInit(void)
-{
-	return;
-}
-
-
 unsigned int hal_cpuGetLastBit(unsigned long v)
 {
 	int lb = 31;
@@ -302,11 +296,6 @@ unsigned int hal_cpuGetFirstBit(unsigned long v)
 }
 
 
-void hal_cpuBroadcastIPI(unsigned int intr)
-{
-}
-
-
 void hal_cpuTlsSet(hal_tls_t *tls, cpu_context_t *ctx)
 {
 	__asm__ volatile("mov %0, %%g7" ::"r"(tls->tls_base + tls->tbss_sz + tls->tdata_sz));
diff --git a/hal/sparcv8leon3/gaisler/gaisler.h b/hal/sparcv8leon3/gaisler/gaisler.h
index 7b0bbae32..b1c379f3c 100644
--- a/hal/sparcv8leon3/gaisler/gaisler.h
+++ b/hal/sparcv8leon3/gaisler/gaisler.h
@@ -26,4 +26,7 @@ int gaisler_setIomuxCfg(u8 pin, u8 opt, u8 pullup, u8 pulldn);
 void gaisler_cpuHalt(void);
 
 
+void hal_cpuStartCores(void);
+
+
 #endif
diff --git a/hal/sparcv8leon3/gaisler/gr712rc/Makefile b/hal/sparcv8leon3/gaisler/gr712rc/Makefile
index 7b91d340d..51aebdac0 100644
--- a/hal/sparcv8leon3/gaisler/gr712rc/Makefile
+++ b/hal/sparcv8leon3/gaisler/gr712rc/Makefile
@@ -4,4 +4,6 @@
 # Copyright 2023 Phoenix Systems
 #
 
+include hal/tlb/Makefile
+
 OBJS += $(addprefix $(PREFIX_O)hal/$(TARGET_SUFF)/gaisler/$(TARGET_SUBFAMILY)/, gr712rc.o _init.o)
diff --git a/hal/sparcv8leon3/gaisler/gr712rc/_init.S b/hal/sparcv8leon3/gaisler/gr712rc/_init.S
index d963a2c70..7974fae0c 100644
--- a/hal/sparcv8leon3/gaisler/gr712rc/_init.S
+++ b/hal/sparcv8leon3/gaisler/gr712rc/_init.S
@@ -67,6 +67,13 @@ _init:
 
 	wr %g0, PSR_S, %psr
 
+	/* Get CPU ID */
+	rd %asr17, %g1
+	srl %g1, 28, %g1
+	cmp %g1, %g0
+	bnz _init_core
+	nop
+
 	/* %g2 = syspage pa (from plo) */
 	set PHY_ADDR(syspage), %g3
 	set_page_align VADDR_SYSPAGE, %g1
@@ -220,3 +227,71 @@ set_pdir3:
 	call %g1
 	mov %g0, %g1
 .size _init, . - _init
+
+
+.global _init_core
+.type _init_core, #function
+_init_core:
+	/* Flush and enable cache */
+	flush
+	set 0x81000f, %g1
+	sta %g1, [%g0] ASI_CACHE_CTRL
+
+	/* Set context table pointer */
+	set ADDR_CTXTAB, %g3
+	set ADDR_PDIR1, %g4
+	calc_ptd %g4, %g4
+	st %g4, [%g3]
+	srl %g3, 6, %g3
+	sll %g3, 2, %g3
+	set MMU_CTX_PTR, %g4
+	sta %g3, [%g4] ASI_MMU_REGS
+
+	clr %fp
+	set VADDR_STACK, %sp
+
+	/* Get CPU ID */
+	rd %asr17, %g1
+	srl %g1, 28, %g1
+	sll %g1, 12, %g1
+	add %sp, %g1, %sp
+	sub %sp, 0x60, %sp
+
+	/* Enable MMU */
+	mov 0x1, %g1
+	sta %g1, [%g0] ASI_MMU_REGS
+
+	/* Set up trap table */
+	sethi %hi(_trap_table), %g1
+	wr %g1, %tbr
+
+	mov %psr, %g1
+	or %g1, (PSR_ET | PSR_S | PSR_PIL), %g1
+	andn %g1, (PSR_CWP), %g1
+	wr %g1, %psr
+
+	/* Switch to virtual memory */
+	sethi %hi(_vmem_switch), %g1
+	jmp %g1 + %lo(_vmem_switch)
+	wr %g0, 0x2, %wim
+
+_vmem_switch:
+	call hal_cpuInitCore
+	nop
+
+	/* Enable interrupts and wait to be scheduled */
+	mov %psr, %g1
+	andn %g1, (PSR_PIL), %g1
+	wr %g1, %psr
+	nop
+	nop
+	nop
+
+	/* GR712RC errata 1.7.8 */
+	set 0xfffffff0, %g1
+_init_core_loop:
+	wr %g0, %asr19
+	lda [%g1] ASI_MMU_BYPASS, %g0
+	ba _init_core_loop
+	nop
+.size _init_core, . - _init_core
diff --git a/hal/sparcv8leon3/gaisler/gr712rc/gr712rc.c b/hal/sparcv8leon3/gaisler/gr712rc/gr712rc.c
index c12638ed2..c1bcbaf8c 100644
--- a/hal/sparcv8leon3/gaisler/gr712rc/gr712rc.c
+++ b/hal/sparcv8leon3/gaisler/gr712rc/gr712rc.c
@@ -13,7 +13,11 @@
  * %LICENSE%
  */
 
+#include <arch/tlb.h>
+
 #include "hal/cpu.h"
+#include "hal/interrupts.h"
+#include "hal/tlb/tlb.h"
 #include "hal/sparcv8leon3/sparcv8leon3.h"
 
 #include "include/arch/gr712rc.h"
@@ -32,9 +36,13 @@
 static struct {
 	spinlock_t pltctlSp;
 	volatile u32 *cguBase;
+	intr_handler_t tlbIrqHandler;
 } gr712rc_common;
 
 
+volatile u32 hal_cpusStarted;
+
+
 void hal_cpuHalt(void)
 {
 	/* GR712RC errata 1.7.8 */
@@ -52,6 +60,24 @@ void hal_cpuHalt(void)
 }
 
 
+void hal_cpuInitCore(void)
+{
+	hal_tlbInitCore(hal_cpuGetID());
+	hal_cpuAtomicInc(&hal_cpusStarted);
+}
+
+
+void _hal_cpuInit(void)
+{
+	hal_cpusStarted = 0;
+	hal_cpuInitCore();
+	hal_cpuStartCores();
+
+	while (hal_cpusStarted != NUM_CPUS) {
+	}
+}
+
+
 int gaisler_setIomuxCfg(u8 pin, u8 opt, u8 pullup, u8 pulldn)
 {
 	(void)pin;
@@ -165,4 +191,10 @@ void _hal_platformInit(void)
 	hal_spinlockCreate(&gr712rc_common.pltctlSp, "pltctl");
 
 	gr712rc_common.cguBase = VADDR_CGU;
+
+	gr712rc_common.tlbIrqHandler.f = hal_tlbIrqHandler;
+	gr712rc_common.tlbIrqHandler.n = TLB_IRQ;
+	gr712rc_common.tlbIrqHandler.data = NULL;
+
+	hal_interruptsSetHandler(&gr712rc_common.tlbIrqHandler);
 }
diff --git a/hal/sparcv8leon3/gaisler/gr716/gr716.c b/hal/sparcv8leon3/gaisler/gr716/gr716.c
index 47637b125..6a3a45b6a 100644
--- a/hal/sparcv8leon3/gaisler/gr716/gr716.c
+++ b/hal/sparcv8leon3/gaisler/gr716/gr716.c
@@ -76,6 +76,11 @@ void hal_cpuHalt(void)
 }
 
 
+void _hal_cpuInit(void)
+{
+}
+
+
 int _gr716_getIomuxCfg(u8 pin, u8 *opt, u8 *pullup, u8 *pulldn)
 {
 	if (pin > 63) {
diff --git a/hal/sparcv8leon3/gaisler/irqamp.c b/hal/sparcv8leon3/gaisler/irqamp.c
index 01c8fbac1..71f9590ef 100644
--- a/hal/sparcv8leon3/gaisler/irqamp.c
+++ b/hal/sparcv8leon3/gaisler/irqamp.c
@@ -121,6 +121,16 @@ struct {
 extern int threads_schedule(unsigned int n, cpu_context_t *context, void *arg);
 
 
+void hal_cpuBroadcastIPI(unsigned int intr)
+{
+}
+
+
+void hal_cpuStartCores(void)
+{
+}
+
+
 static void interrupts_clearIRQ(unsigned int n)
 {
 	*(interrupts_common.int_ctrl + int_pend) &= ~(1 << n);
diff --git a/hal/sparcv8leon3/gaisler/irqmp.c b/hal/sparcv8leon3/gaisler/irqmp.c
index 6e05b146f..6e1954cec 100644
--- a/hal/sparcv8leon3/gaisler/irqmp.c
+++ b/hal/sparcv8leon3/gaisler/irqmp.c
@@ -95,6 +95,32 @@ struct {
 extern int threads_schedule(unsigned int n, cpu_context_t *context, void *arg);
 
 
+void hal_cpuBroadcastIPI(unsigned int intr)
+{
+	unsigned int id = hal_cpuGetID(), i;
+
+	for (i = 0; i < hal_cpuGetCount(); ++i) {
+		if (i != id) {
+			*(interrupts_common.int_ctrl + INT_FORCE_0 + i) |= (1 << intr);
+		}
+	}
+}
+
+
+void hal_cpuStartCores(void)
+{
+	unsigned int id = hal_cpuGetID(), i;
+	u32 msk = 0;
+
+	if (id == 0) {
+		for (i = 1; i < NUM_CPUS; ++i) {
+			msk |= (1 << i);
+		}
+		*(interrupts_common.int_ctrl + INT_MPSTAT) = msk;
+	}
+}
+
+
 void interrupts_dispatch(unsigned int n, cpu_context_t *ctx)
 {
 	intr_handler_t *h;
@@ -134,13 +160,28 @@ void interrupts_dispatch(unsigned int n, cpu_context_t *ctx)
 
 static void interrupts_enableIRQ(unsigned int irqn)
 {
-	*(interrupts_common.int_ctrl + INT_MASK_0) |= (1 << irqn);
+	int i;
+
+	/* TLB and Systick IRQ should fire on all cores */
+	if ((irqn == TLB_IRQ) || (irqn == TIMER_IRQ)) {
+		for (i = 0; i < hal_cpuGetCount(); ++i) {
+			*(interrupts_common.int_ctrl + INT_MASK_0 + i) |= (1 << irqn);
+		}
+	}
+	else {
+		/* Other IRQs only on core 0 - no easy way to manage them */
+		*(interrupts_common.int_ctrl + INT_MASK_0) |= (1 << irqn);
+	}
 }
 
 
 static void interrupts_disableIRQ(unsigned int irqn)
 {
-	*(interrupts_common.int_ctrl + INT_MASK_0) &= ~(1 << irqn);
+	int i;
+
+	for (i = 0; i < hal_cpuGetCount(); ++i) {
+		*(interrupts_common.int_ctrl + INT_MASK_0 + i) &= ~(1 << irqn);
+	}
 }
 
 
@@ -204,4 +245,5 @@ void _hal_interruptsInit(void)
 	}
 
 	interrupts_common.int_ctrl = (void *)VADDR_INT_CTRL;
+	*(interrupts_common.int_ctrl + INT_CLEAR) = 0xffffffff;
 }
diff --git a/hal/sparcv8leon3/hal.c b/hal/sparcv8leon3/hal.c
index 6c8f38b9e..ccc739b8e 100644
--- a/hal/sparcv8leon3/hal.c
+++ b/hal/sparcv8leon3/hal.c
@@ -14,6 +14,8 @@
  */
 
 #include "hal/hal.h"
+#include <arch/tlb.h>
+
 
 struct {
 	int started;
@@ -21,6 +23,7 @@ struct {
 
 syspage_t *syspage;
 unsigned int relOffs;
+u32 hal_multilock;
 
 
 extern void _hal_cpuInit(void);
@@ -52,20 +55,48 @@ void _hal_start(void)
 
 void hal_lockScheduler(void)
 {
+#ifndef NOMMU
+	hal_tlbShootdown();
+#endif
+
+	/* clang-format off */
+
+	__asm__ volatile (
+	".align 16\n\t" /* GRLIB TN-0011 errata */
+	"1: \n\t"
+		"ldstub [%0], %%g2\n\t"
+		"tst %%g2\n\t"
+		"be 3f\n\t"
+		"nop\n\t"
+	"2: \n\t"
+		"ldub [%0], %%g2\n\t"
+		"tst %%g2\n\t"
+		"bne 2b\n\t"
+		"nop\n\t"
+		"ba,a 1b\n\t"
+	"3: \n\t"
+		"nop\n\t"
+	:
+	: "r"(&hal_multilock)
+	: "g2", "memory", "cc"
+	);
+
+	/* clang-format on */
 }
 
 
 void _hal_init(void)
 {
 	_hal_spinlockInit();
-	_hal_platformInit();
 	_hal_exceptionsInit();
 	_hal_interruptsInit();
+	_hal_platformInit();
 	_hal_cpuInit();
 	_hal_consoleInit();
 	_hal_timerInit(SYSTICK_INTERVAL);
 
 	hal_common.started = 0;
+	hal_multilock = 0u;
 
 	return;
 }
diff --git a/hal/sparcv8leon3/pmap.c b/hal/sparcv8leon3/pmap.c
index 377d45d60..024027796 100644
--- a/hal/sparcv8leon3/pmap.c
+++ b/hal/sparcv8leon3/pmap.c
@@ -13,13 +13,14 @@
  * %LICENSE%
  */
 
-#include <arch/cpu.h>
+#include <arch/tlb.h>
 
 #include "hal/cpu.h"
 #include "hal/pmap.h"
 #include "hal/string.h"
 #include "hal/spinlock.h"
 #include "hal/sparcv8leon3/sparcv8leon3.h"
+#include "hal/tlb/tlb.h"
 
 #include "include/errno.h"
 #include "include/mman.h"
@@ -62,7 +63,7 @@ struct {
 	u32 pdir3[64][64] __attribute__((aligned(SIZE_PAGE)));
 
 	u8 heap[SIZE_PAGE] __attribute__((aligned(SIZE_PAGE)));
-	u8 stack[SIZE_KSTACK] __attribute__((aligned(8)));
+	u8 stack[NUM_CPUS][SIZE_KSTACK] __attribute__((aligned(SIZE_PAGE)));
 	u32 ctxMap[MAX_CONTEXTS / 32]; /* Bitmap of context numbers, 0 = taken, 1 = free */
 	u32 numCtxFree;
 	addr_t minAddr;
@@ -165,22 +166,6 @@ static void _pmap_contextDealloc(pmap_t *pmap)
 }
 
 
-static void _pmap_flushTLB(u32 context, void *vaddr)
-{
-	if (hal_srmmuGetContext() == context) {
-		if ((ptr_t)vaddr < VADDR_USR_MAX) {
-			hal_srmmuFlushTLB(vaddr, TLB_FLUSH_L3);
-		}
-		else {
-			hal_srmmuFlushTLB(vaddr, TLB_FLUSH_CTX);
-		}
-	}
-	else {
-		hal_srmmuFlushTLB(vaddr, TLB_FLUSH_ALL);
-	}
-}
-
-
 /* Function creates empty page table */
 int pmap_create(pmap_t *pmap, pmap_t *kpmap, page_t *p, void *vaddr)
 {
@@ -366,10 +351,13 @@ int pmap_enter(pmap_t *pmap, addr_t pa, void *vaddr, int attr, page_t *alloc)
 
 	if (newEntry == 0) {
 		/* Flush TLB only if entry existed earlier */
-		_pmap_flushTLB(pmap->context, vaddr);
+		hal_tlbInvalidateEntry(pmap, vaddr, 1);
+		hal_tlbCommit(&pmap_common.lock, &sc);
+	}
+	else {
+		hal_spinlockClear(&pmap_common.lock, &sc);
 	}
 
-	hal_spinlockClear(&pmap_common.lock, &sc);
 
 	return EOK;
 }
@@ -402,9 +390,9 @@ int pmap_remove(pmap_t *pmap, void *vaddr)
 	hal_cpuStorePaddr(&((u32 *)addr)[idx3], 0);
 	hal_cpuflushDCache();
 
-	_pmap_flushTLB(pmap->context, vaddr);
+	hal_tlbInvalidateEntry(pmap, vaddr, 1);
 
-	hal_spinlockClear(&pmap_common.lock, &sc);
+	hal_tlbCommit(&pmap_common.lock, &sc);
 
 	return EOK;
 }
@@ -452,9 +440,9 @@ int pmap_getPage(page_t *page, addr_t *addr)
 	}
 
 	page->flags = PAGE_OWNER_KERNEL;
-	stack = (addr_t)pmap_common.stack;
+	stack = (addr_t)pmap_common.stack - VADDR_KERNEL + min;
 
-	if ((page->addr >= stack) && (page->addr < stack + SIZE_KSTACK)) {
+	if ((page->addr >= stack) && (page->addr < stack + sizeof(pmap_common.stack))) {
 		page->flags |= PAGE_KERNEL_STACK;
 		return EOK;
 	}
diff --git a/hal/sparcv8leon3/srmmu.c b/hal/sparcv8leon3/srmmu.c
index 1da7ff06e..bf97cf6ef 100644
--- a/hal/sparcv8leon3/srmmu.c
+++ b/hal/sparcv8leon3/srmmu.c
@@ -18,7 +18,7 @@
 #include "hal/sparcv8leon3/sparcv8leon3.h"
 
 
-void hal_srmmuFlushTLB(void *vaddr, u8 type)
+void hal_srmmuFlushTLB(const void *vaddr, u8 type)
 {
 	addr_t addr = (addr_t)(((u32)vaddr & ~(0xfff)) | ((type & 0xf) << 8));
 	hal_cpuStoreAlternate(addr, ASI_FLUSH_ALL, 0);
diff --git a/hal/sparcv8leon3/srmmu.h b/hal/sparcv8leon3/srmmu.h
index 802b5b495..f9032e46e 100644
--- a/hal/sparcv8leon3/srmmu.h
+++ b/hal/sparcv8leon3/srmmu.h
@@ -57,7 +57,7 @@
 #include <arch/types.h>
 
 
-void hal_srmmuFlushTLB(void *vaddr, u8 type);
+void hal_srmmuFlushTLB(const void *vaddr, u8 type);
 
 
 u32 hal_srmmuGetFaultSts(void);
diff --git a/hal/sparcv8leon3/tlb.c b/hal/sparcv8leon3/tlb.c
new file mode 100644
index 000000000..971589b9f
--- /dev/null
+++ b/hal/sparcv8leon3/tlb.c
@@ -0,0 +1,58 @@
+/*
+ * Phoenix-RTOS
+ *
+ * Operating system kernel
+ *
+ * TLB handling
+ *
+ * Copyright 2023 Phoenix Systems
+ * Author: Andrzej Stalke, Lukasz Leczkowski
+ *
+ * This file is part of Phoenix-RTOS.
+ *
+ * %LICENSE%
+ */
+
+#include <arch/tlb.h>
+#include <arch/cpu.h>
+
+#include "srmmu.h"
+
+
+void hal_tlbFlushLocal(const pmap_t *pmap)
+{
+	if ((pmap != NULL) && (hal_srmmuGetContext() == pmap->context)) {
+		hal_srmmuFlushTLB(NULL, TLB_FLUSH_CTX);
+	}
+	else {
+		hal_srmmuFlushTLB(NULL, TLB_FLUSH_ALL);
+	}
+}
+
+
+void hal_tlbInvalidateLocalEntry(const pmap_t *pmap, const void *vaddr)
+{
+	if (hal_srmmuGetContext() == pmap->context) {
+		if ((ptr_t)vaddr < VADDR_USR_MAX) {
+			hal_srmmuFlushTLB(vaddr, TLB_FLUSH_L3);
+		}
+		else {
+			hal_srmmuFlushTLB(vaddr, TLB_FLUSH_CTX);
+		}
+	}
+	else {
+		hal_srmmuFlushTLB(vaddr, TLB_FLUSH_ALL);
+	}
+}
+
+
+int hal_tlbIrqHandler(unsigned int n, cpu_context_t *ctx, void *arg)
+{
+	(void)n;
+	(void)ctx;
+	(void)arg;
+
+	hal_tlbShootdown();
+
+	return 0;
+}