mndp/pkg.go at master · bgptools/mndp · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
package mndp

import (
	"bytes"
	"encoding/binary"
	"fmt"
	"io"
	"net"
	"time"
)

type MMDPPacket struct {
	SeqNo uint32
	Parts []MMDPPart
}

type MMDPPart struct {
	Type  MMDPTLVType
	Value interface{}
}

type MMDPTLVType uint16

const (
	MMDPTypeMACAddress    = MMDPTLVType(1)
	MMDPTypeIdentity      = MMDPTLVType(5)
	MMDPTypeVersion       = MMDPTLVType(7)
	MMDPTypePlatform      = MMDPTLVType(8)
	MMDPTypeUptime        = MMDPTLVType(10)
	MMDPTypeSoftwareID    = MMDPTLVType(11)
	MMDPTypeBoard         = MMDPTLVType(12)
	MMDPTypeUnpack        = MMDPTLVType(14)
	MMDPTypeIPv6Address   = MMDPTLVType(15)
	MMDPTypeInterfaceName = MMDPTLVType(16)
	MMDPTypeIPv4Address   = MMDPTLVType(17)
)

func (t MMDPTLVType) String() string {
	switch t {
	case MMDPTypeMACAddress:
		return "MACAddress"
	case MMDPTypeIdentity:
		return "Identity"
	case MMDPTypeVersion:
		return "Version"
	case MMDPTypePlatform:
		return "Platform"
	case MMDPTypeUptime:
		return "Uptime"
	case MMDPTypeSoftwareID:
		return "SoftwareID"
	case MMDPTypeBoard:
		return "Board"
	case MMDPTypeUnpack:
		return "Unpack"
	case MMDPTypeIPv6Address:
		return "IPv6Address"
	case MMDPTypeInterfaceName:
		return "InterfaceName"
	case MMDPTypeIPv4Address:
		return "IPv4Address"
	default:
		return fmt.Sprintf("Unknown-%d", t)
	}
}

func DecodePacket(contents []byte) (out *MMDPPacket, err error) {
	pkt := MMDPPacket{
		SeqNo: 0,
		Parts: make([]MMDPPart, 0),
	}

	breader := bytes.NewReader(contents)
	err = binary.Read(breader, binary.LittleEndian, &pkt.SeqNo)
	if err != nil {
		return nil, err
	}

	for {
		Part := MMDPPart{}
		err = binary.Read(breader, binary.BigEndian, &Part.Type)
		if err != nil {
			if err == io.EOF {
				break
			}
			return nil, err
		}
		ContentsLength := uint16(0)
		err = binary.Read(breader, binary.BigEndian, &ContentsLength)
		if err != nil {
			return nil, err
		}

		switch Part.Type {
		case MMDPTypeMACAddress:
			MAC := make(net.HardwareAddr, ContentsLength)
			_, err = io.ReadFull(breader, MAC)
			Part.Value = MAC
			if err != nil {
				return nil, err
			}
		case MMDPTypeIdentity, MMDPTypeVersion, MMDPTypePlatform, MMDPTypeSoftwareID, MMDPTypeBoard, MMDPTypeInterfaceName:
			s := make([]byte, ContentsLength)
			_, err = io.ReadFull(breader, s)
			Part.Value = string(s)
			if err != nil {
				return nil, err
			}
		case MMDPTypeUptime:
			t := uint32(0)
			err = binary.Read(breader, binary.LittleEndian, &t)
			if err != nil {
				return nil, err
			}
			Part.Value = time.Duration(time.Second * time.Duration(t))
		case MMDPTypeUnpack:
			dunno := make([]byte, ContentsLength)
			_, err = io.ReadFull(breader, dunno)
			Part.Value = dunno
			if err != nil {
				return nil, err
			}
		case MMDPTypeIPv6Address, MMDPTypeIPv4Address:
			ip := make(net.IP, ContentsLength)
			_, err = io.ReadFull(breader, ip)
			Part.Value = ip
			if err != nil {
				return nil, err
			}
		default:
			dunno := make([]byte, ContentsLength)
			_, err = io.ReadFull(breader, dunno)
			Part.Value = dunno
			if err != nil {
				return nil, err
			}
		}
		pkt.Parts = append(pkt.Parts, Part)
	}

	return &pkt, nil
}