1// Copyright 2018 Google, Inc. All rights reserved.
2//
3// Use of this source code is governed by a BSD-style license
4// that can be found in the LICENSE file in the root of the source
5// tree.
6
7package layers
8
9import (
10	"encoding/binary"
11	"errors"
12	"fmt"
13
14	"github.com/google/gopacket"
15)
16
17const (
18	// LCMShortHeaderMagic is the LCM small message header magic number
19	LCMShortHeaderMagic uint32 = 0x4c433032
20	// LCMFragmentedHeaderMagic is the LCM fragmented message header magic number
21	LCMFragmentedHeaderMagic uint32 = 0x4c433033
22)
23
24// LCM (Lightweight Communications and Marshalling) is a set of libraries and
25// tools for message passing and data marshalling, targeted at real-time systems
26// where high-bandwidth and low latency are critical. It provides a
27// publish/subscribe message passing model and automatic
28// marshalling/unmarshalling code generation with bindings for applications in a
29// variety of programming languages.
30//
31// References
32//   https://lcm-proj.github.io/
33//   https://github.com/lcm-proj/lcm
34type LCM struct {
35	// Common (short & fragmented header) fields
36	Magic          uint32
37	SequenceNumber uint32
38	// Fragmented header only fields
39	PayloadSize    uint32
40	FragmentOffset uint32
41	FragmentNumber uint16
42	TotalFragments uint16
43	// Common field
44	ChannelName string
45	// Gopacket helper fields
46	Fragmented  bool
47	fingerprint LCMFingerprint
48	contents    []byte
49	payload     []byte
50}
51
52// LCMFingerprint is the type of a LCM fingerprint.
53type LCMFingerprint uint64
54
55var (
56	// lcmLayerTypes contains a map of all LCM fingerprints that we support and
57	// their LayerType
58	lcmLayerTypes  = map[LCMFingerprint]gopacket.LayerType{}
59	layerTypeIndex = 1001
60)
61
62// RegisterLCMLayerType allows users to register decoders for the underlying
63// LCM payload. This is done based on the fingerprint that every LCM message
64// contains and which identifies it uniquely. If num is not the zero value it
65// will be used when registering with RegisterLayerType towards gopacket,
66// otherwise an incremental value starting from 1001 will be used.
67func RegisterLCMLayerType(num int, name string, fingerprint LCMFingerprint,
68	decoder gopacket.Decoder) gopacket.LayerType {
69	metadata := gopacket.LayerTypeMetadata{Name: name, Decoder: decoder}
70
71	if num == 0 {
72		num = layerTypeIndex
73		layerTypeIndex++
74	}
75
76	lcmLayerTypes[fingerprint] = gopacket.RegisterLayerType(num, metadata)
77
78	return lcmLayerTypes[fingerprint]
79}
80
81// SupportedLCMFingerprints returns a slice of all LCM fingerprints that has
82// been registered so far.
83func SupportedLCMFingerprints() []LCMFingerprint {
84	fingerprints := make([]LCMFingerprint, 0, len(lcmLayerTypes))
85	for fp := range lcmLayerTypes {
86		fingerprints = append(fingerprints, fp)
87	}
88	return fingerprints
89}
90
91// GetLCMLayerType returns the underlying LCM message's LayerType.
92// This LayerType has to be registered by using RegisterLCMLayerType.
93func GetLCMLayerType(fingerprint LCMFingerprint) gopacket.LayerType {
94	layerType, ok := lcmLayerTypes[fingerprint]
95	if !ok {
96		return gopacket.LayerTypePayload
97	}
98
99	return layerType
100}
101
102func decodeLCM(data []byte, p gopacket.PacketBuilder) error {
103	lcm := &LCM{}
104
105	err := lcm.DecodeFromBytes(data, p)
106	if err != nil {
107		return err
108	}
109
110	p.AddLayer(lcm)
111	p.SetApplicationLayer(lcm)
112
113	return p.NextDecoder(lcm.NextLayerType())
114}
115
116// DecodeFromBytes decodes the given bytes into this layer.
117func (lcm *LCM) DecodeFromBytes(data []byte, df gopacket.DecodeFeedback) error {
118	if len(data) < 8 {
119		df.SetTruncated()
120		return errors.New("LCM < 8 bytes")
121	}
122	offset := 0
123
124	lcm.Magic = binary.BigEndian.Uint32(data[offset:4])
125	offset += 4
126
127	if lcm.Magic != LCMShortHeaderMagic && lcm.Magic != LCMFragmentedHeaderMagic {
128		return fmt.Errorf("Received LCM header magic %v does not match know "+
129			"LCM magic numbers. Dropping packet.", lcm.Magic)
130	}
131
132	lcm.SequenceNumber = binary.BigEndian.Uint32(data[offset:8])
133	offset += 4
134
135	if lcm.Magic == LCMFragmentedHeaderMagic {
136		lcm.Fragmented = true
137
138		lcm.PayloadSize = binary.BigEndian.Uint32(data[offset : offset+4])
139		offset += 4
140
141		lcm.FragmentOffset = binary.BigEndian.Uint32(data[offset : offset+4])
142		offset += 4
143
144		lcm.FragmentNumber = binary.BigEndian.Uint16(data[offset : offset+2])
145		offset += 2
146
147		lcm.TotalFragments = binary.BigEndian.Uint16(data[offset : offset+2])
148		offset += 2
149	} else {
150		lcm.Fragmented = false
151	}
152
153	if !lcm.Fragmented || (lcm.Fragmented && lcm.FragmentNumber == 0) {
154		buffer := make([]byte, 0)
155		for _, b := range data[offset:] {
156			offset++
157
158			if b == 0 {
159				break
160			}
161
162			buffer = append(buffer, b)
163		}
164
165		lcm.ChannelName = string(buffer)
166	}
167
168	lcm.fingerprint = LCMFingerprint(
169		binary.BigEndian.Uint64(data[offset : offset+8]))
170
171	lcm.contents = data[:offset]
172	lcm.payload = data[offset:]
173
174	return nil
175}
176
177// CanDecode returns a set of layers that LCM objects can decode.
178// As LCM objects can only decode the LCM layer, we just return that layer.
179func (lcm LCM) CanDecode() gopacket.LayerClass {
180	return LayerTypeLCM
181}
182
183// NextLayerType specifies the LCM payload layer type following this header.
184// As LCM packets are serialized structs with uniq fingerprints for each uniq
185// combination of data types, lookup of correct layer type is based on that
186// fingerprint.
187func (lcm LCM) NextLayerType() gopacket.LayerType {
188	if !lcm.Fragmented || (lcm.Fragmented && lcm.FragmentNumber == 0) {
189		return GetLCMLayerType(lcm.fingerprint)
190	}
191
192	return gopacket.LayerTypeFragment
193}
194
195// LayerType returns LayerTypeLCM
196func (lcm LCM) LayerType() gopacket.LayerType {
197	return LayerTypeLCM
198}
199
200// LayerContents returns the contents of the LCM header.
201func (lcm LCM) LayerContents() []byte {
202	return lcm.contents
203}
204
205// LayerPayload returns the payload following this LCM header.
206func (lcm LCM) LayerPayload() []byte {
207	return lcm.payload
208}
209
210// Payload returns the payload following this LCM header.
211func (lcm LCM) Payload() []byte {
212	return lcm.LayerPayload()
213}
214
215// Fingerprint returns the LCM fingerprint of the underlying message.
216func (lcm LCM) Fingerprint() LCMFingerprint {
217	return lcm.fingerprint
218}
219