int __cpuinit zynq_cpun_start(u32 address, int cpu)
{
u32 trampoline_code_size = &zynq_secondary_trampoline_end -
&zynq_secondary_trampoline;
if (cpu > ncores) {
pr_warn("CPU No. is not available in the system\n");
return -1;
}
/* MS: Expectation that SLCR are directly map and accessible */
/* Not possible to jump to non aligned address */
if (!(address & 3) && (!address || (address >= trampoline_code_size))) {
/* Store pointer to ioremap area which points to address 0x0 */
static u8 __iomem *zero;
u32 trampoline_size = &zynq_secondary_trampoline_jump -
&zynq_secondary_trampoline;
zynq_slcr_cpu_stop(cpu);
if (__pa(PAGE_OFFSET)) {
zero = ioremap(0, trampoline_code_size);
if (!zero) {
pr_warn("BOOTUP jump vectors not accessible\n");
return -1;
}
} else {
zero = (__force u8 __iomem *)PAGE_OFFSET;
}
/*
* This is elegant way how to jump to any address
* 0x0: Load address at 0x8 to r0
* 0x4: Jump by mov instruction
* 0x8: Jumping address
*/
memcpy((__force void *)zero, &zynq_secondary_trampoline,
trampoline_size);
writel(address, zero + trampoline_size);
flush_cache_all();
outer_flush_range(0, trampoline_code_size);
smp_wmb();
if (__pa(PAGE_OFFSET))
iounmap(zero);
zynq_slcr_cpu_start(cpu);
return 0;
}
pr_warn("Can't start CPU%d: Wrong starting address %x\n", cpu, address);
return -1;
}
static int zynq_cpu_kill(unsigned cpu)
{
unsigned long timeout = jiffies + msecs_to_jiffies(50);
while (zynq_slcr_cpu_state_read(cpu))
if (time_after(jiffies, timeout))
return 0;
zynq_slcr_cpu_stop(cpu);
return 1;
}
static int zynq_cpu_kill(unsigned cpu)
{
zynq_slcr_cpu_stop(cpu);
return 1;
}
