- if (val)
- __invoke_sip_fn_smc(PSCI_SIP_UARTDBG_CFG64, 0,
- 0, UARTDBG_CFG_OSHDL_DEBUG_ENABLE);
- else
- __invoke_sip_fn_smc(PSCI_SIP_UARTDBG_CFG64, 0,
- 0, UARTDBG_CFG_OSHDL_DEBUG_DISABLE);
+ struct arm_smccc_res res;
+
+ fiq_target_cpu = 0;
+
+ /* init fiq debugger callback */
+ sip_fiq_debugger_uart_irq_tf = callback_fn;
+ res = __invoke_sip_fn_smc(SIP_UARTDBG_FN, irq_id,
+ (unsigned long)sip_fiq_debugger_uart_irq_tf_cb,
+ UARTDBG_CFG_INIT);
+ if (IS_SIP_ERROR(res.a0)) {
+ pr_err("%s error: %d\n", __func__, (int)res.a0);
+ return res.a0;
+ }
+
+ /* share memory ioremap */
+ if (!ft_fiq_mem_base) {
+ ft_fiq_mem_phy = res.a1;
+ ft_fiq_mem_base = ioremap(ft_fiq_mem_phy,
+ FIQ_UARTDBG_SHARE_MEM_SIZE);
+ if (!ft_fiq_mem_base) {
+ pr_err("%s: share memory ioremap failed\n", __func__);
+ return -ENOMEM;
+ }
+ }
+
+ fiq_sip_enabled = 1;
+
+ return SIP_RET_SUCCESS;
+}
+
+int sip_fiq_debugger_switch_cpu(u32 cpu)
+{
+ struct arm_smccc_res res;
+
+ fiq_target_cpu = cpu;
+ res = __invoke_sip_fn_smc(SIP_UARTDBG_FN, cpu_logical_map(cpu),
+ 0, UARTDBG_CFG_OSHDL_CPUSW);
+ return res.a0;