[PATCH v21 00/13] acpi, clocksource: add GTDT driver and GTDT support in arm_arch_timer

@@ -554,6 +554,23 @@ static int arch_timer_starting_cpu(unsigned int cpu)
 	return 0;
 }
 
+static u32 arch_timer_get_sysreg_freq(void)
+{
+	/*
+	 * Try to get the frequency from the CNTFRQ of sysreg.
+	 */
+	return arch_timer_get_cntfrq();
+}
+
+static u32 arch_timer_get_mmio_freq(void __iomem *cntbase)
+{
+	/*
+	 * Try to get the frequency from the CNTFRQ of timer frame registers.
+	 * Note: please verify cntbase in caller.
+	 */
+	return readl_relaxed(cntbase + CNTFRQ);
+}
+
 static void
 arch_timer_detect_rate(void __iomem *cntbase, struct device_node *np)
 {

@@ -568,9 +585,9 @@ arch_timer_detect_rate(void __iomem *cntbase, struct device_node *np)
 	if (!acpi_disabled ||
 	    of_property_read_u32(np, "clock-frequency", &arch_timer_rate)) {
 		if (cntbase)
-			arch_timer_rate = readl_relaxed(cntbase + CNTFRQ);
+			arch_timer_rate = arch_timer_get_mmio_freq(cntbase);
 		else
-			arch_timer_rate = arch_timer_get_cntfrq();
+			arch_timer_rate = arch_timer_get_sysreg_freq();
 	}
 
 	/* Check the timer frequency. */

@@ -571,24 +571,19 @@ static u32 arch_timer_get_mmio_freq(void __iomem *cntbase)
 	return readl_relaxed(cntbase + CNTFRQ);
 }
 
-static void
-arch_timer_detect_rate(void __iomem *cntbase, struct device_node *np)
+static void arch_timer_detect_rate(void __iomem *cntbase)
 {
 	/* Who has more than one independent system counter? */
 	if (arch_timer_rate)
 		return;
 
 	/*
-	 * Try to determine the frequency from the device tree or CNTFRQ,
-	 * if ACPI is enabled, get the frequency from CNTFRQ ONLY.
+	 * Try to determine the frequency from the MMIO timer or the sysreg.
 	 */
-	if (!acpi_disabled ||
-	    of_property_read_u32(np, "clock-frequency", &arch_timer_rate)) {
-		if (cntbase)
-			arch_timer_rate = arch_timer_get_mmio_freq(cntbase);
-		else
-			arch_timer_rate = arch_timer_get_sysreg_freq();
-	}
+	if (cntbase)
+		arch_timer_rate = arch_timer_get_mmio_freq(cntbase);
+	else
+		arch_timer_rate = arch_timer_get_sysreg_freq();
 
 	/* Check the timer frequency. */
 	if (arch_timer_rate == 0)

@@ -969,7 +964,13 @@ static int __init arch_timer_of_init(struct device_node *np)
 	for (i = ARCH_TIMER_PHYS_SECURE_PPI; i < ARCH_TIMER_MAX_TIMER_PPI; i++)
 		arch_timer_ppi[i] = irq_of_parse_and_map(np, i);
 
-	arch_timer_detect_rate(NULL, np);
+	/*
+	 * Try to determine the frequency from the device tree,
+	 * if fail, get the frequency from the sysreg CNTFRQ.
+	 */
+	if (!arch_timer_rate &&
+	    of_property_read_u32(np, "clock-frequency", &arch_timer_rate))
+		arch_timer_detect_rate(NULL);
 
 	arch_timer_c3stop = !of_property_read_bool(np, "always-on");
 

@@ -1080,7 +1081,14 @@ static int __init arch_timer_mem_init(struct device_node *np)
 		goto out;
 	}
 
-	arch_timer_detect_rate(base, np);
+	/*
+	 * Try to determine the frequency from the device tree,
+	 * if fail, get the frequency from the CNTFRQ reg of MMIO timer.
+	 */
+	if (!arch_timer_rate &&
+	    of_property_read_u32(np, "clock-frequency", &arch_timer_rate))
+		arch_timer_detect_rate(base);
+
 	ret = arch_timer_mem_register(base, irq);
 	if (ret)
 		goto out;

@@ -1141,8 +1149,8 @@ static int __init arch_timer_acpi_init(struct acpi_table_header *table)
 		map_generic_timer_interrupt(gtdt->non_secure_el2_interrupt,
 		gtdt->non_secure_el2_flags);
 
-	/* Get the frequency from CNTFRQ */
-	arch_timer_detect_rate(NULL, NULL);
+	/* Get the frequency from the sysreg CNTFRQ */
+	arch_timer_detect_rate(NULL);
 
 	arch_timer_uses_ppi = arch_timer_select_ppi();
 	if (!arch_timer_ppi[arch_timer_uses_ppi]) {

@@ -571,25 +571,6 @@ static u32 arch_timer_get_mmio_freq(void __iomem *cntbase)
 	return readl_relaxed(cntbase + CNTFRQ);
 }
 
