Add nanosleep syscall.

9 files changed, 249 insertions, 9 deletions

diff --git a/src/include/kernel/ppcarch.H b/src/include/kernel/ppcarch.H
index 91ee138a2..3d34bf888 100644
--- a/src/include/kernel/ppcarch.H
+++ b/src/include/kernel/ppcarch.H
@@ -74,6 +74,14 @@ inline uint64_t ppc_getDAR()
     return dar;
 }
 
+__attribute__((always_inline))
+inline uint64_t ppc_getTB()
+{
+    register uint64_t tb = 0;
+    asm volatile("mfspr %0, 268" : "=r" (tb));
+    return tb;
+}
+
 
 
 #endif
diff --git a/src/include/kernel/syscalls.H b/src/include/kernel/syscalls.H
index a33a0cef7..330db6922 100644
--- a/src/include/kernel/syscalls.H
+++ b/src/include/kernel/syscalls.H
@@ -28,6 +28,8 @@ namespace Systemcalls
 	MMIO_MAP,
 	MMIO_UNMAP,
 
+	TIME_NANOSLEEP,
+
 	SYSCALL_MAX
     };
 };
diff --git a/src/include/kernel/timemgr.H b/src/include/kernel/timemgr.H
index 58381263f..78de82216 100644
--- a/src/include/kernel/timemgr.H
+++ b/src/include/kernel/timemgr.H
@@ -2,25 +2,58 @@
 #define __KERNEL_TIMEMGR_H
 
 #include <kernel/types.h>
+#include <kernel/ppcarch.H>
+
+#include <util/locked/pqueue.H>
+#include <kernel/spinlock.H>
+
+class Scheduler;
+
+struct _TimeManager_Delay_t
+{
+    _TimeManager_Delay_t * next;
+    _TimeManager_Delay_t * prev;    
+    uint64_t key;
+    task_t* task;
+};
 
 class TimeManager
 {
     public:
+	enum
+	    {
+		TIMESLICE_PER_SEC = 1000,
+	    };
+
 	static void init();
 	static uint64_t getTimeSliceCount()
 	    {
-		return iv_timeSlice;
+		return iv_timebaseFreq / TIMESLICE_PER_SEC;
 	    };
 
+	static uint64_t getCurrentTimeBase()
+	    {
+		return ppc_getTB();
+	    };
+
+	static uint64_t convertSecToTicks(uint64_t i_sec, uint64_t i_nsec);
+
+	static void delayTask(task_t* t, uint64_t i_sec, uint64_t i_nsec);
+	static void checkReleaseTasks(Scheduler* s);
+
     protected:
-	TimeManager() {};
+	TimeManager() : 
+	    iv_taskList() {};
 	~TimeManager() {};
 
     private:
 	void _init();
-
-
-	static uint64_t iv_timeSlice;
+	void _delayTask(task_t* t, uint64_t i_sec, uint64_t i_nsec);
+	void _checkReleaseTasks(Scheduler* s);
+	    
+	Util::Locked::PQueue<_TimeManager_Delay_t, uint64_t, 
+			     true, Spinlock> iv_taskList;
+	static uint64_t iv_timebaseFreq;
 };
 
 #endif
