4 files changed, 11 insertions, 13 deletions
diff --git a/src/soc/intel/xeon_sp/cpx/include/soc/pci_devs.h b/src/soc/intel/xeon_sp/cpx/include/soc/pci_devs.h
index 95290f2f55..848cb48fbb 100644
--- a/src/soc/intel/xeon_sp/cpx/include/soc/pci_devs.h
+++ b/src/soc/intel/xeon_sp/cpx/include/soc/pci_devs.h
@@ -53,6 +53,7 @@
 #define UBOX_DECS_DEV			8
 #define UBOX_DECS_FUNC			2
 #define UBOX_DECS_CPUBUSNO_CSR		0xcc
+#define UBOX_DECS_CPUBUSNO1_CSR		0xd0
 
 #define VTD_TOLM_CSR			0xd0
 #define VTD_TSEG_BASE_CSR		0xa8
diff --git a/src/soc/intel/xeon_sp/include/soc/util.h b/src/soc/intel/xeon_sp/include/soc/util.h
index 2637017c89..8ff54fc1cb 100644
--- a/src/soc/intel/xeon_sp/include/soc/util.h
+++ b/src/soc/intel/xeon_sp/include/soc/util.h
@@ -8,7 +8,7 @@
 
 void get_cpubusnos(uint32_t *bus0, uint32_t *bus1, uint32_t *bus2, uint32_t *bus3);
 void unlock_pam_regions(void);
-void get_stack_busnos(uint32_t *bus);
+uint8_t get_stack_busno(const uint8_t stack);
 msr_t read_msr_ppin(void);
 int get_threads_per_package(void);
 int get_platform_thread_count(void);
diff --git a/src/soc/intel/xeon_sp/skx/include/soc/pci_devs.h b/src/soc/intel/xeon_sp/skx/include/soc/pci_devs.h
index 02061f98f0..39f6212354 100644
--- a/src/soc/intel/xeon_sp/skx/include/soc/pci_devs.h
+++ b/src/soc/intel/xeon_sp/skx/include/soc/pci_devs.h
@@ -77,6 +77,7 @@
 #define UBOX_DECS_DEV			8
 #define UBOX_DECS_FUNC			2
 #define UBOX_DECS_CPUBUSNO_CSR		0xcc
+#define UBOX_DECS_CPUBUSNO1_CSR		0xd0
 
 #define VTD_TOLM_CSR			0xd0
 #define VTD_TSEG_BASE_CSR		0xa8
diff --git a/src/soc/intel/xeon_sp/util.c b/src/soc/intel/xeon_sp/util.c
index b4f7eaab3c..c0b05cbf6c 100644
--- a/src/soc/intel/xeon_sp/util.c
+++ b/src/soc/intel/xeon_sp/util.c
@@ -13,19 +13,15 @@
 #include <soc/util.h>
 #include <timer.h>
 
-void get_stack_busnos(uint32_t *bus)
+uint8_t get_stack_busno(const uint8_t stack)
 {
-	uint32_t reg1, reg2;
-
-	reg1 = pci_mmio_read_config32(PCI_DEV(UBOX_DECS_BUS, UBOX_DECS_DEV, UBOX_DECS_FUNC),
-		0xcc);
-	reg2 = pci_mmio_read_config32(PCI_DEV(UBOX_DECS_BUS, UBOX_DECS_DEV, UBOX_DECS_FUNC),
-		0xd0);
-
-	for (int i = 0; i < 4; ++i)
-		bus[i] = ((reg1 >> (i * 8)) & 0xff);
-	for (int i = 0; i < 2; ++i)
-		bus[4+i] = ((reg2 >> (i * 8)) & 0xff);
+	if (stack >= MAX_IIO_STACK) {
+		printk(BIOS_ERR, "%s: Stack %u does not exist!\n", __func__, stack);
+		return 0;
+	}
+	const pci_devfn_t dev = PCI_DEV(UBOX_DECS_BUS, UBOX_DECS_DEV, UBOX_DECS_FUNC);
+	const uint16_t offset = stack / 4 ? UBOX_DECS_CPUBUSNO1_CSR : UBOX_DECS_CPUBUSNO_CSR;
+	return pci_io_read_config32(dev, offset) >> (8 * (stack % 4)) & 0xff;
 }
 
 void unlock_pam_regions(void)