-static void arch_timer_detect_rate(void __iomem *cntbase)
-{
-	/* Who has more than one independent system counter? */
-	if (arch_timer_rate)
-		return;
-
-	/*
-	 * Try to determine the frequency from the MMIO timer or the sysreg.
-	 */
-	if (cntbase)
-		arch_timer_rate = arch_timer_get_mmio_freq(cntbase);
-	else
-		arch_timer_rate = arch_timer_get_sysreg_freq();
-
-	/* Check the timer frequency. */
-	if (arch_timer_rate == 0)
-		pr_warn("frequency not available\n");
-}
-
 static void arch_timer_banner(unsigned type)
 {
 	pr_info("%s%s%s timer(s) running at %lu.%02luMHz (%s%s%s).\n",

@@ -970,7 +951,11 @@ static int __init arch_timer_of_init(struct device_node *np)
 	 */
 	if (!arch_timer_rate &&
 	    of_property_read_u32(np, "clock-frequency", &arch_timer_rate))
-		arch_timer_detect_rate(NULL);
+		arch_timer_rate = arch_timer_get_sysreg_freq();
+	if (!arch_timer_rate) {
+		pr_err(FW_BUG "frequency not available.\n");
+		return -EINVAL;
+	}
 
 	arch_timer_c3stop = !of_property_read_bool(np, "always-on");
 

@@ -1087,7 +1072,12 @@ static int __init arch_timer_mem_init(struct device_node *np)
 	 */
 	if (!arch_timer_rate &&
 	    of_property_read_u32(np, "clock-frequency", &arch_timer_rate))
-		arch_timer_detect_rate(base);
+		arch_timer_rate = arch_timer_get_mmio_freq(base);
+	if (!arch_timer_rate) {
+		pr_err(FW_BUG "frequency not available for MMIO timer.\n");
+		ret = -EINVAL;
+		goto out;
+	}
 
 	ret = arch_timer_mem_register(base, irq);
 	if (ret)

@@ -1150,7 +1140,11 @@ static int __init arch_timer_acpi_init(struct acpi_table_header *table)
 		gtdt->non_secure_el2_flags);
 
 	/* Get the frequency from the sysreg CNTFRQ */
-	arch_timer_detect_rate(NULL);
+	arch_timer_rate = arch_timer_get_sysreg_freq();
+	if (!arch_timer_rate) {
+		pr_err(FW_BUG "frequency not available.\n");
+		return -EINVAL;
+	}
 
 	arch_timer_uses_ppi = arch_timer_select_ppi();
 	if (!arch_timer_ppi[arch_timer_uses_ppi]) {

@@ -65,7 +65,8 @@ struct arch_timer {
 
 #define to_arch_timer(e) container_of(e, struct arch_timer, evt)
 
-static u32 arch_timer_rate;
+static u32 arch_timer_sysreg_freq;
+static u32 arch_timer_mmio_freq;
 static int arch_timer_ppi[ARCH_TIMER_MAX_TIMER_PPI];
 
 static struct clock_event_device __percpu *arch_timer_evt;

@@ -417,6 +418,7 @@ static void erratum_workaround_set_sne(struct clock_event_device *clk)
 static void __arch_timer_setup(unsigned type,
 			       struct clock_event_device *clk)
 {
+	u32 freq;
 	clk->features = CLOCK_EVT_FEAT_ONESHOT;
 
 	if (type == ARCH_TIMER_TYPE_CP15) {

@@ -444,6 +446,7 @@ static void __arch_timer_setup(unsigned type,
 		}
 
 		erratum_workaround_set_sne(clk);
+		freq = arch_timer_sysreg_freq;
 	} else {
 		clk->features |= CLOCK_EVT_FEAT_DYNIRQ;
 		clk->name = "arch_mem_timer";

@@ -460,11 +463,12 @@ static void __arch_timer_setup(unsigned type,
 			clk->set_next_event =
 				arch_timer_set_next_event_phys_mem;
 		}
+		freq = arch_timer_mmio_freq;
 	}
 
 	clk->set_state_shutdown(clk);
 
-	clockevents_config_and_register(clk, arch_timer_rate, 0xf, 0x7fffffff);
+	clockevents_config_and_register(clk, freq, 0xf, 0x7fffffff);
 }
 
 static void arch_timer_evtstrm_enable(int divider)

@@ -487,7 +491,7 @@ static void arch_timer_configure_evtstream(void)
 	int evt_stream_div, pos;
 
 	/* Find the closest power of two to the divisor */
-	evt_stream_div = arch_timer_rate / ARCH_TIMER_EVT_STREAM_FREQ;
+	evt_stream_div = arch_timer_sysreg_freq / ARCH_TIMER_EVT_STREAM_FREQ;
 	pos = fls(evt_stream_div);
 	if (pos > 1 && !(evt_stream_div & (1 << (pos - 2))))
 		pos--;

@@ -578,8 +582,8 @@ static void arch_timer_banner(unsigned type)
 		type == (ARCH_TIMER_TYPE_CP15 | ARCH_TIMER_TYPE_MEM) ?
 			" and " : "",
 		type & ARCH_TIMER_TYPE_MEM ? "mmio" : "",
-		(unsigned long)arch_timer_rate / 1000000,
-		(unsigned long)(arch_timer_rate / 10000) % 100,
+		(unsigned long)arch_timer_sysreg_freq / 1000000,
+		(unsigned long)(arch_timer_sysreg_freq / 10000) % 100,
 		type & ARCH_TIMER_TYPE_CP15 ?
 			(arch_timer_uses_ppi == ARCH_TIMER_VIRT_PPI) ? "virt" : "phys" :
 			"",

@@ -591,7 +595,7 @@ static void arch_timer_banner(unsigned type)
 
 u32 arch_timer_get_rate(void)
 {
-	return arch_timer_rate;
+	return arch_timer_sysreg_freq;
 }
 
 static u64 arch_counter_get_cntvct_mem(void)

@@ -648,6 +652,7 @@ struct arch_timer_kvm_info *arch_timer_get_kvm_info(void)
 static void __init arch_counter_register(unsigned type)
 {
 	u64 start_count;
+	u32 freq;
 
 	/* Register the CP15 based counter if we have one */
 	if (type & ARCH_TIMER_TYPE_CP15) {

@@ -657,6 +662,8 @@ static void __init arch_counter_register(unsigned type)
 		else
 			arch_timer_read_counter = arch_counter_get_cntpct;
 
+		freq = arch_timer_sysreg_freq;
+
 		clocksource_counter.archdata.vdso_direct = true;
 
 #ifdef CONFIG_ARM_ARCH_TIMER_OOL_WORKAROUND

@@ -669,19 +676,20 @@ static void __init arch_counter_register(unsigned type)
 #endif
 	} else {
 		arch_timer_read_counter = arch_counter_get_cntvct_mem;
+		freq = arch_timer_mmio_freq;
 	}
 
 	if (!arch_counter_suspend_stop)
 		clocksource_counter.flags |= CLOCK_SOURCE_SUSPEND_NONSTOP;
 	start_count = arch_timer_read_counter();
-	clocksource_register_hz(&clocksource_counter, arch_timer_rate);
+	clocksource_register_hz(&clocksource_counter, freq);
 	cyclecounter.mult = clocksource_counter.mult;
 	cyclecounter.shift = clocksource_counter.shift;
 	timecounter_init(&arch_timer_kvm_info.timecounter,
 			 &cyclecounter, start_count);
 
 	/* 56 bits minimum, so we assume worst case rollover */
-	sched_clock_register(arch_timer_read_counter, 56, arch_timer_rate);
+	sched_clock_register(arch_timer_read_counter, 56, freq);
 }
 
 static void arch_timer_stop(struct clock_event_device *clk)

@@ -949,10 +957,9 @@ static int __init arch_timer_of_init(struct device_node *np)
 	 * Try to determine the frequency from the device tree,
 	 * if fail, get the frequency from the sysreg CNTFRQ.
 	 */
-	if (!arch_timer_rate &&
-	    of_property_read_u32(np, "clock-frequency", &arch_timer_rate))
-		arch_timer_rate = arch_timer_get_sysreg_freq();
-	if (!arch_timer_rate) {
+	if (of_property_read_u32(np, "clock-frequency", &arch_timer_sysreg_freq))
+		arch_timer_sysreg_freq = arch_timer_get_sysreg_freq();
+	if (!arch_timer_sysreg_freq) {
 		pr_err(FW_BUG "frequency not available.\n");
 		return -EINVAL;
 	}

@@ -1070,10 +1077,9 @@ static int __init arch_timer_mem_init(struct device_node *np)
 	 * Try to determine the frequency from the device tree,
 	 * if fail, get the frequency from the CNTFRQ reg of MMIO timer.
 	 */
-	if (!arch_timer_rate &&
-	    of_property_read_u32(np, "clock-frequency", &arch_timer_rate))
-		arch_timer_rate = arch_timer_get_mmio_freq(base);
-	if (!arch_timer_rate) {
+	if (of_property_read_u32(np, "clock-frequency", &arch_timer_mmio_freq))
+		arch_timer_mmio_freq = arch_timer_get_mmio_freq(base);
+	if (!arch_timer_mmio_freq) {
 		pr_err(FW_BUG "frequency not available for MMIO timer.\n");
 		ret = -EINVAL;
 		goto out;

@@ -1140,8 +1146,8 @@ static int __init arch_timer_acpi_init(struct acpi_table_header *table)
 		gtdt->non_secure_el2_flags);
 
 	/* Get the frequency from the sysreg CNTFRQ */
-	arch_timer_rate = arch_timer_get_sysreg_freq();
-	if (!arch_timer_rate) {
+	arch_timer_sysreg_freq = arch_timer_get_sysreg_freq();
+	if (!arch_timer_sysreg_freq) {
 		pr_err(FW_BUG "frequency not available.\n");
 		return -EINVAL;
 	}

@@ -887,9 +887,6 @@ static bool __init arch_timer_needs_of_probing(void)
 
 static int __init arch_timer_common_init(void)
 {
-	if (acpi_disabled && arch_timer_needs_of_probing())
-		return 0;
-
 	arch_timer_banner(arch_timers_present);
 	arch_counter_register(arch_timers_present);
 	return arch_timer_arch_init();

@@ -927,26 +924,9 @@ static enum arch_timer_ppi_nr __init arch_timer_select_ppi(void)
 	return ARCH_TIMER_PHYS_SECURE_PPI;
 }
 
-static int __init arch_timer_init(void)
-{
-	int ret;
-
-	ret = arch_timer_register();
-	if (ret)
-		return ret;
-
-	ret = arch_timer_common_init();
-	if (ret)
-		return ret;
-
-	arch_timer_kvm_info.virtual_irq = arch_timer_ppi[ARCH_TIMER_VIRT_PPI];
-
-	return 0;
-}
-
 static int __init arch_timer_of_init(struct device_node *np)
 {
-	int i;
+	int i, ret;
 
 	if (arch_timers_present & ARCH_TIMER_TYPE_CP15) {
 		pr_warn("multiple nodes in dt, skipping\n");

@@ -957,6 +937,8 @@ static int __init arch_timer_of_init(struct device_node *np)
 	for (i = ARCH_TIMER_PHYS_SECURE_PPI; i < ARCH_TIMER_MAX_TIMER_PPI; i++)
 		arch_timer_ppi[i] = irq_of_parse_and_map(np, i);
 
+	arch_timer_kvm_info.virtual_irq = arch_timer_ppi[ARCH_TIMER_VIRT_PPI];
+
 	/*
 	 * Try to determine the frequency from the device tree,
 	 * if fail, get the frequency from the sysreg CNTFRQ.

@@ -1001,7 +983,14 @@ static int __init arch_timer_of_init(struct device_node *np)
 	arch_counter_suspend_stop = of_property_read_bool(np,
 							 "arm,no-tick-in-suspend");
 
-	return arch_timer_init();
+	ret = arch_timer_register();
+	if (ret)
+		return ret;
+
+	if (arch_timer_needs_of_probing())
+		return 0;
+
+	return arch_timer_common_init();
 }
 CLOCKSOURCE_OF_DECLARE(armv7_arch_timer, "arm,armv7-timer", arch_timer_of_init);
 CLOCKSOURCE_OF_DECLARE(armv8_arch_timer, "arm,armv8-timer", arch_timer_of_init);

@@ -1093,7 +1082,8 @@ static int __init arch_timer_mem_init(struct device_node *np)
 	if (ret)
 		goto out;
 
-	return arch_timer_common_init();
+	if (!arch_timer_needs_of_probing())
+		ret = arch_timer_common_init();
 out:
 	iounmap(cntctlbase);
 	of_node_put(best_frame);

@@ -1122,6 +1112,7 @@ static int __init map_generic_timer_interrupt(u32 interrupt, u32 flags)
 /* Initialize per-processor generic timer */
 static int __init arch_timer_acpi_init(struct acpi_table_header *table)
 {
+	int ret;
 	struct acpi_table_gtdt *gtdt;
 
 	if (arch_timers_present & ARCH_TIMER_TYPE_CP15) {

@@ -1149,6 +1140,8 @@ static int __init arch_timer_acpi_init(struct acpi_table_header *table)
 		map_generic_timer_interrupt(gtdt->non_secure_el2_interrupt,
 		gtdt->non_secure_el2_flags);
 
+	arch_timer_kvm_info.virtual_irq = arch_timer_ppi[ARCH_TIMER_VIRT_PPI];
+
 	/* Get the frequency from the sysreg CNTFRQ */
 	arch_timer_sysreg_freq = arch_timer_get_sysreg_freq();
 	if (!arch_timer_sysreg_freq) {

@@ -1165,8 +1158,11 @@ static int __init arch_timer_acpi_init(struct acpi_table_header *table)
 	/* Always-on capability */
 	arch_timer_c3stop = !(gtdt->non_secure_el1_flags & ACPI_GTDT_ALWAYS_ON);
 
-	arch_timer_init();
-	return 0;
+	ret = arch_timer_register();
+	if (ret)
+		return ret;
+
+	return arch_timer_common_init();
 }
 CLOCKSOURCE_ACPI_DECLARE(arch_timer, ACPI_SIG_GTDT, arch_timer_acpi_init);
 #endif

@@ -858,15 +858,28 @@ static const struct of_device_id arch_timer_mem_of_match[] __initconst = {
 	{},
 };
 
-static bool __init
-arch_timer_needs_probing(int type, const struct of_device_id *matches)
+static bool __init arch_timer_needs_of_probing(void)
 {
 	struct device_node *dn;
 	bool needs_probing = false;
+	unsigned int mask = ARCH_TIMER_TYPE_CP15 | ARCH_TIMER_TYPE_MEM;
 
-	dn = of_find_matching_node(NULL, matches);
-	if (dn && of_device_is_available(dn) && !(arch_timers_present & type))
+	/* We have two timers, and both device-tree nodes are probed. */
+	if ((arch_timers_present & mask) == mask)
+		return false;
+
+	/*
+	 * Only one type of timer is probed,
+	 * check if we have another type of timer node in device-tree.
+	 */
+	if (arch_timers_present & ARCH_TIMER_TYPE_CP15)
+		dn = of_find_matching_node(NULL, arch_timer_mem_of_match);
+	else
+		dn = of_find_matching_node(NULL, arch_timer_of_match);
+
+	if (dn && of_device_is_available(dn))
 		needs_probing = true;
+
 	of_node_put(dn);
 
 	return needs_probing;

@@ -874,17 +887,8 @@ arch_timer_needs_probing(int type, const struct of_device_id *matches)
 
 static int __init arch_timer_common_init(void)
 {
-	unsigned mask = ARCH_TIMER_TYPE_CP15 | ARCH_TIMER_TYPE_MEM;
-
-	/* Wait until both nodes are probed if we have two timers */
-	if ((arch_timers_present & mask) != mask) {
-		if (arch_timer_needs_probing(ARCH_TIMER_TYPE_MEM,
-					     arch_timer_mem_of_match))
-			return 0;
-		if (arch_timer_needs_probing(ARCH_TIMER_TYPE_CP15,
-					     arch_timer_of_match))
-			return 0;
-	}
+	if (acpi_disabled && arch_timer_needs_of_probing())
+		return 0;
 
 	arch_timer_banner(arch_timers_present);
 	arch_counter_register(arch_timers_present);

@@ -57,6 +57,8 @@ enum arch_timer_spi_nr {
 #define ARCH_TIMER_MEM_PHYS_ACCESS	2
 #define ARCH_TIMER_MEM_VIRT_ACCESS	3
 
+#define ARCH_TIMER_MEM_MAX_FRAMES	8
+
 #define ARCH_TIMER_USR_PCT_ACCESS_EN	(1 << 0) /* physical counter */
 #define ARCH_TIMER_USR_VCT_ACCESS_EN	(1 << 1) /* virtual counter */
 #define ARCH_TIMER_VIRT_EVT_EN		(1 << 2)

@@ -72,6 +74,21 @@ struct arch_timer_kvm_info {
 	int virtual_irq;
 };
 
+struct arch_timer_mem_frame {
+	int frame_nr;
+	phys_addr_t cntbase;
+	size_t size;
+	int phys_irq;
+	int virt_irq;
+};
+
+struct arch_timer_mem {
+	phys_addr_t cntctlbase;
+	size_t size;
+	int num_frames;
+	struct arch_timer_mem_frame frame[ARCH_TIMER_MEM_MAX_FRAMES];
+};
+
 #ifdef CONFIG_ARM_ARCH_TIMER
 
 extern u32 arch_timer_get_rate(void);

@@ -995,17 +995,20 @@ static int __init arch_timer_of_init(struct device_node *np)
 CLOCKSOURCE_OF_DECLARE(armv7_arch_timer, "arm,armv7-timer", arch_timer_of_init);
 CLOCKSOURCE_OF_DECLARE(armv8_arch_timer, "arm,armv8-timer", arch_timer_of_init);
 
-static int __init arch_timer_mem_init(struct device_node *np)
+static int __init arch_timer_mem_init(struct arch_timer_mem *timer_mem)
 {
-	struct device_node *frame, *best_frame = NULL;
 	void __iomem *cntctlbase, *base;
-	unsigned int irq, ret = -EINVAL;
+	struct arch_timer_mem_frame *best_frame = NULL;
+	unsigned int irq;
 	u32 cnttidr;
+	int i, ret;
 
-	arch_timers_present |= ARCH_TIMER_TYPE_MEM;
-	cntctlbase = of_iomap(np, 0);
+	if (!timer_mem->num_frames)
+		return -ENODEV;
+
+	cntctlbase = ioremap(timer_mem->cntctlbase, timer_mem->size);
 	if (!cntctlbase) {
-		pr_err("Can't find CNTCTLBase\n");
+		pr_err("Can't map CNTCTLBase.\n");
 		return -ENXIO;
 	}
 

@@ -1015,26 +1018,18 @@ static int __init arch_timer_mem_init(struct device_node *np)
 	 * Try to find a virtual capable frame. Otherwise fall back to a
 	 * physical capable frame.
 	 */
-	for_each_available_child_of_node(np, frame) {
-		int n;
-		u32 cntacr;
-
-		if (of_property_read_u32(frame, "frame-number", &n)) {
-			pr_err("Missing frame-number\n");
-			of_node_put(frame);
-			goto out;
-		}
+	for (i = 0; i < timer_mem->num_frames; i++) {
+		u32 cntacr = CNTACR_RFRQ | CNTACR_RWPT | CNTACR_RPCT |
+			     CNTACR_RWVT | CNTACR_RVOFF | CNTACR_RVCT;
+		int n = timer_mem->frame[i].frame_nr;
 
 		/* Try enabling everything, and see what sticks */
-		cntacr = CNTACR_RFRQ | CNTACR_RWPT | CNTACR_RPCT |
-			 CNTACR_RWVT | CNTACR_RVOFF | CNTACR_RVCT;
 		writel_relaxed(cntacr, cntctlbase + CNTACR(n));
 		cntacr = readl_relaxed(cntctlbase + CNTACR(n));
 
 		if ((cnttidr & CNTTIDR_VIRT(n)) &&
 		    !(~cntacr & (CNTACR_RWVT | CNTACR_RVCT))) {
-			of_node_put(best_frame);
-			best_frame = frame;
+			best_frame = &timer_mem->frame[i];
 			arch_timer_mem_use_virtual = true;
 			break;
 		}

@@ -1042,55 +1037,126 @@ static int __init arch_timer_mem_init(struct device_node *np)
 		if (~cntacr & (CNTACR_RWPT | CNTACR_RPCT))
 			continue;
 
-		of_node_put(best_frame);
-		best_frame = of_node_get(frame);
+		best_frame = &timer_mem->frame[i];
 	}
+	iounmap(cntctlbase);
 
-	ret= -ENXIO;
-	base = arch_counter_base = of_io_request_and_map(best_frame, 0,
-							 "arch_mem_timer");
-	if (IS_ERR(base)) {
-		pr_err("Can't map frame's registers\n");
-		goto out;
+	if (!best_frame) {
+		pr_err("Can't find frame for register\n");
+		return -EINVAL;
 	}
 
 	if (arch_timer_mem_use_virtual)
-		irq = irq_of_parse_and_map(best_frame, ARCH_TIMER_VIRT_SPI);
+		irq = best_frame->virt_irq;
 	else
-		irq = irq_of_parse_and_map(best_frame, ARCH_TIMER_PHYS_SPI);
+		irq = best_frame->phys_irq;
 
-	ret = -EINVAL;
 	if (!irq) {
 		pr_err("Frame missing %s irq.\n",
 		       arch_timer_mem_use_virtual ? "virt" : "phys");
-		goto out;
+		return -EINVAL;
 	}
 
-	/*
-	 * Try to determine the frequency from the device tree,
-	 * if fail, get the frequency from the CNTFRQ reg of MMIO timer.
-	 */
-	if (of_property_read_u32(np, "clock-frequency", &arch_timer_mmio_freq))
+	if (!request_mem_region(best_frame->cntbase, best_frame->size,
+				"arch_mem_timer"))
+		return -EBUSY;
+
+	base = ioremap(best_frame->cntbase, best_frame->size);
+	if (!base) {
+		pr_err("Can't map frame's registers\n");
+		return -ENXIO;
+	}
+
+	if (!arch_timer_mmio_freq)
 		arch_timer_mmio_freq = arch_timer_get_mmio_freq(base);
-	if (!arch_timer_mmio_freq) {
-		pr_err(FW_BUG "frequency not available for MMIO timer.\n");
-		ret = -EINVAL;
-		goto out;
+	if (!arch_timer_mmio_freq ||
+	    (arch_timer_sysreg_freq &&
+	     arch_timer_mmio_freq != arch_timer_sysreg_freq)) {
+		pr_err(FW_BUG "invalid frequency for MMIO timer.\n");
+		arch_timer_mmio_freq = 0;
+		iounmap(base);
+		return -EINVAL;
 	}
 
 	ret = arch_timer_mem_register(base, irq);
-	if (ret)
+	if (ret) {
+		iounmap(base);
+		return ret;
+	}
+
+	arch_counter_base = base;
+	arch_timers_present |= ARCH_TIMER_TYPE_MEM;
+
+	return 0;
+}
+
+static int __init arch_timer_mem_of_init(struct device_node *np)
+{
+	struct arch_timer_mem *timer_mem;
+	struct device_node *frame_node;
+	struct resource res;
+	int i, ret = -EINVAL;
+
+	timer_mem = kzalloc(sizeof(*timer_mem), GFP_KERNEL);
+	if (!timer_mem)
+		return -ENOMEM;
+
+	if (of_address_to_resource(np, 0, &res))
 		goto out;
+	timer_mem->cntctlbase = res.start;
+	timer_mem->size = resource_size(&res);
+
+	i = 0;
+	for_each_available_child_of_node(np, frame_node) {
+		int n;
+		struct arch_timer_mem_frame *frame;
+
+		if (i >= ARCH_TIMER_MEM_MAX_FRAMES) {
+			pr_err(FW_BUG "too many frames, only %u are permitted.\n",
+			       ARCH_TIMER_MEM_MAX_FRAMES);
+			goto out;
+		}
 
-	if (!arch_timer_needs_of_probing())
+		frame = &timer_mem->frame[i];
+
+		if (of_property_read_u32(frame_node, "frame-number", &n)) {
+			pr_err(FW_BUG "Missing frame-number\n");
+			of_node_put(frame_node);
+			goto out;
+		}
+		frame->frame_nr = n;
+
+		if (of_address_to_resource(frame_node, 0, &res)) {
+			of_node_put(frame_node);
+			goto out;
+		}
+		frame->cntbase = res.start;
+		frame->size = resource_size(&res);
+
+		frame->virt_irq = irq_of_parse_and_map(frame_node,
+						       ARCH_TIMER_VIRT_SPI);
+		frame->phys_irq = irq_of_parse_and_map(frame_node,
+						       ARCH_TIMER_PHYS_SPI);
+
+		i++;
+	}
+	timer_mem->num_frames = i;
+
+	/*
+	 * Try to get the frequency from the device tree,
+	 * if fail, we will try the CNTFRQ register in arch_timer_mem_init.
+	 */
+	of_property_read_u32(np, "clock-frequency", &arch_timer_mmio_freq);
+
+	ret = arch_timer_mem_init(timer_mem);
+	if (!ret && !arch_timer_needs_of_probing())
 		ret = arch_timer_common_init();
 out:
-	iounmap(cntctlbase);
-	of_node_put(best_frame);
+	kfree(timer_mem);
 	return ret;
 }
 CLOCKSOURCE_OF_DECLARE(armv7_arch_timer_mem, "arm,armv7-timer-mem",
-		       arch_timer_mem_init);
+		       arch_timer_mem_of_init);
 
 #ifdef CONFIG_ACPI
 static int __init map_generic_timer_interrupt(u32 interrupt, u32 flags)

@@ -2,6 +2,7 @@ config ARM64
 	def_bool y
 	select ACPI_CCA_REQUIRED if ACPI
 	select ACPI_GENERIC_GSI if ACPI
+	select ACPI_GTDT if ACPI
 	select ACPI_REDUCED_HARDWARE_ONLY if ACPI
 	select ACPI_MCFG if ACPI
 	select ACPI_SPCR_TABLE if ACPI

@@ -4,3 +4,6 @@
 
 config ACPI_IORT
 	bool
+
+config ACPI_GTDT
+	bool

@@ -1 +1,2 @@
 obj-$(CONFIG_ACPI_IORT) 	+= iort.o
+obj-$(CONFIG_ACPI_GTDT) 	+= gtdt.o

@@ -0,0 +1,157 @@
+/*
+ * ARM Specific GTDT table Support
+ *
+ * Copyright (C) 2016, Linaro Ltd.
+ * Author: Daniel Lezcano <daniel.lezcano@linaro.org>
+ *         Fu Wei <fu.wei@linaro.org>
+ *         Hanjun Guo <hanjun.guo@linaro.org>
+ *
+ * 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.
+ */
+
+#include <linux/acpi.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+
+#include <clocksource/arm_arch_timer.h>
+
+#undef pr_fmt
+#define pr_fmt(fmt) "ACPI GTDT: " fmt
+
+/**
+ * struct acpi_gtdt_descriptor - Store the key info of GTDT for all functions
+ * @gtdt:	The pointer to the struct acpi_table_gtdt of GTDT table.
+ * @gtdt_end:	The pointer to the end of GTDT table.
+ * @platform_timer:	The pointer to the start of Platform Timer Structure
+ *
+ * The struct store the key info of GTDT table, it should be initialized by
+ * acpi_gtdt_init.
+ */
+struct acpi_gtdt_descriptor {
+	struct acpi_table_gtdt *gtdt;
+	void *gtdt_end;
+	void *platform_timer;
+};
+
+static struct acpi_gtdt_descriptor acpi_gtdt_desc __initdata;
+
+static int __init map_gt_gsi(u32 interrupt, u32 flags)
+{
+	int trigger, polarity;
+
+	trigger = (flags & ACPI_GTDT_INTERRUPT_MODE) ? ACPI_EDGE_SENSITIVE
+			: ACPI_LEVEL_SENSITIVE;
+
+	polarity = (flags & ACPI_GTDT_INTERRUPT_POLARITY) ? ACPI_ACTIVE_LOW
+			: ACPI_ACTIVE_HIGH;
+
+	return acpi_register_gsi(NULL, interrupt, trigger, polarity);
+}
+
+/**
+ * acpi_gtdt_map_ppi() - Map the PPIs of per-cpu arch_timer.
+ * @type:	the type of PPI.
+ *
+ * Note: Linux on arm64 isn't supported on the secure side.
+ * So we only handle the non-secure timer PPIs,
+ * ARCH_TIMER_PHYS_SECURE_PPI is treated as invalid type.
+ *
+ * Return: the mapped PPI value, 0 if error.
+ */
+int __init acpi_gtdt_map_ppi(int type)
+{
+	struct acpi_table_gtdt *gtdt = acpi_gtdt_desc.gtdt;
+
+	switch (type) {
+	case ARCH_TIMER_PHYS_NONSECURE_PPI:
+		return map_gt_gsi(gtdt->non_secure_el1_interrupt,
+				  gtdt->non_secure_el1_flags);
+	case ARCH_TIMER_VIRT_PPI:
+		return map_gt_gsi(gtdt->virtual_timer_interrupt,
+				  gtdt->virtual_timer_flags);
+
+	case ARCH_TIMER_HYP_PPI:
+		return map_gt_gsi(gtdt->non_secure_el2_interrupt,
+				  gtdt->non_secure_el2_flags);
+	default:
+		pr_err("Failed to map timer interrupt: invalid type.\n");
+	}
+
+	return 0;
+}
+
+/**
+ * acpi_gtdt_c3stop() - Got c3stop info from GTDT according to the type of PPI.
+ * @type:	the type of PPI.
+ *
+ * Return: 1 if the timer can be in deep idle state, 0 otherwise.
+ */
+bool __init acpi_gtdt_c3stop(int type)
+{
+	struct acpi_table_gtdt *gtdt = acpi_gtdt_desc.gtdt;
+
+	switch (type) {
+	case ARCH_TIMER_PHYS_NONSECURE_PPI:
+		return !(gtdt->non_secure_el1_flags & ACPI_GTDT_ALWAYS_ON);
+
+	case ARCH_TIMER_VIRT_PPI:
+		return !(gtdt->virtual_timer_flags & ACPI_GTDT_ALWAYS_ON);
+
+	case ARCH_TIMER_HYP_PPI:
+		return !(gtdt->non_secure_el2_flags & ACPI_GTDT_ALWAYS_ON);
+
+	default:
+		pr_err("Failed to get c3stop info: invalid type.\n");
+	}
+
+	return 0;
+}
+
+/**
+ * acpi_gtdt_init() - Get the info of GTDT table to prepare for further init.
+ * @table:	The pointer to GTDT table.
+ * @platform_timer_count:	The pointer of int variate for returning the
+ *				number of platform timers. It can be NULL, if
+ *				driver don't need this info.
+ *
+ * Return: 0 if success, -EINVAL if error.
+ */
+int __init acpi_gtdt_init(struct acpi_table_header *table,
+			  int *platform_timer_count)
+{
+	int ret = 0;
+	int timer_count = 0;
+	void *platform_timer = NULL;
+	struct acpi_table_gtdt *gtdt;
+
+	gtdt = container_of(table, struct acpi_table_gtdt, header);
+	acpi_gtdt_desc.gtdt = gtdt;
+	acpi_gtdt_desc.gtdt_end = (void *)table + table->length;
+
+	if (table->revision < 2)
+		pr_warn("Revision:%d doesn't support Platform Timers.\n",
+			table->revision);
+	else if (!gtdt->platform_timer_count)
+		pr_debug("No Platform Timer.\n");
+	else
+		timer_count = gtdt->platform_timer_count;
+
+	if (timer_count) {
+		platform_timer = (void *)gtdt + gtdt->platform_timer_offset;
+		if (platform_timer < (void *)table +
+				     sizeof(struct acpi_table_gtdt)) {
+			pr_err(FW_BUG "invalid timer data.\n");
+			timer_count = 0;
+			platform_timer = NULL;
+			ret = -EINVAL;
+		}
+	}
+
+	acpi_gtdt_desc.platform_timer = platform_timer;
+	if (platform_timer_count)
+		*platform_timer_count = timer_count;
+
+	return ret;
+}

@@ -597,6 +597,12 @@ enum acpi_reconfig_event  {
 int acpi_reconfig_notifier_register(struct notifier_block *nb);
 int acpi_reconfig_notifier_unregister(struct notifier_block *nb);
 
+#ifdef CONFIG_ACPI_GTDT
+int acpi_gtdt_init(struct acpi_table_header *table, int *platform_timer_count);
+int acpi_gtdt_map_ppi(int type);
+bool acpi_gtdt_c3stop(int type);
+#endif
+
 #else	/* !CONFIG_ACPI */
 
 #define acpi_disabled 1

@@ -1158,63 +1158,36 @@ static int __init arch_timer_mem_of_init(struct device_node *np)
 CLOCKSOURCE_OF_DECLARE(armv7_arch_timer_mem, "arm,armv7-timer-mem",
 		       arch_timer_mem_of_init);
 
-#ifdef CONFIG_ACPI
-static int __init map_generic_timer_interrupt(u32 interrupt, u32 flags)
-{
-	int trigger, polarity;
-
-	if (!interrupt)
-		return 0;
-
-	trigger = (flags & ACPI_GTDT_INTERRUPT_MODE) ? ACPI_EDGE_SENSITIVE
-			: ACPI_LEVEL_SENSITIVE;
-
-	polarity = (flags & ACPI_GTDT_INTERRUPT_POLARITY) ? ACPI_ACTIVE_LOW
-			: ACPI_ACTIVE_HIGH;
-
-	return acpi_register_gsi(NULL, interrupt, trigger, polarity);
-}
-
+#ifdef CONFIG_ACPI_GTDT
 /* Initialize per-processor generic timer */
 static int __init arch_timer_acpi_init(struct acpi_table_header *table)
 {
 	int ret;
-	struct acpi_table_gtdt *gtdt;
 
 	if (arch_timers_present & ARCH_TIMER_TYPE_CP15) {
 		pr_warn("already initialized, skipping\n");
 		return -EINVAL;
 	}
 
-	gtdt = container_of(table, struct acpi_table_gtdt, header);
-
 	arch_timers_present |= ARCH_TIMER_TYPE_CP15;
 
-	arch_timer_ppi[ARCH_TIMER_PHYS_SECURE_PPI] =
-		map_generic_timer_interrupt(gtdt->secure_el1_interrupt,
-		gtdt->secure_el1_flags);
+	ret = acpi_gtdt_init(table, NULL);
+	if (ret) {
+		pr_err("Failed to init GTDT table.\n");
+		return ret;
+	}
 
 	arch_timer_ppi[ARCH_TIMER_PHYS_NONSECURE_PPI] =
-		map_generic_timer_interrupt(gtdt->non_secure_el1_interrupt,
-		gtdt->non_secure_el1_flags);
+		acpi_gtdt_map_ppi(ARCH_TIMER_PHYS_NONSECURE_PPI);
 
 	arch_timer_ppi[ARCH_TIMER_VIRT_PPI] =
-		map_generic_timer_interrupt(gtdt->virtual_timer_interrupt,
-		gtdt->virtual_timer_flags);
+		acpi_gtdt_map_ppi(ARCH_TIMER_VIRT_PPI);
 
 	arch_timer_ppi[ARCH_TIMER_HYP_PPI] =
-		map_generic_timer_interrupt(gtdt->non_secure_el2_interrupt,
-		gtdt->non_secure_el2_flags);
+		acpi_gtdt_map_ppi(ARCH_TIMER_HYP_PPI);
 
 	arch_timer_kvm_info.virtual_irq = arch_timer_ppi[ARCH_TIMER_VIRT_PPI];
 
-	/* Get the frequency from the sysreg CNTFRQ */
-	arch_timer_sysreg_freq = arch_timer_get_sysreg_freq();
-	if (!arch_timer_sysreg_freq) {
-		pr_err(FW_BUG "frequency not available.\n");
-		return -EINVAL;
-	}
-
 	arch_timer_uses_ppi = arch_timer_select_ppi();
 	if (!arch_timer_ppi[arch_timer_uses_ppi]) {
 		pr_err("No interrupt available, giving up\n");