blob: 5fe9fa544090ed94a8c670cf34bb5f6019bcf9b0 [file] [log] [blame]
Matteo Scandoloa6a3aee2019-11-26 13:30:14 -07001// 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}