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) #include <linux/bug.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  3) #include <linux/kernel.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  4) #include <asm/opcodes.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  5) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  6) static unsigned long __arm_gen_branch_thumb2(unsigned long pc,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  7) 					     unsigned long addr, bool link,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  8) 					     bool warn)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  9) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 10) 	unsigned long s, j1, j2, i1, i2, imm10, imm11;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 11) 	unsigned long first, second;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 12) 	long offset;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 13) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 14) 	offset = (long)addr - (long)(pc + 4);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 15) 	if (offset < -16777216 || offset > 16777214) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 16) 		WARN_ON_ONCE(warn);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 17) 		return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 18) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 19) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 20) 	s	= (offset >> 24) & 0x1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 21) 	i1	= (offset >> 23) & 0x1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 22) 	i2	= (offset >> 22) & 0x1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 23) 	imm10	= (offset >> 12) & 0x3ff;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 24) 	imm11	= (offset >>  1) & 0x7ff;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 25) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 26) 	j1 = (!i1) ^ s;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 27) 	j2 = (!i2) ^ s;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 28) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 29) 	first = 0xf000 | (s << 10) | imm10;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 30) 	second = 0x9000 | (j1 << 13) | (j2 << 11) | imm11;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 31) 	if (link)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 32) 		second |= 1 << 14;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 33) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 34) 	return __opcode_thumb32_compose(first, second);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 35) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 36) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 37) static unsigned long __arm_gen_branch_arm(unsigned long pc, unsigned long addr,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 38) 					  bool link, bool warn)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 39) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 40) 	unsigned long opcode = 0xea000000;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 41) 	long offset;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 42) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 43) 	if (link)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 44) 		opcode |= 1 << 24;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 45) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 46) 	offset = (long)addr - (long)(pc + 8);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 47) 	if (unlikely(offset < -33554432 || offset > 33554428)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 48) 		WARN_ON_ONCE(warn);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 49) 		return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 50) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 51) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 52) 	offset = (offset >> 2) & 0x00ffffff;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 53) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 54) 	return opcode | offset;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 55) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 56) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 57) unsigned long
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 58) __arm_gen_branch(unsigned long pc, unsigned long addr, bool link, bool warn)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 59) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 60) 	if (IS_ENABLED(CONFIG_THUMB2_KERNEL))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 61) 		return __arm_gen_branch_thumb2(pc, addr, link, warn);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 62) 	else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 63) 		return __arm_gen_branch_arm(pc, addr, link, warn);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 64) }