Implement spinlocks

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__ */
 

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",

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__ */
+

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__ */
+

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__ */
+

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)
 {

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();
+    }
+}
+