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