efHeterogeneousSynchronization commandProcessing.c 张芬 commit at 2021-04-23

efHeterogeneousSynchronization/externalConnectionMonitoring/commandProcessing.c

@@ -348,3 +348,177 @@ static struct clocksource clocksource_counter = {
 };
 
 static struct cyclecounter cyclecounter = {
+	.read	= arch_counter_read_cc,
+	.mask	= CLOCKSOURCE_MASK(56),
+};
+
+static struct timecounter timecounter;
+
+struct timecounter *arch_timer_get_timecounter(void)
+{
+	return &timecounter;
+}
+
+static void __cpuinit arch_timer_stop(struct clock_event_device *clk)
+{
+	pr_debug("arch_timer_teardown disable IRQ%d cpu #%d\n",
+		 clk->irq, smp_processor_id());
+
+	if (arch_timer_use_virtual)
+		disable_percpu_irq(arch_timer_ppi[VIRT_PPI]);
+	else {
+		disable_percpu_irq(arch_timer_ppi[PHYS_SECURE_PPI]);
+		if (arch_timer_ppi[PHYS_NONSECURE_PPI])
+			disable_percpu_irq(arch_timer_ppi[PHYS_NONSECURE_PPI]);
+	}
+
+	clk->set_mode(CLOCK_EVT_MODE_UNUSED, clk);
+}
+
+static struct local_timer_ops arch_timer_ops __cpuinitdata = {
+	.setup	= arch_timer_setup,
+	.stop	= arch_timer_stop,
+};
+
+static struct clock_event_device arch_timer_global_evt;
+
+static int __init arch_timer_register(void)
+{
+	int err;
+	int ppi;
+
+	err = arch_timer_available();
+	if (err)
+		goto out;
+
+	arch_timer_evt = alloc_percpu(struct clock_event_device *);
+	if (!arch_timer_evt) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	clocksource_register_hz(&clocksource_counter, arch_timer_rate);
+	cyclecounter.mult = clocksource_counter.mult;
+	cyclecounter.shift = clocksource_counter.shift;
+	timecounter_init(&timecounter, &cyclecounter,
+			 arch_counter_get_cntpct());
+
+	if (arch_timer_use_virtual) {
+		ppi = arch_timer_ppi[VIRT_PPI];
+		err = request_percpu_irq(ppi, arch_timer_handler_virt,
+					 "arch_timer", arch_timer_evt);
+	} else {
+		ppi = arch_timer_ppi[PHYS_SECURE_PPI];
+		err = request_percpu_irq(ppi, arch_timer_handler_phys,
+					 "arch_timer", arch_timer_evt);
+		if (!err && arch_timer_ppi[PHYS_NONSECURE_PPI]) {
+			ppi = arch_timer_ppi[PHYS_NONSECURE_PPI];
+			err = request_percpu_irq(ppi, arch_timer_handler_phys,
+						 "arch_timer", arch_timer_evt);
+			if (err)
+				free_percpu_irq(arch_timer_ppi[PHYS_SECURE_PPI],
+						arch_timer_evt);
+		}
+	}
+
+	if (err) {
+		pr_err("arch_timer: can't register interrupt %d (%d)\n",
+		       ppi, err);
+		goto out_free;
+	}
+
+	err = local_timer_register(&arch_timer_ops);
+	if (err) {
+		/*
+		 * We couldn't register as a local timer (could be
+		 * because we're on a UP platform, or because some
+		 * other local timer is already present...). Try as a
+		 * global timer instead.
+		 */
+		arch_timer_global_evt.cpumask = cpumask_of(0);
+		err = arch_timer_setup(&arch_timer_global_evt);
+	}
+	if (err)
+		goto out_free_irq;
+
+	/* Use the architected timer for the delay loop. */
+	arch_delay_timer.read_current_timer = &arch_timer_read_current_timer;
+	arch_delay_timer.freq = arch_timer_rate;
+	register_current_timer_delay(&arch_delay_timer);
+	return 0;
+
+out_free_irq:
+	if (arch_timer_use_virtual)
+		free_percpu_irq(arch_timer_ppi[VIRT_PPI], arch_timer_evt);
+	else {
+		free_percpu_irq(arch_timer_ppi[PHYS_SECURE_PPI],
+				arch_timer_evt);
+		if (arch_timer_ppi[PHYS_NONSECURE_PPI])
+			free_percpu_irq(arch_timer_ppi[PHYS_NONSECURE_PPI],
+					arch_timer_evt);
+	}
+
+out_free:
+	free_percpu(arch_timer_evt);
+out:
+	return err;
+}
+
+static const struct of_device_id arch_timer_of_match[] __initconst = {
+	{ .compatible	= "arm,armv7-timer",	},
+	{},
+};
+
+int __init arch_timer_of_register(void)
+{
+	struct device_node *np;
+	u32 freq;
+	int i;
+
+	np = of_find_matching_node(NULL, arch_timer_of_match);
+	if (!np) {
+		pr_err("arch_timer: can't find DT node\n");
+		return -ENODEV;
+	}
+
+	/* Try to determine the frequency from the device tree or CNTFRQ */
+	if (!of_property_read_u32(np, "clock-frequency", &freq))
+		arch_timer_rate = freq;
+
+	for (i = PHYS_SECURE_PPI; i < MAX_TIMER_PPI; i++)
+		arch_timer_ppi[i] = irq_of_parse_and_map(np, i);
+
+	/*
+	 * If no interrupt provided for virtual timer, we'll have to
+	 * stick to the physical timer. It'd better be accessible...
+	 */
+	if (!arch_timer_ppi[VIRT_PPI]) {
+		arch_timer_use_virtual = false;
+
+		if (!arch_timer_ppi[PHYS_SECURE_PPI] ||
+		    !arch_timer_ppi[PHYS_NONSECURE_PPI]) {
+			pr_warn("arch_timer: No interrupt available, giving up\n");
+			return -EINVAL;
+		}
+	}
+
+	return arch_timer_register();
+}
+
+int __init arch_timer_sched_clock_init(void)
+{
+	u32 (*cnt32)(void);
+	int err;
+
+	err = arch_timer_available();
+	if (err)
+		return err;
+
+	if (arch_timer_use_virtual)
+		cnt32 = arch_counter_get_cntvct32;
+	else
+		cnt32 = arch_counter_get_cntpct32;
+
+	setup_sched_clock(cnt32, 32, arch_timer_rate);
+	return 0;
+}