ARM: uniphier: rework SMP operations to use trampoline code

obj-y			:= uniphier.o

obj-$(CONFIG_SMP)	+= platsmp.o

obj-$(CONFIG_SMP)	+= platsmp.o headsmp.o

/*

 * Copyright (C) 2015 Masahiro Yamada <yamada.masahiro@socionext.com>

 *

 * This program is free software; you can redistribute it and/or modify

 * it under the terms of the GNU General Public License as published by

 * the Free Software Foundation; either version 2 of the License, or

 * (at your option) any later version.

 *

 * This program is distributed in the hope that it will be useful,

 * but WITHOUT ANY WARRANTY; without even the implied warranty of

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

 * GNU General Public License for more details.

 */

#include <linux/linkage.h>

#include <asm/assembler.h>

#include <asm/cp15.h>

ENTRY(uniphier_smp_trampoline)

ARM_BE8(setend	be)			@ ensure we are in BE8 mode

	mrc	p15, 0, r0, c0, c0, 5	@ MPIDR (Multiprocessor Affinity Reg)

	and  	r2, r0, #0x3		@ CPU ID

	ldr	r1, uniphier_smp_trampoline_jump

	ldr	r3, uniphier_smp_trampoline_poll_addr

	mrc	p15, 0, r0, c1, c0, 0	@ SCTLR (System Control Register)

	orr	r0, r0, #CR_I		@ Enable ICache

	bic	r0, r0, #(CR_C | CR_M)	@ Disable MMU and Dcache

	mcr	p15, 0, r0, c1, c0, 0

	b	1f			@ cache the following 5 instructions

0:	wfe

1:	ldr	r0, [r3]

	cmp	r0, r2

	bxeq	r1			@ branch to secondary_startup

	b	0b

	.globl	uniphier_smp_trampoline_jump

uniphier_smp_trampoline_jump:

	.word	0			@ set virt_to_phys(secondary_startup)

	.globl	uniphier_smp_trampoline_poll_addr

uniphier_smp_trampoline_poll_addr:

	.word	0			@ set CPU ID to be kicked to this reg

	.globl	uniphier_smp_trampoline_end

uniphier_smp_trampoline_end:

ENDPROC(uniphier_smp_trampoline)

 * GNU General Public License for more details.

 */

#include <linux/sizes.h>

#include <linux/compiler.h>

#define pr_fmt(fmt)		"uniphier: " fmt

#include <linux/init.h>

#include <linux/io.h>

#include <linux/regmap.h>

#include <linux/mfd/syscon.h>

#include <linux/ioport.h>

#include <linux/of.h>

#include <linux/of_address.h>

#include <linux/sizes.h>

#include <asm/cacheflush.h>

#include <asm/hardware/cache-uniphier.h>

#include <asm/pgtable.h>

#include <asm/smp.h>

#include <asm/smp_scu.h>

static struct regmap *sbcm_regmap;

/*

 * The secondary CPUs check this register from the boot ROM for the jump

 * destination.  After that, it can be reused as a scratch register.

 */

#define UNIPHIER_SBC_ROM_BOOT_RSV2	0x1208

static void __init uniphier_smp_prepare_cpus(unsigned int max_cpus)

static void __iomem *uniphier_smp_rom_boot_rsv2;

static unsigned int uniphier_smp_max_cpus;

extern char uniphier_smp_trampoline;

extern char uniphier_smp_trampoline_jump;

extern char uniphier_smp_trampoline_poll_addr;

extern char uniphier_smp_trampoline_end;

/*

 * Copy trampoline code to the tail of the 1st section of the page table used

 * in the boot ROM.  This area is directly accessible by the secondary CPUs

 * for all the UniPhier SoCs.

 */

static const phys_addr_t uniphier_smp_trampoline_dest_end = SECTION_SIZE;

static phys_addr_t uniphier_smp_trampoline_dest;

static int __init uniphier_smp_copy_trampoline(phys_addr_t poll_addr)

