1 #include <linux/bug.h> 2 #include <linux/kernel.h> 3 #include <asm/opcodes.h> 4 5 static 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 35 static 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 55 unsigned 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