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_stat.h>
  26 #include <linux/kmsg_dump.h>
  27 #include <linux/irq.h>
  28 #include <linux/delay.h>
  29 #include <linux/reboot.h>
  30 #include <linux/sched.h>
  31 #include <linux/slab.h>
  32 #include <linux/smp.h>
  33 #include <linux/timer.h>
  34 #include <linux/tty.h>
  35 #include <linux/tty_flip.h>
  36 #include <linux/wakelock.h>
  37
  38 #include <asm/fiq_debugger.h>
  39 #include <asm/fiq_glue.h>
  40 #include <asm/stacktrace.h>
  41
  42 #include <linux/uaccess.h>
  43 #include <linux/rockchip/grf.h>
  44 #include <linux/rockchip/iomap.h>
  45 #include <linux/rockchip/cpu.h>
  46
  47 #include "fiq_debugger_ringbuf.h"
  48
  49 #ifdef CONFIG_RK29_WATCHDOG
  50 extern void rk29_wdt_keepalive(void);
  51 #define wdt_keepalive() rk29_wdt_keepalive()
  52 #else
  53 #define wdt_keepalive() do {} while (0)
  54 #endif
  55 #define DEBUG_MAX 64
  56 #define CMD_COUNT 0x0f
  57 #define MAX_UNHANDLED_FIQ_COUNT 1000000
  58
  59 #ifdef CONFIG_ARCH_ROCKCHIP
  60 #define MAX_FIQ_DEBUGGER_PORTS 1
  61 #else
  62 #define MAX_FIQ_DEBUGGER_PORTS 4
  63 #endif
  64
  65 #define THREAD_INFO(sp) ((struct thread_info *) \
  66                 ((unsigned long)(sp) & ~(THREAD_SIZE - 1)))
  67
  68 struct fiq_debugger_state {
  69         struct fiq_glue_handler handler;
  70
  71         int fiq;
  72         int uart_irq;
  73         int signal_irq;
  74         int wakeup_irq;
  75         bool wakeup_irq_no_set_wake;
  76         struct clk *clk;
  77         struct fiq_debugger_pdata *pdata;
  78         struct platform_device *pdev;
  79
  80         char debug_cmd[DEBUG_MAX];
  81         int debug_busy;
  82         int debug_abort;
  83
  84         char debug_buf[DEBUG_MAX];
  85         int debug_count;
  86
  87 #ifdef CONFIG_ARCH_ROCKCHIP
  88         char cmd_buf[CMD_COUNT+1][DEBUG_MAX];
  89         int back_pointer;
  90         int current_pointer;
  91 #endif
  92         bool no_sleep;
  93         bool debug_enable;
  94         bool ignore_next_wakeup_irq;
  95         struct timer_list sleep_timer;
  96         spinlock_t sleep_timer_lock;
  97         bool uart_enabled;
  98         struct wake_lock debugger_wake_lock;
  99         bool console_enable;
 100         int current_cpu;
 101         atomic_t unhandled_fiq_count;
 102         bool in_fiq;
 103
 104         struct work_struct work;
 105         spinlock_t work_lock;
 106         char work_cmd[DEBUG_MAX];
 107
 108 #ifdef CONFIG_FIQ_DEBUGGER_CONSOLE
 109         spinlock_t console_lock;
 110         struct console console;
 111         struct tty_port tty_port;
 112         struct fiq_debugger_ringbuf *tty_rbuf;
 113         bool syslog_dumping;
 114 #endif
 115
 116 #ifdef CONFIG_ARCH_ROCKCHIP
 117         unsigned int last_irqs[1024];
 118         unsigned int last_local_irqs[NR_CPUS][32];
 119 #else
 120         unsigned int last_irqs[NR_IRQS];
 121         unsigned int last_local_timer_irqs[NR_CPUS];
 122 #endif
 123 };
 124
 125 #ifdef CONFIG_FIQ_DEBUGGER_CONSOLE
 126 struct tty_driver *fiq_tty_driver;
 127 #endif
 128
 129 #ifdef CONFIG_FIQ_DEBUGGER_NO_SLEEP
 130 static bool initial_no_sleep = true;
 131 #else
 132 static bool initial_no_sleep;
 133 #endif
 134
 135 #ifdef CONFIG_FIQ_DEBUGGER_CONSOLE_DEFAULT_ENABLE
 136 static bool initial_debug_enable = true;
 137 static bool initial_console_enable = true;
 138 #else
 139 static bool initial_debug_enable;
 140 static bool initial_console_enable;
 141 #endif
 142
 143 static bool fiq_kgdb_enable;
 144
 145 module_param_named(no_sleep, initial_no_sleep, bool, 0644);
 146 module_param_named(debug_enable, initial_debug_enable, bool, 0644);
 147 module_param_named(console_enable, initial_console_enable, bool, 0644);
 148 module_param_named(kgdb_enable, fiq_kgdb_enable, bool, 0644);
 149
 150 void gic_set_irq_secure(struct irq_data *d);
 151 void gic_set_irq_priority(struct irq_data *d, u8 pri);
 152
 153 #ifdef CONFIG_FIQ_DEBUGGER_WAKEUP_IRQ_ALWAYS_ON
 154 static inline void enable_wakeup_irq(struct fiq_debugger_state *state) {}
 155 static inline void disable_wakeup_irq(struct fiq_debugger_state *state) {}
 156 #else
 157 static inline void enable_wakeup_irq(struct fiq_debugger_state *state)
 158 {
 159         if (state->wakeup_irq < 0)
 160                 return;
 161         enable_irq(state->wakeup_irq);
 162         if (!state->wakeup_irq_no_set_wake)
 163                 enable_irq_wake(state->wakeup_irq);
 164 }
 165 static inline void disable_wakeup_irq(struct fiq_debugger_state *state)
 166 {
 167         if (state->wakeup_irq < 0)
 168                 return;
 169         disable_irq_nosync(state->wakeup_irq);
 170         if (!state->wakeup_irq_no_set_wake)
 171                 disable_irq_wake(state->wakeup_irq);
 172 }
 173 #endif
 174
 175 static bool inline debug_have_fiq(struct fiq_debugger_state *state)
 176 {
 177         return (state->fiq >= 0);
 178 }
 179
 180 static void debug_force_irq(struct fiq_debugger_state *state)
 181 {
 182         unsigned int irq = state->signal_irq;
 183
 184         if (WARN_ON(!debug_have_fiq(state)))
 185                 return;
 186         if (state->pdata->force_irq) {
 187                 state->pdata->force_irq(state->pdev, irq);
 188         } else {
 189                 struct irq_chip *chip = irq_get_chip(irq);
 190                 if (chip && chip->irq_retrigger)
 191                         chip->irq_retrigger(irq_get_irq_data(irq));
 192         }
 193 }
 194
 195 static void debug_uart_enable(struct fiq_debugger_state *state)
 196 {
 197         if (state->clk)
 198                 clk_enable(state->clk);
 199         if (state->pdata->uart_enable)
 200                 state->pdata->uart_enable(state->pdev);
 201 }
 202
 203 static void debug_uart_disable(struct fiq_debugger_state *state)
 204 {
 205         if (state->pdata->uart_disable)
 206                 state->pdata->uart_disable(state->pdev);
 207         if (state->clk)
 208                 clk_disable(state->clk);
 209 }
 210
 211 static void debug_uart_flush(struct fiq_debugger_state *state)
 212 {
 213         if (state->pdata->uart_flush)
 214                 state->pdata->uart_flush(state->pdev);
 215 }
 216
 217 static void debug_putc(struct fiq_debugger_state *state, char c)
 218 {
 219         state->pdata->uart_putc(state->pdev, c);
 220 }
 221
 222 static void debug_puts(struct fiq_debugger_state *state, char *s)
 223 {
 224         unsigned c;
 225         while ((c = *s++)) {
 226                 if (c == '\n')
 227                         debug_putc(state, '\r');
 228                 debug_putc(state, c);
 229         }
 230 }
 231
 232 static void debug_prompt(struct fiq_debugger_state *state)
 233 {
 234         debug_puts(state, "debug> ");
 235 }
 236
 237 static void dump_kernel_log(struct fiq_debugger_state *state)
 238 {
 239         char buf[512];
 240         size_t len;
 241         struct kmsg_dumper dumper = { .active = true };
 242
 243
 244         kmsg_dump_rewind_nolock(&dumper);
 245         while (kmsg_dump_get_line_nolock(&dumper, true, buf,
 246                                          sizeof(buf) - 1, &len)) {
 247                 buf[len] = 0;
 248                 debug_puts(state, buf);
 249                 wdt_keepalive();
 250         }
 251 }
 252
 253 #ifdef CONFIG_RK_LAST_LOG
 254 #include <linux/ctype.h>
 255 extern char *rk_last_log_get(unsigned *size);
 256 static void dump_last_kernel_log(struct fiq_debugger_state *state)
 257 {
 258         unsigned size, i, c;
 259         char *s = rk_last_log_get(&size);
 260
 261         for (i = 0; i < size; i++) {
 262                 if (i % 1024 == 0)
 263                         wdt_keepalive();
 264                 c = s[i];
 265                 if (c == '\n') {
 266                         state->pdata->uart_putc(state->pdev, '\r');
 267                         state->pdata->uart_putc(state->pdev, c);
 268                 } else if (isascii(c) && isprint(c)) {
 269                         state->pdata->uart_putc(state->pdev, c);
 270                 }
 271         }
 272 }
 273 #endif
 274
 275 static char *mode_name(unsigned cpsr)
 276 {
 277         switch (cpsr & MODE_MASK) {
 278         case USR_MODE: return "USR";
 279         case FIQ_MODE: return "FIQ";
 280         case IRQ_MODE: return "IRQ";
 281         case SVC_MODE: return "SVC";
 282         case ABT_MODE: return "ABT";
 283         case UND_MODE: return "UND";
 284         case SYSTEM_MODE: return "SYS";
 285         default: return "???";
 286         }
 287 }
 288
 289 static int debug_printf(void *cookie, const char *fmt, ...)
 290 {
 291         struct fiq_debugger_state *state = cookie;
 292         char buf[256];
 293         va_list ap;
 294
 295         va_start(ap, fmt);
 296         vsnprintf(buf, sizeof(buf), fmt, ap);
 297         va_end(ap);
 298
 299         debug_puts(state, buf);
 300         return state->debug_abort;
 301 }
 302
 303 /* Safe outside fiq context */
 304 static int debug_printf_nfiq(void *cookie, const char *fmt, ...)
 305 {
 306         struct fiq_debugger_state *state = cookie;
 307         char buf[256];
 308         va_list ap;
 309         unsigned long irq_flags;
 310
 311         va_start(ap, fmt);
 312         vsnprintf(buf, 128, fmt, ap);
 313         va_end(ap);
 314
 315         local_irq_save(irq_flags);
 316         debug_puts(state, buf);
 317         debug_uart_flush(state);
 318         local_irq_restore(irq_flags);
 319         return state->debug_abort;
 320 }
 321
 322 static void dump_regs(struct fiq_debugger_state *state, unsigned *regs)
 323 {
 324         debug_printf(state, " r0 %08x  r1 %08x  r2 %08x  r3 %08x\n",
 325                         regs[0], regs[1], regs[2], regs[3]);
 326         debug_printf(state, " r4 %08x  r5 %08x  r6 %08x  r7 %08x\n",
 327                         regs[4], regs[5], regs[6], regs[7]);
 328         debug_printf(state, " r8 %08x  r9 %08x r10 %08x r11 %08x  mode %s\n",
 329                         regs[8], regs[9], regs[10], regs[11],
 330                         mode_name(regs[16]));
 331         if ((regs[16] & MODE_MASK) == USR_MODE)
 332                 debug_printf(state, " ip %08x  sp %08x  lr %08x  pc %08x  "
 333                                 "cpsr %08x\n", regs[12], regs[13], regs[14],
 334                                 regs[15], regs[16]);
 335         else
 336                 debug_printf(state, " ip %08x  sp %08x  lr %08x  pc %08x  "
 337                                 "cpsr %08x  spsr %08x\n", regs[12], regs[13],
 338                                 regs[14], regs[15], regs[16], regs[17]);
 339 }
 340
 341 struct mode_regs {
 342         unsigned long sp_svc;
 343         unsigned long lr_svc;
 344         unsigned long spsr_svc;
 345
 346         unsigned long sp_abt;
 347         unsigned long lr_abt;
 348         unsigned long spsr_abt;
 349
 350         unsigned long sp_und;
 351         unsigned long lr_und;
 352         unsigned long spsr_und;
 353
 354         unsigned long sp_irq;
 355         unsigned long lr_irq;
 356         unsigned long spsr_irq;
 357
 358         unsigned long r8_fiq;
 359         unsigned long r9_fiq;
 360         unsigned long r10_fiq;
 361         unsigned long r11_fiq;
 362         unsigned long r12_fiq;
 363         unsigned long sp_fiq;
 364         unsigned long lr_fiq;
 365         unsigned long spsr_fiq;
 366 };
 367
 368 void __naked get_mode_regs(struct mode_regs *regs)
 369 {
 370         asm volatile (
 371         "mrs    r1, cpsr\n"
 372         "msr    cpsr_c, #0xd3 @(SVC_MODE | PSR_I_BIT | PSR_F_BIT)\n"
 373         "stmia  r0!, {r13 - r14}\n"
 374         "mrs    r2, spsr\n"
 375         "msr    cpsr_c, #0xd7 @(ABT_MODE | PSR_I_BIT | PSR_F_BIT)\n"
 376         "stmia  r0!, {r2, r13 - r14}\n"
 377         "mrs    r2, spsr\n"
 378         "msr    cpsr_c, #0xdb @(UND_MODE | PSR_I_BIT | PSR_F_BIT)\n"
 379         "stmia  r0!, {r2, r13 - r14}\n"
 380         "mrs    r2, spsr\n"
 381         "msr    cpsr_c, #0xd2 @(IRQ_MODE | PSR_I_BIT | PSR_F_BIT)\n"
 382         "stmia  r0!, {r2, r13 - r14}\n"
 383         "mrs    r2, spsr\n"
 384         "msr    cpsr_c, #0xd1 @(FIQ_MODE | PSR_I_BIT | PSR_F_BIT)\n"
 385         "stmia  r0!, {r2, r8 - r14}\n"
 386         "mrs    r2, spsr\n"
 387         "stmia  r0!, {r2}\n"
 388         "msr    cpsr_c, r1\n"
 389         "bx     lr\n");
 390 }
 391
 392
 393 static void dump_allregs(struct fiq_debugger_state *state, unsigned *regs)
 394 {
 395         struct mode_regs mode_regs;
 396         dump_regs(state, regs);
 397         get_mode_regs(&mode_regs);
 398         debug_printf(state, " svc: sp %08x  lr %08x  spsr %08x\n",
 399                         mode_regs.sp_svc, mode_regs.lr_svc, mode_regs.spsr_svc);
 400         debug_printf(state, " abt: sp %08x  lr %08x  spsr %08x\n",
 401                         mode_regs.sp_abt, mode_regs.lr_abt, mode_regs.spsr_abt);
 402         debug_printf(state, " und: sp %08x  lr %08x  spsr %08x\n",
 403                         mode_regs.sp_und, mode_regs.lr_und, mode_regs.spsr_und);
 404         debug_printf(state, " irq: sp %08x  lr %08x  spsr %08x\n",
 405                         mode_regs.sp_irq, mode_regs.lr_irq, mode_regs.spsr_irq);
 406         debug_printf(state, " fiq: r8 %08x  r9 %08x  r10 %08x  r11 %08x  "
 407                         "r12 %08x\n",
 408                         mode_regs.r8_fiq, mode_regs.r9_fiq, mode_regs.r10_fiq,
 409                         mode_regs.r11_fiq, mode_regs.r12_fiq);
 410         debug_printf(state, " fiq: sp %08x  lr %08x  spsr %08x\n",
 411                         mode_regs.sp_fiq, mode_regs.lr_fiq, mode_regs.spsr_fiq);
 412 }
 413
 414 static void dump_irqs(struct fiq_debugger_state *state)
 415 {
 416         int n;
 417         unsigned int cpu;
 418         struct irq_desc *desc;
 419
 420         debug_printf(state, "irqnr       total  since-last   status  name\n");
 421         for_each_irq_desc(n, desc) {
 422                 struct irqaction *act = desc->action;
 423                 if (!act && !kstat_irqs(n))
 424                         continue;
 425                 debug_printf(state, "%5d: %10u %11u %8x  %s\n", n,
 426                         kstat_irqs(n),
 427                         kstat_irqs(n) - state->last_irqs[n],
 428                         desc->status_use_accessors,
 429                         (act && act->name) ? act->name : "???");
 430                 state->last_irqs[n] = kstat_irqs(n);
 431         }
 432
 433 #ifdef CONFIG_ARCH_ROCKCHIP
 434         for (n = 16; n < 32; n++) {
 435                 desc = irq_to_desc(n);
 436                 if (!desc)
 437                         continue;
 438                 for (cpu = 0; cpu < NR_CPUS; cpu++) {
 439                         unsigned int irqs = kstat_irqs_cpu(n, cpu);
 440                         struct irqaction *act = desc->action;
 441                         const char *name = (act && act->name) ? act->name : "???";
 442                         if (!irqs)
 443                                 continue;
 444                         debug_printf(state,
 445                                 "%5d: %10u %11u           %s (CPU%d)\n", n,
 446                                 irqs, irqs - state->last_local_irqs[cpu][n],
 447                                 name, cpu);
 448                         state->last_local_irqs[cpu][n] = irqs;
 449                 }
 450         }
 451         for (n = 0; n < NR_IPI; n++) {
 452                 enum ipi_msg_type {
 453                         IPI_WAKEUP,
 454                         IPI_TIMER,
 455                         IPI_RESCHEDULE,
 456                         IPI_CALL_FUNC,
 457                         IPI_CALL_FUNC_SINGLE,
 458                         IPI_CPU_STOP,
 459                         IPI_COMPLETION,
 460                         IPI_CPU_BACKTRACE,
 461                 };
 462                 static const char *ipi_types[NR_IPI] = {
 463 #define S(x,s)  [x] = s
 464                         S(IPI_WAKEUP, "CPU wakeup"),
 465                         S(IPI_TIMER, "Timer broadcast"),
 466                         S(IPI_RESCHEDULE, "Rescheduling"),
 467                         S(IPI_CALL_FUNC, "Function call"),
 468                         S(IPI_CALL_FUNC_SINGLE, "Single function call"),
 469                         S(IPI_CPU_STOP, "CPU stop"),
 470                         S(IPI_COMPLETION, "Completion"),
 471                         S(IPI_CPU_BACKTRACE, "CPU backtrace"),
 472 #undef S
 473                 };
 474                 for (cpu = 0; cpu < NR_CPUS; cpu++) {
 475                         unsigned int irqs = __get_irq_stat(cpu, ipi_irqs[n]);
 476                         if (irqs == 0)
 477                                 continue;
 478                         debug_printf(state,
 479                                 "%5d: %10u %11u           %s (CPU%d)\n",
 480                                 n, irqs, irqs - state->last_local_irqs[cpu][n],
 481                                 ipi_types[n], cpu);
 482                         state->last_local_irqs[cpu][n] = irqs;
 483                 }
 484         }
 485 #endif
 486 }
 487
 488 struct stacktrace_state {
 489         struct fiq_debugger_state *state;
 490         unsigned int depth;
 491 };
 492
 493 static int report_trace(struct stackframe *frame, void *d)
 494 {
 495         struct stacktrace_state *sts = d;
 496
 497         if (sts->depth) {
 498                 debug_printf(sts->state,
 499                         "  pc: %p (%pF), lr %p (%pF), sp %p, fp %p\n",
 500                         frame->pc, frame->pc, frame->lr, frame->lr,
 501                         frame->sp, frame->fp);
 502                 sts->depth--;
 503                 return 0;
 504         }
 505         debug_printf(sts->state, "  ...\n");
 506
 507         return sts->depth == 0;
 508 }
 509
 510 struct frame_tail {
 511         struct frame_tail *fp;
 512         unsigned long sp;
 513         unsigned long lr;
 514 } __attribute__((packed));
 515
 516 static struct frame_tail *user_backtrace(struct fiq_debugger_state *state,
 517                                         struct frame_tail *tail)
 518 {
 519         struct frame_tail buftail[2];
 520
 521         /* Also check accessibility of one struct frame_tail beyond */
 522         if (!access_ok(VERIFY_READ, tail, sizeof(buftail))) {
 523                 debug_printf(state, "  invalid frame pointer %p\n", tail);
 524                 return NULL;
 525         }
 526         if (__copy_from_user_inatomic(buftail, tail, sizeof(buftail))) {
 527                 debug_printf(state,
 528                         "  failed to copy frame pointer %p\n", tail);
 529                 return NULL;
 530         }
 531
 532         debug_printf(state, "  %p\n", buftail[0].lr);
 533
 534         /* frame pointers should strictly progress back up the stack
 535          * (towards higher addresses) */
 536         if (tail >= buftail[0].fp)
 537                 return NULL;
 538
 539         return buftail[0].fp-1;
 540 }
 541
 542 void dump_stacktrace(struct fiq_debugger_state *state,
 543                 struct pt_regs * const regs, unsigned int depth, void *ssp)
 544 {
 545         struct frame_tail *tail;
 546         struct thread_info *real_thread_info = THREAD_INFO(ssp);
 547         struct stacktrace_state sts;
 548
 549         sts.depth = depth;
 550         sts.state = state;
 551         *current_thread_info() = *real_thread_info;
 552
 553         if (!current)
 554                 debug_printf(state, "current NULL\n");
 555         else
 556                 debug_printf(state, "pid: %d  comm: %s\n",
 557                         current->pid, current->comm);
 558         dump_regs(state, (unsigned *)regs);
 559
 560         if (!user_mode(regs)) {
 561                 struct stackframe frame;
 562                 frame.fp = regs->ARM_fp;
 563                 frame.sp = regs->ARM_sp;
 564                 frame.lr = regs->ARM_lr;
 565                 frame.pc = regs->ARM_pc;
 566                 debug_printf(state,
 567                         "  pc: %p (%pF), lr %p (%pF), sp %p, fp %p\n",
 568                         regs->ARM_pc, regs->ARM_pc, regs->ARM_lr, regs->ARM_lr,
 569                         regs->ARM_sp, regs->ARM_fp);
 570                 walk_stackframe(&frame, report_trace, &sts);
 571                 return;
 572         }
 573
 574         tail = ((struct frame_tail *) regs->ARM_fp) - 1;
 575         while (depth-- && tail && !((unsigned long) tail & 3))
 576                 tail = user_backtrace(state, tail);
 577 }
 578
 579 static void do_ps(struct fiq_debugger_state *state)
 580 {
 581         struct task_struct *g;
 582         struct task_struct *p;
 583         unsigned task_state;
 584         static const char stat_nam[] = "RSDTtZX";
 585
 586         debug_printf(state, "pid   ppid  prio task            pc\n");
 587         read_lock(&tasklist_lock);
 588         do_each_thread(g, p) {
 589                 task_state = p->state ? __ffs(p->state) + 1 : 0;
 590                 debug_printf(state,
 591                              "%5d %5d %4d ", p->pid, p->parent->pid, p->prio);
 592                 debug_printf(state, "%-13.13s %c", p->comm,
 593                              task_state >= sizeof(stat_nam) ? '?' : stat_nam[task_state]);
 594                 if (task_state == TASK_RUNNING)
 595                         debug_printf(state, " running\n");
 596                 else
 597                         debug_printf(state, " %08lx\n", thread_saved_pc(p));
 598         } while_each_thread(g, p);
 599         read_unlock(&tasklist_lock);
 600 }
 601
 602 #ifdef CONFIG_FIQ_DEBUGGER_CONSOLE
 603 static void begin_syslog_dump(struct fiq_debugger_state *state)
 604 {
 605         state->syslog_dumping = true;
 606 }
 607
 608 static void end_syslog_dump(struct fiq_debugger_state *state)
 609 {
 610         state->syslog_dumping = false;
 611 }
 612 #else
 613 extern int do_syslog(int type, char __user *bug, int count);
 614 static void begin_syslog_dump(struct fiq_debugger_state *state)
 615 {
 616         do_syslog(5 /* clear */, NULL, 0);
 617 }
 618
 619 static void end_syslog_dump(struct fiq_debugger_state *state)
 620 {
 621         dump_kernel_log(state);
 622 }
 623 #endif
 624
 625 static void do_sysrq(struct fiq_debugger_state *state, char rq)
 626 {
 627         if ((rq == 'g' || rq == 'G') && !fiq_kgdb_enable) {
 628                 debug_printf(state, "sysrq-g blocked\n");
 629                 return;
 630         }
 631         begin_syslog_dump(state);
 632         handle_sysrq(rq);
 633         end_syslog_dump(state);
 634 }
 635
 636 #ifdef CONFIG_KGDB
 637 static void do_kgdb(struct fiq_debugger_state *state)
 638 {
 639         if (!fiq_kgdb_enable) {
 640                 debug_printf(state, "kgdb through fiq debugger not enabled\n");
 641                 return;
 642         }
 643
 644         debug_printf(state, "enabling console and triggering kgdb\n");
 645         state->console_enable = true;
 646         handle_sysrq('g');
 647 }
 648 #endif
 649
 650 static void debug_schedule_work(struct fiq_debugger_state *state, char *cmd)
 651 {
 652         unsigned long flags;
 653
 654         spin_lock_irqsave(&state->work_lock, flags);
 655         if (state->work_cmd[0] != '\0') {
 656                 debug_printf(state, "work command processor busy\n");
 657                 spin_unlock_irqrestore(&state->work_lock, flags);
 658                 return;
 659         }
 660
 661         strlcpy(state->work_cmd, cmd, sizeof(state->work_cmd));
 662         spin_unlock_irqrestore(&state->work_lock, flags);
 663
 664         schedule_work(&state->work);
 665 }
 666
 667 static void debug_work(struct work_struct *work)
 668 {
 669         struct fiq_debugger_state *state;
 670         char work_cmd[DEBUG_MAX];
 671         char *cmd;
 672         unsigned long flags;
 673
 674         state = container_of(work, struct fiq_debugger_state, work);
 675
 676         spin_lock_irqsave(&state->work_lock, flags);
 677
 678         strlcpy(work_cmd, state->work_cmd, sizeof(work_cmd));
 679         state->work_cmd[0] = '\0';
 680
 681         spin_unlock_irqrestore(&state->work_lock, flags);
 682
 683         cmd = work_cmd;
 684         if (!strncmp(cmd, "reboot", 6)) {
 685                 cmd += 6;
 686                 while (*cmd == ' ')
 687                         cmd++;
 688                 if (cmd != '\0')
 689                         kernel_restart(cmd);
 690                 else
 691                         kernel_restart(NULL);
 692         } else {
 693                 debug_printf(state, "unknown work command '%s'\n", work_cmd);
 694         }
 695 }
 696
 697 /* This function CANNOT be called in FIQ context */
 698 static void debug_irq_exec(struct fiq_debugger_state *state, char *cmd)
 699 {
 700         int invalid_cmd = 0;
 701         if (!strcmp(cmd, "ps"))
 702                 do_ps(state);
 703         else if (!strcmp(cmd, "sysrq"))
 704                 do_sysrq(state, 'h');
 705         else if (!strncmp(cmd, "sysrq ", 6))
 706                 do_sysrq(state, cmd[6]);
 707 #ifdef CONFIG_KGDB
 708         else if (!strcmp(cmd, "kgdb"))
 709                 do_kgdb(state);
 710 #endif
 711         else if (!strncmp(cmd, "reboot", 6))
 712                 debug_schedule_work(state, cmd);
 713 #ifdef CONFIG_ARCH_ROCKCHIP
 714         else {
 715                 invalid_cmd = 1;
 716                 memset(state->debug_buf, 0, DEBUG_MAX);
 717         }
 718
 719         if (invalid_cmd == 0) {
 720                 state->current_pointer = (state->current_pointer-1) & CMD_COUNT;
 721                 if (strcmp(state->cmd_buf[state->current_pointer], state->debug_buf)) {
 722                         state->current_pointer = (state->current_pointer+1) & CMD_COUNT;
 723                         memset(state->cmd_buf[state->current_pointer], 0, DEBUG_MAX);
 724                         strcpy(state->cmd_buf[state->current_pointer], state->debug_buf);
 725                 }
 726                 memset(state->debug_buf, 0, DEBUG_MAX);
 727                 state->current_pointer = (state->current_pointer+1) & CMD_COUNT;
 728                 state->back_pointer = state->current_pointer;
 729         }
 730 #endif
 731 }
 732
 733 #ifdef CONFIG_ARCH_ROCKCHIP
 734 static char cmd_buf[][16] = {
 735                 {"pc"},
 736                 {"regs"},
 737                 {"allregs"},
 738                 {"bt"},
 739                 {"reboot"},
 740                 {"irqs"},
 741                 {"kmsg"},
 742 #ifdef CONFIG_RK_LAST_LOG
 743                 {"last_kmsg"},
 744 #endif
 745                 {"version"},
 746                 {"sleep"},
 747                 {"nosleep"},
 748                 {"console"},
 749                 {"cpu"},
 750                 {"ps"},
 751                 {"sysrq"},
 752                 {"reset"},
 753 #ifdef CONFIG_KGDB
 754                 {"kgdb"},
 755 #endif
 756 };
 757 #endif
 758
 759 static void debug_help(struct fiq_debugger_state *state)
 760 {
 761         debug_printf(state,     "FIQ Debugger commands:\n"
 762                                 " pc            PC status\n"
 763                                 " regs          Register dump\n"
 764                                 " allregs       Extended Register dump\n"
 765                                 " bt            Stack trace\n"
 766                                 " reboot [<c>]  Reboot with command <c>\n"
 767                                 " reset [<c>]   Hard reset with command <c>\n"
 768                                 " irqs          Interupt status\n"
 769                                 " kmsg          Kernel log\n"
 770                                 " version       Kernel version\n");
 771 #ifdef CONFIG_RK_LAST_LOG
 772         debug_printf(state,     " last_kmsg     Last kernel log\n");
 773 #endif
 774         debug_printf(state,     " sleep         Allow sleep while in FIQ\n"
 775                                 " nosleep       Disable sleep while in FIQ\n"
 776                                 " console       Switch terminal to console\n"
 777                                 " cpu           Current CPU\n"
 778                                 " cpu <number>  Switch to CPU<number>\n");
 779         debug_printf(state,     " ps            Process list\n"
 780                                 " sysrq         sysrq options\n"
 781                                 " sysrq <param> Execute sysrq with <param>\n");
 782 #ifdef CONFIG_KGDB
 783         debug_printf(state,     " kgdb          Enter kernel debugger\n");
 784 #endif
 785 }
 786
 787 static void take_affinity(void *info)
 788 {
 789         struct fiq_debugger_state *state = info;
 790         struct cpumask cpumask;
 791
 792         cpumask_clear(&cpumask);
 793         cpumask_set_cpu(get_cpu(), &cpumask);
 794
 795         irq_set_affinity(state->uart_irq, &cpumask);
 796 }
 797
 798 static void switch_cpu(struct fiq_debugger_state *state, int cpu)
 799 {
 800         if (!debug_have_fiq(state))
 801                 smp_call_function_single(cpu, take_affinity, state, false);
 802 #ifdef CONFIG_ARCH_ROCKCHIP
 803         else {
 804                 struct cpumask cpumask;
 805
 806                 if (!cpu_online(cpu)) {
 807                         debug_printf(state, "cpu %d offline\n", cpu);
 808                         return;
 809                 }
 810
 811                 cpumask_clear(&cpumask);
 812                 cpumask_set_cpu(cpu, &cpumask);
 813
 814                 irq_set_affinity(state->fiq, &cpumask);
 815                 irq_set_affinity(state->uart_irq, &cpumask);
 816         }
 817 #endif
 818         state->current_cpu = cpu;
 819 }
 820
 821 static bool debug_fiq_exec(struct fiq_debugger_state *state,
 822                         const char *cmd, unsigned *regs, void *svc_sp)
 823 {
 824         bool signal_helper = false;
 825
 826         if (!strcmp(cmd, "help") || !strcmp(cmd, "?")) {
 827                 debug_help(state);
 828         } else if (!strcmp(cmd, "pc")) {
 829                 debug_printf(state, " pc %08x cpsr %08x mode %s\n",
 830                         regs[15], regs[16], mode_name(regs[16]));
 831         } else if (!strcmp(cmd, "regs")) {
 832                 dump_regs(state, regs);
 833         } else if (!strcmp(cmd, "allregs")) {
 834                 dump_allregs(state, regs);
 835         } else if (!strcmp(cmd, "bt")) {
 836                 dump_stacktrace(state, (struct pt_regs *)regs, 100, svc_sp);
 837         } else if (!strncmp(cmd, "reset", 5)) {
 838                 cmd += 5;
 839                 while (*cmd == ' ')
 840                         cmd++;
 841                 if (*cmd) {
 842                         char tmp_cmd[32];
 843                         strlcpy(tmp_cmd, cmd, sizeof(tmp_cmd));
 844                         machine_restart(tmp_cmd);
 845                 } else {
 846                         machine_restart(NULL);
 847                 }
 848         } else if (!strcmp(cmd, "irqs")) {
 849                 dump_irqs(state);
 850         } else if (!strcmp(cmd, "kmsg")) {
 851                 dump_kernel_log(state);
 852 #ifdef CONFIG_RK_LAST_LOG
 853         } else if (!strcmp(cmd, "last_kmsg")) {
 854                 dump_last_kernel_log(state);
 855 #endif
 856         } else if (!strcmp(cmd, "version")) {
 857                 debug_printf(state, "%s\n", linux_banner);
 858         } else if (!strcmp(cmd, "sleep")) {
 859                 state->no_sleep = false;
 860                 debug_printf(state, "enabling sleep\n");
 861         } else if (!strcmp(cmd, "nosleep")) {
 862                 state->no_sleep = true;
 863                 debug_printf(state, "disabling sleep\n");
 864         } else if (!strcmp(cmd, "console")) {
 865                 debug_printf(state, "console mode\n");
 866                 debug_uart_flush(state);
 867                 state->console_enable = true;
 868         } else if (!strcmp(cmd, "cpu")) {
 869                 debug_printf(state, "cpu %d\n", state->current_cpu);
 870         } else if (!strncmp(cmd, "cpu ", 4)) {
 871                 unsigned long cpu = 0;
 872                 if (strict_strtoul(cmd + 4, 10, &cpu) == 0)
 873                         switch_cpu(state, cpu);
 874                 else
 875                         debug_printf(state, "invalid cpu\n");
 876                 debug_printf(state, "cpu %d\n", state->current_cpu);
 877         } else {
 878                 if (state->debug_busy) {
 879                         debug_printf(state,
 880                                 "command processor busy. trying to abort.\n");
 881                         state->debug_abort = -1;
 882                 } else {
 883                         strcpy(state->debug_cmd, cmd);
 884                         state->debug_busy = 1;
 885                 }
 886
 887                 return true;
 888         }
 889         if (!state->console_enable)
 890                 debug_prompt(state);
 891
 892         return signal_helper;
 893 }
 894
 895 static void sleep_timer_expired(unsigned long data)
 896 {
 897         struct fiq_debugger_state *state = (struct fiq_debugger_state *)data;
 898         unsigned long flags;
 899
 900         spin_lock_irqsave(&state->sleep_timer_lock, flags);
 901         if (state->uart_enabled && !state->no_sleep) {
 902                 if (state->debug_enable && !state->console_enable) {
 903                         state->debug_enable = false;
 904                         debug_printf_nfiq(state, "suspending fiq debugger\n");
 905                 }
 906                 state->ignore_next_wakeup_irq = true;
 907                 debug_uart_disable(state);
 908                 state->uart_enabled = false;
 909                 enable_wakeup_irq(state);
 910         }
 911         wake_unlock(&state->debugger_wake_lock);
 912         spin_unlock_irqrestore(&state->sleep_timer_lock, flags);
 913 }
 914
 915 static void handle_wakeup(struct fiq_debugger_state *state)
 916 {
 917         unsigned long flags;
 918
 919         spin_lock_irqsave(&state->sleep_timer_lock, flags);
 920         if (state->wakeup_irq >= 0 && state->ignore_next_wakeup_irq) {
 921                 state->ignore_next_wakeup_irq = false;
 922         } else if (!state->uart_enabled) {
 923                 wake_lock(&state->debugger_wake_lock);
 924                 debug_uart_enable(state);
 925                 state->uart_enabled = true;
 926                 disable_wakeup_irq(state);
 927                 mod_timer(&state->sleep_timer, jiffies + HZ / 2);
 928         }
 929         spin_unlock_irqrestore(&state->sleep_timer_lock, flags);
 930 }
 931
 932 static irqreturn_t wakeup_irq_handler(int irq, void *dev)
 933 {
 934         struct fiq_debugger_state *state = dev;
 935
 936         if (!state->no_sleep)
 937                 debug_puts(state, "WAKEUP\n");
 938         handle_wakeup(state);
 939
 940         return IRQ_HANDLED;
 941 }
 942
 943 static void debug_handle_console_irq_context(struct fiq_debugger_state *state)
 944 {
 945 #if defined(CONFIG_FIQ_DEBUGGER_CONSOLE)
 946         if (state->tty_port.ops) {
 947                 int i;
 948                 int count = fiq_debugger_ringbuf_level(state->tty_rbuf);
 949                 for (i = 0; i < count; i++) {
 950                         int c = fiq_debugger_ringbuf_peek(state->tty_rbuf, 0);
 951                         tty_insert_flip_char(&state->tty_port, c, TTY_NORMAL);
 952                         if (!fiq_debugger_ringbuf_consume(state->tty_rbuf, 1))
 953                                 pr_warn("fiq tty failed to consume byte\n");
 954                 }
 955                 tty_flip_buffer_push(&state->tty_port);
 956         }
 957 #endif
 958 }
 959
 960 static void debug_handle_irq_context(struct fiq_debugger_state *state)
 961 {
 962         if (!state->no_sleep) {
 963                 unsigned long flags;
 964
 965                 spin_lock_irqsave(&state->sleep_timer_lock, flags);
 966                 wake_lock(&state->debugger_wake_lock);
 967                 mod_timer(&state->sleep_timer, jiffies + HZ * 5);
 968                 spin_unlock_irqrestore(&state->sleep_timer_lock, flags);
 969         }
 970         debug_handle_console_irq_context(state);
 971         if (state->debug_busy) {
 972                 debug_irq_exec(state, state->debug_cmd);
 973                 if (!state->console_enable)
 974                         debug_prompt(state);
 975                 state->debug_busy = 0;
 976         }
 977 }
 978
 979 static int debug_getc(struct fiq_debugger_state *state)
 980 {
 981         return state->pdata->uart_getc(state->pdev);
 982 }
 983
 984
 985 static int debug_cmd_check_back(struct fiq_debugger_state *state, char c)
 986 {
 987         char *s;
 988         int i = 0;
 989         if (c == 'A') {
 990                 state->back_pointer = (state->back_pointer-1) & CMD_COUNT;
 991                 if (state->back_pointer != state->current_pointer) {
 992                         s = state->cmd_buf[state->back_pointer];
 993                         if (*s != 0) {
 994                                 for(i = 0; i < strlen(state->debug_buf)-1; i++) {
 995                                         state->pdata->uart_putc(state->pdev, 8);
 996                                         state->pdata->uart_putc(state->pdev, ' ');
 997                                         state->pdata->uart_putc(state->pdev, 8);
 998                                 }
 999                                 memset(state->debug_buf, 0, DEBUG_MAX);
1000                                 strcpy(state->debug_buf, s);
1001                                 state->debug_count = strlen(state->debug_buf);
1002                                 debug_printf(state, state->debug_buf);
1003                         } else {
1004                                 state->back_pointer = (state->back_pointer+1) & CMD_COUNT;
1005                         }
1006
1007                 } else {
1008                         state->back_pointer = (state->back_pointer+1) & CMD_COUNT;
1009                 }
1010         } else if (c == 'B') {
1011
1012                 if (state->back_pointer != state->current_pointer) {
1013                         state->back_pointer = (state->back_pointer+1) & CMD_COUNT;
1014                         if(state->back_pointer == state->current_pointer){
1015                                 goto cmd_clear;
1016                         } else {
1017                                 s = state->cmd_buf[state->back_pointer];
1018                                 if (*s != 0) {
1019                                         for(i = 0; i < strlen(state->debug_buf)-1; i++) {
1020                                                 state->pdata->uart_putc(state->pdev, 8);
1021                                                 state->pdata->uart_putc(state->pdev, ' ');
1022                                                 state->pdata->uart_putc(state->pdev, 8);
1023                                         }
1024                                         memset(state->debug_buf, 0, DEBUG_MAX);
1025                                         strcpy(state->debug_buf, s);
1026                                         state->debug_count = strlen(state->debug_buf);
1027                                         debug_printf(state, state->debug_buf);
1028                                 }
1029                         }
1030                 } else {
1031 cmd_clear:
1032                         for(i = 0; i < strlen(state->debug_buf)-1; i++) {
1033                                 state->pdata->uart_putc(state->pdev, 8);
1034                                 state->pdata->uart_putc(state->pdev, ' ');
1035                                 state->pdata->uart_putc(state->pdev, 8);
1036                         }
1037                         memset(state->debug_buf, 0, DEBUG_MAX);
1038                         state->debug_count = 0;
1039                 }
1040         }
1041         return 0;
1042 }
1043
1044 static void debug_cmd_tab(struct fiq_debugger_state *state)
1045 {
1046         int i,j;
1047         int count = 0;
1048
1049         for (i = 0; i < ARRAY_SIZE(cmd_buf); i++) {
1050                 cmd_buf[i][15] = 1;
1051         }
1052
1053         for (j = 1; j <= strlen(state->debug_buf); j++) {
1054                 count = 0;
1055                 for (i = 0; i < ARRAY_SIZE(cmd_buf); i++) {
1056                         if (cmd_buf[i][15] == 1) {
1057                                 if (strncmp(state->debug_buf, cmd_buf[i], j)) {
1058                                         cmd_buf[i][15] = 0;
1059                                 } else {
1060                                         count++;
1061                                 }
1062                         }
1063                 }
1064                 if (count == 0)
1065                         break;
1066         }
1067
1068         if (count == 1) {
1069                 for (i = 0; i < ARRAY_SIZE(cmd_buf); i++) {
1070                         if (cmd_buf[i][15] == 1)
1071                                 break;
1072                 }
1073
1074                 for(j = 0; j < strlen(state->debug_buf); j++) {
1075                         state->pdata->uart_putc(state->pdev, 8);
1076                         state->pdata->uart_putc(state->pdev, ' ');
1077                         state->pdata->uart_putc(state->pdev, 8);
1078                 }
1079                 memset(state->debug_buf, 0, DEBUG_MAX);
1080                 strcpy(state->debug_buf, cmd_buf[i]);
1081                 state->debug_count = strlen(state->debug_buf);
1082                 debug_printf(state, state->debug_buf);
1083
1084         }
1085 }
1086
1087 static bool debug_handle_uart_interrupt(struct fiq_debugger_state *state,
1088                         int this_cpu, void *regs, void *svc_sp)
1089 {
1090         int c;
1091         static int last_c;
1092         int count = 0;
1093         bool signal_helper = false;
1094
1095         if (this_cpu != state->current_cpu) {
1096                 if (state->in_fiq)
1097                         return false;
1098
1099                 if (atomic_inc_return(&state->unhandled_fiq_count) !=
1100                                         MAX_UNHANDLED_FIQ_COUNT)
1101                         return false;
1102
1103                 debug_printf(state, "fiq_debugger: cpu %d not responding, "
1104                         "reverting to cpu %d\n", state->current_cpu,
1105                         this_cpu);
1106
1107                 atomic_set(&state->unhandled_fiq_count, 0);
1108                 switch_cpu(state, this_cpu);
1109                 return false;
1110         }
1111
1112         state->in_fiq = true;
1113
1114         while ((c = debug_getc(state)) != FIQ_DEBUGGER_NO_CHAR) {
1115                 count++;
1116                 if (!state->debug_enable) {
1117                         if ((c == 13) || (c == 10)) {
1118                                 state->debug_enable = true;
1119                                 state->debug_count = 0;
1120                                 debug_prompt(state);
1121                         }
1122                 } else if (c == FIQ_DEBUGGER_BREAK) {
1123                         state->console_enable = false;
1124 #ifdef CONFIG_ARCH_ROCKCHIP
1125                         debug_puts(state, "\nWelcome to ");
1126 #endif
1127                         debug_puts(state, "fiq debugger mode\n");
1128                         state->debug_count = 0;
1129 #ifdef CONFIG_ARCH_ROCKCHIP
1130                         debug_puts(state, "Enter ? to get command help\n");
1131                         state->back_pointer = CMD_COUNT;
1132                         state->current_pointer = CMD_COUNT;
1133                         memset(state->cmd_buf, 0, (CMD_COUNT+1)*DEBUG_MAX);
1134 #endif
1135                         debug_prompt(state);
1136 #ifdef CONFIG_FIQ_DEBUGGER_CONSOLE
1137                 } else if (state->console_enable && state->tty_rbuf) {
1138                         fiq_debugger_ringbuf_push(state->tty_rbuf, c);
1139                         signal_helper = true;
1140 #endif
1141 #ifdef CONFIG_ARCH_ROCKCHIP
1142                 } else if (last_c == '[' && (c == 'A' || c == 'B' || c == 'C' || c == 'D')) {
1143                         if (state->debug_count > 0) {
1144                                 state->debug_count--;
1145                                 state->pdata->uart_putc(state->pdev, 8);
1146                                 state->pdata->uart_putc(state->pdev, ' ');
1147                                 state->pdata->uart_putc(state->pdev, 8);
1148                         }
1149                         debug_cmd_check_back(state, c);
1150                         //tab
1151                 } else if (c == 9) {
1152                         debug_cmd_tab(state);
1153 #endif
1154                 } else if ((c >= ' ') && (c < 127)) {
1155                         if (state->debug_count < (DEBUG_MAX - 1)) {
1156                                 state->debug_buf[state->debug_count++] = c;
1157                                 debug_putc(state, c);
1158                         }
1159                 } else if ((c == 8) || (c == 127)) {
1160                         if (state->debug_count > 0) {
1161                                 state->debug_count--;
1162                                 debug_putc(state, 8);
1163                                 debug_putc(state, ' ');
1164                                 debug_putc(state, 8);
1165                         }
1166                 } else if ((c == 13) || (c == 10)) {
1167                         if (c == '\r' || (c == '\n' && last_c != '\r')) {
1168                                 debug_putc(state, '\r');
1169                                 debug_putc(state, '\n');
1170                         }
1171                         if (state->debug_count) {
1172                                 state->debug_buf[state->debug_count] = 0;
1173                                 state->debug_count = 0;
1174 #ifdef CONFIG_ARCH_ROCKCHIP
1175                                 signal_helper |=
1176                                         debug_fiq_exec(state, state->debug_buf,
1177                                                        regs, svc_sp);
1178                                 if (signal_helper == false) {
1179                                         state->current_pointer = (state->current_pointer-1) & CMD_COUNT;
1180                                         if (strcmp(state->cmd_buf[state->current_pointer], state->debug_buf)) {
1181                                                 state->current_pointer = (state->current_pointer+1) & CMD_COUNT;
1182                                                 memset(state->cmd_buf[state->current_pointer], 0, DEBUG_MAX);
1183                                                 strcpy(state->cmd_buf[state->current_pointer], state->debug_buf);
1184                                         }
1185                                         memset(state->debug_buf, 0, DEBUG_MAX);
1186                                         state->current_pointer = (state->current_pointer+1) & CMD_COUNT;
1187                                         state->back_pointer = state->current_pointer;
1188                                 }
1189 #endif
1190                         } else {
1191                                 debug_prompt(state);
1192                         }
1193                 }
1194                 last_c = c;
1195         }
1196         if (!state->console_enable)
1197                 debug_uart_flush(state);
1198         if (state->pdata->fiq_ack)
1199                 state->pdata->fiq_ack(state->pdev, state->fiq);
1200
1201         /* poke sleep timer if necessary */
1202         if (state->debug_enable && !state->no_sleep)
1203                 signal_helper = true;
1204
1205         atomic_set(&state->unhandled_fiq_count, 0);
1206         state->in_fiq = false;
1207
1208         return signal_helper;
1209 }
1210
1211 static void debug_fiq(struct fiq_glue_handler *h, void *regs, void *svc_sp)
1212 {
1213         struct fiq_debugger_state *state =
1214                 container_of(h, struct fiq_debugger_state, handler);
1215         unsigned int this_cpu = THREAD_INFO(svc_sp)->cpu;
1216         bool need_irq;
1217
1218         /* RK2928 USB-UART function, otg dp/dm default in uart status;
1219          * connect with otg cable&usb device, dp/dm will be hi-z status
1220          * and make uart controller enter infinite fiq loop
1221          */
1222 #ifdef CONFIG_RK_USB_UART
1223         if(cpu_is_rk3188()){
1224                 if(!(readl_relaxed(RK_GRF_VIRT + RK3188_GRF_SOC_STATUS0) & (1 << 13))){//id low
1225                         writel_relaxed((0x0300 << 16), RK_GRF_VIRT + RK3188_GRF_UOC0_CON0);   //enter usb phy
1226                 }
1227         }else if(cpu_is_rk3288()){
1228                 if(!(readl_relaxed(RK_GRF_VIRT + RK3288_GRF_SOC_STATUS2) & (1 << 17))){//id low
1229                         writel_relaxed((0x00c0 << 16), RK_GRF_VIRT + RK3288_GRF_UOC0_CON3);//enter usb phy
1230                 }
1231         }
1232 #endif
1233         need_irq = debug_handle_uart_interrupt(state, this_cpu, regs, svc_sp);
1234         if (need_irq)
1235                 debug_force_irq(state);
1236 }
1237
1238 /*
1239  * When not using FIQs, we only use this single interrupt as an entry point.
1240  * This just effectively takes over the UART interrupt and does all the work
1241  * in this context.
1242  */
1243 static irqreturn_t debug_uart_irq(int irq, void *dev)
1244 {
1245         struct fiq_debugger_state *state = dev;
1246         bool not_done;
1247
1248         handle_wakeup(state);
1249
1250         /* handle the debugger irq in regular context */
1251         not_done = debug_handle_uart_interrupt(state, smp_processor_id(),
1252                                               get_irq_regs(),
1253                                               current_thread_info());
1254         if (not_done)
1255                 debug_handle_irq_context(state);
1256
1257         return IRQ_HANDLED;
1258 }
1259
1260 /*
1261  * If FIQs are used, not everything can happen in fiq context.
1262  * FIQ handler does what it can and then signals this interrupt to finish the
1263  * job in irq context.
1264  */
1265 static irqreturn_t debug_signal_irq(int irq, void *dev)
1266 {
1267         struct fiq_debugger_state *state = dev;
1268
1269         if (state->pdata->force_irq_ack)
1270                 state->pdata->force_irq_ack(state->pdev, state->signal_irq);
1271
1272         debug_handle_irq_context(state);
1273
1274         return IRQ_HANDLED;
1275 }
1276
1277 static void debug_resume(struct fiq_glue_handler *h)
1278 {
1279         struct fiq_debugger_state *state =
1280                 container_of(h, struct fiq_debugger_state, handler);
1281         if (state->pdata->uart_resume)
1282                 state->pdata->uart_resume(state->pdev);
1283 }
1284
1285 #if defined(CONFIG_FIQ_DEBUGGER_CONSOLE)
1286 struct tty_driver *debug_console_device(struct console *co, int *index)
1287 {
1288         *index = co->index;
1289         return fiq_tty_driver;
1290 }
1291
1292 static void debug_console_write(struct console *co,
1293                                 const char *s, unsigned int count)
1294 {
1295         struct fiq_debugger_state *state;
1296         unsigned long flags;
1297
1298         state = container_of(co, struct fiq_debugger_state, console);
1299
1300         if (!state->console_enable && !state->syslog_dumping)
1301                 return;
1302
1303 #ifdef CONFIG_RK_CONSOLE_THREAD
1304         if (state->pdata->console_write) {
1305                 state->pdata->console_write(state->pdev, s, count);
1306                 return;
1307         }
1308 #endif
1309
1310         debug_uart_enable(state);
1311         spin_lock_irqsave(&state->console_lock, flags);
1312         while (count--) {
1313                 if (*s == '\n')
1314                         debug_putc(state, '\r');
1315                 debug_putc(state, *s++);
1316         }
1317         debug_uart_flush(state);
1318         spin_unlock_irqrestore(&state->console_lock, flags);
1319         debug_uart_disable(state);
1320 }
1321
1322 static struct console fiq_debugger_console = {
1323         .name = "ttyFIQ",
1324         .device = debug_console_device,
1325         .write = debug_console_write,
1326         .flags = CON_PRINTBUFFER | CON_ANYTIME | CON_ENABLED,
1327 };
1328
1329 int fiq_tty_open(struct tty_struct *tty, struct file *filp)
1330 {
1331         int line = tty->index;
1332         struct fiq_debugger_state **states = tty->driver->driver_state;
1333         struct fiq_debugger_state *state = states[line];
1334
1335         return tty_port_open(&state->tty_port, tty, filp);
1336 }
1337
1338 void fiq_tty_close(struct tty_struct *tty, struct file *filp)
1339 {
1340         tty_port_close(tty->port, tty, filp);
1341 }
1342
1343 int  fiq_tty_write(struct tty_struct *tty, const unsigned char *buf, int count)
1344 {
1345         int i;
1346         int line = tty->index;
1347         struct fiq_debugger_state **states = tty->driver->driver_state;
1348         struct fiq_debugger_state *state = states[line];
1349
1350         if (!state->console_enable)
1351                 return count;
1352
1353         debug_uart_enable(state);
1354         spin_lock_irq(&state->console_lock);
1355         for (i = 0; i < count; i++)
1356                 debug_putc(state, *buf++);
1357         spin_unlock_irq(&state->console_lock);
1358         debug_uart_disable(state);
1359
1360         return count;
1361 }
1362
1363 int  fiq_tty_write_room(struct tty_struct *tty)
1364 {
1365         return 16;
1366 }
1367
1368 #ifdef CONFIG_CONSOLE_POLL
1369 static int fiq_tty_poll_init(struct tty_driver *driver, int line, char *options)
1370 {
1371         return 0;
1372 }
1373
1374 static int fiq_tty_poll_get_char(struct tty_driver *driver, int line)
1375 {
1376         struct fiq_debugger_state **states = driver->driver_state;
1377         struct fiq_debugger_state *state = states[line];
1378         int c = NO_POLL_CHAR;
1379
1380         debug_uart_enable(state);
1381         if (debug_have_fiq(state)) {
1382                 int count = fiq_debugger_ringbuf_level(state->tty_rbuf);
1383                 if (count > 0) {
1384                         c = fiq_debugger_ringbuf_peek(state->tty_rbuf, 0);
1385                         fiq_debugger_ringbuf_consume(state->tty_rbuf, 1);
1386                 }
1387         } else {
1388                 c = debug_getc(state);
1389                 if (c == FIQ_DEBUGGER_NO_CHAR)
1390                         c = NO_POLL_CHAR;
1391         }
1392         debug_uart_disable(state);
1393
1394         return c;
1395 }
1396
1397 static void fiq_tty_poll_put_char(struct tty_driver *driver, int line, char ch)
1398 {
1399         struct fiq_debugger_state **states = driver->driver_state;
1400         struct fiq_debugger_state *state = states[line];
1401         debug_uart_enable(state);
1402         debug_putc(state, ch);
1403         debug_uart_disable(state);
1404 }
1405 #endif
1406
1407 static const struct tty_port_operations fiq_tty_port_ops;
1408
1409 static const struct tty_operations fiq_tty_driver_ops = {
1410         .write = fiq_tty_write,
1411         .write_room = fiq_tty_write_room,
1412         .open = fiq_tty_open,
1413         .close = fiq_tty_close,
1414 #ifdef CONFIG_CONSOLE_POLL
1415         .poll_init = fiq_tty_poll_init,
1416         .poll_get_char = fiq_tty_poll_get_char,
1417         .poll_put_char = fiq_tty_poll_put_char,
1418 #endif
1419 };
1420
1421 static int fiq_debugger_tty_init(void)
1422 {
1423         int ret;
1424         struct fiq_debugger_state **states = NULL;
1425
1426         states = kzalloc(sizeof(*states) * MAX_FIQ_DEBUGGER_PORTS, GFP_KERNEL);
1427         if (!states) {
1428                 pr_err("Failed to allocate fiq debugger state structres\n");
1429                 return -ENOMEM;
1430         }
1431
1432         fiq_tty_driver = alloc_tty_driver(MAX_FIQ_DEBUGGER_PORTS);
1433         if (!fiq_tty_driver) {
1434                 pr_err("Failed to allocate fiq debugger tty\n");
1435                 ret = -ENOMEM;
1436                 goto err_free_state;
1437         }
1438
1439         fiq_tty_driver->owner           = THIS_MODULE;
1440         fiq_tty_driver->driver_name     = "fiq-debugger";
1441         fiq_tty_driver->name            = "ttyFIQ";
1442         fiq_tty_driver->type            = TTY_DRIVER_TYPE_SERIAL;
1443         fiq_tty_driver->subtype         = SERIAL_TYPE_NORMAL;
1444         fiq_tty_driver->init_termios    = tty_std_termios;
1445         fiq_tty_driver->flags           = TTY_DRIVER_REAL_RAW |
1446                                           TTY_DRIVER_DYNAMIC_DEV;
1447         fiq_tty_driver->driver_state    = states;
1448
1449         fiq_tty_driver->init_termios.c_cflag =
1450                                         B115200 | CS8 | CREAD | HUPCL | CLOCAL;
1451         fiq_tty_driver->init_termios.c_ispeed = 115200;
1452         fiq_tty_driver->init_termios.c_ospeed = 115200;
1453
1454         tty_set_operations(fiq_tty_driver, &fiq_tty_driver_ops);
1455
1456         ret = tty_register_driver(fiq_tty_driver);
1457         if (ret) {
1458                 pr_err("Failed to register fiq tty: %d\n", ret);
1459                 goto err_free_tty;
1460         }
1461
1462         pr_info("Registered FIQ tty driver\n");
1463         return 0;
1464
1465 err_free_tty:
1466         put_tty_driver(fiq_tty_driver);
1467         fiq_tty_driver = NULL;
1468 err_free_state:
1469         kfree(states);
1470         return ret;
1471 }
1472
1473 static int fiq_debugger_tty_init_one(struct fiq_debugger_state *state)
1474 {
1475         int ret;
1476         struct device *tty_dev;
1477         struct fiq_debugger_state **states = fiq_tty_driver->driver_state;
1478
1479         states[state->pdev->id] = state;
1480
1481         state->tty_rbuf = fiq_debugger_ringbuf_alloc(1024);
1482         if (!state->tty_rbuf) {
1483                 pr_err("Failed to allocate fiq debugger ringbuf\n");
1484                 ret = -ENOMEM;
1485                 goto err;
1486         }
1487
1488         tty_port_init(&state->tty_port);
1489         state->tty_port.ops = &fiq_tty_port_ops;
1490
1491         tty_dev = tty_port_register_device(&state->tty_port, fiq_tty_driver,
1492                                            state->pdev->id, &state->pdev->dev);
1493         if (IS_ERR(tty_dev)) {
1494                 pr_err("Failed to register fiq debugger tty device\n");
1495                 ret = PTR_ERR(tty_dev);
1496                 goto err;
1497         }
1498
1499         device_set_wakeup_capable(tty_dev, 1);
1500
1501         pr_info("Registered fiq debugger ttyFIQ%d\n", state->pdev->id);
1502
1503         return 0;
1504
1505 err:
1506         fiq_debugger_ringbuf_free(state->tty_rbuf);
1507         state->tty_rbuf = NULL;
1508         return ret;
1509 }
1510 #endif
1511
1512 static int fiq_debugger_dev_suspend(struct device *dev)
1513 {
1514         struct platform_device *pdev = to_platform_device(dev);
1515         struct fiq_debugger_state *state = platform_get_drvdata(pdev);
1516
1517         if (state->pdata->uart_dev_suspend)
1518                 return state->pdata->uart_dev_suspend(pdev);
1519         return 0;
1520 }
1521
1522 static int fiq_debugger_dev_resume(struct device *dev)
1523 {
1524         struct platform_device *pdev = to_platform_device(dev);
1525         struct fiq_debugger_state *state = platform_get_drvdata(pdev);
1526
1527         if (state->pdata->uart_dev_resume)
1528                 return state->pdata->uart_dev_resume(pdev);
1529         return 0;
1530 }
1531
1532 static int fiq_debugger_probe(struct platform_device *pdev)
1533 {
1534         int ret;
1535         struct fiq_debugger_pdata *pdata = dev_get_platdata(&pdev->dev);
1536         struct fiq_debugger_state *state;
1537         int fiq;
1538         int uart_irq;
1539
1540         if (pdev->id >= MAX_FIQ_DEBUGGER_PORTS)
1541                 return -EINVAL;
1542
1543         if (!pdata->uart_getc || !pdata->uart_putc)
1544                 return -EINVAL;
1545         if ((pdata->uart_enable && !pdata->uart_disable) ||
1546             (!pdata->uart_enable && pdata->uart_disable))
1547                 return -EINVAL;
1548
1549         fiq = platform_get_irq_byname(pdev, "fiq");
1550         uart_irq = platform_get_irq_byname(pdev, "uart_irq");
1551
1552         /* uart_irq mode and fiq mode are mutually exclusive, but one of them
1553          * is required */
1554         if ((uart_irq < 0 && fiq < 0) || (uart_irq >= 0 && fiq >= 0))
1555                 return -EINVAL;
1556         if (fiq >= 0 && !pdata->fiq_enable)
1557                 return -EINVAL;
1558
1559         state = kzalloc(sizeof(*state), GFP_KERNEL);
1560         setup_timer(&state->sleep_timer, sleep_timer_expired,
1561                     (unsigned long)state);
1562         state->pdata = pdata;
1563         state->pdev = pdev;
1564         state->no_sleep = initial_no_sleep;
1565         state->debug_enable = initial_debug_enable;
1566         state->console_enable = initial_console_enable;
1567
1568         state->fiq = fiq;
1569         state->uart_irq = uart_irq;
1570         state->signal_irq = platform_get_irq_byname(pdev, "signal");
1571         state->wakeup_irq = platform_get_irq_byname(pdev, "wakeup");
1572
1573         INIT_WORK(&state->work, debug_work);
1574         spin_lock_init(&state->work_lock);
1575
1576         platform_set_drvdata(pdev, state);
1577
1578         spin_lock_init(&state->sleep_timer_lock);
1579
1580         if (state->wakeup_irq < 0 && debug_have_fiq(state))
1581                 state->no_sleep = true;
1582         state->ignore_next_wakeup_irq = !state->no_sleep;
1583
1584         wake_lock_init(&state->debugger_wake_lock,
1585                         WAKE_LOCK_SUSPEND, "serial-debug");
1586
1587         state->clk = clk_get(&pdev->dev, NULL);
1588         if (IS_ERR(state->clk))
1589                 state->clk = NULL;
1590
1591         /* do not call pdata->uart_enable here since uart_init may still
1592          * need to do some initialization before uart_enable can work.
1593          * So, only try to manage the clock during init.
1594          */
1595         if (state->clk)
1596                 clk_enable(state->clk);
1597
1598         if (pdata->uart_init) {
1599                 ret = pdata->uart_init(pdev);
1600                 if (ret)
1601                         goto err_uart_init;
1602         }
1603
1604         debug_printf_nfiq(state, "<hit enter %sto activate fiq debugger>\n",
1605                                 state->no_sleep ? "" : "twice ");
1606
1607         if (debug_have_fiq(state)) {
1608                 state->handler.fiq = debug_fiq;
1609                 state->handler.resume = debug_resume;
1610                 ret = fiq_glue_register_handler(&state->handler);
1611                 if (ret) {
1612                         pr_err("%s: could not install fiq handler\n", __func__);
1613                         goto err_register_fiq;
1614                 }
1615 #ifdef CONFIG_ARCH_ROCKCHIP
1616                 //set state->fiq to secure state, so fiq is avalable
1617                 gic_set_irq_secure(irq_get_irq_data(state->fiq));
1618                 //set state->fiq priority a little higher than other interrupts (normal is 0xa0)
1619                 gic_set_irq_priority(irq_get_irq_data(state->fiq), 0x90);
1620 #endif
1621                 pdata->fiq_enable(pdev, state->fiq, 1);
1622         } else {
1623                 ret = request_irq(state->uart_irq, debug_uart_irq,
1624                                   IRQF_NO_SUSPEND, "debug", state);
1625                 if (ret) {
1626                         pr_err("%s: could not install irq handler\n", __func__);
1627                         goto err_register_irq;
1628                 }
1629
1630                 /* for irq-only mode, we want this irq to wake us up, if it
1631                  * can.
1632                  */
1633                 enable_irq_wake(state->uart_irq);
1634         }
1635
1636         if (state->clk)
1637                 clk_disable(state->clk);
1638
1639         if (state->signal_irq >= 0) {
1640                 ret = request_irq(state->signal_irq, debug_signal_irq,
1641                           IRQF_TRIGGER_RISING, "debug-signal", state);
1642                 if (ret)
1643                         pr_err("serial_debugger: could not install signal_irq");
1644         }
1645
1646         if (state->wakeup_irq >= 0) {
1647                 ret = request_irq(state->wakeup_irq, wakeup_irq_handler,
1648                                   IRQF_TRIGGER_FALLING | IRQF_DISABLED,
1649                                   "debug-wakeup", state);
1650                 if (ret) {
1651                         pr_err("serial_debugger: "
1652                                 "could not install wakeup irq\n");
1653                         state->wakeup_irq = -1;
1654                 } else {
1655                         ret = enable_irq_wake(state->wakeup_irq);
1656                         if (ret) {
1657                                 pr_err("serial_debugger: "
1658                                         "could not enable wakeup\n");
1659                                 state->wakeup_irq_no_set_wake = true;
1660                         }
1661                 }
1662         }
1663         if (state->no_sleep)
1664                 handle_wakeup(state);
1665
1666 #if defined(CONFIG_FIQ_DEBUGGER_CONSOLE)
1667         spin_lock_init(&state->console_lock);
1668         state->console = fiq_debugger_console;
1669         state->console.index = pdev->id;
1670         if (!console_set_on_cmdline)
1671                 add_preferred_console(state->console.name,
1672                         state->console.index, NULL);
1673         register_console(&state->console);
1674         fiq_debugger_tty_init_one(state);
1675 #endif
1676         return 0;
1677
1678 err_register_irq:
1679 err_register_fiq:
1680         if (pdata->uart_free)
1681                 pdata->uart_free(pdev);
1682 err_uart_init:
1683         if (state->clk)
1684                 clk_disable(state->clk);
1685         if (state->clk)
1686                 clk_put(state->clk);
1687         wake_lock_destroy(&state->debugger_wake_lock);
1688         platform_set_drvdata(pdev, NULL);
1689         kfree(state);
1690         return ret;
1691 }
1692
1693 static const struct dev_pm_ops fiq_debugger_dev_pm_ops = {
1694         .suspend        = fiq_debugger_dev_suspend,
1695         .resume         = fiq_debugger_dev_resume,
1696 };
1697
1698 static struct platform_driver fiq_debugger_driver = {
1699         .probe  = fiq_debugger_probe,
1700         .driver = {
1701                 .name   = "fiq_debugger",
1702                 .pm     = &fiq_debugger_dev_pm_ops,
1703         },
1704 };
1705
1706 static int __init fiq_debugger_init(void)
1707 {
1708 #if defined(CONFIG_FIQ_DEBUGGER_CONSOLE)
1709         fiq_debugger_tty_init();
1710 #endif
1711         return platform_driver_register(&fiq_debugger_driver);
1712 }
1713
1714 postcore_initcall(fiq_debugger_init);