{

	static cpumask_t only_cpu_0 = { CPU_BITS_CPU0 };

	unsigned long scu_base_phys = 0;

	void __iomem *scu_base;

	size_t trmp_size;

	static void __iomem *trmp_base;

	sbcm_regmap = syscon_regmap_lookup_by_compatible(

			"socionext,uniphier-system-bus-controller-misc");

	if (IS_ERR(sbcm_regmap)) {

		pr_err("failed to regmap system-bus-controller-misc\n");

		goto err;

	if (!uniphier_cache_l2_is_enabled()) {

		pr_warn("outer cache is needed for SMP, but not enabled\n");

		return -ENODEV;

	}

	uniphier_cache_l2_set_locked_ways(1);

	outer_flush_all();

	trmp_size = &uniphier_smp_trampoline_end - &uniphier_smp_trampoline;

	uniphier_smp_trampoline_dest = uniphier_smp_trampoline_dest_end -

								trmp_size;

	uniphier_cache_l2_touch_range(uniphier_smp_trampoline_dest,

				      uniphier_smp_trampoline_dest_end);

	trmp_base = ioremap_cache(uniphier_smp_trampoline_dest, trmp_size);

	if (!trmp_base) {

		pr_err("failed to map trampoline destination area\n");

		return -ENOMEM;

	}

	memcpy(trmp_base, &uniphier_smp_trampoline, trmp_size);

	writel(virt_to_phys(secondary_startup),

	       trmp_base + (&uniphier_smp_trampoline_jump -

			    &uniphier_smp_trampoline));

	writel(poll_addr, trmp_base + (&uniphier_smp_trampoline_poll_addr -

				       &uniphier_smp_trampoline));

	flush_cache_all();	/* flush out trampoline code to outer cache */

	iounmap(trmp_base);

	return 0;

}

static int __init uniphier_smp_prepare_trampoline(unsigned int max_cpus)

{

	struct device_node *np;

	struct resource res;

	phys_addr_t rom_rsv2_phys;

	int ret;

	np = of_find_compatible_node(NULL, NULL,

				"socionext,uniphier-system-bus-controller");

	ret = of_address_to_resource(np, 1, &res);

	if (ret) {

		pr_err("failed to get resource of system-bus-controller\n");

		return ret;

	}

	rom_rsv2_phys = res.start + UNIPHIER_SBC_ROM_BOOT_RSV2;

	ret = uniphier_smp_copy_trampoline(rom_rsv2_phys);

	if (ret)

		return ret;

	uniphier_smp_rom_boot_rsv2 = ioremap(rom_rsv2_phys, sizeof(SZ_4));

	if (!uniphier_smp_rom_boot_rsv2) {

		pr_err("failed to map ROM_BOOT_RSV2 register\n");

		return -ENOMEM;

	}

	writel(uniphier_smp_trampoline_dest, uniphier_smp_rom_boot_rsv2);

	asm("sev"); /* Bring up all secondary CPUs to the trampoline code */

	uniphier_smp_max_cpus = max_cpus;	/* save for later use */

	return 0;

}

static void __init uniphier_smp_unprepare_trampoline(void)

{

	iounmap(uniphier_smp_rom_boot_rsv2);

	if (uniphier_smp_trampoline_dest)

		outer_inv_range(uniphier_smp_trampoline_dest,

				uniphier_smp_trampoline_dest_end);

	uniphier_cache_l2_set_locked_ways(0);

}

static int __init uniphier_smp_enable_scu(void)

{

	unsigned long scu_base_phys = 0;

	void __iomem *scu_base;

	if (scu_a9_has_base())

		scu_base_phys = scu_a9_get_base();

	if (!scu_base_phys) {

		pr_err("failed to get scu base\n");

		goto err;

		return -ENODEV;

	}

	scu_base = ioremap(scu_base_phys, SZ_128);

	if (!scu_base) {

		pr_err("failed to remap scu base (0x%08lx)\n", scu_base_phys);

		goto err;

		pr_err("failed to map scu base\n");

		return -ENOMEM;

	}

	scu_enable(scu_base);

	iounmap(scu_base);

	return 0;

}

static void __init uniphier_smp_prepare_cpus(unsigned int max_cpus)

{

	static cpumask_t only_cpu_0 = { CPU_BITS_CPU0 };

	int ret;

	ret = uniphier_smp_prepare_trampoline(max_cpus);

	if (ret)

		goto err;

	ret = uniphier_smp_enable_scu();

	if (ret)

		goto err;

	return;

err:

	pr_warn("disabling SMP\n");

	init_cpu_present(&only_cpu_0);

	sbcm_regmap = NULL;

	uniphier_smp_unprepare_trampoline();

}

static int uniphier_boot_secondary(unsigned int cpu,

static int __init uniphier_smp_boot_secondary(unsigned int cpu,

					      struct task_struct *idle)

{

	int ret;

	if (WARN_ON_ONCE(!uniphier_smp_rom_boot_rsv2))

		return -EFAULT;

	if (!sbcm_regmap)

		return -ENODEV;

	writel(cpu, uniphier_smp_rom_boot_rsv2);

	readl(uniphier_smp_rom_boot_rsv2); /* relax */

	ret = regmap_write(sbcm_regmap, 0x1208,

			   virt_to_phys(secondary_startup));

	if (!ret)

		asm("sev"); /* wake up secondary CPU */

	asm("sev"); /* wake up secondary CPUs sleeping in the trampoline */

	return ret;

	if (cpu == uniphier_smp_max_cpus - 1) {

		/* clean up resources if this is the last CPU */

		uniphier_smp_unprepare_trampoline();

	}

	return 0;

}

struct smp_operations uniphier_smp_ops __initdata = {

static struct smp_operations uniphier_smp_ops __initdata = {

	.smp_prepare_cpus	= uniphier_smp_prepare_cpus,

	.smp_boot_secondary	= uniphier_boot_secondary,

	.smp_boot_secondary	= uniphier_smp_boot_secondary,

};

CPU_METHOD_OF_DECLARE(uniphier_smp, "socionext,uniphier-smp",

		      &uniphier_smp_ops);