rk3568 rt init

hc

2023-11-06 e3e12f52b214121840b44c91de5b3e5af5d3eb84

 kernel/drivers/firmware/rockchip_sip.c
..
..
@@ -291,17 +291,20 @@
291
291
  */
292
292
 #ifdef CONFIG_ARM64
293
293
 #define SIP_UARTDBG_FN		SIP_UARTDBG_CFG64
294
+#define SIP_FIQ_DBG_STACK_SIZE	IRQ_STACK_SIZE
294
295
 #else
295
296
 #define SIP_UARTDBG_FN		SIP_UARTDBG_CFG
297
+#define SIP_FIQ_DBG_STACK_SIZE	SZ_8K
298
+
296
299
 static int firmware_64_32bit;
297
300
 #endif
298
301
 
299
302
 static int fiq_sip_enabled;
300
303
 static int fiq_target_cpu;
304
+static unsigned long fiq_stack_top;
301
305
 static phys_addr_t ft_fiq_mem_phy;
302
306
 static void __iomem *ft_fiq_mem_base;
303
-static void (*sip_fiq_debugger_uart_irq_tf)(struct pt_regs *_pt_regs,
304
-					    uint32_t cpu);
307
+static sip_fiq_debugger_uart_irq_tf_cb_t sip_fiq_debugger_uart_irq_tf;
305
308
 static struct pt_regs fiq_pt_regs;
306
309
 
307
310
 int sip_fiq_debugger_is_enabled(void)
..
..
@@ -388,7 +391,7 @@
388
391
 
389
392
 static void sip_fiq_debugger_uart_irq_tf_cb(unsigned long sp_el1,
390
393
 					    unsigned long offset,
391
-					    uint32_t cpu)
394
+					    unsigned long cpu)
392
395
 {
393
396
 	char *cpu_context;
394
397
 
..
..
@@ -403,15 +406,30 @@
403
406
 	__invoke_sip_fn_smc(SIP_UARTDBG_FN, 0, 0, UARTDBG_CFG_OSHDL_TO_OS);
404
407
 }
405
408
 
406
-int sip_fiq_debugger_uart_irq_tf_init(u32 irq_id, void *callback_fn)
409
+int sip_fiq_debugger_uart_irq_tf_init(u32 irq_id, sip_fiq_debugger_uart_irq_tf_cb_t callback_fn)
407
410
 {
408
411
 	struct arm_smccc_res res;
409
412
 
413
+	/* Alloc a page for fiq_debugger's stack */
414
+	if (fiq_stack_top == 0) {
415
+		fiq_stack_top = __get_free_pages(GFP_KERNEL | __GFP_ZERO,
416
+						 get_order(SIP_FIQ_DBG_STACK_SIZE));
417
+		if (fiq_stack_top) {
418
+			fiq_stack_top += SIP_FIQ_DBG_STACK_SIZE;
419
+		} else {
420
+			pr_err("%s: alloc stack failed\n", __func__);
421
+			return -ENOMEM;
422
+		}
423
+	}
424
+
410
425
 	/* init fiq debugger callback */
411
426
 	sip_fiq_debugger_uart_irq_tf = callback_fn;
412
-	res = __invoke_sip_fn_smc(SIP_UARTDBG_FN, irq_id,
413
-				  (unsigned long)sip_fiq_debugger_uart_irq_tf_cb,
414
-				  UARTDBG_CFG_INIT);
427
+	arm_smccc_smc(SIP_UARTDBG_FN,
428
+		      irq_id,
429
+		      (unsigned long)sip_fiq_debugger_uart_irq_tf_cb,
430
+		      UARTDBG_CFG_INIT,
431
+		      fiq_stack_top, 0, 0, 0, &res);
432
+
415
433
 	if (IS_SIP_ERROR(res.a0)) {
416
434
 		pr_err("%s error: %d\n", __func__, (int)res.a0);
417
435
 		return res.a0;