1// SPDX-License-Identifier: GPL-2.0
2// Copyright (C) 2018 Cadence Design Systems Inc.
3
4#include <linux/cpu.h>
5#include <linux/jump_label.h>
6#include <linux/kernel.h>
7#include <linux/memory.h>
8#include <linux/stop_machine.h>
9#include <linux/types.h>
10
11#include <asm/cacheflush.h>
12
13#define J_OFFSET_MASK 0x0003ffff
14#define J_SIGN_MASK (~(J_OFFSET_MASK >> 1))
15
16#if defined(__XTENSA_EL__)
17#define J_INSN 0x6
18#define NOP_INSN 0x0020f0
19#elif defined(__XTENSA_EB__)
20#define J_INSN 0x60000000
21#define NOP_INSN 0x0f020000
22#else
23#error Unsupported endianness.
24#endif
25
26struct patch {
27	atomic_t cpu_count;
28	unsigned long addr;
29	size_t sz;
30	const void *data;
31};
32
33static void local_patch_text(unsigned long addr, const void *data, size_t sz)
34{
35	memcpy((void *)addr, data, sz);
36	local_flush_icache_range(addr, addr + sz);
37}
38
39static int patch_text_stop_machine(void *data)
40{
41	struct patch *patch = data;
42
43	if (atomic_inc_return(&patch->cpu_count) == num_online_cpus()) {
44		local_patch_text(patch->addr, patch->data, patch->sz);
45		atomic_inc(&patch->cpu_count);
46	} else {
47		while (atomic_read(&patch->cpu_count) <= num_online_cpus())
48			cpu_relax();
49		__invalidate_icache_range(patch->addr, patch->sz);
50	}
51	return 0;
52}
53
54static void patch_text(unsigned long addr, const void *data, size_t sz)
55{
56	if (IS_ENABLED(CONFIG_SMP)) {
57		struct patch patch = {
58			.cpu_count = ATOMIC_INIT(0),
59			.addr = addr,
60			.sz = sz,
61			.data = data,
62		};
63		stop_machine_cpuslocked(patch_text_stop_machine,
64					&patch, cpu_online_mask);
65	} else {
66		unsigned long flags;
67
68		local_irq_save(flags);
69		local_patch_text(addr, data, sz);
70		local_irq_restore(flags);
71	}
72}
73
74void arch_jump_label_transform(struct jump_entry *e,
75			       enum jump_label_type type)
76{
77	u32 d = (jump_entry_target(e) - (jump_entry_code(e) + 4));
78	u32 insn;
79
80	/* Jump only works within 128K of the J instruction. */
81	BUG_ON(!((d & J_SIGN_MASK) == 0 ||
82		 (d & J_SIGN_MASK) == J_SIGN_MASK));
83
84	if (type == JUMP_LABEL_JMP) {
85#if defined(__XTENSA_EL__)
86		insn = ((d & J_OFFSET_MASK) << 6) | J_INSN;
87#elif defined(__XTENSA_EB__)
88		insn = ((d & J_OFFSET_MASK) << 8) | J_INSN;
89#endif
90	} else {
91		insn = NOP_INSN;
92	}
93
94	patch_text(jump_entry_code(e), &insn, JUMP_LABEL_NOP_SIZE);
95}
96