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