code:
// SPDX-License-Identifier: GPL-2.0-only
/*
 *  linux/arch/arm/kernel/setup.c
 *
 *  Copyright (C) 1995-2001 Russell King
 */
#include <linux/efi.h>
#include <linux/export.h>
#include <linux/kernel.h>
#include <linux/stddef.h>
#include <linux/ioport.h>
#include <linux/delay.h>
#include <linux/utsname.h>
#include <linux/initrd.h>
#include <linux/console.h>
#include <linux/seq_file.h>
#include <linux/screen_info.h>
#include <linux/of_platform.h>
#include <linux/init.h>
#include <linux/kexec.h>
#include <linux/libfdt.h>
#include <linux/of_fdt.h>
#include <linux/cpu.h>
#include <linux/interrupt.h>
#include <linux/smp.h>
#include <linux/proc_fs.h>
#include <linux/memblock.h>
#include <linux/bug.h>
#include <linux/compiler.h>
#include <linux/sort.h>
#include <linux/psci.h>

#include <asm/unified.h>
#include <asm/cp15.h>
#include <asm/cpu.h>
#include <asm/cputype.h>
#include <asm/efi.h>
#include <asm/elf.h>
#include <asm/early_ioremap.h>
#include <asm/fixmap.h>
#include <asm/procinfo.h>
#include <asm/psci.h>
#include <asm/sections.h>
#include <asm/setup.h>
#include <asm/smp_plat.h>
#include <asm/mach-types.h>
#include <asm/cacheflush.h>
#include <asm/cachetype.h>
#include <asm/tlbflush.h>
#include <asm/xen/hypervisor.h>

#include <asm/prom.h>
#include <asm/mach/arch.h>
#include <asm/mach/irq.h>
#include <asm/mach/time.h>
#include <asm/system_info.h>
#include <asm/system_misc.h>
#include <asm/traps.h>
#include <asm/unwind.h>
#include <asm/memblock.h>
#include <asm/virt.h>
#include <asm/kasan.h>

#include "atags.h"

#if defined(CONFIG_FPE_NWFPE) || defined(CONFIG_FPE_FASTFPE)
char fpe_type[8];

static int __init fpe_setup(char *line)
{
	memcpy(fpe_type, line, 8);
	return 1;
}

__setup("fpe=", fpe_setup);
#endif

extern void init_default_cache_policy(unsigned long);
extern void paging_init(const struct machine_desc *desc);
extern void early_mm_init(const struct machine_desc *);
extern void adjust_lowmem_bounds(void);
extern enum reboot_mode reboot_mode;
extern void setup_dma_zone(const struct machine_desc *desc);

unsigned int processor_id;
EXPORT_SYMBOL(processor_id);
unsigned int __machine_arch_type __read_mostly;
EXPORT_SYMBOL(__machine_arch_type);
unsigned int cacheid __read_mostly;
EXPORT_SYMBOL(cacheid);

unsigned int __atags_pointer __initdata;

unsigned int system_rev;
EXPORT_SYMBOL(system_rev);

const char *system_serial;
EXPORT_SYMBOL(system_serial);

unsigned int system_serial_low;
EXPORT_SYMBOL(system_serial_low);

unsigned int system_serial_high;
EXPORT_SYMBOL(system_serial_high);

unsigned int elf_hwcap __read_mostly;
EXPORT_SYMBOL(elf_hwcap);

unsigned int elf_hwcap2 __read_mostly;
EXPORT_SYMBOL(elf_hwcap2);

#ifdef MULTI_CPU
struct processor processor __ro_after_init;
#if defined(CONFIG_BIG_LITTLE) && defined(CONFIG_HARDEN_BRANCH_PREDICTOR)
struct processor *cpu_vtable[NR_CPUS] = {
	[0] = &processor,
};
#endif
#endif
#ifdef MULTI_TLB
struct cpu_tlb_fns cpu_tlb __ro_after_init;
#endif
#ifdef MULTI_USER
struct cpu_user_fns cpu_user __ro_after_init;
#endif
#ifdef MULTI_CACHE
struct cpu_cache_fns cpu_cache __ro_after_init;
#endif
#ifdef CONFIG_OUTER_CACHE
struct outer_cache_fns outer_cache __ro_after_init;
EXPORT_SYMBOL(outer_cache);
#endif

/*
 * Cached cpu_architecture() result for use by assembler code.
 * C code should use the cpu_architecture() function instead of accessing this
 * variable directly.
 */
int __cpu_architecture __read_mostly = CPU_ARCH_UNKNOWN;

struct stack {
	u32 irq[4];
	u32 abt[4];
	u32 und[4];
	u32 fiq[4];
} ____cacheline_aligned;

#ifndef CONFIG_CPU_V7M
static struct stack stacks[NR_CPUS];
#endif

char elf_platform[ELF_PLATFORM_SIZE];
EXPORT_SYMBOL(elf_platform);

static const char *cpu_name;
static const char *machine_name;
static char __initdata cmd_line[COMMAND_LINE_SIZE];
const struct machine_desc *machine_desc __initdata;

static union { char c[4]; unsigned long l; } endian_test __initdata = { { 'l', '?', '?', 'b' } };
#define ENDIANNESS ((char)endian_test.l)

DEFINE_PER_CPU(struct cpuinfo_arm, cpu_data);

/*
 * Standard memory resources
 */
static struct resource mem_res[] = {
	{
		.name = "Video RAM",
		.start = 0,
		.end = 0,
		.flags = IORESOURCE_MEM
	},
	{
		.name = "Kernel code",
		.start = 0,
		.end = 0,
		.flags = IORESOURCE_SYSTEM_RAM
	},
	{
		.name = "Kernel data",
		.start = 0,
		.end = 0,
		.flags = IORESOURCE_SYSTEM_RAM
	}
};

#define video_ram   mem_res[0]
#define kernel_code mem_res[1]
#define kernel_data mem_res[2]

static struct resource io_res[] = {
	{
		.name = "reserved",
		.start = 0x3bc,
		.end = 0x3be,
		.flags = IORESOURCE_IO | IORESOURCE_BUSY
	},
	{
		.name = "reserved",
		.start = 0x378,
		.end = 0x37f,
		.flags = IORESOURCE_IO | IORESOURCE_BUSY
	},
	{
		.name = "reserved",
		.start = 0x278,
		.end = 0x27f,
		.flags = IORESOURCE_IO | IORESOURCE_BUSY
	}
};

#define lp0 io_res[0]
#define lp1 io_res[1]
#define lp2 io_res[2]

static const char *proc_arch[] = {
	"undefined/unknown",
	"3",
	"4",
	"4T",
	"5",
	"5T",
	"5TE",
	"5TEJ",
	"6TEJ",
	"7",
	"7M",
	"?(12)",
	"?(13)",
	"?(14)",
	"?(15)",
	"?(16)",
	"?(17)",
};

#ifdef CONFIG_CPU_V7M
static int __get_cpu_architecture(void)
{
	return CPU_ARCH_ARMv7M;
}
#else
static int __get_cpu_architecture(void)
{
	int cpu_arch;

	if ((read_cpuid_id() & 0x0008f000) == 0) {
		cpu_arch = CPU_ARCH_UNKNOWN;
	} else if ((read_cpuid_id() & 0x0008f000) == 0x00007000) {
		cpu_arch = (read_cpuid_id() & (1 << 23)) ? CPU_ARCH_ARMv4T : CPU_ARCH_ARMv3;
	} else if ((read_cpuid_id() & 0x00080000) == 0x00000000) {
		cpu_arch = (read_cpuid_id() >> 16) & 7;
		if (cpu_arch)
			cpu_arch += CPU_ARCH_ARMv3;
	} else if ((read_cpuid_id() & 0x000f0000) == 0x000f0000) {
		/* Revised CPUID format. Read the Memory Model Feature
		 * Register 0 and check for VMSAv7 or PMSAv7 */
		unsigned int mmfr0 = read_cpuid_ext(CPUID_EXT_MMFR0);
		if ((mmfr0 & 0x0000000f) >= 0x00000003 ||
		    (mmfr0 & 0x000000f0) >= 0x00000030)
			cpu_arch = CPU_ARCH_ARMv7;
		else if ((mmfr0 & 0x0000000f) == 0x00000002 ||
			 (mmfr0 & 0x000000f0) == 0x00000020)
			cpu_arch = CPU_ARCH_ARMv6;
		else
			cpu_arch = CPU_ARCH_UNKNOWN;
	} else
		cpu_arch = CPU_ARCH_UNKNOWN;

	return cpu_arch;
}
#endif

int __pure cpu_architecture(void)
{
	BUG_ON(__cpu_architecture == CPU_ARCH_UNKNOWN);

	return __cpu_architecture;
}

static int cpu_has_aliasing_icache(unsigned int arch)
{
	int aliasing_icache;
	unsigned int id_reg, num_sets, line_size;

	/* PIPT caches never alias. */
	if (icache_is_pipt())
		return 0;

	/* arch specifies the register format */
	switch (arch) {
	case CPU_ARCH_ARMv7:
		set_csselr(CSSELR_ICACHE | CSSELR_L1);
		isb();
		id_reg = read_ccsidr();
		line_size = 4 << ((id_reg & 0x7) + 2);
		num_sets = ((id_reg >> 13) & 0x7fff) + 1;
		aliasing_icache = (line_size * num_sets) > PAGE_SIZE;
		break;
	case CPU_ARCH_ARMv6:
		aliasing_icache = read_cpuid_cachetype() & (1 << 11);
		break;
	default:
		/* I-cache aliases will be handled by D-cache aliasing code */
		aliasing_icache = 0;
	}

	return aliasing_icache;
}

static void __init cacheid_init(void)
{
	unsigned int arch = cpu_architecture();

	if (arch >= CPU_ARCH_ARMv6) {
		unsigned int cachetype = read_cpuid_cachetype();

		if ((arch == CPU_ARCH_ARMv7M) && !(cachetype & 0xf000f)) {
			cacheid = 0;
		} else if ((cachetype & (7 << 29)) == 4 << 29) {
			/* ARMv7 register format */
			arch = CPU_ARCH_ARMv7;
			cacheid = CACHEID_VIPT_NONALIASING;
			switch (cachetype & (3 << 14)) {
			case (1 << 14):
				cacheid |= CACHEID_ASID_TAGGED;
				break;
			case (3 << 14):
				cacheid |= CACHEID_PIPT;
				break;
			}
		} else {
			arch = CPU_ARCH_ARMv6;
			if (cachetype & (1 << 23))
				cacheid = CACHEID_VIPT_ALIASING;
			else
				cacheid = CACHEID_VIPT_NONALIASING;
		}
		if (cpu_has_aliasing_icache(arch))
			cacheid |= CACHEID_VIPT_I_ALIASING;
	} else {
		cacheid = CACHEID_VIVT;
	}

	pr_info("CPU: %s data cache, %s instruction cache\n",
		cache_is_vivt() ? "VIVT" :
		cache_is_vipt_aliasing() ? "VIPT aliasing" :
		cache_is_vipt_nonaliasing() ? "PIPT / VIPT nonaliasing" : "unknown",
		cache_is_vivt() ? "VIVT" :
		icache_is_vivt_asid_tagged() ? "VIVT ASID tagged" :
		icache_is_vipt_aliasing() ? "VIPT aliasing" :
		icache_is_pipt() ? "PIPT" :
		cache_is_vipt_nonaliasing() ? "VIPT nonaliasing" : "unknown");
}

/*
 * These functions re-use the assembly code in head.S, which
 * already provide the required functionality.
 */
extern struct proc_info_list *lookup_processor_type(unsigned int);

void __init early_print(const char *str, ...)
{
	extern void printascii(const char *);
	char buf[256];
	va_list ap;

	va_start(ap, str);
	vsnprintf(buf, sizeof(buf), str, ap);
	va_end(ap);

#ifdef CONFIG_DEBUG_LL
	printascii(buf);
#endif
	printk("%s", buf);
}

#ifdef CONFIG_ARM_PATCH_IDIV

static inline u32 __attribute_const__ sdiv_instruction(void)
{
	if (IS_ENABLED(CONFIG_THUMB2_KERNEL)) {
		/* "sdiv r0, r0, r1" */
		u32 insn = __opcode_thumb32_compose(0xfb90, 0xf0f1);
		return __opcode_to_mem_thumb32(insn);
	}

	/* "sdiv r0, r0, r1" */
	return __opcode_to_mem_arm(0xe710f110);
}

static inline u32 __attribute_const__ udiv_instruction(void)
{
	if (IS_ENABLED(CONFIG_THUMB2_KERNEL)) {
		/* "udiv r0, r0, r1" */
		u32 insn = __opcode_thumb32_compose(0xfbb0, 0xf0f1);
		return __opcode_to_mem_thumb32(insn);
	}

	/* "udiv r0, r0, r1" */
	return __opcode_to_mem_arm(0xe730f110);
}

static inline u32 __attribute_const__ bx_lr_instruction(void)
{
	if (IS_ENABLED(CONFIG_THUMB2_KERNEL)) {
		/* "bx lr; nop" */
		u32 insn = __opcode_thumb32_compose(0x4770, 0x46c0);
		return __opcode_to_mem_thumb32(insn);
	}

	/* "bx lr" */
	return __opcode_to_mem_arm(0xe12fff1e);
}

static void __init patch_aeabi_idiv(void)
{
	extern void __aeabi_uidiv(void);
	extern void __aeabi_idiv(void);
	uintptr_t fn_addr;
	unsigned int mask;

	mask = IS_ENABLED(CONFIG_THUMB2_KERNEL) ? HWCAP_IDIVT : HWCAP_IDIVA;
	if (!(elf_hwcap & mask))
		return;

	pr_info("CPU: div instructions available: patching division code\n");

	fn_addr = ((uintptr_t)&__aeabi_uidiv) & ~1;
	asm ("" : "+g" (fn_addr));
	((u32 *)fn_addr)[0] = udiv_instruction();
	((u32 *)fn_addr)[1] = bx_lr_instruction();
	flush_icache_range(fn_addr, fn_addr + 8);

	fn_addr = ((uintptr_t)&__aeabi_idiv) & ~1;
	asm ("" : "+g" (fn_addr));
	((u32 *)fn_addr)[0] = sdiv_instruction();
	((u32 *)fn_addr)[1] = bx_lr_instruction();
	flush_icache_range(fn_addr, fn_addr + 8);
}

#else
static inline void patch_aeabi_idiv(void) { }
#endif

static void __init cpuid_init_hwcaps(void)
{
	int block;
	u32 isar5;
	u32 isar6;
	u32 pfr2;

	if (cpu_architecture() < CPU_ARCH_ARMv7)
		return;

	block = cpuid_feature_extract(CPUID_EXT_ISAR0, 24);
	if (block >= 2)
		elf_hwcap |= HWCAP_IDIVA;
	if (block >= 1)
		elf_hwcap |= HWCAP_IDIVT;

	/* LPAE implies atomic ldrd/strd instructions */
	block = cpuid_feature_extract(CPUID_EXT_MMFR0, 0);
	if (block >= 5)
		elf_hwcap |= HWCAP_LPAE;

	/* check for supported v8 Crypto instructions */
	isar5 = read_cpuid_ext(CPUID_EXT_ISAR5);

	block = cpuid_feature_extract_field(isar5, 4);
	if (block >= 2)
		elf_hwcap2 |= HWCAP2_PMULL;
	if (block >= 1)
		elf_hwcap2 |= HWCAP2_AES;

	block = cpuid_feature_extract_field(isar5, 8);
	if (block >= 1)
		elf_hwcap2 |= HWCAP2_SHA1;

	block = cpuid_feature_extract_field(isar5, 12);
	if (block >= 1)
		elf_hwcap2 |= HWCAP2_SHA2;

	block = cpuid_feature_extract_field(isar5, 16);
	if (block >= 1)
		elf_hwcap2 |= HWCAP2_CRC32;

	/* Check for Speculation barrier instruction */
	isar6 = read_cpuid_ext(CPUID_EXT_ISAR6);
	block = cpuid_feature_extract_field(isar6, 12);
	if (block >= 1)
		elf_hwcap2 |= HWCAP2_SB;

	/* Check for Speculative Store Bypassing control */
	pfr2 = read_cpuid_ext(CPUID_EXT_PFR2);
	block = cpuid_feature_extract_field(pfr2, 4);
	if (block >= 1)
		elf_hwcap2 |= HWCAP2_SSBS;
}

