| Russell King | fa0d1db | 2012-03-30 11:51:46 +0100 | [diff] [blame] | 1 | #include <linux/bug.h> |
| Rabin Vincent | d82227c | 2012-02-18 17:50:06 +0100 | [diff] [blame] | 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 | } |