[PATCH] Entirely untested and not working suspend code

Matthew Garrett mjg59 at srcf.ucam.org
Thu Dec 14 23:50:43 EST 2006


This patch adds code which may just about manage to work in some 
respect. Possibly. I wouldn't really expect it to, though. Assuming I 
haven't done anything stupid, once this patch is applied, 
/sys/power/state should show mem. Echoing mem into it may (or may not) 
cause the machine to sequence into some sort of suspended state.

Caveats:

1) Nothing is done to make sure that the memory is in self-refresh mode. 
That needs fixing.
2) Nothing is done to enable any wakeup events.
3) The wakeup code address is never written to a register, so it's not 
going to resume
4) There's no code in the firmware to resume it anyway
5) There's somebody asleep in the room my OLPC is in, so I can't 
actually test it in any way, shape or form

But what the hell. It builds. The wakeup code is the acpi wakeup code, 
except with the video hackery stuff moved out. With luck, it'll help as 
a basis for doing something more useful.

diff --git a/arch/i386/kernel/Makefile b/arch/i386/kernel/Makefile
index 8b365db..01b1dbb 100644
--- a/arch/i386/kernel/Makefile
+++ b/arch/i386/kernel/Makefile
@@ -44,7 +44,7 @@ EXTRA_AFLAGS   := -traditional
 
 obj-$(CONFIG_SCx200)		+= scx200.o
 obj-$(CONFIG_OLPC)		+= olpc.o
-obj-$(CONFIG_OLPC_PM)		+= olpc-pm.o
+obj-$(CONFIG_OLPC_PM)		+= olpc-pm.o olpc-wakeup.o
 
 # vsyscall.o contains the vsyscall DSO images as __initdata.
 # We must build both images before we can assemble it.
diff --git a/arch/i386/kernel/olpc-pm.c b/arch/i386/kernel/olpc-pm.c
index 01c8adc..6da792a 100644
--- a/arch/i386/kernel/olpc-pm.c
+++ b/arch/i386/kernel/olpc-pm.c
@@ -8,20 +8,27 @@
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/input.h>
+#include <linux/suspend.h>
+#include <linux/bootmem.h>
 #include <asm/io.h>
 
 extern int machine_is_olpc;
 #define PM_IRQ 3
 
-
 #define MSR_LBAR_ACPI		0x5140000E
 #define MSR_LBAR_PMS		0x5140000F
 
+unsigned long olpc_wakeup_address = 0;
+extern char wakeup_start, wakeup_end;
+extern void do_olpc_suspend_lowlevel(void);
+extern unsigned long FASTCALL(olpc_copy_wakeup_routine(unsigned long));
+
 static unsigned long acpi_base;
 static unsigned long pms_base;
 static int sci;
 
 static struct input_dev *pm_inputdev;
+static struct pm_ops olpc_pm_ops;
 
 static int olpc_pm_interrupt(int irq, void *id)
 {
@@ -42,6 +49,73 @@ static int olpc_pm_interrupt(int irq, void *id)
 	return IRQ_HANDLED;
 }
 
+static int olpc_pm_state_valid (suspend_state_t pm_state)
+{
+        if (pm_state == PM_SUSPEND_MEM)
+                return 1;
+        return 0;
+}
+
+static int olpc_pm_enter (suspend_state_t pm_state)
+{
+        /* Only STR is supported */
+        if (pm_state != PM_SUSPEND_MEM)
+                return -EINVAL;
+
+        /* Save CPU state */
+        do_olpc_suspend_lowlevel();
+	return 0;
+}
+
+int asmlinkage olpc_enter_sleep_state (u8 sleep_state)
+{
+        /* PM1_STS = ACPI_BASE + 00h
+           PM1_CNT = ACPI_BASE + 08h */
+
+        uint16_t wake_status = 0;
+	uint16_t pm1_reg = inw(acpi_base + 0x8);
+
+	/* Clear the wake status */
+	outw (inw(acpi_base)|0x80, acpi_base);
+
+        /* Should we be waiting until CLK_ACTIVE is deasserted?
+           See page 172 of the companion device docs */
+
+        /* We want to go to S3. According to the ACPI spec, we just write
+           the values to SLP_TYP. On the Geode, this write is caught by VSA
+           and magic happens - we probably want to port this to native
+           code */
+
+        /* SLP_TYPE is bits 12 to 10 */
+
+        pm1_reg |= 0x3 << 12;
+
+        outw (pm1_reg, acpi_base + 0x8);
+
+        /* Do everything we need in order to allow the machine to go to
+           sleep */
+
+	/* FIXME: Enable wake events */
+
+	/* FIXME: Do stuff like put the memory controller in self refresh
+	   and make sure that the rest of the code can continue executing
+	   despite that... - MSR 2004 in the memory controller*/	
+
+	/* Set SLP_EN */
+
+        pm1_reg |= 0x1 << 13;
+
+        outw (pm1_reg, acpi_base + 0x8);
+
+        /* Spin until we're actually asleep */
+
+        do {
+                wake_status = inw (acpi_base) & 0x8000;
+        } while (!wake_status);
+
+        return 0;
+}
+
 static int __init olpc_pm_init(void)
 {
 	uint32_t lo, hi;
@@ -122,10 +196,22 @@ static int __init olpc_pm_init(void)
 	/* Clear pending interrupt */
 	outw(inw(acpi_base), acpi_base);
 
+	olpc_wakeup_address = (unsigned long)alloc_bootmem_low(PAGE_SIZE);
+
+	/* FIXME: stuff this in a register so the firmware can read it
+	   at resume time */
+
+	if (olpc_wakeup_address) {
+		memcpy((void *)olpc_wakeup_address, &wakeup_start,
+		       &wakeup_end - &wakeup_start);
+		olpc_copy_wakeup_routine(olpc_wakeup_address);
+	}
+
+	pm_set_ops(&olpc_pm_ops);
+
 	return 0;
 }
 
