packages/hal/arm/arch/v2_0/src/arm_stub.c

   1 //========================================================================
   2 //
   3 //      arm_stub.c
   4 //
   5 //      Helper functions for stub, generic to all ARM processors
   6 //
   7 //========================================================================
   8 //####ECOSGPLCOPYRIGHTBEGIN####
   9 // -------------------------------------------
  10 // This file is part of eCos, the Embedded Configurable Operating System.
  11 // Copyright (C) 1998, 1999, 2000, 2001, 2002, 2003 Red Hat, Inc.
  12 //
  13 // eCos is free software; you can redistribute it and/or modify it under
  14 // the terms of the GNU General Public License as published by the Free
  15 // Software Foundation; either version 2 or (at your option) any later version.
  16 //
  17 // eCos is distributed in the hope that it will be useful, but WITHOUT ANY
  18 // WARRANTY; without even the implied warranty of MERCHANTABILITY or
  19 // FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  20 // for more details.
  21 //
  22 // You should have received a copy of the GNU General Public License along
  23 // with eCos; if not, write to the Free Software Foundation, Inc.,
  24 // 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  25 //
  26 // As a special exception, if other files instantiate templates or use macros
  27 // or inline functions from this file, or you compile this file and link it
  28 // with other works to produce a work based on this file, this file does not
  29 // by itself cause the resulting work to be covered by the GNU General Public
  30 // License. However the source code for this file must still be made available
  31 // in accordance with section (3) of the GNU General Public License.
  32 //
  33 // This exception does not invalidate any other reasons why a work based on
  34 // this file might be covered by the GNU General Public License.
  35 //
  36 // Alternative licenses for eCos may be arranged by contacting Red Hat, Inc.
  37 // at http://sources.redhat.com/ecos/ecos-license/
  38 // -------------------------------------------
  39 //####ECOSGPLCOPYRIGHTEND####
  40 //========================================================================
  41 //#####DESCRIPTIONBEGIN####
  42 //
  43 // Author(s):     Red Hat, gthomas
  44 // Contributors:  Red Hat, gthomas, jskov
  45 // Date:          1998-11-26
  46 // Purpose:
  47 // Description:   Helper functions for stub, generic to all ARM processors
  48 // Usage:
  49 //
  50 //####DESCRIPTIONEND####
  51 //
  52 //========================================================================
  53
  54 #include <stddef.h>
  55
  56 #include <pkgconf/hal.h>
  57
  58 #ifdef CYGPKG_REDBOOT
  59 #include <pkgconf/redboot.h>
  60 #endif
  61
  62 #ifdef CYGDBG_HAL_DEBUG_GDB_INCLUDE_STUBS
  63
  64 #ifdef CYGPKG_HAL_ARM_SIM
  65 #error "GDB Stub support not implemented for ARM SIM"
  66 #endif
  67
  68 #include <cyg/hal/hal_stub.h>
  69 #include <cyg/hal/hal_arch.h>
  70 #include <cyg/hal/hal_intr.h>
  71
  72 #ifndef FALSE
  73 #define FALSE 0
  74 #define TRUE  1
  75 #endif
  76
  77 // Use bit 0 as a thumb-mode flag for next address to be executed.
  78 // Alternative would be to keep track of it using a C variable, but
  79 // since bit 0 is used by the BX instruction, we might as well do the
  80 // same thing and thus avoid checking two different flags.
  81 #define IS_THUMB_ADDR(addr)     ((addr) & 1)
  82 #define MAKE_THUMB_ADDR(addr) ((addr) | 1)
  83 #define UNMAKE_THUMB_ADDR(addr) ((addr) & ~1)
  84
  85 #ifdef CYGDBG_HAL_DEBUG_GDB_THREAD_SUPPORT
  86 #include <cyg/hal/dbg-threads-api.h>    // dbg_currthread_id
  87 #endif
  88
  89 #if defined(CYGNUM_HAL_BREAKPOINT_LIST_SIZE) && (CYGNUM_HAL_BREAKPOINT_LIST_SIZE > 0)
  90 cyg_uint32 __arm_breakinst = HAL_BREAKINST_ARM;
  91 cyg_uint16 __thumb_breakinst = HAL_BREAKINST_THUMB;
  92 #endif
  93
  94 /* Given a trap value TRAP, return the corresponding signal. */
  95
  96 int __computeSignal (unsigned int trap_number)
  97 {
  98     // Check to see if we stopped because of a hw watchpoint/breakpoint.
  99 #ifdef HAL_STUB_IS_STOPPED_BY_HARDWARE
 100     {
 101         void *daddr;
 102         if (HAL_STUB_IS_STOPPED_BY_HARDWARE(daddr))
 103             return SIGTRAP;
 104     }
 105 #endif
 106     // should also catch CYGNUM_HAL_VECTOR_UNDEF_INSTRUCTION here but we
 107     // can't tell the different between a real one and a breakpoint :-(
 108     switch (trap_number) {
 109     case CYGNUM_HAL_VECTOR_ABORT_PREFETCH:      // Fall through
 110     case CYGNUM_HAL_VECTOR_ABORT_DATA:          // Fall through
 111     case CYGNUM_HAL_VECTOR_reserved:
 112         return SIGBUS;
 113     case CYGNUM_HAL_VECTOR_IRQ:
 114     case CYGNUM_HAL_VECTOR_FIQ:
 115         return SIGINT;
 116     default:
 117         return SIGTRAP;
 118     }
 119 }
 120
 121
 122 /* Return the trap number corresponding to the last-taken trap. */
 123 int __get_trap_number (void)
 124 {
 125     // The vector is not not part of the GDB register set so get it
 126     // directly from the save context.
 127     return _hal_registers->vector;
 128 }
 129
 130
 131 #if defined(CYGSEM_REDBOOT_BSP_SYSCALLS)
 132 int __is_bsp_syscall(void)
 133 {
 134     unsigned long pc = get_register(PC);
 135     unsigned long cpsr = get_register(PS);  // condition codes
 136
 137     if (_hal_registers->vector == CYGNUM_HAL_EXCEPTION_INTERRUPT) {
 138         if (cpsr & CPSR_THUMB_ENABLE)
 139             return *(unsigned short *)pc == 0xdf18;
 140         else
 141             return *(unsigned *)pc == 0xef180001;
 142     }
 143     return 0;
 144 }
 145 #endif
 146
 147 /* Set the currently-saved pc register value to PC. */
 148
 149 void set_pc (target_register_t pc)
 150 {
 151     put_register (PC, pc);
 152 }
 153
 154 // Calculate byte offset a given register from start of register save area.
 155 static int
 156 reg_offset(regnames_t reg)
 157 {
 158     int base_offset;
 159
 160     if (reg < F0)
 161         return reg * 4;
 162
 163     base_offset = 16 * 4;
 164
 165     if (reg < FPS)
 166         return base_offset + ((reg - F0) * 12);
 167
 168     base_offset += (8 * 12);
 169
 170     if (reg <= PS)
 171         return base_offset + ((reg - FPS) * 4);
 172
 173     return -1;  // Should never happen!
 174 }
 175
 176
 177 // Return the currently-saved value corresponding to register REG of
 178 // the exception context.
 179 target_register_t
 180 get_register (regnames_t reg)
 181 {
 182     target_register_t val;
 183     int offset = reg_offset(reg);
 184
 185     if (REGSIZE(reg) > sizeof(target_register_t) || offset == -1)
 186         return -1;
 187
 188     val = _registers[offset/sizeof(target_register_t)];
 189
 190     return val;
 191 }
 192
 193 // Store VALUE in the register corresponding to WHICH in the exception
 194 // context.
 195 void
 196 put_register (regnames_t which, target_register_t value)
 197 {
 198     int offset = reg_offset(which);
 199
 200     if (REGSIZE(which) > sizeof(target_register_t) || offset == -1)
 201         return;
 202
 203     _registers[offset/sizeof(target_register_t)] = value;
 204 }
 205
 206 // Write the contents of register WHICH into VALUE as raw bytes. This
 207 // is only used for registers larger than sizeof(target_register_t).
 208 // Return non-zero if it is a valid register.
 209 int
 210 get_register_as_bytes (regnames_t which, char *value)
 211 {
 212     int offset = reg_offset(which);
 213
 214     if (offset != -1) {
 215         memcpy (value, (char *)_registers + offset, REGSIZE(which));
 216         return 1;
 217     }
 218     return 0;
 219 }
 220
 221 // Alter the contents of saved register WHICH to contain VALUE. This
 222 // is only used for registers larger than sizeof(target_register_t).
 223 // Return non-zero if it is a valid register.
 224 int
 225 put_register_as_bytes (regnames_t which, char *value)
 226 {
 227     int offset = reg_offset(which);
 228
 229     if (offset != -1) {
 230         memcpy ((char *)_registers + offset, value, REGSIZE(which));
 231         return 1;
 232     }
 233     return 0;
 234 }
 235
 236 /*----------------------------------------------------------------------
 237  * Single-step support
 238  */
 239
 240 /* Set things up so that the next user resume will execute one instruction.
 241    This may be done by setting breakpoints or setting a single step flag
 242    in the saved user registers, for example. */
 243
 244 static unsigned long  ss_saved_pc = 0;
 245 static unsigned long  ss_saved_instr;
 246 static unsigned short ss_saved_thumb_instr;
 247
 248 #define FIXME() {diag_printf("FIXME - %s\n", __FUNCTION__); }
 249
 250 // return non-zero for v5 and later
 251 static int
 252 v5T_semantics(void)
 253 {
 254     unsigned id;
 255
 256     asm volatile ("mrc  p15,0,%0,c0,c0,0\n"
 257                   : "=r" (id) : /* no inputs */);
 258
 259     return ((id >> 16) & 0xff) >= 5;
 260 }
 261
 262 static int
 263 ins_will_execute(unsigned long ins)
 264 {
 265     unsigned long psr = get_register(PS);  // condition codes
 266     int res = 0;
 267     switch ((ins & 0xF0000000) >> 28) {
 268     case 0x0: // EQ
 269         res = (psr & PS_Z) != 0;
 270         break;
 271     case 0x1: // NE
 272         res = (psr & PS_Z) == 0;
 273         break;
 274     case 0x2: // CS
 275         res = (psr & PS_C) != 0;
 276         break;
 277     case 0x3: // CC
 278         res = (psr & PS_C) == 0;
 279         break;
 280     case 0x4: // MI
 281         res = (psr & PS_N) != 0;
 282         break;
 283     case 0x5: // PL
 284         res = (psr & PS_N) == 0;
 285         break;
 286     case 0x6: // VS
 287         res = (psr & PS_V) != 0;
 288         break;
 289     case 0x7: // VC
 290         res = (psr & PS_V) == 0;
 291         break;
 292     case 0x8: // HI
 293         res = ((psr & PS_C) != 0) && ((psr & PS_Z) == 0);
 294         break;
 295     case 0x9: // LS
 296         res = ((psr & PS_C) == 0) || ((psr & PS_Z) != 0);
 297         break;
 298     case 0xA: // GE
 299         res = ((psr & (PS_N|PS_V)) == (PS_N|PS_V)) ||
 300             ((psr & (PS_N|PS_V)) == 0);
 301         break;
 302     case 0xB: // LT
 303         res = ((psr & (PS_N|PS_V)) == PS_N) ||
 304             ((psr & (PS_N|PS_V)) == PS_V);
 305         break;
 306     case 0xC: // GT
 307         res = ((psr & (PS_N|PS_V)) == (PS_N|PS_V)) ||
 308             ((psr & (PS_N|PS_V)) == 0);
 309         res = ((psr & PS_Z) == 0) && res;
 310         break;
 311     case 0xD: // LE
 312         res = ((psr & (PS_N|PS_V)) == PS_N) ||
 313             ((psr & (PS_N|PS_V)) == PS_V);
 314         res = ((psr & PS_Z) == PS_Z) || res;
 315         break;
 316     case 0xE: // AL
 317         res = TRUE;
 318         break;
 319     case 0xF: // NV
 320         if (((ins & 0x0E000000) >> 24) == 0xA)
 321             res = TRUE;
 322         else
 323             res = FALSE;
 324         break;
 325     }
 326     return res;
 327 }
 328
 329 static unsigned long
 330 RmShifted(int shift)
 331 {
 332     unsigned long Rm = get_register(shift & 0x00F);
 333     int shift_count;
 334     if ((shift & 0x010) == 0) {
 335         shift_count = (shift & 0xF80) >> 7;
 336     } else {
 337         shift_count = get_register((shift & 0xF00) >> 8);
 338     }
 339     switch ((shift & 0x060) >> 5) {
 340     case 0x0: // Logical left
 341         Rm <<= shift_count;
 342         break;
 343     case 0x1: // Logical right
 344         Rm >>= shift_count;
 345         break;
 346     case 0x2: // Arithmetic right
 347         Rm = (unsigned long)((long)Rm >> shift_count);
 348         break;
 349     case 0x3: // Rotate right
 350         if (shift_count == 0) {
 351             // Special case, RORx
 352             Rm >>= 1;
 353             if (get_register(PS) & PS_C) Rm |= 0x80000000;
 354         } else {
 355             Rm = (Rm >> shift_count) | (Rm << (32-shift_count));
 356         }
 357         break;
 358     }
 359     return Rm;
 360 }
 361
 362 // Decide the next instruction to be executed for a given instruction
 363 static unsigned long *
 364 target_ins(unsigned long *pc, unsigned long ins)
 365 {
 366     unsigned long new_pc, offset, op2;
 367     unsigned long Rn;
 368     int i, reg_count, c;
 369
 370     switch ((ins & 0x0C000000) >> 26) {
 371     case 0x0:
 372         // BX or BLX
 373         if ((ins & 0x0FFFFFD0) == 0x012FFF10) {
 374             new_pc = (unsigned long)get_register(ins & 0x0000000F);
 375             return ((unsigned long *)new_pc);
 376         }
 377         // Data processing
 378         new_pc = (unsigned long)(pc+1);
 379         if ((ins & 0x0000F000) == 0x0000F000) {
 380             // Destination register is PC
 381             if ((ins & 0x0FBF0000) != 0x010F0000) {
 382                 Rn = (unsigned long)get_register((ins & 0x000F0000) >> 16);
 383                 if ((ins & 0x000F0000) == 0x000F0000) Rn += 8;  // PC prefetch!
 384                 if ((ins & 0x02000000) == 0) {
 385                     op2 = RmShifted(ins & 0x00000FFF);
 386                 } else {
 387                     op2 = ins & 0x000000FF;
 388                     i = (ins & 0x00000F00) >> 8;  // Rotate count
 389                     op2 = (op2 >> (i*2)) | (op2 << (32-(i*2)));
 390                 }
 391                 switch ((ins & 0x01E00000) >> 21) {
 392                 case 0x0: // AND
 393                     new_pc = Rn & op2;
 394                     break;
 395                 case 0x1: // EOR
 396                     new_pc = Rn ^ op2;
 397                     break;
 398                 case 0x2: // SUB
 399                     new_pc = Rn - op2;
 400                     break;
 401                 case 0x3: // RSB
 402                     new_pc = op2 - Rn;
 403                     break;
 404                 case 0x4: // ADD
 405                     new_pc = Rn + op2;
 406                     break;
 407                 case 0x5: // ADC
 408                     c = (get_register(PS) & PS_C) != 0;
 409                     new_pc = Rn + op2 + c;
 410                     break;
 411                 case 0x6: // SBC
 412                     c = (get_register(PS) & PS_C) != 0;
 413                     new_pc = Rn - op2 + c - 1;
 414                     break;
 415                 case 0x7: // RSC
 416                     c = (get_register(PS) & PS_C) != 0;
 417                     new_pc = op2 - Rn +c - 1;
 418                     break;
 419                 case 0x8: // TST
 420                 case 0x9: // TEQ
 421                 case 0xA: // CMP
 422                 case 0xB: // CMN
 423                     break; // PC doesn't change
 424                 case 0xC: // ORR
 425                     new_pc = Rn | op2;
 426                     break;
 427                 case 0xD: // MOV
 428                     new_pc = op2;
 429                     break;
 430                 case 0xE: // BIC
 431                     new_pc = Rn & ~op2;
 432                     break;
 433                 case 0xF: // MVN
 434                     new_pc = ~op2;
 435                     break;
 436                 }
 437             }
 438         }
 439         return ((unsigned long *)new_pc);
 440     case 0x1:
 441         if ((ins & 0x02000010) == 0x02000010) {
 442             // Undefined!
 443             return (pc+1);
 444         } else {
 445             if ((ins & 0x00100000) == 0) {
 446                 // STR
 447                 return (pc+1);
 448             } else {
 449                 // LDR
 450                 if ((ins & 0x0000F000) != 0x0000F000) {
 451                     // Rd not PC
 452                     return (pc+1);
 453                 } else {
 454                     Rn = (unsigned long)get_register((ins & 0x000F0000) >> 16);
 455                     if ((ins & 0x000F0000) == 0x000F0000) Rn += 8;  // PC prefetch!
 456                     if (ins & 0x01000000) {
 457                         // Add/subtract offset before
 458                         if ((ins & 0x02000000) == 0) {
 459                             // Immediate offset
 460                             if (ins & 0x00800000) {
 461                                 // Add offset
 462                                 Rn += (ins & 0x00000FFF);
 463                             } else {
 464                                 // Subtract offset
 465                                 Rn -= (ins & 0x00000FFF);
 466                             }
 467                         } else {
 468                             // Offset is in a register
 469                             if (ins & 0x00800000) {
 470                                 // Add offset
 471                                 Rn += RmShifted(ins & 0x00000FFF);
 472                             } else {
 473                                 // Subtract offset
 474                                 Rn -= RmShifted(ins & 0x00000FFF);
 475                             }
 476                         }
 477                     }
 478                     return ((unsigned long *)*(unsigned long *)Rn);
 479                 }
 480             }
 481         }
 482         return (pc+1);
 483     case 0x2:  // Branch, LDM/STM
 484         if ((ins & 0x02000000) == 0) {
 485             // LDM/STM
 486             if ((ins & 0x00100000) == 0) {
 487                 // STM
 488                 return (pc+1);
 489             } else {
 490                 // LDM
 491                 if ((ins & 0x00008000) == 0) {
 492                     // PC not in list
 493                     return (pc+1);
 494                 } else {
 495                     Rn = (unsigned long)get_register((ins & 0x000F0000) >> 16);
 496                     if ((ins & 0x000F0000) == 0x000F0000) Rn += 8;  // PC prefetch!
 497                     offset = ins & 0x0000FFFF;
 498                     reg_count = 0;
 499                     for (i = 0;  i < 15;  i++) {
 500                         if (offset & (1<<i)) reg_count++;
 501                     }
 502                     if (ins & 0x00800000) {
 503                         // Add offset
 504                         Rn += reg_count*4;
 505                     } else {
 506                         // Subtract offset
 507                         Rn -= 4;
 508                     }
 509                     return ((unsigned long *)*(unsigned long *)Rn);
 510                 }
 511             }
 512         } else {
 513             // Branch
 514             if (ins_will_execute(ins)) {
 515                 offset = (ins & 0x00FFFFFF) << 2;
 516                 if (ins & 0x00800000) offset |= 0xFC000000;  // sign extend
 517                 new_pc = (unsigned long)(pc+2) + offset;
 518                 // If its BLX, make new_pc a thumb address.
 519                 if ((ins & 0xFE000000) == 0xFA000000) {
 520                     if ((ins & 0x01000000) == 0x01000000)
 521                         new_pc |= 2;
 522                     new_pc = MAKE_THUMB_ADDR(new_pc);
 523                 }
 524                 return ((unsigned long *)new_pc);
 525             } else {
 526                 // Falls through
 527                 return (pc+1);
 528             }
 529         }
 530     case 0x3:  // Coprocessor & SWI
 531         if (((ins & 0x03000000) == 0x03000000) && ins_will_execute(ins)) {
 532            // SWI
 533            return (unsigned long *)(CYGNUM_HAL_VECTOR_SOFTWARE_INTERRUPT * 4);
 534         } else {
 535            return (pc+1);
 536         }
 537     default:
 538         // Never reached - but fixes compiler warning.
 539         return 0;
 540     }
 541 }
 542
 543 // FIXME: target_ins also needs to check for CPSR/THUMB being set and
 544 //        set the thumb bit accordingly.
 545
 546 static unsigned long
 547 target_thumb_ins(unsigned long pc, unsigned short ins)
 548 {
 549     unsigned long new_pc = MAKE_THUMB_ADDR(pc+2); // default is fall-through
 550                                         // to next thumb instruction
 551     unsigned long offset, arm_ins, sp;
 552     int i;
 553
 554     switch ((ins & 0xf000) >> 12) {
 555     case 0x4:
 556         // Check for BX or BLX
 557         if ((ins & 0xff07) == 0x4700)
 558             new_pc = (unsigned long)get_register((ins & 0x00078) >> 3);
 559         break;
 560     case 0xb:
 561         // push/pop
 562         // Look for "pop {...,pc}"
 563         if ((ins & 0xf00) == 0xd00) {
 564             // find PC
 565             sp = (unsigned long)get_register(SP);
 566
 567             for (offset = i = 0; i < 8; i++)
 568               if (ins & (1 << i))
 569                   offset += 4;
 570
 571             new_pc = *(cyg_uint32 *)(sp + offset);
 572
 573             if (!v5T_semantics())
 574                 new_pc = MAKE_THUMB_ADDR(new_pc);
 575         }
 576         break;
 577     case 0xd:
 578         // Bcc | SWI
 579         // Use ARM function to check condition
 580         arm_ins = ((unsigned long)(ins & 0x0f00)) << 20;
 581         if ((arm_ins & 0xF0000000) == 0xF0000000) {
 582             // SWI
 583             new_pc = CYGNUM_HAL_VECTOR_SOFTWARE_INTERRUPT * 4;
 584         } else if (ins_will_execute(arm_ins)) {
 585             offset = (ins & 0x00FF) << 1;
 586             if (ins & 0x0080) offset |= 0xFFFFFE00;  // sign extend
 587             new_pc = MAKE_THUMB_ADDR((unsigned long)(pc+4) + offset);
 588         }
 589         break;
 590     case 0xe:
 591         // check for B
 592         if ((ins & 0x0800) == 0) {
 593             offset = (ins & 0x07FF) << 1;
 594             if (ins & 0x0400) offset |= 0xFFFFF800;  // sign extend
 595             new_pc = MAKE_THUMB_ADDR((unsigned long)(pc+4) + offset);
 596         }
 597         break;
 598     case 0xf:
 599         // BL/BLX (4byte instruction!)
 600         // First instruction (bit 11 == 0) holds top-part of offset
 601         if ((ins & 0x0800) == 0) {
 602             offset = (ins & 0x07FF) << 12;
 603             if (ins & 0x0400) offset |= 0xFF800000;  // sign extend
 604             // Get second instruction
 605             // Second instruction (bit 11 == 1) holds bottom-part of offset
 606             ins = *(unsigned short*)(pc+2);
 607             // Check for BL/BLX
 608             if ((ins & 0xE800) == 0xE800) {
 609                 offset |= (ins & 0x07ff) << 1;
 610                 new_pc = (unsigned long)(pc+4) + offset;
 611                 // If its BLX, force a full word alignment
 612                 // Otherwise, its a thumb address.
 613                 if (!(ins & 0x1000))
 614                     new_pc &= ~3;
 615                 else
 616                     new_pc = MAKE_THUMB_ADDR(new_pc);
 617             }
 618         }
 619         break;
 620     }
 621
 622     return new_pc;
 623 }
 624
 625 void __single_step (void)
 626 {
 627     unsigned long pc = get_register(PC);
 628     unsigned long cpsr = get_register(PS);
 629
 630     // Calculate address of next instruction to be executed
 631     if (cpsr & CPSR_THUMB_ENABLE) {
 632         // thumb
 633         ss_saved_pc = target_thumb_ins(pc, *(unsigned short*)pc);
 634     } else {
 635         // ARM
 636         unsigned long curins = *(unsigned long*)pc;
 637         if (ins_will_execute(curins)) {
 638             // Decode instruction to decide what the next PC will be
 639             ss_saved_pc = (unsigned long) target_ins((unsigned long*)pc,
 640                                                      curins);
 641         } else {
 642             // The current instruction will not execute (the conditions
 643             // don't hold)
 644             ss_saved_pc = pc+4;
 645         }
 646     }
 647
 648     // Set breakpoint according to type
 649     if (IS_THUMB_ADDR(ss_saved_pc)) {
 650         // Thumb instruction
 651         unsigned long t_pc = UNMAKE_THUMB_ADDR(ss_saved_pc);
 652         ss_saved_thumb_instr = *(unsigned short*)t_pc;
 653         *(unsigned short*)t_pc = HAL_BREAKINST_THUMB;
 654     } else {
 655         // ARM instruction
 656         ss_saved_instr = *(unsigned long*)ss_saved_pc;
 657         *(unsigned long*)ss_saved_pc = HAL_BREAKINST_ARM;
 658     }
 659 }
 660
 661 /* Clear the single-step state. */
 662
 663 void __clear_single_step (void)
 664 {
 665     if (ss_saved_pc != 0) {
 666         // Restore instruction according to type
 667         if (IS_THUMB_ADDR(ss_saved_pc)) {
 668             // Thumb instruction
 669             unsigned long t_pc = UNMAKE_THUMB_ADDR(ss_saved_pc);
 670             *(unsigned short*)t_pc = ss_saved_thumb_instr;
 671         } else {
 672             // ARM instruction
 673             *(unsigned long*)ss_saved_pc = ss_saved_instr;
 674         }
 675         ss_saved_pc = 0;
 676     }
 677 }
 678
 679 void __install_breakpoints (void)
 680 {
 681 #if defined(CYGNUM_HAL_BREAKPOINT_LIST_SIZE) && (CYGNUM_HAL_BREAKPOINT_LIST_SIZE > 0)
 682     /* Install the breakpoints in the breakpoint list */
 683     __install_breakpoint_list();
 684 #endif
 685 }
 686
 687 void __clear_breakpoints (void)
 688 {
 689 #if defined(CYGNUM_HAL_BREAKPOINT_LIST_SIZE) && (CYGNUM_HAL_BREAKPOINT_LIST_SIZE > 0)
 690     __clear_breakpoint_list();
 691 #endif
 692 }
 693
 694 /* If the breakpoint we hit is in the breakpoint() instruction, return a
 695    non-zero value. */
 696
 697 int
 698 __is_breakpoint_function ()
 699 {
 700     return get_register (PC) == (target_register_t)&_breakinst;
 701 }
 702
 703
 704 /* Skip the current instruction.  Since this is only called by the
 705    stub when the PC points to a breakpoint or trap instruction,
 706    we can safely just skip 4. */
 707
 708 void __skipinst (void)
 709 {
 710     unsigned long pc = get_register(PC);
 711     unsigned long cpsr = get_register(PS);
 712
 713     if (cpsr & CPSR_THUMB_ENABLE)
 714         pc += 2;
 715     else
 716         pc += 4;
 717
 718     put_register(PC, pc);
 719 }
 720
 721 //-----------------------------------------------------------------------
 722 // Thumb-aware GDB interrupt handler.
 723 // This is a brute-force replacement of the ones in hal_stub.c. Need to
 724 // find a better way of handling it... Maybe... Probably only ARM/thumb
 725 // that is this weird.
 726
 727 typedef struct
 728 {
 729     cyg_uint32 targetAddr;
 730     union {
 731         cyg_uint32 arm_instr;
 732         cyg_uint16 thumb_instr;
 733     } savedInstr;
 734 } instrBuffer;
 735
 736 static instrBuffer break_buffer;
 737
 738 volatile int cyg_hal_gdb_running_step = 0;
 739
 740 // This function is passed thumb/arm information about the PC address
 741 // in bit 0. This information is passed on to the break_buffer.
 742 void
 743 cyg_hal_gdb_place_break (target_register_t pc)
 744 {
 745     // Clear flag that we Continued instead of Stepping
 746     cyg_hal_gdb_running_step = 0;
 747
 748     if (0 == break_buffer.targetAddr) {
 749         // Setting a breakpoint in Thumb or ARM code?
 750        if (IS_THUMB_ADDR(pc)) {
 751             break_buffer.targetAddr = (cyg_uint32)pc;
 752             pc = UNMAKE_THUMB_ADDR(pc);
 753             break_buffer.savedInstr.thumb_instr = *(cyg_uint16*)pc;
 754             *(cyg_uint16*)pc = HAL_BREAKINST_THUMB;
 755         } else {
 756             break_buffer.targetAddr = (cyg_uint32)pc;
 757             break_buffer.savedInstr.arm_instr = *(cyg_uint32*)pc;
 758             *(cyg_uint32*)pc = HAL_BREAKINST_ARM;
 759         }
 760
 761         __data_cache(CACHE_FLUSH);
 762         __instruction_cache(CACHE_FLUSH);
 763     }
 764 }
 765
 766 int
 767 cyg_hal_gdb_remove_break (target_register_t pc)
 768 {
 769     if ( cyg_hal_gdb_running_step )
 770         return 0; // Do not remove the break: we must hit it!
 771
 772     if (pc == UNMAKE_THUMB_ADDR(break_buffer.targetAddr)) {
 773         if (IS_THUMB_ADDR(break_buffer.targetAddr)) {
 774             *(cyg_uint16*)pc = break_buffer.savedInstr.thumb_instr;
 775         } else {
 776             *(cyg_uint32*)pc = break_buffer.savedInstr.arm_instr;
 777         }
 778         break_buffer.targetAddr = 0;
 779
 780         __data_cache(CACHE_FLUSH);
 781         __instruction_cache(CACHE_FLUSH);
 782         return 1;
 783     }
 784     return 0;
 785 }
 786
 787 void
 788 cyg_hal_gdb_interrupt (target_register_t pc)
 789 {
 790     // Clear flag that we Continued instead of Stepping
 791     cyg_hal_gdb_running_step = 0;
 792     // and override existing break? So that a ^C takes effect...
 793     if (0 != break_buffer.targetAddr)
 794         cyg_hal_gdb_remove_break( break_buffer.targetAddr );
 795
 796     if (0 == break_buffer.targetAddr) {
 797         cyg_uint32 cpsr = get_register(PS);
 798
 799         if (cpsr & CPSR_THUMB_ENABLE) {
 800             break_buffer.targetAddr = MAKE_THUMB_ADDR((cyg_uint32)pc);
 801             break_buffer.savedInstr.thumb_instr = *(cyg_uint16*)pc;
 802             *(cyg_uint16*)pc = HAL_BREAKINST_THUMB;
 803         } else {
 804             break_buffer.targetAddr = (cyg_uint32)pc;
 805             break_buffer.savedInstr.arm_instr = *(cyg_uint32*)pc;
 806             *(cyg_uint32*)pc = HAL_BREAKINST_ARM;
 807         }
 808
 809         __data_cache(CACHE_FLUSH);
 810         __instruction_cache(CACHE_FLUSH);
 811     }
 812 }
 813
 814 int
 815 cyg_hal_gdb_break_is_set (void)
 816 {
 817     if (0 != break_buffer.targetAddr) {
 818         return 1;
 819     }
 820     return 0;
 821 }
 822
 823 #ifdef CYGHWR_HAL_ARM_ICE_THREAD_SUPPORT
 824 #define ICE_THREAD_KEY0 0xDEAD0001
 825 #define ICE_THREAD_KEY1 0xDEAD0002
 826
 827 #define ICE_THREAD_INBUFSIZ  2048
 828 #define ICE_THREAD_OUTBUFSIZ 2048
 829 #define ICE_THREAD_STACKSIZE 4096
 830
 831 static cyg_uint8 ice_thread_inbuf[ICE_THREAD_INBUFSIZ];
 832 static cyg_uint8 ice_thread_outbuf[ICE_THREAD_OUTBUFSIZ];
 833 static cyg_uint8 ice_thread_stack[ICE_THREAD_STACKSIZE];
 834
 835 static void ice_thread_proc(void);
 836
 837 struct {
 838     cyg_uint32 _key0;  // Must be ICE_KEY0
 839     cyg_uint8  *in_buffer;
 840     cyg_int32  in_buffer_size;
 841     cyg_uint8  *out_buffer;
 842     cyg_int32  out_buffer_size;
 843     cyg_uint8  *stack;
 844     cyg_int32  stack_size;
 845     void       (*fun)(void);
 846     cyg_uint32 _key1;  // Must be ICE_KEY1
 847 } hal_arm_ice_thread_handler = {
 848     ICE_THREAD_KEY0,
 849     ice_thread_inbuf,
 850     ICE_THREAD_INBUFSIZ,
 851     ice_thread_outbuf,
 852     ICE_THREAD_OUTBUFSIZ,
 853     ice_thread_stack,
 854     ICE_THREAD_STACKSIZE,
 855     ice_thread_proc,
 856     ICE_THREAD_KEY1,
 857 };
 858
 859 static int
 860 ice_thread_query(void)
 861 {
 862     switch (ice_thread_inbuf[1]) {
 863     case 'L': // get thread list
 864         stub_pkt_getthreadlist(&ice_thread_inbuf[2], ice_thread_outbuf, sizeof(ice_thread_outbuf));
 865         break;
 866     case 'P': // thread or process information
 867         stub_pkt_getthreadinfo(&ice_thread_inbuf[2], ice_thread_outbuf, sizeof(ice_thread_outbuf));
 868         break;
 869     case 'C': // current thread
 870         stub_pkt_currthread(&ice_thread_inbuf[2], ice_thread_outbuf, sizeof(ice_thread_outbuf));
 871         break;
 872     default:
 873         return 0;
 874     }
 875     return 1;
 876 }
 877
 878 static int
 879 ice_thread_set(void)
 880 {
 881     return 0;
 882 }
 883
 884 static void
 885 ice_thread_proc(void)
 886 {
 887     switch (ice_thread_inbuf[0]) {
 888     case 'g': // Fetch thread registers
 889         stub_format_registers(ice_thread_outbuf);
 890         return;
 891     case 'P': // Update a single register
 892     case 'G': // Update all registers
 893         stub_update_registers(ice_thread_inbuf, ice_thread_outbuf);
 894         return;
 895     case 'H': // Thread set/query
 896         stub_pkt_changethread(&ice_thread_inbuf[1], ice_thread_outbuf, sizeof(ice_thread_outbuf));
 897         return;
 898     case 'q': // Thread queries
 899         if (ice_thread_query()) return;
 900         break;
 901     case 'Q': // Thread set operations
 902         if (ice_thread_set()) return;
 903         break;
 904     case 'T': // Thread alive?
 905         stub_pkt_thread_alive(&ice_thread_inbuf[1], ice_thread_outbuf, sizeof(ice_thread_outbuf));
 906         return;
 907     default:
 908     }
 909     strcpy(ice_thread_outbuf, "ENN");  // Dunno
 910 }
 911
 912 #endif // CYGHWR_HAL_ARM_ICE_THREAD_SUPPORT
 913
 914 #endif // CYGDBG_HAL_DEBUG_GDB_INCLUDE_STUBS