diff --git a/hal/src/main/native/include/hal/DriverStation.h b/hal/src/main/native/include/hal/DriverStation.h
index f4fd59e744..1aeccb16be 100644
--- a/hal/src/main/native/include/hal/DriverStation.h
+++ b/hal/src/main/native/include/hal/DriverStation.h
@@ -61,6 +61,7 @@ HAL_ENUM(HAL_MatchType) {
  */
 #define HAL_kMaxJoystickAxes 12
 #define HAL_kMaxJoystickPOVs 12
+#define HAL_kMaxJoysticks 6
 
 struct HAL_JoystickAxes {
   int16_t count;
diff --git a/hal/src/main/native/include/mockdata/MockHooks.h b/hal/src/main/native/include/mockdata/MockHooks.h
index c67d19862d..37eb0e7e0d 100644
--- a/hal/src/main/native/include/mockdata/MockHooks.h
+++ b/hal/src/main/native/include/mockdata/MockHooks.h
@@ -9,9 +9,12 @@
 
 #ifndef __FRC_ROBORIO__
 
+#include "hal/Types.h"
+
 extern "C" {
 void HALSIM_WaitForProgramStart(void);
 void HALSIM_SetProgramStarted(void);
+HAL_Bool HALSIM_GetProgramStarted(void);
 void HALSIM_RestartTiming(void);
 }  // extern "C"
 
diff --git a/hal/src/main/native/include/simulation/SimHooks.h b/hal/src/main/native/include/simulation/SimHooks.h
index d2eafe04be..b4e5054663 100644
--- a/hal/src/main/native/include/simulation/SimHooks.h
+++ b/hal/src/main/native/include/simulation/SimHooks.h
@@ -15,6 +15,7 @@ namespace frc {
 namespace sim {
 void WaitForProgramStart() { HALSIM_WaitForProgramStart(); }
 void SetProgramStarted() { HALSIM_SetProgramStarted(); }
+bool GetProgramStarted() { return HALSIM_GetProgramStarted(); }
 void RestartTiming() { HALSIM_RestartTiming(); }
 }  // namespace sim
 }  // namespace frc
diff --git a/hal/src/main/native/sim/MockHooks.cpp b/hal/src/main/native/sim/MockHooks.cpp
index 62e2dcdccb..25c1d9f33e 100644
--- a/hal/src/main/native/sim/MockHooks.cpp
+++ b/hal/src/main/native/sim/MockHooks.cpp
@@ -36,6 +36,7 @@ int64_t GetFPGATime() {
 double GetFPGATimestamp() { return GetFPGATime() * 1.0e-6; }
 
 void SetProgramStarted() { programStarted = true; }
+bool GetProgramStarted() { return programStarted; }
 }  // namespace hal
 
 using namespace hal;
@@ -52,5 +53,7 @@ void HALSIM_WaitForProgramStart(void) {
 
 void HALSIM_SetProgramStarted(void) { SetProgramStarted(); }
 
+HAL_Bool HALSIM_GetProgramStarted(void) { return GetProgramStarted(); }
+
 void HALSIM_RestartTiming(void) { RestartTiming(); }
 }  // extern "C"
diff --git a/shared/plugins/setupBuild.gradle b/shared/plugins/setupBuild.gradle
index d1fe6aa84d..706ce40a2c 100644
--- a/shared/plugins/setupBuild.gradle
+++ b/shared/plugins/setupBuild.gradle
@@ -1,5 +1,6 @@
 apply plugin: 'cpp'
 apply plugin: 'edu.wpi.first.NativeUtils'
+apply plugin: ExtraTasks
 
 if (!project.hasProperty('onlyAthena')) {
     ext.skipAthena = true
diff --git a/simulation/halsim_ds_socket/build.gradle b/simulation/halsim_ds_socket/build.gradle
index 49faf0eee9..b0bda120ec 100644
--- a/simulation/halsim_ds_socket/build.gradle
+++ b/simulation/halsim_ds_socket/build.gradle
@@ -5,4 +5,54 @@ ext {
     pluginName = 'halsim_ds_socket'
 }
 
+apply plugin: 'google-test-test-suite'
+
+
+ext {
+    staticGtestConfigs = [:]
+}
+
+staticGtestConfigs["${pluginName}Test"] = []
+apply from: "${rootDir}/shared/googletest.gradle"
+
 apply from: "${rootDir}/shared/plugins/setupBuild.gradle"
+
+
+model {
+    testSuites {
+        def comps = $.components
+        if (!project.hasProperty('onlyAthena')) {
+            "${pluginName}Test"(GoogleTestTestSuiteSpec) {
+                for(NativeComponentSpec c : comps) {
+                    if (c.name == pluginName) {
+                        testing c
+                        break
+                    }
+                }
+                sources {
+                    cpp {
+                        source {
+                            srcDirs 'src/test/native/cpp'
+                            include '**/*.cpp'
+                        }
+                        exportedHeaders {
+                            srcDirs 'src/test/native/include', 'src/main/native/cpp'
+                        }
+                    }
+                }
+            }
+        }
+    }
+    binaries {
+        withType(GoogleTestTestSuiteBinarySpec) {
+            lib project: ':hal', library: 'hal', linkage: 'shared'
+            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            lib library: pluginName, linkage: 'shared'
+        }
+    }
+}
+
+tasks.withType(RunTestExecutable) {
+    args "--gtest_output=xml:test_detail.xml"
+    outputs.dir outputDir
+}
diff --git a/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp
index 83c4568502..aba88fbaa7 100644
--- a/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp
+++ b/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp
@@ -8,6 +8,7 @@
 #include <thread>
 
 #include <hal/HAL.h>
+#include <wpi/Format.h>
 #include <wpi/raw_ostream.h>
 
 extern "C" int HALSIM_InitExtension(void);
@@ -15,4 +16,10 @@ extern "C" int HALSIM_InitExtension(void);
 int main() {
   HAL_Initialize(500, 0);
   HALSIM_InitExtension();
+
+  HAL_ObserveUserProgramStarting();
+
+  while (true) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
 }
diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp
index f90bfeb054..3007fa79b9 100644
--- a/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp
+++ b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp
@@ -14,24 +14,24 @@
 #include <thread>
 #include <vector>
 
-#include <FRCComm.h>
 #include <mockdata/DriverStationData.h>
+#include <mockdata/MockHooks.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/Format.h>
 
 using namespace halsim;
 
+DSCommPacket::DSCommPacket() {
+  for (auto& i : m_joystick_packets) {
+    i.ResetTcp();
+    i.ResetUdp();
+  }
+}
+
 /*----------------------------------------------------------------------------
 **  The following methods help parse and hold information about the
 **   driver station and it's joysticks.
 **--------------------------------------------------------------------------*/
-void DSCommPacket::SetIndex(uint8_t hi, uint8_t lo) {
-  m_hi = hi;
-  m_lo = lo;
-}
-
-void DSCommPacket::GetIndex(uint8_t& hi, uint8_t& lo) {
-  hi = m_hi;
-  lo = m_lo;
-}
 
 void DSCommPacket::SetControl(uint8_t control, uint8_t request) {
   std::memset(&m_control_word, 0, sizeof(m_control_word));
@@ -45,215 +45,263 @@ void DSCommPacket::SetControl(uint8_t control, uint8_t request) {
   m_control_sent = control;
 }
 
-void DSCommPacket::GetControl(uint8_t& control) { control = m_control_sent; }
-
-void DSCommPacket::GetStatus(uint8_t& status) { status = kRobotHasCode; }
-
 void DSCommPacket::SetAlliance(uint8_t station_code) {
-  m_alliance_station = static_cast<enum AllianceStationID_t>(station_code);
+  m_alliance_station = static_cast<HAL_AllianceStationID>(station_code);
 }
 
-int DSCommPacket::AddDSCommJoystickPacket(uint8_t* data, int len) {
-  DSCommJoystickPacket stick;
-  if (len > 0) {
-    int axis_count = *data++;
-    len--;
-    if (axis_count > len) return -1;
-    len -= axis_count;
-    for (; axis_count > 0; axis_count--) {
-      stick.axes.push_back(*data++);
+void DSCommPacket::ReadMatchtimeTag(wpi::ArrayRef<uint8_t> tagData) {
+  if (tagData.size() < 6) return;
+
+  uint32_t store = tagData[2] << 24;
+  store |= tagData[3] << 16;
+  store |= tagData[4] << 8;
+  store |= tagData[5];
+
+  float matchTime = *reinterpret_cast<float*>(&store);
+  m_match_time = matchTime;
+}
+
+void DSCommPacket::ReadJoystickTag(wpi::ArrayRef<uint8_t> dataInput,
+                                   int index) {
+  DSCommJoystickPacket& stick = m_joystick_packets[index];
+  stick.ResetUdp();
+
+  if (dataInput.size() == 2) {
+    return;
+  }
+
+  dataInput = dataInput.slice(2);
+
+  // Read axes
+  int axesLength = dataInput[0];
+  for (int i = 0; i < axesLength; i++) {
+    int8_t value = dataInput[1 + i];
+    if (value < 0) {
+      stick.axes.axes[i] = value / 128.0;
+    } else {
+      stick.axes.axes[i] = value / 127.0;
     }
   }
+  stick.axes.count = axesLength;
 
-  if (len > 2) {
-    stick.button_count = *data++;
-    stick.buttons = (*data++) << 8;
-    stick.buttons |= *data++;
-    len -= 3;
+  dataInput = dataInput.slice(1 + axesLength);
+
+  // Read Buttons
+  int buttonCount = dataInput[0];
+  int numBytes = (buttonCount + 7) / 8;
+  stick.buttons.buttons = 0;
+  for (int i = 0; i < numBytes; i++) {
+    stick.buttons.buttons |= dataInput[1 + i] << (8 * (i));
+  }
+  stick.buttons.count = buttonCount;
+
+  dataInput = dataInput.slice(1 + numBytes);
+
+  int povsLength = dataInput[0];
+  for (int i = 0; i < povsLength * 2; i += 2) {
+    stick.povs.povs[i] = (dataInput[1 + i] << 8) | dataInput[2 + i];
   }
 
-  if (len > 0) {
-    int pov_count = *data++;
-    len--;
-    if (pov_count * 2 > len) return -1;
-    len -= pov_count * 2;
-    for (; pov_count > 0; pov_count--) {
-      stick.povs.push_back((data[0] << 8) | data[1]);
-      data += 2;
-    }
-  }
+  stick.povs.count = povsLength;
 
-  m_joystick_packets.push_back(stick);
-
-  return len;
-}
-
-void DSCommPacket::GetControlWord(struct ControlWord_t& control_word) {
-  control_word = m_control_word;
-}
-
-void DSCommPacket::GetAllianceStation(
-    enum AllianceStationID_t& alliance_station) {
-  alliance_station = m_alliance_station;
+  return;
 }
 
 /*----------------------------------------------------------------------------
 **  Communication methods
 **--------------------------------------------------------------------------*/
-int DSCommPacket::DecodeTCP(uint8_t* packet, int len) {
-  if (len < 2) return 0;
-  if (packet[0] == 0 && packet[1] == 0) return 2;
-  int packet_len = packet[1];
-  if (packet_len + 2 > len) return 0;
-  int packet_type = static_cast<int>(packet[2]);
+void DSCommPacket::DecodeTCP(wpi::ArrayRef<uint8_t> packet) {
+  // No header
+  while (!packet.empty()) {
+    int tagLength = packet[0] << 8 | packet[1];
+    auto tagPacket = packet.slice(0, tagLength + 2);
 
-  Lock();
-  if (packet_type == kGameDataType) {
-    std::copy(
-        packet + 3,
-        packet + 3 +
-            std::min(static_cast<int>(sizeof(m_game_data)), packet_len - 1),
-        m_game_data);
-  } else if (packet_type == kJoystickNameType && len >= 7) {
-    int joystick = static_cast<int>(packet[3]);
-    if (joystick < kMaxJoysticks) {
-      m_joystick_types[joystick] = static_cast<int>(packet[5]);
-      int namelen = static_cast<int>(packet[6]);
-      m_joystick_names[joystick] =
-          std::string(reinterpret_cast<char*>(packet + 7), namelen);
+    if (tagLength == 0) {
+      return;
     }
-  } else {
-    std::cerr << "TCP packet type " << packet_type << " unimplemented"
-              << std::endl;
-    for (int i = 0; i < packet_len + 2; i++)
-      std::fprintf(stderr, "%02x ", packet[i]);
-    std::fprintf(stderr, "\n");
+
+    switch (packet[2]) {
+      case kJoystickNameTag:
+        ReadJoystickDescriptionTag(tagPacket);
+        break;
+      case kGameDataTag:
+        ReadGameSpecificMessageTag(tagPacket);
+        break;
+      case kMatchInfoTag:
+        ReadNewMatchInfoTag(tagPacket);
+        break;
+    }
+    packet = packet.slice(tagLength + 2);
   }
-  Unlock();
-  return packet_len + 2;
 }
 
-void DSCommPacket::DecodeUDP(uint8_t* packet, int len) {
-  if (len < 3) return;
+void DSCommPacket::DecodeUDP(wpi::ArrayRef<uint8_t> packet) {
+  if (packet.size() < 6) return;
+  // Decode fixed header
+  m_hi = packet[0];
+  m_lo = packet[1];
+  // Comm Version is packet 2, ignore
+  SetControl(packet[3], packet[4]);
+  SetAlliance(packet[5]);
 
-  Lock();
-  m_joystick_packets.clear();
-  SetIndex(packet[0], packet[1]);
-  if (packet[2] != 0) {
-    if (len >= 6) {
-      SetControl(packet[3], packet[4]);
-      SetAlliance(packet[5]);
-      packet += 6;
-      len -= 6;
-      while (len > 0) {
-        int packet_len = *packet++;
-        if (packet_len > len) break;
-        if (*packet == kTagDsCommJoystick) {
-          if (AddDSCommJoystickPacket(packet + 1, packet_len - 1) < 0) break;
-        }
-        packet += packet_len;
-        len -= packet_len;
-      }
+  // Return if packet finished
+  if (packet.size() == 6) return;
+
+  // Else, handle tagged data
+  packet = packet.slice(6);
+
+  int joystickNum = 0;
+
+  // Loop to handle multiple tags
+  while (!packet.empty()) {
+    auto tagLength = packet[0];
+    auto tagPacket = packet.slice(0, tagLength + 1);
+
+    switch (packet[1]) {
+      case kJoystickDataTag:
+        ReadJoystickTag(tagPacket, joystickNum);
+        joystickNum++;
+        break;
+      case kMatchTimeTag:
+        ReadMatchtimeTag(tagPacket);
+        break;
     }
+    packet = packet.slice(tagLength + 1);
   }
-  m_udp_packets++;
-  Unlock();
+}
+
+void DSCommPacket::ReadNewMatchInfoTag(wpi::ArrayRef<uint8_t> data) {
+  // Size 2 bytes, tag 1 byte
+  if (data.size() <= 3) return;
+
+  int nameLength = std::min<size_t>(data[3], sizeof(matchInfo.eventName) - 1);
+
+  for (int i = 0; i < nameLength; i++) {
+    matchInfo.eventName[i] = data[4 + i];
+  }
+
+  matchInfo.eventName[nameLength] = '\0';
+
+  data = data.slice(4 + nameLength);
+
+  if (data.size() < 4) return;
+
+  matchInfo.matchType = static_cast<HAL_MatchType>(
+      data[0]);  // None, Practice, Qualification, Elimination, Test
+  matchInfo.matchNumber = (data[1] << 8) | data[2];
+  matchInfo.replayNumber = data[3];
+
+  HALSIM_SetMatchInfo(&matchInfo);
+}
+
+void DSCommPacket::ReadGameSpecificMessageTag(wpi::ArrayRef<uint8_t> data) {
+  // Size 2 bytes, tag 1 byte
+  if (data.size() <= 3) return;
+
+  int length = std::min<size_t>(((data[0] << 8) | data[1]) - 1,
+                                sizeof(matchInfo.gameSpecificMessage));
+  for (int i = 0; i < length; i++) {
+    matchInfo.gameSpecificMessage[i] = data[3 + i];
+  }
+
+  matchInfo.gameSpecificMessageSize = length;
+
+  HALSIM_SetMatchInfo(&matchInfo);
+}
+void DSCommPacket::ReadJoystickDescriptionTag(wpi::ArrayRef<uint8_t> data) {
+  if (data.size() < 3) return;
+  data = data.slice(3);
+  int joystickNum = data[0];
+  DSCommJoystickPacket& packet = m_joystick_packets[joystickNum];
+  packet.ResetTcp();
+  packet.descriptor.isXbox = data[1] != 0 ? 1 : 0;
+  packet.descriptor.type = data[2];
+  int nameLength =
+      std::min<size_t>(data[3], (sizeof(packet.descriptor.name) - 1));
+  for (int i = 0; i < nameLength; i++) {
+    packet.descriptor.name[i] = data[4 + i];
+  }
+  data = data.slice(4 + nameLength);
+  packet.descriptor.name[nameLength] = '\0';
+  int axesCount = data[0];
+  packet.descriptor.axisCount = axesCount;
+  for (int i = 0; i < axesCount; i++) {
+    packet.descriptor.axisTypes[i] = data[1 + i];
+  }
+  data = data.slice(1 + axesCount);
+
+  packet.descriptor.buttonCount = data[0];
+  packet.descriptor.povCount = data[1];
 }
 
 void DSCommPacket::SendJoysticks(void) {
-  unsigned int i;
-
-  for (i = 0; i < kMaxJoysticks; i++) {
-    struct HAL_JoystickAxes axes;
-    struct HAL_JoystickPOVs povs;
-    struct HAL_JoystickButtons buttons;
-    struct HAL_JoystickDescriptor descriptor;
-    int j;
-
-    std::memset(&axes, 0, sizeof(axes));
-    std::memset(&povs, 0, sizeof(povs));
-    std::memset(&buttons, 0, sizeof(buttons));
-    std::memset(&descriptor, 0, sizeof(descriptor));
-
-    if (i < m_joystick_packets.size()) {
-      axes.count = std::min(static_cast<int>(m_joystick_packets[i].axes.size()),
-                            HAL_kMaxJoystickAxes);
-      for (j = 0; j < axes.count; j++) {
-        int8_t value = m_joystick_packets[i].axes[j];
-        if (value < 0) {
-          axes.axes[j] = value / 128.0;
-        } else {
-          axes.axes[j] = value / 127.0;
-        }
-      }
-
-      povs.count = std::min(static_cast<int>(m_joystick_packets[i].povs.size()),
-                            HAL_kMaxJoystickPOVs);
-      for (j = 0; j < povs.count; j++)
-        povs.povs[j] = m_joystick_packets[i].povs[j];
-
-      buttons.count = m_joystick_packets[i].button_count;
-      buttons.buttons = m_joystick_packets[i].buttons;
-
-      descriptor.axisCount = axes.count;
-      descriptor.povCount = povs.count;
-      descriptor.buttonCount = buttons.count;
-    }
-    descriptor.type = m_joystick_types[i];
-    m_joystick_names[i].copy(descriptor.name, sizeof(descriptor.name) - 1, 0);
-
-    HALSIM_SetJoystickAxes(i, &axes);
-    HALSIM_SetJoystickPOVs(i, &povs);
-    HALSIM_SetJoystickButtons(i, &buttons);
-    HALSIM_SetJoystickDescriptor(i, &descriptor);
-
-    /* TODO(jwhite@codeweavers.com):  If we want to support rumble, etc,
-            implement SetJoyStickOutputs, although that would be a callback  */
+  for (int i = 0; i < HAL_kMaxJoysticks; i++) {
+    DSCommJoystickPacket& packet = m_joystick_packets[i];
+    HALSIM_SetJoystickAxes(i, &packet.axes);
+    HALSIM_SetJoystickPOVs(i, &packet.povs);
+    HALSIM_SetJoystickButtons(i, &packet.buttons);
+    HALSIM_SetJoystickDescriptor(i, &packet.descriptor);
   }
 }
 
-void DSCommPacket::SendTCPToHALSim(void) {
-  struct HAL_MatchInfo info;
-  Lock();
-  std::strncpy(info.eventName, "Simulation", sizeof(info.eventName));
-  info.matchType = HAL_MatchType::HAL_kMatchType_none;
-  info.matchNumber = 1;
-  info.replayNumber = 0;
-  std::copy(info.gameSpecificMessage,
-            info.gameSpecificMessage +
-                std::min(sizeof(info.gameSpecificMessage), sizeof(m_game_data)),
-            m_game_data);
-  HALSIM_SetMatchInfo(&info);
-  Unlock();
+void DSCommPacket::SetupSendBuffer(wpi::raw_uv_ostream& buf) {
+  SetupSendHeader(buf);
+  SetupJoystickTag(buf);
+}
+
+void DSCommPacket::SetupSendHeader(wpi::raw_uv_ostream& buf) {
+  static constexpr uint8_t kCommVersion = 0x01;
+
+  // High low packet index, comm version
+  buf << m_hi << m_lo << kCommVersion;
+
+  // Control word and status check
+  buf << m_control_sent
+      << static_cast<uint8_t>(HALSIM_GetProgramStarted() ? kRobotHasCode : 0);
+
+  // Battery voltage high and low
+  buf << static_cast<uint8_t>(12) << static_cast<uint8_t>(0);
+
+  // Request (Always 0)
+  buf << static_cast<uint8_t>(0);
+}
+
+void DSCommPacket::SetupJoystickTag(wpi::raw_uv_ostream& buf) {
+  static constexpr uint8_t kHIDTag = 0x01;
+
+  // HID tags are sent 1 per device
+  int64_t outputs;
+  int32_t rightRumble;
+  int32_t leftRumble;
+  for (size_t i = 0; i < m_joystick_packets.size(); i++) {
+    // Length is 9, 1 tag and 8 data.
+    buf << static_cast<uint8_t>(9) << kHIDTag;
+    HALSIM_GetJoystickOutputs(i, &outputs, &leftRumble, &rightRumble);
+    auto op = static_cast<uint32_t>(outputs);
+    auto rr = static_cast<uint16_t>(rightRumble);
+    auto lr = static_cast<uint16_t>(leftRumble);
+    buf.write((op >> 24 & 0xFF));
+    buf.write((op >> 16 & 0xFF));
+    buf.write((op >> 8 & 0xFF));
+    buf.write((op & 0xFF));
+    buf.write((rr >> 8 & 0xFF));
+    buf.write((rr & 0xFF));
+    buf.write((lr >> 8 & 0xFF));
+    buf.write((lr & 0xFF));
+  }
 }
 
 void DSCommPacket::SendUDPToHALSim(void) {
-  struct ControlWord_t control_word;
-  AllianceStationID_t alliance_station;
-
-  Lock();
-  GetControlWord(control_word);
-  GetAllianceStation(alliance_station);
-  auto now = std::chrono::high_resolution_clock::now();
-  if (m_udp_packets == 1) {
-    m_match_time = 0.0;
-  } else if (control_word.enabled) {
-    std::chrono::duration<double> delta = (now - m_packet_time);
-    m_match_time += delta.count();
-  }
-  m_packet_time = now;
   SendJoysticks();
-  Unlock();
 
   HALSIM_SetDriverStationMatchTime(m_match_time);
-  HALSIM_SetDriverStationEnabled(control_word.enabled);
-  HALSIM_SetDriverStationAutonomous(control_word.autonomous);
-  HALSIM_SetDriverStationTest(control_word.test);
-  HALSIM_SetDriverStationEStop(control_word.eStop);
-  HALSIM_SetDriverStationFmsAttached(control_word.fmsAttached);
-  HALSIM_SetDriverStationDsAttached(control_word.dsAttached);
-  HALSIM_SetDriverStationAllianceStationId(
-      static_cast<HAL_AllianceStationID>(alliance_station));
+  HALSIM_SetDriverStationEnabled(m_control_word.enabled);
+  HALSIM_SetDriverStationAutonomous(m_control_word.autonomous);
+  HALSIM_SetDriverStationTest(m_control_word.test);
+  HALSIM_SetDriverStationEStop(m_control_word.eStop);
+  HALSIM_SetDriverStationFmsAttached(m_control_word.fmsAttached);
+  HALSIM_SetDriverStationDsAttached(m_control_word.dsAttached);
+  HALSIM_SetDriverStationAllianceStationId(m_alliance_station);
 
   HALSIM_NotifyDriverStationNewData();
 }
diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
index 1503055f16..12cbf7429e 100644
--- a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
+++ b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
@@ -23,6 +23,7 @@
 #include <wpi/EventLoopRunner.h>
 #include <wpi/StringRef.h>
 #include <wpi/raw_ostream.h>
+#include <wpi/raw_uv_ostream.h>
 #include <wpi/uv/Tcp.h>
 #include <wpi/uv/Timer.h>
 #include <wpi/uv/Udp.h>
@@ -38,16 +39,21 @@ static std::unique_ptr<Buffer> singleByte;
 
 namespace {
 struct DataStore {
-  wpi::SmallVector<char, 128> m_frame;
+  wpi::SmallVector<uint8_t, 128> m_frame;
   size_t m_frameSize = std::numeric_limits<size_t>::max();
   halsim::DSCommPacket* dsPacket;
 };
 }  // namespace
 
+static SimpleBufferPool<4>& GetBufferPool() {
+  static SimpleBufferPool<4> bufferPool;
+  return bufferPool;
+}
+
 static void HandleTcpDataStream(Buffer& buf, size_t size, DataStore& store) {
   wpi::StringRef data{buf.base, size};
   while (!data.empty()) {
-    if (store.m_frameSize != std::numeric_limits<size_t>::max()) {
+    if (store.m_frameSize == std::numeric_limits<size_t>::max()) {
       if (store.m_frame.size() < 2u) {
         size_t toCopy = std::min(2u - store.m_frame.size(), data.size());
         store.m_frame.append(data.bytes_begin(), data.bytes_begin() + toCopy);
@@ -57,7 +63,7 @@ static void HandleTcpDataStream(Buffer& buf, size_t size, DataStore& store) {
       store.m_frameSize = (static_cast<uint16_t>(store.m_frame[0]) << 8) |
                           static_cast<uint16_t>(store.m_frame[1]);
     }
-    if (store.m_frameSize != 0) {
+    if (store.m_frameSize != std::numeric_limits<size_t>::max()) {
       size_t need = store.m_frameSize - (store.m_frame.size() - 2);
       size_t toCopy = std::min(need, data.size());
       store.m_frame.append(data.bytes_begin(), data.bytes_begin() + toCopy);
@@ -65,9 +71,7 @@ static void HandleTcpDataStream(Buffer& buf, size_t size, DataStore& store) {
       need -= toCopy;
       if (need == 0) {
         auto ds = store.dsPacket;
-        ds->DecodeTCP(reinterpret_cast<uint8_t*>(store.m_frame.data()),
-                      store.m_frame.size());
-        ds->SendTCPToHALSim();
+        ds->DecodeTCP(store.m_frame);
         store.m_frame.clear();
         store.m_frameSize = std::numeric_limits<size_t>::max();
       }
@@ -80,6 +84,7 @@ static void SetupTcp(wpi::uv::Loop& loop) {
   auto tcpWaitTimer = Timer::Create(loop);
 
   auto recStore = std::make_shared<DataStore>();
+  recStore->dsPacket = loop.GetData<halsim::DSCommPacket>().get();
 
   tcp->SetData(recStore);
 
@@ -87,31 +92,15 @@ static void SetupTcp(wpi::uv::Loop& loop) {
 
   tcp->Listen([t = tcp.get()] {
     auto client = t->Accept();
-    t->data.connect([t](Buffer& buf, size_t len) {
+
+    client->data.connect([t](Buffer& buf, size_t len) {
       HandleTcpDataStream(buf, len, *t->GetData<DataStore>());
     });
+    client->StartRead();
+    client->end.connect([c = client.get()] { c->Close(); });
   });
 }
 
-/*----------------------------------------------------------------------------
-**  Send a reply packet back to the DS
-**--------------------------------------------------------------------------*/
-static void SetupReplyPacket(halsim::DSCommPacket* ds) {
-  static const uint8_t kTagGeneral = 0x01;
-
-  uint8_t* data = reinterpret_cast<uint8_t*>(ds->GetSendBuffer().base);
-
-  ds->GetIndex(data[0], data[1]);
-
-  data[2] = kTagGeneral;
-  ds->GetControl(data[3]);
-  ds->GetStatus(data[4]);
-
-  data[5] = 12;  // Voltage upper
-  data[6] = 0;   // Voltage lower
-  data[7] = 0;   // Request
-}
-
 static void SetupUdp(wpi::uv::Loop& loop) {
   auto udp = wpi::uv::Udp::Create(loop);
   udp->Bind("0.0.0.0", 1110);
@@ -135,21 +124,25 @@ static void SetupUdp(wpi::uv::Loop& loop) {
   udp->received.connect([udpLocal = udp.get()](
       Buffer & buf, size_t len, const sockaddr& recSock, unsigned int port) {
     auto ds = udpLocal->GetLoop()->GetData<halsim::DSCommPacket>();
-    ds->DecodeUDP(reinterpret_cast<uint8_t*>(buf.base), len);
-    SetupReplyPacket(ds.get());
+    ds->DecodeUDP(
+        wpi::ArrayRef<uint8_t>{reinterpret_cast<uint8_t*>(buf.base), len});
 
     struct sockaddr_in outAddr;
     std::memcpy(&outAddr, &recSock, sizeof(sockaddr_in));
     outAddr.sin_family = PF_INET;
     outAddr.sin_port = htons(1150);
 
-    udpLocal->Send(outAddr, wpi::ArrayRef<Buffer>{&ds->GetSendBuffer(), 1},
-                   [](auto buf, Error err) {
-                     if (err) {
-                       wpi::errs() << err.str() << "\n";
-                       wpi::errs().flush();
-                     }
-                   });
+    wpi::SmallVector<wpi::uv::Buffer, 4> sendBufs;
+    wpi::raw_uv_ostream stream{sendBufs, GetBufferPool()};
+    ds->SetupSendBuffer(stream);
+
+    udpLocal->Send(outAddr, sendBufs, [](auto bufs, Error err) {
+      GetBufferPool().Release(bufs);
+      if (err) {
+        wpi::errs() << err.str() << "\n";
+        wpi::errs().flush();
+      }
+    });
     ds->SendUDPToHALSim();
   });
 
diff --git a/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h b/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h
index 410c807d14..4d34b8ff64 100644
--- a/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h
+++ b/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h
@@ -6,15 +6,26 @@
 /*----------------------------------------------------------------------------*/
 
 #pragma once
-#include <vector>
+
+#include <cstring>
+
+#include <hal/DriverStation.h>
 
 namespace halsim {
 
 typedef struct {
-  std::vector<int16_t> axes;
-  uint8_t button_count;
-  uint32_t buttons;
-  std::vector<int16_t> povs;
+  HAL_JoystickAxes axes;
+  HAL_JoystickButtons buttons;
+  HAL_JoystickPOVs povs;
+  HAL_JoystickDescriptor descriptor;
+
+  void ResetUdp() {
+    std::memset(&axes, 0, sizeof(axes));
+    std::memset(&buttons, 0, sizeof(buttons));
+    std::memset(&povs, 0, sizeof(povs));
+  }
+
+  void ResetTcp() { std::memset(&descriptor, 0, sizeof(descriptor)); }
 } DSCommJoystickPacket;
 
 }  // namespace halsim
diff --git a/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h b/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h
index a61a4d83ad..ebe36e4f68 100644
--- a/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h
+++ b/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h
@@ -7,46 +7,35 @@
 
 #pragma once
 
-#include <chrono>
-#include <mutex>
-#include <string>
-#include <thread>
-#include <vector>
+#include <array>
 
 #include <DSCommJoystickPacket.h>
-#include <FRCComm.h>
 #include <mockdata/DriverStationData.h>
-#include <wpi/uv/Buffer.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/raw_uv_ostream.h>
+
+class DSCommPacketTest;
 
 namespace halsim {
 
 class DSCommPacket {
- public:
-  DSCommPacket(void) {
-    std::fill_n(m_joystick_types, kMaxJoysticks, -1);
-    sendDataBuffer = wpi::uv::Buffer::Allocate(8);
-  }
-  void Lock() { m_mutex.lock(); }
-  void Unlock() { m_mutex.unlock(); }
-  void SetIndex(uint8_t hi, uint8_t lo);
-  void GetIndex(uint8_t& hi, uint8_t& lo);
-  void SetControl(uint8_t control, uint8_t request);
-  void GetControl(uint8_t& control);
-  void GetStatus(uint8_t& status);
-  void SetAlliance(uint8_t station_code);
-  int AddDSCommJoystickPacket(uint8_t* data, int len);
-  void GetControlWord(struct ControlWord_t& control_word);
-  void GetAllianceStation(enum AllianceStationID_t& allianceStation);
-  int DecodeTCP(uint8_t* packet, int len);
-  void DecodeUDP(uint8_t* packet, int len);
-  void SendTCPToHALSim(void);
-  void SendUDPToHALSim(void);
-  void SendJoysticks(void);
-  wpi::uv::Buffer& GetSendBuffer(void) { return sendDataBuffer; }
+  friend class ::DSCommPacketTest;
 
-  /* TCP (FMS) types */
-  static const uint8_t kGameDataType = 0x0e;
-  static const uint8_t kJoystickNameType = 0x02;
+ public:
+  DSCommPacket(void);
+  void DecodeTCP(wpi::ArrayRef<uint8_t> packet);
+  void DecodeUDP(wpi::ArrayRef<uint8_t> packet);
+  void SendUDPToHALSim(void);
+  void SetupSendBuffer(wpi::raw_uv_ostream& buf);
+
+  /* TCP Tags */
+  static const uint8_t kGameDataTag = 0x0e;
+  static const uint8_t kJoystickNameTag = 0x02;
+  static const uint8_t kMatchInfoTag = 0x07;
+
+  /* UDP Tags*/
+  static const uint8_t kJoystickDataTag = 0x0c;
+  static const uint8_t kMatchTimeTag = 0x07;
 
   /* Control word bits */
   static const uint8_t kTest = 0x01;
@@ -61,29 +50,26 @@ class DSCommPacket {
   /* Status bits */
   static const uint8_t kRobotHasCode = 0x20;
 
-  /* Joystick tag bits */
-  static const uint8_t kTagDsCommJoystick = 0x0c;
-
-  /* Joystick max count */
-  /* TODO(jwhite@codeweavers.com) This is a magic number in the HAL; fix it
-   * there */
-  static const uint8_t kMaxJoysticks = 6;
-
  private:
-  uint8_t m_game_data[64];
+  void SendJoysticks(void);
+  void SetControl(uint8_t control, uint8_t request);
+  void SetAlliance(uint8_t station_code);
+  void SetupSendHeader(wpi::raw_uv_ostream& buf);
+  void SetupJoystickTag(wpi::raw_uv_ostream& buf);
+  void ReadMatchtimeTag(wpi::ArrayRef<uint8_t> tagData);
+  void ReadJoystickTag(wpi::ArrayRef<uint8_t> data, int index);
+  void ReadNewMatchInfoTag(wpi::ArrayRef<uint8_t> data);
+  void ReadGameSpecificMessageTag(wpi::ArrayRef<uint8_t> data);
+  void ReadJoystickDescriptionTag(wpi::ArrayRef<uint8_t> data);
+
   uint8_t m_hi;
   uint8_t m_lo;
   uint8_t m_control_sent;
-  struct ControlWord_t m_control_word;
-  enum AllianceStationID_t m_alliance_station;
-  std::vector<DSCommJoystickPacket> m_joystick_packets;
-  std::string m_joystick_names[kMaxJoysticks];
-  int m_joystick_types[kMaxJoysticks];
-  std::mutex m_mutex;
-  int m_udp_packets;
-  std::chrono::high_resolution_clock::time_point m_packet_time;
+  HAL_ControlWord m_control_word;
+  HAL_AllianceStationID m_alliance_station;
+  HAL_MatchInfo matchInfo;
+  std::array<DSCommJoystickPacket, HAL_kMaxJoysticks> m_joystick_packets;
   double m_match_time;
-  wpi::uv::Buffer sendDataBuffer;
 };
 
 }  // namespace halsim
diff --git a/simulation/halsim_ds_socket/src/main/native/include/FRCComm.h b/simulation/halsim_ds_socket/src/main/native/include/FRCComm.h
deleted file mode 100644
index 538aa3edd3..0000000000
--- a/simulation/halsim_ds_socket/src/main/native/include/FRCComm.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-/*----------------------------------------------------------------------------
- *  The defines and enums in this file were copied from:
- *     ni-libraries/include/FRC_NetworkCommunication/FRCComm.h
- *  to avoid the complexities of trying to get gradle to
- *  reliably build against it.
- *----------------------------------------------------------------------------*/
-
-#ifndef WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_
-#define WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_
-
-#include <stdint.h>
-
-#define ERR_FRCSystem_NetCommNotResponding -44049
-#define ERR_FRCSystem_NoDSConnection -44018
-
-enum AllianceStationID_t {
-  kAllianceStationID_red1,
-  kAllianceStationID_red2,
-  kAllianceStationID_red3,
-  kAllianceStationID_blue1,
-  kAllianceStationID_blue2,
-  kAllianceStationID_blue3,
-};
-
-enum MatchType_t {
-  kMatchType_none,
-  kMatchType_practice,
-  kMatchType_qualification,
-  kMatchType_elimination,
-};
-
-struct ControlWord_t {
-#ifndef __vxworks
-  uint32_t enabled : 1;
-  uint32_t autonomous : 1;
-  uint32_t test : 1;
-  uint32_t eStop : 1;
-  uint32_t fmsAttached : 1;
-  uint32_t dsAttached : 1;
-  uint32_t control_reserved : 26;
-#else
-  uint32_t control_reserved : 26;
-  uint32_t dsAttached : 1;
-  uint32_t fmsAttached : 1;
-  uint32_t eStop : 1;
-  uint32_t test : 1;
-  uint32_t autonomous : 1;
-  uint32_t enabled : 1;
-#endif
-};
-
-struct JoystickAxes_t {
-  uint16_t count;
-  int16_t axes[1];
-};
-
-struct JoystickPOV_t {
-  uint16_t count;
-  int16_t povs[1];
-};
-#endif  // WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_
diff --git a/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp b/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp
new file mode 100644
index 0000000000..a72aafcc0c
--- /dev/null
+++ b/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp
@@ -0,0 +1,146 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "DSCommPacket.h"
+#include "gtest/gtest.h"
+
+class DSCommPacketTest : public ::testing::Test {
+ public:
+  DSCommPacketTest() {}
+
+  void SendJoysticks() { commPacket.SendJoysticks(); }
+
+  halsim::DSCommJoystickPacket& ReadJoystickTag(wpi::ArrayRef<uint8_t> data,
+                                                int index) {
+    commPacket.ReadJoystickTag(data, index);
+    return commPacket.m_joystick_packets[index];
+  }
+
+  halsim::DSCommJoystickPacket& ReadDescriptorTag(wpi::ArrayRef<uint8_t> data) {
+    commPacket.ReadJoystickDescriptionTag(data);
+    return commPacket.m_joystick_packets[data[3]];
+  }
+
+  HAL_MatchInfo& ReadNewMatchInfoTag(wpi::ArrayRef<uint8_t> data) {
+    commPacket.ReadNewMatchInfoTag(data);
+    return commPacket.matchInfo;
+  }
+
+  HAL_MatchInfo& ReadGameSpecificTag(wpi::ArrayRef<uint8_t> data) {
+    commPacket.ReadGameSpecificMessageTag(data);
+    return commPacket.matchInfo;
+  }
+
+ protected:
+  halsim::DSCommPacket commPacket;
+};
+
+TEST_F(DSCommPacketTest, EmptyJoystickTag) {
+  for (int i = 0; i < HAL_kMaxJoysticks; i++) {
+    uint8_t arr[2];
+    auto& data = ReadJoystickTag(arr, 0);
+    ASSERT_EQ(data.axes.count, 0);
+    ASSERT_EQ(data.povs.count, 0);
+    ASSERT_EQ(data.buttons.count, 0);
+  }
+}
+
+TEST_F(DSCommPacketTest, BlankJoystickTag) {
+  for (int i = 0; i < HAL_kMaxJoysticks; i++) {
+    uint8_t arr[5];
+    arr[0] = 4;
+    arr[1] = 2;
+    arr[2] = 0;
+    arr[3] = 0;
+    arr[4] = 0;
+    auto& data = ReadJoystickTag(arr, 0);
+    ASSERT_EQ(data.axes.count, 0);
+    ASSERT_EQ(data.povs.count, 0);
+    ASSERT_EQ(data.buttons.count, 0);
+  }
+}
+
+TEST_F(DSCommPacketTest, MainJoystickTag) {
+  for (int i = 0; i < HAL_kMaxJoysticks; i++) {
+    // 5 for base, 4 joystick, 12 buttons (2 bytes) 3 povs
+    uint8_t arr[5 + 4 + 2 + 6] = {// Size, Tag
+                                  16, 12,
+                                  // Axes
+                                  4, 0x9C, 0xCE, 0, 75,
+                                  // Buttons
+                                  12, 0xFF, 0x0F,
+                                  // POVs
+                                  3, 0, 50, 0, 100, 0x0F, 0x00};
+
+    auto& data = ReadJoystickTag(arr, 0);
+    ASSERT_EQ(data.axes.count, 4);
+    ASSERT_EQ(data.povs.count, 3);
+    ASSERT_EQ(data.buttons.count, 12);
+  }
+}
+
+TEST_F(DSCommPacketTest, DescriptorTag) {
+  for (int i = 0; i < HAL_kMaxJoysticks; i++) {
+    uint8_t arr[] = {// Size (2), tag
+                     0, 0, 7,
+                     // Joystick index, Is Xbox, Type
+                     static_cast<uint8_t>(i), 1, 0,
+                     // NameLen, Name (Not null terminated)
+                     11, 'H', 'e', 'l', 'l', 'o', ' ', 'W', 'o', 'r', 'l', 'd',
+                     // Axes count, Axes types
+                     4, 1, 2, 3, 4,
+                     // Button count, pov count,
+                     12, 3};
+    arr[1] = sizeof(arr) - 2;
+    auto& data = ReadDescriptorTag(arr);
+    ASSERT_EQ(data.descriptor.isXbox, 1);
+    ASSERT_EQ(data.descriptor.type, 0);
+    ASSERT_STREQ(data.descriptor.name, "Hello World");
+    ASSERT_EQ(data.descriptor.axisCount, 4);
+    for (int i = 0; i < 4; i++) {
+      ASSERT_EQ(data.descriptor.axisTypes[i], i + 1);
+    }
+    ASSERT_EQ(data.descriptor.buttonCount, 12);
+    ASSERT_EQ(data.descriptor.povCount, 3);
+  }
+}
+
+TEST_F(DSCommPacketTest, MatchInfoTag) {
+  uint8_t arr[]{// Size (2), tag
+                0, 0, 8,
+                // Event Name Len, Event Name
+                4, 'W', 'C', 'B', 'C',
+                // Match type, Match num (2), replay num
+                2, 0, 18, 1};
+  arr[1] = sizeof(arr) - 2;
+  auto& matchInfo = ReadNewMatchInfoTag(arr);
+  ASSERT_STREQ(matchInfo.eventName, "WCBC");
+  ASSERT_EQ(matchInfo.matchType, HAL_MatchType::HAL_kMatchType_qualification);
+  ASSERT_EQ(matchInfo.matchNumber, 18);
+  ASSERT_EQ(matchInfo.replayNumber, 1);
+}
+
+TEST_F(DSCommPacketTest, GameDataTag) {
+  uint8_t arr[]{
+      // Size (2), tag
+      0,
+      0,
+      17,
+      // Match data (length is taglength - 1)
+      'W',
+      'C',
+      'B',
+      'C',
+  };
+  arr[1] = sizeof(arr) - 2;
+  auto& matchInfo = ReadGameSpecificTag(arr);
+  ASSERT_EQ(matchInfo.gameSpecificMessageSize, 4);
+  ASSERT_EQ(matchInfo.gameSpecificMessage[0], 'W');
+  ASSERT_EQ(matchInfo.gameSpecificMessage[1], 'C');
+  ASSERT_EQ(matchInfo.gameSpecificMessage[2], 'B');
+  ASSERT_EQ(matchInfo.gameSpecificMessage[3], 'C');
+}
diff --git a/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp
new file mode 100644
index 0000000000..aa4f4b0e85
--- /dev/null
+++ b/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <hal/HAL.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}