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