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