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 u32 arm_shift(uint8_t shift, u32 Rm, u32 shift_amount, uint8_t *carry)
  35 {
  36         u32 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 u32 arm_shifter_operand(armv4_5_common_t *armv4_5, int variant, union arm_shifter_operand shifter_operand, uint8_t *shifter_carry_out)
 126 {
 127         u32 return_value;
 128         int instruction_size;
 129
 130         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
 131                 instruction_size = 4;
 132         else
 133                 instruction_size = 2;
 134
 135         *shifter_carry_out = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 29, 1);
 136
 137         if (variant == 0) /* 32-bit immediate */
 138         {
 139                 return_value = shifter_operand.immediate.immediate;
 140         }
 141         else if (variant == 1) /* immediate shift */
 142         {
 143                 u32 Rm = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, shifter_operand.immediate_shift.Rm).value, 0, 32);
 144
 145                 /* adjust RM in case the PC is being read */
 146                 if (shifter_operand.immediate_shift.Rm == 15)
 147                         Rm += 2 * instruction_size;
 148
 149                 return_value = arm_shift(shifter_operand.immediate_shift.shift, Rm, shifter_operand.immediate_shift.shift_imm, shifter_carry_out);
 150         }
 151         else if (variant == 2) /* register shift */
 152         {
 153                 u32 Rm = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, shifter_operand.register_shift.Rm).value, 0, 32);
 154                 u32 Rs = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, shifter_operand.register_shift.Rs).value, 0, 32);
 155
 156                 /* adjust RM in case the PC is being read */
 157                 if (shifter_operand.register_shift.Rm == 15)
 158                         Rm += 2 * instruction_size;
 159
 160                 return_value = arm_shift(shifter_operand.immediate_shift.shift, Rm, Rs, shifter_carry_out);
 161         }
 162         else
 163         {
 164                 LOG_ERROR("BUG: shifter_operand.variant not 0, 1 or 2");
 165                 return_value = 0xffffffff;
 166         }
 167
 168         return return_value;
 169 }
 170
 171 int pass_condition(u32 cpsr, u32 opcode)
 172 {
 173         switch ((opcode & 0xf0000000) >> 28)
 174         {
 175                 case 0x0:       /* EQ */
 176                         if (cpsr & 0x40000000)
 177                                 return 1;
 178                         else
 179                                 return 0;
 180                 case 0x1:       /* NE */
 181                         if (!(cpsr & 0x40000000))
 182                                 return 1;
 183                         else
 184                                 return 0;
 185                 case 0x2:       /* CS */
 186                         if (cpsr & 0x20000000)
 187                                 return 1;
 188                         else
 189                                 return 0;
 190                 case 0x3:       /* CC */
 191                         if (!(cpsr & 0x20000000))
 192                                 return 1;
 193                         else
 194                                 return 0;
 195                 case 0x4:       /* MI */
 196                         if (cpsr & 0x80000000)
 197                                 return 1;
 198                         else
 199                                 return 0;
 200                 case 0x5:       /* PL */
 201                         if (!(cpsr & 0x80000000))
 202                                 return 1;
 203                         else
 204                                 return 0;
 205                 case 0x6:       /* VS */
 206                         if (cpsr & 0x10000000)
 207                                 return 1;
 208                         else
 209                                 return 0;
 210                 case 0x7:       /* VC */
 211                         if (!(cpsr & 0x10000000))
 212                                 return 1;
 213                         else
 214                                 return 0;
 215                 case 0x8:       /* HI */
 216                         if ((cpsr & 0x20000000) && !(cpsr & 0x40000000))
 217                                 return 1;
 218                         else
 219                                 return 0;
 220                 case 0x9:       /* LS */
 221                         if (!(cpsr & 0x20000000) || (cpsr & 0x40000000))
 222                                 return 1;
 223                         else
 224                                 return 0;
 225                 case 0xa:       /* GE */
 226                         if (((cpsr & 0x80000000) && (cpsr & 0x10000000))
 227                                 || (!(cpsr & 0x80000000) && !(cpsr & 0x10000000)))
 228                                 return 1;
 229                         else
 230                                 return 0;
 231                 case 0xb:       /* LT */
 232                         if (((cpsr & 0x80000000) && !(cpsr & 0x10000000))
 233                                 || (!(cpsr & 0x80000000) && (cpsr & 0x10000000)))
 234                                 return 1;
 235                         else
 236                                 return 0;
 237                 case 0xc:       /* GT */
 238                         if (!(cpsr & 0x40000000) &&
 239                                 (((cpsr & 0x80000000) && (cpsr & 0x10000000))
 240                                 || (!(cpsr & 0x80000000) && !(cpsr & 0x10000000))))
 241                                 return 1;
 242                         else
 243                                 return 0;
 244                 case 0xd:       /* LE */
 245                         if ((cpsr & 0x40000000) &&
 246                                 (((cpsr & 0x80000000) && !(cpsr & 0x10000000))
 247                                 || (!(cpsr & 0x80000000) && (cpsr & 0x10000000))))
 248                                 return 1;
 249                         else
 250                                 return 0;
 251                 case 0xe:
 252                 case 0xf:
 253                         return 1;
 254
 255         }
 256
 257         LOG_ERROR("BUG: should never get here");
 258         return 0;
 259 }
 260
 261 int thumb_pass_branch_condition(u32 cpsr, u16 opcode)
 262 {
 263         return pass_condition(cpsr, (opcode & 0x0f00) << 20);
 264 }
 265
 266 /* simulate a single step (if possible)
 267  * if the dry_run_pc argument is provided, no state is changed,
 268  * but the new pc is stored in the variable pointed at by the argument
 269  */
 270 int arm_simulate_step(target_t *target, u32 *dry_run_pc)
 271 {
 272         armv4_5_common_t *armv4_5 = target->arch_info;
 273         u32 current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
 274         arm_instruction_t instruction;
 275         int instruction_size;
 276         int retval = ERROR_OK;
 277
 278         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
 279         {
 280                 u32 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(buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32), opcode))
 295                 {
 296                         if (dry_run_pc)
 297                         {
 298                                 *dry_run_pc = current_pc + instruction_size;
 299                         }
 300                         else
 301                         {
 302                                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, current_pc + instruction_size);
 303                         }
 304
 305                         return ERROR_OK;
 306                 }
 307         }
 308         else
 309         {
 310                 u16 opcode;
 311
 312                 if((retval = target_read_u16(target, current_pc, &opcode)) != ERROR_OK)
 313                 {
 314                         return retval;
 315                 }
 316                 if((retval = thumb_evaluate_opcode(opcode, current_pc, &instruction)) != ERROR_OK)
 317                 {
 318                         return retval;
 319                         }
 320                 instruction_size = 2;
 321
 322                 /* check condition code (only for branch instructions) */
 323                 if ((!thumb_pass_branch_condition(buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32), opcode)) &&
 324                         (instruction.type == ARM_B))
 325                 {
 326                         if (dry_run_pc)
 327                         {
 328                                 *dry_run_pc = current_pc + instruction_size;
 329                         }
 330                         else
 331                         {
 332                                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, current_pc + instruction_size);
 333                         }
 334
 335                         return ERROR_OK;
 336                 }
 337         }
 338
 339         /* examine instruction type */
 340
 341         /* branch instructions */
 342         if ((instruction.type >= ARM_B) && (instruction.type <= ARM_BLX))
 343         {
 344                 u32 target;
 345
 346                 if (instruction.info.b_bl_bx_blx.reg_operand == -1)
 347                 {
 348                         target = instruction.info.b_bl_bx_blx.target_address;
 349                 }
 350                 else
 351                 {
 352                         target = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, instruction.info.b_bl_bx_blx.reg_operand).value, 0, 32);
 353                         if(instruction.info.b_bl_bx_blx.reg_operand == 15)
 354                         {
 355                                 target += 2 * instruction_size;
 356                         }
 357                 }
 358
 359                 if (dry_run_pc)
 360                 {
 361                         *dry_run_pc = target;
 362                         return ERROR_OK;
 363                 }
 364                 else
 365                 {
 366                         if (instruction.type == ARM_B)
 367                         {
 368                                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, target);
 369                         }
 370                         else if (instruction.type == ARM_BL)
 371                         {
 372                                 u32 old_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
 373                                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 14).value, 0, 32, old_pc + 4);
 374                                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, target);
 375                         }
 376                         else if (instruction.type == ARM_BX)
 377                         {
 378                                 if (target & 0x1)
 379                                 {
 380                                         armv4_5->core_state = ARMV4_5_STATE_THUMB;
 381                                 }
 382                                 else
 383                                 {
 384                                         armv4_5->core_state = ARMV4_5_STATE_ARM;
 385                                 }
 386                                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, target & 0xfffffffe);
 387                         }
 388                         else if (instruction.type == ARM_BLX)
 389                         {
 390                                 u32 old_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
 391                                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 14).value, 0, 32, old_pc + 4);
 392
 393                                 if (target & 0x1)
 394                                 {
 395                                         armv4_5->core_state = ARMV4_5_STATE_THUMB;
 396                                 }
 397                                 else
 398                                 {
 399                                         armv4_5->core_state = ARMV4_5_STATE_ARM;
 400                                 }
 401                                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, target & 0xfffffffe);
 402                         }
 403
 404                         return ERROR_OK;
 405                 }
 406         }
 407         /* data processing instructions, except compare instructions (CMP, CMN, TST, TEQ) */
 408         else if (((instruction.type >= ARM_AND) && (instruction.type <= ARM_RSC))
 409                         || ((instruction.type >= ARM_ORR) && (instruction.type <= ARM_MVN)))
 410         {
 411                 u32 Rd, Rn, shifter_operand;
 412                 uint8_t C = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 29, 1);
 413                 uint8_t carry_out;
 414
 415                 Rd = 0x0;
 416                 /* ARM_MOV and ARM_MVN does not use Rn */
 417                 if ((instruction.type != ARM_MOV) && (instruction.type != ARM_MVN))
 418                         Rn = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, instruction.info.data_proc.Rn).value, 0, 32);
 419                 else
 420                         Rn = 0;
 421
 422                 shifter_operand = arm_shifter_operand(armv4_5, instruction.info.data_proc.variant, instruction.info.data_proc.shifter_operand, &carry_out);
 423
 424                 /* adjust Rn in case the PC is being read */
 425                 if (instruction.info.data_proc.Rn == 15)
 426                         Rn += 2 * instruction_size;
 427
 428                 if (instruction.type == ARM_AND)
 429                         Rd = Rn & shifter_operand;
 430                 else if (instruction.type == ARM_EOR)
 431                         Rd = Rn ^ shifter_operand;
 432                 else if (instruction.type == ARM_SUB)
 433                         Rd = Rn - shifter_operand;
 434                 else if (instruction.type == ARM_RSB)
 435                         Rd = shifter_operand - Rn;
 436                 else if (instruction.type == ARM_ADD)
 437                         Rd = Rn + shifter_operand;
 438                 else if (instruction.type == ARM_ADC)
 439                         Rd = Rn + shifter_operand + (C & 1);
 440                 else if (instruction.type == ARM_SBC)
 441                         Rd = Rn - shifter_operand - (C & 1) ? 0 : 1;
 442                 else if (instruction.type == ARM_RSC)
 443                         Rd = shifter_operand - Rn - (C & 1) ? 0 : 1;
 444                 else if (instruction.type == ARM_ORR)
 445                         Rd = Rn | shifter_operand;
 446                 else if (instruction.type == ARM_BIC)
 447                         Rd = Rn & ~(shifter_operand);
 448                 else if (instruction.type == ARM_MOV)
 449                         Rd = shifter_operand;
 450                 else if (instruction.type == ARM_MVN)
 451                         Rd = ~shifter_operand;
 452                 else
 453                         LOG_WARNING("unhandled instruction type");
 454
 455                 if (dry_run_pc)
 456                 {
 457                         if (instruction.info.data_proc.Rd == 15)
 458                         {
 459                                 *dry_run_pc = Rd;
 460                                 return ERROR_OK;
 461                         }
 462                         else
 463                         {
 464                                 *dry_run_pc = current_pc + instruction_size;
 465                         }
 466
 467                         return ERROR_OK;
 468                 }
 469                 else
 470                 {
 471                         buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, instruction.info.data_proc.Rd).value, 0, 32, Rd);
 472                         LOG_WARNING("no updating of flags yet");
 473
 474                         if (instruction.info.data_proc.Rd == 15)
 475                                 return ERROR_OK;
 476                 }
 477         }
 478         /* compare instructions (CMP, CMN, TST, TEQ) */
 479         else if ((instruction.type >= ARM_TST) && (instruction.type <= ARM_CMN))
 480         {
 481                 if (dry_run_pc)
 482                 {
 483                         *dry_run_pc = current_pc + instruction_size;
 484                         return ERROR_OK;
 485                 }
 486                 else
 487                 {
 488                         LOG_WARNING("no updating of flags yet");
 489                 }
 490         }
 491         /* load register instructions */
 492         else if ((instruction.type >= ARM_LDR) && (instruction.type <= ARM_LDRSH))
 493         {
 494                 u32 load_address = 0, modified_address = 0, load_value;
 495                 u32 Rn = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, instruction.info.load_store.Rn).value, 0, 32);
 496
 497                 /* adjust Rn in case the PC is being read */
 498                 if (instruction.info.load_store.Rn == 15)
 499                         Rn += 2 * instruction_size;
 500
 501                 if (instruction.info.load_store.offset_mode == 0)
 502                 {
 503                         if (instruction.info.load_store.U)
 504                                 modified_address = Rn + instruction.info.load_store.offset.offset;
 505                         else
 506                                 modified_address = Rn - instruction.info.load_store.offset.offset;
 507                 }
 508                 else if (instruction.info.load_store.offset_mode == 1)
 509                 {
 510                         u32 offset;
 511                         u32 Rm = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, instruction.info.load_store.offset.reg.Rm).value, 0, 32);
 512                         uint8_t shift = instruction.info.load_store.offset.reg.shift;
 513                         uint8_t shift_imm = instruction.info.load_store.offset.reg.shift_imm;
 514                         uint8_t carry = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 29, 1);
 515
 516                         offset = arm_shift(shift, Rm, shift_imm, &carry);
 517
 518                         if (instruction.info.load_store.U)
 519                                 modified_address = Rn + offset;
 520                         else
 521                                 modified_address = Rn - offset;
 522                 }
 523                 else
 524                 {
 525                         LOG_ERROR("BUG: offset_mode neither 0 (offset) nor 1 (scaled register)");
 526                 }
 527
 528                 if (instruction.info.load_store.index_mode == 0)
 529                 {
 530                         /* offset mode
 531                          * we load from the modified address, but don't change the base address register */
 532                         load_address = modified_address;
 533                         modified_address = Rn;
 534                 }
 535                 else if (instruction.info.load_store.index_mode == 1)
 536                 {
 537                         /* pre-indexed mode
 538                          * we load from the modified address, and write it back to the base address register */
 539                         load_address = modified_address;
 540                 }
 541                 else if (instruction.info.load_store.index_mode == 2)
 542                 {
 543                         /* post-indexed mode
 544                          * we load from the unmodified address, and write the modified address back */
 545                          load_address = Rn;
 546                 }
 547
 548                 if((!dry_run_pc) || (instruction.info.load_store.Rd == 15))
 549                 {
 550                         if((retval = target_read_u32(target, load_address, &load_value)) != ERROR_OK)
 551                         {
 552                                 return retval;
 553                         }
 554                 }
 555
 556                 if (dry_run_pc)
 557                 {
 558                         if (instruction.info.load_store.Rd == 15)
 559                         {
 560                                 *dry_run_pc = load_value;
 561                                 return ERROR_OK;
 562                         }
 563                         else
 564                         {
 565                                 *dry_run_pc = current_pc + instruction_size;
 566                         }
 567
 568                         return ERROR_OK;
 569                 }
 570                 else
 571                 {
 572                         if ((instruction.info.load_store.index_mode == 1) ||
 573                                 (instruction.info.load_store.index_mode == 2))
 574                         {
 575                                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, instruction.info.load_store.Rn).value, 0, 32, modified_address);
 576                         }
 577                         buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, instruction.info.load_store.Rd).value, 0, 32, load_value);
 578
 579                         if (instruction.info.load_store.Rd == 15)
 580                                 return ERROR_OK;
 581                 }
 582         }
 583         /* load multiple instruction */
 584         else if (instruction.type == ARM_LDM)
 585         {
 586                 int i;
 587                 u32 Rn = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, instruction.info.load_store_multiple.Rn).value, 0, 32);
 588                 u32 load_values[16];
 589                 int bits_set = 0;
 590
 591                 for (i = 0; i < 16; i++)
 592                 {
 593                         if (instruction.info.load_store_multiple.register_list & (1 << i))
 594                                 bits_set++;
 595                 }
 596
 597                 switch (instruction.info.load_store_multiple.addressing_mode)
 598                 {
 599                         case 0: /* Increment after */
 600                                 Rn = Rn;
 601                                 break;
 602                         case 1: /* Increment before */
 603                                 Rn = Rn + 4;
 604                                 break;
 605                         case 2: /* Decrement after */
 606                                 Rn = Rn - (bits_set * 4) + 4;
 607                                 break;
 608                         case 3: /* Decrement before */
 609                                 Rn = Rn - (bits_set * 4);
 610                                 break;
 611                 }
 612
 613                 for (i = 0; i < 16; i++)
 614                 {
 615                         if (instruction.info.load_store_multiple.register_list & (1 << i))
 616                         {
 617                                 if((!dry_run_pc) || (i == 15))
 618                                 {
 619                                         target_read_u32(target, Rn, &load_values[i]);
 620                                 }
 621                                 Rn += 4;
 622                         }
 623                 }
 624
 625                 if (dry_run_pc)
 626                 {
 627                         if (instruction.info.load_store_multiple.register_list & 0x8000)
 628                         {
 629                                 *dry_run_pc = load_values[15];
 630                                 return ERROR_OK;
 631                         }
 632                 }
 633                 else
 634                 {
 635                         enum armv4_5_mode mode = armv4_5->core_mode;
 636                         int update_cpsr = 0;
 637
 638                         if (instruction.info.load_store_multiple.S)
 639                         {
 640                                 if (instruction.info.load_store_multiple.register_list & 0x8000)
 641                                         update_cpsr = 1;
 642                                 else
 643                                         mode = ARMV4_5_MODE_USR;
 644                         }
 645
 646                         for (i = 0; i < 16; i++)
 647                         {
 648                                 if (instruction.info.load_store_multiple.register_list & (1 << i))
 649                                 {
 650                                         buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, i).value, 0, 32, load_values[i]);
 651                                 }
 652                         }
 653
 654                         if (update_cpsr)
 655                         {
 656                                 u32 spsr = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32);
 657                                 buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, spsr);
 658                         }
 659
 660                         /* base register writeback */
 661                         if (instruction.info.load_store_multiple.W)
 662                                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, instruction.info.load_store_multiple.Rn).value, 0, 32, Rn);
 663
 664                         if (instruction.info.load_store_multiple.register_list & 0x8000)
 665                                 return ERROR_OK;
 666                 }
 667         }
 668         /* store multiple instruction */
 669         else if (instruction.type == ARM_STM)
 670         {
 671                 int i;
 672
 673                 if (dry_run_pc)
 674                 {
 675                         /* STM wont affect PC (advance by instruction size */
 676                 }
 677                 else
 678                 {
 679                         u32 Rn = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, instruction.info.load_store_multiple.Rn).value, 0, 32);
 680                         int bits_set = 0;
 681                         enum armv4_5_mode mode = armv4_5->core_mode;
 682
 683                         for (i = 0; i < 16; i++)
 684                         {
 685                                 if (instruction.info.load_store_multiple.register_list & (1 << i))
 686                                         bits_set++;
 687                         }
 688
 689                         if (instruction.info.load_store_multiple.S)
 690                         {
 691                                 mode = ARMV4_5_MODE_USR;
 692                         }
 693
 694                         switch (instruction.info.load_store_multiple.addressing_mode)
 695                         {
 696                                 case 0: /* Increment after */
 697                                         Rn = Rn;
 698                                         break;
 699                                 case 1: /* Increment before */
 700                                         Rn = Rn + 4;
 701                                         break;
 702                                 case 2: /* Decrement after */
 703                                         Rn = Rn - (bits_set * 4) + 4;
 704                                         break;
 705                                 case 3: /* Decrement before */
 706                                         Rn = Rn - (bits_set * 4);
 707                                         break;
 708                         }
 709
 710                         for (i = 0; i < 16; i++)
 711                         {
 712                                 if (instruction.info.load_store_multiple.register_list & (1 << i))
 713                                 {
 714                                         target_write_u32(target, Rn, buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32));
 715                                         Rn += 4;
 716                                 }
 717                         }
 718
 719                         /* base register writeback */
 720                         if (instruction.info.load_store_multiple.W)
 721                                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, instruction.info.load_store_multiple.Rn).value, 0, 32, Rn);
 722
 723                 }
 724         }
 725         else if (!dry_run_pc)
 726         {
 727                 /* the instruction wasn't handled, but we're supposed to simulate it
 728                  */
 729                 return ERROR_ARM_SIMULATOR_NOT_IMPLEMENTED;
 730         }
 731
 732         if (dry_run_pc)
 733         {
 734                 *dry_run_pc = current_pc + instruction_size;
 735                 return ERROR_OK;
 736         }
 737         else
 738         {
 739                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, current_pc + instruction_size);
 740                 return ERROR_OK;
 741         }
 742
 743 }