4 files changed, 203 insertions, 7 deletions

diff --git a/Source/Core/Common/Network.cpp b/Source/Core/Common/Network.cpp
index a928e8e831..c84cf5fd88 100644
--- a/Source/Core/Common/Network.cpp
+++ b/Source/Core/Common/Network.cpp
@@ -6,6 +6,15 @@
 
 #include <algorithm>
 #include <cctype>
+#include <string_view>
+
+#ifndef _WIN32
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#else
+#include <WinSock2.h>
+#endif
 
 #include <fmt/format.h>
 
@@ -72,4 +81,110 @@ std::optional<MACAddress> StringToMacAddress(std::string_view mac_string)
 
   return std::make_optional(mac);
 }
+
+EthernetHeader::EthernetHeader() = default;
+
+EthernetHeader::EthernetHeader(u16 ether_type)
+{
+  ethertype = htons(ether_type);
+}
+
+u16 EthernetHeader::Size() const
+{
+  return static_cast<u16>(SIZE);
+}
+
+IPv4Header::IPv4Header() = default;
+
+IPv4Header::IPv4Header(u16 data_size, u8 ip_proto, const sockaddr_in& from, const sockaddr_in& to)
+{
+  version_ihl = 0x45;
+  total_len = htons(Size() + data_size);
+  flags_fragment_offset = htons(0x4000);
+  ttl = 0x40;
+  protocol = ip_proto;
+  std::memcpy(&source_addr, &from.sin_addr, IPV4_ADDR_LEN);
+  std::memcpy(&destination_addr, &to.sin_addr, IPV4_ADDR_LEN);
+
+  header_checksum = htons(ComputeNetworkChecksum(this, Size()));
+}
+
+u16 IPv4Header::Size() const
+{
+  return static_cast<u16>(SIZE);
+}
+
+TCPHeader::TCPHeader() = default;
+
+TCPHeader::TCPHeader(const sockaddr_in& from, const sockaddr_in& to, u32 seq, const u8* data,
+                     u16 length)
+{
+  std::memcpy(&source_port, &from.sin_port, 2);
+  std::memcpy(&destination_port, &to.sin_port, 2);
+  sequence_number = htonl(seq);
+
+  // TODO: Write flags
+  // Write data offset
+  std::memset(&properties, 0x50, 1);
+
+  window_size = 0xFFFF;
+
+  // Compute the TCP checksum with its pseudo header
+  const u32 source_addr = ntohl(from.sin_addr.s_addr);
+  const u32 destination_addr = ntohl(to.sin_addr.s_addr);
+  const u32 initial_value = (source_addr >> 16) + (source_addr & 0xFFFF) +
+                            (destination_addr >> 16) + (destination_addr & 0xFFFF) + IPProto() +
+                            Size() + length;
+  u32 tcp_checksum = ComputeNetworkChecksum(this, Size(), initial_value);
+  tcp_checksum += ComputeNetworkChecksum(data, length);
+  while (tcp_checksum > 0xFFFF)
+    tcp_checksum = (tcp_checksum >> 16) + (tcp_checksum & 0xFFFF);
+  checksum = htons(static_cast<u16>(tcp_checksum));
+}
+
+u16 TCPHeader::Size() const
+{
+  return static_cast<u16>(SIZE);
+}
+
+u8 TCPHeader::IPProto() const
+{
+  return static_cast<u8>(IPPROTO_TCP);
+}
+
+UDPHeader::UDPHeader() = default;
+
+UDPHeader::UDPHeader(const sockaddr_in& from, const sockaddr_in& to, u16 data_length)
+{
+  std::memcpy(&source_port, &from.sin_port, 2);
+  std::memcpy(&destination_port, &to.sin_port, 2);
+  length = htons(Size() + data_length);
+}
+
+u16 UDPHeader::Size() const
+{
+  return static_cast<u16>(SIZE);
+}
+
+u8 UDPHeader::IPProto() const
+{
+  return static_cast<u8>(IPPROTO_UDP);
+}
+
+// Compute the network checksum with a 32-bit accumulator using the
+// "Normal" order, see RFC 1071 for more details.
+u16 ComputeNetworkChecksum(const void* data, u16 length, u32 initial_value)
+{
+  u32 checksum = initial_value;
+  std::size_t index = 0;
+  const std::string_view data_view{reinterpret_cast<const char*>(data), length};
+  for (u8 b : data_view)
+  {
+    const bool is_hi = index++ % 2 == 0;
+    checksum += is_hi ? b << 8 : b;
+  }
+  while (checksum > 0xFFFF)
+    checksum = (checksum >> 16) + (checksum & 0xFFFF);
+  return ~static_cast<u16>(checksum);
+}
 }  // namespace Common
diff --git a/Source/Core/Common/Network.h b/Source/Core/Common/Network.h
index 18a2f887e9..2f1666b152 100644
--- a/Source/Core/Common/Network.h
+++ b/Source/Core/Common/Network.h
@@ -11,6 +11,8 @@
 
 #include "Common/CommonTypes.h"
 
+struct sockaddr_in;
+
 namespace Common
 {
 enum class MACConsumer
@@ -25,8 +27,81 @@ enum
 };
 
 using MACAddress = std::array<u8, MAC_ADDRESS_SIZE>;
