From c4c6281fd76657b320bb68a1698d469a620ccb6b Mon Sep 17 00:00:00 2001
From: =?utf8?q?=E9=BB=84=E6=B6=9B?= <huangtao@rock-chips.com>
Date: Fri, 22 Jul 2011 16:19:53 +0800
Subject: [PATCH] rk29: add hr_msleep/hr_msleep_interruptible/usleep

---
 include/linux/delay.h |  9 +++++++
 kernel/timer.c        | 59 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 68 insertions(+)

diff --git a/include/linux/delay.h b/include/linux/delay.h
index a6ecb34cf547..b39e4c9703d0 100644
--- a/include/linux/delay.h
+++ b/include/linux/delay.h
@@ -52,4 +52,13 @@ static inline void ssleep(unsigned int seconds)
 	msleep(seconds * 1000);
 }
 
+#ifdef CONFIG_ARCH_RK29
+void hr_msleep(unsigned int msecs);
+unsigned long hr_msleep_interruptible(unsigned int msecs);
+
+static inline void usleep(unsigned long usecs)
+{
+	usleep_range(usecs, usecs);
+}
+#endif /* CONFIG_ARCH_RK29 */
 #endif /* defined(_LINUX_DELAY_H) */
diff --git a/kernel/timer.c b/kernel/timer.c
index 0015407e18b1..289419bdd4cd 100644
--- a/kernel/timer.c
+++ b/kernel/timer.c
@@ -1712,3 +1712,62 @@ void usleep_range(unsigned long min, unsigned long max)
 	do_usleep_range(min, max);
 }
 EXPORT_SYMBOL(usleep_range);
+
+#ifdef CONFIG_ARCH_RK29
+static void do_nsleep(unsigned int nsecs, struct hrtimer_sleeper *sleeper,
+	int sigs)
+{
+	enum hrtimer_mode mode = HRTIMER_MODE_REL;
+	int state = sigs ? TASK_INTERRUPTIBLE : TASK_UNINTERRUPTIBLE;
+
+	/*
+	 * This is really just a reworked and simplified version
+	 * of do_nanosleep().
+	 */
+	hrtimer_init(&sleeper->timer, CLOCK_MONOTONIC, mode);
+	sleeper->timer._expires = ktime_set(0, nsecs);
+	hrtimer_init_sleeper(sleeper, current);
+
+	do {
+		set_current_state(state);
+		hrtimer_start(&sleeper->timer, sleeper->timer._expires, mode);
+		if (sleeper->task)
+			schedule();
+		hrtimer_cancel(&sleeper->timer);
+		mode = HRTIMER_MODE_ABS;
+	} while (sleeper->task && !(sigs && signal_pending(current)));
+}
+
+/**
+ * msleep - sleep safely even with waitqueue interruptions
+ * @msecs: Time in milliseconds to sleep for
+ */
+void hr_msleep(unsigned int msecs)
+{
+	struct hrtimer_sleeper sleeper;
+
+	do_nsleep(msecs * NSEC_PER_MSEC, &sleeper, 0);
+}
+
+EXPORT_SYMBOL(hr_msleep);
+
+/**
+ * msleep_interruptible - sleep waiting for signals
+ * @msecs: Time in milliseconds to sleep for
+ */
+unsigned long hr_msleep_interruptible(unsigned int msecs)
+{
+	struct hrtimer_sleeper sleeper;
+	ktime_t left;
+
+	do_nsleep(msecs * NSEC_PER_MSEC, &sleeper, 1);
+
+	if (!sleeper.task)
+		return 0;
+	left = ktime_sub(sleeper.timer._expires,
+			 sleeper.timer.base->get_time());
+	return max(((long) ktime_to_ns(left))/(long)NSEC_PER_MSEC, 1L);
+}
+
+EXPORT_SYMBOL(hr_msleep_interruptible);
+#endif /* CONFIG_ARCH_RK29 */
-- 
2.34.1