diff --git a/sw/device/lib/testing/test_framework/kelvin_gloss.c b/sw/device/lib/testing/test_framework/kelvin_gloss.c
index f08e0f4..becd61a 100644
--- a/sw/device/lib/testing/test_framework/kelvin_gloss.c
+++ b/sw/device/lib/testing/test_framework/kelvin_gloss.c
@@ -17,6 +17,7 @@
 /* Syscall stubs for newlib on Kelvin */
 #include <errno.h>
 #include <sys/stat.h>
+#include <unistd.h>
 
 int _close(int file) { return -1; }
 
@@ -34,33 +35,79 @@
   return -1;
 }
 
-int _write(int file, char* ptr, int len) {
-  errno = EBADF;
-  return -1;
+#ifndef LOG_MAX_SZ
+#define LOG_MAX_SZ 256
+#endif
+// TODO(lundong): Handle stdout and stderr separately
+int _write(int file, char* buf, int nbytes) {
+  static int _write_line_buffer_len = 0;
+  static char _write_line_buffer[LOG_MAX_SZ];
+
+  if (file != STDOUT_FILENO && file != STDERR_FILENO) {
+    errno = EBADF;
+    return -1;
+  }
+
+  if (nbytes <= 0) {
+    return 0;
+  }
+
+  if (buf == NULL) {
+    errno = EFAULT;
+    return -1;
+  }
+
+  int bytes_read = 0;
+  char c;
+  do {
+    int len = _write_line_buffer_len;
+    c = *(buf++);
+    bytes_read++;
+
+    _write_line_buffer[len++] = c;
+    if (len == LOG_MAX_SZ - 1 || c == '\n') {
+      _write_line_buffer[len] = '\0';
+    }
+    if ((_write_line_buffer[len] == '\0')) {
+      asm volatile("flog %0" : : "r"(_write_line_buffer));
+      len = 0;
+    }
+    _write_line_buffer_len = len;
+  } while (bytes_read < nbytes);
+
+  return bytes_read;
 }
 
 int _open(const char* path, int flags, ...) { return -1; }
 
 void _exit(int status) {
+  asm volatile("ebreak");
   while (1) {
   }
 }
 
-int _kill(int pid, int sig) { return -1; }
+int _kill(int pid, int sig) {
+  asm volatile("ebreak");
+  return -1;
+}
 
-int _getpid(void) { return -1; }
+int _getpid(void) {
+  asm volatile("ebreak");
+  return -1;
+}
 
+char* _heap_ptr;  // Set to __heap_start__ in kelvin_start.S
 // Based on newlib's nosys sbrk
 void* _sbrk(int bytes) {
   extern char __heap_end__;
-  static char* heap_end;
   char* prev_heap_end;
-
-  if (heap_end == 0) {
-    heap_end = &__heap_end__;
+  if ((bytes < 0) || (_heap_ptr + bytes > &__heap_end__)) {
+    errno = ENOMEM;
+    return (void*)-1;
   }
-  prev_heap_end = heap_end;
-  heap_end += bytes;
+
+  prev_heap_end = _heap_ptr;
+  _heap_ptr += bytes;
 
   return (void*)prev_heap_end;
 }
diff --git a/sw/device/lib/testing/test_framework/kelvin_start.S b/sw/device/lib/testing/test_framework/kelvin_start.S
index 479b96c..568aec7 100644
--- a/sw/device/lib/testing/test_framework/kelvin_start.S
+++ b/sw/device/lib/testing/test_framework/kelvin_start.S
@@ -25,7 +25,7 @@
         ###############################################
         # Put all scalar registers into a known state #
         ###############################################
-        la   sp, _stack_end
+        la   sp, __stack_end__
         la   gp, _global_pointer
         mv   tp, zero
         mv   t1, zero
@@ -59,6 +59,10 @@
         la   a1, __bss_end__
         call crt_section_clear
 
+        # Initialize the heap ptr after clearing BSS
+        la   s0, __heap_start__
+        sw   s0, _heap_ptr, s1
+
         # Initialize arrays
         la   s0, __init_array_start
         la   s1, __init_array_end
diff --git a/sw/device/tests/kelvin/kelvin_hello_world.c b/sw/device/tests/kelvin/kelvin_hello_world.c
index 7109422..f3b2aaf 100644
--- a/sw/device/tests/kelvin/kelvin_hello_world.c
+++ b/sw/device/tests/kelvin/kelvin_hello_world.c
@@ -18,12 +18,14 @@
 
 #include <stddef.h>
 #include <stdint.h>
+#include <stdio.h>
 
 const uint32_t kDataInput = 0x100000;
 const uint32_t kDataSize = 0x1000;
 
 int main(int argc, char *argv[]) {
   uint32_t *data = (uint32_t *)kDataInput;
+  printf("hello world!!\n");
   for (uint32_t i = 0; i < kDataSize / sizeof(uint32_t); ++i) {
     data[i] += 1;
   }