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