[mirror_ubuntu-eoan-kernel.git] / arch / mips / kernel / smp-cps.c

/*
 * Copyright (C) 2013 Imagination Technologies
 * Author: Paul Burton <paul.burton@imgtec.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.
 */

#include <linux/io.h>
#include <linux/sched.h>
#include <linux/slab.h>
#include <linux/smp.h>
#include <linux/types.h>

#include <asm/cacheflush.h>
#include <asm/gic.h>
#include <asm/mips-cm.h>
#include <asm/mips-cpc.h>
#include <asm/mips_mt.h>
#include <asm/mipsregs.h>
#include <asm/pm-cps.h>
#include <asm/smp-cps.h>
#include <asm/time.h>
#include <asm/uasm.h>

static DECLARE_BITMAP(core_power, NR_CPUS);

struct core_boot_config *mips_cps_core_bootcfg;

static unsigned core_vpe_count(unsigned core)
{
	unsigned cfg;

	if (!config_enabled(CONFIG_MIPS_MT_SMP) || !cpu_has_mipsmt)
		return 1;

	write_gcr_cl_other(core << CM_GCR_Cx_OTHER_CORENUM_SHF);
	cfg = read_gcr_co_config() & CM_GCR_Cx_CONFIG_PVPE_MSK;
	return (cfg >> CM_GCR_Cx_CONFIG_PVPE_SHF) + 1;
}

static void __init cps_smp_setup(void)
{
	unsigned int ncores, nvpes, core_vpes;
	int c, v;

	/* Detect & record VPE topology */
	ncores = mips_cm_numcores();
	pr_info("VPE topology ");
	for (c = nvpes = 0; c < ncores; c++) {
		core_vpes = core_vpe_count(c);
		pr_cont("%c%u", c ? ',' : '{', core_vpes);

		/* Use the number of VPEs in core 0 for smp_num_siblings */
		if (!c)
			smp_num_siblings = core_vpes;

		for (v = 0; v < min_t(int, core_vpes, NR_CPUS - nvpes); v++) {
			cpu_data[nvpes + v].core = c;
#ifdef CONFIG_MIPS_MT_SMP
			cpu_data[nvpes + v].vpe_id = v;
#endif
		}

		nvpes += core_vpes;
	}
	pr_cont("} total %u\n", nvpes);

	/* Indicate present CPUs (CPU being synonymous with VPE) */
	for (v = 0; v < min_t(unsigned, nvpes, NR_CPUS); v++) {
		set_cpu_possible(v, true);
		set_cpu_present(v, true);
		__cpu_number_map[v] = v;
		__cpu_logical_map[v] = v;
	}

	/* Core 0 is powered up (we're running on it) */
	bitmap_set(core_power, 0, 1);

	/* Initialise core 0 */
	mips_cps_core_init();

	/* Make core 0 coherent with everything */
	write_gcr_cl_coherence(0xff);
}

static void __init cps_prepare_cpus(unsigned int max_cpus)
{
	unsigned ncores, core_vpes, c;
	u32 *entry_code;

	mips_mt_set_cpuoptions();

	/* Patch the start of mips_cps_core_entry to provide the CM base */
	entry_code = (u32 *)&mips_cps_core_entry;
	UASM_i_LA(&entry_code, 3, (long)mips_cm_base);
	dma_cache_wback_inv((unsigned long)&mips_cps_core_entry,
			    (void *)entry_code - (void *)&mips_cps_core_entry);

	/* Allocate core boot configuration structs */
	ncores = mips_cm_numcores();
	mips_cps_core_bootcfg = kcalloc(ncores, sizeof(*mips_cps_core_bootcfg),
					GFP_KERNEL);
	if (!mips_cps_core_bootcfg) {
		pr_err("Failed to allocate boot config for %u cores\n", ncores);
		goto err_out;
	}

	/* Allocate VPE boot configuration structs */
	for (c = 0; c < ncores; c++) {
		core_vpes = core_vpe_count(c);
		mips_cps_core_bootcfg[c].vpe_config = kcalloc(core_vpes,
				sizeof(*mips_cps_core_bootcfg[c].vpe_config),
				GFP_KERNEL);
		if (!mips_cps_core_bootcfg[c].vpe_config) {
			pr_err("Failed to allocate %u VPE boot configs\n",
			       core_vpes);
			goto err_out;
		}
	}

	/* Mark this CPU as booted */
	atomic_set(&mips_cps_core_bootcfg[current_cpu_data.core].vpe_mask,
		   1 << cpu_vpe_id(&current_cpu_data));

	return;
err_out:
	/* Clean up allocations */
	if (mips_cps_core_bootcfg) {
		for (c = 0; c < ncores; c++)
			kfree(mips_cps_core_bootcfg[c].vpe_config);
		kfree(mips_cps_core_bootcfg);
		mips_cps_core_bootcfg = NULL;
	}

	/* Effectively disable SMP by declaring CPUs not present */
	for_each_possible_cpu(c) {
		if (c == 0)
			continue;
		set_cpu_present(c, false);
	}
}

