drivers/staging/android/fiq_debugger/fiq_debugger.c

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