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