bsd/dev/arm/disassembler.c

   1 /*
   2  * Copyright (c) 2017 Apple Inc. All rights reserved.
   3  *
   4  * Disassemblers for ARM (arm), Thumb (thumb16), and Thumb2 (thumb32).
   5  *
   6  * Each disassembly begins with a call to dtrace_decode_arm or dtrace_decode_thumb. The thumb
   7  * decoder will then call dtrace_decode_thumb16 or dtrace_decode_thumb32 as appropriate.
   8  *
   9  * The respective disassembly functions are all of the form {arm,thumb16,thumb32}_type. They
  10  * follow the ordering and breakdown in the ARMv7 Architecture Reference Manual.
  11  */
  12
  13 #include  <sys/fasttrap_isa.h>
  14
  15 #define BITS(x,n,mask) (((x) >> (n)) & (mask))
  16
  17 static uint32_t thumb32_instword_to_arm(uint16_t hw1, uint16_t hw2)
  18 {
  19         return (hw1 << 16) | hw2;
  20 }
  21
  22 int dtrace_decode_arm(uint32_t instr);
  23 int dtrace_decode_thumb(uint32_t instr);
  24
  25 /*
  26  * VFP decoder - shared between ARM and THUMB32 mode
  27  */
  28
  29 static
  30 int vfp_struct_loadstore(uint32_t instr)
  31 {
  32         if (ARM_RM(instr) != REG_PC && ARM_RN(instr) != REG_PC)
  33                 return FASTTRAP_T_COMMON;
  34
  35         return FASTTRAP_T_INV;
  36 }
  37
  38 static
  39 int vfp_64transfer(uint32_t instr)
  40 {
  41         /* These instructions all use RD and RN */
  42         if (ARM_RD(instr) != REG_PC && ARM_RN(instr) != REG_PC)
  43                 return FASTTRAP_T_COMMON;
  44
  45         return FASTTRAP_T_INV;
  46 }
  47
  48 static
  49 int vfp_transfer(uint32_t instr)
  50 {
  51         /* These instructions all use RD only */
  52         if (ARM_RD(instr) != REG_PC)
  53                 return FASTTRAP_T_COMMON;
  54
  55         return FASTTRAP_T_INV;
  56 }
  57
  58 static
  59 int vfp_loadstore(uint32_t instr)
  60 {
  61         int opcode = BITS(instr,20,0x1F);
  62
  63         /* Instrument VLDR */
  64         if ((opcode & 0x13) == 0x11 && ARM_RN(instr) == REG_PC)
  65                 return FASTTRAP_T_VLDR_PC_IMMED;
  66
  67         /* These instructions all use RN only */
  68         if (ARM_RN(instr) != REG_PC)
  69                 return FASTTRAP_T_COMMON;
  70
  71         return FASTTRAP_T_INV;
  72 }
  73
  74 /*
  75  * ARM decoder
  76  */
  77
  78 static
  79 int arm_unconditional_misc(uint32_t instr)
  80 {
  81         int op = BITS(instr,20,0x7F);
  82
  83         if ((op & 0x60) == 0x20) {
  84                 /* VFP data processing uses its own registers */
  85                 return FASTTRAP_T_COMMON;
  86         }
  87
  88         if ((op & 0x71) == 0x40) {
  89                 return vfp_struct_loadstore(instr);
  90         }
  91
  92         return FASTTRAP_T_INV;
  93 }
  94
  95 static
  96 int arm_unconditional(uint32_t instr)
  97 {
  98         if (BITS(instr,27,0x1) == 0)
  99                 return arm_unconditional_misc(instr);
 100
 101         /* The rest are privileged or BL/BLX, do not instrument */
 102
 103         /* Do not need to instrument BL/BLX either, see comment in arm_misc(uint32_t) */
 104
 105         return FASTTRAP_T_INV;
 106 }
 107
 108 static
 109 int arm_syscall_coproc(uint32_t instr)
 110 {
 111         /* Instrument any VFP data processing instructions, ignore the rest */
 112
 113         int op1 = BITS(instr,20,0x3F), coproc = BITS(instr,8,0xF), op = BITS(instr,4,0x1);
 114
 115         if ((op1 & 0x3E) == 0 || (op1 & 0x30) == 0x30) {
 116                 /* Undefined or swi */
 117                 return FASTTRAP_T_INV;
 118         }
 119
 120         if ((coproc & 0xE) == 0xA) {
 121                 /* VFP instruction */
 122
 123                 if ((op1 & 0x20) == 0 && (op1 & 0x3A) != 0)
 124                         return vfp_loadstore(instr);
 125
 126                 if ((op1 & 0x3E) == 0x04)
 127                         return vfp_64transfer(instr);
 128
 129                 if ((op1 & 0x30) == 0x20) {
 130                         /* VFP data processing or 8, 16, or 32 bit move between ARM reg and VFP reg */
 131                         if (op == 0) {
 132                                 /* VFP data processing uses its own registers */
 133                                 return FASTTRAP_T_COMMON;
 134                         } else {
 135                                 return vfp_transfer(instr);
 136                         }
 137                 }
 138         }
 139
 140         return FASTTRAP_T_INV;
 141 }
 142
 143 static
 144 int arm_branch_link_blockdata(uint32_t instr)
 145 {
 146         int branch = BITS(instr,25,0x1), link = BITS(instr,24,0x1), op = BITS(instr,20,0x1F), uses_pc = BITS(instr,15,0x1), uses_lr = BITS(instr,14,0x1);
 147
 148         if (branch == 1) {
 149                 if (link == 0)
 150                         return FASTTRAP_T_B_COND;
 151                 return FASTTRAP_T_INV;
 152         } else {
 153                 /* Only emulate a use of the pc if it's a return from function: ldmia sp!, { ... pc } */
 154                 if (op == 0x0B && ARM_RN(instr) == REG_SP && uses_pc == 1)
 155                         return FASTTRAP_T_LDM_PC;
 156
 157                 /* stmia sp!, { ... lr } doesn't touch the pc, but it is very common, so special case it */
 158                 if (op == 0x12 && ARM_RN(instr) == REG_SP && uses_lr == 1)
 159                         return FASTTRAP_T_STM_LR;
 160
 161                 if (ARM_RN(instr) != REG_PC && uses_pc == 0)
 162                         return FASTTRAP_T_COMMON;
 163         }
 164
 165         return FASTTRAP_T_INV;
 166 }
 167
 168 static
 169 int arm_signed_multiplies(uint32_t instr)
 170 {
 171         int op1 = BITS(instr,20,0x7), op2 = BITS(instr,5,0x7);
 172
 173         /* smlald, smlsld, smmls use RD in addition to RM, RS, and RN */
 174         if ((op1 == 0x4 && (op2 & 0x4) == 0) || (op1 == 0x5 && (op2 & 0x6) == 0x6)) {
 175                 if (ARM_RD(instr) == REG_PC)
 176                         return FASTTRAP_T_INV;
 177         }
 178
 179         if (ARM_RM(instr) != REG_PC && ARM_RS(instr) != REG_PC && ARM_RN(instr) != REG_PC)
 180                 return FASTTRAP_T_COMMON;
 181
 182         return FASTTRAP_T_INV;
 183 }
 184
 185 static
 186 int arm_pack_unpack_sat_reversal(uint32_t instr)
 187 {
 188         int op1 = BITS(instr,20,0x7), op2 = BITS(instr,5,0x7);
 189
 190         /* pkh, sel use RN in addition to RD and RM */
 191         if ((op1 == 0 && (op2 & 0x1) == 0) || (op1 == 0 && op2 == 0x5)) {
 192                 if (ARM_RN(instr) == REG_PC)
 193                         return FASTTRAP_T_INV;
 194         }
 195
 196         if (ARM_RM(instr) != REG_PC && ARM_RD(instr) != REG_PC)
 197                 return FASTTRAP_T_COMMON;
 198
 199         return FASTTRAP_T_INV;
 200 }
 201
 202 static
 203 int arm_parallel_addsub_unsigned(uint32_t instr)
 204 {
 205         if (ARM_RM(instr) != REG_PC && ARM_RD(instr) != REG_PC && ARM_RN(instr) != REG_PC)
 206                 return FASTTRAP_T_COMMON;
 207
 208         return FASTTRAP_T_INV;
 209 }
 210
 211 static
 212 int arm_parallel_addsub_signed(uint32_t instr)
 213 {
 214         if (ARM_RM(instr) != REG_PC && ARM_RD(instr) != REG_PC && ARM_RN(instr) != REG_PC)
 215                 return FASTTRAP_T_COMMON;
 216
 217         return FASTTRAP_T_INV;
 218 }
 219
 220 static
 221 int arm_media(uint32_t instr)
 222 {
 223         int op1 = BITS(instr,20,0x1F), op2 = BITS(instr,5,0x7);
 224
 225         if ((op1 & 0x1C) == 0)
 226                 return arm_parallel_addsub_signed(instr);
 227
 228         if ((op1 & 0x1C) == 0x04)
 229                 return arm_parallel_addsub_unsigned(instr);
 230
 231         if ((op1 & 0x18) == 0x08)
 232                 return arm_pack_unpack_sat_reversal(instr);
 233
 234         if ((op1 & 0x18) == 0x10)
 235                 return arm_signed_multiplies(instr);
 236
 237         if (op1 == 0x1F && op2 == 0x7) {
 238                 /* Undefined instruction */
 239                 return FASTTRAP_T_INV;
 240         }
 241
 242         if (op1 == 0x18 && op2 == 0) {
 243                 /* usad8 usada8 */
 244                 /* The registers are named differently in the reference manual for this instruction
 245                  * but the following positions are correct */
 246
 247                 if (ARM_RM(instr) != REG_PC && ARM_RS(instr) != REG_PC && ARM_RN(instr) != REG_PC)
 248                         return FASTTRAP_T_COMMON;
 249
 250                 return FASTTRAP_T_INV;
 251         }
 252
 253         if ((op1 & 0x1E) == 0x1C && (op2 & 0x3) == 0) {
 254                 /* bfc bfi */
 255                 if (ARM_RD(instr) != REG_PC)
 256                         return FASTTRAP_T_COMMON;
 257
 258                 return FASTTRAP_T_INV;
 259         }
 260
 261         if (((op1 & 0x1E) == 0x1A || (op1 & 0x1E) == 0x1E) && ((op2 & 0x3) == 0x2)) {
 262                 /* sbfx ubfx */
 263                 if (ARM_RM(instr) != REG_PC && ARM_RD(instr) != REG_PC)
 264                         return FASTTRAP_T_COMMON;
 265
 266                 return FASTTRAP_T_INV;
 267         }
 268
 269         return FASTTRAP_T_INV;
 270 }
 271
 272 static
 273 int arm_loadstore_wordbyte(uint32_t instr)
 274 {
 275         /* Instrument PC relative load with immediate, ignore any other uses of the PC */
 276         int R = BITS(instr,25,0x1), L = BITS(instr,20,0x1);
 277
 278         if (R == 1) {
 279                 /* Three register load/store */
 280                 if (ARM_RM(instr) != REG_PC && ARM_RD(instr) != REG_PC && ARM_RN(instr) != REG_PC)
 281                         return FASTTRAP_T_COMMON;
 282         } else {
 283                 /* Immediate load/store, but still do not support ldr pc, [pc...] */
 284                 if (L == 1 && ARM_RN(instr) == REG_PC && ARM_RD(instr) != REG_PC)
 285                         return FASTTRAP_T_LDR_PC_IMMED;
 286
 287                 if (ARM_RD(instr) != REG_PC && ARM_RN(instr) != REG_PC)
 288                         return FASTTRAP_T_COMMON;
 289         }
 290
 291         return FASTTRAP_T_INV;
 292 }
 293
 294 static
 295 int arm_saturating(uint32_t instr)
 296 {
 297         if (ARM_RM(instr) != REG_PC && ARM_RD(instr) != REG_PC && ARM_RN(instr) != REG_PC)
 298                 return FASTTRAP_T_COMMON;
 299
 300         return FASTTRAP_T_INV;
 301 }
 302
 303 static
 304 int arm_misc(uint32_t instr)
 305 {
 306         int op = BITS(instr,21,0x3), __unused op1 = BITS(instr,16,0xF), op2 = BITS(instr,4,0x7);
 307
 308         if (op2 == 1 && op == 1)
 309                 return FASTTRAP_T_BX_REG;
 310
 311         /* We do not need to emulate BLX for entry/return probes; if we eventually support full offset
 312          * tracing, then we will. This is because BLX overwrites the link register, so a function that
 313          * can execute this as its first instruction is a special function indeed.
 314          */
 315
 316         if (op2 == 0x5)
 317                 return arm_saturating(instr);
 318
 319         return FASTTRAP_T_INV;
 320 }
 321
 322 static
 323 int arm_msr_hints(__unused uint32_t instr)
 324 {
 325         /* These deal with the psr, not instrumented */
 326
 327         return FASTTRAP_T_INV;
 328 }
 329
 330 static
 331 int arm_sync_primitive(__unused uint32_t instr)
 332 {
 333         /* TODO will instrumenting these interfere with any kernel usage of these instructions? */
 334         /* Don't instrument for now */
 335
 336         return FASTTRAP_T_INV;
 337 }
 338
 339 static
 340 int arm_extra_loadstore_unpriv(uint32_t instr)
 341 {
 342         int op = BITS(instr,20,0x1), __unused op2 = BITS(instr,5,0x3), immed = BITS(instr,22,0x1);
 343
 344         if (op == 0 && (op2 & 0x2) == 0x2) {
 345                 /* Unpredictable or undefined */
 346                 return FASTTRAP_T_INV;
 347         }
 348
 349         if (immed == 1) {
 350                 if (ARM_RD(instr) != REG_PC && ARM_RN(instr) != REG_PC)
 351                         return FASTTRAP_T_COMMON;
 352         } else {
 353                 if (ARM_RM(instr) != REG_PC && ARM_RD(instr) != REG_PC && ARM_RN(instr) != REG_PC)
 354                         return FASTTRAP_T_COMMON;
 355         }
 356
 357         return FASTTRAP_T_INV;
 358 }
 359
 360 static
 361 int arm_extra_loadstore(uint32_t instr)
 362 {
 363         int op1 = BITS(instr,20,0x1F);
 364
 365         /* There are two variants, and we do not instrument either of them that use the PC */
 366
 367         if ((op1 & 0x4) == 0) {
 368                 /* Variant 1, register */
 369                 if (ARM_RM(instr) != REG_PC && ARM_RD(instr) != REG_PC && ARM_RN(instr) != REG_PC)
 370                         return FASTTRAP_T_COMMON;
 371         } else {
 372                 /* Variant 2, immediate */
 373                 if (ARM_RD(instr) != REG_PC && ARM_RN(instr) != REG_PC)
 374                         return FASTTRAP_T_COMMON;
 375         }
 376
 377         return FASTTRAP_T_INV;
 378 }
 379
 380 static
 381 int arm_halfword_multiply(uint32_t instr)
 382 {
 383         /* Not all multiply instructions use all four registers. The ones that don't should have those
 384          * register locations set to 0, so we can test them anyway.
 385          */
 386
 387         if (ARM_RN(instr) != REG_PC && ARM_RD(instr) != REG_PC && ARM_RS(instr) != REG_PC && ARM_RM(instr) != REG_PC)
 388                 return FASTTRAP_T_COMMON;
 389
 390         return FASTTRAP_T_INV;
 391 }
 392
 393 static
 394 int arm_multiply(uint32_t instr)
 395 {
 396         /* Not all multiply instructions use all four registers. The ones that don't should have those
 397          * register locations set to 0, so we can test them anyway.
 398          */
 399
 400         if (ARM_RN(instr) != REG_PC && ARM_RD(instr) != REG_PC && ARM_RS(instr) != REG_PC && ARM_RM(instr) != REG_PC)
 401                 return FASTTRAP_T_COMMON;
 402
 403         return FASTTRAP_T_INV;
 404 }
 405
 406 static
 407 int arm_dataproc_immed(uint32_t instr)
 408 {
 409         /* All these instructions are either two registers, or one register and have 0 where the other reg would be used */
 410         if (ARM_RN(instr) != REG_PC && ARM_RD(instr) != REG_PC)
 411                 return FASTTRAP_T_COMMON;
 412
 413         return FASTTRAP_T_INV;
 414 }
 415
 416 static
 417 int arm_dataproc_regshift(uint32_t instr)
 418 {
 419         /* All these instructions are either four registers, or three registers and have 0 where there last reg would be used */
 420         if (ARM_RN(instr) != REG_PC && ARM_RD(instr) != REG_PC && ARM_RS(instr) != REG_PC && ARM_RM(instr) != REG_PC)
 421                 return FASTTRAP_T_COMMON;
 422
 423         return FASTTRAP_T_INV;
 424 }
 425
 426 static
 427 int arm_dataproc_reg(uint32_t instr)
 428 {
 429         int op1 = BITS(instr,20,0x1F), op2 = BITS(instr,7,0x1F), op3 = BITS(instr,5,0x3);
 430
 431         if (op1 == 0x11 || op1 == 0x13 || op1 == 0x15 || op1 == 0x17) {
 432                 /* These are comparison flag setting instructions and do not have RD */
 433                 if (ARM_RN(instr) != REG_PC && ARM_RM(instr) != REG_PC)
 434                         return FASTTRAP_T_COMMON;
 435
 436                 return FASTTRAP_T_INV;
 437         }
 438
 439         /* The rest can, in theory, write or use the PC. The only one we instrument is mov pc, reg.
 440          * movs pc, reg is a privileged instruction so we don't instrument that variant. The s bit
 441          * is bit 0 of op1 and should be zero.
 442          */
 443         if (op1 == 0x1A && op2 == 0 && op3 == 0 && ARM_RD(instr) == REG_PC)
 444                 return FASTTRAP_T_MOV_PC_REG;
 445
 446         /* Any instruction at this point is a three register instruction or two register instruction with RN = 0 */
 447         if (ARM_RN(instr) != REG_PC && ARM_RD(instr) != REG_PC && ARM_RM(instr) != REG_PC)
 448                 return FASTTRAP_T_COMMON;
 449
 450         return FASTTRAP_T_INV;
 451 }
 452
 453 static
 454 int arm_dataproc_misc(uint32_t instr)
 455 {
 456         int op = BITS(instr,25,0x1), op1 = BITS(instr,20,0x1F), op2 = BITS(instr,4,0xF);
 457
 458         if (op == 0) {
 459                 if ((op1 & 0x19) != 0x10 && (op2 & 0x1) == 0)
 460                         return arm_dataproc_reg(instr);
 461
 462                 if ((op1 & 0x19) != 0x10 && (op2 & 0x9) == 0x1)
 463                         return arm_dataproc_regshift(instr);
 464
 465                 if ((op1 & 0x19) == 0x10 && (op2 & 0x8) == 0)
 466                         return arm_misc(instr);
 467
 468                 if ((op1 & 0x19) == 0x19 && (op2 & 0x9) == 0x8)
 469                         return arm_halfword_multiply(instr);
 470
 471                 if ((op1 & 0x10) == 0 && op2 == 0x9)
 472                         return arm_multiply(instr);
 473
 474                 if ((op1 & 0x10) == 0x10 && op2 == 0x9)
 475                         return arm_sync_primitive(instr);
 476
 477                 if ((op1 & 0x12) != 0x02 && (op2 == 0xB || (op2 & 0xD) == 0xD))
 478                         return arm_extra_loadstore(instr);
 479
 480                 if ((op1 & 0x12) == 0x02 && (op2 == 0xB || (op2 & 0xD) == 0xD))
 481                         return arm_extra_loadstore_unpriv(instr);
 482         } else {
 483                 if ((op1 & 0x19) != 0x10)
 484                         return arm_dataproc_immed(instr);
 485
 486                 if (op1 == 0x10) {
 487                         /* 16 bit immediate load (mov (immed)) [encoding A2] */
 488                         if (ARM_RD(instr) != REG_PC)
 489                                 return FASTTRAP_T_COMMON;
 490
 491                         return FASTTRAP_T_INV;
 492                 }
 493
 494                 if (op1 == 0x14) {
 495                         /* high halfword 16 bit immediate load (movt) [encoding A1] */
 496                         if (ARM_RD(instr) != REG_PC)
 497                                 return FASTTRAP_T_COMMON;
 498
 499                         return FASTTRAP_T_INV;
 500                 }
 501
 502                 if ((op1 & 0x1B) == 0x12)
 503                         return arm_msr_hints(instr);
 504         }
 505
 506         return FASTTRAP_T_INV;
 507 }
 508
 509 int dtrace_decode_arm(uint32_t instr)
 510 {
 511         int cond = BITS(instr,28,0xF), op1 = BITS(instr,25,0x7), op = BITS(instr,4,0x1);
 512
 513         if (cond == 0xF)
 514                 return arm_unconditional(instr);
 515
 516         if ((op1 & 0x6) == 0)
 517                 return arm_dataproc_misc(instr);
 518
 519         if (op1 == 0x2)
 520                 return arm_loadstore_wordbyte(instr);
 521
 522         if (op1 == 0x3 && op == 0)
 523                 return arm_loadstore_wordbyte(instr);
 524
 525         if (op1 == 0x3 && op == 1)
 526                 return arm_media(instr);
 527
 528         if ((op1 & 0x6) == 0x4)
 529                 return arm_branch_link_blockdata(instr);
 530
 531         if ((op1 & 0x6) == 0x6)
 532                 return arm_syscall_coproc(instr);
 533
 534         return FASTTRAP_T_INV;
 535 }
 536
 537
 538 /*
 539  * Thumb 16-bit decoder
 540  */
 541
 542 static
 543 int thumb16_cond_supervisor(uint16_t instr)
 544 {
 545         int opcode = BITS(instr,8,0xF);
 546
 547         if ((opcode & 0xE) != 0xE)
 548                 return FASTTRAP_T_B_COND;
 549
 550         return FASTTRAP_T_INV;
 551 }
 552
 553 static
 554 int thumb16_misc(uint16_t instr)
 555 {
 556         int opcode = BITS(instr,5,0x7F);
 557
 558         if ((opcode & 0x70) == 0x30 || (opcode & 0x70) == 0x70) {
 559                 /* setend, cps, breakpoint, or if-then, not instrumentable */
 560                 return FASTTRAP_T_INV;
 561         } else if ((opcode & 0x78) == 0x28) {
 562                 /* Doesn't modify pc, but this happens a lot so make this a special case for emulation */
 563                 return FASTTRAP_T_PUSH_LR;
 564         } else if ((opcode & 0x78) == 0x68) {
 565                 return FASTTRAP_T_POP_PC;
 566         } else if ((opcode & 0x28) == 0x08) {
 567                 return FASTTRAP_T_CB_N_Z;
 568         }
 569
 570         /* All other instructions work on low regs only and are instrumentable */
 571         return FASTTRAP_T_COMMON;
 572 }
 573
 574 static
 575 int thumb16_loadstore_single(__unused uint16_t instr)
 576 {
 577         /* These all access the low registers or SP only */
 578         return FASTTRAP_T_COMMON;
 579 }
 580
 581 static
 582 int thumb16_data_special_and_branch(uint16_t instr)
 583 {
 584         int opcode = BITS(instr,6,0xF);
 585
 586         if (opcode == 0x4) {
 587                 /* Unpredictable */
 588                 return FASTTRAP_T_INV;
 589         } else if ((opcode & 0xC) == 0xC) {
 590                 /* bx or blx */
 591                 /* Only instrument the bx */
 592                 if ((opcode & 0x2) == 0)
 593                         return FASTTRAP_T_BX_REG;
 594                 return FASTTRAP_T_INV;
 595         } else {
 596                 /* Data processing on high registers, only instrument mov pc, reg */
 597                 if ((opcode & 0xC) == 0x8 && THUMB16_HRD(instr) == REG_PC)
 598                         return FASTTRAP_T_CPY_PC;
 599
 600                 if (THUMB16_HRM(instr) != REG_PC && THUMB16_HRD(instr) != REG_PC)
 601                         return FASTTRAP_T_COMMON;
 602         }
 603
 604         return FASTTRAP_T_INV;
 605 }
 606
 607 static
 608 int thumb16_data_proc(__unused uint16_t instr)
 609 {
 610         /* These all access the low registers only */
 611         return FASTTRAP_T_COMMON;
 612 }
 613
 614 static
 615 int thumb16_shift_addsub_move_compare(__unused uint16_t instr)
 616 {
 617         /* These all access the low registers only */
 618         return FASTTRAP_T_COMMON;
 619 }
 620
 621 static
 622 int dtrace_decode_thumb16(uint16_t instr)
 623 {
 624         int opcode = BITS(instr,10,0x3F);
 625
 626         if ((opcode & 0x30) == 0)
 627                 return thumb16_shift_addsub_move_compare(instr);
 628
 629         if (opcode == 0x10)
 630                 return thumb16_data_proc(instr);
 631
 632         if (opcode == 0x11)
 633                 return thumb16_data_special_and_branch(instr);
 634
 635         if ((opcode & 0x3E) == 0x12) {
 636                 /* ldr (literal) */
 637                 return FASTTRAP_T_LDR_PC_IMMED;
 638         }
 639
 640         if ((opcode & 0x3C) == 0x14 || (opcode & 0x38) == 0x18 || (opcode & 0x38) == 0x20)
 641                 return thumb16_loadstore_single(instr);
 642
 643         if ((opcode & 0x3E) == 0x28) {
 644                 /* adr, uses the pc */
 645                 return FASTTRAP_T_INV;
 646         }
 647
 648         if ((opcode & 0x3E) == 0x2A) {
 649                 /* add (sp plus immediate) */
 650                 return FASTTRAP_T_COMMON;
 651         }
 652
 653         if ((opcode & 0x3C) == 0x2C)
 654                 return thumb16_misc(instr);
 655
 656         if ((opcode & 0x3E) == 0x30) {
 657                 /* stm - can't access high registers */
 658                 return FASTTRAP_T_COMMON;
 659         }
 660
 661         if ((opcode & 0x3E) == 0x32) {
 662                 /* ldm - can't access high registers */
 663                 return FASTTRAP_T_COMMON;
 664         }
 665
 666         if ((opcode & 0x3C) == 0x34) {
 667                 return thumb16_cond_supervisor(instr);
 668         }
 669
 670         if ((opcode & 0x3E) == 0x38) {
 671                 /* b unconditional */
 672                 return FASTTRAP_T_B_UNCOND;
 673         }
 674
 675         return FASTTRAP_T_INV;
 676 }
 677
 678 /*
 679  * Thumb 32-bit decoder
 680  */
 681
 682 static
 683 int thumb32_coproc(uint16_t instr1, uint16_t instr2)
 684 {
 685         /* Instrument any VFP data processing instructions, ignore the rest */
 686
 687         int op1 = BITS(instr1,4,0x3F), coproc = BITS(instr2,8,0xF), op = BITS(instr2,4,0x1);
 688
 689         if ((op1 & 0x3E) == 0) {
 690                 /* Undefined */
 691                 return FASTTRAP_T_INV;
 692         }
 693
 694         if ((coproc & 0xE) == 0xA || (op1 & 0x30) == 0x30) {
 695                 /* VFP instruction */
 696                 uint32_t instr = thumb32_instword_to_arm(instr1,instr2);
 697
 698                 if ((op1 & 0x30) == 0x30) {
 699                         /* VFP data processing uses its own registers */
 700                         return FASTTRAP_T_COMMON;
 701                 }
 702
 703                 if ((op1 & 0x3A) == 0x02 || (op1 & 0x38) == 0x08 || (op1 & 0x30) == 0x10)
 704                         return vfp_loadstore(instr);
 705
 706                 if ((op1 & 0x3E) == 0x04)
 707                         return vfp_64transfer(instr);
 708
 709                 if ((op1 & 0x30) == 0x20) {
 710                         /* VFP data processing or 8, 16, or 32 bit move between ARM reg and VFP reg */
 711                         if (op == 0) {
 712                                 /* VFP data processing uses its own registers */
 713                                 return FASTTRAP_T_COMMON;
 714                         } else {
 715                                 return vfp_transfer(instr);
 716                         }
 717                 }
 718         }
 719
 720         return FASTTRAP_T_INV;
 721 }
 722
 723 static
 724 int thumb32_longmultiply(uint16_t instr1, uint16_t instr2)
 725 {
 726         int op1 = BITS(instr1,4,0x7), op2 = BITS(instr2,4,0xF);
 727
 728         if ((op1 == 1 && op2 == 0xF) || (op1 == 0x3 && op2 == 0xF)) {
 729                 /* Three register instruction */
 730                 if (THUMB32_RM(instr1,instr2) != REG_PC && THUMB32_RD(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 731                         return FASTTRAP_T_COMMON;
 732         } else {
 733                 /* Four register instruction */
 734                 if (THUMB32_RM(instr1,instr2) != REG_PC && THUMB32_RD(instr1,instr2) != REG_PC &&
 735                     THUMB32_RT(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 736                         return FASTTRAP_T_COMMON;
 737         }
 738
 739         return FASTTRAP_T_INV;
 740 }
 741
 742 static
 743 int thumb32_multiply(uint16_t instr1, uint16_t instr2)
 744 {
 745         int op1 = BITS(instr1,4,0x7), op2 = BITS(instr2,4,0x3);
 746
 747         if ((op1 == 0 && op2 == 1) || (op1 == 0x6 && (op2 & 0x2) == 0)) {
 748                 if (THUMB32_RT(instr1,instr2) == REG_PC)
 749                         return FASTTRAP_T_INV;
 750         }
 751
 752         if (THUMB32_RM(instr1,instr2) != REG_PC && THUMB32_RD(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 753                 return FASTTRAP_T_COMMON;
 754
 755         return FASTTRAP_T_INV;
 756 }
 757
 758 static
 759 int thumb32_misc(uint16_t instr1, uint16_t instr2)
 760 {
 761         if (THUMB32_RM(instr1,instr2) != REG_PC && THUMB32_RD(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 762                 return FASTTRAP_T_COMMON;
 763
 764         return FASTTRAP_T_INV;
 765 }
 766
 767 static
 768 int thumb32_parallel_addsub_unsigned(uint16_t instr1, uint16_t instr2)
 769 {
 770         if (THUMB32_RM(instr1,instr2) != REG_PC && THUMB32_RD(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 771                 return FASTTRAP_T_COMMON;
 772
 773         return FASTTRAP_T_INV;
 774 }
 775
 776 static
 777 int thumb32_parallel_addsub_signed(uint16_t instr1, uint16_t instr2)
 778 {
 779         if (THUMB32_RM(instr1,instr2) != REG_PC && THUMB32_RD(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 780                 return FASTTRAP_T_COMMON;
 781
 782         return FASTTRAP_T_INV;
 783 }
 784
 785 static
 786 int thumb32_dataproc_reg(uint16_t instr1, uint16_t instr2)
 787 {
 788         int op1 = BITS(instr1,4,0xF), op2 = BITS(instr2,4,0xF);
 789
 790         if (((0 <= op1) && (op1 <= 5)) && (op2 & 0x8) == 0x8) {
 791                 if (THUMB32_RM(instr1,instr2) != REG_PC && THUMB32_RD(instr1,instr2) != REG_PC)
 792                         return FASTTRAP_T_COMMON;
 793         }
 794
 795         if ((op1 & 0x8) == 0 && op2 == 0) {
 796                 if (THUMB32_RM(instr1,instr2) != REG_PC && THUMB32_RD(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 797                         return FASTTRAP_T_COMMON;
 798         }
 799
 800         if ((op1 & 0x8) == 0x8 && (op2 & 0xC) == 0)
 801                 return thumb32_parallel_addsub_signed(instr1,instr2);
 802
 803         if ((op1 & 0x8) == 0x8 && (op2 & 0xC) == 0x4)
 804                 return thumb32_parallel_addsub_unsigned(instr1,instr2);
 805
 806         if ((op1 & 0xC) == 0x8 && (op2 & 0xC) == 0x8)
 807                 return thumb32_misc(instr1,instr2);
 808
 809         return FASTTRAP_T_INV;
 810 }
 811
 812 static
 813 int thumb32_dataproc_regshift(uint16_t instr1, uint16_t instr2)
 814 {
 815         int op = BITS(instr1,5,0xF), S = BITS(instr1,4,0x1);
 816
 817         if (op == 0 || op == 0x4 || op == 0x8 || op == 0xD) {
 818                 /* These become test instructions if S is 1 and Rd is PC, otherwise they are data instructions. */
 819                 if (S == 1) {
 820                         if (THUMB32_RM(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 821                                 return FASTTRAP_T_COMMON;
 822                 } else {
 823                         if (THUMB32_RM(instr1,instr2) != REG_PC && THUMB32_RD(instr1,instr2) != REG_PC &&
 824                             THUMB32_RN(instr1,instr2) != REG_PC)
 825                                 return FASTTRAP_T_COMMON;
 826                 }
 827         } else if (op == 0x2 || op == 0x3) {
 828                 /* These become moves if RN is PC, otherwise they are data insts. We don't instrument mov pc, reg here */
 829                 if (THUMB32_RM(instr1,instr2) != REG_PC && THUMB32_RD(instr1,instr2) != REG_PC)
 830                         return FASTTRAP_T_COMMON;
 831         } else {
 832                 /* Normal three register instruction */
 833                 if (THUMB32_RM(instr1,instr2) != REG_PC && THUMB32_RD(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 834                         return FASTTRAP_T_COMMON;
 835         }
 836
 837         return FASTTRAP_T_INV;
 838 }
 839
 840 static
 841 int thumb32_store_single(uint16_t instr1, uint16_t instr2)
 842 {
 843         int op1 = BITS(instr1,5,0x7), op2 = BITS(instr2,6,0x3F);
 844
 845         /* Do not support any use of the pc yet */
 846         if ((op1 == 0 || op1 == 1 || op1 == 2) && (op2 & 0x20) == 0) {
 847                 /* str (register) uses RM */
 848                 if (THUMB32_RM(instr1,instr2) == REG_PC)
 849                         return FASTTRAP_T_INV;
 850         }
 851
 852         if (THUMB32_RT(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 853                 return FASTTRAP_T_COMMON;
 854
 855         return FASTTRAP_T_INV;
 856 }
 857
 858 static
 859 int thumb32_loadbyte_memhint(uint16_t instr1, uint16_t instr2)
 860 {
 861         int op1 = BITS(instr1,7,0x3), __unused op2 = BITS(instr2,6,0x3F);
 862
 863         /* Do not support any use of the pc yet */
 864         if ((op1 == 0 || op1 == 0x2) && THUMB32_RM(instr1,instr2) == REG_PC)
 865                 return FASTTRAP_T_INV;
 866
 867         if (THUMB32_RT(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 868                 return FASTTRAP_T_COMMON;
 869
 870         return FASTTRAP_T_INV;
 871 }
 872
 873 static
 874 int thumb32_loadhalfword_memhint(uint16_t instr1, uint16_t instr2)
 875 {
 876         int op1 = BITS(instr1,7,0x3), op2 = BITS(instr2,6,0x3F);
 877
 878         /* Do not support any use of the PC yet */
 879         if (op1 == 0 && op2 == 0 && THUMB32_RM(inst1,instr2) == REG_PC)
 880                 return FASTTRAP_T_INV;
 881
 882         if (THUMB32_RT(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 883                 return FASTTRAP_T_COMMON;
 884
 885         return FASTTRAP_T_INV;
 886 }
 887
 888 static
 889 int thumb32_loadword(uint16_t instr1, uint16_t instr2)
 890 {
 891         int op1 = BITS(instr1,7,0x3), op2 = BITS(instr2,6,0x3F);
 892
 893         if ((op1 & 0x2) == 0 && THUMB32_RN(instr1,instr2) == REG_PC && THUMB32_RT(instr1,instr2) != REG_PC)
 894                 return FASTTRAP_T_LDR_PC_IMMED;
 895
 896         if (op1 == 0 && op2 == 0) {
 897                 /* ldr (register) uses an additional reg */
 898                 if (THUMB32_RM(instr1,instr2) == REG_PC)
 899                         return FASTTRAP_T_INV;
 900         }
 901
 902         if (THUMB32_RT(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 903                 return FASTTRAP_T_COMMON;
 904
 905         return FASTTRAP_T_INV;
 906 }
 907
 908 static
 909 int thumb32_loadstore_double_exclusive_table(__unused uint16_t instr1, __unused uint16_t instr2)
 910 {
 911         /* Don't instrument any of these */
 912
 913         return FASTTRAP_T_INV;
 914 }
 915
 916 static
 917 int thumb32_loadstore_multiple(uint16_t instr1, uint16_t instr2)
 918 {
 919         int op = BITS(instr1,7,0x3), L = BITS(instr1,4,0x1), uses_pc = BITS(instr2,15,0x1), uses_lr = BITS(instr2,14,0x1);
 920
 921         if (op == 0 || op == 0x3) {
 922                 /* Privileged instructions: srs, rfe */
 923                 return FASTTRAP_T_INV;
 924         }
 925
 926         /* Only emulate a use of the pc if it's a return from function: ldmia sp!, { ... pc }, aka pop { ... pc } */
 927         if (op == 0x1 && L == 1 && THUMB32_RN(instr1,instr2) == REG_SP && uses_pc == 1)
 928                 return FASTTRAP_T_LDM_PC;
 929
 930         /* stmia sp!, { ... lr }, aka push { ... lr } doesn't touch the pc, but it is very common, so special case it */
 931         if (op == 0x2 && L == 0 && THUMB32_RN(instr1,instr2) == REG_SP && uses_lr == 1)
 932                 return FASTTRAP_T_STM_LR;
 933
 934         if (THUMB32_RN(instr1,instr2) != REG_PC && uses_pc == 0)
 935                 return FASTTRAP_T_COMMON;
 936
 937         return FASTTRAP_T_INV;
 938 }
 939
 940 static
 941 int thumb32_misc_control(__unused uint16_t instr1, __unused uint16_t instr2)
 942 {
 943         /* Privileged, and instructions dealing with ThumbEE */
 944         return FASTTRAP_T_INV;
 945 }
 946
 947 static
 948 int thumb32_cps_hints(__unused uint16_t instr1, __unused uint16_t instr2)
 949 {
 950         /* Privileged */
 951         return FASTTRAP_T_INV;
 952 }
 953
 954 static
 955 int thumb32_b_misc_control(uint16_t instr1, uint16_t instr2)
 956 {
 957         int op = BITS(instr1,4,0x7F), op1 = BITS(instr2,12,0x7), __unused op2 = BITS(instr2,8,0xF);
 958
 959         if ((op1 & 0x5) == 0) {
 960                 if ((op & 0x38) != 0x38)
 961                         return FASTTRAP_T_B_COND;
 962
 963                 if (op == 0x3A)
 964                         return thumb32_cps_hints(instr1,instr2);
 965
 966                 if (op == 0x3B)
 967                         return thumb32_misc_control(instr1,instr2);
 968         }
 969
 970         if ((op1 & 0x5) == 1)
 971                 return FASTTRAP_T_B_UNCOND;
 972
 973         return FASTTRAP_T_INV;
 974 }
 975
 976 static
 977 int thumb32_dataproc_plain_immed(uint16_t instr1, uint16_t instr2)
 978 {
 979         int op = BITS(instr1,4,0x1F);
 980
 981         if (op == 0x04 || op == 0x0C || op == 0x16) {
 982                 /* mov, movt, bfi, bfc */
 983                 /* These use only RD */
 984                 if (THUMB32_RD(instr1,instr2) != REG_PC)
 985                         return FASTTRAP_T_COMMON;
 986         } else {
 987                 if (THUMB32_RD(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
 988                         return FASTTRAP_T_COMMON;
 989         }
 990
 991         return FASTTRAP_T_INV;
 992 }
 993
 994 static
 995 int thumb32_dataproc_mod_immed(uint16_t instr1, uint16_t instr2)
 996 {
 997         int op = BITS(instr1,5,0xF), S = BITS(instr1,4,0x1);
 998
 999         if (op == 0x2 || op == 0x3) {
1000                 /* These allow REG_PC in RN, but it doesn't mean use the PC! */
1001                 if (THUMB32_RD(instr1,instr2) != REG_PC)
1002                         return FASTTRAP_T_COMMON;
1003         }
1004
1005         if (op == 0 || op == 0x4 || op == 0x8 || op == 0xD) {
1006                 /* These are test instructions, if the sign bit is set and RD is the PC. */
1007                 if (S && THUMB32_RD(instr1,instr2) == REG_PC)
1008                         return FASTTRAP_T_COMMON;
1009         }
1010
1011         if (THUMB32_RD(instr1,instr2) != REG_PC && THUMB32_RN(instr1,instr2) != REG_PC)
1012                 return FASTTRAP_T_COMMON;
1013
1014         return FASTTRAP_T_INV;
1015 }
1016
1017 static
1018 int dtrace_decode_thumb32(uint16_t instr1, uint16_t instr2)
1019 {
1020         int op1 = BITS(instr1,11,0x3), op2 = BITS(instr1,4,0x7F), op = BITS(instr2,15,0x1);
1021
1022         if (op1 == 0x1) {
1023                 if ((op2 & 0x64) == 0)
1024                         return thumb32_loadstore_multiple(instr1,instr2);
1025
1026                 if ((op2 & 0x64) == 0x04)
1027                         return thumb32_loadstore_double_exclusive_table(instr1,instr2);
1028
1029                 if ((op2 & 0x60) == 0x20)
1030                         return thumb32_dataproc_regshift(instr1,instr2);
1031
1032                 if ((op2 & 0x40) == 0x40)
1033                         return thumb32_coproc(instr1,instr2);
1034         }
1035
1036         if (op1 == 0x2) {
1037                 if ((op2 & 0x20) == 0 && op == 0)
1038                         return thumb32_dataproc_mod_immed(instr1,instr2);
1039
1040                 if ((op2 & 0x20) == 0x20 && op == 0)
1041                         return thumb32_dataproc_plain_immed(instr1,instr2);
1042
1043                 if (op == 1)
1044                         return thumb32_b_misc_control(instr1,instr2);
1045         }
1046
1047         if (op1 == 0x3) {
1048                 if ((op2 & 0x71) == 0)
1049                         return thumb32_store_single(instr1,instr2);
1050
1051                 if ((op2 & 0x71) == 0x10) {
1052                         return vfp_struct_loadstore(thumb32_instword_to_arm(instr1,instr2));
1053                 }
1054
1055                 if ((op2 & 0x67) == 0x01)
1056                         return thumb32_loadbyte_memhint(instr1,instr2);
1057
1058                 if ((op2 & 0x67) == 0x03)
1059                         return thumb32_loadhalfword_memhint(instr1,instr2);
1060
1061                 if ((op2 & 0x67) == 0x05)
1062                         return thumb32_loadword(instr1,instr2);
1063
1064                 if ((op2 & 0x67) == 0x07) {
1065                         /* Undefined instruction */
1066                         return FASTTRAP_T_INV;
1067                 }
1068
1069                 if ((op2 & 0x70) == 0x20)
1070                         return thumb32_dataproc_reg(instr1,instr2);
1071
1072                 if ((op2 & 0x78) == 0x30)
1073                         return thumb32_multiply(instr1,instr2);
1074
1075                 if ((op2 & 0x78) == 0x38)
1076                         return thumb32_longmultiply(instr1,instr2);
1077
1078                 if ((op2 & 0x40) == 0x40)
1079                         return thumb32_coproc(instr1,instr2);
1080         }
1081
1082         return FASTTRAP_T_INV;
1083 }
1084
1085 int dtrace_decode_thumb(uint32_t instr)
1086 {
1087         uint16_t* pInstr = (uint16_t*) &instr;
1088         uint16_t hw1 = pInstr[0], hw2 = pInstr[1];
1089
1090         int size = BITS(hw1,11,0x1F);
1091
1092         if (size == 0x1D || size == 0x1E || size == 0x1F)
1093                 return dtrace_decode_thumb32(hw1,hw2);
1094         else
1095                 return dtrace_decode_thumb16(hw1);
1096 }
1097