1 files changed, 48 insertions, 5 deletions
diff --git a/pw_sys_io_zephyr/sys_io.cc b/pw_sys_io_zephyr/sys_io.cc
index fe0faf57e..fd9255e63 100644
--- a/pw_sys_io_zephyr/sys_io.cc
+++ b/pw_sys_io_zephyr/sys_io.cc
@@ -14,17 +14,60 @@
 
 #include "pw_sys_io/sys_io.h"
 
-#include <sys/printk.h>
+#include <console/console.h>
+#include <init.h>
+#include <usb/usb_device.h>
+#include <zephyr.h>
+
+static int sys_io_init(const struct device* dev) {
+  int err;
+  ARG_UNUSED(dev);
+
+  if (IS_ENABLED(CONFIG_PIGWEED_SYS_IO_USB)) {
+    err = usb_enable(nullptr);
+    if (err) {
+      return err;
+    }
+  }
+  err = console_init();
+  return err;
+}
+
+SYS_INIT(sys_io_init, APPLICATION, CONFIG_PIGWEED_SYS_IO_INIT_PRIORITY);
 
 namespace pw::sys_io {
 
-Status ReadByte(std::byte*) { return Status::Unimplemented(); }
+Status ReadByte(std::byte* dest) {
+  if (dest == nullptr) {
+    return Status::InvalidArgument();
+  }
+
+  const int c = console_getchar();
+  *dest = static_cast<std::byte>(c);
 
-Status TryReadByte(std::byte*) { return Status::Unimplemented(); }
+  return c < 0 ? Status::FailedPrecondition() : OkStatus();
+}
+
+Status TryReadByte(std::byte* dest) {
+  if (dest == nullptr) {
+    return Status::InvalidArgument();
+  }
+
+  uint8_t byte;
+  int result = console_read(nullptr, &byte, 1);
+
+  if (result >= 0) {
+    *dest = static_cast<std::byte>(byte);
+    return OkStatus();
+  }
+
+  return Status::Unavailable();
+}
 
 Status WriteByte(std::byte b) {
-  printk("%c", static_cast<char>(b));
-  return OkStatus();
+  return console_putchar(static_cast<char>(b)) < 0
+             ? Status::FailedPrecondition()
+             : OkStatus();
 }
 
 StatusWithSize WriteLine(const std::string_view& s) {