static void boot_core(unsigned core)
{
	u32 access;

	/* Select the appropriate core */
	write_gcr_cl_other(core << CM_GCR_Cx_OTHER_CORENUM_SHF);

	/* Set its reset vector */
	write_gcr_co_reset_base(CKSEG1ADDR((unsigned long)mips_cps_core_entry));

	/* Ensure its coherency is disabled */
	write_gcr_co_coherence(0);

	/* Ensure the core can access the GCRs */
	access = read_gcr_access();
	access |= 1 << (CM_GCR_ACCESS_ACCESSEN_SHF + core);
	write_gcr_access(access);

	if (mips_cpc_present()) {
		/* Reset the core */
		mips_cpc_lock_other(core);
		write_cpc_co_cmd(CPC_Cx_CMD_RESET);
		mips_cpc_unlock_other();
	} else {
		/* Take the core out of reset */
		write_gcr_co_reset_release(0);
	}

	/* The core is now powered up */
	bitmap_set(core_power, core, 1);
}

static void remote_vpe_boot(void *dummy)
{
	mips_cps_boot_vpes();
}

static void cps_boot_secondary(int cpu, struct task_struct *idle)
{
	unsigned core = cpu_data[cpu].core;
	unsigned vpe_id = cpu_vpe_id(&cpu_data[cpu]);
	struct core_boot_config *core_cfg = &mips_cps_core_bootcfg[core];
	struct vpe_boot_config *vpe_cfg = &core_cfg->vpe_config[vpe_id];
	unsigned int remote;
	int err;

	vpe_cfg->pc = (unsigned long)&smp_bootstrap;
	vpe_cfg->sp = __KSTK_TOS(idle);
	vpe_cfg->gp = (unsigned long)task_thread_info(idle);

	atomic_or(1 << cpu_vpe_id(&cpu_data[cpu]), &core_cfg->vpe_mask);

	preempt_disable();

	if (!test_bit(core, core_power)) {
		/* Boot a VPE on a powered down core */
		boot_core(core);
		goto out;
	}

	if (core != current_cpu_data.core) {
		/* Boot a VPE on another powered up core */
		for (remote = 0; remote < NR_CPUS; remote++) {
			if (cpu_data[remote].core != core)
				continue;
			if (cpu_online(remote))
				break;
		}
		BUG_ON(remote >= NR_CPUS);

		err = smp_call_function_single(remote, remote_vpe_boot,
					       NULL, 1);
		if (err)
			panic("Failed to call remote CPU\n");
		goto out;
	}

	BUG_ON(!cpu_has_mipsmt);

	/* Boot a VPE on this core */
	mips_cps_boot_vpes();
out:
	preempt_enable();
}

static void cps_init_secondary(void)
{
	/* Disable MT - we only want to run 1 TC per VPE */
	if (cpu_has_mipsmt)
		dmt();

	change_c0_status(ST0_IM, STATUSF_IP3 | STATUSF_IP4 |
				 STATUSF_IP6 | STATUSF_IP7);
}

