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

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