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