diff --git a/src/include/sys/time.h b/src/include/sys/time.h
new file mode 100644
index 000000000..2ed63710a
--- /dev/null
+++ b/src/include/sys/time.h
@@ -0,0 +1,17 @@
+#ifndef __SYS_TIME_H
+#define __SYS_TIME_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+int nanosleep(uint64_t sec, uint64_t nsec);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/include/util/locked/pqueue.H b/src/include/util/locked/pqueue.H
new file mode 100644
index 000000000..c32e4e718
--- /dev/null
+++ b/src/include/util/locked/pqueue.H
@@ -0,0 +1,112 @@
+#ifndef __UTIL_LOCKED_PQUEUE_H
+#define __UTIL_LOCKED_PQUEUE_H
+
+#include <util/locked/queue.H>
+
+namespace Util
+{
+    namespace Locked
+    {
+	template <typename _T, typename _K,
+		  bool locked = false, typename _S = int>
+	class PQueue : public Queue<_T, locked, _S>
+	{
+	    public:
+		void insert(_T*);
+		_T* remove_if(_K&);
+
+	    private:
+		void bubbleUp(_T*);
+	};
+
+	template <typename _T, typename _K, bool locked, typename _S>
+	void PQueue<_T,_K,locked,_S>::insert(_T* item)
+	{
+	    this->__lock();
+	    
+	    if (this->head == NULL)
+	    {
+		item->next = item->prev = NULL;
+		this->head = this->tail = item;
+	    }
+	    else
+	    {
+		item->prev = NULL;
+		item->next = this->head;
+		this->head = this->head->prev = item;
+
+		bubbleUp(item);
+	    }
+	    
+	    this->__unlock();
+	}
+
+	template <typename _T, typename _K, bool locked, typename _S>
+	_T* PQueue<_T,_K,locked,_S>::remove_if(_K& key)
+	{
+	    _T* item = NULL;
+
+	    this->__lock();
+	    
+	    if ((this->tail != NULL) && (this->tail->key <= key))
+	    {
+		item = this->tail;
+		if (this->head == this->tail)
+		    this->head = this->tail = NULL;
+		else
+		    this->tail = item->prev;
+	    }
+
+	    this->__unlock();
+	    
+	    return item;
+	}
+
+
+	template <typename _T, typename _K, bool locked, typename _S>
+	void PQueue<_T,_K,locked,_S>::bubbleUp(_T* item)
+	{
+	    if (!item->next)
+		return;
+	    
+	    if (item->next->key <= item->key)
+		return;
+
+	    if (this->head == item)
+		this->head = item->next;
+	    if (this->tail == item->next)
+		this->tail = item;
+	    
+	    _T* temp = item->next;
+
+	    if (temp->next)
+	    {
+		temp->next->prev = item;
+		item->next = item->next->next;
+	    }
+	    else
+	    {
+		item->next = NULL;
+	    }
+
+	    if (item->prev)
+	    {
+		item->prev->next = temp;
+		temp->prev = item->prev;
+	    }
+	    else
+	    {
+		temp->prev = NULL;
+	    }
+
+	    temp->next = item;
+	    item->prev = temp;
+
+	    bubbleUp(item);
+	}
+
+
+    };
+};
+
+#endif
diff --git a/src/kernel/syscall.C b/src/kernel/syscall.C
index 3a6f128ed..d9ac13b17 100644
--- a/src/kernel/syscall.C
+++ b/src/kernel/syscall.C
@@ -8,11 +8,13 @@
 #include <kernel/pagemgr.H>
 #include <kernel/usermutex.H>
 #include <kernel/msg.H>
+#include <kernel/timemgr.H>
 
 extern "C"
 void kernel_execute_decrementer()
 {
     Scheduler* s = CpuManager::getCurrentCPU()->scheduler;
+    TimeManager::checkReleaseTasks(s);        
     s->returnRunnable();
     s->setNextRunnable();
 }
@@ -38,6 +40,7 @@ namespace Systemcalls
     void MsgWait(task_t*);
     void MmioMap(task_t*);
     void MmioUnmap(task_t*);
+    void TimeNanosleep(task_t*);
 
     syscall syscalls[] =
 	{
@@ -63,6 +66,8 @@ namespace Systemcalls
 
 	    &MmioMap,
 	    &MmioUnmap,
+
+	    &TimeNanosleep,
 	};
 };
 
@@ -339,5 +344,14 @@ namespace Systemcalls
 	TASK_SETRTN(t, VmmManager::mmioUnmap(ea,pages));
     }
 