static void __init elf_hwcap_fixup(void)
{
	unsigned id = read_cpuid_id();

	/*
	 * HWCAP_TLS is available only on 1136 r1p0 and later,
	 * see also kuser_get_tls_init.
	 */
	if (read_cpuid_part() == ARM_CPU_PART_ARM1136 &&
	    ((id >> 20) & 3) == 0) {
		elf_hwcap &= ~HWCAP_TLS;
		return;
	}

	/* Verify if CPUID scheme is implemented */
	if ((id & 0x000f0000) != 0x000f0000)
		return;

	/*
	 * If the CPU supports LDREX/STREX and LDREXB/STREXB,
	 * avoid advertising SWP; it may not be atomic with
	 * multiprocessing cores.
	 */
	if (cpuid_feature_extract(CPUID_EXT_ISAR3, 12) > 1 ||
	    (cpuid_feature_extract(CPUID_EXT_ISAR3, 12) == 1 &&
	     cpuid_feature_extract(CPUID_EXT_ISAR4, 20) >= 3))
		elf_hwcap &= ~HWCAP_SWP;
}

/*
 * cpu_init - initialise one CPU.
 *
 * cpu_init sets up the per-CPU stacks.
 */
void notrace cpu_init(void)
{
#ifndef CONFIG_CPU_V7M
	unsigned int cpu = smp_processor_id();
	struct stack *stk = &stacks[cpu];

	if (cpu >= NR_CPUS) {
		pr_crit("CPU%u: bad primary CPU number\n", cpu);
		BUG();
	}

	/*
	 * This only works on resume and secondary cores. For booting on the
	 * boot cpu, smp_prepare_boot_cpu is called after percpu area setup.
	 */
	set_my_cpu_offset(per_cpu_offset(cpu));

	cpu_proc_init();

	/*
	 * Define the placement constraint for the inline asm directive below.
	 * In Thumb-2, msr with an immediate value is not allowed.
	 */
#ifdef CONFIG_THUMB2_KERNEL
#define PLC_l	"l"
#define PLC_r	"r"
#else
#define PLC_l	"I"
#define PLC_r	"I"
#endif

	/*
	 * setup stacks for re-entrant exception handlers
	 */
	__asm__ (
	"msr	cpsr_c, %1\n\t"
	"add	r14, %0, %2\n\t"
	"mov	sp, r14\n\t"
	"msr	cpsr_c, %3\n\t"
	"add	r14, %0, %4\n\t"
	"mov	sp, r14\n\t"
	"msr	cpsr_c, %5\n\t"
	"add	r14, %0, %6\n\t"
	"mov	sp, r14\n\t"
	"msr	cpsr_c, %7\n\t"
	"add	r14, %0, %8\n\t"
	"mov	sp, r14\n\t"
	"msr	cpsr_c, %9"
	    :
	    : "r" (stk),
	      PLC_r (PSR_F_BIT | PSR_I_BIT | IRQ_MODE),
	      "I" (offsetof(struct stack, irq[0])),
	      PLC_r (PSR_F_BIT | PSR_I_BIT | ABT_MODE),
	      "I" (offsetof(struct stack, abt[0])),
	      PLC_r (PSR_F_BIT | PSR_I_BIT | UND_MODE),
	      "I" (offsetof(struct stack, und[0])),
	      PLC_r (PSR_F_BIT | PSR_I_BIT | FIQ_MODE),
	      "I" (offsetof(struct stack, fiq[0])),
	      PLC_l (PSR_F_BIT | PSR_I_BIT | SVC_MODE)
	    : "r14");
#endif
}

u32 __cpu_logical_map[NR_CPUS] = { [0 ... NR_CPUS-1] = MPIDR_INVALID };

void __init smp_setup_processor_id(void)
{
	int i;
	u32 mpidr = is_smp() ? read_cpuid_mpidr() & MPIDR_HWID_BITMASK : 0;
	u32 cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);

	cpu_logical_map(0) = cpu;
	for (i = 1; i < nr_cpu_ids; ++i)
		cpu_logical_map(i) = i == cpu ? 0 : i;

	/*
	 * clear __my_cpu_offset on boot CPU to avoid hang caused by
	 * using percpu variable early, for example, lockdep will
	 * access percpu variable inside lock_release
	 */
	set_my_cpu_offset(0);

	pr_info("Booting Linux on physical CPU 0x%x\n", mpidr);
}

struct mpidr_hash mpidr_hash;
#ifdef CONFIG_SMP
/**
 * smp_build_mpidr_hash - Pre-compute shifts required at each affinity
 *			  level in order to build a linear index from an
 *			  MPIDR value. Resulting algorithm is a collision
 *			  free hash carried out through shifting and ORing
 */
static void __init smp_build_mpidr_hash(void)
{
	u32 i, affinity;
	u32 fs[3], bits[3], ls, mask = 0;
	/*
	 * Pre-scan the list of MPIDRS and filter out bits that do
	 * not contribute to affinity levels, ie they never toggle.
	 */
	for_each_possible_cpu(i)
		mask |= (cpu_logical_map(i) ^ cpu_logical_map(0));
	pr_debug("mask of set bits 0x%x\n", mask);
	/*
	 * Find and stash the last and first bit set at all affinity levels to
	 * check how many bits are required to represent them.
	 */
	for (i = 0; i < 3; i++) {
		affinity = MPIDR_AFFINITY_LEVEL(mask, i);
		/*
		 * Find the MSB bit and LSB bits position
		 * to determine how many bits are required
		 * to express the affinity level.
		 */
		ls = fls(affinity);
		fs[i] = affinity ? ffs(affinity) - 1 : 0;
		bits[i] = ls - fs[i];
	}
	/*
	 * An index can be created from the MPIDR by isolating the
	 * significant bits at each affinity level and by shifting
	 * them in order to compress the 24 bits values space to a
	 * compressed set of values. This is equivalent to hashing
	 * the MPIDR through shifting and ORing. It is a collision free
	 * hash though not minimal since some levels might contain a number
	 * of CPUs that is not an exact power of 2 and their bit
	 * representation might contain holes, eg MPIDR[7:0] = {0x2, 0x80}.
	 */
	mpidr_hash.shift_aff[0] = fs[0];
	mpidr_hash.shift_aff[1] = MPIDR_LEVEL_BITS + fs[1] - bits[0];
	mpidr_hash.shift_aff[2] = 2*MPIDR_LEVEL_BITS + fs[2] -
						(bits[1] + bits[0]);
	mpidr_hash.mask = mask;
	mpidr_hash.bits = bits[2] + bits[1] + bits[0];
	pr_debug("MPIDR hash: aff0[%u] aff1[%u] aff2[%u] mask[0x%x] bits[%u]\n",
				mpidr_hash.shift_aff[0],
				mpidr_hash.shift_aff[1],
				mpidr_hash.shift_aff[2],
				mpidr_hash.mask,
				mpidr_hash.bits);
	/*
	 * 4x is an arbitrary value used to warn on a hash table much bigger
	 * than expected on most systems.
	 */
	if (mpidr_hash_size() > 4 * num_possible_cpus())
		pr_warn("Large number of MPIDR hash buckets detected\n");
	sync_cache_w(&mpidr_hash);
}
#endif

/*
 * locate processor in the list of supported processor types.  The linker
 * builds this table for us from the entries in arch/arm/mm/proc-*.S
 */
struct proc_info_list *lookup_processor(u32 midr)
{
	struct proc_info_list *list = lookup_processor_type(midr);

	if (!list) {
		pr_err("CPU%u: configuration botched (ID %08x), CPU halted\n",
		       smp_processor_id(), midr);
		while (1)
		/* can't use cpu_relax() here as it may require MMU setup */;
	}

	return list;
}

static void __init setup_processor(void)
{
	unsigned int midr = read_cpuid_id();
	struct proc_info_list *list = lookup_processor(midr);

	cpu_name = list->cpu_name;
	__cpu_architecture = __get_cpu_architecture();

	init_proc_vtable(list->proc);
#ifdef MULTI_TLB
	cpu_tlb = *list->tlb;
#endif
#ifdef MULTI_USER
	cpu_user = *list->user;
#endif
#ifdef MULTI_CACHE
	cpu_cache = *list->cache;
#endif

	pr_info("CPU: %s [%08x] revision %d (ARMv%s), cr=%08lx\n",
		list->cpu_name, midr, midr & 15,
		proc_arch[cpu_architecture()], get_cr());

	snprintf(init_utsname()->machine, __NEW_UTS_LEN + 1, "%s%c",
		 list->arch_name, ENDIANNESS);
	snprintf(elf_platform, ELF_PLATFORM_SIZE, "%s%c",
		 list->elf_name, ENDIANNESS);
	elf_hwcap = list->elf_hwcap;

	cpuid_init_hwcaps();
	patch_aeabi_idiv();

#ifndef CONFIG_ARM_THUMB
	elf_hwcap &= ~(HWCAP_THUMB | HWCAP_IDIVT);
#endif
#ifdef CONFIG_MMU
	init_default_cache_policy(list->__cpu_mm_mmu_flags);
#endif
	erratum_a15_798181_init();

	elf_hwcap_fixup();

	cacheid_init();
	cpu_init();
}

void __init dump_machine_table(void)
{
	const struct machine_desc *p;

	early_print("Available machine support:\n\nID (hex)\tNAME\n");
	for_each_machine_desc(p)
		early_print("%08x\t%s\n", p->nr, p->name);

	early_print("\nPlease check your kernel config and/or bootloader.\n");

	while (true)
		/* can't use cpu_relax() here as it may require MMU setup */;
}

int __init arm_add_memory(u64 start, u64 size)
{
	u64 aligned_start;

	/*
	 * Ensure that start/size are aligned to a page boundary.
	 * Size is rounded down, start is rounded up.
	 */
	aligned_start = PAGE_ALIGN(start);
	if (aligned_start > start + size)
		size = 0;
	else
		size -= aligned_start - start;

#ifndef CONFIG_PHYS_ADDR_T_64BIT
	if (aligned_start > ULONG_MAX) {
		pr_crit("Ignoring memory at 0x%08llx outside 32-bit physical address space\n",
			start);
		return -EINVAL;
	}

	if (aligned_start + size > ULONG_MAX) {
		pr_crit("Truncating memory at 0x%08llx to fit in 32-bit physical address space\n",
			(long long)start);
		/*
		 * To ensure bank->start + bank->size is representable in
		 * 32 bits, we use ULONG_MAX as the upper limit rather than 4GB.
		 * This means we lose a page after masking.
		 */
		size = ULONG_MAX - aligned_start;
	}
#endif

	if (aligned_start < PHYS_OFFSET) {
		if (aligned_start + size <= PHYS_OFFSET) {
			pr_info("Ignoring memory below PHYS_OFFSET: 0x%08llx-0x%08llx\n",
				aligned_start, aligned_start + size);
			return -EINVAL;
		}

		pr_info("Ignoring memory below PHYS_OFFSET: 0x%08llx-0x%08llx\n",
			aligned_start, (u64)PHYS_OFFSET);

		size -= PHYS_OFFSET - aligned_start;
		aligned_start = PHYS_OFFSET;
	}

	start = aligned_start;
	size = size & ~(phys_addr_t)(PAGE_SIZE - 1);

	/*
	 * Check whether this memory region has non-zero size or
	 * invalid node number.
	 */
	if (size == 0)
		return -EINVAL;

	memblock_add(start, size);
	return 0;
}

/*
 * Pick out the memory size.  We look for mem=size@start,
 * where start and size are "size[KkMm]"
 */

static int __init early_mem(char *p)
{
	static int usermem __initdata = 0;
	u64 size;
	u64 start;
	char *endp;

	/*
	 * If the user specifies memory size, we
	 * blow away any automatically generated
	 * size.
	 */
	if (usermem == 0) {
		usermem = 1;
		memblock_remove(memblock_start_of_DRAM(),
			memblock_end_of_DRAM() - memblock_start_of_DRAM());
	}

	start = PHYS_OFFSET;
	size  = memparse(p, &endp);
	if (*endp == '@')
		start = memparse(endp + 1, NULL);

	arm_add_memory(start, size);

	return 0;
}
early_param("mem", early_mem);

static void __init request_standard_resources(const struct machine_desc *mdesc)
{
	phys_addr_t start, end, res_end;
	struct resource *res;
	u64 i;

	kernel_code.start   = virt_to_phys(_text);
	kernel_code.end     = virt_to_phys(__init_begin - 1);
	kernel_data.start   = virt_to_phys(_sdata);
	kernel_data.end     = virt_to_phys(_end - 1);

	for_each_mem_range(i, &start, &end) {
		unsigned long boot_alias_start;

		/*
		 * In memblock, end points to the first byte after the
		 * range while in resourses, end points to the last byte in
		 * the range.
		 */
		res_end = end - 1;

		/*
		 * Some systems have a special memory alias which is only
		 * used for booting.  We need to advertise this region to
		 * kexec-tools so they know where bootable RAM is located.
		 */
		boot_alias_start = phys_to_idmap(start);
		if (arm_has_idmap_alias() && boot_alias_start != IDMAP_INVALID_ADDR) {
			res = memblock_alloc(sizeof(*res), SMP_CACHE_BYTES);
			if (!res)
				panic("%s: Failed to allocate %zu bytes\n",
				      __func__, sizeof(*res));
			res->name = "System RAM (boot alias)";
			res->start = boot_alias_start;
			res->end = phys_to_idmap(res_end);
			res->flags = IORESOURCE_MEM | IORESOURCE_BUSY;
			request_resource(&iomem_resource, res);
		}

		res = memblock_alloc(sizeof(*res), SMP_CACHE_BYTES);
		if (!res)
			panic("%s: Failed to allocate %zu bytes\n", __func__,
			      sizeof(*res));
		res->name  = "System RAM";
		res->start = start;
		res->end = res_end;
		res->flags = IORESOURCE_SYSTEM_RAM | IORESOURCE_BUSY;

		request_resource(&iomem_resource, res);

		if (kernel_code.start >= res->start &&
		    kernel_code.end <= res->end)
			request_resource(res, &kernel_code);
		if (kernel_data.start >= res->start &&
		    kernel_data.end <= res->end)
			request_resource(res, &kernel_data);
	}

	if (mdesc->video_start) {
		video_ram.start = mdesc->video_start;
		video_ram.end   = mdesc->video_end;
		request_resource(&iomem_resource, &video_ram);
	}

	/*
	 * Some machines don't have the possibility of ever
	 * possessing lp0, lp1 or lp2
	 */
	if (mdesc->reserve_lp0)
		request_resource(&ioport_resource, &lp0);
	if (mdesc->reserve_lp1)
		request_resource(&ioport_resource, &lp1);
	if (mdesc->reserve_lp2)
		request_resource(&ioport_resource, &lp2);
}

#if defined(CONFIG_VGA_CONSOLE) || defined(CONFIG_DUMMY_CONSOLE) || \
    defined(CONFIG_EFI)
struct screen_info screen_info = {
 .orig_video_lines	= 30,
 .orig_video_cols	= 80,
 .orig_video_mode	= 0,
 .orig_video_ega_bx	= 0,
 .orig_video_isVGA	= 1,
 .orig_video_points	= 8
};
#endif

static int __init customize_machine(void)
{
	/*
	 * customizes platform devices, or adds new ones
	 * On DT based machines, we fall back to populating the
	 * machine from the device tree, if no callback is provided,
	 * otherwise we would always need an init_machine callback.
	 */
	if (machine_desc->init_machine)
		machine_desc->init_machine();

	return 0;
}
arch_initcall(customize_machine);

static int __init init_machine_late(void)
{
	struct device_node *root;
	int ret;

	if (machine_desc->init_late)
		machine_desc->init_late();

	root = of_find_node_by_path("/");
	if (root) {
		ret = of_property_read_string(root, "serial-number",
					      &system_serial);
		if (ret)
			system_serial = NULL;
	}

	if (!system_serial)
		system_serial = kasprintf(GFP_KERNEL, "%08x%08x",
					  system_serial_high,
					  system_serial_low);

	return 0;
}
late_initcall(init_machine_late);

#ifdef CONFIG_KEXEC
/*
 * The crash region must be aligned to 128MB to avoid
 * zImage relocating below the reserved region.
 */
#define CRASH_ALIGN	(128 << 20)

static inline unsigned long long get_total_mem(void)
{
	unsigned long total;

	total = max_low_pfn - min_low_pfn;
	return total << PAGE_SHIFT;
}

