tdo

decode.go (5234B)

---

 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)
 }