quark_se: power_mgmt: Fixes a cpu context save bug

The cpu context save function was manipulating stack and
returning to C caller. This can corrupt stack if the calling
function has data saved and it pops before entering deep
sleep. Moved sleep functions into assembly to avoid this.

Jira: ZEP-1345
Change-Id: I8a6d279ec14e42424f764d9ce8cbbef32149fe84
Signed-off-by: Ramesh Thomas <[email protected]>

Author: ramesh-thomas

arch/x86/soc/intel_quark/quark_se/power.c

@@ -30,15 +30,15 @@ uint64_t _pm_save_gdtr;
 uint64_t _pm_save_idtr;
 uint32_t _pm_save_esp;
 
+extern void _power_soc_sleep(void);
+extern void _power_restore_cpu_context(void);
+extern void _power_soc_deep_sleep(void);
+
 #if (defined(CONFIG_SYS_POWER_DEEP_SLEEP))
 static uint32_t  *__x86_restore_info = (uint32_t *)CONFIG_BSP_SHARED_RAM_ADDR;
 
 static void _deep_sleep(enum power_states state)
 {
-	int restore;
-
-	__asm__ volatile ("wbinvd");
-
 	/*
 	 * Setting resume vector inside the restore_cpu_context
 	 * function since we have nothing to do before cpu context
@@ -47,22 +47,20 @@ static void _deep_sleep(enum power_states state)
 	 * can be done before cpu context is restored and control
 	 * transferred to _sys_soc_suspend.
 	 */
-	qm_x86_set_resume_vector(_sys_soc_restore_cpu_context,
+	qm_x86_set_resume_vector(_power_restore_cpu_context,
 				 *__x86_restore_info);
 
-	restore = _sys_soc_save_cpu_context();
+	power_soc_set_x86_restore_flag();
 
-	if (!restore) {
-		power_soc_set_x86_restore_flag();
-
-		switch (state) {
-		case SYS_POWER_STATE_DEEP_SLEEP_1:
-			power_soc_sleep();
-		case SYS_POWER_STATE_DEEP_SLEEP:
-			power_soc_deep_sleep();
-		default:
-			break;
-		}
+	switch (state) {
+	case SYS_POWER_STATE_DEEP_SLEEP_1:
+		_power_soc_sleep();
+		break;
+	case SYS_POWER_STATE_DEEP_SLEEP:
+		_power_soc_deep_sleep();
+		break;
+	default:
+		break;
 	}
 }
 #endif

arch/x86/soc/intel_quark/quark_se/soc_power.S

@@ -23,49 +23,57 @@ GDATA(_pm_save_gdtr)
 GDATA(_pm_save_idtr)
 GDATA(_pm_save_esp)
 
-GTEXT(_sys_soc_save_cpu_context)
-GTEXT(_sys_soc_restore_cpu_context)
 GTEXT(_sys_soc_resume_from_deep_sleep)
+GTEXT(_power_restore_cpu_context)
+GTEXT(_power_soc_sleep)
+GTEXT(_power_soc_deep_sleep)
+
+SECTION_FUNC(TEXT, save_cpu_context)
+	movl %esp, %eax                 /* save ptr to return address */
 
-SECTION_FUNC(TEXT, _sys_soc_save_cpu_context)
-	movl %esp, %eax			/* save ptr to return address */
 	pushf				/* save flags */
 	pusha				/* save GPRs */
-
 	movl %esp, _pm_save_esp		/* save stack ptr */
 	sidtl _pm_save_idtr		/* save idtr */
 	sgdtl _pm_save_gdtr		/* save gdtr */
 
-	pushl (%eax)			/* push return address */
-
-	xorl %eax, %eax			/* 0 indicates saved context */
+	pushl (%eax)                    /* push return address */
 	ret
 
-SECTION_FUNC(TEXT, _sys_soc_restore_cpu_context)
-	/*
-	 * Will transfer control to _sys_power_save_cpu_context,
-	 * from where it will return 1 indicating the function
-	 * is exiting after a context switch.
-	 */
+SECTION_FUNC(TEXT, _power_restore_cpu_context)
 	lgdtl _pm_save_gdtr		/* restore gdtr */
 	lidtl _pm_save_idtr		/* restore idtr */
 	movl _pm_save_esp, %esp		/* restore saved stack ptr */
 	popa				/* restore saved GPRs */
 	popf				/* restore saved flags */
 
 	/*
-	 * At this point context is restored as it was saved
-	 * in _sys_soc_save_cpu_context. The following ret
-	 * will emulate a return from that function. Move 1
-	 * to eax to emulate a return 1. The caller of
-	 * _sys_soc_save_cpu_context will identify it is
-	 * returning from a context restore based on the
-	 * return value = 1.
+	 * At this point the stack contents will be as follows:
+	 *
+	 *          Saved context
+	 * ESP ---> Return address of save_cpu_context
+	 *          Return address of _power_soc_sleep/deep_sleep
+	 *
+	 * We just popped the saved context. Next we pop out the address
+	 * of the caller of save_cpu_context.Then the ret would return
+	 * to caller of _power_soc_sleep or _power_soc_deep_sleep.
+	 *
 	 */
-	xorl %eax, %eax
-	incl %eax
+	addl $4, %esp
 	ret
 
+SECTION_FUNC(TEXT, _power_soc_sleep)
+	call save_cpu_context
+	wbinvd
+	call power_soc_sleep
+	/* Does not return */
+
+SECTION_FUNC(TEXT, _power_soc_deep_sleep)
+	call save_cpu_context
+	wbinvd
+	call power_soc_deep_sleep
+	/* Does not return */
+
 /*
  * This is an example function to handle the deep sleep resume notification
  * in the absence of bootloader context restore support.
@@ -78,8 +86,8 @@ SECTION_FUNC(TEXT, _sys_soc_restore_cpu_context)
  */
 SECTION_FUNC(TEXT, _sys_soc_resume_from_deep_sleep)
 	movl $CONFIG_BSP_SHARED_RAM_ADDR, %eax
-	cmpl $_sys_soc_restore_cpu_context, (%eax)
-	je _sys_soc_restore_cpu_context
+	cmpl $_power_restore_cpu_context, (%eax)
+	je _power_restore_cpu_context
 	ret
 
 #endif

arch/x86/soc/intel_quark/quark_se/soc_power.h

@@ -30,35 +30,6 @@ enum power_states {
 	SYS_POWER_STATE_MAX
 };
 
-/**
- * @brief Save CPU context
- *
- * This function would save the CPU context in the stack. It
- * would also save the idtr and gdtr registers. When context is
- * restored by _sys_soc_restore_cpu_context(), control will be
- * transferred into this function where the context was originally
- * saved.  The return values would indicate whether it is returning
- * after saving context or after a context restore transferred
- * control to it.
- *
- * @retval 0 Indicates it is returning after saving cpu context
- * @retval 1 Indicates cpu context restore transferred control to it.
- */
-int _sys_soc_save_cpu_context(void);
-
-/**
- * @brief Restore CPU context
- *
- * This function would restore the CPU context that was saved in
- * the stack by _sys_soc_save_cpu_context(). It would also restore
- * the idtr and gdtr registers.
- *
- * After context is restored, control will be transferred into
- * _sys_soc_save_cpu_context() function where the context was originally
- * saved.
- */
-FUNC_NORETURN void _sys_soc_restore_cpu_context(void);
-
 /**
  * @brief Put processor into low power state
  *