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  }