[PATCH RT] x86/preempt-lazy: fixup should_resched()

[Sebastian Andrzej Siewior <bigeasy@linutronix.de>]

should_resched() returns true if NEED_RESCHED is set and the
preempt_count is 0 _or_ if NEED_RESCHED_LAZY is set ignoring the preempt
counter. Ignoring the preemp counter is wrong. This patch adds this into
account.
While at it, __preempt_count_dec_and_test() ignores preempt_lazy_count
while checking TIF_NEED_RESCHED_LAZY so we this check, too.

Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>
---
 arch/x86/include/asm/preempt.h | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/arch/x86/include/asm/preempt.h b/arch/x86/include/asm/preempt.h
index 190af4271b5c..58fd4ff3f53a 100644
--- a/arch/x86/include/asm/preempt.h
+++ b/arch/x86/include/asm/preempt.h
@@ -89,6 +89,8 @@ static __always_inline bool __preempt_count_dec_and_test(void)
 	if (____preempt_count_dec_and_test())
 		return true;
 #ifdef CONFIG_PREEMPT_LAZY
+	if (current_thread_info()->preempt_lazy_count)
+		return false;
 	return test_thread_flag(TIF_NEED_RESCHED_LAZY);
 #else
 	return false;
@@ -101,8 +103,19 @@ static __always_inline bool __preempt_count_dec_and_test(void)
 static __always_inline bool should_resched(int preempt_offset)
 {
 #ifdef CONFIG_PREEMPT_LAZY
-	return unlikely(raw_cpu_read_4(__preempt_count) == preempt_offset ||
-			test_thread_flag(TIF_NEED_RESCHED_LAZY));
+	u32 tmp;
+
+	tmp = raw_cpu_read_4(__preempt_count);
+	if (tmp == preempt_offset)
+		return true;
+
+	/* preempt count == 0 ? */
+	tmp &= ~PREEMPT_NEED_RESCHED;
+	if (tmp)
+		return false;
+	if (current_thread_info()->preempt_lazy_count)
+		return false;
+	return test_thread_flag(TIF_NEED_RESCHED_LAZY);
 #else
 	return unlikely(raw_cpu_read_4(__preempt_count) == preempt_offset);
 #endif
-- 
2.9.3