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