arch/arm/kernel/insn.c

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