static void cps_smp_finish(void)
{
	write_c0_compare(read_c0_count() + (8 * mips_hpt_frequency / HZ));

#ifdef CONFIG_MIPS_MT_FPAFF
	/* If we have an FPU, enroll ourselves in the FPU-full mask */
	if (cpu_has_fpu)
		cpu_set(smp_processor_id(), mt_fpu_cpumask);
#endif /* CONFIG_MIPS_MT_FPAFF */

	local_irq_enable();
}

static void cps_cpus_done(void)
{
}

#ifdef CONFIG_HOTPLUG_CPU

static int cps_cpu_disable(void)
{
	unsigned cpu = smp_processor_id();
	struct core_boot_config *core_cfg;

	if (!cpu)
		return -EBUSY;

	if (!cps_pm_support_state(CPS_PM_POWER_GATED))
		return -EINVAL;

	core_cfg = &mips_cps_core_bootcfg[current_cpu_data.core];
	atomic_sub(1 << cpu_vpe_id(&current_cpu_data), &core_cfg->vpe_mask);
	smp_mb__after_atomic_dec();
	set_cpu_online(cpu, false);
	cpu_clear(cpu, cpu_callin_map);

	return 0;
}

static DECLARE_COMPLETION(cpu_death_chosen);
static unsigned cpu_death_sibling;
static enum {
	CPU_DEATH_HALT,
	CPU_DEATH_POWER,
} cpu_death;

void play_dead(void)
{
	unsigned cpu, core;

	local_irq_disable();
	idle_task_exit();
	cpu = smp_processor_id();
	cpu_death = CPU_DEATH_POWER;

	if (cpu_has_mipsmt) {
		core = cpu_data[cpu].core;

		/* Look for another online VPE within the core */
		for_each_online_cpu(cpu_death_sibling) {
			if (cpu_data[cpu_death_sibling].core != core)
				continue;

			/*
			 * There is an online VPE within the core. Just halt
			 * this TC and leave the core alone.
			 */
			cpu_death = CPU_DEATH_HALT;
			break;
		}
	}

	/* This CPU has chosen its way out */
	complete(&cpu_death_chosen);

	if (cpu_death == CPU_DEATH_HALT) {
		/* Halt this TC */
		write_c0_tchalt(TCHALT_H);
		instruction_hazard();
	} else {
		/* Power down the core */
		cps_pm_enter_state(CPS_PM_POWER_GATED);
	}

	/* This should never be reached */
	panic("Failed to offline CPU %u", cpu);
}

static void wait_for_sibling_halt(void *ptr_cpu)
{
	unsigned cpu = (unsigned)ptr_cpu;
	unsigned vpe_id = cpu_data[cpu].vpe_id;
	unsigned halted;
	unsigned long flags;

	do {
		local_irq_save(flags);
		settc(vpe_id);
		halted = read_tc_c0_tchalt();
		local_irq_restore(flags);
	} while (!(halted & TCHALT_H));
}

static void cps_cpu_die(unsigned int cpu)
{
	unsigned core = cpu_data[cpu].core;
	unsigned stat;
	int err;

	/* Wait for the cpu to choose its way out */
	if (!wait_for_completion_timeout(&cpu_death_chosen,
					 msecs_to_jiffies(5000))) {
		pr_err("CPU%u: didn't offline\n", cpu);
		return;
	}

	/*
	 * Now wait for the CPU to actually offline. Without doing this that
	 * offlining may race with one or more of:
	 *
	 *   - Onlining the CPU again.
	 *   - Powering down the core if another VPE within it is offlined.
	 *   - A sibling VPE entering a non-coherent state.
	 *
	 * In the non-MT halt case (ie. infinite loop) the CPU is doing nothing
	 * with which we could race, so do nothing.
	 */
	if (cpu_death == CPU_DEATH_POWER) {
		/*
		 * Wait for the core to enter a powered down or clock gated
		 * state, the latter happening when a JTAG probe is connected
		 * in which case the CPC will refuse to power down the core.
		 */
		do {
			mips_cpc_lock_other(core);
			stat = read_cpc_co_stat_conf();
			stat &= CPC_Cx_STAT_CONF_SEQSTATE_MSK;
			mips_cpc_unlock_other();
		} while (stat != CPC_Cx_STAT_CONF_SEQSTATE_D0 &&
			 stat != CPC_Cx_STAT_CONF_SEQSTATE_D2 &&
			 stat != CPC_Cx_STAT_CONF_SEQSTATE_U2);

		/* Indicate the core is powered off */
		bitmap_clear(core_power, core, 1);
	} else if (cpu_has_mipsmt) {
		/*
		 * Have a CPU with access to the offlined CPUs registers wait
		 * for its TC to halt.
		 */
		err = smp_call_function_single(cpu_death_sibling,
					       wait_for_sibling_halt,
					       (void *)cpu, 1);
		if (err)
			panic("Failed to call remote sibling CPU\n");
	}
}

