4 files changed, 127 insertions, 0 deletions
diff --git a/src/arch/x86/lib/Makefile.inc b/src/arch/x86/lib/Makefile.inc
index 82f4e62d21..7cdd3c64dd 100644
--- a/src/arch/x86/lib/Makefile.inc
+++ b/src/arch/x86/lib/Makefile.inc
@@ -9,6 +9,8 @@ ramstage-y += memset.c
 ramstage-y += memcpy.c
 ramstage-y += ebda.c
 ramstage-y += rom_media.c
+ramstage-$(CONFIG_COOP_MULTITASKING) += thread.c
+ramstage-$(CONFIG_COOP_MULTITASKING) += thread_switch.S
 
 romstage-$(CONFIG_EARLY_CONSOLE) += romstage_console.c
 romstage-y += cbfs_and_run.c
diff --git a/src/arch/x86/lib/c_start.S b/src/arch/x86/lib/c_start.S
index 1e38acaf02..c725f82670 100644
--- a/src/arch/x86/lib/c_start.S
+++ b/src/arch/x86/lib/c_start.S
@@ -10,6 +10,11 @@
 _stack:
 .space CONFIG_MAX_CPUS*CONFIG_STACK_SIZE
 _estack:
+#if CONFIG_COOP_MULTITASKING
+.global thread_stacks
+thread_stacks:
+.space CONFIG_STACK_SIZE*CONFIG_NUM_THREADS
+#endif
 
 	.section ".textfirst", "ax", @progbits
 	.code32
@@ -45,6 +50,10 @@ _start:
 	/* set new stack */
 	movl	$_estack, %esp
 
+#if CONFIG_COOP_MULTITASKING
+	/* Push the thread pointer. */
+	pushl	$0
+#endif
 	/* Push the cpu index and struct cpu */
 	pushl	$0
 	pushl	$0
diff --git a/src/arch/x86/lib/thread.c b/src/arch/x86/lib/thread.c
new file mode 100644
index 0000000000..06f8a154e2
--- /dev/null
+++ b/src/arch/x86/lib/thread.c
@@ -0,0 +1,58 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2013 Google, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#include <thread.h>
+
+/* The stack frame looks like the following after a pushad instruction. */
+struct pushad_regs {
+	uint32_t edi; /* Offset 0x00 */
+	uint32_t esi; /* Offset 0x04 */
+	uint32_t ebp; /* Offset 0x08 */
+	uint32_t esp; /* Offset 0x0c */
+	uint32_t ebx; /* Offset 0x10 */
+	uint32_t edx; /* Offset 0x14 */
+	uint32_t ecx; /* Offset 0x18 */
+	uint32_t eax; /* Offset 0x1c */
+};
+
+static inline uintptr_t push_stack(uintptr_t cur_stack, uintptr_t value)
+{
+	uintptr_t *addr;
+
+	cur_stack -= sizeof(value);
+	addr = (uintptr_t *)cur_stack;
+	*addr = value;
+	return cur_stack;
+}
+
+void arch_prepare_thread(struct thread *t,
+                         void asmlinkage (*thread_entry)(void *), void *arg)
+{
+	uintptr_t stack = t->stack_current;
+
+	/* Imitate thread_entry(t) with return address of 0. thread_entry()
+	 * is assumed to never return. */
+	stack = push_stack(stack, (uintptr_t)arg);
+	stack = push_stack(stack, (uintptr_t)0);
+	stack = push_stack(stack, (uintptr_t)thread_entry);
+	/* Make room for the registers. Ignore intial values. */
+	stack -= sizeof(struct pushad_regs);
+
+	t->stack_current = stack;
+}
diff --git a/src/arch/x86/lib/thread_switch.S b/src/arch/x86/lib/thread_switch.S
new file mode 100644
index 0000000000..8de1948d46
--- /dev/null
+++ b/src/arch/x86/lib/thread_switch.S
@@ -0,0 +1,58 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2013 Google, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+.code32
+.text
+
+/*
+ * stack layout after pushad:
+ * +------------+
+ * | save stack | <-- esp + 0x28
+ * +------------+
+ * |  new stack | <-- esp + 0x24
+ * +------------+
+ * |  ret addr  | <-- esp + 0x20
+ * +------------+
+ * |    eax     | <-- esp + 0x1c
+ * +------------+
+ * |    ecx     | <-- esp + 0x18
+ * +------------+
+ * |    edx     | <-- esp + 0x14
+ * +------------+
+ * |    ebx     | <-- esp + 0x10
+ * +------------+
+ * |  orig esp  | <-- esp + 0x0c
+ * +------------+
+ * |    ebp     | <-- esp + 0x08
+ * +------------+
+ * |    esi     | <-- esp + 0x04
+ * +------------+
+ * |    edi     | <-- esp + 0x00
+ * +------------+
+ */
+.globl switch_to_thread
+switch_to_thread:
+	pusha
+	/* Save the current stack */
+	movl	0x28(%esp), %ebx
+	movl	%esp, (%ebx)
+	/* Switch to the new stack. */
+	movl	0x24(%esp), %eax
+	movl	%eax, %esp
+	popa
+	ret