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