#endif /* CONFIG_HOTPLUG_CPU */

static struct plat_smp_ops cps_smp_ops = {
	.smp_setup		= cps_smp_setup,
	.prepare_cpus		= cps_prepare_cpus,
	.boot_secondary		= cps_boot_secondary,
	.init_secondary		= cps_init_secondary,
	.smp_finish		= cps_smp_finish,
	.send_ipi_single	= gic_send_ipi_single,
	.send_ipi_mask		= gic_send_ipi_mask,
	.cpus_done		= cps_cpus_done,
#ifdef CONFIG_HOTPLUG_CPU
	.cpu_disable		= cps_cpu_disable,
	.cpu_die		= cps_cpu_die,
#endif
};

bool mips_cps_smp_in_use(void)
{
	extern struct plat_smp_ops *mp_ops;
	return mp_ops == &cps_smp_ops;
}

int register_cps_smp_ops(void)
{
	if (!mips_cm_present()) {
		pr_warn("MIPS CPS SMP unable to proceed without a CM\n");
		return -ENODEV;
	}

	/* check we have a GIC - we need one for IPIs */
	if (!(read_gcr_gic_status() & CM_GCR_GIC_STATUS_EX_MSK)) {
		pr_warn("MIPS CPS SMP unable to proceed without a GIC\n");
		return -ENODEV;
	}

	register_smp_ops(&cps_smp_ops);
	return 0;
}
Commit	Line	Data
0ee958e1 PB	1	/*
	2	* Copyright (C) 2013 Imagination Technologies
	3	* Author: Paul Burton <paul.burton@imgtec.com>
	4	*
	5	* This program is free software; you can redistribute it and/or modify it
	6	* under the terms of the GNU General Public License as published by the
	7	* Free Software Foundation; either version 2 of the License, or (at your
	8	* option) any later version.
	9	*/
	10
	11	#include <linux/io.h>
	12	#include <linux/sched.h>
	13	#include <linux/slab.h>
	14	#include <linux/smp.h>
	15	#include <linux/types.h>
	16
	17	#include <asm/cacheflush.h>
	18	#include <asm/gic.h>
	19	#include <asm/mips-cm.h>
	20	#include <asm/mips-cpc.h>
	21	#include <asm/mips_mt.h>
	22	#include <asm/mipsregs.h>
1d8f1f5a	23	#include <asm/pm-cps.h>
0ee958e1 PB	24	#include <asm/smp-cps.h>
	25	#include <asm/time.h>
	26	#include <asm/uasm.h>
	27
	28	static DECLARE_BITMAP(core_power, NR_CPUS);
	29
