git.gag.com Git - fw/openocd/blob - src/target/arm_simulator.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2
   3 /***************************************************************************
   4  *   Copyright (C) 2006 by Dominic Rath                                    *
   5  *   Dominic.Rath@gmx.de                                                   *
   6  *                                                                         *
   7  *   Copyright (C) 2008 by Hongtao Zheng                                   *
   8  *   hontor@126.com                                                        *
   9  ***************************************************************************/
  10
  11 #ifdef HAVE_CONFIG_H
  12 #include "config.h"
  13 #endif
  14
  15 #include "arm.h"
  16 #include "armv4_5.h"
  17 #include "arm_disassembler.h"
  18 #include "arm_simulator.h"
  19 #include <helper/binarybuffer.h>
  20 #include "register.h"
  21 #include <helper/log.h>
  22
  23 static uint32_t arm_shift(uint8_t shift, uint32_t rm,
  24         uint32_t shift_amount, uint8_t *carry)
  25 {
  26         uint32_t return_value = 0;
  27         shift_amount &= 0xff;
  28
  29         if (shift == 0x0) {     /* LSL */
  30                 if ((shift_amount > 0) && (shift_amount <= 32)) {
  31                         return_value = rm << shift_amount;
  32                         *carry = rm >> (32 - shift_amount);
  33                 } else if (shift_amount > 32) {
  34                         return_value = 0x0;
  35                         *carry = 0x0;
  36                 } else /* (shift_amount == 0) */
  37                         return_value = rm;
  38         } else if (shift == 0x1) {      /* LSR */
  39                 if ((shift_amount > 0) && (shift_amount <= 32)) {
  40                         return_value = rm >> shift_amount;
  41                         *carry = (rm >> (shift_amount - 1)) & 1;
  42                 } else if (shift_amount > 32) {
  43                         return_value = 0x0;
  44                         *carry = 0x0;
  45                 } else /* (shift_amount == 0) */
  46                         return_value = rm;
  47         } else if (shift == 0x2) {      /* ASR */
  48                 if ((shift_amount > 0) && (shift_amount <= 32)) {
  49                         /* C right shifts of unsigned values are guaranteed to
  50                          * be logical (shift in zeroes); simulate an arithmetic
  51                          * shift (shift in signed-bit) by adding the sign bit
  52                          * manually
  53                          */
  54                         return_value = rm >> shift_amount;
  55                         if (rm & 0x80000000)
  56                                 return_value |= 0xffffffff << (32 - shift_amount);
  57                 } else if (shift_amount > 32) {
  58                         if (rm & 0x80000000) {
  59                                 return_value = 0xffffffff;
  60                                 *carry = 0x1;
  61                         } else {
  62                                 return_value = 0x0;
  63                                 *carry = 0x0;
  64                         }
  65                 } else /* (shift_amount == 0) */
  66                         return_value = rm;
  67         } else if (shift == 0x3) {      /* ROR */
  68                 if (shift_amount == 0)
  69                         return_value = rm;
  70                 else {
  71                         shift_amount = shift_amount % 32;
  72                         return_value = (rm >> shift_amount) | (rm << (32 - shift_amount));
  73                         *carry = (return_value >> 31) & 0x1;
  74                 }
  75         } else if (shift == 0x4) {      /* RRX */
  76                 return_value = rm >> 1;
  77                 if (*carry)
  78                         rm |= 0x80000000;
  79                 *carry = rm & 0x1;
  80         }
  81
  82         return return_value;
  83 }
  84
  85
  86 static uint32_t arm_shifter_operand(struct arm_sim_interface *sim,
  87         int variant, union arm_shifter_operand shifter_operand,
  88         uint8_t *shifter_carry_out)
  89 {
  90         uint32_t return_value;
  91         int instruction_size;
  92
  93         if (sim->get_state(sim) == ARM_STATE_ARM)
  94                 instruction_size = 4;
  95         else
  96                 instruction_size = 2;
  97
  98         *shifter_carry_out = sim->get_cpsr(sim, 29, 1);
  99
 100         if (variant == 0) /* 32-bit immediate */
 101                 return_value = shifter_operand.immediate.immediate;
 102         else if (variant == 1) {/* immediate shift */
 103                 uint32_t rm = sim->get_reg_mode(sim, shifter_operand.immediate_shift.rm);
 104
 105                 /* adjust RM in case the PC is being read */
 106                 if (shifter_operand.immediate_shift.rm == 15)
 107                         rm += 2 * instruction_size;
 108
 109                 return_value = arm_shift(shifter_operand.immediate_shift.shift,
 110                                 rm, shifter_operand.immediate_shift.shift_imm,
 111                                 shifter_carry_out);
 112         } else if (variant == 2) {      /* register shift */
 113                 uint32_t rm = sim->get_reg_mode(sim, shifter_operand.register_shift.rm);
 114                 uint32_t rs = sim->get_reg_mode(sim, shifter_operand.register_shift.rs);
 115
 116                 /* adjust RM in case the PC is being read */
 117                 if (shifter_operand.register_shift.rm == 15)
 118                         rm += 2 * instruction_size;
 119
 120                 return_value = arm_shift(shifter_operand.immediate_shift.shift,
 121                                 rm, rs, shifter_carry_out);
 122         } else {
 123                 LOG_ERROR("BUG: shifter_operand.variant not 0, 1 or 2");
 124                 return_value = 0xffffffff;
 125         }
 126
 127         return return_value;
 128 }
 129
 130 static int pass_condition(uint32_t cpsr, uint32_t opcode)
 131 {
 132         switch ((opcode & 0xf0000000) >> 28) {
 133                 case 0x0:       /* EQ */
 134                         if (cpsr & 0x40000000)
 135                                 return 1;
 136                         else
 137                                 return 0;
 138                 case 0x1:       /* NE */
 139                         if (!(cpsr & 0x40000000))
 140                                 return 1;
 141                         else
 142                                 return 0;
 143                 case 0x2:       /* CS */
 144                         if (cpsr & 0x20000000)
 145                                 return 1;
 146                         else
 147                                 return 0;
 148                 case 0x3:       /* CC */
 149                         if (!(cpsr & 0x20000000))
 150                                 return 1;
 151                         else
 152                                 return 0;
 153                 case 0x4:       /* MI */
 154                         if (cpsr & 0x80000000)
 155                                 return 1;
 156                         else
 157                                 return 0;
 158                 case 0x5:       /* PL */
 159                         if (!(cpsr & 0x80000000))
 160                                 return 1;
 161                         else
 162                                 return 0;
 163                 case 0x6:       /* VS */
 164                         if (cpsr & 0x10000000)
 165                                 return 1;
 166                         else
 167                                 return 0;
 168                 case 0x7:       /* VC */
 169                         if (!(cpsr & 0x10000000))
 170                                 return 1;
 171                         else
 172                                 return 0;
 173                 case 0x8:       /* HI */
 174                         if ((cpsr & 0x20000000) && !(cpsr & 0x40000000))
 175                                 return 1;
 176                         else
 177                                 return 0;
 178                 case 0x9:       /* LS */
 179                         if (!(cpsr & 0x20000000) || (cpsr & 0x40000000))
 180                                 return 1;
 181                         else
 182                                 return 0;
 183                 case 0xa:       /* GE */
 184                         if (((cpsr & 0x80000000) && (cpsr & 0x10000000))
 185                                 || (!(cpsr & 0x80000000) && !(cpsr & 0x10000000)))
 186                                 return 1;
 187                         else
 188                                 return 0;
 189                 case 0xb:       /* LT */
 190                         if (((cpsr & 0x80000000) && !(cpsr & 0x10000000))
 191                                 || (!(cpsr & 0x80000000) && (cpsr & 0x10000000)))
 192                                 return 1;
 193                         else
 194                                 return 0;
 195                 case 0xc:       /* GT */
 196                         if (!(cpsr & 0x40000000) &&
 197                                 (((cpsr & 0x80000000) && (cpsr & 0x10000000))
 198                                 || (!(cpsr & 0x80000000) && !(cpsr & 0x10000000))))
 199                                 return 1;
 200                         else
 201                                 return 0;
 202                 case 0xd:       /* LE */
 203                         if ((cpsr & 0x40000000) ||
 204                                 ((cpsr & 0x80000000) && !(cpsr & 0x10000000))
 205                                 || (!(cpsr & 0x80000000) && (cpsr & 0x10000000)))
 206                                 return 1;
 207                         else
 208                                 return 0;
 209                 case 0xe:
 210                 case 0xf:
 211                         return 1;
 212
 213         }
 214
 215         LOG_ERROR("BUG: should never get here");
 216         return 0;
 217 }
 218
 219 static int thumb_pass_branch_condition(uint32_t cpsr, uint16_t opcode)
 220 {
 221         return pass_condition(cpsr, (opcode & 0x0f00) << 20);
 222 }
 223
 224 /* simulate a single step (if possible)
 225  * if the dry_run_pc argument is provided, no state is changed,
 226  * but the new pc is stored in the variable pointed at by the argument
 227  */
 228 static int arm_simulate_step_core(struct target *target,
 229         uint32_t *dry_run_pc, struct arm_sim_interface *sim)
 230 {
 231         uint32_t current_pc = sim->get_reg(sim, 15);
 232         struct arm_instruction instruction;
 233         int instruction_size;
 234         int retval = ERROR_OK;
 235
 236         if (sim->get_state(sim) == ARM_STATE_ARM) {
 237                 uint32_t opcode;
 238
 239                 /* get current instruction, and identify it */
 240                 retval = target_read_u32(target, current_pc, &opcode);
 241                 if (retval != ERROR_OK)
 242                         return retval;
 243                 retval = arm_evaluate_opcode(opcode, current_pc, &instruction);
 244                 if (retval != ERROR_OK)
 245                         return retval;
 246                 instruction_size = 4;
 247
 248                 /* check condition code (for all instructions) */
 249                 if (!pass_condition(sim->get_cpsr(sim, 0, 32), opcode)) {
 250                         if (dry_run_pc)
 251                                 *dry_run_pc = current_pc + instruction_size;
 252                         else
 253                                 sim->set_reg(sim, 15, current_pc + instruction_size);
 254
 255                         return ERROR_OK;
 256                 }
 257         } else {
 258                 uint16_t opcode;
 259
 260                 retval = target_read_u16(target, current_pc, &opcode);
 261                 if (retval != ERROR_OK)
 262                         return retval;
 263                 retval = thumb_evaluate_opcode(opcode, current_pc, &instruction);
 264                 if (retval != ERROR_OK)
 265                         return retval;
 266                 instruction_size = 2;
 267
 268                 /* check condition code (only for branch (1) instructions) */
 269                 if ((opcode & 0xf000) == 0xd000
 270                         && !thumb_pass_branch_condition(
 271                         sim->get_cpsr(sim, 0, 32), opcode)) {
 272                         if (dry_run_pc)
 273                                 *dry_run_pc = current_pc + instruction_size;
 274                         else
 275                                 sim->set_reg(sim, 15, current_pc + instruction_size);
 276
 277                         return ERROR_OK;
 278                 }
 279
 280                 /* Deal with 32-bit BL/BLX */
 281                 if ((opcode & 0xf800) == 0xf000) {
 282                         uint32_t high = instruction.info.b_bl_bx_blx.target_address;
 283                         retval = target_read_u16(target, current_pc+2, &opcode);
 284                         if (retval != ERROR_OK)
 285                                 return retval;
 286                         retval = thumb_evaluate_opcode(opcode, current_pc, &instruction);
 287                         if (retval != ERROR_OK)
 288                                 return retval;
 289                         instruction.info.b_bl_bx_blx.target_address += high;
 290                 }
 291         }
 292
 293         /* examine instruction type */
 294
 295         /* branch instructions */
 296         if ((instruction.type >= ARM_B) && (instruction.type <= ARM_BLX)) {
 297                 uint32_t target_address;
 298
 299                 if (instruction.info.b_bl_bx_blx.reg_operand == -1)
 300                         target_address = instruction.info.b_bl_bx_blx.target_address;
 301                 else {
 302                         target_address = sim->get_reg_mode(sim,
 303                                         instruction.info.b_bl_bx_blx.reg_operand);
 304                         if (instruction.info.b_bl_bx_blx.reg_operand == 15)
 305                                 target_address += 2 * instruction_size;
 306                 }
 307
 308                 if (dry_run_pc) {
 309                         *dry_run_pc = target_address & ~1;
 310                         return ERROR_OK;
 311                 } else {
 312                         if (instruction.type == ARM_B)
 313                                 sim->set_reg(sim, 15, target_address);
 314                         else if (instruction.type == ARM_BL) {
 315                                 uint32_t old_pc = sim->get_reg(sim, 15);
 316                                 int t = (sim->get_state(sim) == ARM_STATE_THUMB);
 317                                 sim->set_reg_mode(sim, 14, old_pc + 4 + t);
 318                                 sim->set_reg(sim, 15, target_address);
 319                         } else if (instruction.type == ARM_BX) {
 320                                 if (target_address & 0x1)
 321                                         sim->set_state(sim, ARM_STATE_THUMB);
 322                                 else
 323                                         sim->set_state(sim, ARM_STATE_ARM);
 324                                 sim->set_reg(sim, 15, target_address & 0xfffffffe);
 325                         } else if (instruction.type == ARM_BLX) {
 326                                 uint32_t old_pc = sim->get_reg(sim, 15);
 327                                 int t = (sim->get_state(sim) == ARM_STATE_THUMB);
 328                                 sim->set_reg_mode(sim, 14, old_pc + 4 + t);
 329
 330                                 if (target_address & 0x1)
 331                                         sim->set_state(sim, ARM_STATE_THUMB);
 332                                 else
 333                                         sim->set_state(sim, ARM_STATE_ARM);
 334                                 sim->set_reg(sim, 15, target_address & 0xfffffffe);
 335                         }
 336
 337                         return ERROR_OK;
 338                 }
 339         }
 340         /* data processing instructions, except compare instructions (CMP, CMN, TST, TEQ) */
 341         else if (((instruction.type >= ARM_AND) && (instruction.type <= ARM_RSC))
 342                 || ((instruction.type >= ARM_ORR) && (instruction.type <= ARM_MVN))) {
 343                 uint32_t rd, rn, shifter_operand;
 344                 uint8_t c = sim->get_cpsr(sim, 29, 1);
 345                 uint8_t carry_out;
 346
 347                 rd = 0x0;
 348                 /* ARM_MOV and ARM_MVN does not use Rn */
 349                 if ((instruction.type != ARM_MOV) && (instruction.type != ARM_MVN))
 350                         rn = sim->get_reg_mode(sim, instruction.info.data_proc.rn);
 351                 else
 352                         rn = 0;
 353
 354                 shifter_operand = arm_shifter_operand(sim,
 355                                 instruction.info.data_proc.variant,
 356                                 instruction.info.data_proc.shifter_operand,
 357                                 &carry_out);
 358
 359                 /* adjust Rn in case the PC is being read */
 360                 if (instruction.info.data_proc.rn == 15)
 361                         rn += 2 * instruction_size;
 362
 363                 if (instruction.type == ARM_AND)
 364                         rd = rn & shifter_operand;
 365                 else if (instruction.type == ARM_EOR)
 366                         rd = rn ^ shifter_operand;
 367                 else if (instruction.type == ARM_SUB)
 368                         rd = rn - shifter_operand;
 369                 else if (instruction.type == ARM_RSB)
 370                         rd = shifter_operand - rn;
 371                 else if (instruction.type == ARM_ADD)
 372                         rd = rn + shifter_operand;
 373                 else if (instruction.type == ARM_ADC)
 374                         rd = rn + shifter_operand + (c & 1);
 375                 else if (instruction.type == ARM_SBC)
 376                         rd = rn - shifter_operand - (c & 1) ? 0 : 1;
 377                 else if (instruction.type == ARM_RSC)
 378                         rd = shifter_operand - rn - (c & 1) ? 0 : 1;
 379                 else if (instruction.type == ARM_ORR)
 380                         rd = rn | shifter_operand;
 381                 else if (instruction.type == ARM_BIC)
 382                         rd = rn & ~(shifter_operand);
 383                 else if (instruction.type == ARM_MOV)
 384                         rd = shifter_operand;
 385                 else if (instruction.type == ARM_MVN)
 386                         rd = ~shifter_operand;
 387                 else
 388                         LOG_WARNING("unhandled instruction type");
 389
 390                 if (dry_run_pc) {
 391                         if (instruction.info.data_proc.rd == 15)
 392                                 *dry_run_pc = rd & ~1;
 393                         else
 394                                 *dry_run_pc = current_pc + instruction_size;
 395
 396                         return ERROR_OK;
 397                 } else {
 398                         if (instruction.info.data_proc.rd == 15) {
 399                                 sim->set_reg_mode(sim, 15, rd & ~1);
 400                                 if (rd & 1)
 401                                         sim->set_state(sim, ARM_STATE_THUMB);
 402                                 else
 403                                         sim->set_state(sim, ARM_STATE_ARM);
 404                                 return ERROR_OK;
 405                         }
 406                         sim->set_reg_mode(sim, instruction.info.data_proc.rd, rd);
 407                         LOG_WARNING("no updating of flags yet");
 408                 }
 409         }
 410         /* compare instructions (CMP, CMN, TST, TEQ) */
 411         else if ((instruction.type >= ARM_TST) && (instruction.type <= ARM_CMN)) {
 412                 if (dry_run_pc) {
 413                         *dry_run_pc = current_pc + instruction_size;
 414                         return ERROR_OK;
 415                 } else
 416                         LOG_WARNING("no updating of flags yet");
 417         }
 418         /* load register instructions */
 419         else if ((instruction.type >= ARM_LDR) && (instruction.type <= ARM_LDRSH)) {
 420                 uint32_t load_address = 0, modified_address = 0, load_value = 0;
 421                 uint32_t rn = sim->get_reg_mode(sim, instruction.info.load_store.rn);
 422
 423                 /* adjust Rn in case the PC is being read */
 424                 if (instruction.info.load_store.rn == 15)
 425                         rn += 2 * instruction_size;
 426
 427                 if (instruction.info.load_store.offset_mode == 0) {
 428                         if (instruction.info.load_store.u)
 429                                 modified_address = rn + instruction.info.load_store.offset.offset;
 430                         else
 431                                 modified_address = rn - instruction.info.load_store.offset.offset;
 432                 } else if (instruction.info.load_store.offset_mode == 1) {
 433                         uint32_t offset;
 434                         uint32_t rm = sim->get_reg_mode(sim,
 435                                         instruction.info.load_store.offset.reg.rm);
 436                         uint8_t shift = instruction.info.load_store.offset.reg.shift;
 437                         uint8_t shift_imm = instruction.info.load_store.offset.reg.shift_imm;
 438                         uint8_t carry = sim->get_cpsr(sim, 29, 1);
 439
 440                         offset = arm_shift(shift, rm, shift_imm, &carry);
 441
 442                         if (instruction.info.load_store.u)
 443                                 modified_address = rn + offset;
 444                         else
 445                                 modified_address = rn - offset;
 446                 } else
 447                         LOG_ERROR("BUG: offset_mode neither 0 (offset) nor 1 (scaled register)");
 448
 449                 if (instruction.info.load_store.index_mode == 0) {
 450                         /* offset mode
 451                          * we load from the modified address, but don't change
 452                          * the base address register
 453                          */
 454                         load_address = modified_address;
 455                         modified_address = rn;
 456                 } else if (instruction.info.load_store.index_mode == 1) {
 457                         /* pre-indexed mode
 458                          * we load from the modified address, and write it
 459                          * back to the base address register
 460                          */
 461                         load_address = modified_address;
 462                 } else if (instruction.info.load_store.index_mode == 2) {
 463                         /* post-indexed mode
 464                          * we load from the unmodified address, and write the
 465                          * modified address back
 466                          */
 467                         load_address = rn;
 468                 }
 469
 470                 if ((!dry_run_pc) || (instruction.info.load_store.rd == 15)) {
 471                         retval = target_read_u32(target, load_address, &load_value);
 472                         if (retval != ERROR_OK)
 473                                 return retval;
 474                 }
 475
 476                 if (dry_run_pc) {
 477                         if (instruction.info.load_store.rd == 15)
 478                                 *dry_run_pc = load_value & ~1;
 479                         else
 480                                 *dry_run_pc = current_pc + instruction_size;
 481                         return ERROR_OK;
 482                 } else {
 483                         if ((instruction.info.load_store.index_mode == 1) ||
 484                                 (instruction.info.load_store.index_mode == 2))
 485                                 sim->set_reg_mode(sim,
 486                                         instruction.info.load_store.rn,
 487                                         modified_address);
 488
 489                         if (instruction.info.load_store.rd == 15) {
 490                                 sim->set_reg_mode(sim, 15, load_value & ~1);
 491                                 if (load_value & 1)
 492                                         sim->set_state(sim, ARM_STATE_THUMB);
 493                                 else
 494                                         sim->set_state(sim, ARM_STATE_ARM);
 495                                 return ERROR_OK;
 496                         }
 497                         sim->set_reg_mode(sim, instruction.info.load_store.rd, load_value);
 498                 }
 499         }
 500         /* load multiple instruction */
 501         else if (instruction.type == ARM_LDM) {
 502                 int i;
 503                 uint32_t rn = sim->get_reg_mode(sim, instruction.info.load_store_multiple.rn);
 504                 uint32_t load_values[16];
 505                 int bits_set = 0;
 506
 507                 for (i = 0; i < 16; i++) {
 508                         if (instruction.info.load_store_multiple.register_list & (1 << i))
 509                                 bits_set++;
 510                 }
 511
 512                 switch (instruction.info.load_store_multiple.addressing_mode) {
 513                         case 0: /* Increment after */
 514                                 /* rn = rn; */
 515                                 break;
 516                         case 1: /* Increment before */
 517                                 rn = rn + 4;
 518                                 break;
 519                         case 2: /* Decrement after */
 520                                 rn = rn - (bits_set * 4) + 4;
 521                                 break;
 522                         case 3: /* Decrement before */
 523                                 rn = rn - (bits_set * 4);
 524                                 break;
 525                 }
 526
 527                 for (i = 0; i < 16; i++) {
 528                         if (instruction.info.load_store_multiple.register_list & (1 << i)) {
 529                                 if ((!dry_run_pc) || (i == 15))
 530                                         target_read_u32(target, rn, &load_values[i]);
 531                                 rn += 4;
 532                         }
 533                 }
 534
 535                 if (dry_run_pc) {
 536                         if (instruction.info.load_store_multiple.register_list & 0x8000) {
 537                                 *dry_run_pc = load_values[15] & ~1;
 538                                 return ERROR_OK;
 539                         }
 540                 } else {
 541                         int update_cpsr = 0;
 542
 543                         if (instruction.info.load_store_multiple.s) {
 544                                 if (instruction.info.load_store_multiple.register_list & 0x8000)
 545                                         update_cpsr = 1;
 546                         }
 547
 548                         for (i = 0; i < 16; i++) {
 549                                 if (instruction.info.load_store_multiple.register_list & (1 << i)) {
 550                                         if (i == 15) {
 551                                                 uint32_t val = load_values[i];
 552                                                 sim->set_reg_mode(sim, i, val & ~1);
 553                                                 if (val & 1)
 554                                                         sim->set_state(sim, ARM_STATE_THUMB);
 555                                                 else
 556                                                         sim->set_state(sim, ARM_STATE_ARM);
 557                                         } else
 558                                                 sim->set_reg_mode(sim, i, load_values[i]);
 559                                 }
 560                         }
 561
 562                         if (update_cpsr) {
 563                                 uint32_t spsr = sim->get_reg_mode(sim, 16);
 564                                 sim->set_reg(sim, ARMV4_5_CPSR, spsr);
 565                         }
 566
 567                         /* base register writeback */
 568                         if (instruction.info.load_store_multiple.w)
 569                                 sim->set_reg_mode(sim, instruction.info.load_store_multiple.rn, rn);
 570
 571
 572                         if (instruction.info.load_store_multiple.register_list & 0x8000)
 573                                 return ERROR_OK;
 574                 }
 575         }
 576         /* store multiple instruction */
 577         else if (instruction.type == ARM_STM) {
 578                 int i;
 579
 580                 if (dry_run_pc) {
 581                         /* STM wont affect PC (advance by instruction size */
 582                 } else {
 583                         uint32_t rn = sim->get_reg_mode(sim,
 584                                         instruction.info.load_store_multiple.rn);
 585                         int bits_set = 0;
 586
 587                         for (i = 0; i < 16; i++) {
 588                                 if (instruction.info.load_store_multiple.register_list & (1 << i))
 589                                         bits_set++;
 590                         }
 591
 592                         switch (instruction.info.load_store_multiple.addressing_mode) {
 593                                 case 0: /* Increment after */
 594                                         /* rn = rn; */
 595                                         break;
 596                                 case 1: /* Increment before */
 597                                         rn = rn + 4;
 598                                         break;
 599                                 case 2: /* Decrement after */
 600                                         rn = rn - (bits_set * 4) + 4;
 601                                         break;
 602                                 case 3: /* Decrement before */
 603                                         rn = rn - (bits_set * 4);
 604                                         break;
 605                         }
 606
 607                         for (i = 0; i < 16; i++) {
 608                                 if (instruction.info.load_store_multiple.register_list & (1 << i)) {
 609                                         target_write_u32(target, rn, sim->get_reg_mode(sim, i));
 610                                         rn += 4;
 611                                 }
 612                         }
 613
 614                         /* base register writeback */
 615                         if (instruction.info.load_store_multiple.w)
 616                                 sim->set_reg_mode(sim,
 617                                         instruction.info.load_store_multiple.rn, rn);
 618
 619                 }
 620         } else if (!dry_run_pc) {
 621                 /* the instruction wasn't handled, but we're supposed to simulate it
 622                  */
 623                 LOG_ERROR("Unimplemented instruction, could not simulate it.");
 624                 return ERROR_FAIL;
 625         }
 626
 627         if (dry_run_pc) {
 628                 *dry_run_pc = current_pc + instruction_size;
 629                 return ERROR_OK;
 630         } else {
 631                 sim->set_reg(sim, 15, current_pc + instruction_size);
 632                 return ERROR_OK;
 633         }
 634
 635 }
 636
 637 static uint32_t armv4_5_get_reg(struct arm_sim_interface *sim, int reg)
 638 {
 639         struct arm *arm = (struct arm *)sim->user_data;
 640
 641         return buf_get_u32(arm->core_cache->reg_list[reg].value, 0, 32);
 642 }
 643
 644 static void armv4_5_set_reg(struct arm_sim_interface *sim, int reg, uint32_t value)
 645 {
 646         struct arm *arm = (struct arm *)sim->user_data;
 647
 648         buf_set_u32(arm->core_cache->reg_list[reg].value, 0, 32, value);
 649 }
 650
 651 static uint32_t armv4_5_get_reg_mode(struct arm_sim_interface *sim, int reg)
 652 {
 653         struct arm *arm = (struct arm *)sim->user_data;
 654
 655         return buf_get_u32(ARMV4_5_CORE_REG_MODE(arm->core_cache,
 656                         arm->core_mode, reg).value, 0, 32);
 657 }
 658
 659 static void armv4_5_set_reg_mode(struct arm_sim_interface *sim, int reg, uint32_t value)
 660 {
 661         struct arm *arm = (struct arm *)sim->user_data;
 662
 663         buf_set_u32(ARMV4_5_CORE_REG_MODE(arm->core_cache,
 664                 arm->core_mode, reg).value, 0, 32, value);
 665 }
 666
 667 static uint32_t armv4_5_get_cpsr(struct arm_sim_interface *sim, int pos, int bits)
 668 {
 669         struct arm *arm = (struct arm *)sim->user_data;
 670
 671         return buf_get_u32(arm->cpsr->value, pos, bits);
 672 }
 673
 674 static enum arm_state armv4_5_get_state(struct arm_sim_interface *sim)
 675 {
 676         struct arm *arm = (struct arm *)sim->user_data;
 677
 678         return arm->core_state;
 679 }
 680
 681 static void armv4_5_set_state(struct arm_sim_interface *sim, enum arm_state mode)
 682 {
 683         struct arm *arm = (struct arm *)sim->user_data;
 684
 685         arm->core_state = mode;
 686 }
 687
 688 static enum arm_mode armv4_5_get_mode(struct arm_sim_interface *sim)
 689 {
 690         struct arm *arm = (struct arm *)sim->user_data;
 691
 692         return arm->core_mode;
 693 }
 694
 695 int arm_simulate_step(struct target *target, uint32_t *dry_run_pc)
 696 {
 697         struct arm *arm = target_to_arm(target);
 698         struct arm_sim_interface sim;
 699
 700         sim.user_data = arm;
 701         sim.get_reg = &armv4_5_get_reg;
 702         sim.set_reg = &armv4_5_set_reg;
 703         sim.get_reg_mode = &armv4_5_get_reg_mode;
 704         sim.set_reg_mode = &armv4_5_set_reg_mode;
 705         sim.get_cpsr = &armv4_5_get_cpsr;
 706         sim.get_mode = &armv4_5_get_mode;
 707         sim.get_state = &armv4_5_get_state;
 708         sim.set_state = &armv4_5_set_state;
 709
 710         return arm_simulate_step_core(target, dry_run_pc, &sim);
 711 }