Add vfbdev to soo

so3/arch/arm64/asm-offsets.c

@@ -86,6 +86,11 @@ int main(void)
 	DEFINE(OFFSET_PC, offsetof(struct cpu_regs, pc));
 	DEFINE(OFFSET_PSTATE, offsetof(struct cpu_regs, pstate));
 	DEFINE(OFFSET_TLS_USR, offsetof(struct cpu_regs, tls_usr));
+#ifdef CONFIG_AVZ
+	DEFINE(OFFSET_ELR_EL1, offsetof(struct cpu_regs, elr_el1));
+	DEFINE(OFFSET_SPSR_EL1, offsetof(struct cpu_regs, spsr_el1));
+#endif
+	DEFINE(S_FRAME_SIZE, sizeof(struct cpu_regs));
 
 	BLANK();
 

so3/arch/arm64/domain.c

@@ -30,6 +30,10 @@
 #include <mach/ipamap.h>
 #endif
 
+#ifdef CONFIG_SOO
+#include <avz/fbdev_gnt.h>
+#endif
+
 /**
  * @brief Initialize the content of the EL2 stack associated to this domain.
  *
@@ -131,6 +135,10 @@ void __setup_dom_pgtable(struct domain *d, addr_t paddr_start, unsigned long map
 		d->grant_pfn[i].pfn = phys_to_pfn(memslot[slotID].ipa_addr + map_size + 2 * PAGE_SIZE) + i;
 		d->grant_pfn[i].free = true;
 	}
+
+	/* Set staring framebuffer ipa address after the grant pfn area */
+	d->fbdev_start_pfn = phys_to_pfn(memslot[slotID].ipa_addr + map_size + 2 * PAGE_SIZE) + NR_GRANT_PFN;
+	fbdev_ipamap_domain(d, slotID);
 #endif /* CONFIG_SOO */
 }
 

so3/arch/arm64/exception.S

@@ -303,7 +303,7 @@ ENTRY(pre_ret_to_el1)
 
 #endif /* CONFIG_CPU_PSCI */
 
