From 039ade7ad0f0f88f25db7200848899a7c51b4253 Mon Sep 17 00:00:00 2001
From: Johannes Schlatow <johannes.schlatow@genode-labs.com>
Date: Mon, 27 Nov 2023 22:31:59 +0100
Subject: [PATCH] pc/platform: allow booting with enabled IOMMU

genodelabs/genode#5002
---
 .../src/drivers/platform/pc/intel/io_mmu.cc   | 29 +++++++++++--------
 .../pc/src/drivers/platform/pc/intel/io_mmu.h |  3 ++
 2 files changed, 20 insertions(+), 12 deletions(-)

diff --git a/repos/pc/src/drivers/platform/pc/intel/io_mmu.cc b/repos/pc/src/drivers/platform/pc/intel/io_mmu.cc
index de436015a3..ee8b762dc3 100644
--- a/repos/pc/src/drivers/platform/pc/intel/io_mmu.cc
+++ b/repos/pc/src/drivers/platform/pc/intel/io_mmu.cc
@@ -134,7 +134,6 @@ void Intel::Io_mmu::flush_write_buffer()
  */
 void Intel::Io_mmu::invalidate_iotlb(Domain_id domain_id, addr_t, size_t)
 {
-
 	unsigned requested_scope = Context_command::Cirg::GLOBAL;
 	if (domain_id.valid())
 		requested_scope = Context_command::Cirg::DOMAIN;
@@ -376,26 +375,29 @@ void Intel::Io_mmu::add_default_range(Range const & range, addr_t paddr)
 
 void Intel::Io_mmu::default_mappings_complete()
 {
-	/* skip if already enabled */
-	if (read<Global_status::Enabled>())
-		return;
+	Root_table_address::access_t rtp =
+		Root_table_address::Address::masked(_managed_root_table.phys_addr());
 
-	/* caches must be cleared if Esrtps is not set (see 6.6) */
-	if (!read<Capability::Esrtps>())
-		invalidate_all();
+	/* skip if already set */
+	if (read<Root_table_address>() == rtp)
+		return;
 
 	/* insert contexts into managed root table */
 	_default_mappings.copy_stage2(_managed_root_table);
 
 	/* set root table address */
-	write<Root_table_address>(
-		Root_table_address::Address::masked(_managed_root_table.phys_addr()));
+	write<Root_table_address>(rtp);
 
 	/* issue set root table pointer command */
 	_global_command<Global_command::Srtp>(1);
 
+	/* caches must be cleared if Esrtps is not set (see 6.6) */
+	if (!read<Capability::Esrtps>())
+		invalidate_all();
+
 	/* enable IOMMU */
-	_global_command<Global_command::Enable>(1);
+	if (!read<Global_status::Enabled>())
+		_global_command<Global_command::Enable>(1);
 
 	log("enabled IOMMU ", name(), " with default mappings");
 }
@@ -426,8 +428,11 @@ Intel::Io_mmu::Io_mmu(Env                      & env,
 	}
 
 	if (read<Global_status::Enabled>()) {
-		error("IOMMU already enabled");
-		return;
+		log("IOMMU has been enabled during boot");
+
+		/* disable queued invalidation interface */
+		if (read<Global_status::Qies>())
+			_global_command<Global_command::Qie>(false);
 	}
 
 	/* enable fault event interrupts */
diff --git a/repos/pc/src/drivers/platform/pc/intel/io_mmu.h b/repos/pc/src/drivers/platform/pc/intel/io_mmu.h
index 54529f197e..42ad666c53 100644
--- a/repos/pc/src/drivers/platform/pc/intel/io_mmu.h
+++ b/repos/pc/src/drivers/platform/pc/intel/io_mmu.h
@@ -251,6 +251,9 @@ class Intel::Io_mmu : private Attached_mmio,
 			/* write-buffer flush */
 			struct Wbf    : Bitfield<27,1> { };
 
+			/* queued invalidation enable */
+			struct Qie    : Bitfield<26,1> { };
+
 			/* set interrupt remap table pointer */
 			struct Sirtp  : Bitfield<24,1> { };
 		};