245a7868	30	struct core_boot_config *mips_cps_core_bootcfg;
0ee958e1	31
245a7868	32	static unsigned core_vpe_count(unsigned core)
0ee958e1	33	{
245a7868	34	unsigned cfg;
0ee958e1	35
245a7868 PB	36	if (!config_enabled(CONFIG_MIPS_MT_SMP) \|\| !cpu_has_mipsmt)
245a7868 PB	37	return 1;
0ee958e1	38
245a7868 PB	39	write_gcr_cl_other(core << CM_GCR_Cx_OTHER_CORENUM_SHF);
	40	cfg = read_gcr_co_config() & CM_GCR_Cx_CONFIG_PVPE_MSK;
	41	return (cfg >> CM_GCR_Cx_CONFIG_PVPE_SHF) + 1;
0ee958e1 PB	42	}
	43
	44	static void __init cps_smp_setup(void)
	45	{
	46	unsigned int ncores, nvpes, core_vpes;
	47	int c, v;
0ee958e1 PB	48
	49	/* Detect & record VPE topology */
	50	ncores = mips_cm_numcores();
	51	pr_info("VPE topology ");
	52	for (c = nvpes = 0; c < ncores; c++) {
245a7868	53	core_vpes = core_vpe_count(c);
0ee958e1 PB	54	pr_cont("%c%u", c ? ',' : '{', core_vpes);
0ee958e1 PB	55
245a7868 PB	56	/* Use the number of VPEs in core 0 for smp_num_siblings */
	57	if (!c)
	58	smp_num_siblings = core_vpes;
	59
0ee958e1 PB	60	for (v = 0; v < min_t(int, core_vpes, NR_CPUS - nvpes); v++) {
	61	cpu_data[nvpes + v].core = c;
	62	#ifdef CONFIG_MIPS_MT_SMP
	63	cpu_data[nvpes + v].vpe_id = v;
	64	#endif
	65	}
	66
	67	nvpes += core_vpes;
	68	}
	69	pr_cont("} total %u\n", nvpes);
	70
	71	/* Indicate present CPUs (CPU being synonymous with VPE) */
	72	for (v = 0; v < min_t(unsigned, nvpes, NR_CPUS); v++) {
	73	set_cpu_possible(v, true);
	74	set_cpu_present(v, true);
	75	__cpu_number_map[v] = v;
	76	__cpu_logical_map[v] = v;
	77	}
	78
	79	/* Core 0 is powered up (we're running on it) */
	80	bitmap_set(core_power, 0, 1);
	81
0ee958e1	82	/* Initialise core 0 */
245a7868	83	mips_cps_core_init();
0ee958e1	84
0ee958e1 PB	85	/* Make core 0 coherent with everything */
	86	write_gcr_cl_coherence(0xff);
	87	}
	88
	89	static void __init cps_prepare_cpus(unsigned int max_cpus)
	90	{
245a7868	91	unsigned ncores, core_vpes, c;
0f4d3d11	92	u32 *entry_code;
245a7868	93
0ee958e1	94	mips_mt_set_cpuoptions();
245a7868	95
0f4d3d11 PB	96	/* Patch the start of mips_cps_core_entry to provide the CM base */
	97	entry_code = (u32 *)&mips_cps_core_entry;
	98	UASM_i_LA(&entry_code, 3, (long)mips_cm_base);
	99	dma_cache_wback_inv((unsigned long)&mips_cps_core_entry,
	100	(void )entry_code - (void )&mips_cps_core_entry);
	101
245a7868 PB	102	/* Allocate core boot configuration structs */
	103	ncores = mips_cm_numcores();
	104	mips_cps_core_bootcfg = kcalloc(ncores, sizeof(*mips_cps_core_bootcfg),
	105	GFP_KERNEL);
	106	if (!mips_cps_core_bootcfg) {
	107	pr_err("Failed to allocate boot config for %u cores\n", ncores);
	108	goto err_out;
	109	}
	110
	111	/* Allocate VPE boot configuration structs */
	112	for (c = 0; c < ncores; c++) {
	113	core_vpes = core_vpe_count(c);
	114	mips_cps_core_bootcfg[c].vpe_config = kcalloc(core_vpes,
	115	sizeof(*mips_cps_core_bootcfg[c].vpe_config),
	116	GFP_KERNEL);
	117	if (!mips_cps_core_bootcfg[c].vpe_config) {
	118	pr_err("Failed to allocate %u VPE boot configs\n",
	119	core_vpes);
	120	goto err_out;
	121	}
	122	}
	123
	124	/* Mark this CPU as booted */
	125	atomic_set(&mips_cps_core_bootcfg[current_cpu_data.core].vpe_mask,
	126	1 << cpu_vpe_id(&current_cpu_data));
	127
	128	return;
	129	err_out:
	130	/* Clean up allocations */
	131	if (mips_cps_core_bootcfg) {
	132	for (c = 0; c < ncores; c++)
	133	kfree(mips_cps_core_bootcfg[c].vpe_config);
	134	kfree(mips_cps_core_bootcfg);
	135	mips_cps_core_bootcfg = NULL;
	136	}
	137
	138	/* Effectively disable SMP by declaring CPUs not present */
	139	for_each_possible_cpu(c) {
	140	if (c == 0)
	141	continue;
	142	set_cpu_present(c, false);
	143	}
0ee958e1 PB	144	}
0ee958e1 PB	145
245a7868	146	static void boot_core(unsigned core)
0ee958e1 PB	147	{
	148	u32 access;
	149
	150	/* Select the appropriate core */
245a7868	151	write_gcr_cl_other(core << CM_GCR_Cx_OTHER_CORENUM_SHF);
0ee958e1 PB	152
	153	/* Set its reset vector */
	154	write_gcr_co_reset_base(CKSEG1ADDR((unsigned long)mips_cps_core_entry));
	155
	156	/* Ensure its coherency is disabled */
	157	write_gcr_co_coherence(0);
	158
	159	/* Ensure the core can access the GCRs */
	160	access = read_gcr_access();
245a7868	161	access \|= 1 << (CM_GCR_ACCESS_ACCESSEN_SHF + core);
0ee958e1 PB	162	write_gcr_access(access);
0ee958e1 PB	163
0ee958e1	164	if (mips_cpc_present()) {
0ee958e1	165	/* Reset the core */
dd9233d0	166	mips_cpc_lock_other(core);
0ee958e1	167	write_cpc_co_cmd(CPC_Cx_CMD_RESET);
dd9233d0	168	mips_cpc_unlock_other();
0ee958e1 PB	169	} else {
	170	/* Take the core out of reset */
	171	write_gcr_co_reset_release(0);
	172	}
	173
	174	/* The core is now powered up */
245a7868	175	bitmap_set(core_power, core, 1);
0ee958e1 PB	176	}
0ee958e1 PB	177
245a7868	178	static void remote_vpe_boot(void *dummy)
0ee958e1	179	{
245a7868	180	mips_cps_boot_vpes();
0ee958e1 PB	181	}
	182
	183	static void cps_boot_secondary(int cpu, struct task_struct *idle)
	184	{
245a7868 PB	185	unsigned core = cpu_data[cpu].core;
	186	unsigned vpe_id = cpu_vpe_id(&cpu_data[cpu]);
	187	struct core_boot_config *core_cfg = &mips_cps_core_bootcfg[core];
	188	struct vpe_boot_config *vpe_cfg = &core_cfg->vpe_config[vpe_id];
0ee958e1 PB	189	unsigned int remote;
	190	int err;
	191
245a7868 PB	192	vpe_cfg->pc = (unsigned long)&smp_bootstrap;
	193	vpe_cfg->sp = __KSTK_TOS(idle);
	194	vpe_cfg->gp = (unsigned long)task_thread_info(idle);
0ee958e1	195
245a7868 PB	196	atomic_or(1 << cpu_vpe_id(&cpu_data[cpu]), &core_cfg->vpe_mask);
245a7868 PB	197
1d8f1f5a PB	198	preempt_disable();
1d8f1f5a PB	199
245a7868	200	if (!test_bit(core, core_power)) {
0ee958e1	201	/* Boot a VPE on a powered down core */
245a7868	202	boot_core(core);
1d8f1f5a	203	goto out;
0ee958e1 PB	204	}
0ee958e1 PB	205
245a7868	206	if (core != current_cpu_data.core) {
0ee958e1 PB	207	/* Boot a VPE on another powered up core */
0ee958e1 PB	208	for (remote = 0; remote < NR_CPUS; remote++) {
245a7868	209	if (cpu_data[remote].core != core)
0ee958e1 PB	210	continue;
	211	if (cpu_online(remote))
	212	break;
	213	}
	214	BUG_ON(remote >= NR_CPUS);
	215
245a7868 PB	216	err = smp_call_function_single(remote, remote_vpe_boot,
245a7868 PB	217	NULL, 1);
0ee958e1 PB	218	if (err)
0ee958e1 PB	219	panic("Failed to call remote CPU\n");
1d8f1f5a	220	goto out;
0ee958e1 PB	221	}
	222
	223	BUG_ON(!cpu_has_mipsmt);
	224
	225	/* Boot a VPE on this core */
245a7868	226	mips_cps_boot_vpes();
1d8f1f5a PB	227	out:
1d8f1f5a PB	228	preempt_enable();
0ee958e1 PB	229	}
	230
	231	static void cps_init_secondary(void)
	232	{
	233	/* Disable MT - we only want to run 1 TC per VPE */
	234	if (cpu_has_mipsmt)
	235	dmt();
	236
0ee958e1 PB	237	change_c0_status(ST0_IM, STATUSF_IP3 \| STATUSF_IP4 \|
	238	STATUSF_IP6 \| STATUSF_IP7);
	239	}
	240
	241	static void cps_smp_finish(void)
	242	{
	243	write_c0_compare(read_c0_count() + (8 * mips_hpt_frequency / HZ));
	244
	245	#ifdef CONFIG_MIPS_MT_FPAFF
	246	/* If we have an FPU, enroll ourselves in the FPU-full mask */
	247	if (cpu_has_fpu)
	248	cpu_set(smp_processor_id(), mt_fpu_cpumask);
	249	#endif /* CONFIG_MIPS_MT_FPAFF */
	250
	251	local_irq_enable();
	252	}
	253
	254	static void cps_cpus_done(void)
	255	{
	256	}
	257