/**
 * reserve_crashkernel() - reserves memory are for crash kernel
 *
 * This function reserves memory area given in "crashkernel=" kernel command
 * line parameter. The memory reserved is used by a dump capture kernel when
 * primary kernel is crashing.
 */
static void __init reserve_crashkernel(void)
{
	unsigned long long crash_size, crash_base;
	unsigned long long total_mem;
	int ret;

	total_mem = get_total_mem();
	ret = parse_crashkernel(boot_command_line, total_mem,
				&crash_size, &crash_base);
	/* invalid value specified or crashkernel=0 */
	if (ret || !crash_size)
		return;

	if (crash_base <= 0) {
		unsigned long long crash_max = idmap_to_phys((u32)~0);
		unsigned long long lowmem_max = __pa(high_memory - 1) + 1;
		if (crash_max > lowmem_max)
			crash_max = lowmem_max;

		crash_base = memblock_phys_alloc_range(crash_size, CRASH_ALIGN,
						       CRASH_ALIGN, crash_max);
		if (!crash_base) {
			pr_err("crashkernel reservation failed - No suitable area found.\n");
			return;
		}
	} else {
		unsigned long long crash_max = crash_base + crash_size;
		unsigned long long start;

		start = memblock_phys_alloc_range(crash_size, SECTION_SIZE,
						  crash_base, crash_max);
		if (!start) {
			pr_err("crashkernel reservation failed - memory is in use.\n");
			return;
		}
	}

	pr_info("Reserving %ldMB of memory at %ldMB for crashkernel (System RAM: %ldMB)\n",
		(unsigned long)(crash_size >> 20),
		(unsigned long)(crash_base >> 20),
		(unsigned long)(total_mem >> 20));

	/* The crashk resource must always be located in normal mem */
	crashk_res.start = crash_base;
	crashk_res.end = crash_base + crash_size - 1;
	insert_resource(&iomem_resource, &crashk_res);

	if (arm_has_idmap_alias()) {
		/*
		 * If we have a special RAM alias for use at boot, we
		 * need to advertise to kexec tools where the alias is.
		 */
		static struct resource crashk_boot_res = {
			.name = "Crash kernel (boot alias)",
			.flags = IORESOURCE_BUSY | IORESOURCE_MEM,
		};

		crashk_boot_res.start = phys_to_idmap(crash_base);
		crashk_boot_res.end = crashk_boot_res.start + crash_size - 1;
		insert_resource(&iomem_resource, &crashk_boot_res);
	}
}
#else
static inline void reserve_crashkernel(void) {}
#endif /* CONFIG_KEXEC */

void __init hyp_mode_check(void)
{
#ifdef CONFIG_ARM_VIRT_EXT
	sync_boot_mode();

	if (is_hyp_mode_available()) {
		pr_info("CPU: All CPU(s) started in HYP mode.\n");
		pr_info("CPU: Virtualization extensions available.\n");
	} else if (is_hyp_mode_mismatched()) {
		pr_warn("CPU: WARNING: CPU(s) started in wrong/inconsistent modes (primary CPU mode 0x%x)\n",
			__boot_cpu_mode & MODE_MASK);
		pr_warn("CPU: This may indicate a broken bootloader or firmware.\n");
	} else
		pr_info("CPU: All CPU(s) started in SVC mode.\n");
#endif
}

static void (*__arm_pm_restart)(enum reboot_mode reboot_mode, const char *cmd);

static int arm_restart(struct notifier_block *nb, unsigned long action,
		       void *data)
{
	__arm_pm_restart(action, data);
	return NOTIFY_DONE;
}

static struct notifier_block arm_restart_nb = {
	.notifier_call = arm_restart,
	.priority = 128,
};

void __init setup_arch(char **cmdline_p)
{
	const struct machine_desc *mdesc = NULL;
	void *atags_vaddr = NULL;

	if (__atags_pointer)
		atags_vaddr = FDT_VIRT_BASE(__atags_pointer);

	setup_processor();
	if (atags_vaddr) {
		mdesc = setup_machine_fdt(atags_vaddr);
		if (mdesc)
			memblock_reserve(__atags_pointer,
					 fdt_totalsize(atags_vaddr));
	}
	if (!mdesc)
		mdesc = setup_machine_tags(atags_vaddr, __machine_arch_type);
	if (!mdesc) {
		early_print("\nError: invalid dtb and unrecognized/unsupported machine ID\n");
		early_print("  r1=0x%08x, r2=0x%08x\n", __machine_arch_type,
			    __atags_pointer);
		if (__atags_pointer)
			early_print("  r2[]=%*ph\n", 16, atags_vaddr);
		dump_machine_table();
	}

	machine_desc = mdesc;
	machine_name = mdesc->name;
	dump_stack_set_arch_desc("%s", mdesc->name);

	if (mdesc->reboot_mode != REBOOT_HARD)
		reboot_mode = mdesc->reboot_mode;

	setup_initial_init_mm(_text, _etext, _edata, _end);

	/* populate cmd_line too for later use, preserving boot_command_line */
	strlcpy(cmd_line, boot_command_line, COMMAND_LINE_SIZE);
	*cmdline_p = cmd_line;

	early_fixmap_init();
	early_ioremap_init();

	parse_early_param();

#ifdef CONFIG_MMU
	early_mm_init(mdesc);
#endif
	setup_dma_zone(mdesc);
	xen_early_init();
	arm_efi_init();
	/*
	 * Make sure the calculation for lowmem/highmem is set appropriately
	 * before reserving/allocating any memory
	 */
	adjust_lowmem_bounds();
	arm_memblock_init(mdesc);
	/* Memory may have been removed so recalculate the bounds. */
	adjust_lowmem_bounds();

	early_ioremap_reset();

	paging_init(mdesc);
	kasan_init();
	request_standard_resources(mdesc);

	if (mdesc->restart) {
		__arm_pm_restart = mdesc->restart;
		register_restart_handler(&arm_restart_nb);
	}

	unflatten_device_tree();

	arm_dt_init_cpu_maps();
	psci_dt_init();
#ifdef CONFIG_SMP
	if (is_smp()) {
		if (!mdesc->smp_init || !mdesc->smp_init()) {
			if (psci_smp_available())
				smp_set_ops(&psci_smp_ops);
			else if (mdesc->smp)
				smp_set_ops(mdesc->smp);
		}
		smp_init_cpus();
		smp_build_mpidr_hash();
	}
#endif

	if (!is_smp())
		hyp_mode_check();

	reserve_crashkernel();

#ifdef CONFIG_GENERIC_IRQ_MULTI_HANDLER
	handle_arch_irq = mdesc->handle_irq;
#endif

#ifdef CONFIG_VT
#if defined(CONFIG_VGA_CONSOLE)
	conswitchp = &vga_con;
#endif
#endif

	if (mdesc->init_early)
		mdesc->init_early();
}

static int __init topology_init(void)
{
	int cpu;

	for_each_possible_cpu(cpu) {
		struct cpuinfo_arm *cpuinfo = &per_cpu(cpu_data, cpu);
		cpuinfo->cpu.hotpluggable = platform_can_hotplug_cpu(cpu);
		register_cpu(&cpuinfo->cpu, cpu);
	}

	return 0;
}
subsys_initcall(topology_init);

#ifdef CONFIG_HAVE_PROC_CPU
static int __init proc_cpu_init(void)
{
	struct proc_dir_entry *res;

	res = proc_mkdir("cpu", NULL);
	if (!res)
		return -ENOMEM;
	return 0;
}
fs_initcall(proc_cpu_init);
#endif

static const char *hwcap_str[] = {
	"swp",
	"half",
	"thumb",
	"26bit",
	"fastmult",
	"fpa",
	"vfp",
	"edsp",
	"java",
	"iwmmxt",
	"crunch",
	"thumbee",
	"neon",
	"vfpv3",
	"vfpv3d16",
	"tls",
	"vfpv4",
	"idiva",
	"idivt",
	"vfpd32",
	"lpae",
	"evtstrm",
	"fphp",
	"asimdhp",
	"asimddp",
	"asimdfhm",
	"asimdbf16",
	"i8mm",
	NULL
};

static const char *hwcap2_str[] = {
	"aes",
	"pmull",
	"sha1",
	"sha2",
	"crc32",
	"sb",
	"ssbs",
	NULL
};

static int c_show(struct seq_file *m, void *v)
{
	int i, j;
	u32 cpuid;

	for_each_online_cpu(i) {
		/*
		 * glibc reads /proc/cpuinfo to determine the number of
		 * online processors, looking for lines beginning with
		 * "processor".  Give glibc what it expects.
		 */
		seq_printf(m, "processor\t: %d\n", i);
		cpuid = is_smp() ? per_cpu(cpu_data, i).cpuid : read_cpuid_id();
		seq_printf(m, "model name\t: %s rev %d (%s)\n",
			   cpu_name, cpuid & 15, elf_platform);

#if defined(CONFIG_SMP)
		seq_printf(m, "BogoMIPS\t: %lu.%02lu\n",
			   per_cpu(cpu_data, i).loops_per_jiffy / (500000UL/HZ),
			   (per_cpu(cpu_data, i).loops_per_jiffy / (5000UL/HZ)) % 100);
#else
		seq_printf(m, "BogoMIPS\t: %lu.%02lu\n",
			   loops_per_jiffy / (500000/HZ),
			   (loops_per_jiffy / (5000/HZ)) % 100);
#endif
		/* dump out the processor features */
		seq_puts(m, "Features\t: ");

		for (j = 0; hwcap_str[j]; j++)
			if (elf_hwcap & (1 << j))
				seq_printf(m, "%s ", hwcap_str[j]);

		for (j = 0; hwcap2_str[j]; j++)
			if (elf_hwcap2 & (1 << j))
				seq_printf(m, "%s ", hwcap2_str[j]);

		seq_printf(m, "\nCPU implementer\t: 0x%02x\n", cpuid >> 24);
		seq_printf(m, "CPU architecture: %s\n",
			   proc_arch[cpu_architecture()]);

		if ((cpuid & 0x0008f000) == 0x00000000) {
			/* pre-ARM7 */
			seq_printf(m, "CPU part\t: %07x\n", cpuid >> 4);
		} else {
			if ((cpuid & 0x0008f000) == 0x00007000) {
				/* ARM7 */
				seq_printf(m, "CPU variant\t: 0x%02x\n",
					   (cpuid >> 16) & 127);
			} else {
				/* post-ARM7 */
				seq_printf(m, "CPU variant\t: 0x%x\n",
					   (cpuid >> 20) & 15);
			}
			seq_printf(m, "CPU part\t: 0x%03x\n",
				   (cpuid >> 4) & 0xfff);
		}
		seq_printf(m, "CPU revision\t: %d\n\n", cpuid & 15);
	}

	seq_printf(m, "Hardware\t: %s\n", machine_name);
	seq_printf(m, "Revision\t: %04x\n", system_rev);
	seq_printf(m, "Serial\t\t: %s\n", system_serial);

	return 0;
}

static void *c_start(struct seq_file *m, loff_t *pos)
{
	return *pos < 1 ? (void *)1 : NULL;
}

static void *c_next(struct seq_file *m, void *v, loff_t *pos)
{
	++*pos;
	return NULL;
}

static void c_stop(struct seq_file *m, void *v)
{
}

const struct seq_operations cpuinfo_op = {
	.start	= c_start,
	.next	= c_next,
	.stop	= c_stop,
	.show	= c_show
};


code:
// SPDX-License-Identifier: GPL-2.0-only
/*
 *  linux/arch/arm/kernel/signal.c
 *
 *  Copyright (C) 1995-2009 Russell King
 */
#include <linux/errno.h>
#include <linux/random.h>
#include <linux/signal.h>
#include <linux/personality.h>
#include <linux/uaccess.h>
#include <linux/resume_user_mode.h>
#include <linux/uprobes.h>
#include <linux/syscalls.h>

#include <asm/elf.h>
#include <asm/cacheflush.h>
#include <asm/traps.h>
#include <asm/unistd.h>
#include <asm/vfp.h>

#include "signal.h"

extern const unsigned long sigreturn_codes[17];

static unsigned long signal_return_offset;

#ifdef CONFIG_IWMMXT

static int preserve_iwmmxt_context(struct iwmmxt_sigframe __user *frame)
{
	char kbuf[sizeof(*frame) + 8];
	struct iwmmxt_sigframe *kframe;
	int err = 0;

	/* the iWMMXt context must be 64 bit aligned */
	kframe = (struct iwmmxt_sigframe *)((unsigned long)(kbuf + 8) & ~7);

	if (test_thread_flag(TIF_USING_IWMMXT)) {
		kframe->magic = IWMMXT_MAGIC;
		kframe->size = IWMMXT_STORAGE_SIZE;
		iwmmxt_task_copy(current_thread_info(), &kframe->storage);
	} else {
		/*
		 * For bug-compatibility with older kernels, some space
		 * has to be reserved for iWMMXt even if it's not used.
		 * Set the magic and size appropriately so that properly
		 * written userspace can skip it reliably:
		 */
		*kframe = (struct iwmmxt_sigframe) {
			.magic = DUMMY_MAGIC,
			.size  = IWMMXT_STORAGE_SIZE,
		};
	}

	err = __copy_to_user(frame, kframe, sizeof(*kframe));

	return err;
}

static int restore_iwmmxt_context(char __user **auxp)
{
	struct iwmmxt_sigframe __user *frame =
		(struct iwmmxt_sigframe __user *)*auxp;
	char kbuf[sizeof(*frame) + 8];
	struct iwmmxt_sigframe *kframe;

	/* the iWMMXt context must be 64 bit aligned */
	kframe = (struct iwmmxt_sigframe *)((unsigned long)(kbuf + 8) & ~7);
	if (__copy_from_user(kframe, frame, sizeof(*frame)))
		return -1;

	/*
	 * For non-iWMMXt threads: a single iwmmxt_sigframe-sized dummy
	 * block is discarded for compatibility with setup_sigframe() if
	 * present, but we don't mandate its presence.  If some other
	 * magic is here, it's not for us:
	 */
	if (!test_thread_flag(TIF_USING_IWMMXT) &&
	    kframe->magic != DUMMY_MAGIC)
		return 0;

	if (kframe->size != IWMMXT_STORAGE_SIZE)
		return -1;

	if (test_thread_flag(TIF_USING_IWMMXT)) {
		if (kframe->magic != IWMMXT_MAGIC)
			return -1;

		iwmmxt_task_restore(current_thread_info(), &kframe->storage);
	}

	*auxp += IWMMXT_STORAGE_SIZE;
	return 0;
}

#endif

#ifdef CONFIG_VFP

static int preserve_vfp_context(struct vfp_sigframe __user *frame)
{
	struct vfp_sigframe kframe;
	int err = 0;

	memset(&kframe, 0, sizeof(kframe));
	kframe.magic = VFP_MAGIC;
	kframe.size = VFP_STORAGE_SIZE;

	err = vfp_preserve_user_clear_hwstate(&kframe.ufp, &kframe.ufp_exc);
	if (err)
		return err;

	return __copy_to_user(frame, &kframe, sizeof(kframe));
}

static int restore_vfp_context(char __user **auxp)
{
	struct vfp_sigframe frame;
	int err;

	err = __copy_from_user(&frame, *auxp, sizeof(frame));
	if (err)
		return err;

	if (frame.magic != VFP_MAGIC || frame.size != VFP_STORAGE_SIZE)
		return -EINVAL;

	*auxp += sizeof(frame);
	return vfp_restore_user_hwstate(&frame.ufp, &frame.ufp_exc);
}

#endif

/*
 * Do a signal return; undo the signal stack.  These are aligned to 64-bit.
 */

