arch/arm/common/fiq_debugger.c

   1 /*
   2  * arch/arm/common/fiq_debugger.c
   3  *
   4  * Serial Debugger Interface accessed through an FIQ interrupt.
   5  *
   6  * Copyright (C) 2008 Google, Inc.
   7  *
   8  * This software is licensed under the terms of the GNU General Public
   9  * License version 2, as published by the Free Software Foundation, and
  10  * may be copied, distributed, and modified under those terms.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  */
  17
  18 #include <stdarg.h>
  19 #include <linux/module.h>
  20 #include <linux/io.h>
  21 #include <linux/console.h>
  22 #include <linux/interrupt.h>
  23 #include <linux/clk.h>
  24 #include <linux/platform_device.h>
  25 #include <linux/kernel_debugger.h>
  26 #include <linux/kernel_stat.h>
  27 #include <linux/irq.h>
  28 #include <linux/delay.h>
  29 #include <linux/sched.h>
  30 #include <linux/slab.h>
  31 #include <linux/timer.h>
  32 #include <linux/tty.h>
  33 #include <linux/tty_flip.h>
  34 #include <linux/wakelock.h>
  35
  36 #include <asm/fiq_debugger.h>
  37 #include <asm/fiq_glue.h>
  38 #include <asm/stacktrace.h>
  39
  40 #include <mach/system.h>
  41
  42 #include <linux/uaccess.h>
  43
  44 #include "fiq_debugger_ringbuf.h"
  45
  46 #define DEBUG_MAX 64
  47 #define MAX_UNHANDLED_FIQ_COUNT 1000000
  48
  49 #define THREAD_INFO(sp) ((struct thread_info *) \
  50                 ((unsigned long)(sp) & ~(THREAD_SIZE - 1)))
  51
  52 struct fiq_debugger_state {
  53         struct fiq_glue_handler handler;
  54
  55         int fiq;
  56         int signal_irq;
  57         int wakeup_irq;
  58         bool wakeup_irq_no_set_wake;
  59         struct clk *clk;
  60         struct fiq_debugger_pdata *pdata;
  61         struct platform_device *pdev;
  62
  63         char debug_cmd[DEBUG_MAX];
  64         int debug_busy;
  65         int debug_abort;
  66
  67         char debug_buf[DEBUG_MAX];
  68         int debug_count;
  69
  70         bool no_sleep;
  71         bool debug_enable;
  72         bool ignore_next_wakeup_irq;
  73         struct timer_list sleep_timer;
  74         bool uart_clk_enabled;
  75         struct wake_lock debugger_wake_lock;
  76         bool console_enable;
  77         int current_cpu;
  78         atomic_t unhandled_fiq_count;
  79         bool in_fiq;
  80
  81 #ifdef CONFIG_FIQ_DEBUGGER_CONSOLE
  82         struct console console;
  83         struct tty_driver *tty_driver;
  84         struct tty_struct *tty;
  85         int tty_open_count;
  86         struct fiq_debugger_ringbuf *tty_rbuf;
  87 #endif
  88
  89         unsigned int last_irqs[NR_IRQS];
  90         unsigned int last_local_timer_irqs[NR_CPUS];
  91 };
  92
  93 #ifdef CONFIG_FIQ_DEBUGGER_NO_SLEEP
  94 static bool initial_no_sleep = true;
  95 #else
  96 static bool initial_no_sleep;
  97 #endif
  98
  99 #ifdef CONFIG_FIQ_DEBUGGER_CONSOLE_DEFAULT_ENABLE
 100 static bool initial_debug_enable = true;
 101 static bool initial_console_enable = true;
 102 #else
 103 static bool initial_debug_enable;
 104 static bool initial_console_enable;
 105 #endif
 106
 107 module_param_named(no_sleep, initial_no_sleep, bool, 0644);
 108 module_param_named(debug_enable, initial_debug_enable, bool, 0644);
 109 module_param_named(console_enable, initial_console_enable, bool, 0644);
 110
 111 #ifdef CONFIG_FIQ_DEBUGGER_WAKEUP_IRQ_ALWAYS_ON
 112 static inline void enable_wakeup_irq(struct fiq_debugger_state *state) {}
 113 static inline void disable_wakeup_irq(struct fiq_debugger_state *state) {}
 114 #else
 115 static inline void enable_wakeup_irq(struct fiq_debugger_state *state)
 116 {
 117         if (state->wakeup_irq < 0)
 118                 return;
 119         enable_irq(state->wakeup_irq);
 120         if (!state->wakeup_irq_no_set_wake)
 121                 enable_irq_wake(state->wakeup_irq);
 122 }
 123 static inline void disable_wakeup_irq(struct fiq_debugger_state *state)
 124 {
 125         if (state->wakeup_irq < 0)
 126                 return;
 127         disable_irq_nosync(state->wakeup_irq);
 128         if (!state->wakeup_irq_no_set_wake)
 129                 disable_irq_wake(state->wakeup_irq);
 130 }
 131 #endif
 132
 133 static void debug_force_irq(struct fiq_debugger_state *state)
 134 {
 135         unsigned int irq = state->signal_irq;
 136         if (state->pdata->force_irq)
 137                 state->pdata->force_irq(state->pdev, irq);
 138         else {
 139                 struct irq_chip *chip = get_irq_chip(irq);
 140                 if (chip && chip->retrigger)
 141                         chip->retrigger(irq);
 142         }
 143 }
 144
 145 static void debug_uart_flush(struct fiq_debugger_state *state)
 146 {
 147         if (state->pdata->uart_flush)
 148                 state->pdata->uart_flush(state->pdev);
 149 }
 150
 151 static void debug_puts(struct fiq_debugger_state *state, char *s)
 152 {
 153         unsigned c;
 154         while ((c = *s++)) {
 155                 if (c == '\n')
 156                         state->pdata->uart_putc(state->pdev, '\r');
 157                 state->pdata->uart_putc(state->pdev, c);
 158         }
 159 }
 160
 161 static void debug_prompt(struct fiq_debugger_state *state)
 162 {
 163         debug_puts(state, "debug> ");
 164 }
 165
 166 int log_buf_copy(char *dest, int idx, int len);
 167 static void dump_kernel_log(struct fiq_debugger_state *state)
 168 {
 169         char buf[1024];
 170         int idx = 0;
 171         int ret;
 172         int saved_oip;
 173
 174         /* setting oops_in_progress prevents log_buf_copy()
 175          * from trying to take a spinlock which will make it
 176          * very unhappy in some cases...
 177          */
 178         saved_oip = oops_in_progress;
 179         oops_in_progress = 1;
 180         for (;;) {
 181                 ret = log_buf_copy(buf, idx, 1023);
 182                 if (ret <= 0)
 183                         break;
 184                 buf[ret] = 0;
 185                 debug_puts(state, buf);
 186                 idx += ret;
 187         }
 188         oops_in_progress = saved_oip;
 189 }
 190
 191 static char *mode_name(unsigned cpsr)
 192 {
 193         switch (cpsr & MODE_MASK) {
 194         case USR_MODE: return "USR";
 195         case FIQ_MODE: return "FIQ";
 196         case IRQ_MODE: return "IRQ";
 197         case SVC_MODE: return "SVC";
 198         case ABT_MODE: return "ABT";
 199         case UND_MODE: return "UND";
 200         case SYSTEM_MODE: return "SYS";
 201         default: return "???";
 202         }
 203 }
 204
 205 static int debug_printf(void *cookie, const char *fmt, ...)
 206 {
 207         struct fiq_debugger_state *state = cookie;
 208         char buf[256];
 209         va_list ap;
 210
 211         va_start(ap, fmt);
 212         vsnprintf(buf, sizeof(buf), fmt, ap);
 213         va_end(ap);
 214
 215         debug_puts(state, buf);
 216         return state->debug_abort;
 217 }
 218
 219 /* Safe outside fiq context */
 220 static int debug_printf_nfiq(void *cookie, const char *fmt, ...)
 221 {
 222         struct fiq_debugger_state *state = cookie;
 223         char buf[256];
 224         va_list ap;
 225         unsigned long irq_flags;
 226
 227         va_start(ap, fmt);
 228         vsnprintf(buf, 128, fmt, ap);
 229         va_end(ap);
 230
 231         local_irq_save(irq_flags);
 232         debug_puts(state, buf);
 233         debug_uart_flush(state);
 234         local_irq_restore(irq_flags);
 235         return state->debug_abort;
 236 }
 237
 238 static void dump_regs(struct fiq_debugger_state *state, unsigned *regs)
 239 {
 240         debug_printf(state, " r0 %08x  r1 %08x  r2 %08x  r3 %08x\n",
 241                         regs[0], regs[1], regs[2], regs[3]);
 242         debug_printf(state, " r4 %08x  r5 %08x  r6 %08x  r7 %08x\n",
 243                         regs[4], regs[5], regs[6], regs[7]);
 244         debug_printf(state, " r8 %08x  r9 %08x r10 %08x r11 %08x  mode %s\n",
 245                         regs[8], regs[9], regs[10], regs[11],
 246                         mode_name(regs[16]));
 247         if ((regs[16] & MODE_MASK) == USR_MODE)
 248                 debug_printf(state, " ip %08x  sp %08x  lr %08x  pc %08x  "
 249                                 "cpsr %08x\n", regs[12], regs[13], regs[14],
 250                                 regs[15], regs[16]);
 251         else
 252                 debug_printf(state, " ip %08x  sp %08x  lr %08x  pc %08x  "
 253                                 "cpsr %08x  spsr %08x\n", regs[12], regs[13],
 254                                 regs[14], regs[15], regs[16], regs[17]);
 255 }
 256
 257 struct mode_regs {
 258         unsigned long sp_svc;
 259         unsigned long lr_svc;
 260         unsigned long spsr_svc;
 261
 262         unsigned long sp_abt;
 263         unsigned long lr_abt;
 264         unsigned long spsr_abt;
 265
 266         unsigned long sp_und;
 267         unsigned long lr_und;
 268         unsigned long spsr_und;
 269
 270         unsigned long sp_irq;
 271         unsigned long lr_irq;
 272         unsigned long spsr_irq;
 273
 274         unsigned long r8_fiq;
 275         unsigned long r9_fiq;
 276         unsigned long r10_fiq;
 277         unsigned long r11_fiq;
 278         unsigned long r12_fiq;
 279         unsigned long sp_fiq;
 280         unsigned long lr_fiq;
 281         unsigned long spsr_fiq;
 282 };
 283
 284 void __naked get_mode_regs(struct mode_regs *regs)
 285 {
 286         asm volatile (
 287         "mrs    r1, cpsr\n"
 288         "msr    cpsr_c, #0xd3 @(SVC_MODE | PSR_I_BIT | PSR_F_BIT)\n"
 289         "stmia  r0!, {r13 - r14}\n"
 290         "mrs    r2, spsr\n"
 291         "msr    cpsr_c, #0xd7 @(ABT_MODE | PSR_I_BIT | PSR_F_BIT)\n"
 292         "stmia  r0!, {r2, r13 - r14}\n"
 293         "mrs    r2, spsr\n"
 294         "msr    cpsr_c, #0xdb @(UND_MODE | PSR_I_BIT | PSR_F_BIT)\n"
 295         "stmia  r0!, {r2, r13 - r14}\n"
 296         "mrs    r2, spsr\n"
 297         "msr    cpsr_c, #0xd2 @(IRQ_MODE | PSR_I_BIT | PSR_F_BIT)\n"
 298         "stmia  r0!, {r2, r13 - r14}\n"
 299         "mrs    r2, spsr\n"
 300         "msr    cpsr_c, #0xd1 @(FIQ_MODE | PSR_I_BIT | PSR_F_BIT)\n"
 301         "stmia  r0!, {r2, r8 - r14}\n"
 302         "mrs    r2, spsr\n"
 303         "stmia  r0!, {r2}\n"
 304         "msr    cpsr_c, r1\n"
 305         "bx     lr\n");
 306 }
 307
 308
 309 static void dump_allregs(struct fiq_debugger_state *state, unsigned *regs)
 310 {
 311         struct mode_regs mode_regs;
 312         dump_regs(state, regs);
 313         get_mode_regs(&mode_regs);
 314         debug_printf(state, " svc: sp %08x  lr %08x  spsr %08x\n",
 315                         mode_regs.sp_svc, mode_regs.lr_svc, mode_regs.spsr_svc);
 316         debug_printf(state, " abt: sp %08x  lr %08x  spsr %08x\n",
 317                         mode_regs.sp_abt, mode_regs.lr_abt, mode_regs.spsr_abt);
 318         debug_printf(state, " und: sp %08x  lr %08x  spsr %08x\n",
 319                         mode_regs.sp_und, mode_regs.lr_und, mode_regs.spsr_und);
 320         debug_printf(state, " irq: sp %08x  lr %08x  spsr %08x\n",
 321                         mode_regs.sp_irq, mode_regs.lr_irq, mode_regs.spsr_irq);
 322         debug_printf(state, " fiq: r8 %08x  r9 %08x  r10 %08x  r11 %08x  "
 323                         "r12 %08x\n",
 324                         mode_regs.r8_fiq, mode_regs.r9_fiq, mode_regs.r10_fiq,
 325                         mode_regs.r11_fiq, mode_regs.r12_fiq);
 326         debug_printf(state, " fiq: sp %08x  lr %08x  spsr %08x\n",
 327                         mode_regs.sp_fiq, mode_regs.lr_fiq, mode_regs.spsr_fiq);
 328 }
 329
 330 static void dump_irqs(struct fiq_debugger_state *state)
 331 {
 332         int n;
 333         unsigned int cpu;
 334
 335         debug_printf(state, "irqnr       total  since-last   status  name\n");
 336         for (n = 0; n < NR_IRQS; n++) {
 337                 struct irqaction *act = irq_desc[n].action;
 338                 if (!act && !kstat_irqs(n))
 339                         continue;
 340                 debug_printf(state, "%5d: %10u %11u %8x  %s\n", n,
 341                         kstat_irqs(n),
 342                         kstat_irqs(n) - state->last_irqs[n],
 343                         irq_desc[n].status,
 344                         (act && act->name) ? act->name : "???");
 345                 state->last_irqs[n] = kstat_irqs(n);
 346         }
 347
 348         for (cpu = 0; cpu < NR_CPUS; cpu++) {
 349
 350                 debug_printf(state, "LOC %d: %10u %11u\n", cpu,
 351                              __IRQ_STAT(cpu, local_timer_irqs),
 352                              __IRQ_STAT(cpu, local_timer_irqs) -
 353                              state->last_local_timer_irqs[cpu]);
 354                 state->last_local_timer_irqs[cpu] =
 355                         __IRQ_STAT(cpu, local_timer_irqs);
 356         }
 357 }
 358
 359 struct stacktrace_state {
 360         struct fiq_debugger_state *state;
 361         unsigned int depth;
 362 };
 363
 364 static int report_trace(struct stackframe *frame, void *d)
 365 {
 366         struct stacktrace_state *sts = d;
 367
 368         if (sts->depth) {
 369                 debug_printf(sts->state,
 370                         "  pc: %p (%pF), lr %p (%pF), sp %p, fp %p\n",
 371                         frame->pc, frame->pc, frame->lr, frame->lr,
 372                         frame->sp, frame->fp);
 373                 sts->depth--;
 374                 return 0;
 375         }
 376         debug_printf(sts->state, "  ...\n");
 377
 378         return sts->depth == 0;
 379 }
 380
 381 struct frame_tail {
 382         struct frame_tail *fp;
 383         unsigned long sp;
 384         unsigned long lr;
 385 } __attribute__((packed));
 386
 387 static struct frame_tail *user_backtrace(struct fiq_debugger_state *state,
 388                                         struct frame_tail *tail)
 389 {
 390         struct frame_tail buftail[2];
 391
 392         /* Also check accessibility of one struct frame_tail beyond */
 393         if (!access_ok(VERIFY_READ, tail, sizeof(buftail))) {
 394                 debug_printf(state, "  invalid frame pointer %p\n", tail);
 395                 return NULL;
 396         }
 397         if (__copy_from_user_inatomic(buftail, tail, sizeof(buftail))) {
 398                 debug_printf(state,
 399                         "  failed to copy frame pointer %p\n", tail);
 400                 return NULL;
 401         }
 402
 403         debug_printf(state, "  %p\n", buftail[0].lr);
 404
 405         /* frame pointers should strictly progress back up the stack
 406          * (towards higher addresses) */
 407         if (tail >= buftail[0].fp)
 408                 return NULL;
 409
 410         return buftail[0].fp-1;
 411 }
 412
 413 void dump_stacktrace(struct fiq_debugger_state *state,
 414                 struct pt_regs * const regs, unsigned int depth, void *ssp)
 415 {
 416         struct frame_tail *tail;
 417         struct thread_info *real_thread_info = THREAD_INFO(ssp);
 418         struct stacktrace_state sts;
 419
 420         sts.depth = depth;
 421         sts.state = state;
 422         *current_thread_info() = *real_thread_info;
 423
 424         if (!current)
 425                 debug_printf(state, "current NULL\n");
 426         else
 427                 debug_printf(state, "pid: %d  comm: %s\n",
 428                         current->pid, current->comm);
 429         dump_regs(state, (unsigned *)regs);
 430
 431         if (!user_mode(regs)) {
 432                 struct stackframe frame;
 433                 frame.fp = regs->ARM_fp;
 434                 frame.sp = regs->ARM_sp;
 435                 frame.lr = regs->ARM_lr;
 436                 frame.pc = regs->ARM_pc;
 437                 debug_printf(state,
 438                         "  pc: %p (%pF), lr %p (%pF), sp %p, fp %p\n",
 439                         regs->ARM_pc, regs->ARM_pc, regs->ARM_lr, regs->ARM_lr,
 440                         regs->ARM_sp, regs->ARM_fp);
 441                 walk_stackframe(&frame, report_trace, &sts);
 442                 return;
 443         }
 444
 445         tail = ((struct frame_tail *) regs->ARM_fp) - 1;
 446         while (depth-- && tail && !((unsigned long) tail & 3))
 447                 tail = user_backtrace(state, tail);
 448 }
 449
 450 static void debug_help(struct fiq_debugger_state *state)
 451 {
 452         debug_printf(state,     "FIQ Debugger commands:\n"
 453                                 " pc            PC status\n"
 454                                 " regs          Register dump\n"
 455                                 " allregs       Extended Register dump\n"
 456                                 " bt            Stack trace\n"
 457                                 " reboot        Reboot\n"
 458                                 " irqs          Interupt status\n"
 459                                 " kmsg          Kernel log\n"
 460                                 " version       Kernel version\n");
 461         debug_printf(state,     " sleep         Allow sleep while in FIQ\n"
 462                                 " nosleep       Disable sleep while in FIQ\n"
 463                                 " console       Switch terminal to console\n"
 464                                 " cpu           Current CPU\n"
 465                                 " cpu <number>  Switch to CPU<number>\n");
 466         if (!state->debug_busy) {
 467                 strcpy(state->debug_cmd, "help");
 468                 state->debug_busy = 1;
 469                 debug_force_irq(state);
 470         }
 471 }
 472
 473 static void debug_exec(struct fiq_debugger_state *state,
 474                         const char *cmd, unsigned *regs, void *svc_sp)
 475 {
 476         if (!strcmp(cmd, "help") || !strcmp(cmd, "?")) {
 477                 debug_help(state);
 478         } else if (!strcmp(cmd, "pc")) {
 479                 debug_printf(state, " pc %08x cpsr %08x mode %s\n",
 480                         regs[15], regs[16], mode_name(regs[16]));
 481         } else if (!strcmp(cmd, "regs")) {
 482                 dump_regs(state, regs);
 483         } else if (!strcmp(cmd, "allregs")) {
 484                 dump_allregs(state, regs);
 485         } else if (!strcmp(cmd, "bt")) {
 486                 dump_stacktrace(state, (struct pt_regs *)regs, 100, svc_sp);
 487         } else if (!strcmp(cmd, "reboot")) {
 488                 arch_reset(0, 0);
 489         } else if (!strcmp(cmd, "irqs")) {
 490                 dump_irqs(state);
 491         } else if (!strcmp(cmd, "kmsg")) {
 492                 dump_kernel_log(state);
 493         } else if (!strcmp(cmd, "version")) {
 494                 debug_printf(state, "%s\n", linux_banner);
 495         } else if (!strcmp(cmd, "sleep")) {
 496                 state->no_sleep = false;
 497         } else if (!strcmp(cmd, "nosleep")) {
 498                 state->no_sleep = true;
 499         } else if (!strcmp(cmd, "console")) {
 500                 state->console_enable = true;
 501                 debug_printf(state, "console mode\n");
 502         } else if (!strcmp(cmd, "cpu")) {
 503                 debug_printf(state, "cpu %d\n", state->current_cpu);
 504         } else if (!strncmp(cmd, "cpu ", 4)) {
 505                 unsigned long cpu = 0;
 506                 if (strict_strtoul(cmd + 4, 10, &cpu) == 0)
 507                         state->current_cpu = cpu;
 508                 else
 509                         debug_printf(state, "invalid cpu\n");
 510                 debug_printf(state, "cpu %d\n", state->current_cpu);
 511         } else {
 512                 if (state->debug_busy) {
 513                         debug_printf(state,
 514                                 "command processor busy. trying to abort.\n");
 515                         state->debug_abort = -1;
 516                 } else {
 517                         strcpy(state->debug_cmd, cmd);
 518                         state->debug_busy = 1;
 519                 }
 520
 521                 debug_force_irq(state);
 522
 523                 return;
 524         }
 525         if (!state->console_enable)
 526                 debug_prompt(state);
 527 }
 528
 529 static void sleep_timer_expired(unsigned long data)
 530 {
 531         struct fiq_debugger_state *state = (struct fiq_debugger_state *)data;
 532
 533         if (state->uart_clk_enabled && !state->no_sleep) {
 534                 if (state->debug_enable) {
 535                         state->debug_enable = false;
 536                         debug_printf_nfiq(state, "suspending fiq debugger\n");
 537                 }
 538                 state->ignore_next_wakeup_irq = true;
 539                 if (state->clk)
 540                         clk_disable(state->clk);
 541                 state->uart_clk_enabled = false;
 542                 enable_wakeup_irq(state);
 543         }
 544         wake_unlock(&state->debugger_wake_lock);
 545 }
 546
 547 static irqreturn_t wakeup_irq_handler(int irq, void *dev)
 548 {
 549         struct fiq_debugger_state *state = dev;
 550
 551         if (!state->no_sleep)
 552                 debug_puts(state, "WAKEUP\n");
 553         if (state->ignore_next_wakeup_irq)
 554                 state->ignore_next_wakeup_irq = false;
 555         else if (!state->uart_clk_enabled) {
 556                 wake_lock(&state->debugger_wake_lock);
 557                 if (state->clk)
 558                         clk_enable(state->clk);
 559                 state->uart_clk_enabled = true;
 560                 disable_wakeup_irq(state);
 561                 mod_timer(&state->sleep_timer, jiffies + HZ / 2);
 562         }
 563         return IRQ_HANDLED;
 564 }
 565
 566 static irqreturn_t debug_irq(int irq, void *dev)
 567 {
 568         struct fiq_debugger_state *state = dev;
 569         if (state->pdata->force_irq_ack)
 570                 state->pdata->force_irq_ack(state->pdev, state->signal_irq);
 571
 572         if (!state->no_sleep) {
 573                 wake_lock(&state->debugger_wake_lock);
 574                 mod_timer(&state->sleep_timer, jiffies + HZ * 5);
 575         }
 576 #if defined(CONFIG_FIQ_DEBUGGER_CONSOLE)
 577         if (state->tty) {
 578                 int i;
 579                 int count = fiq_debugger_ringbuf_level(state->tty_rbuf);
 580                 for (i = 0; i < count; i++) {
 581                         int c = fiq_debugger_ringbuf_peek(state->tty_rbuf, i);
 582                         tty_insert_flip_char(state->tty, c, TTY_NORMAL);
 583                         if (!fiq_debugger_ringbuf_consume(state->tty_rbuf, 1))
 584                                 pr_warn("fiq tty failed to consume byte\n");
 585                 }
 586                 tty_flip_buffer_push(state->tty);
 587         }
 588 #endif
 589         if (state->debug_busy) {
 590                 struct kdbg_ctxt ctxt;
 591
 592                 ctxt.printf = debug_printf_nfiq;
 593                 ctxt.cookie = state;
 594                 kernel_debugger(&ctxt, state->debug_cmd);
 595                 debug_prompt(state);
 596
 597                 state->debug_busy = 0;
 598         }
 599         return IRQ_HANDLED;
 600 }
 601
 602 static int debug_getc(struct fiq_debugger_state *state)
 603 {
 604         return state->pdata->uart_getc(state->pdev);
 605 }
 606
 607 static void debug_fiq(struct fiq_glue_handler *h, void *regs, void *svc_sp)
 608 {
 609         struct fiq_debugger_state *state =
 610                 container_of(h, struct fiq_debugger_state, handler);
 611         int c;
 612         static int last_c;
 613         int count = 0;
 614         unsigned int this_cpu = THREAD_INFO(svc_sp)->cpu;
 615
 616         if (this_cpu != state->current_cpu) {
 617                 if (state->in_fiq)
 618                         return;
 619
 620                 if (atomic_inc_return(&state->unhandled_fiq_count) !=
 621                                         MAX_UNHANDLED_FIQ_COUNT)
 622                         return;
 623
 624                 debug_printf(state, "fiq_debugger: cpu %d not responding, "
 625                         "reverting to cpu %d\n", state->current_cpu,
 626                         this_cpu);
 627
 628                 atomic_set(&state->unhandled_fiq_count, 0);
 629                 state->current_cpu = this_cpu;
 630                 return;
 631         }
 632
 633         state->in_fiq = true;
 634
 635         while ((c = debug_getc(state)) != FIQ_DEBUGGER_NO_CHAR) {
 636                 count++;
 637                 if (!state->debug_enable) {
 638                         if ((c == 13) || (c == 10)) {
 639                                 state->debug_enable = true;
 640                                 state->debug_count = 0;
 641                                 debug_prompt(state);
 642                         }
 643                 } else if (c == FIQ_DEBUGGER_BREAK) {
 644                         state->console_enable = false;
 645                         debug_puts(state, "fiq debugger mode\n");
 646                         state->debug_count = 0;
 647                         debug_prompt(state);
 648 #ifdef CONFIG_FIQ_DEBUGGER_CONSOLE
 649                 } else if (state->console_enable && state->tty_rbuf) {
 650                         fiq_debugger_ringbuf_push(state->tty_rbuf, c);
 651                         debug_force_irq(state);
 652 #endif
 653                 } else if ((c >= ' ') && (c < 127)) {
 654                         if (state->debug_count < (DEBUG_MAX - 1)) {
 655                                 state->debug_buf[state->debug_count++] = c;
 656                                 state->pdata->uart_putc(state->pdev, c);
 657                         }
 658                 } else if ((c == 8) || (c == 127)) {
 659                         if (state->debug_count > 0) {
 660                                 state->debug_count--;
 661                                 state->pdata->uart_putc(state->pdev, 8);
 662                                 state->pdata->uart_putc(state->pdev, ' ');
 663                                 state->pdata->uart_putc(state->pdev, 8);
 664                         }
 665                 } else if ((c == 13) || (c == 10)) {
 666                         if (c == '\r' || (c == '\n' && last_c != '\r')) {
 667                                 state->pdata->uart_putc(state->pdev, '\r');
 668                                 state->pdata->uart_putc(state->pdev, '\n');
 669                         }
 670                         if (state->debug_count) {
 671                                 state->debug_buf[state->debug_count] = 0;
 672                                 state->debug_count = 0;
 673                                 debug_exec(state, state->debug_buf,
 674                                         regs, svc_sp);
 675                         } else {
 676                                 debug_prompt(state);
 677                         }
 678                 }
 679                 last_c = c;
 680         }
 681         debug_uart_flush(state);
 682         if (state->pdata->fiq_ack)
 683                 state->pdata->fiq_ack(state->pdev, state->fiq);
 684
 685         /* poke sleep timer if necessary */
 686         if (state->debug_enable && !state->no_sleep)
 687                 debug_force_irq(state);
 688
 689         atomic_set(&state->unhandled_fiq_count, 0);
 690         state->in_fiq = false;
 691 }
 692
 693 static void debug_resume(struct fiq_glue_handler *h)
 694 {
 695         struct fiq_debugger_state *state =
 696                 container_of(h, struct fiq_debugger_state, handler);
 697         if (state->pdata->uart_resume)
 698                 state->pdata->uart_resume(state->pdev);
 699 }
 700
 701 #if defined(CONFIG_FIQ_DEBUGGER_CONSOLE)
 702 struct tty_driver *debug_console_device(struct console *co, int *index)
 703 {
 704         struct fiq_debugger_state *state;
 705         state = container_of(co, struct fiq_debugger_state, console);
 706         *index = 0;
 707         return state->tty_driver;
 708 }
 709
 710 static void debug_console_write(struct console *co,
 711                                 const char *s, unsigned int count)
 712 {
 713         struct fiq_debugger_state *state;
 714
 715         state = container_of(co, struct fiq_debugger_state, console);
 716
 717         if (!state->console_enable)
 718                 return;
 719
 720         while (count--) {
 721                 if (*s == '\n')
 722                         state->pdata->uart_putc(state->pdev, '\r');
 723                 state->pdata->uart_putc(state->pdev, *s++);
 724         }
 725         debug_uart_flush(state);
 726 }
 727
 728 static struct console fiq_debugger_console = {
 729         .name = "ttyFIQ",
 730         .device = debug_console_device,
 731         .write = debug_console_write,
 732         .flags = CON_PRINTBUFFER | CON_ANYTIME | CON_ENABLED,
 733 };
 734
 735 int fiq_tty_open(struct tty_struct *tty, struct file *filp)
 736 {
 737         struct fiq_debugger_state *state = tty->driver->driver_state;
 738         if (state->tty_open_count++)
 739                 return 0;
 740
 741         tty->driver_data = state;
 742         state->tty = tty;
 743         return 0;
 744 }
 745
 746 void fiq_tty_close(struct tty_struct *tty, struct file *filp)
 747 {
 748         struct fiq_debugger_state *state = tty->driver_data;
 749         if (--state->tty_open_count)
 750                 return;
 751         state->tty = NULL;
 752 }
 753
 754 int  fiq_tty_write(struct tty_struct *tty, const unsigned char *buf, int count)
 755 {
 756         int i;
 757         struct fiq_debugger_state *state = tty->driver_data;
 758
 759         if (!state->console_enable)
 760                 return count;
 761
 762         if (state->clk)
 763                 clk_enable(state->clk);
 764         for (i = 0; i < count; i++)
 765                 state->pdata->uart_putc(state->pdev, *buf++);
 766         if (state->clk)
 767                 clk_disable(state->clk);
 768
 769         return count;
 770 }
 771
 772 int  fiq_tty_write_room(struct tty_struct *tty)
 773 {
 774         return 1024;
 775 }
 776
 777 static const struct tty_operations fiq_tty_driver_ops = {
 778         .write = fiq_tty_write,
 779         .write_room = fiq_tty_write_room,
 780         .open = fiq_tty_open,
 781         .close = fiq_tty_close,
 782 };
 783
 784 static int fiq_debugger_tty_init(struct fiq_debugger_state *state)
 785 {
 786         int ret = -EINVAL;
 787
 788         state->tty_driver = alloc_tty_driver(1);
 789         if (!state->tty_driver) {
 790                 pr_err("Failed to allocate fiq debugger tty\n");
 791                 return -ENOMEM;
 792         }
 793
 794         state->tty_driver->owner                = THIS_MODULE;
 795         state->tty_driver->driver_name  = "fiq-debugger";
 796         state->tty_driver->name         = "ttyFIQ";
 797         state->tty_driver->type         = TTY_DRIVER_TYPE_SERIAL;
 798         state->tty_driver->subtype      = SERIAL_TYPE_NORMAL;
 799         state->tty_driver->init_termios = tty_std_termios;
 800         state->tty_driver->init_termios.c_cflag =
 801                                         B115200 | CS8 | CREAD | HUPCL | CLOCAL;
 802         state->tty_driver->init_termios.c_ispeed =
 803                 state->tty_driver->init_termios.c_ospeed = 115200;
 804         state->tty_driver->flags                = TTY_DRIVER_REAL_RAW;
 805         tty_set_operations(state->tty_driver, &fiq_tty_driver_ops);
 806         state->tty_driver->driver_state = state;
 807
 808         ret = tty_register_driver(state->tty_driver);
 809         if (ret) {
 810                 pr_err("Failed to register fiq tty: %d\n", ret);
 811                 goto err;
 812         }
 813
 814         state->tty_rbuf = fiq_debugger_ringbuf_alloc(1024);
 815         if (!state->tty_rbuf) {
 816                 pr_err("Failed to allocate fiq debugger ringbuf\n");
 817                 ret = -ENOMEM;
 818                 goto err;
 819         }
 820
 821         pr_info("Registered FIQ tty driver %p\n", state->tty_driver);
 822         return 0;
 823
 824 err:
 825         fiq_debugger_ringbuf_free(state->tty_rbuf);
 826         state->tty_rbuf = NULL;
 827         put_tty_driver(state->tty_driver);
 828         return ret;
 829 }
 830 #endif
 831
 832 static int fiq_debugger_probe(struct platform_device *pdev)
 833 {
 834         int ret;
 835         struct fiq_debugger_pdata *pdata = dev_get_platdata(&pdev->dev);
 836         struct fiq_debugger_state *state;
 837
 838         if (!pdata->uart_getc || !pdata->uart_putc || !pdata->fiq_enable)
 839                 return -EINVAL;
 840
 841         state = kzalloc(sizeof(*state), GFP_KERNEL);
 842         state->handler.fiq = debug_fiq;
 843         state->handler.resume = debug_resume;
 844         setup_timer(&state->sleep_timer, sleep_timer_expired,
 845                     (unsigned long)state);
 846         state->pdata = pdata;
 847         state->pdev = pdev;
 848         state->no_sleep = initial_no_sleep;
 849         state->debug_enable = initial_debug_enable;
 850         state->console_enable = initial_console_enable;
 851
 852         state->fiq = platform_get_irq_byname(pdev, "fiq");
 853         state->signal_irq = platform_get_irq_byname(pdev, "signal");
 854         state->wakeup_irq = platform_get_irq_byname(pdev, "wakeup");
 855
 856         if (state->wakeup_irq < 0)
 857                 state->no_sleep = true;
 858         state->ignore_next_wakeup_irq = !state->no_sleep;
 859
 860         wake_lock_init(&state->debugger_wake_lock,
 861                         WAKE_LOCK_SUSPEND, "serial-debug");
 862
 863         state->clk = clk_get(&pdev->dev, NULL);
 864         if (IS_ERR(state->clk))
 865                 state->clk = NULL;
 866
 867         if (state->clk)
 868                 clk_enable(state->clk);
 869
 870         if (pdata->uart_init) {
 871                 ret = pdata->uart_init(pdev);
 872                 if (ret)
 873                         goto err_uart_init;
 874         }
 875
 876         debug_printf_nfiq(state, "<hit enter %sto activate fiq debugger>\n",
 877                                 state->no_sleep ? "" : "twice ");
 878
 879         ret = fiq_glue_register_handler(&state->handler);
 880         if (ret) {
 881                 pr_err("serial_debugger: could not install fiq handler\n");
 882                 goto err_register_fiq;
 883         }
 884
 885         pdata->fiq_enable(pdev, state->fiq, 1);
 886
 887         if (state->clk)
 888                 clk_disable(state->clk);
 889
 890         ret = request_irq(state->signal_irq, debug_irq,
 891                           IRQF_TRIGGER_RISING, "debug", state);
 892         if (ret)
 893                 pr_err("serial_debugger: could not install signal_irq");
 894
 895         if (state->wakeup_irq >= 0) {
 896                 ret = request_irq(state->wakeup_irq, wakeup_irq_handler,
 897                                   IRQF_TRIGGER_FALLING | IRQF_DISABLED,
 898                                   "debug-wakeup", state);
 899                 if (ret) {
 900                         pr_err("serial_debugger: "
 901                                 "could not install wakeup irq\n");
 902                         state->wakeup_irq = -1;
 903                 } else {
 904                         ret = enable_irq_wake(state->wakeup_irq);
 905                         if (ret) {
 906                                 pr_err("serial_debugger: "
 907                                         "could not enable wakeup\n");
 908                                 state->wakeup_irq_no_set_wake = true;
 909                         }
 910                 }
 911         }
 912         if (state->no_sleep)
 913                 wakeup_irq_handler(state->wakeup_irq, state);
 914
 915 #if defined(CONFIG_FIQ_DEBUGGER_CONSOLE)
 916         state->console = fiq_debugger_console;
 917         register_console(&state->console);
 918         fiq_debugger_tty_init(state);
 919 #endif
 920         return 0;
 921
 922 err_register_fiq:
 923         if (pdata->uart_free)
 924                 pdata->uart_free(pdev);
 925 err_uart_init:
 926         kfree(state);
 927         if (state->clk)
 928                 clk_put(state->clk);
 929         return ret;
 930 }
 931
 932 static struct platform_driver fiq_debugger_driver = {
 933         .probe = fiq_debugger_probe,
 934         .driver.name = "fiq_debugger",
 935 };
 936
 937 static int __init fiq_debugger_init(void)
 938 {
 939         return platform_driver_register(&fiq_debugger_driver);
 940 }
 941
 942 postcore_initcall(fiq_debugger_init);