+constexpr std::size_t IPV4_ADDR_LEN = 4;
+
+struct EthernetHeader
+{
+  EthernetHeader();
+  explicit EthernetHeader(u16 ether_type);
+  u16 Size() const;
+
+  static constexpr std::size_t SIZE = 14;
+
+  MACAddress destination = {};
+  MACAddress source = {};
+  u16 ethertype = 0;
+};
+static_assert(sizeof(EthernetHeader) == EthernetHeader::SIZE);
+
+struct IPv4Header
+{
+  IPv4Header();
+  IPv4Header(u16 data_size, u8 ip_proto, const sockaddr_in& from, const sockaddr_in& to);
+  u16 Size() const;
+
+  static constexpr std::size_t SIZE = 20;
+
+  u8 version_ihl = 0;
+  u8 dscp_esn = 0;
+  u16 total_len = 0;
+  u16 identification = 0;
+  u16 flags_fragment_offset = 0;
+  u8 ttl = 0;
+  u8 protocol = 0;
+  u16 header_checksum = 0;
+  u8 source_addr[IPV4_ADDR_LEN]{};
+  u8 destination_addr[IPV4_ADDR_LEN]{};
+};
+static_assert(sizeof(IPv4Header) == IPv4Header::SIZE);
+
+struct TCPHeader
+{
+  TCPHeader();
+  TCPHeader(const sockaddr_in& from, const sockaddr_in& to, u32 seq, const u8* data, u16 length);
+  u16 Size() const;
+  u8 IPProto() const;
+
+  static constexpr std::size_t SIZE = 20;
+
+  u16 source_port = 0;
+  u16 destination_port = 0;
+  u32 sequence_number = 0;
+  u32 acknowledgement_number = 0;
+  u16 properties = 0;
+  u16 window_size = 0;
+  u16 checksum = 0;
+  u16 urgent_pointer = 0;
+};
+static_assert(sizeof(TCPHeader) == TCPHeader::SIZE);
+
+struct UDPHeader
+{
+  UDPHeader();
+  UDPHeader(const sockaddr_in& from, const sockaddr_in& to, u16 data_length);
+  u16 Size() const;
+  u8 IPProto() const;
+
+  static constexpr std::size_t SIZE = 8;
+
+  u16 source_port = 0;
+  u16 destination_port = 0;
+  u16 length = 0;
+  u16 checksum = 0;
+};
+static_assert(sizeof(UDPHeader) == UDPHeader::SIZE);
 
 MACAddress GenerateMacAddress(MACConsumer type);
 std::string MacAddressToString(const MACAddress& mac);
 std::optional<MACAddress> StringToMacAddress(std::string_view mac_string);
+u16 ComputeNetworkChecksum(const void* data, u16 length, u32 initial_value = 0);
 }  // namespace Common
diff --git a/Source/Core/Common/PcapFile.cpp b/Source/Core/Common/PcapFile.cpp
index 540f641d54..b7feb4f4eb 100644
--- a/Source/Core/Common/PcapFile.cpp
+++ b/Source/Core/Common/PcapFile.cpp
@@ -17,9 +17,6 @@ const u16 PCAP_VERSION_MAJOR = 2;
 const u16 PCAP_VERSION_MINOR = 4;
 const u32 PCAP_CAPTURE_LENGTH = 65535;
 
-// TODO(delroth): Make this configurable at PCAP creation time?
-const u32 PCAP_DATA_LINK_TYPE = 147;  // Reserved for internal use.
-
 // Designed to be directly written into the PCAP file. The PCAP format is
 // endian independent, so this works just fine.
 #pragma pack(push, 1)
@@ -45,10 +42,10 @@ struct PCAPRecordHeader
 
 }  // namespace
 
-void PCAP::AddHeader()
+void PCAP::AddHeader(u32 link_type)
 {
   PCAPHeader hdr = {PCAP_MAGIC, PCAP_VERSION_MAJOR,  PCAP_VERSION_MINOR, 0,
-                    0,          PCAP_CAPTURE_LENGTH, PCAP_DATA_LINK_TYPE};
+                    0,          PCAP_CAPTURE_LENGTH, link_type};
   m_fp->WriteBytes(&hdr, sizeof(hdr));
 }
 
diff --git a/Source/Core/Common/PcapFile.h b/Source/Core/Common/PcapFile.h
index dae0c1b2bb..957c5ca6e4 100644
--- a/Source/Core/Common/PcapFile.h
+++ b/Source/Core/Common/PcapFile.h
@@ -24,9 +24,18 @@ namespace Common
 class PCAP final
 {
 public:
+  enum class LinkType : u32
+  {
+    Ethernet = 1,  // IEEE 802.3 Ethernet
+    User = 147,    // Reserved for internal use
+  };
+
   // Takes ownership of the file object. Assumes the file object is already
   // opened in write mode.
-  explicit PCAP(File::IOFile* fp) : m_fp(fp) { AddHeader(); }
+  explicit PCAP(File::IOFile* fp, LinkType link_type = LinkType::User) : m_fp(fp)
+  {
+    AddHeader(static_cast<u32>(link_type));
+  }
   template <typename T>
   void AddPacket(const T& obj)
   {
@@ -36,7 +45,7 @@ public:
   void AddPacket(const u8* bytes, size_t size);
 
 private:
-  void AddHeader();
+  void AddHeader(u32 link_type);
 
   std::unique_ptr<File::IOFile> m_fp;
 };