-// Enter macro to jump into EL1 from EL0 *or* from EL1
+// Enter macro to jump into EL2 from EL0 or from EL1
 .macro prepare_to_enter_to_el2
 	mrs	x0, elr_el2
 	str	x0, [sp, #OFFSET_PC]
@@ -313,11 +313,23 @@ ENTRY(pre_ret_to_el1)
 
 	mrs	x0, spsr_el2
 	str	x0, [sp, #OFFSET_PSTATE]
+
+	// Save EL0 register. Use OFFSET_SP as it is not used on the stack frame.
+	mrs	x0, sp_el0
+	str	x0, [sp, #OFFSET_SP]
+	mrs	x0, tpidr_el0
+	str	x0, [sp, #OFFSET_TLS_USR]
+
+	// Save EL1 exception register as they may not yet be saved.
+	mrs	x0, spsr_el1
+	str	x0, [sp, #OFFSET_SPSR_EL1]
+	mrs	x0, elr_el1
+	str	x0, [sp, #OFFSET_ELR_EL1]
+
 .endm
 
 
-// Exit macro at the end of an exception routine
-// It restores the sp_el0 as well.
+// Exit macro at the end of an exception routine to restore all saved registers
 .macro prepare_to_exit_to_el1
 	ldr	x0, [sp, #OFFSET_PC]
 	msr	elr_el2, x0
@@ -327,6 +339,18 @@ ENTRY(pre_ret_to_el1)
 
 	ldr	x0, [sp, #OFFSET_PSTATE]
 	msr	spsr_el2, x0
+
+	// Restore EL0 register
+	ldr	x0, [sp, #OFFSET_SP]
+	msr	sp_el0, x0
+	ldr	x0, [sp, #OFFSET_TLS_USR]
+	msr	tpidr_el0, x0
+
+	// Restore EL1 exception register as they may not yet be saved.
+	ldr	x0, [sp, #OFFSET_SPSR_EL1]
+	msr	spsr_el1, x0
+	ldr	x0, [sp, #OFFSET_ELR_EL1]
+	msr	elr_el1, x0
 .endm
 
 .align 5

so3/arch/arm64/include/asm/mmu.h

@@ -606,6 +606,15 @@ static inline int pte_type(u64 *pte)
 		ttbr;                                                  \
 	})
 
+#ifdef CONFIG_AVZ
+#define cpu_get_vttbr()                                                 \
+	({                                                              \
+		unsigned long vttbr;                                    \
+		__asm__("mrs	%0, vttbr_el2" : "=r"(vttbr) : : "cc"); \
+		vttbr;                                                  \
+	})
+#endif
+
 /**
  * Check if a virtual address is within the user space range.
  *
@@ -703,6 +712,7 @@ void *new_root_pgtable(void);
 
 void __create_mapping(void *pgtable, addr_t virt_base, addr_t phys_base, size_t size, bool nocache, mmu_stage_t stage);
 void __mmu_switch_kernel(void *pgtable, bool vttbr);
+void mmu_get_current_domain_pgtable(addr_t *pgtable_paddr);
 
 #endif /* CONFIG_AVZ */
 

so3/arch/arm64/include/asm/processor.h

@@ -871,10 +871,6 @@
 /* Safe value for MPIDR_EL1: Bit31:RES1, Bit30:U:0, Bit24:MT:0 */
 #define SYS_MPIDR_SAFE_VAL	(BIT(31))
 
-/* The stack must be 16-byte aligned */
-
-#define S_FRAME_SIZE	(8 * 36)
-
 #ifdef __ASSEMBLY__
 
 .macro current_cpu reg
@@ -1132,6 +1128,12 @@ typedef struct __attribute__((packed, aligned(8))) cpu_regs {
 	/* TLS is used by userspace to store thread context */
 	u64 tls_usr;
 
+#ifdef CONFIG_AVZ
+	/* Used to keep track of EL1 exception registers in case they haven't been saved by the domain yet */
+	u64 elr_el1;
+	u64 spsr_el1;
+#endif
+
 	/* Already aligned on 16 bytes no padding required */
 } cpu_regs_t;
 

so3/arch/arm64/mmu.c

@@ -46,13 +46,16 @@ void *current_pgtable(void)
  */
 void mmu_get_current_pgtable(addr_t *pgtable_paddr)
 {
-	int cpu;
-
-	cpu = smp_processor_id();
-
 	*pgtable_paddr = cpu_get_ttbr1();
 }
 
+#ifdef CONFIG_AVZ
+void mmu_get_current_domain_pgtable(addr_t *pgtable_paddr)
+{
+	*pgtable_paddr = cpu_get_vttbr();
+}
+#endif
+
 static void alloc_init_l3(u64 *l0pgtable, addr_t addr, addr_t end, addr_t phys, bool nocache, mmu_stage_t stage)
 {
 	u64 *l1pte, *l2pte, *l3pte;

so3/arch/arm64/thread.c

@@ -18,6 +18,7 @@
 
 #include <process.h>
 #include <thread.h>
+#include <generated/asm-offsets.h>
 
 /**
  * Set the CPU registers with thread related information

so3/arch/arm64/virt64/include/mach/ipamap.h

@@ -27,6 +27,24 @@ ipamap_t agency_ipamap[] = {
 		.phys_addr = 0x08000000,
 		.size = 0x3000000,
 	},
+	{
+		/* PCIe configuration ranges */
+		.ipa_addr = 0x4010000000,
+		.phys_addr = 0x4010000000,
+		.size = 0x10000000,
+	},
+	{
+		/* PCIe devices IO and 32 bits memory ranges */
+		.ipa_addr = 0x10000000,
+		.phys_addr = 0x10000000,
+		.size = 0x2f000000,
+	},
+	{
+		/* PCIe devices 64 bits memory ranges */
+		.ipa_addr = 0x8000000000,
+		.phys_addr = 0x8000000000,
+		.size = 0x8000000000,
+	},
 };
 
 /**

so3/avz/include/avz/domain.h

@@ -117,6 +117,9 @@ struct domain {
 
 	/* IPA reserved page frame numbers for mapping granted pages belonging to other domains */
 	grant_pfn_t grant_pfn[NR_GRANT_PFN];
+
+	/* IPA reserved starting page frame number for framebuffer mapping */
+	addr_t fbdev_start_pfn;
 #endif /* CONFIG_SOO */
 
 	int processor;

so3/avz/include/avz/fbdev_gnt.h

@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2026 Clément Dieperink <clement.dieperink@heig-vd.ch>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#ifndef FBDEV_GNT_H
+#define FBDEV_GNT_H
+
+#include <soo/uapi/soo.h>
+
+/**
+ * IPA map for the framebuffer for the given domain, either on actual buffer
+ * or the fake one depending on currently shown slot.
+ *
+ * @param d pointer to the domain to map
+ * @param slotID Slot ID of the domain
+ */
+void fbdev_ipamap_domain(struct domain *d, int slotID);
+
+/**
+ * Set the actual framebuffer physical pages ranges for
+ * futur mapping.
+ *
+ * @param fbdev Framebuffer pfns informations.
+ */
+void fbdev_set_pfns(fbdev_pfns_t *fbdev);
+
+/**
+ * Change the currently shown slot ID by remapping corresponding ipa.
+ *
+ * @param new_slotID to be shown.
+ */
+void fbdev_change_focus(int new_slotID);
+
+/**
+ * Get the frambuffer starting ipa for the current domain.
+ */
+addr_t fbdev_get_domain_ipa(void);
+
+#endif /* FBDEV_GNT_H */

so3/avz/include/avz/injector.h

@@ -55,6 +55,9 @@ struct dom_context {
 	/* IPA reserved page frame numbers for granted pages */
 	grant_pfn_t grant_pfn[NR_GRANT_PFN];
 
+	/* IPA reserved starting page frame number for framebuffer mapping */
+	addr_t fbdev_start_pfn;
+
 	/* Stack frame of this domain */
 	struct cpu_regs stack_frame;
 

so3/avz/kernel/Makefile

@@ -16,6 +16,7 @@ obj-y += sched_flip.o
 obj-y += schedule.o
 obj-y += hypercalls.o
 obj-${CONFIG_SOO} += gnttab.o
+obj-${CONFIG_SOO} += fbdev_gnt.o
 
 obj-y += image-fit.o
 obj-y += domain_utils.o

so3/avz/kernel/fbdev_gnt.c

@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) 2026 Clément Dieperink <clement.dieperink@heig-vd.ch>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include <heap.h>
+#include <avz/fbdev_gnt.h>
+#include <avz/domain.h>
+#include <avz/memslot.h>
+#include <avz/sched.h>
+
+typedef struct {
+	fbdev_pfns_t fbdev_pfns;
+	void *fake_fbdev;
+	int current_slotID;
+} fbdev_priv_t;
+
+static fbdev_priv_t priv = {};
+
+static void __map_fbdev(struct domain *d, const fbdev_pfns_t *pfn_info)
+{
+	size_t i;
+	addr_t phys_addr;
+	addr_t ipa_addr;
+	size_t size;
+	void *pgtable;
+
+	pgtable = (void *) d->pagetable_vaddr;
+	ipa_addr = pfn_to_phys(d->fbdev_start_pfn);
+
+	/* Map all distincts ranges of the framebuffer to the capsule */
+	for (i = 0; i < pfn_info->pfn_count; i++) {
+		phys_addr = pfn_to_phys(pfn_info->pfn[i]);
+		size = pfn_info->page_count[i] * PAGE_SIZE;
+
+		__create_mapping(pgtable, ipa_addr, phys_addr, size, true, S2);
+
+		ipa_addr += size;
+	}
+}
+
+static void __map_fake_fbdev(struct domain *d, const fbdev_pfns_t *real_fb)
+{
+	size_t i, j;
+	addr_t phys_addr;
+	addr_t ipa_addr;
+	void *pgtable;
+
+	/* One time malloc of fake framebuffer page */
+	if (priv.fake_fbdev == NULL) {
+		priv.fake_fbdev = malloc(PAGE_SIZE);
+		BUG_ON(!priv.fake_fbdev);
+	}
+
+	pgtable = (void *) d->pagetable_vaddr;
+	ipa_addr = pfn_to_phys(d->fbdev_start_pfn);
+	phys_addr = __pa(priv.fake_fbdev);
+
+	/* Map the capsule framebuffer to the fake one */
+	for (i = 0; i < real_fb->pfn_count; i++) {
+		for (j = 0; j < real_fb->page_count[i]; j++) {
+			__create_mapping(pgtable, ipa_addr, phys_addr, PAGE_SIZE, true, S2);
+
+			ipa_addr += PAGE_SIZE;
+		}
+	}
+}
+
+void fbdev_ipamap_domain(struct domain *d, int slotID)
+{
+	/* Only capsules have virtual framebuffer */
+	if ((slotID < MEMSLOT_BASE) && !memslot[slotID].busy)
+		return;
+
+	if (slotID == priv.current_slotID)
+		__map_fbdev(d, &priv.fbdev_pfns);
+	else
+		__map_fake_fbdev(d, &priv.fbdev_pfns);
+}
+
+void fbdev_set_pfns(fbdev_pfns_t *fbdev)
+{
+	int slotID;
+
+	memcpy(&priv.fbdev_pfns, fbdev, sizeof(*fbdev));
+
+	/* Map framebuffer to all capsules. */
+	for (slotID = MEMSLOT_BASE; slotID < MEMSLOT_NR; slotID++)
+		if (memslot[slotID].busy)
+			fbdev_ipamap_domain(domains[slotID], slotID);
+}
+
+void fbdev_change_focus(int new_slotID)
+{
+	/* Remap old capsule to fake framebuffer */
+	if ((priv.current_slotID >= MEMSLOT_BASE) && memslot[priv.current_slotID].busy)
+		__map_fake_fbdev(domains[priv.current_slotID], &priv.fbdev_pfns);
+
+	/* Map the new capsule to the framebuffer */
+	if ((new_slotID >= MEMSLOT_BASE) && memslot[new_slotID].busy)
+		__map_fbdev(domains[new_slotID], &priv.fbdev_pfns);
+
+	priv.current_slotID = new_slotID;
+}
+
+addr_t fbdev_get_domain_ipa(void)
+{
+	return pfn_to_phys(current_domain->fbdev_start_pfn);
+}