commit 5449f48e21138128d9936541f3d5dbf2eeba3b76
parent 6c9427bf7b51432e5a5f7bc7c1e478df440d30e3
Author: david cochran <about.trout@gmail.com>
Date: Mon, 31 Jan 2022 02:47:48 +0000
add layer4 solution
Diffstat:
| A | cmd/layer4/decode.go | | | 209 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 file changed, 209 insertions(+), 0 deletions(-)
diff --git a/cmd/layer4/decode.go b/cmd/layer4/decode.go
@@ -0,0 +1,209 @@
+package main
+
+import (
+ "encoding/binary"
+ "errors"
+ "log"
+ "net"
+ "os"
+
+ "github.com/abtrout/tdo"
+)
+
+func main() {
+ bs, err := tdo.DecodePipedInput()
+ if err != nil {
+ log.Fatalf("Failed to read layer input: %v", err)
+ }
+
+ var out []byte
+ packets, err := ParsePackets(bs)
+ if err != nil {
+ log.Fatalf("Failed to ParsePackets: %v", err)
+ }
+ for _, p := range FilterPackets(packets) {
+ out = append(out, p.udpDG.bs...)
+ }
+
+ if _, err := os.Stdout.Write(out); err != nil {
+ log.Fatalf("Failed to write decoded output: %v", err)
+ }
+}
+
+func ParsePackets(bs []byte) ([]*Packet, error) {
+ var packets []*Packet
+ var offset int
+ for {
+ if offset >= len(bs) {
+ break
+ }
+ // Parse and validate IPv4 header.
+ ipH, err := ParseIPv4Header(bs[offset:])
+ if err != nil {
+ return nil, err
+ }
+ // Parse and validate UDP header inside IPv4 data.
+ ipData := bs[offset+ipH.Len : offset+ipH.TotalLen]
+ udpH, err := ParseUDPHeader(ipData)
+ if err != nil {
+ return nil, err
+ }
+ udpData := ipData[8:] // skip header (4 uint16 = 8 bytes)
+ udpDG := &UDPDatagram{udpH, udpData}
+ packets = append(packets, &Packet{ipH, ipData, udpDG})
+
+ offset += ipH.TotalLen
+ }
+ return packets, nil
+}
+
+func FilterPackets(packets []*Packet) []*Packet {
+ // Filter packets as follows:
+ // - The packet was sent FROM any port of 10.1.1.10
+ // - The packet was sent TO port 42069 of 10.1.1.200
+ // - The IPv4 header checksum is correct
+ // - The UDP header checksum is correct
+ var filtered []*Packet
+ wantSrc := net.IPv4(10, 1, 1, 10)
+ wantDst := net.IPv4(10, 1, 1, 200)
+ wantDstPort := uint16(42069)
+ for _, p := range packets {
+ if !p.ipH.Valid || !p.UDPChecksum() {
+ continue
+ } else if !p.ipH.Src.Equal(wantSrc) {
+ continue
+ } else if !p.ipH.Dst.Equal(wantDst) {
+ continue
+ } else if p.udpDG.h.dstPort != wantDstPort {
+ continue
+ }
+ filtered = append(filtered, p)
+ }
+ return filtered
+}
+
+type Packet struct {
+ ipH *IPv4Header
+ ipData []byte
+ udpDG *UDPDatagram
+}
+
+type IPv4Header struct {
+ Version int // protocol version
+ Len int // header length
+ TotalLen int // packet total length
+ Flags int // flags
+ FragOff int // fragment offset
+ TTL int // time-to-live
+ Protocol int // next protocol
+ Checksum int // checksum
+ Src net.IP // source address
+ Dst net.IP // destination address
+
+ Valid bool // the checksum matches wire format data.
+}
+
+// Parse an IPv4 header.
+// https://datatracker.ietf.org/doc/html/rfc791#page-11
+//
+// This is mostly copied from x/net/ipv4, and modified to
+// work with wire format.
+func ParseIPv4Header(bs []byte) (*IPv4Header, error) {
+ if len(bs) < 20 {
+ return nil, errors.New("header too short")
+ }
+ h := IPv4Header{}
+ h.Version = int(bs[0] >> 4)
+ h.Len = int(bs[0]&0x0f) << 2
+ h.TotalLen = int(binary.BigEndian.Uint16(bs[2:4]))
+ h.TTL = int(bs[8])
+ h.Protocol = int(bs[9])
+ h.Checksum = int(binary.BigEndian.Uint16(bs[10:12]))
+ h.Src = net.IPv4(bs[12], bs[13], bs[14], bs[15])
+ h.Dst = net.IPv4(bs[16], bs[17], bs[18], bs[19])
+
+ tmp := int(binary.BigEndian.Uint16(bs[6:8]))
+ h.Flags = int(tmp&0xe000) >> 13
+ h.FragOff = tmp & 0x1fff
+
+ // Validate checksum.
+ var sum uint16
+ for i := 0; i < 20; i += 2 {
+ sum = Sum(sum, binary.BigEndian.Uint16(bs[i:i+2]))
+ }
+ h.Valid = (sum == 0xFFFF)
+
+ return &h, nil
+}
+
+type UDPHeader struct {
+ srcPort uint16 // all fields are 2 bytes.
+ dstPort uint16
+ length uint16
+ checksum uint16
+}
+
+func ParseUDPHeader(bs []byte) (*UDPHeader, error) {
+ if len(bs) < 8 {
+ return nil, errors.New("can't parse UDPHeader; not enough bytes")
+ }
+ return &UDPHeader{
+ srcPort: binary.BigEndian.Uint16(bs[0:2]),
+ dstPort: binary.BigEndian.Uint16(bs[2:4]),
+ length: binary.BigEndian.Uint16(bs[4:6]),
+ checksum: binary.BigEndian.Uint16(bs[6:8]),
+ }, nil
+}
+
+type UDPDatagram struct {
+ h *UDPHeader
+ bs []byte
+}
+
+// Checks if checksum computed from header matches checksum field.
+//
+// Checksum is the 16-bit one's complement of the one's complement
+// sum of a pseudo header of information from the IP header, the UDP header,
+// and the data, padded with zero octets at the end (if necessary) to make
+// a multiple of two octets.
+func (p *Packet) UDPChecksum() bool {
+ var sum uint16
+
+ // Source IPv4 address
+ ip := p.ipH.Src.To4()
+ sum = Sum(sum, binary.BigEndian.Uint16(ip[:2]))
+ sum = Sum(sum, binary.BigEndian.Uint16(ip[2:]))
+ // Destination IPv4 address
+ ip = p.ipH.Dst.To4()
+ sum = Sum(sum, binary.BigEndian.Uint16(ip[:2]))
+ sum = Sum(sum, binary.BigEndian.Uint16(ip[2:]))
+ // Protocol; fixed (17) since using UDP
+ sum = Sum(sum, uint16(17))
+ // UDP length = IPv4 data payload size
+ sum = Sum(sum, uint16(len(p.ipData)))
+ // Source port
+ sum = Sum(sum, p.udpDG.h.srcPort)
+ // Destination port
+ sum = Sum(sum, p.udpDG.h.dstPort)
+ // Length
+ sum = Sum(sum, p.udpDG.h.length)
+
+ // Data
+ bs := p.udpDG.bs
+ for i := 0; i < len(bs)-1; i += 2 {
+ sum = Sum(sum, binary.BigEndian.Uint16(bs[i:i+2]))
+ }
+ if len(bs)%2 != 0 {
+ sum = Sum(sum, binary.BigEndian.Uint16([]byte{bs[len(bs)-1], 0}))
+ }
+
+ // Finally, take ones complement of this sum.
+ sum = ^sum
+ return sum == p.udpDG.h.checksum
+}
+
+// One's complement sum.
+func Sum(a, b uint16) uint16 {
+ sum := uint32(a) + uint32(b)
+ return uint16(sum&0xFFFF) + uint16(sum>>16)
+}