static int restore_sigframe(struct pt_regs *regs, struct sigframe __user *sf)
{
	struct sigcontext context;
	char __user *aux;
	sigset_t set;
	int err;

	err = __copy_from_user(&set, &sf->uc.uc_sigmask, sizeof(set));
	if (err == 0)
		set_current_blocked(&set);

	err |= __copy_from_user(&context, &sf->uc.uc_mcontext, sizeof(context));
	if (err == 0) {
		regs->ARM_r0 = context.arm_r0;
		regs->ARM_r1 = context.arm_r1;
		regs->ARM_r2 = context.arm_r2;
		regs->ARM_r3 = context.arm_r3;
		regs->ARM_r4 = context.arm_r4;
		regs->ARM_r5 = context.arm_r5;
		regs->ARM_r6 = context.arm_r6;
		regs->ARM_r7 = context.arm_r7;
		regs->ARM_r8 = context.arm_r8;
		regs->ARM_r9 = context.arm_r9;
		regs->ARM_r10 = context.arm_r10;
		regs->ARM_fp = context.arm_fp;
		regs->ARM_ip = context.arm_ip;
		regs->ARM_sp = context.arm_sp;
		regs->ARM_lr = context.arm_lr;
		regs->ARM_pc = context.arm_pc;
		regs->ARM_cpsr = context.arm_cpsr;
	}

	err |= !valid_user_regs(regs);

	aux = (char __user *) sf->uc.uc_regspace;
#ifdef CONFIG_IWMMXT
	if (err == 0)
		err |= restore_iwmmxt_context(&aux);
#endif
#ifdef CONFIG_VFP
	if (err == 0)
		err |= restore_vfp_context(&aux);
#endif

	return err;
}

asmlinkage int sys_sigreturn(struct pt_regs *regs)
{
	struct sigframe __user *frame;

	/* Always make any pending restarted system calls return -EINTR */
	current->restart_block.fn = do_no_restart_syscall;

	/*
	 * Since we stacked the signal on a 64-bit boundary,
	 * then 'sp' should be word aligned here.  If it's
	 * not, then the user is trying to mess with us.
	 */
	if (regs->ARM_sp & 7)
		goto badframe;

	frame = (struct sigframe __user *)regs->ARM_sp;

	if (!access_ok(frame, sizeof (*frame)))
		goto badframe;

	if (restore_sigframe(regs, frame))
		goto badframe;

	return regs->ARM_r0;

badframe:
	force_sig(SIGSEGV);
	return 0;
}

asmlinkage int sys_rt_sigreturn(struct pt_regs *regs)
{
	struct rt_sigframe __user *frame;

	/* Always make any pending restarted system calls return -EINTR */
	current->restart_block.fn = do_no_restart_syscall;

	/*
	 * Since we stacked the signal on a 64-bit boundary,
	 * then 'sp' should be word aligned here.  If it's
	 * not, then the user is trying to mess with us.
	 */
	if (regs->ARM_sp & 7)
		goto badframe;

	frame = (struct rt_sigframe __user *)regs->ARM_sp;

	if (!access_ok(frame, sizeof (*frame)))
		goto badframe;

	if (restore_sigframe(regs, &frame->sig))
		goto badframe;

	if (restore_altstack(&frame->sig.uc.uc_stack))
		goto badframe;

	return regs->ARM_r0;

badframe:
	force_sig(SIGSEGV);
	return 0;
}

static int
setup_sigframe(struct sigframe __user *sf, struct pt_regs *regs, sigset_t *set)
{
	struct aux_sigframe __user *aux;
	struct sigcontext context;
	int err = 0;

	context = (struct sigcontext) {
		.arm_r0        = regs->ARM_r0,
		.arm_r1        = regs->ARM_r1,
		.arm_r2        = regs->ARM_r2,
		.arm_r3        = regs->ARM_r3,
		.arm_r4        = regs->ARM_r4,
		.arm_r5        = regs->ARM_r5,
		.arm_r6        = regs->ARM_r6,
		.arm_r7        = regs->ARM_r7,
		.arm_r8        = regs->ARM_r8,
		.arm_r9        = regs->ARM_r9,
		.arm_r10       = regs->ARM_r10,
		.arm_fp        = regs->ARM_fp,
		.arm_ip        = regs->ARM_ip,
		.arm_sp        = regs->ARM_sp,
		.arm_lr        = regs->ARM_lr,
		.arm_pc        = regs->ARM_pc,
		.arm_cpsr      = regs->ARM_cpsr,

		.trap_no       = current->thread.trap_no,
		.error_code    = current->thread.error_code,
		.fault_address = current->thread.address,
		.oldmask       = set->sig[0],
	};

	err |= __copy_to_user(&sf->uc.uc_mcontext, &context, sizeof(context));

	err |= __copy_to_user(&sf->uc.uc_sigmask, set, sizeof(*set));

	aux = (struct aux_sigframe __user *) sf->uc.uc_regspace;
#ifdef CONFIG_IWMMXT
	if (err == 0)
		err |= preserve_iwmmxt_context(&aux->iwmmxt);
#endif
#ifdef CONFIG_VFP
	if (err == 0)
		err |= preserve_vfp_context(&aux->vfp);
#endif
	err |= __put_user(0, &aux->end_magic);

	return err;
}

static inline void __user *
get_sigframe(struct ksignal *ksig, struct pt_regs *regs, int framesize)
{
	unsigned long sp = sigsp(regs->ARM_sp, ksig);
	void __user *frame;

	/*
	 * ATPCS B01 mandates 8-byte alignment
	 */
	frame = (void __user *)((sp - framesize) & ~7);

	/*
	 * Check that we can actually write to the signal frame.
	 */
	if (!access_ok(frame, framesize))
		frame = NULL;

	return frame;
}

static int
setup_return(struct pt_regs *regs, struct ksignal *ksig,
	     unsigned long __user *rc, void __user *frame)
{
	unsigned long handler = (unsigned long)ksig->ka.sa.sa_handler;
	unsigned long handler_fdpic_GOT = 0;
	unsigned long retcode;
	unsigned int idx, thumb = 0;
	unsigned long cpsr = regs->ARM_cpsr & ~(PSR_f | PSR_E_BIT);
	bool fdpic = IS_ENABLED(CONFIG_BINFMT_ELF_FDPIC) &&
		     (current->personality & FDPIC_FUNCPTRS);

	if (fdpic) {
		unsigned long __user *fdpic_func_desc =
					(unsigned long __user *)handler;
		if (__get_user(handler, &fdpic_func_desc[0]) ||
		    __get_user(handler_fdpic_GOT, &fdpic_func_desc[1]))
			return 1;
	}

	cpsr |= PSR_ENDSTATE;

	/*
	 * Maybe we need to deliver a 32-bit signal to a 26-bit task.
	 */
	if (ksig->ka.sa.sa_flags & SA_THIRTYTWO)
		cpsr = (cpsr & ~MODE_MASK) | USR_MODE;

#ifdef CONFIG_ARM_THUMB
	if (elf_hwcap & HWCAP_THUMB) {
		/*
		 * The LSB of the handler determines if we're going to
		 * be using THUMB or ARM mode for this signal handler.
		 */
		thumb = handler & 1;

		/*
		 * Clear the If-Then Thumb-2 execution state.  ARM spec
		 * requires this to be all 000s in ARM mode.  Snapdragon
		 * S4/Krait misbehaves on a Thumb=>ARM signal transition
		 * without this.
		 *
		 * We must do this whenever we are running on a Thumb-2
		 * capable CPU, which includes ARMv6T2.  However, we elect
		 * to always do this to simplify the code; this field is
		 * marked UNK/SBZP for older architectures.
		 */
		cpsr &= ~PSR_IT_MASK;

		if (thumb) {
			cpsr |= PSR_T_BIT;
		} else
			cpsr &= ~PSR_T_BIT;
	}
#endif

	if (ksig->ka.sa.sa_flags & SA_RESTORER) {
		retcode = (unsigned long)ksig->ka.sa.sa_restorer;
		if (fdpic) {
			/*
			 * We need code to load the function descriptor.
			 * That code follows the standard sigreturn code
			 * (6 words), and is made of 3 + 2 words for each
			 * variant. The 4th copied word is the actual FD
			 * address that the assembly code expects.
			 */
			idx = 6 + thumb * 3;
			if (ksig->ka.sa.sa_flags & SA_SIGINFO)
				idx += 5;
			if (__put_user(sigreturn_codes[idx],   rc  ) ||
			    __put_user(sigreturn_codes[idx+1], rc+1) ||
			    __put_user(sigreturn_codes[idx+2], rc+2) ||
			    __put_user(retcode,                rc+3))
				return 1;
			goto rc_finish;
		}
	} else {
		idx = thumb << 1;
		if (ksig->ka.sa.sa_flags & SA_SIGINFO)
			idx += 3;

		/*
		 * Put the sigreturn code on the stack no matter which return
		 * mechanism we use in order to remain ABI compliant
		 */
		if (__put_user(sigreturn_codes[idx],   rc) ||
		    __put_user(sigreturn_codes[idx+1], rc+1))
			return 1;

rc_finish:
#ifdef CONFIG_MMU
		if (cpsr & MODE32_BIT) {
			struct mm_struct *mm = current->mm;

			/*
			 * 32-bit code can use the signal return page
			 * except when the MPU has protected the vectors
			 * page from PL0
			 */
			retcode = mm->context.sigpage + signal_return_offset +
				  (idx << 2) + thumb;
		} else
#endif
		{
			/*
			 * Ensure that the instruction cache sees
			 * the return code written onto the stack.
			 */
			flush_icache_range((unsigned long)rc,
					   (unsigned long)(rc + 3));

			retcode = ((unsigned long)rc) + thumb;
		}
	}

	regs->ARM_r0 = ksig->sig;
	regs->ARM_sp = (unsigned long)frame;
	regs->ARM_lr = retcode;
	regs->ARM_pc = handler;
	if (fdpic)
		regs->ARM_r9 = handler_fdpic_GOT;
	regs->ARM_cpsr = cpsr;

	return 0;
}

static int
setup_frame(struct ksignal *ksig, sigset_t *set, struct pt_regs *regs)
{
	struct sigframe __user *frame = get_sigframe(ksig, regs, sizeof(*frame));
	int err = 0;

	if (!frame)
		return 1;

	/*
	 * Set uc.uc_flags to a value which sc.trap_no would never have.
	 */
	err = __put_user(0x5ac3c35a, &frame->uc.uc_flags);

	err |= setup_sigframe(frame, regs, set);
	if (err == 0)
		err = setup_return(regs, ksig, frame->retcode, frame);

	return err;
}

static int
setup_rt_frame(struct ksignal *ksig, sigset_t *set, struct pt_regs *regs)
{
	struct rt_sigframe __user *frame = get_sigframe(ksig, regs, sizeof(*frame));
	int err = 0;

	if (!frame)
		return 1;

	err |= copy_siginfo_to_user(&frame->info, &ksig->info);

	err |= __put_user(0, &frame->sig.uc.uc_flags);
	err |= __put_user(NULL, &frame->sig.uc.uc_link);

	err |= __save_altstack(&frame->sig.uc.uc_stack, regs->ARM_sp);
	err |= setup_sigframe(&frame->sig, regs, set);
	if (err == 0)
		err = setup_return(regs, ksig, frame->sig.retcode, frame);

	if (err == 0) {
		/*
		 * For realtime signals we must also set the second and third
		 * arguments for the signal handler.
		 *   -- Peter Maydell <pmaydell@chiark.greenend.org.uk> 2000-12-06
		 */
		regs->ARM_r1 = (unsigned long)&frame->info;
		regs->ARM_r2 = (unsigned long)&frame->sig.uc;
	}

	return err;
}

/*
 * OK, we're invoking a handler
 */
static void handle_signal(struct ksignal *ksig, struct pt_regs *regs)
{
	sigset_t *oldset = sigmask_to_save();
	int ret;

	/*
	 * Perform fixup for the pre-signal frame.
	 */
	rseq_signal_deliver(ksig, regs);

	/*
	 * Set up the stack frame
	 */
	if (ksig->ka.sa.sa_flags & SA_SIGINFO)
		ret = setup_rt_frame(ksig, oldset, regs);
	else
		ret = setup_frame(ksig, oldset, regs);

	/*
	 * Check that the resulting registers are actually sane.
	 */
	ret |= !valid_user_regs(regs);

	signal_setup_done(ret, ksig, 0);
}

/*
 * Note that 'init' is a special process: it doesn't get signals it doesn't
 * want to handle. Thus you cannot kill init even with a SIGKILL even by
 * mistake.
 *
 * Note that we go through the signals twice: once to check the signals that
 * the kernel can handle, and then we build all the user-level signal handling
 * stack-frames in one go after that.
 */
static int do_signal(struct pt_regs *regs, int syscall)
{
	unsigned int retval = 0, continue_addr = 0, restart_addr = 0;
	struct ksignal ksig;
	int restart = 0;

	/*
	 * If we were from a system call, check for system call restarting...
	 */
	if (syscall) {
		continue_addr = regs->ARM_pc;
		restart_addr = continue_addr - (thumb_mode(regs) ? 2 : 4);
		retval = regs->ARM_r0;

		/*
		 * Prepare for system call restart.  We do this here so that a
		 * debugger will see the already changed PSW.
		 */
		switch (retval) {
		case -ERESTART_RESTARTBLOCK:
			restart -= 2;
			fallthrough;
		case -ERESTARTNOHAND:
		case -ERESTARTSYS:
		case -ERESTARTNOINTR:
			restart++;
			regs->ARM_r0 = regs->ARM_ORIG_r0;
			regs->ARM_pc = restart_addr;
			break;
		}
	}

	/*
	 * Get the signal to deliver.  When running under ptrace, at this
	 * point the debugger may change all our registers ...
	 */
	/*
	 * Depending on the signal settings we may need to revert the
	 * decision to restart the system call.  But skip this if a
	 * debugger has chosen to restart at a different PC.
	 */
	if (get_signal(&ksig)) {
		/* handler */
		if (unlikely(restart) && regs->ARM_pc == restart_addr) {
			if (retval == -ERESTARTNOHAND ||
			    retval == -ERESTART_RESTARTBLOCK
			    || (retval == -ERESTARTSYS
				&& !(ksig.ka.sa.sa_flags & SA_RESTART))) {
				regs->ARM_r0 = -EINTR;
				regs->ARM_pc = continue_addr;
			}
		}
		handle_signal(&ksig, regs);
	} else {
		/* no handler */
		restore_saved_sigmask();
		if (unlikely(restart) && regs->ARM_pc == restart_addr) {
			regs->ARM_pc = continue_addr;
			return restart;
		}
	}
	return 0;
}

asmlinkage int
do_work_pending(struct pt_regs *regs, unsigned int thread_flags, int syscall)
{
	/*
	 * The assembly code enters us with IRQs off, but it hasn't
	 * informed the tracing code of that for efficiency reasons.
	 * Update the trace code with the current status.
	 */
	trace_hardirqs_off();
	do {
		if (likely(thread_flags & _TIF_NEED_RESCHED)) {
			schedule();
		} else {
			if (unlikely(!user_mode(regs)))
				return 0;
			local_irq_enable();
			if (thread_flags & (_TIF_SIGPENDING | _TIF_NOTIFY_SIGNAL)) {
				int restart = do_signal(regs, syscall);
				if (unlikely(restart)) {
					/*
					 * Restart without handlers.
					 * Deal with it without leaving
					 * the kernel space.
					 */
					return restart;
				}
				syscall = 0;
			} else if (thread_flags & _TIF_UPROBE) {
				uprobe_notify_resume(regs);
			} else {
				resume_user_mode_work(regs);
			}
		}
		local_irq_disable();
		thread_flags = read_thread_flags();
	} while (thread_flags & _TIF_WORK_MASK);
	return 0;
}

struct page *get_signal_page(void)
{
	unsigned long ptr;
	unsigned offset;
	struct page *page;
	void *addr;

	page = alloc_pages(GFP_KERNEL, 0);

	if (!page)
		return NULL;

	addr = page_address(page);

	/* Poison the entire page */
	memset32(addr, __opcode_to_mem_arm(0xe7fddef1),
		 PAGE_SIZE / sizeof(u32));

	/* Give the signal return code some randomness */
	offset = 0x200 + (get_random_u16() & 0x7fc);
	signal_return_offset = offset;

	/* Copy signal return handlers into the page */
	memcpy(addr + offset, sigreturn_codes, sizeof(sigreturn_codes));

	/* Flush out all instructions in this page */
	ptr = (unsigned long)addr;
	flush_icache_range(ptr, ptr + PAGE_SIZE);

	return page;
}

#ifdef CONFIG_DEBUG_RSEQ
asmlinkage void do_rseq_syscall(struct pt_regs *regs)
{
	rseq_syscall(regs);
}
#endif

/*
 * Compile-time assertions for siginfo_t offsets. Check NSIG* as well, as
 * changes likely come with new fields that should be added below.
 */
