CE implement stall command, libsystem Add stall_ms ()

ce/interp.c

@@ -16,8 +16,10 @@
 #include <proc_info.h>
 #include <process.h>
 #include <process_self.h>
+#include <stall.h>
 #include <stddef.h>
 #include <str_status.h>
+#include <strconv.h>
 #include <streams.h>
 #include <string.h>
 #include <system.h>
@@ -325,6 +327,8 @@ static void mkvol (struct context* context, const char* volume, const char* str_
     cprintf (context, "ERROR Could not create volume: %s\n", str_status[-ret]);
 }
 
+static void stall (uint64_t ms) { stall_ms (ms); }
+
 static void help (struct context* context) {
   cprintf (context, "Available commands:\n");
   cprintf (context, "help\n");
@@ -340,6 +344,7 @@ static void help (struct context* context) {
   cprintf (context, "mkvol        <volume> <filesystem> <device>\n");
   cprintf (context, "procinfo\n");
   cprintf (context, "quit\n");
+  cprintf (context, "stall        <milliseconds>\n");
 }
 
 struct cmd_write_proc_ctx {
@@ -414,6 +419,13 @@ static void execute_cmd (struct ast_cmd* cmd, struct context* context, bool run_
       cprintf (context, "ERROR No volume key, filesystem type or device key provided\n");
   } else if (strcmp (cmd->name, "procinfo") == 0) {
     procinfo (context);
+  } else if (strcmp (cmd->name, "stall") == 0) {
+    if (cmd->arg_count == 1) {
+      uint64_t ms = str_to_uint64 (cmd->args[0]);
+      stall (ms);
+    } else {
+      cprintf (context, "ERROR stall requires a timeout argument\n");
+    }
   } else {
     char volume[VOLUME_MAX];
     const char* path;

libsystem/amd64/src.mk

@@ -1,6 +1,8 @@
-c += amd64/syscall.c
+c += amd64/syscall.c \
+		 amd64/stall.c
 
 S += amd64/_start.S
 
 o += amd64/_start.o \
-		 amd64/syscall.o
+		 amd64/syscall.o \
+		 amd64/stall.o

libsystem/amd64/stall.c

@@ -0,0 +1,30 @@
+#include <stall.h>
+#include <stdint.h>
+
+static uint64_t stall_read_tsc (void) {
+  uint32_t lo, hi;
+  __asm__ volatile ("rdtsc" : "=a"(lo), "=d"(hi));
+  return ((uint64_t)hi << 32) | lo;
+}
+
+static uint64_t stall_get_tsc_freq_hz (void) {
+  uint32_t eax, ebx, ecx, edx;
+  __asm__ volatile ("cpuid" : "=a"(eax), "=b"(ebx), "=c"(ecx), "=d"(edx) : "a"(0x15));
+
+  if (eax == 0 || ebx == 0 || ecx == 0)
+    return 2500000000ULL;
+
+  return (uint64_t)ecx * ebx / eax;
+}
+
+void stall_ms (uint64_t ms) {
+  uint64_t freq_hz = stall_get_tsc_freq_hz ();
+
+  uint64_t cycles = freq_hz / 1000;
+  uint64_t wait_cycles = ms * cycles;
+
+  uint64_t now = stall_read_tsc ();
+
+  while ((stall_read_tsc () - now) < wait_cycles)
+    __asm__ volatile ("pause" ::: "memory");
+}

libsystem/stall.h

@@ -0,0 +1,8 @@
+#ifndef _LIBSYSTEM_STALL_H
+#define _LIBSYSTEM_STALL_H
+
+#include <stdint.h>
+
+void stall_ms (uint64_t ms);
+
+#endif // _LIBSYSTEM_STALL_H