From 92e42b007430ae4cc6f21083837a0a25f779ecdc Mon Sep 17 00:00:00 2001
From: Matthew Wilcox <matthew@wil.cx>
Date: Sun, 27 Jan 2008 19:48:50 -0500
Subject: [PATCH] m68k: Implement down_killable

Signed-off-by: Matthew Wilcox <matthew@wil.cx>
---
 arch/m68k/kernel/m68k_ksyms.c       |    1 +
 arch/m68k/kernel/semaphore.c        |   20 +++++++++++++++++
 include/asm-m68k/semaphore-helper.h |   41 +++++++++++++++++++++++++++++++++++
 include/asm-m68k/semaphore.h        |   24 ++++++++++++++++++++
 4 files changed, 86 insertions(+), 0 deletions(-)

diff --git a/arch/m68k/kernel/m68k_ksyms.c b/arch/m68k/kernel/m68k_ksyms.c
index 6fc69c7..47f0549 100644
--- a/arch/m68k/kernel/m68k_ksyms.c
+++ b/arch/m68k/kernel/m68k_ksyms.c
@@ -17,6 +17,7 @@ EXPORT_SYMBOL(__muldi3);
 
 EXPORT_SYMBOL(__down_failed);
 EXPORT_SYMBOL(__down_failed_interruptible);
+EXPORT_SYMBOL(__down_failed_killable);
 EXPORT_SYMBOL(__down_failed_trylock);
 EXPORT_SYMBOL(__up_wakeup);
 
diff --git a/arch/m68k/kernel/semaphore.c b/arch/m68k/kernel/semaphore.c
index d12cbbf..735f80d 100644
--- a/arch/m68k/kernel/semaphore.c
+++ b/arch/m68k/kernel/semaphore.c
@@ -126,6 +126,26 @@ int __sched __down_interruptible(struct semaphore * sem)
 	return ret;
 }
 
+int __sched __down_killable(struct semaphore * sem)
+{
+	DECLARE_WAITQUEUE(wait, current);
+	int ret = 0;
+
+	DOWN_HEAD(TASK_KILLABLE)
+
+	ret = waking_non_zero_killable(sem, current);
+	if (ret)
+	{
+		if (ret == 1)
+			/* ret != 0 only if we get interrupted -arca */
+			ret = 0;
+		break;
+	}
+	schedule();
+	DOWN_TAIL(TASK_KILLABLE)
+	return ret;
+}
+
 int __down_trylock(struct semaphore * sem)
 {
 	return waking_non_zero_trylock(sem);
diff --git a/include/asm-m68k/semaphore-helper.h b/include/asm-m68k/semaphore-helper.h
index eef30ba..32bd47b 100644
--- a/include/asm-m68k/semaphore-helper.h
+++ b/include/asm-m68k/semaphore-helper.h
@@ -101,6 +101,47 @@ next:
 	return ret;
 }
 
+static inline int waking_non_zero_killable(struct semaphore *sem,
+						struct task_struct *tsk)
+{
+	int ret;
+#ifndef CONFIG_RMW_INSNS
+	unsigned long flags;
+
+	spin_lock_irqsave(&semaphore_wake_lock, flags);
+	ret = 0;
+	if (atomic_read(&sem->waking) > 0) {
+		atomic_dec(&sem->waking);
+		ret = 1;
+	} else if (fatal_signal_pending(tsk)) {
+		atomic_inc(&sem->count);
+		ret = -EINTR;
+	}
+	spin_unlock_irqrestore(&semaphore_wake_lock, flags);
+#else
+	int tmp1, tmp2;
+
+	__asm__ __volatile__
+	  ("1:	movel	%1,%2\n"
+	   "	jle	2f\n"
+	   "	subql	#1,%2\n"
+	   "	casl	%1,%2,%3\n"
+	   "	jne	1b\n"
+	   "	moveq	#1,%0\n"
+	   "	jra	%a4\n"
+	   "2:"
+	   : "=d" (ret), "=d" (tmp1), "=d" (tmp2)
+	   : "m" (sem->waking), "i" (&&next), "0" (0), "1" (sem->waking));
+	if (fatal_signal_pending(tsk)) {
+		atomic_inc(&sem->count);
+		ret = -EINTR;
+	}
+next:
+#endif
+
+	return ret;
+}
+
 /*
  * waking_non_zero_trylock:
  *	1	failed to lock
diff --git a/include/asm-m68k/semaphore.h b/include/asm-m68k/semaphore.h
index 64d6b11..6e28071 100644
--- a/include/asm-m68k/semaphore.h
+++ b/include/asm-m68k/semaphore.h
@@ -58,11 +58,13 @@ static inline void init_MUTEX_LOCKED (struct semaphore *sem)
 
 asmlinkage void __down_failed(void /* special register calling convention */);
 asmlinkage int  __down_failed_interruptible(void  /* params in registers */);
+asmlinkage int  __down_failed_killable(void  /* params in registers */);
 asmlinkage int  __down_failed_trylock(void  /* params in registers */);
 asmlinkage void __up_wakeup(void /* special register calling convention */);
 
 asmlinkage void __down(struct semaphore * sem);
 asmlinkage int  __down_interruptible(struct semaphore * sem);
+asmlinkage int  __down_killable(struct semaphore * sem);
 asmlinkage int  __down_trylock(struct semaphore * sem);
 asmlinkage void __up(struct semaphore * sem);
 
@@ -112,6 +114,28 @@ static inline int down_interruptible(struct semaphore *sem)
 	return result;
 }
 
+static inline int down_killable(struct semaphore *sem)
+{
+	register struct semaphore *sem1 __asm__ ("%a1") = sem;
+	register int result __asm__ ("%d0");
+
+	might_sleep();
+	__asm__ __volatile__(
+		"| atomic killable down operation\n\t"
+		"subql #1,%1@\n\t"
+		"jmi 2f\n\t"
+		"clrl %0\n"
+		"1:\n"
+		LOCK_SECTION_START(".even\n\t")
+		"2:\tpea 1b\n\t"
+		"jbra __down_failed_killable\n"
+		LOCK_SECTION_END
+		: "=d" (result)
+		: "a" (sem1)
+		: "memory");
+	return result;
+}
+
 static inline int down_trylock(struct semaphore *sem)
 {
 	register struct semaphore *sem1 __asm__ ("%a1") = sem;
-- 
1.5.3.8