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