Orange Pi5 kernel

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