static_assert(NSIGILL	== 11);
static_assert(NSIGFPE	== 15);
static_assert(NSIGSEGV	== 9);
static_assert(NSIGBUS	== 5);
static_assert(NSIGTRAP	== 6);
static_assert(NSIGCHLD	== 6);
static_assert(NSIGSYS	== 2);
static_assert(sizeof(siginfo_t) == 128);
static_assert(__alignof__(siginfo_t) == 4);
static_assert(offsetof(siginfo_t, si_signo)	== 0x00);
static_assert(offsetof(siginfo_t, si_errno)	== 0x04);
static_assert(offsetof(siginfo_t, si_code)	== 0x08);
static_assert(offsetof(siginfo_t, si_pid)	== 0x0c);
static_assert(offsetof(siginfo_t, si_uid)	== 0x10);
static_assert(offsetof(siginfo_t, si_tid)	== 0x0c);
static_assert(offsetof(siginfo_t, si_overrun)	== 0x10);
static_assert(offsetof(siginfo_t, si_status)	== 0x14);
static_assert(offsetof(siginfo_t, si_utime)	== 0x18);
static_assert(offsetof(siginfo_t, si_stime)	== 0x1c);
static_assert(offsetof(siginfo_t, si_value)	== 0x14);
static_assert(offsetof(siginfo_t, si_int)	== 0x14);
static_assert(offsetof(siginfo_t, si_ptr)	== 0x14);
static_assert(offsetof(siginfo_t, si_addr)	== 0x0c);
static_assert(offsetof(siginfo_t, si_addr_lsb)	== 0x10);
static_assert(offsetof(siginfo_t, si_lower)	== 0x14);
static_assert(offsetof(siginfo_t, si_upper)	== 0x18);
static_assert(offsetof(siginfo_t, si_pkey)	== 0x14);
static_assert(offsetof(siginfo_t, si_perf_data)	== 0x10);
static_assert(offsetof(siginfo_t, si_perf_type)	== 0x14);
static_assert(offsetof(siginfo_t, si_perf_flags) == 0x18);
static_assert(offsetof(siginfo_t, si_band)	== 0x0c);
static_assert(offsetof(siginfo_t, si_fd)	== 0x10);
static_assert(offsetof(siginfo_t, si_call_addr)	== 0x0c);
static_assert(offsetof(siginfo_t, si_syscall)	== 0x10);
static_assert(offsetof(siginfo_t, si_arch)	== 0x14);

repo_name: angery/linux


code:
#include <asm/ucontext.h>

struct sigframe {
	struct ucontext uc;
	unsigned long retcode[4];
};

struct rt_sigframe {
	struct siginfo info;
	struct sigframe sig;
};

extern struct page *get_signal_page(void);


code:
/* SPDX-License-Identifier: GPL-2.0-only */
/*
 * sigreturn_codes.S - code sinpets for sigreturn syscalls
 *
 * Created by:	Victor Kamensky, 2013-08-13
 * Copyright:	(C) 2013  Linaro Limited
 */

#include <asm/assembler.h>
#include <asm/asm-offsets.h>
#include <asm/unistd.h>

/*
 * For ARM syscalls, we encode the syscall number into the instruction.
 * With EABI, the syscall number has to be loaded into r7. As result
 * ARM syscall sequence snippet will have move and svc in .arm encoding
 *
 * For Thumb syscalls, we pass the syscall number via r7.  We therefore
 * need two 16-bit instructions in .thumb encoding
 *
 * Please note sigreturn_codes code are not executed in place. Instead
 * they just copied by kernel into appropriate places. Code inside of
 * arch/arm/kernel/signal.c is very sensitive to layout of these code
 * snippets.
 */

/*
 * In CPU_THUMBONLY case kernel arm opcodes are not allowed.
 * Note in this case codes skips those instructions but it uses .org
 * directive to keep correct layout of sigreturn_codes array.
 */
#ifndef CONFIG_CPU_THUMBONLY
#define ARM_OK(code...)	code
#else
#define ARM_OK(code...)
#endif

	.macro arm_slot n
	.org	sigreturn_codes + 12 * (\n)
ARM_OK(	.arm	)
	.endm

	.macro thumb_slot n
	.org	sigreturn_codes + 12 * (\n) + 8
	.thumb
	.endm

	.macro arm_fdpic_slot n
	.org	sigreturn_codes + 24 + 20 * (\n)
ARM_OK(	.arm	)
	.endm

	.macro thumb_fdpic_slot n
	.org	sigreturn_codes + 24 + 20 * (\n) + 12
	.thumb
	.endm

#if __LINUX_ARM_ARCH__ <= 4
	/*
	 * Note we manually set minimally required arch that supports
	 * required thumb opcodes for early arch versions. It is OK
	 * for this file to be used in combination with other
	 * lower arch variants, since these code snippets are only
	 * used as input data.
	 */
	.arch armv4t
#endif

	.section .rodata
	.global sigreturn_codes
	.type	sigreturn_codes, #object

	.align

sigreturn_codes:

	/* ARM sigreturn syscall code snippet */
	arm_slot 0
