From 3c091323080ed55c3e279adfa1ceb939f4eeabe4 Mon Sep 17 00:00:00 2001
From: Ali Mashtizadeh <mashti@cs.stanford.edu>
Date: Thu, 10 Jul 2014 15:55:32 -0700
Subject: [PATCH] Implement spinlocks

---
 include/string.h       |  1 +
 sys/SConscript         |  1 +
 sys/amd64/atomic.h     | 30 ++++++++++++++++++++
 sys/include/mp.h       | 10 +++++++
 sys/include/spinlock.h | 22 +++++++++++++++
 sys/kern/libc.c        | 13 +++++++++
 sys/kern/spinlock.c    | 64 ++++++++++++++++++++++++++++++++++++++++++
 7 files changed, 141 insertions(+)
 create mode 100644 sys/amd64/atomic.h
 create mode 100644 sys/include/mp.h
 create mode 100644 sys/include/spinlock.h
 create mode 100644 sys/kern/spinlock.c

diff --git a/include/string.h b/include/string.h
index d4625ea..b42fa4c 100644
--- a/include/string.h
+++ b/include/string.h
@@ -10,6 +10,7 @@ void *memset(void *dst, int c, size_t len);
 int strcmp(const char *s1, const char *s2);
 char *strcpy(char *to, const char *from);
 size_t strlen(const char *str);
+char *strncpy(char *to, const char *from, size_t len);
 
 #endif /* __STRING_H__ */
 
diff --git a/sys/SConscript b/sys/SConscript
index 49c5ae4..e6d0edd 100644
--- a/sys/SConscript
+++ b/sys/SConscript
@@ -29,6 +29,7 @@ src_common = [
     "kern/libc.c",
     "kern/palloc.c",
     "kern/printf.c",
+    "kern/spinlock.c",
     "dev/ahci.c",
     "dev/console.c",
     "dev/pci.c",
diff --git a/sys/amd64/atomic.h b/sys/amd64/atomic.h
new file mode 100644
index 0000000..e2f8734
--- /dev/null
+++ b/sys/amd64/atomic.h
@@ -0,0 +1,30 @@
+
+#ifndef __ATOMIC_H__
+#define __ATOMIC_H__
+
+static INLINE uint64_t
+atomic_swap_uint32(volatile uint32_t *dst, uint32_t newval)
+{
+    asm volatile("lock; xchgl %0, %1;"
+	    : "+m" (*dst), "+r" (newval));
+
+    return newval;
+}
+
+static INLINE uint64_t
+atomic_swap_uint64(volatile uint64_t *dst, uint64_t newval)
+{
+    asm volatile("lock; xchgq %0, %1;"
+	    : "+m" (*dst), "+r" (newval));
+
+    return newval;
+}
+
+static inline void
+atomic_set_uint64(volatile uint64_t *dst, uint64_t newval)
+{
+    *dst = newval;
+}
+
+#endif /* __ATOMIC_H__ */
+
diff --git a/sys/include/mp.h b/sys/include/mp.h
new file mode 100644
index 0000000..2fda7c0
--- /dev/null
+++ b/sys/include/mp.h
@@ -0,0 +1,10 @@
+
+#ifndef __MP_H__
+#define __MP_H__
+
+uint32_t LAPIC_CPU();
+
+#define CPU LAPIC_CPU
+
+#endif /* __MP_H__ */
+
diff --git a/sys/include/spinlock.h b/sys/include/spinlock.h
new file mode 100644
index 0000000..64cc7eb
--- /dev/null
+++ b/sys/include/spinlock.h
@@ -0,0 +1,22 @@
+
+#ifndef __SPINLOCK_H__
+#define __SPINLOCK_H__
+
+#define SPINLOCK_NAMELEN    32
+
+typedef struct Spinlock
+{
+    volatile uint64_t	lock;
+    uint64_t		cpu;
+    uint64_t		count;
+    uint64_t		wait;
+    char		name[SPINLOCK_NAMELEN];
+} Spinlock;
+
+void Spinlock_SystemInit();
+void Spinlock_Init(Spinlock *lock, const char *name);
+void Spinlock_Lock(Spinlock *lock);
+void Spinlock_Unlock(Spinlock *lock);
+
+#endif /* __SPINLOCK_H__ */
+
diff --git a/sys/kern/libc.c b/sys/kern/libc.c
index b9ca85f..092e1ef 100644
--- a/sys/kern/libc.c
+++ b/sys/kern/libc.c
@@ -4,6 +4,7 @@
  * All rights reserved.
  */
 
+#include <stdint.h>
 #include <string.h>
 
 char *
@@ -16,6 +17,18 @@ strcpy(char *to, const char *from)
     return save;
 }
 
+char *
+strncpy(char *to, const char *from, size_t length)
+{
+    char *save = to;
+
+    for (; (*to = *from) != '\0' && length > 0; ++from, ++to, length--);
+
+    *to = '\0';
+
+    return save;
+}
+
 int
 strcmp(const char *s1, const char *s2)
 {
diff --git a/sys/kern/spinlock.c b/sys/kern/spinlock.c
new file mode 100644
index 0000000..4393078
--- /dev/null
+++ b/sys/kern/spinlock.c
@@ -0,0 +1,64 @@
+
+#include <stdint.h>
+#include <string.h>
+
+#include <kassert.h>
+#include <kconfig.h>
+#include <mp.h>
+#include <spinlock.h>
+
+#include "../amd64/amd64.h"
+#include "../amd64/amd64op.h"
+#include "../amd64/atomic.h"
+
+uint32_t lockLevel[MAX_CPUS];
+
+void
+Spinlock_SystemInit()
+{
+    int c;
+    for (c = 0; c < MAX_CPUS; c++)
+    {
+	lockLevel[c] = 0;
+    }
+}
+
+void
+Spinlock_Init(Spinlock *lock, const char *name)
+{
+    lock->lock = 0;
+    lock->cpu = 0;
+    lock->count = 0;
+    lock->wait = 0;
+
+    strncpy(&lock->name[0], name, SPINLOCK_NAMELEN);
+}
+
+void
+Spinlock_Lock(Spinlock *lock)
+{
+    disable_interrupts();
+    lockLevel[CPU()]++;
+
+    while (atomic_swap_uint64(&lock->lock, 1) == 1)
+    {
+    }
+
+    lock->cpu = CPU();
+    lock->count++;
+}
+
+void
+Spinlock_Unlock(Spinlock *lock)
+{
+    lock->cpu = 0;
+    
+    atomic_set_uint64(&lock->lock, 0);
+
+    lockLevel[CPU()]--;
+    if (lockLevel[CPU()] == 0)
+    {
+	enable_interrupts();
+    }
+}
+