From a85a4496f70bf320230426afc25d4c88425de09b Mon Sep 17 00:00:00 2001
From: Michal Simek <monstr@monstr.eu>
Date: Tue, 12 Jan 2010 09:55:10 +0100
Subject: [PATCH] --- yaml --- r: 188364 b: refs/heads/master c:
 777537905744c28b02c283692e7f75f5445c1afa h: refs/heads/master v: v3

---
 [refs]                                       |  2 +-
 trunk/arch/microblaze/include/asm/tlbflush.h |  2 +-
 trunk/arch/microblaze/kernel/asm-offsets.c   |  1 +
 trunk/arch/microblaze/kernel/entry.S         | 25 +++++++++++++++++++-
 4 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/[refs] b/[refs]
index df86bf7045a5..fa70872757ee 100644
--- a/[refs]
+++ b/[refs]
@@ -1,2 +1,2 @@
 ---
-refs/heads/master: 733cc2183116b216abb52e709709bb0e626c9a75
+refs/heads/master: 777537905744c28b02c283692e7f75f5445c1afa
diff --git a/trunk/arch/microblaze/include/asm/tlbflush.h b/trunk/arch/microblaze/include/asm/tlbflush.h
index 10ec70cd8735..bcb8b41d55af 100644
--- a/trunk/arch/microblaze/include/asm/tlbflush.h
+++ b/trunk/arch/microblaze/include/asm/tlbflush.h
@@ -23,7 +23,7 @@
 extern void _tlbie(unsigned long address);
 extern void _tlbia(void);
 
-#define __tlbia()	_tlbia()
+#define __tlbia()	{ preempt_disable(); _tlbia(); preempt_enable(); }
 
 static inline void local_flush_tlb_all(void)
 	{ __tlbia(); }
diff --git a/trunk/arch/microblaze/kernel/asm-offsets.c b/trunk/arch/microblaze/kernel/asm-offsets.c
index 7bc7b68f97db..0071260a672c 100644
--- a/trunk/arch/microblaze/kernel/asm-offsets.c
+++ b/trunk/arch/microblaze/kernel/asm-offsets.c
@@ -90,6 +90,7 @@ int main(int argc, char *argv[])
 	DEFINE(TI_FLAGS, offsetof(struct thread_info, flags));
 	DEFINE(TI_ADDR_LIMIT, offsetof(struct thread_info, addr_limit));
 	DEFINE(TI_CPU_CONTEXT, offsetof(struct thread_info, cpu_context));
+	DEFINE(TI_PREEMPT_COUNT, offsetof(struct thread_info, preempt_count));
 	BLANK();
 
 	/* struct cpu_context */
diff --git a/trunk/arch/microblaze/kernel/entry.S b/trunk/arch/microblaze/kernel/entry.S
index 3bad4ff49471..1a6729dde49e 100644
--- a/trunk/arch/microblaze/kernel/entry.S
+++ b/trunk/arch/microblaze/kernel/entry.S
@@ -853,7 +853,30 @@ no_intr_resched:
 	lwi	r1, r1, PT_R1 - PT_SIZE;
 	bri	6f;
 /* MS: Return to kernel state. */
-2:	VM_OFF /* MS: turn off MMU */
+2:
+#ifdef CONFIG_PREEMPT
+	add	r11, r0, CURRENT_TASK;
+	lwi	r11, r11, TS_THREAD_INFO;
+	/* MS: get preempt_count from thread info */
+	lwi	r5, r11, TI_PREEMPT_COUNT;
+	bgti	r5, restore;
+
+	lwi	r5, r11, TI_FLAGS;		/* get flags in thread info */
+	andi	r5, r5, _TIF_NEED_RESCHED;
+	beqi	r5, restore /* if zero jump over */
+
+preempt:
+	/* interrupts are off that's why I am calling preempt_chedule_irq */
+	bralid	r15, preempt_schedule_irq
+	nop
+	add	r11, r0, CURRENT_TASK;	/* Get current task ptr into r11 */
+	lwi	r11, r11, TS_THREAD_INFO;	/* get thread info */
+	lwi	r5, r11, TI_FLAGS;		/* get flags in thread info */
+	andi	r5, r5, _TIF_NEED_RESCHED;
+	bnei	r5, preempt /* if non zero jump to resched */
+restore:
+#endif
+	VM_OFF /* MS: turn off MMU */
 	tophys(r1,r1)
 	lwi	r3, r1, PTO + PT_R3; /* MS: restore saved r3, r4 registers */
 	lwi	r4, r1, PTO + PT_R4;