Add linaro directory which contains the bootwrapper

Signed-off-by: John Rigby <john.rigby@linaro.org>

1 files changed, 269 insertions, 0 deletions

diff --git a/linaro/arm-virt-bl/bootwrapper/c_start.c b/linaro/arm-virt-bl/bootwrapper/c_start.c
new file mode 100644
index 0000000..fd25394
--- /dev/null
+++ b/linaro/arm-virt-bl/bootwrapper/c_start.c
@@ -0,0 +1,269 @@
+/*
+ * Copyright (c) 2012, ARM Limited. All rights reserved.
+ *       
+ * Redistribution and use in source and binary forms, with
+ * or without modification, are permitted provided that the
+ * following conditions are met:
+ *     
+ * Redistributions of source code must retain the above
+ * copyright notice, this list of conditions and the 
+ * following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the
+ * above copyright notice, this list of conditions and 
+ * the following disclaimer in the documentation 
+ * and/or other materials provided with the distribution.
+ *      
+ * Neither the name of ARM nor the names of its
+ * contributors may be used to endorse or promote products
+ * derived from this software without specific prior written
+ * permission.                        
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include "helpers.h"
+#include "bootwrapper.h"
+#include "vgic.h"
+
+extern void *vector_table, *__entry_bl_sec;
+extern void __entry_bl(unsigned, unsigned, unsigned, unsigned);
+extern void kernel_start(int, int, int, int);
+
+/* Reserve some stack space - STACK_SIZE words per CPU */
+unsigned stack[NUM_CPUS * STACK_SIZE];
+unsigned stack_size = STACK_SIZE * sizeof(unsigned);
+unsigned char cpus_ready[NUM_CLUSTERS][NUM_CPUS];
+volatile unsigned model_pen = 0;
+
+unsigned gic_int_num(void)
+{
+	unsigned intcount = 0;
+
+	intcount = read32(GIC_ID_PHY_BASE + GICD_CTR);
+	intcount = ((intcount & 0x1F) + 1) * 32;
+
+	return intcount;
+}
+
+/*
+ * Function to configure the GIC ready for use in Non-Secure State
+ */
+void setup_gic_nonsecure(unsigned cluster_id, unsigned cpu_id)
+{
+	unsigned ctr = 0, num_ints = gic_int_num();
+
+	/* Ensure all GIC interrupts are Non-Secure */
+	write32(GIC_ID_PHY_BASE + GICD_SEC + (ctr << 2), 0xffffffff);	/* IRQs  0-31 are Non-Secure */
+	if (cpu_id == 0 && cluster_id == 0) {
+		for (ctr = 1; ctr <= (num_ints >> 5); ctr++)
+			write32(GIC_ID_PHY_BASE + GICD_SEC + (ctr << 2), 0xffffffff);	/* Set all SPIs as non-secure */
+	}
+
+	/* Ensure all interrupts can get through the priority mask */
+	write32(GIC_IC_PHY_BASE + GICC_PRIMASK, 0xff);
+}
+
+/*
+ * Function to send wakeup IPI to the secondary CPUs
+ */
+void kick(unsigned cpu_id, int secondary_cpus)
+{
+	int cpu_mask = ((1 << (secondary_cpus + 1)) - 1) & ~(1 << cpu_id);
+
+	write32(VE_SYS_BASE + FLAGS_CLR, 0xffffffff);	// clear the flags register
+	write32(VE_SYS_BASE + FLAGS_SET, (unsigned)start);	// set the start address in the flags register
+	write32(GIC_ID_PHY_BASE, 0x1);	// turn on the GIC distributor
+	write32(GIC_IC_PHY_BASE, 0x1);	// turn on the GIC CPU interface
+	write32(GIC_ID_PHY_BASE + GICD_SW, cpu_mask << 16);	// send an interrupt to everyone else
+}
+
+/*
+ * This function doesn't retun - it waits for an address to be 
+ * written into the FLAGs register, then jumps to that address.
+ */
+void secondary_main(unsigned cluster_id, unsigned cpu_id)
+{
+	unsigned val;
+	void (*secondary_start) (void);
+
+	/* Ensure I cache is on and interrupts are masked */
+	inv_icache_all();
+	write_sctlr(read_sctlr() | (1 << 12));
+	write_cpsr(read_cpsr() | 0x80);
+
+	/* tell CPU0 that we are ready */
+	cpus_ready[cluster_id][cpu_id] = 1;
+
+	/*
+	 * We're not the primary core, so we need to wait for the primary
+	 * core to tell us what to do. While doing that, go into WFI so we
+	 * don't just sit here consuming system resources (i.e. bus
+	 * badwidth); make sure a soft IRQ gets through to the core, but
+	 * don't actually take the interrupt - that way we'll come out of
+	 * WFI without worrying about interrupt vectors (which may have gone
+	 * away, since the primary core is playing with our memory).
+	 */
+	write32(GIC_IC_PHY_BASE + GICC_CTL, 0x1);	/* Enable GIC CPU Interface */
+	write32(GIC_IC_PHY_BASE + GICC_PRIMASK, 0x000000f0);	/* Set Priority Mask to allow interrupts */
+
+	/* If the start address isn't already set, go to sleep */
+	while (val = read32(VE_SYS_BASE + FLAGS_SET), val == 0
+	       || val == (unsigned)start) {
+		wfi();
+		/* Acknowledge the interrupt that woke us */
+		/* Read the Acknowledge register, write End Of Interrupt */
+		write32(GIC_IC_PHY_BASE + GICC_EOI,
+			read32(GIC_IC_PHY_BASE + GICC_INTACK));
+	}
+
+	/* TODO: If MMU is enabled, then synchronise caches here */
+	secondary_start = (void (*)())val;
+	secondary_start();	/* No return from here */
+}
+
+void wait_for_secondaries(unsigned active_clusters, unsigned secondary_cpus)
+{
+	int i, j, ready;
+
+	printf("Waiting for %d secondary CPUs\n", secondary_cpus);
+
+	while (TRUE) {
+		ready = 0;
+
+		for (i = 0; i < active_clusters; i++) {
+			for (j = 0; j < NUM_CPUS; j++) {
+				if (cpus_ready[i][j]) {
+					++ready;
+				}
+			}
+		}
+
+		if (ready == secondary_cpus) {
+			break;
+		}
+
+		/* Don't thrash the memory system, give the secondaries some time */
+		for (j = 0; j < 1000; ++j) {
+#if !BUILD_RVCT
+#define __nop() asm("nop")
+#endif
+
+			__nop();
+		}
+	}
+}
+
+/*
+ * Function to determine number of clusters in the system.
+ * The way to do this will differ on different systems.
+ * Add support for the system you are running on.
+ * At the moment, it supports Kingfisher dual cluster.
+ */
+int get_cluster_count(void)
+{
+	unsigned int kfs_id, active_clusters;
+	int num_clusters = 0;
+
+	kfs_id = read32(VE_KFSCB_BASE + KFS_ID_OFFSET);
+
+	switch (((kfs_id & KFS_ID_ARCH_MASK) >> KFS_ID_ARCH_SHIFT)) {
+	case 0:
+	case 1:
+	case 2:
+	case 3:
+		active_clusters =
+		    read32(VE_KFSCB_BASE +
+			   KFS_CFG_R_OFFSET) & ACTIVE_CLUSTER_MASK;
+		num_clusters = (active_clusters == 0x3) ? 2 : 1;
+		break;
+	}
+
+	return num_clusters;
+}
+
+void c_start(void)
+{
+	unsigned cpu_id = read_cpuid();
+	unsigned cluster_id = read_clusterid();
+	unsigned secondary_cpus = 0;
+	unsigned platform = VERSATILE_EXPRESS;
+	unsigned active_clusters = get_cluster_count();
+
+	secondary_cpus = CLUSTER_CPU_COUNT(cluster_id) - 1;
+	if (active_clusters > 1) {
+		secondary_cpus += CLUSTER_CPU_COUNT(!cluster_id);
+	}
+
+	write_vbar((unsigned)&vector_table);
+	write_mvbar((unsigned)&__entry_bl_sec);
+	if (cpu_id == 0 && cluster_id == 0)
+		config_uart();
+
+	enable_user_perfmon_access();
+	enable_perfmon();
+	enable_swp();
+	write_cpacr(read_cpacr() | 0xfffffff);
+	write_nsacr(0x00073fff);
+	enable_coherency();
+
+	/* Also grant NS access to CCI registers */
+	if (cpu_id == 0 && cluster_id == 0)
+		write32(CCI_BASE + SECURE_ACCESS_REG, 0x1);
+
+	/*
+	 * Secondaries wait here while initialisation of global peripherals is done
+	 */
+	if (model_pen == 0 && (cpu_id || cluster_id)) {
+		do {
+			wfe();
+		} while (model_pen == 0);
+	}
+
+	setup_gic_nonsecure(cluster_id, cpu_id);
+
+	if (cpu_id == 0 && cluster_id == 0) {
+		model_pen = 1;
+		dsb();
+		sev();
+	}
+
+	enter_monitor_mode();
+	write_scr(0x131);	/* HVC, NS, FW and AW bits  */
+	write_cnthctl(PL1PCTEN | PL1PCEN);
+	write_cntkctl(PL0PCTEN | PL0VCTEN | PL0VTEN | PL0PTEN);
+	write_cntfrq(CP15_TIMER_FREQ);
+
+	/* Start secondary CPUs, if any */
+	if (cpu_id == 0 && cluster_id == 0 && secondary_cpus > 0) {
+		printf("Kicking %d secondary CPU(s)\n", secondary_cpus);
+		drain_uart_fifo();
+		kick(cpu_id, secondary_cpus);
+	}
+
+	enter_nonsecure_world((unsigned)__entry_bl);
+
+	/* Secondary CPUs go off to secondary_main() */
+	if (cpu_id || cluster_id) {
+		secondary_main(cluster_id, cpu_id);	/* no return */
+	}
+
+	/* Primary waits for the secondaries to get ready before loading the payload */
+	wait_for_secondaries(active_clusters, secondary_cpus);
+
+	/* Load the payload kernel */
+	printf("Kernel entry point 0x%x (%s)\n", kernel_start,
+	       ((unsigned)kernel_start & 1) ? "thumb" : "arm");
+
+	drain_uart_fifo();
+
+	/* Clear FLAGS register, as this is what Linux expects to find */
+	write32(VE_SYS_BASE + FLAGS_CLR, 0xffffffff);
+
+	/* TODO: If MMU is enabled then caches need to be cleaned here */
+
+	/* Start the kernel */
+	kernel_start(0, platform & PLATFORM_MASK, 0, 0);	/* No return from here */
+
+	return;
+}