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