ARM_OK(	mov	r7, #(__NR_sigreturn - __NR_SYSCALL_BASE)	)
ARM_OK(	swi	#(__NR_sigreturn)|(__NR_OABI_SYSCALL_BASE)	)

	/* Thumb sigreturn syscall code snippet */
	thumb_slot 0
	movs	r7, #(__NR_sigreturn - __NR_SYSCALL_BASE)
	swi	#0

	/* ARM sigreturn_rt syscall code snippet */
	arm_slot 1
ARM_OK(	mov	r7, #(__NR_rt_sigreturn - __NR_SYSCALL_BASE)	)
ARM_OK(	swi	#(__NR_rt_sigreturn)|(__NR_OABI_SYSCALL_BASE)	)

	/* Thumb sigreturn_rt syscall code snippet */
	thumb_slot 1
	movs	r7, #(__NR_rt_sigreturn - __NR_SYSCALL_BASE)
	swi	#0

	/* ARM sigreturn restorer FDPIC bounce code snippet */
	arm_fdpic_slot 0
ARM_OK(	ldr	r3, [sp, #SIGFRAME_RC3_OFFSET] )
ARM_OK(	ldmia	r3, {r3, r9} )
#ifdef CONFIG_ARM_THUMB
ARM_OK(	bx	r3 )
#else
ARM_OK(	ret	r3 )
#endif

	/* Thumb sigreturn restorer FDPIC bounce code snippet */
	thumb_fdpic_slot 0
	ldr	r3, [sp, #SIGFRAME_RC3_OFFSET]
	ldmia	r3, {r2, r3}
	mov	r9, r3
	bx	r2

	/* ARM sigreturn_rt restorer FDPIC bounce code snippet */
	arm_fdpic_slot 1
ARM_OK(	ldr	r3, [sp, #RT_SIGFRAME_RC3_OFFSET] )
ARM_OK(	ldmia	r3, {r3, r9} )
#ifdef CONFIG_ARM_THUMB
ARM_OK(	bx	r3 )
#else
ARM_OK(	ret	r3 )
#endif

	/* Thumb sigreturn_rt restorer FDPIC bounce code snippet */
	thumb_fdpic_slot 1
	ldr	r3, [sp, #RT_SIGFRAME_RC3_OFFSET]
	ldmia	r3, {r2, r3}
	mov	r9, r3
	bx	r2

	/*
	 * Note on additional space: setup_return in signal.c
	 * always copies the same number of words regardless whether
	 * it is thumb case or not, so we need one additional padding
	 * word after the last entry.
	 */
	.space	4

	.size	sigreturn_codes, . - sigreturn_codes


code:
/* SPDX-License-Identifier: GPL-2.0 */
#include <linux/linkage.h>
#include <linux/threads.h>
#include <asm/asm-offsets.h>
#include <asm/assembler.h>
#include <asm/glue-cache.h>
#include <asm/glue-proc.h>
	.text

/*
 * Implementation of MPIDR hash algorithm through shifting
 * and OR'ing.
 *
 * @dst: register containing hash result
 * @rs0: register containing affinity level 0 bit shift
 * @rs1: register containing affinity level 1 bit shift
 * @rs2: register containing affinity level 2 bit shift
 * @mpidr: register containing MPIDR value
 * @mask: register containing MPIDR mask
 *
 * Pseudo C-code:
 *
 *u32 dst;
 *
 *compute_mpidr_hash(u32 rs0, u32 rs1, u32 rs2, u32 mpidr, u32 mask) {
 *	u32 aff0, aff1, aff2;
 *	u32 mpidr_masked = mpidr & mask;
 *	aff0 = mpidr_masked & 0xff;
 *	aff1 = mpidr_masked & 0xff00;
 *	aff2 = mpidr_masked & 0xff0000;
 *	dst = (aff0 >> rs0 | aff1 >> rs1 | aff2 >> rs2);
 *}
 * Input registers: rs0, rs1, rs2, mpidr, mask
 * Output register: dst
 * Note: input and output registers must be disjoint register sets
         (eg: a macro instance with mpidr = r1 and dst = r1 is invalid)
 */
	.macro compute_mpidr_hash dst, rs0, rs1, rs2, mpidr, mask
	and	\mpidr, \mpidr, \mask			@ mask out MPIDR bits
	and	\dst, \mpidr, #0xff			@ mask=aff0
 ARM(	mov	\dst, \dst, lsr \rs0		)	@ dst=aff0>>rs0
 THUMB(	lsr	\dst, \dst, \rs0		)
	and	\mask, \mpidr, #0xff00			@ mask = aff1
 ARM(	orr	\dst, \dst, \mask, lsr \rs1	)	@ dst|=(aff1>>rs1)
 THUMB(	lsr	\mask, \mask, \rs1		)
 THUMB(	orr	\dst, \dst, \mask		)
	and	\mask, \mpidr, #0xff0000		@ mask = aff2
 ARM(	orr	\dst, \dst, \mask, lsr \rs2	)	@ dst|=(aff2>>rs2)
 THUMB(	lsr	\mask, \mask, \rs2		)
 THUMB(	orr	\dst, \dst, \mask		)
	.endm

/*
 * Save CPU state for a suspend.  This saves the CPU general purpose
 * registers, and allocates space on the kernel stack to save the CPU
 * specific registers and some other data for resume.
 *  r0 = suspend function arg0
 *  r1 = suspend function
 *  r2 = MPIDR value the resuming CPU will use
 */
ENTRY(__cpu_suspend)
	stmfd	sp!, {r4 - r11, lr}
#ifdef MULTI_CPU
	ldr	r10, =processor
	ldr	r4, [r10, #CPU_SLEEP_SIZE] @ size of CPU sleep state
#else
	ldr	r4, =cpu_suspend_size
#endif
	mov	r5, sp			@ current virtual SP
#ifdef CONFIG_VMAP_STACK
	@ Run the suspend code from the overflow stack so we don't have to rely
	@ on vmalloc-to-phys conversions anywhere in the arch suspend code.
	@ The original SP value captured in R5 will be restored on the way out.
	ldr_this_cpu sp, overflow_stack_ptr, r6, r7
#endif
	add	r4, r4, #12		@ Space for pgd, virt sp, phys resume fn
	sub	sp, sp, r4		@ allocate CPU state on stack
	ldr	r3, =sleep_save_sp
	stmfd	sp!, {r0, r1}		@ save suspend func arg and pointer
	ldr	r3, [r3, #SLEEP_SAVE_SP_VIRT]
	ALT_SMP(W(nop))			@ don't use adr_l inside ALT_SMP()
	ALT_UP_B(1f)
	adr_l	r0, mpidr_hash
	/* This ldmia relies on the memory layout of the mpidr_hash struct */
	ldmia	r0, {r1, r6-r8}	@ r1 = mpidr mask (r6,r7,r8) = l[0,1,2] shifts
	compute_mpidr_hash	r0, r6, r7, r8, r2, r1
	add	r3, r3, r0, lsl #2
1:	mov	r2, r5			@ virtual SP
	mov	r1, r4			@ size of save block
	add	r0, sp, #8		@ pointer to save block
	bl	__cpu_suspend_save
	badr	lr, cpu_suspend_abort
	ldmfd	sp!, {r0, pc}		@ call suspend fn
ENDPROC(__cpu_suspend)
	.ltorg

cpu_suspend_abort:
	ldmia	sp!, {r1 - r3}		@ pop phys pgd, virt SP, phys resume fn
	teq	r0, #0
	moveq	r0, #1			@ force non-zero value
	mov	sp, r2
	ldmfd	sp!, {r4 - r11, pc}
ENDPROC(cpu_suspend_abort)

/*
 * r0 = control register value
 */
	.align	5
	.pushsection	.idmap.text,"ax"
ENTRY(cpu_resume_mmu)
	ldr	r3, =cpu_resume_after_mmu
	instr_sync
	mcr	p15, 0, r0, c1, c0, 0	@ turn on MMU, I-cache, etc
	mrc	p15, 0, r0, c0, c0, 0	@ read id reg
	instr_sync
	mov	r0, r0
	mov	r0, r0
	ret	r3			@ jump to virtual address
ENDPROC(cpu_resume_mmu)
	.popsection
cpu_resume_after_mmu:
#if defined(CONFIG_VMAP_STACK) && !defined(CONFIG_ARM_LPAE)
	@ Before using the vmap'ed stack, we have to switch to swapper_pg_dir
	@ as the ID map does not cover the vmalloc region.
	mrc	p15, 0, ip, c2, c0, 1	@ read TTBR1
	mcr	p15, 0, ip, c2, c0, 0	@ set TTBR0
	instr_sync
#endif
	bl	cpu_init		@ restore the und/abt/irq banked regs
	mov	r0, #0			@ return zero on success
	ldmfd	sp!, {r4 - r11, pc}
ENDPROC(cpu_resume_after_mmu)

	.text
	.align

#ifdef CONFIG_MCPM
	.arm
THUMB(	.thumb			)
ENTRY(cpu_resume_no_hyp)
ARM_BE8(setend be)			@ ensure we are in BE mode
	b	no_hyp
#endif

#ifdef CONFIG_MMU
	.arm
ENTRY(cpu_resume_arm)
 THUMB(	badr	r9, 1f		)	@ Kernel is entered in ARM.
 THUMB(	bx	r9		)	@ If this is a Thumb-2 kernel,
 THUMB(	.thumb			)	@ switch to Thumb now.
 THUMB(1:			)
#endif

ENTRY(cpu_resume)
ARM_BE8(setend be)			@ ensure we are in BE mode
#ifdef CONFIG_ARM_VIRT_EXT
	bl	__hyp_stub_install_secondary
#endif
	safe_svcmode_maskall r1
no_hyp:
	mov	r1, #0
	ALT_SMP(mrc p15, 0, r0, c0, c0, 5)
	ALT_UP_B(1f)
	adr_l	r2, mpidr_hash		@ r2 = struct mpidr_hash phys address

	/*
	 * This ldmia relies on the memory layout of the mpidr_hash
	 * struct mpidr_hash.
	 */
	ldmia	r2, { r3-r6 }	@ r3 = mpidr mask (r4,r5,r6) = l[0,1,2] shifts
	compute_mpidr_hash	r1, r4, r5, r6, r0, r3
1:
	ldr_l	r0, sleep_save_sp + SLEEP_SAVE_SP_PHYS
	ldr	r0, [r0, r1, lsl #2]

	@ load phys pgd, stack, resume fn
  ARM(	ldmia	r0!, {r1, sp, pc}	)
THUMB(	ldmia	r0!, {r1, r2, r3}	)
THUMB(	mov	sp, r2			)
THUMB(	bx	r3			)
ENDPROC(cpu_resume)

#ifdef CONFIG_MMU
ENDPROC(cpu_resume_arm)
#endif
#ifdef CONFIG_MCPM
ENDPROC(cpu_resume_no_hyp)
#endif

	.data
	.align	2
	.type	sleep_save_sp, #object
ENTRY(sleep_save_sp)
	.space	SLEEP_SAVE_SP_SZ		@ struct sleep_save_sp


code:
/* SPDX-License-Identifier: GPL-2.0-only */
/*
 * Copyright (c) 2015, Linaro Limited
 */
#include <linux/linkage.h>
#include <linux/arm-smccc.h>

#include <asm/asm-offsets.h>
#include <asm/opcodes-sec.h>
#include <asm/opcodes-virt.h>
#include <asm/unwind.h>

	/*
	 * Wrap c macros in asm macros to delay expansion until after the
	 * SMCCC asm macro is expanded.
	 */
	.macro SMCCC_SMC
	__SMC(0)
	.endm

	.macro SMCCC_HVC
	__HVC(0)
	.endm

	.macro SMCCC instr
UNWIND(	.fnstart)
	mov	r12, sp
	push	{r4-r7}
UNWIND(	.save	{r4-r7})
	ldm	r12, {r4-r7}
	\instr
	ldr	r4, [sp, #36]
	cmp	r4, #0
	beq	1f			// No quirk structure
	ldr     r5, [r4, #ARM_SMCCC_QUIRK_ID_OFFS]
	cmp     r5, #ARM_SMCCC_QUIRK_QCOM_A6
	bne	1f			// No quirk present
	str	r6, [r4, #ARM_SMCCC_QUIRK_STATE_OFFS]
1:	pop	{r4-r7}
	ldr	r12, [sp, #(4 * 4)]
	stm	r12, {r0-r3}
	bx	lr
UNWIND(	.fnend)
	.endm

/*
 * void smccc_smc(unsigned long a0, unsigned long a1, unsigned long a2,
 *		  unsigned long a3, unsigned long a4, unsigned long a5,
 *		  unsigned long a6, unsigned long a7, struct arm_smccc_res *res,
 *		  struct arm_smccc_quirk *quirk)
 */
ENTRY(__arm_smccc_smc)
	SMCCC SMCCC_SMC
ENDPROC(__arm_smccc_smc)

/*
 * void smccc_hvc(unsigned long a0, unsigned long a1, unsigned long a2,
 *		  unsigned long a3, unsigned long a4, unsigned long a5,
 *		  unsigned long a6, unsigned long a7, struct arm_smccc_res *res,
 *		  struct arm_smccc_quirk *quirk)
 */
ENTRY(__arm_smccc_hvc)
	SMCCC SMCCC_HVC
ENDPROC(__arm_smccc_hvc)


code:
// SPDX-License-Identifier: GPL-2.0-only
/*
 *  linux/arch/arm/kernel/smp.c
 *
 *  Copyright (C) 2002 ARM Limited, All Rights Reserved.
 */
#include <linux/module.h>
#include <linux/delay.h>
#include <linux/init.h>
#include <linux/spinlock.h>
#include <linux/sched/mm.h>
#include <linux/sched/hotplug.h>
#include <linux/sched/task_stack.h>
#include <linux/interrupt.h>
#include <linux/cache.h>
#include <linux/profile.h>
#include <linux/errno.h>
#include <linux/mm.h>
#include <linux/err.h>
#include <linux/cpu.h>
#include <linux/seq_file.h>
#include <linux/irq.h>
#include <linux/nmi.h>
#include <linux/percpu.h>
#include <linux/clockchips.h>
#include <linux/completion.h>
#include <linux/cpufreq.h>
#include <linux/irq_work.h>
#include <linux/kernel_stat.h>

#include <linux/atomic.h>
#include <asm/bugs.h>
#include <asm/smp.h>
#include <asm/cacheflush.h>
#include <asm/cpu.h>
#include <asm/cputype.h>
#include <asm/exception.h>
#include <asm/idmap.h>
#include <asm/topology.h>
#include <asm/mmu_context.h>
#include <asm/procinfo.h>
#include <asm/processor.h>
#include <asm/sections.h>
#include <asm/tlbflush.h>
#include <asm/ptrace.h>
#include <asm/smp_plat.h>
#include <asm/virt.h>
#include <asm/mach/arch.h>
#include <asm/mpu.h>

#define CREATE_TRACE_POINTS
#include <trace/events/ipi.h>

/*
 * as from 2.5, kernels no longer have an init_tasks structure
 * so we need some other way of telling a new secondary core
 * where to place its SVC stack
 */
struct secondary_data secondary_data;

enum ipi_msg_type {
	IPI_WAKEUP,
	IPI_TIMER,
	IPI_RESCHEDULE,
	IPI_CALL_FUNC,
	IPI_CPU_STOP,
	IPI_IRQ_WORK,
	IPI_COMPLETION,
	NR_IPI,
	/*
	 * CPU_BACKTRACE is special and not included in NR_IPI
	 * or tracable with trace_ipi_*
	 */
	IPI_CPU_BACKTRACE = NR_IPI,
	/*
	 * SGI8-15 can be reserved by secure firmware, and thus may
	 * not be usable by the kernel. Please keep the above limited
	 * to at most 8 entries.
	 */
	MAX_IPI
};

static int ipi_irq_base __read_mostly;
static int nr_ipi __read_mostly = NR_IPI;
static struct irq_desc *ipi_desc[MAX_IPI] __read_mostly;

static void ipi_setup(int cpu);

static DECLARE_COMPLETION(cpu_running);

static struct smp_operations smp_ops __ro_after_init;

void __init smp_set_ops(const struct smp_operations *ops)
{
	if (ops)
		smp_ops = *ops;
};

static unsigned long get_arch_pgd(pgd_t *pgd)
{
#ifdef CONFIG_ARM_LPAE
	return __phys_to_pfn(virt_to_phys(pgd));
#else
	return virt_to_phys(pgd);
#endif
}

#if defined(CONFIG_BIG_LITTLE) && defined(CONFIG_HARDEN_BRANCH_PREDICTOR)
static int secondary_biglittle_prepare(unsigned int cpu)
{
	if (!cpu_vtable[cpu])
		cpu_vtable[cpu] = kzalloc(sizeof(*cpu_vtable[cpu]), GFP_KERNEL);

	return cpu_vtable[cpu] ? 0 : -ENOMEM;
}

static void secondary_biglittle_init(void)
{
	init_proc_vtable(lookup_processor(read_cpuid_id())->proc);
}
#else
static int secondary_biglittle_prepare(unsigned int cpu)
{
	return 0;
}

static void secondary_biglittle_init(void)
{
}
#endif

int __cpu_up(unsigned int cpu, struct task_struct *idle)
{
	int ret;

	if (!smp_ops.smp_boot_secondary)
		return -ENOSYS;

	ret = secondary_biglittle_prepare(cpu);
	if (ret)
		return ret;

	/*
	 * We need to tell the secondary core where to find
	 * its stack and the page tables.
	 */
	secondary_data.stack = task_stack_page(idle) + THREAD_START_SP;
#ifdef CONFIG_ARM_MPU
	secondary_data.mpu_rgn_info = &mpu_rgn_info;
#endif

#ifdef CONFIG_MMU
	secondary_data.pgdir = virt_to_phys(idmap_pgd);
	secondary_data.swapper_pg_dir = get_arch_pgd(swapper_pg_dir);
#endif
	secondary_data.task = idle;
	sync_cache_w(&secondary_data);

	/*
	 * Now bring the CPU into our world.
	 */
	ret = smp_ops.smp_boot_secondary(cpu, idle);
	if (ret == 0) {
		/*
		 * CPU was successfully started, wait for it
		 * to come online or time out.
		 */
		wait_for_completion_timeout(&cpu_running,
						 msecs_to_jiffies(1000));

		if (!cpu_online(cpu)) {
			pr_crit("CPU%u: failed to come online\n", cpu);
			ret = -EIO;
		}
	} else {
		pr_err("CPU%u: failed to boot: %d\n", cpu, ret);
	}

	memset(&secondary_data, 0, sizeof(secondary_data));
	return ret;
}

/* platform specific SMP operations */
void __init smp_init_cpus(void)
{
	if (smp_ops.smp_init_cpus)
		smp_ops.smp_init_cpus();
}

int platform_can_secondary_boot(void)
{
	return !!smp_ops.smp_boot_secondary;
}

int platform_can_cpu_hotplug(void)
{
#ifdef CONFIG_HOTPLUG_CPU
	if (smp_ops.cpu_kill)
		return 1;
#endif

	return 0;
}

#ifdef CONFIG_HOTPLUG_CPU
static int platform_cpu_kill(unsigned int cpu)
{
	if (smp_ops.cpu_kill)
		return smp_ops.cpu_kill(cpu);
	return 1;
}

static int platform_cpu_disable(unsigned int cpu)
{
	if (smp_ops.cpu_disable)
		return smp_ops.cpu_disable(cpu);

	return 0;
}

int platform_can_hotplug_cpu(unsigned int cpu)
{
	/* cpu_die must be specified to support hotplug */
	if (!smp_ops.cpu_die)
		return 0;

	if (smp_ops.cpu_can_disable)
		return smp_ops.cpu_can_disable(cpu);

	/*
	 * By default, allow disabling all CPUs except the first one,
	 * since this is special on a lot of platforms, e.g. because
	 * of clock tick interrupts.
	 */
	return cpu != 0;
}

static void ipi_teardown(int cpu)
{
	int i;

	if (WARN_ON_ONCE(!ipi_irq_base))
		return;

	for (i = 0; i < nr_ipi; i++)
		disable_percpu_irq(ipi_irq_base + i);
}

/*
 * __cpu_disable runs on the processor to be shutdown.
 */
int __cpu_disable(void)
{
	unsigned int cpu = smp_processor_id();
	int ret;

	ret = platform_cpu_disable(cpu);
	if (ret)
		return ret;

#ifdef CONFIG_GENERIC_ARCH_TOPOLOGY
	remove_cpu_topology(cpu);
#endif

	/*
	 * Take this CPU offline.  Once we clear this, we can't return,
	 * and we must not schedule until we're ready to give up the cpu.
	 */
	set_cpu_online(cpu, false);
	ipi_teardown(cpu);

	/*
	 * OK - migrate IRQs away from this CPU
	 */
	irq_migrate_all_off_this_cpu();

	/*
	 * Flush user cache and TLB mappings, and then remove this CPU
	 * from the vm mask set of all processes.
	 *
	 * Caches are flushed to the Level of Unification Inner Shareable
	 * to write-back dirty lines to unified caches shared by all CPUs.
	 */
	flush_cache_louis();
	local_flush_tlb_all();

	return 0;
}

/*
 * called on the thread which is asking for a CPU to be shutdown -
 * waits until shutdown has completed, or it is timed out.
 */
void __cpu_die(unsigned int cpu)
{
	if (!cpu_wait_death(cpu, 5)) {
		pr_err("CPU%u: cpu didn't die\n", cpu);
		return;
	}
	pr_debug("CPU%u: shutdown\n", cpu);

	clear_tasks_mm_cpumask(cpu);
	/*
	 * platform_cpu_kill() is generally expected to do the powering off
	 * and/or cutting of clocks to the dying CPU.  Optionally, this may
	 * be done by the CPU which is dying in preference to supporting
	 * this call, but that means there is _no_ synchronisation between
	 * the requesting CPU and the dying CPU actually losing power.
	 */
	if (!platform_cpu_kill(cpu))
		pr_err("CPU%u: unable to kill\n", cpu);
}

/*
 * Called from the idle thread for the CPU which has been shutdown.
 *
 * Note that we disable IRQs here, but do not re-enable them
 * before returning to the caller. This is also the behaviour
 * of the other hotplug-cpu capable cores, so presumably coming
 * out of idle fixes this.
 */
void arch_cpu_idle_dead(void)
{
	unsigned int cpu = smp_processor_id();

	idle_task_exit();

	local_irq_disable();

	/*
	 * Flush the data out of the L1 cache for this CPU.  This must be
	 * before the completion to ensure that data is safely written out
	 * before platform_cpu_kill() gets called - which may disable
	 * *this* CPU and power down its cache.
	 */
	flush_cache_louis();

	/*
	 * Tell __cpu_die() that this CPU is now safe to dispose of.  Once
	 * this returns, power and/or clocks can be removed at any point
	 * from this CPU and its cache by platform_cpu_kill().
	 */
	(void)cpu_report_death();

	/*
	 * Ensure that the cache lines associated with that completion are
	 * written out.  This covers the case where _this_ CPU is doing the
	 * powering down, to ensure that the completion is visible to the
	 * CPU waiting for this one.
	 */
	flush_cache_louis();

	/*
	 * The actual CPU shutdown procedure is at least platform (if not
	 * CPU) specific.  This may remove power, or it may simply spin.
	 *
	 * Platforms are generally expected *NOT* to return from this call,
	 * although there are some which do because they have no way to
	 * power down the CPU.  These platforms are the _only_ reason we
	 * have a return path which uses the fragment of assembly below.
	 *
	 * The return path should not be used for platforms which can
	 * power off the CPU.
	 */
	if (smp_ops.cpu_die)
		smp_ops.cpu_die(cpu);

	pr_warn("CPU%u: smp_ops.cpu_die() returned, trying to resuscitate\n",
		cpu);

	/*
	 * Do not return to the idle loop - jump back to the secondary
	 * cpu initialisation.  There's some initialisation which needs
	 * to be repeated to undo the effects of taking the CPU offline.
	 */
	__asm__("mov	sp, %0\n"
	"	mov	fp, #0\n"
	"	mov	r0, %1\n"
	"	b	secondary_start_kernel"
		:
		: "r" (task_stack_page(current) + THREAD_SIZE - 8),
		  "r" (current)
		: "r0");
}
#endif /* CONFIG_HOTPLUG_CPU */

/*
 * Called by both boot and secondaries to move global data into
 * per-processor storage.
 */
static void smp_store_cpu_info(unsigned int cpuid)
{
	struct cpuinfo_arm *cpu_info = &per_cpu(cpu_data, cpuid);

	cpu_info->loops_per_jiffy = loops_per_jiffy;
	cpu_info->cpuid = read_cpuid_id();

	store_cpu_topology(cpuid);
	check_cpu_icache_size(cpuid);
}

static void set_current(struct task_struct *cur)
{
	/* Set TPIDRURO */
	asm("mcr p15, 0, %0, c13, c0, 3" :: "r"(cur) : "memory");
}

/*
 * This is the secondary CPU boot entry.  We're using this CPUs
 * idle thread stack, but a set of temporary page tables.
 */
asmlinkage void secondary_start_kernel(struct task_struct *task)
{
	struct mm_struct *mm = &init_mm;
	unsigned int cpu;

	set_current(task);

	secondary_biglittle_init();

	/*
	 * The identity mapping is uncached (strongly ordered), so
	 * switch away from it before attempting any exclusive accesses.
	 */
	cpu_switch_mm(mm->pgd, mm);
	local_flush_bp_all();
	enter_lazy_tlb(mm, current);
	local_flush_tlb_all();

	/*
	 * All kernel threads share the same mm context; grab a
	 * reference and switch to it.
	 */
	cpu = smp_processor_id();
	mmgrab(mm);
	current->active_mm = mm;
	cpumask_set_cpu(cpu, mm_cpumask(mm));

	cpu_init();

#ifndef CONFIG_MMU
	setup_vectors_base();
#endif
	pr_debug("CPU%u: Booted secondary processor\n", cpu);

	trace_hardirqs_off();

	/*
	 * Give the platform a chance to do its own initialisation.
	 */
	if (smp_ops.smp_secondary_init)
		smp_ops.smp_secondary_init(cpu);

	notify_cpu_starting(cpu);

	ipi_setup(cpu);

	calibrate_delay();

	smp_store_cpu_info(cpu);

	/*
	 * OK, now it's safe to let the boot CPU continue.  Wait for
	 * the CPU migration code to notice that the CPU is online
	 * before we continue - which happens after __cpu_up returns.
	 */
	set_cpu_online(cpu, true);

	check_other_bugs();

	complete(&cpu_running);

	local_irq_enable();
	local_fiq_enable();
	local_abt_enable();

	/*
	 * OK, it's off to the idle thread for us
	 */
	cpu_startup_entry(CPUHP_AP_ONLINE_IDLE);
}

void __init smp_cpus_done(unsigned int max_cpus)
{
	int cpu;
	unsigned long bogosum = 0;

	for_each_online_cpu(cpu)
		bogosum += per_cpu(cpu_data, cpu).loops_per_jiffy;

	printk(KERN_INFO "SMP: Total of %d processors activated "
	       "(%lu.%02lu BogoMIPS).\n",
	       num_online_cpus(),
	       bogosum / (500000/HZ),
	       (bogosum / (5000/HZ)) % 100);

	hyp_mode_check();
}

void __init smp_prepare_boot_cpu(void)
{
	set_my_cpu_offset(per_cpu_offset(smp_processor_id()));
}

void __init smp_prepare_cpus(unsigned int max_cpus)
{
	unsigned int ncores = num_possible_cpus();

	init_cpu_topology();

	smp_store_cpu_info(smp_processor_id());

	/*
	 * are we trying to boot more cores than exist?
	 */
	if (max_cpus > ncores)
		max_cpus = ncores;
	if (ncores > 1 && max_cpus) {
		/*
		 * Initialise the present map, which describes the set of CPUs
		 * actually populated at the present time. A platform should
		 * re-initialize the map in the platforms smp_prepare_cpus()
		 * if present != possible (e.g. physical hotplug).
		 */
		init_cpu_present(cpu_possible_mask);

		/*
		 * Initialise the SCU if there are more than one CPU
		 * and let them know where to start.
		 */
		if (smp_ops.smp_prepare_cpus)
			smp_ops.smp_prepare_cpus(max_cpus);
	}
}

static const char *ipi_types[NR_IPI] __tracepoint_string = {
	[IPI_WAKEUP]		= "CPU wakeup interrupts",
	[IPI_TIMER]		= "Timer broadcast interrupts",
	[IPI_RESCHEDULE]	= "Rescheduling interrupts",
	[IPI_CALL_FUNC]		= "Function call interrupts",
	[IPI_CPU_STOP]		= "CPU stop interrupts",
	[IPI_IRQ_WORK]		= "IRQ work interrupts",
	[IPI_COMPLETION]	= "completion interrupts",
};

static void smp_cross_call(const struct cpumask *target, unsigned int ipinr);

void show_ipi_list(struct seq_file *p, int prec)
{
	unsigned int cpu, i;

	for (i = 0; i < NR_IPI; i++) {
		if (!ipi_desc[i])
			continue;

		seq_printf(p, "%*s%u: ", prec - 1, "IPI", i);

		for_each_online_cpu(cpu)
			seq_printf(p, "%10u ", irq_desc_kstat_cpu(ipi_desc[i], cpu));

		seq_printf(p, " %s\n", ipi_types[i]);
	}
}

void arch_send_call_function_ipi_mask(const struct cpumask *mask)
{
	smp_cross_call(mask, IPI_CALL_FUNC);
}

void arch_send_wakeup_ipi_mask(const struct cpumask *mask)
{
	smp_cross_call(mask, IPI_WAKEUP);
}

void arch_send_call_function_single_ipi(int cpu)
{
	smp_cross_call(cpumask_of(cpu), IPI_CALL_FUNC);
}

#ifdef CONFIG_IRQ_WORK
void arch_irq_work_raise(void)
{
	if (arch_irq_work_has_interrupt())
		smp_cross_call(cpumask_of(smp_processor_id()), IPI_IRQ_WORK);
}
#endif

#ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
void tick_broadcast(const struct cpumask *mask)
{
	smp_cross_call(mask, IPI_TIMER);
}
#endif

static DEFINE_RAW_SPINLOCK(stop_lock);

/*
 * ipi_cpu_stop - handle IPI from smp_send_stop()
 */
static void ipi_cpu_stop(unsigned int cpu)
{
	local_fiq_disable();

	if (system_state <= SYSTEM_RUNNING) {
		raw_spin_lock(&stop_lock);
		pr_crit("CPU%u: stopping\n", cpu);
		dump_stack();
		raw_spin_unlock(&stop_lock);
	}

	set_cpu_online(cpu, false);

	while (1) {
		cpu_relax();
		wfe();
	}
}

static DEFINE_PER_CPU(struct completion *, cpu_completion);

int register_ipi_completion(struct completion *completion, int cpu)
{
	per_cpu(cpu_completion, cpu) = completion;
	return IPI_COMPLETION;
}

static void ipi_complete(unsigned int cpu)
{
	complete(per_cpu(cpu_completion, cpu));
}

/*
 * Main handler for inter-processor interrupts
 */
static void do_handle_IPI(int ipinr)
{
	unsigned int cpu = smp_processor_id();

	if ((unsigned)ipinr < NR_IPI)
		trace_ipi_entry_rcuidle(ipi_types[ipinr]);

	switch (ipinr) {
	case IPI_WAKEUP:
		break;

#ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
	case IPI_TIMER:
		tick_receive_broadcast();
		break;
#endif

	case IPI_RESCHEDULE:
		scheduler_ipi();
		break;

	case IPI_CALL_FUNC:
		generic_smp_call_function_interrupt();
		break;

	case IPI_CPU_STOP:
		ipi_cpu_stop(cpu);
		break;

#ifdef CONFIG_IRQ_WORK
	case IPI_IRQ_WORK:
		irq_work_run();
		break;
#endif

	case IPI_COMPLETION:
		ipi_complete(cpu);
		break;

	case IPI_CPU_BACKTRACE:
		printk_deferred_enter();
		nmi_cpu_backtrace(get_irq_regs());
		printk_deferred_exit();
		break;

	default:
		pr_crit("CPU%u: Unknown IPI message 0x%x\n",
		        cpu, ipinr);
		break;
	}

	if ((unsigned)ipinr < NR_IPI)
		trace_ipi_exit_rcuidle(ipi_types[ipinr]);
}

/* Legacy version, should go away once all irqchips have been converted */
void handle_IPI(int ipinr, struct pt_regs *regs)
{
	struct pt_regs *old_regs = set_irq_regs(regs);

	irq_enter();
	do_handle_IPI(ipinr);
	irq_exit();

	set_irq_regs(old_regs);
}

static irqreturn_t ipi_handler(int irq, void *data)
{
	do_handle_IPI(irq - ipi_irq_base);
	return IRQ_HANDLED;
}

static void smp_cross_call(const struct cpumask *target, unsigned int ipinr)
{
	trace_ipi_raise_rcuidle(target, ipi_types[ipinr]);
	__ipi_send_mask(ipi_desc[ipinr], target);
}

static void ipi_setup(int cpu)
{
	int i;

	if (WARN_ON_ONCE(!ipi_irq_base))
		return;

	for (i = 0; i < nr_ipi; i++)
		enable_percpu_irq(ipi_irq_base + i, 0);
}

void __init set_smp_ipi_range(int ipi_base, int n)
{
	int i;

	WARN_ON(n < MAX_IPI);
	nr_ipi = min(n, MAX_IPI);

	for (i = 0; i < nr_ipi; i++) {
		int err;

		err = request_percpu_irq(ipi_base + i, ipi_handler,
					 "IPI", &irq_stat);
		WARN_ON(err);

		ipi_desc[i] = irq_to_desc(ipi_base + i);
		irq_set_status_flags(ipi_base + i, IRQ_HIDDEN);
	}

	ipi_irq_base = ipi_base;

	/* Setup the boot CPU immediately */
	ipi_setup(smp_processor_id());
}

void smp_send_reschedule(int cpu)
{
	smp_cross_call(cpumask_of(cpu), IPI_RESCHEDULE);
}

void smp_send_stop(void)
{
	unsigned long timeout;
	struct cpumask mask;

	cpumask_copy(&mask, cpu_online_mask);
	cpumask_clear_cpu(smp_processor_id(), &mask);
	if (!cpumask_empty(&mask))
		smp_cross_call(&mask, IPI_CPU_STOP);

	/* Wait up to one second for other CPUs to stop */
	timeout = USEC_PER_SEC;
	while (num_online_cpus() > 1 && timeout--)
		udelay(1);

	if (num_online_cpus() > 1)
		pr_warn("SMP: failed to stop secondary CPUs\n");
}

/* In case panic() and panic() called at the same time on CPU1 and CPU2,
 * and CPU 1 calls panic_smp_self_stop() before crash_smp_send_stop()
 * CPU1 can't receive the ipi irqs from CPU2, CPU1 will be always online,
 * kdump fails. So split out the panic_smp_self_stop() and add
 * set_cpu_online(smp_processor_id(), false).
 */
void panic_smp_self_stop(void)
{
	pr_debug("CPU %u will stop doing anything useful since another CPU has paniced\n",
	         smp_processor_id());
	set_cpu_online(smp_processor_id(), false);
	while (1)
		cpu_relax();
}

#ifdef CONFIG_CPU_FREQ

static DEFINE_PER_CPU(unsigned long, l_p_j_ref);
static DEFINE_PER_CPU(unsigned long, l_p_j_ref_freq);
static unsigned long global_l_p_j_ref;
static unsigned long global_l_p_j_ref_freq;

static int cpufreq_callback(struct notifier_block *nb,
					unsigned long val, void *data)
{
	struct cpufreq_freqs *freq = data;
	struct cpumask *cpus = freq->policy->cpus;
	int cpu, first = cpumask_first(cpus);
	unsigned int lpj;

	if (freq->flags & CPUFREQ_CONST_LOOPS)
		return NOTIFY_OK;

	if (!per_cpu(l_p_j_ref, first)) {
		for_each_cpu(cpu, cpus) {
			per_cpu(l_p_j_ref, cpu) =
				per_cpu(cpu_data, cpu).loops_per_jiffy;
			per_cpu(l_p_j_ref_freq, cpu) = freq->old;
		}

		if (!global_l_p_j_ref) {
			global_l_p_j_ref = loops_per_jiffy;
			global_l_p_j_ref_freq = freq->old;
		}
	}

	if ((val == CPUFREQ_PRECHANGE  && freq->old < freq->new) ||
	    (val == CPUFREQ_POSTCHANGE && freq->old > freq->new)) {
		loops_per_jiffy = cpufreq_scale(global_l_p_j_ref,
						global_l_p_j_ref_freq,
						freq->new);

		lpj = cpufreq_scale(per_cpu(l_p_j_ref, first),
				    per_cpu(l_p_j_ref_freq, first), freq->new);
		for_each_cpu(cpu, cpus)
			per_cpu(cpu_data, cpu).loops_per_jiffy = lpj;
	}
	return NOTIFY_OK;
}

static struct notifier_block cpufreq_notifier = {
	.notifier_call  = cpufreq_callback,
};

static int __init register_cpufreq_notifier(void)
{
	return cpufreq_register_notifier(&cpufreq_notifier,
						CPUFREQ_TRANSITION_NOTIFIER);
}
core_initcall(register_cpufreq_notifier);

#endif

static void raise_nmi(cpumask_t *mask)
{
	__ipi_send_mask(ipi_desc[IPI_CPU_BACKTRACE], mask);
}

void arch_trigger_cpumask_backtrace(const cpumask_t *mask, bool exclude_self)
{
	nmi_trigger_cpumask_backtrace(mask, exclude_self, raise_nmi);
}


code:
// SPDX-License-Identifier: GPL-2.0-only
/*
 *  linux/arch/arm/kernel/smp_scu.c
 *
 *  Copyright (C) 2002 ARM Ltd.
 *  All Rights Reserved
 */
#include <linux/init.h>
#include <linux/io.h>

#include <asm/smp_plat.h>
#include <asm/smp_scu.h>
#include <asm/cacheflush.h>
#include <asm/cputype.h>

#define SCU_CTRL		0x00
#define SCU_ENABLE		(1 << 0)
#define SCU_STANDBY_ENABLE	(1 << 5)
#define SCU_CONFIG		0x04
#define SCU_CPU_STATUS		0x08
#define SCU_CPU_STATUS_MASK	GENMASK(1, 0)
#define SCU_INVALIDATE		0x0c
#define SCU_FPGA_REVISION	0x10

#ifdef CONFIG_SMP
/*
 * Get the number of CPU cores from the SCU configuration
 */
unsigned int __init scu_get_core_count(void __iomem *scu_base)
{
	unsigned int ncores = readl_relaxed(scu_base + SCU_CONFIG);
	return (ncores & 0x03) + 1;
}

/*
 * Enable the SCU
 */
void scu_enable(void __iomem *scu_base)
{
	u32 scu_ctrl;

#ifdef CONFIG_ARM_ERRATA_764369
	/* Cortex-A9 only */
	if ((read_cpuid_id() & 0xff0ffff0) == 0x410fc090) {
		scu_ctrl = readl_relaxed(scu_base + 0x30);
		if (!(scu_ctrl & 1))
			writel_relaxed(scu_ctrl | 0x1, scu_base + 0x30);
	}
#endif

	scu_ctrl = readl_relaxed(scu_base + SCU_CTRL);
	/* already enabled? */
	if (scu_ctrl & SCU_ENABLE)
		return;

	scu_ctrl |= SCU_ENABLE;

	/* Cortex-A9 earlier than r2p0 has no standby bit in SCU */
	if ((read_cpuid_id() & 0xff0ffff0) == 0x410fc090 &&
	    (read_cpuid_id() & 0x00f0000f) >= 0x00200000)
		scu_ctrl |= SCU_STANDBY_ENABLE;

	writel_relaxed(scu_ctrl, scu_base + SCU_CTRL);

	/*
	 * Ensure that the data accessed by CPU0 before the SCU was
	 * initialised is visible to the other CPUs.
	 */
	flush_cache_all();
}
#endif

static int scu_set_power_mode_internal(void __iomem *scu_base,
				       unsigned int logical_cpu,
				       unsigned int mode)
{
	unsigned int val;
	int cpu = MPIDR_AFFINITY_LEVEL(cpu_logical_map(logical_cpu), 0);

	if (mode > 3 || mode == 1 || cpu > 3)
		return -EINVAL;

	val = readb_relaxed(scu_base + SCU_CPU_STATUS + cpu);
	val &= ~SCU_CPU_STATUS_MASK;
	val |= mode;
	writeb_relaxed(val, scu_base + SCU_CPU_STATUS + cpu);

	return 0;
}

/*
 * Set the executing CPUs power mode as defined.  This will be in
 * preparation for it executing a WFI instruction.
 *
 * This function must be called with preemption disabled, and as it
 * has the side effect of disabling coherency, caches must have been
 * flushed.  Interrupts must also have been disabled.
 */
int scu_power_mode(void __iomem *scu_base, unsigned int mode)
{
	return scu_set_power_mode_internal(scu_base, smp_processor_id(), mode);
}

/*
 * Set the given (logical) CPU's power mode to SCU_PM_NORMAL.
 */
int scu_cpu_power_enable(void __iomem *scu_base, unsigned int cpu)
{
	return scu_set_power_mode_internal(scu_base, cpu, SCU_PM_NORMAL);
}

int scu_get_cpu_power_mode(void __iomem *scu_base, unsigned int logical_cpu)
{
	unsigned int val;
	int cpu = MPIDR_AFFINITY_LEVEL(cpu_logical_map(logical_cpu), 0);

	if (cpu > 3)
		return -EINVAL;

	val = readb_relaxed(scu_base + SCU_CPU_STATUS + cpu);
	val &= SCU_CPU_STATUS_MASK;

	return val;
}


code:
// SPDX-License-Identifier: GPL-2.0-only
/*
 *  linux/arch/arm/kernel/smp_tlb.c
 *
 *  Copyright (C) 2002 ARM Limited, All Rights Reserved.
 */
#include <linux/preempt.h>
#include <linux/smp.h>
#include <linux/uaccess.h>

#include <asm/smp_plat.h>
#include <asm/tlbflush.h>
#include <asm/mmu_context.h>

/**********************************************************************/

/*
 * TLB operations
 */
struct tlb_args {
	struct vm_area_struct *ta_vma;
	unsigned long ta_start;
	unsigned long ta_end;
};

static inline void ipi_flush_tlb_all(void *ignored)
{
	local_flush_tlb_all();
}

static inline void ipi_flush_tlb_mm(void *arg)
{
	struct mm_struct *mm = (struct mm_struct *)arg;

	local_flush_tlb_mm(mm);
}

static inline void ipi_flush_tlb_page(void *arg)
{
	struct tlb_args *ta = (struct tlb_args *)arg;
	unsigned int __ua_flags = uaccess_save_and_enable();

	local_flush_tlb_page(ta->ta_vma, ta->ta_start);

	uaccess_restore(__ua_flags);
}

static inline void ipi_flush_tlb_kernel_page(void *arg)
{
	struct tlb_args *ta = (struct tlb_args *)arg;

	local_flush_tlb_kernel_page(ta->ta_start);
}

static inline void ipi_flush_tlb_range(void *arg)
{
	struct tlb_args *ta = (struct tlb_args *)arg;
	unsigned int __ua_flags = uaccess_save_and_enable();

	local_flush_tlb_range(ta->ta_vma, ta->ta_start, ta->ta_end);

	uaccess_restore(__ua_flags);
}

static inline void ipi_flush_tlb_kernel_range(void *arg)
{
	struct tlb_args *ta = (struct tlb_args *)arg;

	local_flush_tlb_kernel_range(ta->ta_start, ta->ta_end);
}

static inline void ipi_flush_bp_all(void *ignored)
{
	local_flush_bp_all();
}

#ifdef CONFIG_ARM_ERRATA_798181
bool (*erratum_a15_798181_handler)(void);

static bool erratum_a15_798181_partial(void)
{
	asm("mcr p15, 0, %0, c8, c3, 1" : : "r" (0));
	dsb(ish);
	return false;
}

static bool erratum_a15_798181_broadcast(void)
{
	asm("mcr p15, 0, %0, c8, c3, 1" : : "r" (0));
	dsb(ish);
	return true;
}

void erratum_a15_798181_init(void)
{
	unsigned int midr = read_cpuid_id();
	unsigned int revidr = read_cpuid(CPUID_REVIDR);

	/* Brahma-B15 r0p0..r0p2 affected
	 * Cortex-A15 r0p0..r3p3 w/o ECO fix affected
	 * Fixes applied to A15 with respect to the revision and revidr are:
	 *
	 * r0p0-r2p1: No fixes applied
	 * r2p2,r2p3:
	 *	REVIDR[4]: 798181 Moving a virtual page that is being accessed
	 *		   by an active process can lead to unexpected behavior
	 *	REVIDR[9]: Not defined
	 * r2p4,r3p0,r3p1,r3p2:
	 *	REVIDR[4]: 798181 Moving a virtual page that is being accessed
	 *		   by an active process can lead to unexpected behavior
	 *	REVIDR[9]: 798181 Moving a virtual page that is being accessed
	 *		   by an active process can lead to unexpected behavior
	 *		   - This is an update to a previously released ECO.
	 * r3p3:
	 *	REVIDR[4]: Reserved
	 *	REVIDR[9]: 798181 Moving a virtual page that is being accessed
	 *		   by an active process can lead to unexpected behavior
	 *		   - This is an update to a previously released ECO.
	 *
	 * Handling:
	 *	REVIDR[9] set -> No WA
	 *	REVIDR[4] set, REVIDR[9] cleared -> Partial WA
	 *	Both cleared -> Full WA
	 */
	if ((midr & 0xff0ffff0) == 0x420f00f0 && midr <= 0x420f00f2) {
		erratum_a15_798181_handler = erratum_a15_798181_broadcast;
	} else if ((midr & 0xff0ffff0) == 0x410fc0f0 && midr < 0x412fc0f2) {
		erratum_a15_798181_handler = erratum_a15_798181_broadcast;
	} else if ((midr & 0xff0ffff0) == 0x410fc0f0 && midr < 0x412fc0f4) {
		if (revidr & 0x10)
			erratum_a15_798181_handler =
				erratum_a15_798181_partial;
		else
			erratum_a15_798181_handler =
				erratum_a15_798181_broadcast;
	} else if ((midr & 0xff0ffff0) == 0x410fc0f0 && midr < 0x413fc0f3) {
		if ((revidr & 0x210) == 0)
			erratum_a15_798181_handler =
				erratum_a15_798181_broadcast;
		else if (revidr & 0x10)
			erratum_a15_798181_handler =
				erratum_a15_798181_partial;
	} else if ((midr & 0xff0ffff0) == 0x410fc0f0 && midr < 0x414fc0f0) {
		if ((revidr & 0x200) == 0)
			erratum_a15_798181_handler =
				erratum_a15_798181_partial;
	}
}
#endif

static void ipi_flush_tlb_a15_erratum(void *arg)
{
	dmb();
}

static void broadcast_tlb_a15_erratum(void)
{
	if (!erratum_a15_798181())
		return;

	smp_call_function(ipi_flush_tlb_a15_erratum, NULL, 1);
}

static void broadcast_tlb_mm_a15_erratum(struct mm_struct *mm)
{
	int this_cpu;
	cpumask_t mask = { CPU_BITS_NONE };

	if (!erratum_a15_798181())
		return;

	this_cpu = get_cpu();
	a15_erratum_get_cpumask(this_cpu, mm, &mask);
	smp_call_function_many(&mask, ipi_flush_tlb_a15_erratum, NULL, 1);
	put_cpu();
}

void flush_tlb_all(void)
{
	if (tlb_ops_need_broadcast())
		on_each_cpu(ipi_flush_tlb_all, NULL, 1);
	else
		__flush_tlb_all();
	broadcast_tlb_a15_erratum();
}

void flush_tlb_mm(struct mm_struct *mm)
{
	if (tlb_ops_need_broadcast())
		on_each_cpu_mask(mm_cpumask(mm), ipi_flush_tlb_mm, mm, 1);
	else
		__flush_tlb_mm(mm);
	broadcast_tlb_mm_a15_erratum(mm);
}

void flush_tlb_page(struct vm_area_struct *vma, unsigned long uaddr)
{
	if (tlb_ops_need_broadcast()) {
		struct tlb_args ta;
		ta.ta_vma = vma;
		ta.ta_start = uaddr;
		on_each_cpu_mask(mm_cpumask(vma->vm_mm), ipi_flush_tlb_page,
					&ta, 1);
	} else
		__flush_tlb_page(vma, uaddr);
	broadcast_tlb_mm_a15_erratum(vma->vm_mm);
}

void flush_tlb_kernel_page(unsigned long kaddr)
{
	if (tlb_ops_need_broadcast()) {
		struct tlb_args ta;
		ta.ta_start = kaddr;
		on_each_cpu(ipi_flush_tlb_kernel_page, &ta, 1);
	} else
		__flush_tlb_kernel_page(kaddr);
	broadcast_tlb_a15_erratum();
}

void flush_tlb_range(struct vm_area_struct *vma,
                     unsigned long start, unsigned long end)
{
	if (tlb_ops_need_broadcast()) {
		struct tlb_args ta;
		ta.ta_vma = vma;
		ta.ta_start = start;
		ta.ta_end = end;
		on_each_cpu_mask(mm_cpumask(vma->vm_mm), ipi_flush_tlb_range,
					&ta, 1);
	} else
		local_flush_tlb_range(vma, start, end);
	broadcast_tlb_mm_a15_erratum(vma->vm_mm);
}

void flush_tlb_kernel_range(unsigned long start, unsigned long end)
{
	if (tlb_ops_need_broadcast()) {
		struct tlb_args ta;
		ta.ta_start = start;
		ta.ta_end = end;
		on_each_cpu(ipi_flush_tlb_kernel_range, &ta, 1);
	} else
		local_flush_tlb_kernel_range(start, end);
	broadcast_tlb_a15_erratum();
}

void flush_bp_all(void)
{
	if (tlb_ops_need_broadcast())
		on_each_cpu(ipi_flush_bp_all, NULL, 1);
	else
		__flush_bp_all();
}


code:
// SPDX-License-Identifier: GPL-2.0-only
/*
 *  linux/arch/arm/kernel/smp_twd.c
 *
 *  Copyright (C) 2002 ARM Ltd.
 *  All Rights Reserved
 */
#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/clk.h>
#include <linux/cpu.h>
#include <linux/delay.h>
#include <linux/device.h>
#include <linux/err.h>
#include <linux/smp.h>
#include <linux/jiffies.h>
#include <linux/clockchips.h>
#include <linux/interrupt.h>
#include <linux/io.h>
#include <linux/of_irq.h>
#include <linux/of_address.h>

#include <asm/smp_twd.h>

/* set up by the platform code */
static void __iomem *twd_base;

static struct clk *twd_clk;
static unsigned long twd_timer_rate;
static DEFINE_PER_CPU(bool, percpu_setup_called);

static struct clock_event_device __percpu *twd_evt;
static unsigned int twd_features =
		CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT;
static int twd_ppi;

static int twd_shutdown(struct clock_event_device *clk)
{
	writel_relaxed(0, twd_base + TWD_TIMER_CONTROL);
	return 0;
}

static int twd_set_oneshot(struct clock_event_device *clk)
{
	/* period set, and timer enabled in 'next_event' hook */
	writel_relaxed(TWD_TIMER_CONTROL_IT_ENABLE | TWD_TIMER_CONTROL_ONESHOT,
		       twd_base + TWD_TIMER_CONTROL);
	return 0;
}

static int twd_set_periodic(struct clock_event_device *clk)
{
	unsigned long ctrl = TWD_TIMER_CONTROL_ENABLE |
			     TWD_TIMER_CONTROL_IT_ENABLE |
			     TWD_TIMER_CONTROL_PERIODIC;

	writel_relaxed(DIV_ROUND_CLOSEST(twd_timer_rate, HZ),
		       twd_base + TWD_TIMER_LOAD);
	writel_relaxed(ctrl, twd_base + TWD_TIMER_CONTROL);
	return 0;
}

static int twd_set_next_event(unsigned long evt,
			struct clock_event_device *unused)
{
	unsigned long ctrl = readl_relaxed(twd_base + TWD_TIMER_CONTROL);

	ctrl |= TWD_TIMER_CONTROL_ENABLE;

	writel_relaxed(evt, twd_base + TWD_TIMER_COUNTER);
	writel_relaxed(ctrl, twd_base + TWD_TIMER_CONTROL);

	return 0;
}

/*
 * local_timer_ack: checks for a local timer interrupt.
 *
 * If a local timer interrupt has occurred, acknowledge and return 1.
 * Otherwise, return 0.
 */
static int twd_timer_ack(void)
{
	if (readl_relaxed(twd_base + TWD_TIMER_INTSTAT)) {
		writel_relaxed(1, twd_base + TWD_TIMER_INTSTAT);
		return 1;
	}

	return 0;
}

static void twd_timer_stop(void)
{
	struct clock_event_device *clk = raw_cpu_ptr(twd_evt);

	twd_shutdown(clk);
	disable_percpu_irq(clk->irq);
}

/*
 * Updates clockevent frequency when the cpu frequency changes.
 * Called on the cpu that is changing frequency with interrupts disabled.
 */
static void twd_update_frequency(void *new_rate)
{
	twd_timer_rate = *((unsigned long *) new_rate);

	clockevents_update_freq(raw_cpu_ptr(twd_evt), twd_timer_rate);
}

static int twd_rate_change(struct notifier_block *nb,
	unsigned long flags, void *data)
{
	struct clk_notifier_data *cnd = data;

	/*
	 * The twd clock events must be reprogrammed to account for the new
	 * frequency.  The timer is local to a cpu, so cross-call to the
	 * changing cpu.
	 */
	if (flags == POST_RATE_CHANGE)
		on_each_cpu(twd_update_frequency,
				  (void *)&cnd->new_rate, 1);

	return NOTIFY_OK;
}

static struct notifier_block twd_clk_nb = {
	.notifier_call = twd_rate_change,
};

static int twd_clk_init(void)
{
	if (twd_evt && raw_cpu_ptr(twd_evt) && !IS_ERR(twd_clk))
		return clk_notifier_register(twd_clk, &twd_clk_nb);

	return 0;
}
core_initcall(twd_clk_init);

static void twd_calibrate_rate(void)
{
	unsigned long count;
	u64 waitjiffies;

	/*
	 * If this is the first time round, we need to work out how fast
	 * the timer ticks
	 */
	if (twd_timer_rate == 0) {
		pr_info("Calibrating local timer... ");

		/* Wait for a tick to start */
		waitjiffies = get_jiffies_64() + 1;

		while (get_jiffies_64() < waitjiffies)
			udelay(10);

		/* OK, now the tick has started, let's get the timer going */
		waitjiffies += 5;

				 /* enable, no interrupt or reload */
		writel_relaxed(0x1, twd_base + TWD_TIMER_CONTROL);

				 /* maximum value */
		writel_relaxed(0xFFFFFFFFU, twd_base + TWD_TIMER_COUNTER);

		while (get_jiffies_64() < waitjiffies)
			udelay(10);

		count = readl_relaxed(twd_base + TWD_TIMER_COUNTER);

		twd_timer_rate = (0xFFFFFFFFU - count) * (HZ / 5);

		pr_cont("%lu.%02luMHz.\n", twd_timer_rate / 1000000,
			(twd_timer_rate / 10000) % 100);
	}
}

static irqreturn_t twd_handler(int irq, void *dev_id)
{
	struct clock_event_device *evt = dev_id;

	if (twd_timer_ack()) {
		evt->event_handler(evt);
		return IRQ_HANDLED;
	}

	return IRQ_NONE;
}

static void twd_get_clock(struct device_node *np)
{
	int err;

	if (np)
		twd_clk = of_clk_get(np, 0);
	else
		twd_clk = clk_get_sys("smp_twd", NULL);

	if (IS_ERR(twd_clk)) {
		pr_err("smp_twd: clock not found %d\n", (int) PTR_ERR(twd_clk));
		return;
	}

	err = clk_prepare_enable(twd_clk);
	if (err) {
		pr_err("smp_twd: clock failed to prepare+enable: %d\n", err);
		clk_put(twd_clk);
		return;
	}

	twd_timer_rate = clk_get_rate(twd_clk);
}

/*
 * Setup the local clock events for a CPU.
 */
static void twd_timer_setup(void)
{
	struct clock_event_device *clk = raw_cpu_ptr(twd_evt);
	int cpu = smp_processor_id();

	/*
	 * If the basic setup for this CPU has been done before don't
	 * bother with the below.
	 */
	if (per_cpu(percpu_setup_called, cpu)) {
		writel_relaxed(0, twd_base + TWD_TIMER_CONTROL);
		clockevents_register_device(clk);
		enable_percpu_irq(clk->irq, 0);
		return;
	}
	per_cpu(percpu_setup_called, cpu) = true;

	twd_calibrate_rate();

	/*
	 * The following is done once per CPU the first time .setup() is
	 * called.
	 */
	writel_relaxed(0, twd_base + TWD_TIMER_CONTROL);

	clk->name = "local_timer";
	clk->features = twd_features;
	clk->rating = 350;
	clk->set_state_shutdown = twd_shutdown;
	clk->set_state_periodic = twd_set_periodic;
	clk->set_state_oneshot = twd_set_oneshot;
	clk->tick_resume = twd_shutdown;
	clk->set_next_event = twd_set_next_event;
	clk->irq = twd_ppi;
	clk->cpumask = cpumask_of(cpu);

	clockevents_config_and_register(clk, twd_timer_rate,
					0xf, 0xffffffff);
	enable_percpu_irq(clk->irq, 0);
}

static int twd_timer_starting_cpu(unsigned int cpu)
{
	twd_timer_setup();
	return 0;
}

static int twd_timer_dying_cpu(unsigned int cpu)
{
	twd_timer_stop();
	return 0;
}

static int __init twd_local_timer_common_register(struct device_node *np)
{
	int err;

	twd_evt = alloc_percpu(struct clock_event_device);
	if (!twd_evt) {
		err = -ENOMEM;
		goto out_free;
	}

	err = request_percpu_irq(twd_ppi, twd_handler, "twd", twd_evt);
	if (err) {
		pr_err("twd: can't register interrupt %d (%d)\n", twd_ppi, err);
		goto out_free;
	}

	cpuhp_setup_state_nocalls(CPUHP_AP_ARM_TWD_STARTING,
				  "arm/timer/twd:starting",
				  twd_timer_starting_cpu, twd_timer_dying_cpu);

	twd_get_clock(np);
	if (!of_property_read_bool(np, "always-on"))
		twd_features |= CLOCK_EVT_FEAT_C3STOP;

	/*
	 * Immediately configure the timer on the boot CPU, unless we need
	 * jiffies to be incrementing to calibrate the rate in which case
	 * setup the timer in late_time_init.
	 */
	if (twd_timer_rate)
		twd_timer_setup();
	else
		late_time_init = twd_timer_setup;

	return 0;

out_free:
	iounmap(twd_base);
	twd_base = NULL;
	free_percpu(twd_evt);

	return err;
}

static int __init twd_local_timer_of_register(struct device_node *np)
{
	int err;

	twd_ppi = irq_of_parse_and_map(np, 0);
	if (!twd_ppi) {
		err = -EINVAL;
		goto out;
	}

	twd_base = of_iomap(np, 0);
	if (!twd_base) {
		err = -ENOMEM;
		goto out;
	}

	err = twd_local_timer_common_register(np);

out:
	WARN(err, "twd_local_timer_of_register failed (%d)\n", err);
	return err;
}
TIMER_OF_DECLARE(arm_twd_a9, "arm,cortex-a9-twd-timer", twd_local_timer_of_register);
TIMER_OF_DECLARE(arm_twd_a5, "arm,cortex-a5-twd-timer", twd_local_timer_of_register);
TIMER_OF_DECLARE(arm_twd_11mp, "arm,arm11mp-twd-timer", twd_local_timer_of_register);