1d8f1f5a PB	258	#ifdef CONFIG_HOTPLUG_CPU
	259
	260	static int cps_cpu_disable(void)
	261	{
	262	unsigned cpu = smp_processor_id();
	263	struct core_boot_config *core_cfg;
	264
	265	if (!cpu)
	266	return -EBUSY;
	267
	268	if (!cps_pm_support_state(CPS_PM_POWER_GATED))
	269	return -EINVAL;
	270
	271	core_cfg = &mips_cps_core_bootcfg[current_cpu_data.core];
	272	atomic_sub(1 << cpu_vpe_id(&current_cpu_data), &core_cfg->vpe_mask);
	273	smp_mb__after_atomic_dec();
	274	set_cpu_online(cpu, false);
	275	cpu_clear(cpu, cpu_callin_map);
	276
	277	return 0;
	278	}
	279
	280	static DECLARE_COMPLETION(cpu_death_chosen);
	281	static unsigned cpu_death_sibling;
	282	static enum {
	283	CPU_DEATH_HALT,
	284	CPU_DEATH_POWER,
	285	} cpu_death;
	286
	287	void play_dead(void)
	288	{
	289	unsigned cpu, core;
	290
	291	local_irq_disable();
	292	idle_task_exit();
	293	cpu = smp_processor_id();
	294	cpu_death = CPU_DEATH_POWER;
	295
	296	if (cpu_has_mipsmt) {
	297	core = cpu_data[cpu].core;
	298
	299	/* Look for another online VPE within the core */
	300	for_each_online_cpu(cpu_death_sibling) {
	301	if (cpu_data[cpu_death_sibling].core != core)
	302	continue;
	303
	304	/*
	305	* There is an online VPE within the core. Just halt
	306	* this TC and leave the core alone.
	307	*/
	308	cpu_death = CPU_DEATH_HALT;
	309	break;
	310	}
	311	}
	312
	313	/* This CPU has chosen its way out */
	314	complete(&cpu_death_chosen);
	315
	316	if (cpu_death == CPU_DEATH_HALT) {
	317	/* Halt this TC */
	318	write_c0_tchalt(TCHALT_H);
	319	instruction_hazard();
	320	} else {
	321	/* Power down the core */
322	cps_pm_enter_state(CPS_PM_POWER_GATED);
323	}
324
325	/* This should never be reached */
326	panic("Failed to offline CPU %u", cpu);
327	}
328
329	static void wait_for_sibling_halt(void *ptr_cpu)
330	{
331	unsigned cpu = (unsigned)ptr_cpu;
332	unsigned vpe_id = cpu_data[cpu].vpe_id;
333	unsigned halted;
334	unsigned long flags;
335
336	do {
337	local_irq_save(flags);
338	settc(vpe_id);
339	halted = read_tc_c0_tchalt();
340	local_irq_restore(flags);
341	} while (!(halted & TCHALT_H));
342	}
343
344	static void cps_cpu_die(unsigned int cpu)
345	{
346	unsigned core = cpu_data[cpu].core;
347	unsigned stat;
348	int err;
349
350	/* Wait for the cpu to choose its way out */
351	if (!wait_for_completion_timeout(&cpu_death_chosen,
352	msecs_to_jiffies(5000))) {
353	pr_err("CPU%u: didn't offline\n", cpu);
354	return;
355	}
356
357	/*
358	* Now wait for the CPU to actually offline. Without doing this that
359	* offlining may race with one or more of:
360	*
361	* - Onlining the CPU again.
362	* - Powering down the core if another VPE within it is offlined.
363	* - A sibling VPE entering a non-coherent state.
364	*
365	* In the non-MT halt case (ie. infinite loop) the CPU is doing nothing
366	* with which we could race, so do nothing.
367	*/
368	if (cpu_death == CPU_DEATH_POWER) {
369	/*
370	* Wait for the core to enter a powered down or clock gated
371	* state, the latter happening when a JTAG probe is connected
372	* in which case the CPC will refuse to power down the core.
373	*/
374	do {
375	mips_cpc_lock_other(core);
376	stat = read_cpc_co_stat_conf();
377	stat &= CPC_Cx_STAT_CONF_SEQSTATE_MSK;
378	mips_cpc_unlock_other();
379	} while (stat != CPC_Cx_STAT_CONF_SEQSTATE_D0 &&
380	stat != CPC_Cx_STAT_CONF_SEQSTATE_D2 &&
381	stat != CPC_Cx_STAT_CONF_SEQSTATE_U2);
382
383	/* Indicate the core is powered off */
384	bitmap_clear(core_power, core, 1);
385	} else if (cpu_has_mipsmt) {
386	/*
387	* Have a CPU with access to the offlined CPUs registers wait
388	* for its TC to halt.
389	*/
390	err = smp_call_function_single(cpu_death_sibling,
391	wait_for_sibling_halt,
392	(void *)cpu, 1);
393	if (err)
394	panic("Failed to call remote sibling CPU\n");
395	}
396	}
397
398	#endif /* CONFIG_HOTPLUG_CPU */
399
0ee958e1 PB	400	static struct plat_smp_ops cps_smp_ops = {
	401	.smp_setup = cps_smp_setup,
	402	.prepare_cpus = cps_prepare_cpus,
	403	.boot_secondary = cps_boot_secondary,
	404	.init_secondary = cps_init_secondary,
	405	.smp_finish = cps_smp_finish,
	406	.send_ipi_single = gic_send_ipi_single,
	407	.send_ipi_mask = gic_send_ipi_mask,
	408	.cpus_done = cps_cpus_done,
1d8f1f5a PB	409	#ifdef CONFIG_HOTPLUG_CPU
	410	.cpu_disable = cps_cpu_disable,
	411	.cpu_die = cps_cpu_die,
	412	#endif
0ee958e1 PB	413	};
0ee958e1 PB	414
68c1232f PB	415	bool mips_cps_smp_in_use(void)
	416	{
	417	extern struct plat_smp_ops *mp_ops;
	418	return mp_ops == &cps_smp_ops;
	419	}
	420
0ee958e1 PB	421	int register_cps_smp_ops(void)
	422	{
	423	if (!mips_cm_present()) {
	424	pr_warn("MIPS CPS SMP unable to proceed without a CM\n");
	425	return -ENODEV;
	426	}
	427
	428	/* check we have a GIC - we need one for IPIs */
	429	if (!(read_gcr_gic_status() & CM_GCR_GIC_STATUS_EX_MSK)) {
	430	pr_warn("MIPS CPS SMP unable to proceed without a GIC\n");
	431	return -ENODEV;
	432	}
	433
	434	register_smp_ops(&cps_smp_ops);
	435	return 0;
	436	}