sys/contrib/octeon-sdk/cvmx-debug.c

   1 /***********************license start***************
   2  * Copyright (c) 2003-2010  Cavium Inc. (support@cavium.com). All rights
   3  * reserved.
   4  *
   5  *
   6  * Redistribution and use in source and binary forms, with or without
   7  * modification, are permitted provided that the following conditions are
   8  * met:
   9  *
  10  *   * Redistributions of source code must retain the above copyright
  11  *     notice, this list of conditions and the following disclaimer.
  12  *
  13  *   * Redistributions in binary form must reproduce the above
  14  *     copyright notice, this list of conditions and the following
  15  *     disclaimer in the documentation and/or other materials provided
  16  *     with the distribution.
  17
  18  *   * Neither the name of Cavium Inc. nor the names of
  19  *     its contributors may be used to endorse or promote products
  20  *     derived from this software without specific prior written
  21  *     permission.
  22
  23  * This Software, including technical data, may be subject to U.S. export  control
  24  * laws, including the U.S. Export Administration Act and its  associated
  25  * regulations, and may be subject to export or import  regulations in other
  26  * countries.
  27
  28  * TO THE MAXIMUM EXTENT PERMITTED BY LAW, THE SOFTWARE IS PROVIDED "AS IS"
  29  * AND WITH ALL FAULTS AND CAVIUM INC. MAKES NO PROMISES, REPRESENTATIONS OR
  30  * WARRANTIES, EITHER EXPRESS, IMPLIED, STATUTORY, OR OTHERWISE, WITH RESPECT TO
  31  * THE SOFTWARE, INCLUDING ITS CONDITION, ITS CONFORMITY TO ANY REPRESENTATION OR
  32  * DESCRIPTION, OR THE EXISTENCE OF ANY LATENT OR PATENT DEFECTS, AND CAVIUM
  33  * SPECIFICALLY DISCLAIMS ALL IMPLIED (IF ANY) WARRANTIES OF TITLE,
  34  * MERCHANTABILITY, NONINFRINGEMENT, FITNESS FOR A PARTICULAR PURPOSE, LACK OF
  35  * VIRUSES, ACCURACY OR COMPLETENESS, QUIET ENJOYMENT, QUIET POSSESSION OR
  36  * CORRESPONDENCE TO DESCRIPTION. THE ENTIRE  RISK ARISING OUT OF USE OR
  37  * PERFORMANCE OF THE SOFTWARE LIES WITH YOU.
  38  ***********************license end**************************************/
  39
  40
  41 /*
  42  * @file
  43  *
  44  * Interface to debug exception handler
  45  * NOTE: CARE SHOULD BE TAKE WHEN USING STD C LIBRARY FUNCTIONS IN
  46  * THIS FILE IF SOMEONE PUTS A BREAKPOINT ON THOSE FUNCTIONS
  47  * DEBUGGING WILL FAIL.
  48  *
  49  * <hr>$Revision: 50060 $<hr>
  50  */
  51
  52 #ifdef CVMX_BUILD_FOR_LINUX_KERNEL
  53 #include <linux/module.h>
  54 #include <asm/octeon/octeon.h>
  55 #include <asm/octeon/cvmx.h>
  56 #include <asm/octeon/cvmx-debug.h>
  57 #include <asm/octeon/cvmx-core.h>
  58 #include <asm/octeon/cvmx-bootmem.h>
  59 #include <asm/octeon/octeon-boot-info.h>
  60 #else
  61 #include <stdint.h>
  62 #include "cvmx.h"
  63 #include "cvmx-debug.h"
  64 #include "cvmx-bootmem.h"
  65 #include "cvmx-core.h"
  66 #include "cvmx-coremask.h"
  67 #include "octeon-boot-info.h"
  68 #endif
  69
  70 #ifdef CVMX_DEBUG_LOGGING
  71 # undef CVMX_DEBUG_LOGGING
  72 # define CVMX_DEBUG_LOGGING 1
  73 #else
  74 # define CVMX_DEBUG_LOGGING 0
  75 #endif
  76
  77 #ifndef CVMX_DEBUG_ATTACH
  78 # define CVMX_DEBUG_ATTACH 1
  79 #endif
  80
  81 #define CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_STATUS            (0xFFFFFFFFFF301000ull)
  82 #define CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_ADDRESS(num)      (0xFFFFFFFFFF301100ull + 0x100 * (num))
  83 #define CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_ADDRESS_MASK(num) (0xFFFFFFFFFF301108ull + 0x100 * (num))
  84 #define CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_ASID(num)         (0xFFFFFFFFFF301110ull + 0x100 * (num))
  85 #define CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_CONTROL(num)      (0xFFFFFFFFFF301118ull + 0x100 * (num))
  86
  87 #define CVMX_DEBUG_HW_DATA_BREAKPOINT_STATUS                   (0xFFFFFFFFFF302000ull)
  88 #define CVMX_DEBUG_HW_DATA_BREAKPOINT_ADDRESS(num)             (0xFFFFFFFFFF302100ull + 0x100 * (num))
  89 #define CVMX_DEBUG_HW_DATA_BREAKPOINT_ADDRESS_MASK(num)        (0xFFFFFFFFFF302108ull + 0x100 * (num))
  90 #define CVMX_DEBUG_HW_DATA_BREAKPOINT_ASID(num)                (0xFFFFFFFFFF302110ull + 0x100 * (num))
  91 #define CVMX_DEBUG_HW_DATA_BREAKPOINT_CONTROL(num)             (0xFFFFFFFFFF302118ull + 0x100 * (num))
  92
  93 #define ERET_INSN  0x42000018U      /* Hexcode for eret */
  94 #define ISR_DELAY_COUNTER     120000000       /* Could be tuned down */
  95
  96 extern cvmx_debug_comm_t cvmx_debug_uart_comm;
  97 extern cvmx_debug_comm_t cvmx_debug_remote_comm;
  98 static const cvmx_debug_comm_t *cvmx_debug_comms[COMM_SIZE] = {&cvmx_debug_uart_comm, &cvmx_debug_remote_comm};
  99
 100
 101
 102 static cvmx_debug_globals_t *cvmx_debug_globals;
 103
 104 /**
 105  * @file
 106  *
 107  */
 108
 109 #ifndef CVMX_BUILD_FOR_LINUX_KERNEL
 110 uint64_t __cvmx_debug_save_regs_area[32];
 111
 112 volatile uint64_t __cvmx_debug_mode_exception_ignore;
 113 volatile uint64_t __cvmx_debug_mode_exception_occured;
 114
 115 static char cvmx_debug_stack[8*1024] __attribute ((aligned (16)));
 116 char *__cvmx_debug_stack_top = &cvmx_debug_stack[8*1024];
 117
 118 #ifndef CVMX_BUILD_FOR_TOOLCHAIN
 119 extern int cvmx_interrupt_in_isr;
 120 #else
 121 #define cvmx_interrupt_in_isr 0
 122 #endif
 123
 124 #else
 125 uint64_t __cvmx_debug_save_regs_area_all[CVMX_MAX_CORES][32];
 126 #define __cvmx_debug_save_regs_area __cvmx_debug_save_regs_area_all[cvmx_get_core_num()]
 127
 128 volatile uint64_t __cvmx_debug_mode_exception_ignore_all[CVMX_MAX_CORES];
 129 #define __cvmx_debug_mode_exception_ignore __cvmx_debug_mode_exception_ignore_all[cvmx_get_core_num()]
 130 volatile uint64_t __cvmx_debug_mode_exception_occured_all[CVMX_MAX_CORES];
 131 #define __cvmx_debug_mode_exception_occured __cvmx_debug_mode_exception_occured_all[cvmx_get_core_num()]
 132
 133 static char cvmx_debug_stack_all[CVMX_MAX_CORES][8*1024] __attribute ((aligned (16)));
 134 char *__cvmx_debug_stack_top_all[CVMX_MAX_CORES];
 135
 136 #define cvmx_interrupt_in_isr 0
 137
 138 #endif
 139
 140
 141 static size_t cvmx_debug_strlen (const char *str)
 142 {
 143     size_t size = 0;
 144     while (*str)
 145     {
 146         size++;
 147         str++;
 148     }
 149     return size;
 150 }
 151 static void cvmx_debug_strcpy (char *dest, const char *src)
 152 {
 153     while (*src)
 154     {
 155         *dest = *src;
 156         src++;
 157         dest++;
 158     }
 159     *dest = 0;
 160 }
 161
 162 static void cvmx_debug_memcpy_align (void *dest, const void *src, int size) __attribute__ ((__noinline__));
 163 static void cvmx_debug_memcpy_align (void *dest, const void *src, int size)
 164 {
 165   long long *dest1 = (long long*)dest;
 166   const long long *src1 = (const long long*)src;
 167   int i;
 168   if (size == 40)
 169   {
 170     long long a0, a1, a2, a3, a4;
 171     a0 = src1[0];
 172     a1 = src1[1];
 173     a2 = src1[2];
 174     a3 = src1[3];
 175     a4 = src1[4];
 176     dest1[0] = a0;
 177     dest1[1] = a1;
 178     dest1[2] = a2;
 179     dest1[3] = a3;
 180     dest1[4] = a4;
 181     return;
 182   }
 183   for(i = 0;i < size;i+=8)
 184   {
 185     *dest1 = *src1;
 186     dest1++;
 187     src1++;
 188   }
 189 }
 190
 191
 192 static inline uint32_t cvmx_debug_core_mask(void)
 193 {
 194 #ifndef CVMX_BUILD_FOR_LINUX_KERNEL
 195 #ifdef CVMX_BUILD_FOR_TOOLCHAIN
 196   extern int __octeon_core_mask;
 197   return __octeon_core_mask;
 198 #endif
 199 return cvmx_sysinfo_get()->core_mask;
 200 #else
 201 return octeon_get_boot_coremask ();
 202 #endif
 203 }
 204
 205 static inline void cvmx_debug_update_state(cvmx_debug_state_t state)
 206 {
 207     cvmx_debug_memcpy_align(cvmx_debug_globals->state, &state, sizeof(cvmx_debug_state_t));
 208 }
 209
 210 static inline cvmx_debug_state_t cvmx_debug_get_state(void)
 211 {
 212     cvmx_debug_state_t state;
 213     cvmx_debug_memcpy_align(&state, cvmx_debug_globals->state, sizeof(cvmx_debug_state_t));
 214     return state;
 215 }
 216
 217 static void cvmx_debug_printf(char *format, ...) __attribute__((format(__printf__, 1, 2)));
 218 static void cvmx_debug_printf(char *format, ...)
 219 {
 220     va_list ap;
 221
 222     if (!CVMX_DEBUG_LOGGING)
 223         return;
 224
 225     va_start(ap, format);
 226     cvmx_dvprintf(format, ap);
 227     va_end(ap);
 228 }
 229
 230 static inline int __cvmx_debug_in_focus(cvmx_debug_state_t state, unsigned core)
 231 {
 232     return state.focus_core == core;
 233 }
 234
 235 static void cvmx_debug_install_handler(unsigned core)
 236 {
 237     extern void __cvmx_debug_handler_stage2(void);
 238     int32_t *trampoline = CASTPTR(int32_t, CVMX_ADD_SEG32(CVMX_MIPS32_SPACE_KSEG0, BOOTLOADER_DEBUG_TRAMPOLINE_CORE));
 239     trampoline += core;
 240
 241     *trampoline = (int32_t)(long)&__cvmx_debug_handler_stage2;
 242
 243     cvmx_debug_printf("Debug handled installed on core %d at %p\n", core, trampoline);
 244 }
 245
 246 static int cvmx_debug_enabled(void)
 247 {
 248     return cvmx_debug_booted() || CVMX_DEBUG_ATTACH;
 249 }
 250
 251 static void cvmx_debug_init_global_ptr (void *ptr)
 252 {
 253     uint64_t phys = cvmx_ptr_to_phys (ptr);
 254     cvmx_debug_globals_t *p;
 255     /* Since at this point, TLBs are not mapped 1 to 1, we should just use KSEG0 accesses. */
 256     p = CASTPTR(cvmx_debug_globals_t, CVMX_ADD_SEG32(CVMX_MIPS32_SPACE_KSEG0, phys));
 257     memset (p, 0, sizeof(cvmx_debug_globals_t));
 258     p->version = CVMX_DEBUG_GLOBALS_VERSION;
 259     p->tlb_entries = cvmx_core_get_tlb_entries();
 260 }
 261
 262 static void cvmx_debug_init_globals(void)
 263 {
 264     uint64_t phys;
 265     void *ptr;
 266
 267     if (cvmx_debug_globals)
 268         return;
 269     ptr = cvmx_bootmem_alloc_named_range_once(sizeof(cvmx_debug_globals_t), 0, /* KSEG0 max, 512MB=*/0/*1024*1024*512*/, 8,
 270                                               CVMX_DEBUG_GLOBALS_BLOCK_NAME, cvmx_debug_init_global_ptr);
 271     phys = cvmx_ptr_to_phys (ptr);
 272
 273     /* Since TLBs are not always mapped 1 to 1, we should just use access via KSEG0. */
 274     cvmx_debug_globals = CASTPTR(cvmx_debug_globals_t, CVMX_ADD_SEG32(CVMX_MIPS32_SPACE_KSEG0, phys));
 275     cvmx_debug_printf("Debug named block at %p\n", cvmx_debug_globals);
 276 }
 277
 278
 279 static void cvmx_debug_globals_check_version(void)
 280 {
 281     if (cvmx_debug_globals->version != CVMX_DEBUG_GLOBALS_VERSION)
 282     {
 283         cvmx_dprintf("Wrong version on the globals struct spinining; expected %d, got:  %d.\n", (int)CVMX_DEBUG_GLOBALS_VERSION, (int)(cvmx_debug_globals->version));
 284 #ifdef CVMX_BUILD_FOR_LINUX_KERNEL
 285         panic("Wrong version.\n");
 286 #endif
 287         while (1)
 288             ;
 289     }
 290 }
 291
 292 static inline volatile cvmx_debug_core_context_t *cvmx_debug_core_context(void);
 293 static inline void cvmx_debug_save_core_context(volatile cvmx_debug_core_context_t *context, uint64_t hi, uint64_t lo);
 294
 295 void cvmx_debug_init(void)
 296 {
 297     cvmx_debug_state_t state;
 298     int core;
 299     const cvmx_debug_comm_t *comm;
 300     cvmx_spinlock_t *lock;
 301     unsigned int coremask = cvmx_debug_core_mask();
 302
 303     if (!cvmx_debug_enabled())
 304         return;
 305
 306     cvmx_debug_init_globals();
 307
 308 #ifndef CVMX_BUILD_FOR_LINUX_KERNEL
 309     // Put a barrier until all cores have got to this point.
 310     cvmx_coremask_barrier_sync(coremask);
 311 #endif
 312     cvmx_debug_globals_check_version();
 313
 314
 315     comm = cvmx_debug_comms[cvmx_debug_globals->comm_type];
 316     lock = &cvmx_debug_globals->lock;
 317
 318     core = cvmx_get_core_num();
 319 #ifdef CVMX_BUILD_FOR_LINUX_KERNEL
 320     /*  Install the debugger handler on the cores. */
 321     {
 322         int core1 = 0;
 323         for (core1 = 0; core1 < CVMX_MAX_CORES; core1++)
 324         {
 325             if ((1u<<core1) & coremask)
 326                 cvmx_debug_install_handler(core1);
 327         }
 328     }
 329 #else
 330     cvmx_debug_install_handler(core);
 331 #endif
 332
 333     if (comm->init)
 334         comm->init();
 335
 336     {
 337         cvmx_spinlock_lock(lock);
 338         state = cvmx_debug_get_state();
 339 #ifdef CVMX_BUILD_FOR_LINUX_KERNEL
 340         state.known_cores |= coremask;
 341         state.core_finished &= ~coremask;
 342 #else
 343         state.known_cores |= (1u << core);
 344         state.core_finished &= ~(1u << core);
 345 #endif
 346         cvmx_debug_update_state(state);
 347         cvmx_spinlock_unlock(lock);
 348     }
 349
 350 #ifndef CVMX_BUILD_FOR_LINUX_KERNEL
 351     // Put a barrier until all cores have got to this point.
 352     cvmx_coremask_barrier_sync(coremask);
 353
 354     if (cvmx_coremask_first_core(coremask))
 355 #endif
 356     {
 357         cvmx_debug_printf("cvmx_debug_init core: %d\n", core);
 358         state = cvmx_debug_get_state();
 359         state.focus_core = core;
 360         state.active_cores = state.known_cores;
 361         state.focus_switch = 1;
 362         state.step_isr = 1;
 363         cvmx_debug_printf("Known cores at init: 0x%x\n", (int)state.known_cores);
 364         cvmx_debug_update_state(state);
 365
 366         /* Initialize __cvmx_debug_stack_top_all. */
 367 #ifdef CVMX_BUILD_FOR_LINUX_KERNEL
 368         {
 369             int i;
 370             for (i = 0; i < CVMX_MAX_CORES; i++)
 371                 __cvmx_debug_stack_top_all[i] = &cvmx_debug_stack_all[i][8*1024];
 372         }
 373 #endif
 374         cvmx_debug_globals->init_complete = 1;
 375         CVMX_SYNCW;
 376     }
 377     while (!cvmx_debug_globals->init_complete)
 378     {
 379         /* Spin waiting for init to complete */
 380     }
 381
 382     if (cvmx_debug_booted())
 383         cvmx_debug_trigger_exception();
 384
 385     /*  Install the break handler after might tripper the debugger exception. */
 386 #ifndef CVMX_BUILD_FOR_LINUX_KERNEL
 387     if (cvmx_coremask_first_core(coremask))
 388 #endif
 389     {
 390         if (comm->install_break_handler)
 391             comm->install_break_handler();
 392     }
 393 }
 394
 395 static const char cvmx_debug_hexchar[] = "0123456789ABCDEF";
 396 /* Put the hex value of t into str. */
 397 static void cvmx_debug_int8_to_strhex(char *str, unsigned char t)
 398 {
 399   str[0] = cvmx_debug_hexchar[(t>>4)&0xf];
 400   str[1] = cvmx_debug_hexchar[t&0xF];
 401   str[2] = 0;
 402 }
 403
 404 static void cvmx_debug_int64_to_strhex(char *str, uint64_t t)
 405 {
 406   str[0] = cvmx_debug_hexchar[(t>>60)&0xF];
 407   str[1] = cvmx_debug_hexchar[(t>>56)&0xF];
 408   str[2] = cvmx_debug_hexchar[(t>>52)&0xF];
 409   str[3] = cvmx_debug_hexchar[(t>>48)&0xF];
 410   str[4] = cvmx_debug_hexchar[(t>>44)&0xF];
 411   str[5] = cvmx_debug_hexchar[(t>>40)&0xF];
 412   str[6] = cvmx_debug_hexchar[(t>>36)&0xF];
 413   str[7] = cvmx_debug_hexchar[(t>>32)&0xF];
 414   str[8] = cvmx_debug_hexchar[(t>>28)&0xF];
 415   str[9] = cvmx_debug_hexchar[(t>>24)&0xF];
 416   str[10] = cvmx_debug_hexchar[(t>>20)&0xF];
 417   str[11] = cvmx_debug_hexchar[(t>>16)&0xF];
 418   str[12] = cvmx_debug_hexchar[(t>>12)&0xF];
 419   str[13] = cvmx_debug_hexchar[(t>>8)&0xF];
 420   str[14] = cvmx_debug_hexchar[(t>>4)&0xF];
 421   str[15] = cvmx_debug_hexchar[(t>>0)&0xF];
 422   str[16] = 0;
 423 }
 424
 425 static int cvmx_debug_putpacket_noformat(char *packet)
 426 {
 427     if (cvmx_debug_comms[cvmx_debug_globals->comm_type]->putpacket == NULL)
 428         return 0;
 429     cvmx_debug_printf("Reply: %s\n", packet);
 430     return cvmx_debug_comms[cvmx_debug_globals->comm_type]->putpacket(packet);
 431 }
 432
 433 static int cvmx_debug_putcorepacket(char *buf, int core)
 434 {
 435      char *tmp = "!Core XX ";
 436      int tmpsize = cvmx_debug_strlen(tmp);
 437      int bufsize = cvmx_debug_strlen(buf);
 438      char *packet = __builtin_alloca(tmpsize + bufsize + 1);
 439      cvmx_debug_strcpy(packet, tmp);
 440      cvmx_debug_strcpy(&packet[tmpsize], buf);
 441      if (core < 10)
 442      {
 443          packet[6] = ' ';
 444          packet[7] = core + '0';
 445      }
 446      else if (core < 20)
 447      {
 448          packet[6] = '1';
 449          packet[7] = core - 10 + '0';
 450      }
 451      else if (core < 30)
 452      {
 453          packet[6] = '2';
 454          packet[7] = core - 20 + '0';
 455      }
 456      else
 457      {
 458          packet[6] = '3';
 459          packet[7] = core - 30 + '0';
 460      }
 461      return cvmx_debug_putpacket_noformat(packet);
 462 }
 463
 464 /* Put a buf followed by an integer formated as a hex.  */
 465 static int cvmx_debug_putpacket_hexint(char *buf, uint64_t value)
 466 {
 467     size_t size = cvmx_debug_strlen(buf);
 468     char *packet = __builtin_alloca(size + 16 + 1);
 469     cvmx_debug_strcpy(packet, buf);
 470     cvmx_debug_int64_to_strhex(&packet[size], value);
 471     return cvmx_debug_putpacket_noformat(packet);
 472 }
 473
 474 static int cvmx_debug_active_core(cvmx_debug_state_t state, unsigned core)
 475 {
 476     return state.active_cores & (1u << core);
 477 }
 478
 479 static volatile cvmx_debug_core_context_t *cvmx_debug_core_context(void)
 480 {
 481     return &cvmx_debug_globals->contextes[cvmx_get_core_num()];
 482 }
 483
 484 static volatile uint64_t *cvmx_debug_regnum_to_context_ref(int regnum, volatile cvmx_debug_core_context_t *context)
 485 {
 486     /* Must be kept in sync with mips_octeon_reg_names in gdb/mips-tdep.c. */
 487     if (regnum < 32)
 488         return &context->regs[regnum];
 489     switch (regnum)
 490     {
 491         case 32: return &context->cop0.status;
 492         case 33: return &context->lo;
 493         case 34: return &context->hi;
 494         case 35: return &context->cop0.badvaddr;
 495         case 36: return &context->cop0.cause;
 496         case 37: return &context->cop0.depc;
 497         default: return NULL;
 498     }
 499 }
 500
 501 static int cvmx_debug_probe_load(unsigned char *ptr, unsigned char *result)
 502 {
 503     volatile unsigned char *p = ptr;
 504     int ok;
 505     unsigned char tem;
 506
 507     {
 508         __cvmx_debug_mode_exception_ignore = 1;
 509         __cvmx_debug_mode_exception_occured = 0;
 510         /* We don't handle debug-mode exceptions in delay slots.  Avoid them.  */
 511         asm volatile (".set push                \n\t"
 512                       ".set noreorder   \n\t"
 513                       "nop                      \n\t"
 514                       "lbu %0, %1               \n\t"
 515                       "nop                      \n\t"
 516                       ".set pop" : "=r"(tem) : "m"(*p));
 517         ok = __cvmx_debug_mode_exception_occured == 0;
 518         __cvmx_debug_mode_exception_ignore = 0;
 519         __cvmx_debug_mode_exception_occured = 0;
 520         *result = tem;
 521     }
 522     return ok;
 523 }
 524
 525 static int cvmx_debug_probe_store(unsigned char *ptr)
 526 {
 527     volatile unsigned char *p = ptr;
 528     int ok;
 529
 530     __cvmx_debug_mode_exception_ignore = 1;
 531     __cvmx_debug_mode_exception_occured = 0;
 532     /* We don't handle debug-mode exceptions in delay slots.  Avoid them.  */
 533     asm volatile (".set push            \n\t"
 534                   ".set noreorder       \n\t"
 535                   "nop                  \n\t"
 536                   "sb $0, %0            \n\t"
 537                   "nop                  \n\t"
 538                   ".set pop" : "=m"(*p));
 539     ok = __cvmx_debug_mode_exception_occured == 0;
 540
 541     __cvmx_debug_mode_exception_ignore = 0;
 542     __cvmx_debug_mode_exception_occured = 0;
 543     return ok;
 544 }
 545
 546
 547 /**
 548  * Routines to handle hex data
 549  *
 550  * @param ch
 551  * @return
 552  */
 553 static inline int cvmx_debug_hex(char ch)
 554 {
 555     if ((ch >= 'a') && (ch <= 'f'))
 556         return(ch - 'a' + 10);
 557     if ((ch >= '0') && (ch <= '9'))
 558         return(ch - '0');
 559     if ((ch >= 'A') && (ch <= 'F'))
 560         return(ch - 'A' + 10);
 561     return(-1);
 562 }
 563
 564 /**
 565  * While we find nice hex chars, build an int.
 566  * Return number of chars processed.
 567  *
 568  * @param ptr
 569  * @param intValue
 570  * @return
 571  */
 572 static int cvmx_debug_hexToLong(const char **ptr, uint64_t *intValue)
 573 {
 574     int numChars = 0;
 575     long hexValue;
 576
 577     *intValue = 0;
 578     while (**ptr)
 579     {
 580         hexValue = cvmx_debug_hex(**ptr);
 581         if (hexValue < 0)
 582             break;
 583
 584         *intValue = (*intValue << 4) | hexValue;
 585         numChars ++;
 586
 587         (*ptr)++;
 588     }
 589
 590     return(numChars);
 591 }
 592
 593 /**
 594   * Initialize the performance counter control registers.
 595   *
 596   */
 597 static void cvmx_debug_set_perf_control_reg (volatile cvmx_debug_core_context_t *context, int perf_event, int perf_counter)
 598 {
 599     cvmx_core_perf_control_t control;
 600
 601     control.u32 = 0;
 602     control.s.u = 1;
 603     control.s.s = 1;
 604     control.s.k = 1;
 605     control.s.ex = 1;
 606     control.s.w = 1;
 607     control.s.m = 1 - perf_counter;
 608     control.s.event = perf_event;
 609
 610     context->cop0.perfctrl[perf_counter] = control.u32;
 611 }
 612
 613 static cvmx_debug_command_t cvmx_debug_process_packet(const char *packet)
 614 {
 615     const char *buf = packet;
 616     cvmx_debug_command_t result = COMMAND_NOP;
 617     cvmx_debug_state_t state = cvmx_debug_get_state();
 618
 619     /* A one letter command code represents what to do.  */
 620     switch (*buf++)
 621     {
 622         case '?':   /* What protocol do I speak? */
 623             cvmx_debug_putpacket_noformat("S0A");
 624             break;
 625
 626         case '\003':   /* Control-C */
 627             cvmx_debug_putpacket_noformat("T9");
 628             break;
 629
 630         case 'F':   /* Change the focus core */
 631         {
 632             uint64_t core;
 633             if (!cvmx_debug_hexToLong(&buf, &core))
 634             {
 635                 cvmx_debug_putpacket_noformat("!Uknown core.  Focus not changed.");
 636             }
 637             /* Only cores in the exception handler may become the focus.
 638                If a core not in the exception handler got focus the
 639                debugger would hang since nobody would talk to it.  */
 640             else if (state.handler_cores & (1u << core))
 641             {
 642                 /* Focus change reply must be sent before the focus
 643                    changes. Otherwise the new focus core will eat our ACK
 644                    from the debugger.  */
 645                 cvmx_debug_putpacket_hexint("F", core);
 646                 cvmx_debug_comms[cvmx_debug_globals->comm_type]->change_core(state.focus_core, core);
 647                 state.focus_core = core;
 648                 cvmx_debug_update_state(state);
 649                 break;
 650             }
 651             else
 652                 cvmx_debug_putpacket_noformat("!Core is not in the exception handler. Focus not changed.");
 653         /* Nothing changed, so we send back the old value */
 654         }
 655         /* fall through */
 656         case 'f':   /* Get the focus core */
 657             cvmx_debug_putpacket_hexint("F", state.focus_core);
 658             break;
 659
 660         case 'J': /* Set the flag for skip-over-isr in Single-Stepping mode */
 661         {
 662             if (*buf == '1')
 663                 state.step_isr = 1;   /* Step in ISR */
 664             else
 665                 state.step_isr = 0;   /* Step over ISR */
 666             cvmx_debug_update_state(state);
 667         }
 668         /* Fall through. The reply to the set step-isr command is the
 669            same as the get step-isr command */
 670
 671         case 'j':   /* Reply with step_isr status  */
 672             cvmx_debug_putpacket_hexint("J", (unsigned)state.step_isr);
 673             break;
 674
 675
 676         case 'I':   /* Set the active cores */
 677         {
 678             uint64_t active_cores;
 679             if (!cvmx_debug_hexToLong(&buf, &active_cores))
 680                 active_cores = 0;
 681             /* Limit the active mask to the known to exist cores */
 682             state.active_cores = active_cores & state.known_cores;
 683
 684             /* Lazy user hack to have 0 be all cores */
 685             if (state.active_cores == 0)
 686                 state.active_cores = state.known_cores;
 687
 688             /* The focus core must be in the active_cores mask */
 689             if ((state.active_cores & (1u << state.focus_core)) == 0)
 690             {
 691                 cvmx_debug_putpacket_noformat("!Focus core was added to the masked.");
 692                 state.active_cores |= 1u << state.focus_core;
 693             }
 694
 695             cvmx_debug_update_state(state);
 696         }
 697         /* Fall through. The reply to the set active cores command is the
 698            same as the get active cores command */
 699
 700         case 'i':   /* Get the active cores */
 701             cvmx_debug_putpacket_hexint("I", state.active_cores);
 702             break;
 703
 704         case 'A':   /* Setting the step mode all or one */
 705         {
 706             if (*buf == '1')
 707                 state.step_all = 1;   /* A step or continue will start all cores */
 708             else
 709                 state.step_all = 0;   /* A step or continue only affects the focus core */
 710             cvmx_debug_update_state(state);
 711         }
 712         /* Fall through. The reply to the set step-all command is the
 713            same as the get step-all command */
 714
 715         case 'a':   /* Getting the current step mode */
 716             cvmx_debug_putpacket_hexint("A", state.step_all);
 717             break;
 718
 719         case 'g':   /* read a register from global place. */
 720         {
 721             volatile cvmx_debug_core_context_t *context = cvmx_debug_core_context();
 722             uint64_t regno;
 723             volatile uint64_t *reg;
 724
 725             /* Get the register number to read */
 726             if (!cvmx_debug_hexToLong(&buf, &regno))
 727             {
 728                 cvmx_debug_printf("Register number cannot be read.\n");
 729                 cvmx_debug_putpacket_hexint("", 0xDEADBEEF);
 730                 break;
 731             }
 732
 733             reg = cvmx_debug_regnum_to_context_ref(regno, context);
 734             if (!reg)
 735             {
 736                 cvmx_debug_printf("Register #%d is not valid\n", (int)regno);
 737                 cvmx_debug_putpacket_hexint("", 0xDEADBEEF);
 738                 break;
 739             }
 740             cvmx_debug_putpacket_hexint("", *reg);
 741         }
 742         break;
 743
 744         case 'G':   /* set the value of a register. */
 745         {
 746             volatile cvmx_debug_core_context_t *context = cvmx_debug_core_context();
 747             uint64_t regno;
 748             volatile uint64_t *reg;
 749             uint64_t value;
 750
 751             /* Get the register number to write. It should be followed by
 752                a comma */
 753             if (!cvmx_debug_hexToLong(&buf, &regno)
 754                 || (*buf++ != ',')
 755                 || !cvmx_debug_hexToLong(&buf, &value))
 756             {
 757                 cvmx_debug_printf("G packet corrupt: %s\n", buf);
 758                 goto error_packet;
 759             }
 760
 761             reg = cvmx_debug_regnum_to_context_ref(regno, context);
 762             if (!reg)
 763             {
 764                 cvmx_debug_printf("Register #%d is not valid\n", (int)regno);
 765                 goto error_packet;
 766             }
 767             *reg = value;
 768         }
 769         break;
 770
 771         case 'm':   /* Memory read. mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
 772         {
 773             uint64_t addr, i, length;
 774             unsigned char *ptr;
 775             char *reply;
 776
 777             /* Get the memory address, a comma, and the length */
 778             if (!cvmx_debug_hexToLong(&buf, &addr)
 779                 || (*buf++ != ',')
 780                 || !cvmx_debug_hexToLong(&buf, &length))
 781             {
 782                 cvmx_debug_printf("m packet corrupt: %s\n", buf);
 783                 goto error_packet;
 784             }
 785             if (length >= 1024)
 786             {
 787                 cvmx_debug_printf("m packet length out of range: %lld\n", (long long)length);
 788                 goto error_packet;
 789             }
 790
 791             reply = __builtin_alloca(length * 2 + 1);
 792             ptr = (unsigned char *)(long)addr;
 793             for (i = 0; i < length; i++)
 794             {
 795                 /* Probe memory.  If not accessible fail.   */
 796                 unsigned char t;
 797                 if (!cvmx_debug_probe_load(&ptr[i], &t))
 798                   goto error_packet;
 799                 cvmx_debug_int8_to_strhex(&reply[i * 2], t);
 800             }
 801             cvmx_debug_putpacket_noformat(reply);
 802         }
 803         break;
 804
 805         case 'M':   /* Memory write. MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */
 806         {
 807             uint64_t addr, i, length;
 808             unsigned char *ptr;
 809
 810             if (!cvmx_debug_hexToLong(&buf, &addr)
 811                 || *buf++ != ','
 812                 || !cvmx_debug_hexToLong(&buf, &length)
 813                 || *buf++ != ':')
 814             {
 815                 cvmx_debug_printf("M packet corrupt: %s\n", buf);
 816                 goto error_packet;
 817             }
 818
 819             ptr = (unsigned char *)(long)addr;
 820             for (i = 0; i < length; i++)
 821             {
 822                 int n, n1;
 823                 unsigned char c;
 824
 825                 n = cvmx_debug_hex(buf[i * 2]);
 826                 n1 = cvmx_debug_hex(buf[i * 2 + 1]);
 827                 c = (n << 4) | n1;
 828
 829                 if (n == -1 || n1 == -1)
 830                 {
 831                     cvmx_debug_printf("M packet corrupt: %s\n", &buf[i * 2]);
 832                     goto error_packet;
 833                 }
 834                 /* Probe memory.  If not accessible fail.   */
 835                 if (!cvmx_debug_probe_store(&ptr[i]))
 836                 {
 837                     cvmx_debug_printf("M cannot write: %p\n", &ptr[i]);
 838                     goto error_packet;
 839                 }
 840                 ptr[i] = c;
 841             }
 842             cvmx_debug_putpacket_noformat("+");
 843         }
 844         break;
 845
 846         case 'e':  /* Set/get performance counter events. e[1234]XX..X: [01]
 847                       is the performance counter to set X is the performance
 848                       event.  [34] is to get the same thing.  */
 849         {
 850             uint64_t perf_event = 0;
 851             char encoded_counter = *buf++;
 852             uint64_t counter;
 853             volatile cvmx_debug_core_context_t *context = cvmx_debug_core_context();
 854
 855             /* Ignore errors from the packet. */
 856             cvmx_debug_hexToLong(&buf, &perf_event);
 857
 858             switch (encoded_counter)
 859             {
 860                 case '1': /* Set performance counter0 event. */
 861                 case '2': /* Set performance counter1 event. */
 862
 863                 counter = encoded_counter - '1';
 864                 context->cop0.perfval[counter] = 0;
 865                 cvmx_debug_set_perf_control_reg(context, perf_event, counter);
 866                 break;
 867
 868                 case '3': /* Get performance counter0 event. */
 869                 case '4': /* Get performance counter1 event. */
 870                 {
 871                     cvmx_core_perf_control_t c;
 872                     char outpacket[16*2 +2];
 873                     counter = encoded_counter - '3';
 874                     /* Pass performance counter0 event and counter to
 875                        the debugger.  */
 876                     c.u32 = context->cop0.perfctrl[counter];
 877                     cvmx_debug_int64_to_strhex(outpacket, context->cop0.perfval[counter]);
 878                     outpacket[16] = ',';
 879                     cvmx_debug_int64_to_strhex(&outpacket[17], c.s.event);
 880                     outpacket[33] = 0;
 881                     cvmx_debug_putpacket_noformat(outpacket);
 882                 }
 883                 break;
 884             }
 885         }
 886         break;
 887
 888 #if 0
 889         case 't': /* Return the trace buffer read data register contents. */
 890         {
 891             uint64_t tra_data;
 892             uint64_t tra_ctl;
 893             char tmp[64];
 894
 895             /* If trace buffer is disabled no trace data information is available. */
 896             if ((tra_ctl & 0x1) == 0)
 897             {
 898                 cvmx_debug_putpacket_noformat("!Trace buffer not enabled\n");
 899                 cvmx_debug_putpacket_noformat("t");
 900             }
 901             else
 902             {
 903                 cvmx_debug_putpacket_noformat("!Trace buffer is enabled\n");
 904                 tra_data = cvmx_read_csr(OCTEON_TRA_READ_DATA);
 905                 mem2hex (&tra_data, tmp, 8);
 906                 strcpy (debug_output_buffer, "t");
 907                 strcat (debug_output_buffer, tmp);
 908                 cvmx_debug_putpacket_noformat(debug_output_buffer);
 909             }
 910         }
 911         break;
 912 #endif
 913
 914         case 'Z': /* Insert hardware breakpoint: Z[di]NN..N,AA.A, [di] data or
 915                      instruction, NN..Nth at address AA..A */
 916         {
 917             enum type
 918             {
 919                 WP_LOAD = 1,
 920                 WP_STORE = 2,
 921                 WP_ACCESS = 3
 922             };
 923
 924             uint64_t num, size;
 925             uint64_t addr;
 926             uint64_t type;
 927             char bp_type = *buf++;
 928             const int BE = 1, TE = 4;
 929             volatile cvmx_debug_core_context_t *context = cvmx_debug_core_context();
 930
 931             if (!cvmx_debug_hexToLong(&buf, &num)
 932                 || *buf++ != ','
 933                 || !cvmx_debug_hexToLong(&buf, &addr))
 934             {
 935                 cvmx_debug_printf("Z packet corrupt: %s\n", &packet[1]);
 936                 goto error_packet;
 937             }
 938
 939             switch (bp_type)
 940             {
 941                 case 'i':       // Instruction hardware breakpoint
 942                     if (num > 4)
 943                     {
 944                         cvmx_debug_printf("Z packet corrupt: %s\n", &packet[1]);
 945                         goto error_packet;
 946                     }
 947
 948                     context->hw_ibp.address[num] = addr;
 949                     context->hw_ibp.address_mask[num] = 0;
 950                     context->hw_ibp.asid[num] = 0;
 951                     context->hw_ibp.control[num] = BE | TE;
 952                     break;
 953
 954                 case 'd':       // Data hardware breakpoint
 955                 {
 956                     uint64_t dbc = 0xff0 | BE | TE;
 957                     uint64_t dbm;
 958                     if (num > 4
 959                         || *buf++ != ','
 960                         || !cvmx_debug_hexToLong(&buf, &size)
 961                         || *buf++ != ','
 962                         || !cvmx_debug_hexToLong(&buf, &type)
 963                         || type > WP_ACCESS
 964                         || type < WP_LOAD)
 965                     {
 966                         cvmx_debug_printf("Z packet corrupt: %s\n", &packet[1]);
 967                         goto error_packet;
 968                     }
 969
 970                     /* Set DBC[BE,TE,BLM]. */
 971                     context->hw_dbp.address[num] = addr;
 972                     context->hw_dbp.asid[num] = 0;
 973
 974                     dbc |= type == WP_STORE ? 0x1000 : type == WP_LOAD ? 0x2000 : 0;
 975                     /* Mask the bits depending on the size for
 976                     debugger to stop while accessing parts of the
 977                     memory location.  */
 978                     dbm = (size == 8) ? 0x7 : ((size == 4) ? 3
 979                                         : (size == 2) ? 1 : 0);
 980                     context->hw_dbp.address_mask[num] = dbm;
 981                     context->hw_dbp.control[num] = dbc;
 982                     break;
 983                 }
 984                 default:
 985                     cvmx_debug_printf("Z packet corrupt: %s\n", &packet[1]);
 986                     goto error_packet;
 987             }
 988         }
 989         break;
 990
 991         case 'z': /* Remove hardware breakpoint: z[di]NN..N remove NN..Nth
 992 breakpoint.  */
 993         {
 994             uint64_t num;
 995             char bp_type = *buf++;
 996             volatile cvmx_debug_core_context_t *context = cvmx_debug_core_context();
 997
 998             if (!cvmx_debug_hexToLong(&buf, &num) || num > 4)
 999             {
1000                 cvmx_debug_printf("z packet corrupt: %s\n", buf);
1001                 goto error_packet;
1002             }
1003
1004             switch (bp_type)
1005             {
1006                 case 'i':       // Instruction hardware breakpoint
1007                     context->hw_ibp.address[num] = 0;
1008                     context->hw_ibp.address_mask[num] = 0;
1009                     context->hw_ibp.asid[num] = 0;
1010                     context->hw_ibp.control[num] = 0;
1011                     break;
1012                 case 'd':       // Data hardware breakpoint
1013                     context->hw_dbp.address[num] = 0;
1014                     context->hw_dbp.address_mask[num] = 0;
1015                     context->hw_dbp.asid[num] = 0;
1016                     context->hw_dbp.control[num] = 0;
1017                     break;
1018                 default:
1019                     cvmx_debug_printf("z packet corrupt: %s\n", buf);
1020                     goto error_packet;
1021             }
1022         }
1023         break;
1024
1025         case 's':   /* Single step. sAA..AA Step one instruction from AA..AA (optional) */
1026             result = COMMAND_STEP;
1027             break;
1028
1029         case 'c':   /* Continue. cAA..AA Continue at address AA..AA (optional) */
1030             result = COMMAND_CONTINUE;
1031             break;
1032
1033         case '+':   /* Don't know. I think it is a communications sync */
1034             /* Ignoring this command */
1035             break;
1036
1037         default:
1038             cvmx_debug_printf("Unknown debug command: %s\n", buf - 1);
1039 error_packet:
1040             cvmx_debug_putpacket_noformat("-");
1041             break;
1042     }
1043
1044     return result;
1045 }
1046
1047 static cvmx_debug_command_t cvmx_debug_process_next_packet(void)
1048 {
1049     char packet[CVMX_DEBUG_MAX_REQUEST_SIZE];
1050     if (cvmx_debug_comms[cvmx_debug_globals->comm_type]->getpacket(packet, CVMX_DEBUG_MAX_REQUEST_SIZE))
1051     {
1052         cvmx_debug_printf("Request: %s\n", packet);
1053         return cvmx_debug_process_packet(packet);
1054     }
1055     return COMMAND_NOP;
1056 }
1057
1058 /* If a core isn't in the active core mask we need to start him up again. We
1059    can only do this if the core didn't hit a breakpoint or single step. If the
1060    core hit CVMX_CIU_DINT interrupt (generally happens when while executing
1061    _exit() at the end of the program). Remove the core from known cores so
1062    that when the cores in active core mask are done executing the program, the
1063    focus will not be transfered to this core.  */
1064
1065 static int cvmx_debug_stop_core(cvmx_debug_state_t state, unsigned core, cvmx_debug_register_t *debug_reg, int proxy)
1066 {
1067     if (!cvmx_debug_active_core(state, core) && !debug_reg->s.dbp && !debug_reg->s.dss && (debug_reg->s.dint != 1))
1068     {
1069         debug_reg->s.sst = 0;
1070         cvmx_debug_printf("Core #%d not in active cores, continuing.\n", core);
1071         return 0;
1072     }
1073     if ((state.core_finished & (1u<<core)) && proxy)
1074       return 0;
1075     return 1;
1076 }
1077
1078 /* check to see if current exc is single-stepped and  that no other exc
1079    was also simultaneously noticed. */
1080 static int cvmx_debug_single_step_exc(cvmx_debug_register_t *debug_reg)
1081 {
1082     if (debug_reg->s.dss && !debug_reg->s.dib && !debug_reg->s.dbp && !debug_reg->s.ddbs && !debug_reg->s.ddbl)
1083         return 1;
1084     return 0;
1085 }
1086
1087 static void cvmx_debug_set_focus_core(cvmx_debug_state_t *state, int core)
1088 {
1089     if (state->ever_been_in_debug)
1090         cvmx_debug_putcorepacket("taking focus.", core);
1091     cvmx_debug_comms[cvmx_debug_globals->comm_type]->change_core (state->focus_core, core);
1092     state->focus_core = core;
1093 }
1094
1095 static void cvmx_debug_may_elect_as_focus_core(cvmx_debug_state_t *state, int core, cvmx_debug_register_t *debug_reg)
1096 {
1097     /* If another core has already elected itself as the focus core, we're late.  */
1098     if (state->handler_cores & (1u << state->focus_core))
1099         return;
1100
1101     /* If we hit a breakpoint, elect ourselves.  */
1102     if (debug_reg->s.dib || debug_reg->s.dbp || debug_reg->s.ddbs || debug_reg->s.ddbl)
1103         cvmx_debug_set_focus_core(state, core);
1104
1105     /* It is possible the focus core has completed processing and exited the
1106        program. When this happens the focus core will not be in
1107        known_cores. If this is the case we need to elect a new focus. */
1108     if ((state->known_cores & (1u << state->focus_core)) == 0)
1109         cvmx_debug_set_focus_core(state, core);
1110 }
1111
1112 static void cvmx_debug_send_stop_reason(cvmx_debug_register_t *debug_reg, volatile cvmx_debug_core_context_t *context)
1113 {
1114     /* Handle Debug Data Breakpoint Store/Load Exception. */
1115     if (debug_reg->s.ddbs || debug_reg->s.ddbl)
1116         cvmx_debug_putpacket_hexint("T8:", (int) context->hw_dbp.status);
1117     else
1118         cvmx_debug_putpacket_noformat("T9");
1119 }
1120
1121
1122 static void cvmx_debug_clear_status(volatile cvmx_debug_core_context_t *context)
1123 {
1124     /* SW needs to clear the BreakStatus bits after a watchpoint is hit or on
1125        reset.  */
1126     context->hw_dbp.status &= ~0x3fff;
1127
1128     /* Clear MCD0, which is write-1-to-clear.  */
1129     context->cop0.multicoredebug |= 1;
1130 }
1131
1132 static void cvmx_debug_sync_up_cores(void)
1133 {
1134     /* NOTE this reads directly from the state array for speed reasons
1135        and we don't change the array. */
1136     do {
1137       asm("": : : "memory");
1138     } while (cvmx_debug_globals->state[offsetof(cvmx_debug_state_t, step_all)/sizeof(uint32_t)]
1139              && cvmx_debug_globals->state[offsetof(cvmx_debug_state_t, handler_cores)/sizeof(uint32_t)] != 0);
1140 }
1141
1142 /* Delay the focus core a little if it is likely another core needs to steal
1143    focus. Once we enter the main loop focus can't be stolen */
1144 static void cvmx_debug_delay_focus_core(cvmx_debug_state_t state, unsigned core, cvmx_debug_register_t *debug_reg)
1145 {
1146     volatile int i;
1147     if (debug_reg->s.dss || debug_reg->s.dbp || core != state.focus_core)
1148         return;
1149     for (i = 0; i < 2400; i++)
1150     {
1151         asm volatile (".set push                \n\t"
1152                       ".set noreorder           \n\t"
1153                       "nop                      \n\t"
1154                       "nop                      \n\t"
1155                       "nop                      \n\t"
1156                       "nop                      \n\t"
1157                       ".set pop");
1158         /* Spin giving the breakpoint core time to steal focus */
1159     }
1160
1161 }
1162
1163 /* If this core was single-stepping in a group,
1164    && it was not the last focus-core,
1165    && last focus-core happens to be inside an ISR, blocking focus-switch
1166    then burn some cycles, to avoid unnecessary focus toggles. */
1167 static void cvmx_debug_delay_isr_core(unsigned core, uint32_t depc, int single_stepped_exc_only,
1168                                       cvmx_debug_state_t state)
1169 {
1170     volatile uint64_t i;
1171     if(!single_stepped_exc_only || state.step_isr || core == state.focus_core || state.focus_switch)
1172         return;
1173
1174     cvmx_debug_printf ("Core #%u spinning for focus at 0x%x\n", core, (unsigned int)depc);
1175
1176     for(i = ISR_DELAY_COUNTER; i > 0 ; i--)
1177     {
1178        state = cvmx_debug_get_state();
1179        /* Spin giving the focus core time to service ISR */
1180        /* But cut short the loop, if we can.  Shrink down i, only once. */
1181        if (i > 600000 && state.focus_switch)
1182            i = 500000;
1183     }
1184
1185 }
1186
1187 static int cvmx_debug_perform_proxy(cvmx_debug_register_t *debug_reg, volatile cvmx_debug_core_context_t *context)
1188 {
1189     unsigned core = cvmx_get_core_num();
1190     cvmx_debug_state_t state = cvmx_debug_get_state();
1191     cvmx_debug_command_t command = COMMAND_NOP;
1192     int single_stepped_exc_only = cvmx_debug_single_step_exc (debug_reg);
1193
1194     /* All cores should respect the focus core if it has to
1195        stop focus switching while servicing an interrupt.
1196        If the system is single-stepping, then the following
1197        code path is valid. If the current core tripped on a
1198        break-point or some other error while going through
1199        an ISR, then we shouldn't be returning unconditionally.
1200        In that case (non-single-step case) we must enter
1201        the debugger exception stub fully. */
1202     if (!state.step_isr && (cvmx_interrupt_in_isr || (context->cop0.status & 0x2ULL)) && single_stepped_exc_only)
1203     {
1204         cvmx_spinlock_lock(&cvmx_debug_globals->lock);
1205         state = cvmx_debug_get_state();
1206         /* If this is the focus core, switch off focus switching
1207            till ISR_DELAY_COUNTER. This will let focus core
1208            keep the focus until the ISR is completed. */
1209         if(state.focus_switch && core == state.focus_core)
1210         {
1211             cvmx_debug_printf ("Core #%u stopped focus stealing at 0x%llx\n", core, (unsigned long long)context->cop0.depc);
1212             state.focus_switch = 0;
1213         }
1214         /* Alow other cores to steal focus.
1215            Focus core has completed ISR. */
1216         if (*(uint32_t*)((__SIZE_TYPE__)context->cop0.depc) == ERET_INSN && core == state.focus_core)
1217         {
1218             cvmx_debug_printf ("Core #%u resumed focus stealing at 0x%llx\n", core, (unsigned long long)context->cop0.depc);
1219             state.focus_switch = 1;
1220         }
1221         cvmx_debug_update_state(state);
1222         cvmx_spinlock_unlock(&cvmx_debug_globals->lock);
1223         cvmx_debug_printf ("Core #%u resumed skipping isr.\n", core);
1224         return 0;
1225     }
1226
1227     /* Delay the focus core a little if it is likely another core needs to
1228        steal focus. Once we enter the main loop focus can't be stolen */
1229     cvmx_debug_delay_focus_core(state, core, debug_reg);
1230
1231     cvmx_debug_delay_isr_core (core, context->cop0.depc, single_stepped_exc_only, state);
1232
1233     /* The following section of code does two critical things. First, it
1234        populates the handler_cores bitmask of all cores in the exception
1235        handler. Only one core at a time can update this field. Second it
1236        changes the focus core if needed. */
1237     {
1238         cvmx_debug_printf("Core #%d stopped\n", core);
1239         cvmx_spinlock_lock(&cvmx_debug_globals->lock);
1240         state = cvmx_debug_get_state();
1241
1242         state.handler_cores |= (1u << core);
1243         cvmx_debug_may_elect_as_focus_core(&state, core, debug_reg);
1244
1245 /* Push all updates before exiting the critical section */
1246         state.focus_switch = 1;
1247         cvmx_debug_update_state(state);
1248         cvmx_spinlock_unlock(&cvmx_debug_globals->lock);
1249     }
1250     if (__cvmx_debug_in_focus(state, core))
1251         cvmx_debug_send_stop_reason(debug_reg, context);
1252
1253     do {
1254         unsigned oldfocus = state.focus_core;
1255         state = cvmx_debug_get_state();
1256         /* Note the focus core can change in this loop. */
1257         if (__cvmx_debug_in_focus(state, core))
1258         {
1259             /* If the focus has changed and the old focus has exited, then send a signal
1260                that we should stop if step_all is off.  */
1261             if (oldfocus != state.focus_core && ((1u << oldfocus) & state.core_finished)
1262                 && !state.step_all)
1263               cvmx_debug_send_stop_reason(debug_reg, context);
1264
1265             command = cvmx_debug_process_next_packet();
1266             state = cvmx_debug_get_state();
1267             /* When resuming let the other cores resume as well with
1268                step-all.  */
1269             if (command != COMMAND_NOP && state.step_all)
1270             {
1271                 state.command = command;
1272                 cvmx_debug_update_state(state);
1273             }
1274         }
1275         /* When steping all cores, update the non focus core's command too. */
1276         else if (state.step_all)
1277             command = state.command;
1278
1279         /* If we did not get a command and the communication changed return,
1280            we are changing the communications. */
1281         if (command == COMMAND_NOP && cvmx_debug_globals->comm_changed)
1282         {
1283             /* FIXME, this should a sync not based on cvmx_coremask_barrier_sync.  */
1284 #ifndef CVMX_BUILD_FOR_LINUX_KERNEL
1285             /* Sync up.  */
1286             cvmx_coremask_barrier_sync(state.handler_cores);
1287 #endif
1288             return 1;
1289         }
1290     } while (command == COMMAND_NOP);
1291
1292     debug_reg->s.sst = command == COMMAND_STEP;
1293     cvmx_debug_printf("Core #%d running\n", core);
1294
1295     {
1296         cvmx_spinlock_lock(&cvmx_debug_globals->lock);
1297         state = cvmx_debug_get_state();
1298         state.handler_cores ^= (1u << core);
1299         cvmx_debug_update_state(state);
1300         cvmx_spinlock_unlock(&cvmx_debug_globals->lock);
1301     }
1302
1303     cvmx_debug_sync_up_cores();
1304     /* Now that all cores are out, reset the command.  */
1305     if (__cvmx_debug_in_focus(state, core))
1306     {
1307         cvmx_spinlock_lock(&cvmx_debug_globals->lock);
1308         state = cvmx_debug_get_state();
1309         state.command = COMMAND_NOP;
1310         cvmx_debug_update_state(state);
1311         cvmx_spinlock_unlock(&cvmx_debug_globals->lock);
1312     }
1313     return 0;
1314 }
1315
1316 static void cvmx_debug_save_core_context(volatile cvmx_debug_core_context_t *context, uint64_t hi, uint64_t lo)
1317 {
1318     unsigned i;
1319     cvmx_debug_memcpy_align ((char *) context->regs, __cvmx_debug_save_regs_area, sizeof(context->regs));
1320     context->lo = lo;
1321     context->hi = hi;
1322     CVMX_MF_COP0(context->cop0.index, COP0_INDEX);
1323     CVMX_MF_COP0(context->cop0.entrylo[0], COP0_ENTRYLO0);
1324     CVMX_MF_COP0(context->cop0.entrylo[1], COP0_ENTRYLO1);
1325     CVMX_MF_COP0(context->cop0.entryhi, COP0_ENTRYHI);
1326     CVMX_MF_COP0(context->cop0.pagemask, COP0_PAGEMASK);
1327     CVMX_MF_COP0(context->cop0.status, COP0_STATUS);
1328     CVMX_MF_COP0(context->cop0.cause, COP0_CAUSE);
1329     CVMX_MF_COP0(context->cop0.debug, COP0_DEBUG);
1330     CVMX_MF_COP0(context->cop0.multicoredebug, COP0_MULTICOREDEBUG);
1331     CVMX_MF_COP0(context->cop0.perfval[0], COP0_PERFVALUE0);
1332     CVMX_MF_COP0(context->cop0.perfval[1], COP0_PERFVALUE1);
1333     CVMX_MF_COP0(context->cop0.perfctrl[0], COP0_PERFCONTROL0);
1334     CVMX_MF_COP0(context->cop0.perfctrl[1], COP0_PERFCONTROL1);
1335     /* Save DEPC and DESAVE since debug-mode exceptions (see
1336        debug_probe_{load,store}) can clobber these.  */
1337     CVMX_MF_COP0(context->cop0.depc, COP0_DEPC);
1338     CVMX_MF_COP0(context->cop0.desave, COP0_DESAVE);
1339
1340     context->hw_ibp.status = cvmx_read_csr(CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_STATUS);
1341     for (i = 0; i < 4; i++)
1342     {
1343         context->hw_ibp.address[i] = cvmx_read_csr(CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_ADDRESS(i));
1344         context->hw_ibp.address_mask[i] = cvmx_read_csr(CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_ADDRESS_MASK(i));
1345         context->hw_ibp.asid[i] = cvmx_read_csr(CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_ASID(i));
1346         context->hw_ibp.control[i] = cvmx_read_csr(CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_CONTROL(i));
1347     }
1348
1349     context->hw_dbp.status = cvmx_read_csr(CVMX_DEBUG_HW_DATA_BREAKPOINT_STATUS);
1350     for (i = 0; i < 4; i++)
1351     {
1352         context->hw_dbp.address[i] = cvmx_read_csr(CVMX_DEBUG_HW_DATA_BREAKPOINT_ADDRESS(i));
1353         context->hw_dbp.address_mask[i] = cvmx_read_csr(CVMX_DEBUG_HW_DATA_BREAKPOINT_ADDRESS_MASK(i));
1354         context->hw_dbp.asid[i] = cvmx_read_csr(CVMX_DEBUG_HW_DATA_BREAKPOINT_ASID(i));
1355         context->hw_dbp.control[i] = cvmx_read_csr(CVMX_DEBUG_HW_DATA_BREAKPOINT_CONTROL(i));
1356     }
1357
1358     for (i = 0; i < cvmx_debug_globals->tlb_entries; i++)
1359     {
1360         CVMX_MT_COP0(i, COP0_INDEX);
1361         asm volatile ("tlbr");
1362         CVMX_MF_COP0(context->tlbs[i].entrylo[0], COP0_ENTRYLO0);
1363         CVMX_MF_COP0(context->tlbs[i].entrylo[1], COP0_ENTRYLO1);
1364         CVMX_MF_COP0(context->tlbs[i].entryhi, COP0_ENTRYHI);
1365         CVMX_MF_COP0(context->tlbs[i].pagemask, COP0_PAGEMASK);
1366     }
1367     CVMX_SYNCW;
1368 }
1369
1370 static void cvmx_debug_restore_core_context(volatile cvmx_debug_core_context_t *context)
1371 {
1372     uint64_t hi, lo;
1373     int i;
1374     cvmx_debug_memcpy_align (__cvmx_debug_save_regs_area, (char *) context->regs, sizeof(context->regs));
1375     /* We don't change the TLB so no need to restore it.  */
1376     cvmx_write_csr(CVMX_DEBUG_HW_DATA_BREAKPOINT_STATUS, context->hw_dbp.status);
1377     for (i = 0; i < 4; i++)
1378     {
1379         cvmx_write_csr(CVMX_DEBUG_HW_DATA_BREAKPOINT_ADDRESS(i), context->hw_dbp.address[i]);
1380         cvmx_write_csr(CVMX_DEBUG_HW_DATA_BREAKPOINT_ADDRESS_MASK(i), context->hw_dbp.address_mask[i]);
1381         cvmx_write_csr(CVMX_DEBUG_HW_DATA_BREAKPOINT_ASID(i), context->hw_dbp.asid[i]);
1382         cvmx_write_csr(CVMX_DEBUG_HW_DATA_BREAKPOINT_CONTROL(i), context->hw_dbp.control[i]);
1383     }
1384     cvmx_write_csr(CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_STATUS, context->hw_ibp.status);
1385     for (i = 0; i < 4; i++)
1386     {
1387         cvmx_write_csr(CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_ADDRESS(i), context->hw_ibp.address[i]);
1388         cvmx_write_csr(CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_ADDRESS_MASK(i), context->hw_ibp.address_mask[i]);
1389         cvmx_write_csr(CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_ASID(i), context->hw_ibp.asid[i]);
1390         cvmx_write_csr(CVMX_DEBUG_HW_INSTRUCTION_BREAKPOINT_CONTROL(i), context->hw_ibp.control[i]);
1391     }
1392     CVMX_MT_COP0(context->cop0.index, COP0_INDEX);
1393     CVMX_MT_COP0(context->cop0.entrylo[0], COP0_ENTRYLO0);
1394     CVMX_MT_COP0(context->cop0.entrylo[1], COP0_ENTRYLO1);
1395     CVMX_MT_COP0(context->cop0.entryhi, COP0_ENTRYHI);
1396     CVMX_MT_COP0(context->cop0.pagemask, COP0_PAGEMASK);
1397     CVMX_MT_COP0(context->cop0.status, COP0_STATUS);
1398     CVMX_MT_COP0(context->cop0.cause, COP0_CAUSE);
1399     CVMX_MT_COP0(context->cop0.debug, COP0_DEBUG);
1400     CVMX_MT_COP0(context->cop0.multicoredebug, COP0_MULTICOREDEBUG);
1401     CVMX_MT_COP0(context->cop0.perfval[0], COP0_PERFVALUE0);
1402     CVMX_MT_COP0(context->cop0.perfval[1], COP0_PERFVALUE1);
1403     CVMX_MT_COP0(context->cop0.perfctrl[0], COP0_PERFCONTROL0);
1404     CVMX_MT_COP0(context->cop0.perfctrl[1], COP0_PERFCONTROL1);
1405     CVMX_MT_COP0(context->cop0.depc, COP0_DEPC);
1406     CVMX_MT_COP0(context->cop0.desave, COP0_DESAVE);
1407     lo = context->lo;
1408     hi = context->hi;
1409     asm("mtlo %0" :: "r"(lo));
1410     asm("mthi %0" :: "r"(hi));
1411 }
1412
1413 static inline void cvmx_debug_print_cause(volatile cvmx_debug_core_context_t *context)
1414 {
1415     if (!CVMX_DEBUG_LOGGING)
1416         return;
1417     if (context->cop0.multicoredebug & 1)
1418         cvmx_dprintf("MCD0 was pulsed\n");
1419     if (context->cop0.multicoredebug & (1 << 16))
1420         cvmx_dprintf("Exception %lld in Debug Mode\n", (long long)((context->cop0.debug >> 10) & 0x1f));
1421     if (context->cop0.debug & (1 << 19))
1422         cvmx_dprintf("DDBSImpr\n");
1423     if (context->cop0.debug & (1 << 18))
1424         cvmx_dprintf("DDBLImpr\n");
1425     if (context->cop0.debug & (1 << 5))
1426         cvmx_dprintf("DINT\n");
1427     if (context->cop0.debug & (1 << 4))
1428         cvmx_dprintf("Debug Instruction Breakpoint (DIB) exception\n");
1429     if (context->cop0.debug & (1 << 3))
1430         cvmx_dprintf("Debug Date Break Store (DDBS) exception\n");
1431     if (context->cop0.debug & (1 << 2))
1432         cvmx_dprintf("Debug Date Break Load (DDBL) exception\n");
1433     if (context->cop0.debug & (1 << 1))
1434         cvmx_dprintf("Debug Breakpoint (DBp) exception\n");
1435     if (context->cop0.debug & (1 << 0))
1436         cvmx_dprintf("Debug Single Step (DSS) exception\n");
1437 }
1438
1439 void __cvmx_debug_handler_stage3 (uint64_t lo, uint64_t hi)
1440 {
1441     volatile cvmx_debug_core_context_t *context;
1442     int comms_changed = 0;
1443
1444     cvmx_debug_printf("Entering debug exception handler\n");
1445     cvmx_debug_printf("Debug named block at %p\n", cvmx_debug_globals);
1446     if (__cvmx_debug_mode_exception_occured)
1447     {
1448         uint64_t depc;
1449         CVMX_MF_COP0(depc, COP0_DEPC);
1450         cvmx_dprintf("Unexpected debug-mode exception occured at 0x%llx, 0x%llx spinning\n", (long long) depc, (long long)(__cvmx_debug_mode_exception_occured));
1451 #ifdef CVMX_BUILD_FOR_LINUX_KERNEL
1452         panic("Unexpected debug-mode exception occured at 0x%llx, 0x%llx\n", (long long) depc, (long long)(__cvmx_debug_mode_exception_occured));
1453 #endif
1454         while (1)
1455             ;
1456     }
1457
1458     context = cvmx_debug_core_context();
1459     cvmx_debug_save_core_context(context, hi, lo);
1460
1461     {
1462         cvmx_debug_state_t state;
1463         cvmx_spinlock_lock(&cvmx_debug_globals->lock);
1464         state = cvmx_debug_get_state();
1465         state.ever_been_in_debug = 1;
1466         cvmx_debug_update_state (state);
1467         cvmx_spinlock_unlock(&cvmx_debug_globals->lock);
1468     }
1469     cvmx_debug_print_cause(context);
1470
1471     do
1472     {
1473         int needs_proxy;
1474         comms_changed = 0;
1475         /* If the communication changes, change it. */
1476         cvmx_spinlock_lock(&cvmx_debug_globals->lock);
1477         if (cvmx_debug_globals->comm_changed)
1478         {
1479             cvmx_debug_printf("Communication changed: %d\n", (int)cvmx_debug_globals->comm_changed);
1480             if (cvmx_debug_globals->comm_changed > COMM_SIZE)
1481             {
1482                 cvmx_dprintf("Unknown communication spinning: %lld > %d.\n", (long long)cvmx_debug_globals->comm_changed, (int)(COMM_SIZE));
1483 #ifdef CVMX_BUILD_FOR_LINUX_KERNEL
1484                 panic("Unknown communication.\n");
1485 #endif
1486                 while (1)
1487                     ;
1488             }
1489             cvmx_debug_globals->comm_type = cvmx_debug_globals->comm_changed - 1;
1490             cvmx_debug_globals->comm_changed = 0;
1491         }
1492         cvmx_spinlock_unlock(&cvmx_debug_globals->lock);
1493         needs_proxy = cvmx_debug_comms[cvmx_debug_globals->comm_type]->needs_proxy;
1494
1495         {
1496             cvmx_debug_register_t debug_reg;
1497             cvmx_debug_state_t state;
1498             unsigned core = cvmx_get_core_num();
1499
1500             state = cvmx_debug_get_state();
1501             debug_reg.u64 = context->cop0.debug;
1502             /* All cores stop on any exception.  See if we want nothing from this and
1503                it should resume.  This needs to be done for non proxy based debugging
1504                so that some non active-cores can control the other cores.  */
1505             if (!cvmx_debug_stop_core(state, core, &debug_reg, needs_proxy))
1506             {
1507                 context->cop0.debug = debug_reg.u64;
1508                 break;
1509             }
1510         }
1511
1512         if (needs_proxy)
1513         {
1514             cvmx_debug_register_t debug_reg;
1515             debug_reg.u64 = context->cop0.debug;
1516             cvmx_debug_printf("Starting to proxy\n");
1517             comms_changed = cvmx_debug_perform_proxy(&debug_reg, context);
1518             context->cop0.debug = debug_reg.u64;
1519         }
1520         else
1521         {
1522             cvmx_debug_printf("Starting to wait for remote host\n");
1523             cvmx_debug_comms[cvmx_debug_globals->comm_type]->wait_for_resume(context, cvmx_debug_get_state());
1524         }
1525     } while (comms_changed);
1526
1527     cvmx_debug_clear_status(context);
1528
1529     cvmx_debug_restore_core_context(context);
1530     cvmx_debug_printf("Exiting debug exception handler\n");
1531 }
1532
1533 void cvmx_debug_trigger_exception(void)
1534 {
1535   /* Set CVMX_CIU_DINT to enter debug exception handler.  */
1536   cvmx_write_csr (CVMX_CIU_DINT, 1u << cvmx_get_core_num ());
1537   /* Perform an immediate read after every write to an RSL register to force
1538      the write to complete. It doesn't matter what RSL read we do, so we
1539      choose CVMX_MIO_BOOT_BIST_STAT because it is fast and harmless */
1540   cvmx_read_csr (CVMX_MIO_BOOT_BIST_STAT);
1541 }
1542
1543 /**
1544  * Inform debugger about the end of the program. This is
1545  * called from crt0 after all the C cleanup code finishes.
1546  * Our current stack is the C one, not the debug exception
1547  * stack. */
1548 void cvmx_debug_finish(void)
1549 {
1550     unsigned coreid = cvmx_get_core_num();
1551     cvmx_debug_state_t state;
1552
1553     if (!cvmx_debug_globals) return;
1554     cvmx_debug_printf ("Debug _exit reached!, core %d, cvmx_debug_globals = %p\n", coreid, cvmx_debug_globals);
1555
1556 #ifndef CVMX_BUILD_FOR_LINUX_KERNEL
1557     fflush (stdout);
1558     fflush (stderr);
1559 #endif
1560
1561     cvmx_spinlock_lock(&cvmx_debug_globals->lock);
1562     state = cvmx_debug_get_state();
1563     state.known_cores ^= (1u << coreid);
1564     state.core_finished |= (1u <<coreid);
1565     cvmx_debug_update_state(state);
1566
1567     /* Tell the user the core has finished. */
1568     if (state.ever_been_in_debug)
1569         cvmx_debug_putcorepacket("finished.", coreid);
1570
1571     /* Notify the debugger if all cores have completed the program */
1572     if ((cvmx_debug_core_mask () & state.core_finished) == cvmx_debug_core_mask ())
1573     {
1574         cvmx_debug_printf("All cores done!\n");
1575         if (state.ever_been_in_debug)
1576             cvmx_debug_putpacket_noformat("D0");
1577     }
1578     if (state.focus_core == coreid && state.known_cores != 0)
1579     {
1580         /* Loop through cores looking for someone to handle interrupts.
1581            Since we already check that known_cores is non zero, this
1582            should always find a core */
1583         unsigned newcore;
1584         for (newcore = 0; newcore < CVMX_MAX_CORES; newcore++)
1585         {
1586            if (state.known_cores & (1u<<newcore))
1587            {
1588                cvmx_debug_printf("Routing uart interrupts to Core #%u.\n", newcore);
1589                cvmx_debug_set_focus_core(&state, newcore);
1590                cvmx_debug_update_state(state);
1591                break;
1592             }
1593         }
1594     }
1595     cvmx_spinlock_unlock(&cvmx_debug_globals->lock);
1596
1597     /* If we ever been in the debug, report to it that we have exited the core. */
1598     if (state.ever_been_in_debug)
1599         cvmx_debug_trigger_exception();
1600 }