-
 static void olpc_pm_exit(void)
 {
 	/* Disable all events */
@@ -135,6 +221,11 @@ static void olpc_pm_exit(void)
 	input_unregister_device(pm_inputdev);
 }
 
+static struct pm_ops olpc_pm_ops = {
+        .valid = olpc_pm_state_valid,
+        .enter = olpc_pm_enter,
+};
+
 module_init(olpc_pm_init);
 module_exit(olpc_pm_exit);
 
diff --git a/arch/i386/kernel/olpc-wakeup.S b/arch/i386/kernel/olpc-wakeup.S
new file mode 100644
index 0000000..21ef6b2
--- /dev/null
+++ b/arch/i386/kernel/olpc-wakeup.S
@@ -0,0 +1,228 @@
+.text
+#include <linux/linkage.h>
+#include <asm/segment.h>
+#include <asm/page.h>
+
+#
+# wakeup_code runs in real mode, and at unknown address (determined at run-time).
+# Therefore it must only use relative jumps/calls. 
+#
+# Do we need to deal with A20? It is okay: ACPI specs says A20 must be enabled
+#
+# If physical address of wakeup_code is 0x12345, BIOS should call us with
+# cs = 0x1234, eip = 0x05
+# 
+
+ALIGN
+	.align	4096
+ENTRY(wakeup_start)
+wakeup_code:
+	wakeup_code_start = .
+	.code16
+
+ 	movw	$0xb800, %ax
+	movw	%ax,%fs
+
+	cli
+	cld
+
+	# setup data segment
+	movw	%cs, %ax
+	movw	%ax, %ds					# Make ds:0 point to wakeup_start
+	movw	%ax, %ss
+	mov	$(wakeup_stack - wakeup_code), %sp		# Private stack is needed for ASUS board
+
+	pushl	$0						# Kill any dangerous flags
+	popfl
+
+	movl	real_magic - wakeup_code, %eax
+	cmpl	$0x12345678, %eax
+	jne	bogus_real_magic
+
+	# set up page table
+	movl	$swsusp_pg_dir-__PAGE_OFFSET, %eax
+	movl	%eax, %cr3
+
+	testl	$1, real_efer_save_restore - wakeup_code
+	jz	4f
+	# restore efer setting
+	movl	real_save_efer_edx - wakeup_code, %edx
+	movl	real_save_efer_eax - wakeup_code, %eax
+	mov     $0xc0000080, %ecx
+	wrmsr
+4:
+	# make sure %cr4 is set correctly (features, etc)
+	movl	real_save_cr4 - wakeup_code, %eax
+	movl	%eax, %cr4
+	movw	$0xb800, %ax
+	movw	%ax,%fs
+	
+	# need a gdt -- use lgdtl to force 32-bit operands, in case
+	# the GDT is located past 16 megabytes.
+	lgdtl	real_save_gdt - wakeup_code
+
+	movl	real_save_cr0 - wakeup_code, %eax
+	movl	%eax, %cr0
+	jmp 1f
+1:
+
+	movl	real_magic - wakeup_code, %eax
+	cmpl	$0x12345678, %eax
+	jne	bogus_real_magic
+
+	ljmpl	$__KERNEL_CS,$wakeup_pmode_return
+
+real_save_gdt:	.word 0
+		.long 0
+real_save_cr0:	.long 0
+real_save_cr3:	.long 0
+real_save_cr4:	.long 0
+real_magic:	.long 0
+real_efer_save_restore:	.long 0
+real_save_efer_edx: 	.long 0
+real_save_efer_eax: 	.long 0
+
+bogus_real_magic:
+	jmp bogus_real_magic
+
+setbad:	clc
+	ret
+
+_setbad: jmp setbad
+
+	.code32
+	ALIGN
+
+.org	0x800
+wakeup_stack_begin:	# Stack grows down
+
+.org	0xff0		# Just below end of page
+wakeup_stack:
+ENTRY(wakeup_end)
+	
+.org	0x1000
+
+wakeup_pmode_return:
+	movw	$__KERNEL_DS, %ax
+	movw	%ax, %ss
+	movw	%ax, %ds
+	movw	%ax, %es
+	movw	%ax, %fs
+	movw	%ax, %gs
+
+	# reload the gdt, as we need the full 32 bit address
+	lgdt	saved_gdt
+	lidt	saved_idt
+	lldt	saved_ldt
+	ljmp	$(__KERNEL_CS),$1f
+1:
+	movl	%cr3, %eax
+	movl	%eax, %cr3
+	wbinvd
+
+	# and restore the stack ... but you need gdt for this to work
+	movl	saved_context_esp, %esp
+
+	movl	%cs:saved_magic, %eax
+	cmpl	$0x12345678, %eax
+	jne	bogus_magic
+
+	# jump to place where we left off
+	movl	saved_eip,%eax
+	jmp	*%eax
+
+bogus_magic:
+	jmp	bogus_magic
+
+
+##
+# olpc_copy_wakeup_routine
+#
+# Copy the above routine to low memory.
+#
+# Parameters:
+# %eax:	place to copy wakeup routine to
+#
+# Returned address is location of code in low memory (past data and stack)
+#
+ENTRY(olpc_copy_wakeup_routine)
+
+	sgdt	saved_gdt
+	sidt	saved_idt
+	sldt	saved_ldt
+	str	saved_tss
+
+	movl	nx_enabled, %edx
+	movl	%edx, real_efer_save_restore - wakeup_start (%eax)
+	testl	$1, real_efer_save_restore - wakeup_start (%eax)
+	jz	2f
+	# save efer setting
+	pushl	%eax
+	movl	%eax, %ebx
+	mov     $0xc0000080, %ecx
+	rdmsr
+	movl	%edx, real_save_efer_edx - wakeup_start (%ebx)
+	movl	%eax, real_save_efer_eax - wakeup_start (%ebx)
+	popl	%eax
+2:
+
+	movl    %cr3, %edx
+	movl    %edx, real_save_cr3 - wakeup_start (%eax)
+	movl    %cr4, %edx
+	movl    %edx, real_save_cr4 - wakeup_start (%eax)
+	movl	%cr0, %edx
+	movl	%edx, real_save_cr0 - wakeup_start (%eax)
+	sgdt    real_save_gdt - wakeup_start (%eax)
+
+	movl	$0x12345678, real_magic - wakeup_start (%eax)
+	movl	$0x12345678, saved_magic
+	ret
+
+save_registers:
+	leal	4(%esp), %eax
+	movl	%eax, saved_context_esp
+	movl %ebx, saved_context_ebx
+	movl %ebp, saved_context_ebp
+	movl %esi, saved_context_esi
+	movl %edi, saved_context_edi
+	pushfl ; popl saved_context_eflags
+
+	movl $ret_point, saved_eip
+	ret
+
+
+restore_registers:
+	movl saved_context_ebp, %ebp
+	movl saved_context_ebx, %ebx
+	movl saved_context_esi, %esi
+	movl saved_context_edi, %edi
+	pushl saved_context_eflags ; popfl
+	ret	
+
+ENTRY(do_olpc_suspend_lowlevel)
+	call	save_processor_state
+	call	save_registers
+	pushl	$3
+	call	olpc_enter_sleep_state
+	addl	$4, %esp
+
+#	In case of S3 failure, we'll emerge here.  Jump
+# 	to ret_point to recover
+	jmp	ret_point
+	.p2align 4,,7
+ret_point:
+	call	restore_registers
+	call	restore_processor_state
+	ret
+
+.data
+ALIGN
+ENTRY(saved_magic)	.long	0
+ENTRY(saved_eip)	.long	0
+
+# saved registers
+saved_gdt:	.long	0,0
+saved_idt:	.long	0,0
+saved_ldt:	.long	0
+saved_tss:	.long	0
+

-- 
Matthew Garrett | mjg59 at srcf.ucam.org



More information about the Devel mailing list