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