dm: acpi: retrieve physical APIC IDs and use them to fill in the ACPI MADT table

Two utility functions are copied and adapted from hyerpervisor:
ffs64
bitmap_clear_nolock

Two public functions are provided for future use (such as for RTCTv2)
pcpuid_from_vcpuid
lapicid_from_pcpuid

Tracked-On: #6020
Reviewed-by: Wang, Yu1 <yu1.wang@intel.com>
Signed-off-by: dongshen <dongsheng.x.zhang@intel.com>

devicemodel/core/vmmapi.c

@@ -758,6 +758,8 @@ vm_get_config(struct vmctx *ctx, struct acrn_vm_config *vm_cfg, struct platform_
 	memcpy((void *)vm_cfg, (void *)pcfg, sizeof(struct acrn_vm_config));
 	if (plat_info != NULL) {
 		memcpy((void *)plat_info, (void *)&platform_info, sizeof(struct platform_info));
+		pr_info("%s, l2_cat_shift=%u, l3_cat_shift=%u\n",
+			__func__, platform_info.l2_cat_shift, platform_info.l3_cat_shift);
 	}
 
 exit:

devicemodel/hw/platform/acpi/acpi.c

@@ -72,7 +72,6 @@
 #include "hpet.h"
 #include "log.h"
 #include "rtct.h"
-#include "vhm_ioctl_defs.h"
 #include "vmmapi.h"
 
 /*
@@ -249,10 +248,65 @@ basl_fwrite_xsdt(FILE *fp, struct vmctx *ctx)
 	return 0;
 }
 
+/*
+ * Find the nth (least significant) bit set of val
+ * and return the index of that bit.
+ */
+static int find_nth_set_bit_index(uint64_t val, int n)
+{
+	int idx, set;
+
+	set = 0;
+	while (val != 0UL) {
+		idx = ffs64(val);
+		bitmap_clear_nolock(idx, &val);
+		if (set == n) {
+			return idx;
+		}
+		set++;
+	}
+
+	return -1;
+}
+
+int pcpuid_from_vcpuid(uint64_t guest_pcpu_bitmask, int vcpu_id)
+{
+	return find_nth_set_bit_index(guest_pcpu_bitmask, vcpu_id);
+}
+
+int lapicid_from_pcpuid(struct platform_info *plat_info, int pcpu_id)
+{
+	return plat_info->lapic_ids[pcpu_id];
+}
+
 static int
 basl_fwrite_madt(FILE *fp, struct vmctx *ctx)
 {
 	int i;
+	struct acrn_vm_config vm_cfg;
+	struct platform_info plat_info;
+	uint64_t dm_cpu_bitmask, hv_cpu_bitmask, guest_pcpu_bitmask;
+
+	if (vm_get_config(ctx, &vm_cfg, &plat_info)) {
+		pr_err("%s, get VM configuration fail.\n", __func__);
+		return -1;
+	}
+
+	hv_cpu_bitmask = vm_cfg.cpu_affinity;
+	dm_cpu_bitmask = vm_get_cpu_affinity_dm();
+	if ((dm_cpu_bitmask != 0) && ((dm_cpu_bitmask & ~hv_cpu_bitmask) == 0)) {
+		guest_pcpu_bitmask = dm_cpu_bitmask;
+	} else {
+		guest_pcpu_bitmask = hv_cpu_bitmask;
+	}
+
+	if (guest_pcpu_bitmask == 0) {
+		pr_err("%s,Err: Invalid guest_pcpu_bitmask.\n", __func__);
+		return -1;
+	}
+
+	pr_info("%s, dm_cpu_bitmask:0x%x, hv_cpu_bitmask:0x%x, guest_cpu_bitmask: 0x%x\n",
+		__func__, dm_cpu_bitmask, hv_cpu_bitmask, guest_pcpu_bitmask);
 
 	EFPRINTF(fp, "/*\n");
 	EFPRINTF(fp, " * dm MADT template\n");
@@ -277,11 +331,31 @@ basl_fwrite_madt(FILE *fp, struct vmctx *ctx)
 
 	/* Add a Processor Local APIC entry for each CPU */
 	for (i = 0; i < basl_ncpu; i++) {
+		int pcpu_id = pcpuid_from_vcpuid(guest_pcpu_bitmask, i);
+		int lapic_id;
+
+		if (pcpu_id < 0) {
+			pr_err("%s,Err: pcpu id is not found in guest_pcpu_bitmask.\n", __func__);
+			return -1;
+		}
+
+		assert(pcpu_id < MAX_PLATFORM_LAPIC_IDS);
+		if (pcpu_id >= MAX_PLATFORM_LAPIC_IDS) {
+			pr_err("%s,Err: pcpu id %u should be less than MAX_PLATFORM_LAPIC_IDS.\n", __func__, pcpu_id);
+			return -1;
+		}
+
+		lapic_id = lapicid_from_pcpuid(&plat_info, pcpu_id);
+
 		EFPRINTF(fp, "[0001]\t\tSubtable Type : 00\n");
 		EFPRINTF(fp, "[0001]\t\tLength : 08\n");
 		/* iasl expects hex values for the proc and apic id's */
 		EFPRINTF(fp, "[0001]\t\tProcessor ID : %02x\n", i);
-		EFPRINTF(fp, "[0001]\t\tLocal Apic ID : %02x\n", i);
+
+		pr_info("%s, vcpu_id=0x%x, pcpu_id=0x%x, lapic_id=0x%x\n", __func__, i, pcpu_id, lapic_id);
+
+		EFPRINTF(fp, "[0001]\t\tLocal Apic ID : %02x\n", lapic_id);
+
 		EFPRINTF(fp, "[0004]\t\tFlags (decoded below) : 00000001\n");
 		EFPRINTF(fp, "\t\t\tProcessor Enabled : 1\n");
 		EFPRINTF(fp, "\t\t\tRuntime Online Capable : 0\n");

devicemodel/include/acpi.h

@@ -29,6 +29,8 @@
 #ifndef _ACPI_H_
 #define _ACPI_H_
 
+#include "vhm_ioctl_defs.h"
+
 #define	SCI_INT			9
 
 #define	SMI_CMD			0xb2
@@ -83,4 +85,7 @@ void	inject_power_button_event(struct vmctx *ctx);
 void	power_button_init(struct vmctx *ctx);
 void	power_button_deinit(struct vmctx *ctx);
 
+int pcpuid_from_vcpuid(uint64_t guest_pcpu_bitmask, int vcpu_id);
+int lapicid_from_pcpuid(struct platform_info *plat_info, int pcpu_id);
+
 #endif /* _ACPI_H_ */

devicemodel/include/types.h

@@ -97,6 +97,34 @@ bitmap_weight(uint64_t bits)
 
 }
 
+#define build_bitmap_clear(name, op_len, op_type, lock)			\
+static inline void name(uint16_t nr_arg, volatile op_type *addr)	\
+{									\
+	uint16_t nr;							\
+	nr = nr_arg & ((8U * sizeof(op_type)) - 1U);			\
+	asm volatile(lock "and" op_len " %1,%0"				\
+			:  "+m" (*addr)					\
+			:  "r" ((op_type)(~(1UL<<(nr))))		\
+			:  "cc", "memory");				\
+}
+build_bitmap_clear(bitmap_clear_nolock, "q", uint64_t, "")
+
+/*
+ * ffs64 - Find the first (least significant) bit set of value
+ * and return the index of that bit.
+ *
+ * @return value: zero-based bit index, or if value is zero, returns INVALID_BIT_INDEX
+ */
+#define INVALID_BIT_INDEX  0xffffU
+static inline uint16_t ffs64(uint64_t value)
+{
+	/*
+	 * __builtin_ffsl: returns one plus the index of the least significant 1-bit of value,
+	 * or if value is zero, returns zero.
+	 */
+	return value ? (uint16_t)__builtin_ffsl(value) - 1U: INVALID_BIT_INDEX;
+}
+
 /* memory barrier */
 #define mb()    ({ asm volatile("mfence" ::: "memory"); (void)0; })