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

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