123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213 |
- package layers
- import (
- "encoding/binary"
- "fmt"
- "github.com/google/gopacket"
- )
- const (
-
- LCMShortHeaderMagic uint32 = 0x4c433032
-
- LCMFragmentedHeaderMagic uint32 = 0x4c433033
- )
- type LCM struct {
-
- Magic uint32
- SequenceNumber uint32
-
- PayloadSize uint32
- FragmentOffset uint32
- FragmentNumber uint16
- TotalFragments uint16
-
- ChannelName string
-
- Fragmented bool
- fingerprint LCMFingerprint
- contents []byte
- payload []byte
- }
- type LCMFingerprint uint64
- var (
-
-
- lcmLayerTypes = map[LCMFingerprint]gopacket.LayerType{}
- layerTypeIndex = 1001
- )
- func RegisterLCMLayerType(num int, name string, fingerprint LCMFingerprint,
- decoder gopacket.Decoder) gopacket.LayerType {
- metadata := gopacket.LayerTypeMetadata{Name: name, Decoder: decoder}
- if num == 0 {
- num = layerTypeIndex
- layerTypeIndex++
- }
- lcmLayerTypes[fingerprint] = gopacket.RegisterLayerType(num, metadata)
- return lcmLayerTypes[fingerprint]
- }
- func SupportedLCMFingerprints() []LCMFingerprint {
- fingerprints := make([]LCMFingerprint, 0, len(lcmLayerTypes))
- for fp := range lcmLayerTypes {
- fingerprints = append(fingerprints, fp)
- }
- return fingerprints
- }
- func GetLCMLayerType(fingerprint LCMFingerprint) gopacket.LayerType {
- layerType, ok := lcmLayerTypes[fingerprint]
- if !ok {
- return gopacket.LayerTypePayload
- }
- return layerType
- }
- func decodeLCM(data []byte, p gopacket.PacketBuilder) error {
- lcm := &LCM{}
- err := lcm.DecodeFromBytes(data, p)
- if err != nil {
- return err
- }
- p.AddLayer(lcm)
- p.SetApplicationLayer(lcm)
- return p.NextDecoder(lcm.NextLayerType())
- }
- func (lcm *LCM) DecodeFromBytes(data []byte, df gopacket.DecodeFeedback) error {
- offset := 0
- lcm.Magic = binary.BigEndian.Uint32(data[offset:4])
- offset += 4
- if lcm.Magic != LCMShortHeaderMagic && lcm.Magic != LCMFragmentedHeaderMagic {
- return fmt.Errorf("Received LCM header magic %v does not match know "+
- "LCM magic numbers. Dropping packet.", lcm.Magic)
- }
- lcm.SequenceNumber = binary.BigEndian.Uint32(data[offset:8])
- offset += 4
- if lcm.Magic == LCMFragmentedHeaderMagic {
- lcm.Fragmented = true
- lcm.PayloadSize = binary.BigEndian.Uint32(data[offset : offset+4])
- offset += 4
- lcm.FragmentOffset = binary.BigEndian.Uint32(data[offset : offset+4])
- offset += 4
- lcm.FragmentNumber = binary.BigEndian.Uint16(data[offset : offset+2])
- offset += 2
- lcm.TotalFragments = binary.BigEndian.Uint16(data[offset : offset+2])
- offset += 2
- } else {
- lcm.Fragmented = false
- }
- if !lcm.Fragmented || (lcm.Fragmented && lcm.FragmentNumber == 0) {
- buffer := make([]byte, 0)
- for _, b := range data[offset:] {
- offset++
- if b == 0 {
- break
- }
- buffer = append(buffer, b)
- }
- lcm.ChannelName = string(buffer)
- }
- lcm.fingerprint = LCMFingerprint(
- binary.BigEndian.Uint64(data[offset : offset+8]))
- lcm.contents = data[:offset]
- lcm.payload = data[offset:]
- return nil
- }
- func (lcm LCM) CanDecode() gopacket.LayerClass {
- return LayerTypeLCM
- }
- func (lcm LCM) NextLayerType() gopacket.LayerType {
- if !lcm.Fragmented || (lcm.Fragmented && lcm.FragmentNumber == 0) {
- return GetLCMLayerType(lcm.fingerprint)
- }
- return gopacket.LayerTypeFragment
- }
- func (lcm LCM) LayerType() gopacket.LayerType {
- return LayerTypeLCM
- }
- func (lcm LCM) LayerContents() []byte {
- return lcm.contents
- }
- func (lcm LCM) LayerPayload() []byte {
- return lcm.payload
- }
- func (lcm LCM) Payload() []byte {
- return lcm.LayerPayload()
- }
- func (lcm LCM) Fingerprint() LCMFingerprint {
- return lcm.fingerprint
- }
|