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