+    void TimeNanosleep(task_t* t)
+    {
+	TimeManager::delayTask(t, TASK_GETARG0(t), TASK_GETARG1(t));
+	TASK_SETRTN(t, 0);
+
+	Scheduler* s = t->cpu->scheduler;
+	s->setNextRunnable();
+    }
+
 
 };
diff --git a/src/kernel/timemgr.C b/src/kernel/timemgr.C
index 73a920524..3871e1ceb 100644
--- a/src/kernel/timemgr.C
+++ b/src/kernel/timemgr.C
@@ -1,7 +1,8 @@
 #include <kernel/timemgr.H>
+#include <kernel/scheduler.H>
 #include <util/singleton.H>
 
-uint64_t TimeManager::iv_timeSlice = 0xFFFFFFFF;
+uint64_t TimeManager::iv_timebaseFreq = 0xFFFFFFFF;
 
 void TimeManager::init()
 {
@@ -10,6 +11,50 @@ void TimeManager::init()
 
 void TimeManager::_init()
 {
-    /*              TB freq  /  Timeslice per Sec */
-    iv_timeSlice = 512000000 / 1000;
+    iv_timebaseFreq = 512000000ULL;
+}
+
+uint64_t TimeManager::convertSecToTicks(uint64_t i_sec, uint64_t i_nsec)
+{
+    // This code will handle times almost up to a year without overflowing a
+    // uint64.  This should be more than sufficient for our purposes.
+    
+    // Result = ((sec * 10^9 + nsec) * tb) / 10^9.
+    uint64_t result = ((i_sec * 1000000000ULL) + i_nsec);
+    result *= (iv_timebaseFreq / 1000000);
+    result /= 1000;
+    return result;
+}
+
+void TimeManager::delayTask(task_t* t, uint64_t i_sec, uint64_t i_nsec)
+{
+    Singleton<TimeManager>::instance()._delayTask(t,i_sec,i_nsec);
+}
+
+void TimeManager::_delayTask(task_t* t, uint64_t i_sec, uint64_t i_nsec)
+{
+    _TimeManager_Delay_t* node = new _TimeManager_Delay_t();
+
+    node->key = this->getCurrentTimeBase() + 
+		this->convertSecToTicks(i_sec, i_nsec);
+    node->task = t;
+
+    iv_taskList.insert(node);
+}
+
+void TimeManager::checkReleaseTasks(Scheduler* s)
+{
+    Singleton<TimeManager>::instance()._checkReleaseTasks(s);
+}
+
+void TimeManager::_checkReleaseTasks(Scheduler* s)
+{
+    uint64_t l_currentTime = getCurrentTimeBase();
+    _TimeManager_Delay_t* node = NULL;
+
+    while(NULL != (node = iv_taskList.remove_if(l_currentTime)))
+    {
+	s->addTask(node->task);
+	delete node;
+    }
 }
diff --git a/src/lib/makefile b/src/lib/makefile
index 19d29f7ad..447149e48 100644
--- a/src/lib/makefile
+++ b/src/lib/makefile
@@ -3,7 +3,7 @@ OBJDIR = ${ROOTPATH}/obj/hbicore
 
 OBJS = string.o stdlib.o 
 OBJS += syscall_stub.o syscall_task.o syscall_mutex.o syscall_msg.o
-OBJS += syscall_mmio.o
+OBJS += syscall_mmio.o syscall_time.o
 OBJECTS = $(addprefix ${OBJDIR}/, ${OBJS})
 
 all: ${OBJECTS}
diff --git a/src/lib/syscall_time.C b/src/lib/syscall_time.C
new file mode 100644
index 000000000..5c45fa80b
--- /dev/null
+++ b/src/lib/syscall_time.C
@@ -0,0 +1,9 @@
+#include <sys/time.h>
+#include <sys/syscall.h>
+
+using namespace Systemcalls;
+
+int nanosleep(uint64_t sec, uint64_t nsec)
+{
+    return (int64_t) _syscall2(TIME_NANOSLEEP, (void*)sec, (void*)nsec);
+}