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