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