From 7f7614165dc91d4048b2063b295f2d2f323e9e89 Mon Sep 17 00:00:00 2001 From: Huibin Hong Date: Mon, 21 Nov 2016 18:08:39 +0800 Subject: [PATCH] fiq_debugger: merge from linux 3.10 update some features: 1. rename sip smc function name; 2. add serial hw irq and phyical base address parse; 3. use FIQ_DEBUGGER_TRUST_ZONE for armv7 and armv8. Change-Id: I920899f30cadf1ec8380a2e70f5d1e0e801ec5c2 Signed-off-by: chenjh --- drivers/soc/rockchip/rk_fiq_debugger.c | 197 ++++++++--- drivers/staging/android/fiq_debugger/Kconfig | 8 +- .../android/fiq_debugger/fiq_debugger.c | 324 +++++++++++++++++- .../android/fiq_debugger/fiq_debugger.h | 4 + include/linux/soc/rockchip/rk_fiq_debugger.h | 12 +- 5 files changed, 479 insertions(+), 66 deletions(-) diff --git a/drivers/soc/rockchip/rk_fiq_debugger.c b/drivers/soc/rockchip/rk_fiq_debugger.c index 7db9a8442e9f..d8777bcf3d71 100755 --- a/drivers/soc/rockchip/rk_fiq_debugger.c +++ b/drivers/soc/rockchip/rk_fiq_debugger.c @@ -17,6 +17,8 @@ */ #include +#include +#include #include #include #include @@ -39,7 +41,7 @@ #include #include -#ifdef CONFIG_FIQ_DEBUGGER_EL3_TO_EL1 +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE #include #endif @@ -63,9 +65,9 @@ struct rk_fiq_debugger { }; static int rk_fiq_debugger_id; -static int serial_id; +static int serial_hwirq; -#ifdef CONFIG_FIQ_DEBUGGER_EL3_TO_EL1 +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE static bool tf_fiq_sup; #endif @@ -272,55 +274,156 @@ static void fiq_enable(struct platform_device *pdev, unsigned int irq, bool on) disable_irq(irq); } -#ifdef CONFIG_FIQ_DEBUGGER_EL3_TO_EL1 +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE static struct pt_regs fiq_pt_regs; static void rk_fiq_debugger_switch_cpu(struct platform_device *pdev, unsigned int cpu) { - psci_fiq_debugger_switch_cpu(cpu); + sip_fiq_debugger_switch_cpu(cpu); } static void rk_fiq_debugger_enable_debug(struct platform_device *pdev, bool val) { - psci_fiq_debugger_enable_debug(val); + sip_fiq_debugger_enable_debug(val); } -static void fiq_debugger_uart_irq_tf(void *reg_base, u64 sp_el1) +static void fiq_debugger_uart_irq_tf(struct pt_regs _pt_regs, u64 cpu) { - memcpy(&fiq_pt_regs, reg_base, 8*31); + fiq_pt_regs = _pt_regs; - memcpy(&fiq_pt_regs.pstate, reg_base + 0x110, 8); - if (fiq_pt_regs.pstate & 0x10) - memcpy(&fiq_pt_regs.sp, reg_base + 0xf8, 8); - else - fiq_pt_regs.sp = sp_el1; - - memcpy(&fiq_pt_regs.pc, reg_base + 0x118, 8); - - fiq_debugger_fiq(&fiq_pt_regs); + fiq_debugger_fiq(&fiq_pt_regs, cpu); } -static int fiq_debugger_uart_dev_resume(struct platform_device *pdev) +static int rk_fiq_debugger_uart_dev_resume(struct platform_device *pdev) { struct rk_fiq_debugger *t; t = container_of(dev_get_platdata(&pdev->dev), typeof(*t), pdata); - psci_fiq_debugger_uart_irq_tf_init(serial_id, fiq_debugger_uart_irq_tf); + sip_fiq_debugger_uart_irq_tf_init(serial_hwirq, + fiq_debugger_uart_irq_tf); + return 0; +} + +static int fiq_debugger_cpu_resume_fiq(struct notifier_block *nb, + unsigned long action, void *hcpu) +{ + switch (action) { + case CPU_PM_EXIT: + if ((sip_fiq_debugger_is_enabled()) && + (sip_fiq_debugger_get_target_cpu() == smp_processor_id())) + sip_fiq_debugger_enable_fiq(true, smp_processor_id()); + break; + default: + break; + } + + return NOTIFY_OK; +} + +static int fiq_debugger_cpu_migrate_fiq(struct notifier_block *nb, + unsigned long action, void *hcpu) +{ + int target_cpu, cpu = (long)hcpu; + + switch (action) { + case CPU_DEAD: + if ((sip_fiq_debugger_is_enabled()) && + (sip_fiq_debugger_get_target_cpu() == cpu)) { + target_cpu = cpumask_first(cpu_online_mask); + sip_fiq_debugger_switch_cpu(target_cpu); + } + break; + default: + break; + } + + return NOTIFY_OK; +} + +static struct notifier_block fiq_debugger_pm_notifier = { + .notifier_call = fiq_debugger_cpu_resume_fiq, + .priority = 100, +}; + +static struct notifier_block fiq_debugger_cpu_notifier = { + .notifier_call = fiq_debugger_cpu_migrate_fiq, + .priority = 100, +}; + +static int rk_fiq_debugger_register_cpu_pm_notify(void) +{ + int err; + + err = register_cpu_notifier(&fiq_debugger_cpu_notifier); + if (err) { + pr_err("fiq debugger register cpu notifier failed!\n"); + return err; + } + + err = cpu_pm_register_notifier(&fiq_debugger_pm_notifier); + if (err) { + pr_err("fiq debugger register pm notifier failed!\n"); + return err; + } + + return 0; +} + +static int fiq_debugger_bind_sip_smc(struct rk_fiq_debugger *t, + phys_addr_t phy_base, + int hwirq, + int signal_irq, + unsigned int baudrate) +{ + int err; + + err = sip_fiq_debugger_request_share_memory(); + if (err) { + pr_err("fiq debugger request share memory failed: %d\n", err); + goto exit; + } + + err = rk_fiq_debugger_register_cpu_pm_notify(); + if (err) { + pr_err("fiq debugger register cpu pm notify failed: %d\n", err); + goto exit; + } + + err = sip_fiq_debugger_uart_irq_tf_init(hwirq, + fiq_debugger_uart_irq_tf); + if (err) { + pr_err("fiq debugger bind fiq to trustzone failed: %d\n", err); + goto exit; + } + + t->pdata.uart_dev_resume = rk_fiq_debugger_uart_dev_resume; + t->pdata.switch_cpu = rk_fiq_debugger_switch_cpu; + t->pdata.enable_debug = rk_fiq_debugger_enable_debug; + sip_fiq_debugger_set_print_port(phy_base, baudrate); + + pr_info("fiq debugger fiq mode enabled\n"); + return 0; + +exit: + t->pdata.switch_cpu = NULL; + t->pdata.enable_debug = NULL; + + return err; } #endif -void rk_serial_debug_init(void __iomem *base, int irq, int signal_irq, +void rk_serial_debug_init(void __iomem *base, phys_addr_t phy_base, + int irq, int signal_irq, int wakeup_irq, unsigned int baudrate) { struct rk_fiq_debugger *t = NULL; struct platform_device *pdev = NULL; struct resource *res = NULL; int res_count = 0; -#ifdef CONFIG_FIQ_DEBUGGER_EL3_TO_EL1 - /* tf means trust firmware*/ - int tf_ver = 0; +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE + int ret = 0; #endif if (!base) { @@ -359,24 +462,14 @@ void rk_serial_debug_init(void __iomem *base, int irq, int signal_irq, goto out3; }; -#ifdef CONFIG_FIQ_DEBUGGER_EL3_TO_EL1 - tf_ver = rockchip_psci_smc_get_tf_ver(); - - if (tf_ver >= 0x00010005) - tf_fiq_sup = true; - else - tf_fiq_sup = false; - - if (tf_fiq_sup && (signal_irq > 0)) { - t->pdata.switch_cpu = rk_fiq_debugger_switch_cpu; - t->pdata.enable_debug = rk_fiq_debugger_enable_debug; - t->pdata.uart_dev_resume = fiq_debugger_uart_dev_resume; - psci_fiq_debugger_set_print_port(serial_id, 0); - psci_fiq_debugger_uart_irq_tf_init(serial_id, - fiq_debugger_uart_irq_tf); - } else { - t->pdata.switch_cpu = NULL; - t->pdata.enable_debug = NULL; +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE + if ((signal_irq > 0) && (serial_hwirq > 0)) { + ret = fiq_debugger_bind_sip_smc(t, phy_base, serial_hwirq, + signal_irq, baudrate); + if (ret) + tf_fiq_sup = false; + else + tf_fiq_sup = true; } #endif @@ -389,7 +482,7 @@ void rk_serial_debug_init(void __iomem *base, int irq, int signal_irq, res[0].name = "fiq"; else res[0].name = "uart_irq"; -#elif defined(CONFIG_FIQ_DEBUGGER_EL3_TO_EL1) +#elif defined(CONFIG_FIQ_DEBUGGER_TRUST_ZONE) if (tf_fiq_sup && (signal_irq > 0)) res[0].name = "fiq"; else @@ -451,10 +544,14 @@ static int __init rk_fiq_debugger_init(void) { void __iomem *base; struct device_node *np; unsigned int id, ok = 0; - unsigned int irq, signal_irq = 0, wake_irq = 0; + int irq, signal_irq = 0, wake_irq = 0; unsigned int baudrate = 0, irq_mode = 0; + phys_addr_t phy_base = 0; + int serial_id; struct clk *clk; struct clk *pclk; + struct of_phandle_args oirq; + struct resource res; np = of_find_matching_node(NULL, ids); @@ -477,7 +574,7 @@ static int __init rk_fiq_debugger_init(void) { if (irq_mode == 1) signal_irq = -1; else if (of_property_read_u32(np, "rockchip,signal-irq", &signal_irq)) - signal_irq = -1; + signal_irq = -1; if (of_property_read_u32(np, "rockchip,wake-irq", &wake_irq)) wake_irq = -1; @@ -507,6 +604,14 @@ static int __init rk_fiq_debugger_init(void) { if (!ok) return -EINVAL; + /* parse serial hw irq */ + if (!of_irq_parse_one(np, 0, &oirq)) + serial_hwirq = oirq.args[1] + 32; + + /* parse serial phy base address */ + if (!of_address_to_resource(np, 0, &res)) + phy_base = res.start; + pclk = of_clk_get_by_name(np, "apb_pclk"); clk = of_clk_get_by_name(np, "baudclk"); if (unlikely(IS_ERR(clk)) || unlikely(IS_ERR(pclk))) { @@ -523,8 +628,8 @@ static int __init rk_fiq_debugger_init(void) { base = of_iomap(np, 0); if (base) - rk_serial_debug_init(base, irq, signal_irq, wake_irq, baudrate); - + rk_serial_debug_init(base, phy_base, + irq, signal_irq, wake_irq, baudrate); return 0; } #ifdef CONFIG_FIQ_GLUE diff --git a/drivers/staging/android/fiq_debugger/Kconfig b/drivers/staging/android/fiq_debugger/Kconfig index 7b50df1528b4..38eeac85acc0 100644 --- a/drivers/staging/android/fiq_debugger/Kconfig +++ b/drivers/staging/android/fiq_debugger/Kconfig @@ -42,12 +42,12 @@ config FIQ_DEBUGGER_CONSOLE_DEFAULT_ENABLE If enabled, this puts the fiq debugger into console mode by default. Otherwise, the fiq debugger will start out in debug mode. -config FIQ_DEBUGGER_EL3_TO_EL1 - bool "Uart FIQ is captured by EL3, then passed to EL1" - depends on FIQ_DEBUGGER && ARM64 +config FIQ_DEBUGGER_TRUST_ZONE + bool "Uart FIQ is captured by trust zone, then passed to non-secure world" + depends on FIQ_DEBUGGER default n help - It is for ARM V8 arch. + It is for ARM v7/V8 arch. config FIQ_DEBUGGER_UART_OVERLAY bool "Install uart DT overlay" diff --git a/drivers/staging/android/fiq_debugger/fiq_debugger.c b/drivers/staging/android/fiq_debugger/fiq_debugger.c index c9a38705f3ad..7eb678b6b3eb 100644 --- a/drivers/staging/android/fiq_debugger/fiq_debugger.c +++ b/drivers/staging/android/fiq_debugger/fiq_debugger.c @@ -34,6 +34,11 @@ #include #include #include +#include + +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE +#include +#endif #ifdef CONFIG_FIQ_GLUE #include @@ -50,9 +55,14 @@ #include "fiq_debugger_ringbuf.h" #define DEBUG_MAX 64 +#define CMD_COUNT 0x0f #define MAX_UNHANDLED_FIQ_COUNT 1000000 +#ifdef CONFIG_ARCH_ROCKCHIP +#define MAX_FIQ_DEBUGGER_PORTS 1 +#else #define MAX_FIQ_DEBUGGER_PORTS 4 +#endif struct fiq_debugger_state { #ifdef CONFIG_FIQ_GLUE @@ -76,6 +86,12 @@ struct fiq_debugger_state { char debug_buf[DEBUG_MAX]; int debug_count; +#ifdef CONFIG_ARCH_ROCKCHIP + char cmd_buf[CMD_COUNT + 1][DEBUG_MAX]; + int back_pointer; + int current_pointer; +#endif + bool no_sleep; bool debug_enable; bool ignore_next_wakeup_irq; @@ -100,8 +116,13 @@ struct fiq_debugger_state { bool syslog_dumping; #endif +#ifdef CONFIG_ARCH_ROCKCHIP + unsigned int last_irqs[1024]; + unsigned int last_local_irqs[NR_CPUS][32]; +#else unsigned int last_irqs[NR_IRQS]; unsigned int last_local_timer_irqs[NR_CPUS]; +#endif }; #ifdef CONFIG_FIQ_DEBUGGER_CONSOLE @@ -122,6 +143,10 @@ static bool initial_debug_enable; static bool initial_console_enable; #endif +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE +static struct fiq_debugger_state *state_tf; +#endif + static bool fiq_kgdb_enable; static bool fiq_debugger_disable; @@ -162,17 +187,21 @@ static inline bool fiq_debugger_have_fiq(struct fiq_debugger_state *state) return (state->fiq >= 0); } -#ifdef CONFIG_FIQ_GLUE +#if defined(CONFIG_FIQ_GLUE) || defined(CONFIG_FIQ_DEBUGGER_TRUST_ZONE) static void fiq_debugger_force_irq(struct fiq_debugger_state *state) { unsigned int irq = state->signal_irq; if (WARN_ON(!fiq_debugger_have_fiq(state))) return; + if (irq < 0) + return; + if (state->pdata->force_irq) { state->pdata->force_irq(state->pdev, irq); } else { struct irq_chip *chip = irq_get_chip(irq); + if (chip && chip->irq_retrigger) chip->irq_retrigger(irq_get_irq_data(irq)); } @@ -414,6 +443,8 @@ static void fiq_debugger_work(struct work_struct *work) /* This function CANNOT be called in FIQ context */ static void fiq_debugger_irq_exec(struct fiq_debugger_state *state, char *cmd) { + int invalid_cmd = 0; + if (!strcmp(cmd, "ps")) fiq_debugger_do_ps(state); if (!strcmp(cmd, "sysrq")) @@ -426,8 +457,51 @@ static void fiq_debugger_irq_exec(struct fiq_debugger_state *state, char *cmd) #endif if (!strncmp(cmd, "reboot", 6)) fiq_debugger_schedule_work(state, cmd); +#ifdef CONFIG_ARCH_ROCKCHIP + else { + invalid_cmd = 1; + memset(state->debug_buf, 0, DEBUG_MAX); + } + + if (invalid_cmd == 0) { + state->current_pointer = + (state->current_pointer - 1) & CMD_COUNT; + if (strcmp(state->cmd_buf[state->current_pointer], state->debug_buf)) { + state->current_pointer = + (state->current_pointer + 1) & CMD_COUNT; + memset(state->cmd_buf[state->current_pointer], 0, DEBUG_MAX); + strcpy(state->cmd_buf[state->current_pointer], state->debug_buf); + } + memset(state->debug_buf, 0, DEBUG_MAX); + state->current_pointer = (state->current_pointer + 1) & CMD_COUNT; + state->back_pointer = state->current_pointer; + } +#endif } +#ifdef CONFIG_ARCH_ROCKCHIP +static char cmd_buf[][16] = { + {"pc"}, + {"regs"}, + {"allregs"}, + {"bt"}, + {"reboot"}, + {"irqs"}, + {"kmsg"}, + {"version"}, + {"sleep"}, + {"nosleep"}, + {"console"}, + {"cpu"}, + {"ps"}, + {"sysrq"}, + {"reset"}, +#ifdef CONFIG_KGDB + {"kgdb"}, +#endif +}; +#endif + static void fiq_debugger_help(struct fiq_debugger_state *state) { fiq_debugger_printf(&state->output, @@ -438,7 +512,8 @@ static void fiq_debugger_help(struct fiq_debugger_state *state) " bt Stack trace\n" " reboot [] Reboot with command \n" " reset [] Hard reset with command \n" - " irqs Interupt status\n" + " irqs Interupt status\n"); + fiq_debugger_printf(&state->output, " kmsg Kernel log\n" " version Kernel version\n"); fiq_debugger_printf(&state->output, @@ -464,15 +539,42 @@ static void fiq_debugger_take_affinity(void *info) cpumask_clear(&cpumask); cpumask_set_cpu(get_cpu(), &cpumask); + put_cpu(); irq_set_affinity(state->uart_irq, &cpumask); } static void fiq_debugger_switch_cpu(struct fiq_debugger_state *state, int cpu) { + if (!cpu_online(cpu)) { + fiq_debugger_printf(&state->output, "cpu %d offline\n", cpu); + return; + } + if (!fiq_debugger_have_fiq(state)) smp_call_function_single(cpu, fiq_debugger_take_affinity, state, false); +#ifdef CONFIG_ARCH_ROCKCHIP + else { +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE + if (sip_fiq_debugger_is_enabled()) { + if (state->pdata->switch_cpu) { + state->pdata->switch_cpu(state->pdev, cpu); + state->current_cpu = cpu; + } + return; + } +#else + struct cpumask cpumask; + + cpumask_clear(&cpumask); + cpumask_set_cpu(cpu, &cpumask); + + irq_set_affinity(state->fiq, &cpumask); + irq_set_affinity(state->uart_irq, &cpumask); +#endif + } +#endif state->current_cpu = cpu; } @@ -491,7 +593,11 @@ static bool fiq_debugger_fiq_exec(struct fiq_debugger_state *state, } else if (!strcmp(cmd, "allregs")) { fiq_debugger_dump_allregs(&state->output, regs); } else if (!strcmp(cmd, "bt")) { - fiq_debugger_dump_stacktrace(&state->output, regs, 100, svc_sp); + if (!user_mode((struct pt_regs *)regs)) + fiq_debugger_dump_stacktrace(&state->output, regs, + 100, svc_sp); + else + fiq_debugger_printf(&state->output, "User mode\n"); } else if (!strncmp(cmd, "reset", 5)) { cmd += 5; while (*cmd == ' ') @@ -499,8 +605,6 @@ static bool fiq_debugger_fiq_exec(struct fiq_debugger_state *state, if (*cmd) { char tmp_cmd[32]; strlcpy(tmp_cmd, cmd, sizeof(tmp_cmd)); - blocking_notifier_call_chain(&reboot_notifier_list, - SYS_RESTART, (char *)cmd); machine_restart(tmp_cmd); } else { machine_restart(NULL); @@ -521,6 +625,12 @@ static bool fiq_debugger_fiq_exec(struct fiq_debugger_state *state, fiq_debugger_printf(&state->output, "console mode\n"); fiq_debugger_uart_flush(state); state->console_enable = true; +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE + if (sip_fiq_debugger_is_enabled()) { + if (state->pdata->enable_debug) + state->pdata->enable_debug(state->pdev, false); + } +#endif } else if (!strcmp(cmd, "cpu")) { fiq_debugger_printf(&state->output, "cpu %d\n", state->current_cpu); } else if (!strncmp(cmd, "cpu ", 4)) { @@ -529,6 +639,7 @@ static bool fiq_debugger_fiq_exec(struct fiq_debugger_state *state, fiq_debugger_switch_cpu(state, cpu); else fiq_debugger_printf(&state->output, "invalid cpu\n"); + fiq_debugger_printf(&state->output, "cpu %d\n", state->current_cpu); } else { if (state->debug_busy) { @@ -639,6 +750,105 @@ static int fiq_debugger_getc(struct fiq_debugger_state *state) return state->pdata->uart_getc(state->pdev); } +static int fiq_debugger_cmd_check_back(struct fiq_debugger_state *state, char c) +{ + char *s; + int i = 0; + + if (c == 'A') { + state->back_pointer = (state->back_pointer - 1) & CMD_COUNT; + if (state->back_pointer != state->current_pointer) { + s = state->cmd_buf[state->back_pointer]; + if (*s != 0) { + for (i = 0; i < strlen(state->debug_buf) - 1; i++) { + state->pdata->uart_putc(state->pdev, 8); + state->pdata->uart_putc(state->pdev, ' '); + state->pdata->uart_putc(state->pdev, 8); + } + memset(state->debug_buf, 0, DEBUG_MAX); + strcpy(state->debug_buf, s); + state->debug_count = strlen(state->debug_buf); + fiq_debugger_printf(&state->output, state->debug_buf); + } else { + state->back_pointer = (state->back_pointer + 1) & CMD_COUNT; + } + + } else { + state->back_pointer = (state->back_pointer + 1) & CMD_COUNT; + } + } else if (c == 'B') { + if (state->back_pointer != state->current_pointer) { + state->back_pointer = (state->back_pointer + 1) & CMD_COUNT; + if (state->back_pointer == state->current_pointer) { + goto cmd_clear; + } else { + s = state->cmd_buf[state->back_pointer]; + if (*s != 0) { + for (i = 0; i < strlen(state->debug_buf) - 1; i++) { + state->pdata->uart_putc(state->pdev, 8); + state->pdata->uart_putc(state->pdev, ' '); + state->pdata->uart_putc(state->pdev, 8); + } + memset(state->debug_buf, 0, DEBUG_MAX); + strcpy(state->debug_buf, s); + state->debug_count = strlen(state->debug_buf); + fiq_debugger_printf(&state->output, state->debug_buf); + } + } + } else { +cmd_clear: + for (i = 0; i < strlen(state->debug_buf) - 1; i++) { + state->pdata->uart_putc(state->pdev, 8); + state->pdata->uart_putc(state->pdev, ' '); + state->pdata->uart_putc(state->pdev, 8); + } + memset(state->debug_buf, 0, DEBUG_MAX); + state->debug_count = 0; + } + } + return 0; +} + +static void fiq_debugger_cmd_tab(struct fiq_debugger_state *state) +{ + int i, j; + int count = 0; + + for (i = 0; i < ARRAY_SIZE(cmd_buf); i++) + cmd_buf[i][15] = 1; + + for (j = 1; j <= strlen(state->debug_buf); j++) { + count = 0; + for (i = 0; i < ARRAY_SIZE(cmd_buf); i++) { + if (cmd_buf[i][15] == 1) { + if (strncmp(state->debug_buf, cmd_buf[i], j)) + cmd_buf[i][15] = 0; + else + count++; + } + } + if (count == 0) + break; + } + + if (count == 1) { + for (i = 0; i < ARRAY_SIZE(cmd_buf); i++) { + if (cmd_buf[i][15] == 1) + break; + } + + for (j = 0; j < strlen(state->debug_buf); j++) { + state->pdata->uart_putc(state->pdev, 8); + state->pdata->uart_putc(state->pdev, ' '); + state->pdata->uart_putc(state->pdev, 8); + } + memset(state->debug_buf, 0, DEBUG_MAX); + strcpy(state->debug_buf, cmd_buf[i]); + state->debug_count = strlen(state->debug_buf); + fiq_debugger_printf(&state->output, state->debug_buf); + } +} + static bool fiq_debugger_handle_uart_interrupt(struct fiq_debugger_state *state, int this_cpu, const struct pt_regs *regs, void *svc_sp) { @@ -647,7 +857,8 @@ static bool fiq_debugger_handle_uart_interrupt(struct fiq_debugger_state *state, int count = 0; bool signal_helper = false; - if ((this_cpu != state->current_cpu) && (cpu_online(state->current_cpu))) { + if ((this_cpu != state->current_cpu) && + (cpu_online(state->current_cpu))) { if (state->in_fiq) return false; @@ -680,13 +891,49 @@ static bool fiq_debugger_handle_uart_interrupt(struct fiq_debugger_state *state, } } else if (c == FIQ_DEBUGGER_BREAK) { state->console_enable = false; - fiq_debugger_puts(state, "fiq debugger mode\n"); +#ifdef CONFIG_ARCH_ROCKCHIP + fiq_debugger_puts(state, "\nWelcome to "); +#endif + if (fiq_debugger_have_fiq(state)) + fiq_debugger_puts(state, + "fiq debugger mode\n"); + else + fiq_debugger_puts(state, + "irq debugger mode\n"); state->debug_count = 0; +#ifdef CONFIG_ARCH_ROCKCHIP + fiq_debugger_puts(state, "Enter ? to get command help\n"); + state->back_pointer = CMD_COUNT; + state->current_pointer = CMD_COUNT; + memset(state->cmd_buf, 0, (CMD_COUNT + 1) * DEBUG_MAX); +#endif + +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE + if (sip_fiq_debugger_is_enabled()) { + if (state->pdata->enable_debug) + state->pdata->enable_debug(state->pdev, + true); + } +#endif fiq_debugger_prompt(state); + fiq_debugger_ringbuf_push(state->tty_rbuf, 8); + fiq_debugger_ringbuf_push(state->tty_rbuf, 8); #ifdef CONFIG_FIQ_DEBUGGER_CONSOLE } else if (state->console_enable && state->tty_rbuf) { fiq_debugger_ringbuf_push(state->tty_rbuf, c); signal_helper = true; +#endif +#ifdef CONFIG_ARCH_ROCKCHIP + } else if (last_c == '[' && (c == 'A' || c == 'B' || c == 'C' || c == 'D')) { + if (state->debug_count > 0) { + state->debug_count--; + state->pdata->uart_putc(state->pdev, 8); + state->pdata->uart_putc(state->pdev, ' '); + state->pdata->uart_putc(state->pdev, 8); + } + fiq_debugger_cmd_check_back(state, c); + } else if (c == 9) { + fiq_debugger_cmd_tab(state); #endif } else if ((c >= ' ') && (c < 127)) { if (state->debug_count < (DEBUG_MAX - 1)) { @@ -712,6 +959,23 @@ static bool fiq_debugger_handle_uart_interrupt(struct fiq_debugger_state *state, fiq_debugger_fiq_exec(state, state->debug_buf, regs, svc_sp); +#ifdef CONFIG_ARCH_ROCKCHIP + if (signal_helper == false) { + state->current_pointer = + (state->current_pointer - 1) & CMD_COUNT; + if (strcmp(state->cmd_buf[state->current_pointer], state->debug_buf)) { + state->current_pointer = + (state->current_pointer + 1) & CMD_COUNT; + memset(state->cmd_buf[state->current_pointer], 0, DEBUG_MAX); + strcpy(state->cmd_buf[state->current_pointer], state->debug_buf); + } + memset(state->debug_buf, 0, DEBUG_MAX); + state->current_pointer = + (state->current_pointer + 1) & CMD_COUNT; + state->back_pointer = + state->current_pointer; + } +#endif } else { fiq_debugger_prompt(state); } @@ -749,6 +1013,27 @@ static void fiq_debugger_fiq(struct fiq_glue_handler *h, } #endif +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE +void fiq_debugger_fiq(void *regs, u32 cpu) +{ + struct fiq_debugger_state *state = state_tf; + bool need_irq; + + if (!state) + return; + + if (!user_mode((struct pt_regs *)regs)) + need_irq = fiq_debugger_handle_uart_interrupt(state, + smp_processor_id(), + regs, current_thread_info()); + else + need_irq = fiq_debugger_handle_uart_interrupt(state, cpu, + regs, current_thread_info()); + if (need_irq) + fiq_debugger_force_irq(state); +} +#endif + /* * When not using FIQs, we only use this single interrupt as an entry point. * This just effectively takes over the UART interrupt and does all the work @@ -1122,8 +1407,13 @@ static int fiq_debugger_probe(struct platform_device *pdev) "\n", state->no_sleep ? "" : "twice "); -#ifdef CONFIG_FIQ_GLUE if (fiq_debugger_have_fiq(state)) { +#ifdef CONFIG_FIQ_GLUE +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE + if (sip_fiq_debugger_is_enabled()) { + } else +#endif + { state->handler.fiq = fiq_debugger_fiq; state->handler.resume = fiq_debugger_resume; ret = fiq_glue_register_handler(&state->handler); @@ -1131,11 +1421,19 @@ static int fiq_debugger_probe(struct platform_device *pdev) pr_err("%s: could not install fiq handler\n", __func__); goto err_register_irq; } - +#ifdef CONFIG_ARCH_ROCKCHIP + /* set state->fiq to secure state, so fiq is available */ + gic_set_irq_secure(irq_get_irq_data(state->fiq)); + /* + * set state->fiq priority a little higher than other + * interrupts (normal is 0xa0) + */ + gic_set_irq_priority(irq_get_irq_data(state->fiq), 0x90); +#endif pdata->fiq_enable(pdev, state->fiq, 1); - } else + } #endif - { + } else { ret = request_irq(state->uart_irq, fiq_debugger_uart_irq, IRQF_NO_SUSPEND, "debug", state); if (ret) { @@ -1180,6 +1478,10 @@ static int fiq_debugger_probe(struct platform_device *pdev) if (state->no_sleep) fiq_debugger_handle_wakeup(state); +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE + state_tf = state; +#endif + #if defined(CONFIG_FIQ_DEBUGGER_CONSOLE) spin_lock_init(&state->console_lock); state->console = fiq_debugger_console; diff --git a/drivers/staging/android/fiq_debugger/fiq_debugger.h b/drivers/staging/android/fiq_debugger/fiq_debugger.h index caf75155110f..2e7e33eea862 100644 --- a/drivers/staging/android/fiq_debugger/fiq_debugger.h +++ b/drivers/staging/android/fiq_debugger/fiq_debugger.h @@ -64,6 +64,10 @@ struct fiq_debugger_pdata { void (*console_write)(struct platform_device *pdev, const char *s, unsigned int count); #endif +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE + void (*switch_cpu)(struct platform_device *pdev, u32 cpu); + void (*enable_debug)(struct platform_device *pdev, bool val); +#endif }; #endif diff --git a/include/linux/soc/rockchip/rk_fiq_debugger.h b/include/linux/soc/rockchip/rk_fiq_debugger.h index f519fa8cc3bb..c5ea29302644 100644 --- a/include/linux/soc/rockchip/rk_fiq_debugger.h +++ b/include/linux/soc/rockchip/rk_fiq_debugger.h @@ -2,18 +2,20 @@ #define __PLAT_RK_FIQ_DEBUGGER_H #ifdef CONFIG_FIQ_DEBUGGER -void rk_serial_debug_init(void __iomem *base, int irq, int signal_irq, +void rk_serial_debug_init(void __iomem *base, phys_addr_t phy_base, + int irq, int signal_irq, int wakeup_irq, unsigned int baudrate); #else static inline void -rk_serial_debug_init(void __iomem *base, int irq, int signal_irq, - int wakeup_irq, unsigned int baudrate) +void rk_serial_debug_init(void __iomem *base, phys_addr_t phy_base, + int irq, int signal_irq, + int wakeup_irq, unsigned int baudrate) { } #endif -#ifdef CONFIG_FIQ_DEBUGGER_EL3_TO_EL1 -void fiq_debugger_fiq(void *regs); +#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE +void fiq_debugger_fiq(void *regs, u32 cpu); #endif #endif -- 2.34.1