unified MX27, MX25, MX37 trees
[karo-tx-redboot.git] / 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