MADT contains the information for MPIDR which is essential for
SMP initialization, parse the GIC cpu interface structures to
get the MPIDR value and map it to cpu_logical_map(), and add
enabled cpu with valid MPIDR into cpu_possible_map and
cpu_present_map.

Signed-off-by: Hanjun Guo <redacted>
Signed-off-by: Tomasz Nowicki <redacted>
---
  arch/arm64/include/asm/acpi.h |    2 +
  arch/arm64/kernel/acpi.c      |  129 ++++++++++++++++++++++++++++++++++++++++-
  arch/arm64/kernel/smp.c       |   10 +++-
  3 files changed, 138 insertions(+), 3 deletions(-)

diff --git a/arch/arm64/include/asm/acpi.h b/arch/arm64/include/asm/acpi.h
index 6e04868..e877967 100644
--- a/arch/arm64/include/asm/acpi.h
+++ b/arch/arm64/include/asm/acpi.h

@@ -64,6 +64,8 @@ static inline void arch_fix_phys_package_id(int num, u32 slot) { }
  extern int (*acpi_suspend_lowlevel)(void);
  #define acpi_wakeup_address 0

+#define MAX_GIC_CPU_INTERFACE 65535
+

  #endif /* CONFIG_ACPI */

  #endif /*_ASM_ACPI_H*/

diff --git a/arch/arm64/kernel/acpi.c b/arch/arm64/kernel/acpi.c
index 69a315d..9e07d99 100644
--- a/arch/arm64/kernel/acpi.c
+++ b/arch/arm64/kernel/acpi.c

@@ -22,6 +22,9 @@
  #include <linux/bootmem.h>
  #include <linux/smp.h>

+#include <asm/smp_plat.h>
+#include <asm/cputype.h>
+
  int acpi_noirq;			/* skip ACPI IRQ initialization */
  int acpi_disabled;
  EXPORT_SYMBOL(acpi_disabled);

@@ -29,6 +32,8 @@ EXPORT_SYMBOL(acpi_disabled);
  int acpi_pci_disabled;		/* skip ACPI PCI scan and IRQ initialization */
  EXPORT_SYMBOL(acpi_pci_disabled);

+static int enabled_cpus;	/* Processors (GICC) with enabled flag in MADT */
+
  /*
   * __acpi_map_table() will be called before page_init(), so early_ioremap()
   * or early_memremap() should be called here to for ACPI table mapping.

@@ -49,6 +54,122 @@ void __init __acpi_unmap_table(char *map, unsigned long size)
  	early_memunmap(map, size);
  }

+/**
+ * acpi_register_gic_cpu_interface - register a gic cpu interface and
+ * generates a logical cpu number
+ * @mpidr: CPU's hardware id to register, MPIDR represented in MADT
+ * @enabled: this cpu is enabled or not
+ *
+ * Returns the logical cpu number which maps to the gic cpu interface
+ */
+static int acpi_register_gic_cpu_interface(u64 mpidr, u8 enabled)
+{

+
+	return 0;
+}
+
  static int __init acpi_parse_fadt(struct acpi_table_header *table)
  {
  	struct acpi_table_fadt *fadt = (struct acpi_table_fadt *)table;

@@ -99,8 +220,12 @@ int __init acpi_boot_init(void)
  		return -ENODEV;

  	err = acpi_table_parse(ACPI_SIG_FADT, acpi_parse_fadt);
-	if (err)
-		pr_err("Can't find FADT\n");
+	if (err) {
+		pr_err("Can't find FADT or error happened during parsing FADT\n");
+		return err;
+	}
+
+	err = acpi_parse_madt_gic_cpu_interface_entries();

  	return err;
  }

diff --git a/arch/arm64/kernel/smp.c b/arch/arm64/kernel/smp.c
index 40f38f4..8f1d37c 100644
--- a/arch/arm64/kernel/smp.c
+++ b/arch/arm64/kernel/smp.c

@@ -36,6 +36,7 @@
  #include <linux/completion.h>
  #include <linux/of.h>
  #include <linux/irq_work.h>
+#include <linux/acpi.h>

  #include <asm/atomic.h>
  #include <asm/cacheflush.h>

@@ -458,7 +459,14 @@ void __init smp_prepare_cpus(unsigned int max_cpus)
  		if (err)
  			continue;

-		set_cpu_present(cpu, true);
+		/*
+		 * In ACPI mode, cpu_present_map was initialised when
+		 * MADT table was parsed which before this function
+		 * is called.
+		 */
+		if (acpi_disabled)
+			set_cpu_present(cpu, true);
+