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