From 954edcd1474efee73459171ccdb59673383cf64a Mon Sep 17 00:00:00 2001 From: Jesus Trujillo Date: Mon, 20 Feb 2017 16:11:10 -0800 Subject: [PATCH] refactor link control and add talker alias support --- lc/gpsinfo.go | 70 +++++++++++ lc/lc.go | 173 ++++++++++++++++++++++++++ lc/serviceoptions/serviceoptions.go | 87 +++++++++++++ lc/talkeralias.go | 89 +++++++++++++ lc/voicechanneluser.go | 46 +++++++ terminal/terminal.go | 15 +-- voice.go | 186 ---------------------------- 7 files changed, 473 insertions(+), 193 deletions(-) create mode 100644 lc/gpsinfo.go create mode 100644 lc/lc.go create mode 100644 lc/serviceoptions/serviceoptions.go create mode 100644 lc/talkeralias.go create mode 100644 lc/voicechanneluser.go diff --git a/lc/gpsinfo.go b/lc/gpsinfo.go new file mode 100644 index 0000000..d48c626 --- /dev/null +++ b/lc/gpsinfo.go @@ -0,0 +1,70 @@ +package lc + +import ( + "fmt" + + dmr "github.com/pd0mz/go-dmr" +) + +// Data Format +// ref: ETSI TS 102 361-2 7.2.18 +const ( + ErrorLT2m uint8 = iota + ErrorLT20m + ErrorLT200m + ErrorLT2km + ErrorLT20km + ErrorLE200km + ErrorGT200km + ErrorUnknown +) + +// PositionErrorName is a map of position error to string. +var PositionErrorName = map[uint8]string{ + ErrorLT2m: "< 2m", + ErrorLT20m: "< 20m", + ErrorLT200m: "< 200m", + ErrorLT2km: "< 2km", + ErrorLT20km: "< 20km", + ErrorLE200km: "<= 200km", + ErrorGT200km: "> 200km", + ErrorUnknown: "unknown", +} + +// GpsInfoPDU Conforms to ETSI TS 102 361-2 7.1.1.3 +type GpsInfoPDU struct { + PositionError uint8 + Longitude uint32 + Latitude uint32 +} + +// ParseGpsInfoPDU parse gps info pdu +func ParseGpsInfoPDU(data []byte) (*GpsInfoPDU, error) { + if len(data) != 7 { + return nil, fmt.Errorf("dmr/lc/talkeralias: expected 7 bytes, got %d", len(data)) + } + + return &GpsInfoPDU{ + PositionError: (data[0] & dmr.B00001110) >> 1, + Longitude: uint32(data[0]&dmr.B00000001)<<24 | uint32(data[1])<<16 | uint32(data[2])<<8 | uint32(data[3]), + Latitude: uint32(data[4])<<16 | uint32(data[5])<<8 | uint32(data[6]), + }, nil +} + +// Bytes returns GpsInfoPDU as bytes +func (g *GpsInfoPDU) Bytes() []byte { + return []byte{ + uint8((g.PositionError&dmr.B00000111)<<1) | uint8((g.Longitude>>24)&dmr.B00000001), + uint8(g.Longitude >> 16), + uint8(g.Longitude >> 8), + uint8(g.Longitude), + uint8(g.Latitude >> 16), + uint8(g.Latitude >> 8), + uint8(g.Latitude), + } +} + +func (g *GpsInfoPDU) String() string { + return fmt.Sprintf("GpsInfo: [ error: %s lon: %d lat: %d ]", + PositionErrorName[g.PositionError], g.Longitude, g.Latitude) +} diff --git a/lc/lc.go b/lc/lc.go new file mode 100644 index 0000000..3e1d0ee --- /dev/null +++ b/lc/lc.go @@ -0,0 +1,173 @@ +package lc + +import ( + "errors" + "fmt" + + "github.com/pd0mz/go-dmr" + "github.com/pd0mz/go-dmr/fec" +) + +// Full Link Control Opcode +const ( + GroupVoiceChannelUser uint8 = 0x00 // B000000 + UnitToUnitVoiceChannelUser uint8 = 0x03 // B000011 + TalkerAliasHeader uint8 = 0x04 // B000100 + TalkerAliasBlk1 uint8 = 0x05 // B000101 + TalkerAliasBlk2 uint8 = 0x06 // B000110 + TalkerAliasBlk3 uint8 = 0x07 // B000111 + GpsInfo uint8 = 0x08 // B001000 +) + +// LC is a Link Control message. +type LC struct { + CallType uint8 + Opcode uint8 + FeatureSetID uint8 + VoiceChannelUser *VoiceChannelUserPDU + GpsInfo *GpsInfoPDU + TalkerAliasHeader *TalkerAliasHeaderPDU + TalkerAliasBlocks [3]*TalkerAliasBlockPDU +} + +// Bytes packs the Link Control message to bytes. +func (lc *LC) Bytes() []byte { + var ( + lcHeader = []byte{ + lc.Opcode, + lc.FeatureSetID, + } + innerPdu []byte + ) + + switch lc.Opcode { + case GroupVoiceChannelUser: + fallthrough + case UnitToUnitVoiceChannelUser: + innerPdu = lc.VoiceChannelUser.Bytes() + case TalkerAliasHeader: + innerPdu = lc.TalkerAliasHeader.Bytes() + case TalkerAliasBlk1: + innerPdu = lc.TalkerAliasBlocks[0].Bytes() + case TalkerAliasBlk2: + innerPdu = lc.TalkerAliasBlocks[1].Bytes() + case TalkerAliasBlk3: + innerPdu = lc.TalkerAliasBlocks[2].Bytes() + case GpsInfo: + innerPdu = lc.GpsInfo.Bytes() + } + + return append(lcHeader, innerPdu...) +} + +func (lc *LC) String() string { + var ( + header = fmt.Sprintf("opcode %d, call type %s, feature set id %d", + lc.Opcode, dmr.CallTypeName[lc.CallType], lc.FeatureSetID) + r string + ) + + switch lc.Opcode { + case GroupVoiceChannelUser: + fallthrough + case UnitToUnitVoiceChannelUser: + r = fmt.Sprintf("%s %v", header, lc.VoiceChannelUser) + case GpsInfo: + r = fmt.Sprintf("%s %v", header, lc.GpsInfo) + case TalkerAliasHeader: + r = fmt.Sprintf("%s %v", header, lc.TalkerAliasHeader) + case TalkerAliasBlk1: + r = fmt.Sprintf("%s %v", header, lc.TalkerAliasBlocks[0]) + case TalkerAliasBlk2: + r = fmt.Sprintf("%s %v", header, lc.TalkerAliasBlocks[1]) + case TalkerAliasBlk3: + r = fmt.Sprintf("%s %v", header, lc.TalkerAliasBlocks[2]) + } + + return r +} + +// ParseLC parses a packed Link Control message. +func ParseLC(data []byte) (*LC, error) { + if data == nil { + return nil, errors.New("dmr/lc: data can't be nil") + } + if len(data) != 9 { + return nil, fmt.Errorf("dmr/lc: expected 9 LC bytes, got %d", len(data)) + } + + if data[0]&dmr.B10000000 > 0 { + return nil, errors.New("dmr/lc: protect flag is not 0") + } + + var ( + err error + fclo = data[0] & dmr.B00111111 + lc = &LC{ + Opcode: fclo, + FeatureSetID: data[1], + } + ) + switch fclo { + case GroupVoiceChannelUser: + var pdu *VoiceChannelUserPDU + lc.CallType = dmr.CallTypeGroup + pdu, err = ParseVoiceChannelUserPDU(data[2:9]) + lc.VoiceChannelUser = pdu + case UnitToUnitVoiceChannelUser: + var pdu *VoiceChannelUserPDU + lc.CallType = dmr.CallTypePrivate + pdu, err = ParseVoiceChannelUserPDU(data[2:9]) + lc.VoiceChannelUser = pdu + case TalkerAliasHeader: + var pdu *TalkerAliasHeaderPDU + pdu, err = ParseTalkerAliasHeaderPDU(data[2:9]) + lc.TalkerAliasHeader = pdu + case TalkerAliasBlk1: + var pdu *TalkerAliasBlockPDU + pdu, err = ParseTalkerAliasBlockPDU(data[2:9]) + lc.TalkerAliasBlocks[0] = pdu + case TalkerAliasBlk2: + var pdu *TalkerAliasBlockPDU + pdu, err = ParseTalkerAliasBlockPDU(data[2:9]) + lc.TalkerAliasBlocks[1] = pdu + case TalkerAliasBlk3: + var pdu *TalkerAliasBlockPDU + pdu, err = ParseTalkerAliasBlockPDU(data[2:9]) + lc.TalkerAliasBlocks[2] = pdu + case GpsInfo: + var pdu *GpsInfoPDU + pdu, err = ParseGpsInfoPDU(data[2:9]) + lc.GpsInfo = pdu + default: + return nil, fmt.Errorf("dmr/lc: unknown FCLO %06b (%d)", fclo, fclo) + } + + if err != nil { + return nil, fmt.Errorf("error parsing link control header pdu: %s", err) + } + + return lc, nil +} + +// ParseFullLC parses a packed Link Control message and checks/corrects the Reed-Solomon check data. +func ParseFullLC(data []byte) (*LC, error) { + if data == nil { + return nil, errors.New("dmr/full lc: data can't be nil") + } + if len(data) != 12 { + return nil, fmt.Errorf("dmr/full lc: expected 12 bytes, got %d", len(data)) + } + + syndrome := &fec.RS_12_9_Poly{} + if err := fec.RS_12_9_CalcSyndrome(data, syndrome); err != nil { + return nil, err + } + if !fec.RS_12_9_CheckSyndrome(syndrome) { + if _, err := fec.RS_12_9_Correct(data, syndrome); err != nil { + return nil, err + } + } + + return ParseLC(data[:9]) +} diff --git a/lc/serviceoptions/serviceoptions.go b/lc/serviceoptions/serviceoptions.go new file mode 100644 index 0000000..112440a --- /dev/null +++ b/lc/serviceoptions/serviceoptions.go @@ -0,0 +1,87 @@ +package serviceoptions + +import ( + "fmt" + "strings" + + dmr "github.com/pd0mz/go-dmr" +) + +// Priority Levels +const ( + NoPriority uint8 = iota + Priority1 + Priority2 + Priority3 +) + +// PriorityName is a map of priority level to string. +var PriorityName = map[uint8]string{ + NoPriority: "no priority", + Priority1: "priority 1", + Priority2: "priority 2", + Priority3: "priority 3", +} + +// ServiceOptions Conforms to ETSI TS 102-361-2 7.2.1 +type ServiceOptions struct { + // Emergency service + Emergency bool + // Not defined in document + Privacy bool + // Broadcast service (only defined in group calls) + Broadcast bool + // Open Voice Call Mode + OpenVoiceCallMode bool + // Priority 3 (0b11) is the highest priority + Priority uint8 +} + +// Byte packs the service options to a single byte. +func (so *ServiceOptions) Byte() byte { + var b byte + if so.Emergency { + b |= dmr.B00000001 + } + if so.Privacy { + b |= dmr.B00000010 + } + if so.Broadcast { + b |= dmr.B00010000 + } + if so.OpenVoiceCallMode { + b |= dmr.B00100000 + } + b |= (so.Priority << 6) + return b +} + +// String representatation of the service options. +func (so *ServiceOptions) String() string { + var part = []string{} + if so.Emergency { + part = append(part, "emergency") + } + if so.Privacy { + part = append(part, "privacy") + } + if so.Broadcast { + part = append(part, "broadcast") + } + if so.OpenVoiceCallMode { + part = append(part, "Open Voice Call Mode") + } + part = append(part, fmt.Sprintf("%s (%d)", PriorityName[so.Priority], so.Priority)) + return strings.Join(part, ", ") +} + +// ParseServiceOptions parses the service options byte. +func ParseServiceOptions(data byte) ServiceOptions { + return ServiceOptions{ + Emergency: (data & dmr.B00000001) > 0, + Privacy: (data & dmr.B00000010) > 0, + Broadcast: (data & dmr.B00010000) > 0, + OpenVoiceCallMode: (data & dmr.B00100000) > 0, + Priority: (data & dmr.B11000000) >> 6, + } +} diff --git a/lc/talkeralias.go b/lc/talkeralias.go new file mode 100644 index 0000000..4f244aa --- /dev/null +++ b/lc/talkeralias.go @@ -0,0 +1,89 @@ +package lc + +import ( + "fmt" + + dmr "github.com/pd0mz/go-dmr" +) + +// Data Format +// ref: ETSI TS 102 361-2 7.2.18 +const ( + Format7Bit uint8 = iota + FormatISO8Bit + FormatUTF8 + FormatUTF16BE +) + +// DataFormatName is a map of data format to string. +var DataFormatName = map[uint8]string{ + Format7Bit: "7 bit", + FormatISO8Bit: "ISO 8 bit", + FormatUTF8: "unicode utf-8", + FormatUTF16BE: "unicode utf-16be", +} + +// TalkerAliasHeaderPDU Conforms to ETSI TS 102 361-2 7.1.1.4 +type TalkerAliasHeaderPDU struct { + DataFormat uint8 + Length uint8 + Data []byte +} + +// TalkerAliasBlockPDU Conforms to ETSI TS 102 361-2 7.1.1.5 +type TalkerAliasBlockPDU struct { + Data []byte +} + +// ParseTalkerAliasHeaderPDU parses TalkerAliasHeader PDU from bytes +func ParseTalkerAliasHeaderPDU(data []byte) (*TalkerAliasHeaderPDU, error) { + if len(data) != 7 { + return nil, fmt.Errorf("dmr/lc/talkeralias: expected 7 bytes, got %d", len(data)) + } + + // TODO parse bit 49 + + return &TalkerAliasHeaderPDU{ + DataFormat: (data[0] & dmr.B11000000) >> 6, + Length: (data[0] & dmr.B00111110) >> 1, + Data: data[1:6], + }, nil +} + +// Bytes returns object as bytes +func (t *TalkerAliasHeaderPDU) Bytes() []byte { + return []byte{ + ((t.DataFormat << 6) & dmr.B11000000) | ((t.Length << 1) & dmr.B00111110), // TODO bit 49 + t.Data[0], + t.Data[1], + t.Data[2], + t.Data[3], + t.Data[4], + t.Data[5], + } +} + +func (t *TalkerAliasHeaderPDU) String() string { + return fmt.Sprintf("TalkerAliasHeader: [ format: %s, length: %d, data: \"%s\" ]", + DataFormatName[t.DataFormat], t.Length, string(t.Data)) +} + +// ParseTalkerAliasBlockPDU parse talker alias block pdu +func ParseTalkerAliasBlockPDU(data []byte) (*TalkerAliasBlockPDU, error) { + if len(data) != 7 { + return nil, fmt.Errorf("dmr/lc/talkeralias: expected 7 bytes, got %d", len(data)) + } + + return &TalkerAliasBlockPDU{ + Data: data[0:6], + }, nil +} + +// Bytes returns object as bytes +func (t *TalkerAliasBlockPDU) Bytes() []byte { + return t.Data +} + +func (t *TalkerAliasBlockPDU) String() string { + return fmt.Sprintf("TalkerAliasBlock: [ data: \"%s\" ]", string(t.Data)) +} diff --git a/lc/voicechanneluser.go b/lc/voicechanneluser.go new file mode 100644 index 0000000..d1228f1 --- /dev/null +++ b/lc/voicechanneluser.go @@ -0,0 +1,46 @@ +package lc + +import ( + "fmt" + + "github.com/pd0mz/go-dmr/lc/serviceoptions" +) + +// VoiceChannelUserPDU Conforms to ETSI TS 102-361-2 7.1.1.(1 and 2) +type VoiceChannelUserPDU struct { + ServiceOptions serviceoptions.ServiceOptions + DstID uint32 + SrcID uint32 +} + +// ParseVoiceChannelUserPDU Parses either Group Voice Channel User or +// Unit to Unit Channel User PDUs +func ParseVoiceChannelUserPDU(data []byte) (*VoiceChannelUserPDU, error) { + if len(data) != 7 { + return nil, fmt.Errorf("dmr/lc/voicechanneluser: expected 7 bytes, got %d", len(data)) + } + + return &VoiceChannelUserPDU{ + ServiceOptions: serviceoptions.ParseServiceOptions(data[0]), + DstID: uint32(data[1])<<16 | uint32(data[2])<<8 | uint32(data[3]), + SrcID: uint32(data[4])<<16 | uint32(data[5])<<8 | uint32(data[6]), + }, nil +} + +// Bytes packs the Voice Channel User PDU message to bytes. +func (v *VoiceChannelUserPDU) Bytes() []byte { + return []byte{ + v.ServiceOptions.Byte(), + uint8(v.DstID >> 16), + uint8(v.DstID >> 8), + uint8(v.DstID), + uint8(v.SrcID >> 16), + uint8(v.SrcID >> 8), + uint8(v.SrcID), + } +} + +func (v *VoiceChannelUserPDU) String() string { + return fmt.Sprintf("VoiceChannelUser: [ %d->%d, service options %s ]", + v.SrcID, v.DstID, v.ServiceOptions.String()) +} diff --git a/terminal/terminal.go b/terminal/terminal.go index bf0f09d..118c7a5 100644 --- a/terminal/terminal.go +++ b/terminal/terminal.go @@ -10,6 +10,7 @@ import ( "github.com/op/go-logging" "github.com/pd0mz/go-dmr" "github.com/pd0mz/go-dmr/bptc" + "github.com/pd0mz/go-dmr/lc" "github.com/pd0mz/go-dmr/trellis" "github.com/pd0mz/go-dmr/vbptc" ) @@ -370,7 +371,7 @@ func (t *Terminal) handlePacket(r dmr.Repeater, p *dmr.Packet) error { var err error t.warningf(p, "handle packet: %s", dmr.DataTypeName[p.DataType]) - log.Debug(hex.Dump(p.Data)) + //log.Debug(hex.Dump(p.Data)) // switch p.DataType { @@ -533,12 +534,12 @@ func (t *Terminal) handleTerminatorWithLC(p *dmr.Packet) error { data[10] ^= 0x99 data[11] ^= 0x99 - lc, err := dmr.ParseFullLC(data) + lc, err := lc.ParseFullLC(data) if err != nil { return err } - t.debugf(p, "lc: %s", lc.String()) + t.debugf(p, "terminator with lc: %s", lc.String()) return nil } @@ -612,11 +613,11 @@ func (t *Terminal) handleVoice(p *dmr.Packet) error { return errors.New("embedded signalling LC checksum failed") } - lc, err := dmr.ParseLC(dmr.BitsToBytes(eslc.Bits)) + lc, err := lc.ParseLC(dmr.BitsToBytes(eslc.Bits)) if err != nil { return err } - t.debugf(p, "lc: %s", lc.String()) + t.debugf(p, "voice embedded lc: %s", lc.String()) } } @@ -649,12 +650,12 @@ func (t *Terminal) handleVoiceLC(p *dmr.Packet) error { data[10] ^= 0x96 data[11] ^= 0x96 - lc, err := dmr.ParseFullLC(data) + lc, err := lc.ParseFullLC(data) if err != nil { return err } - t.debugf(p, "lc: %s", lc.String()) + t.debugf(p, "voice header lc: %s", lc.String()) return nil } diff --git a/voice.go b/voice.go index 9ff4cfa..14b3717 100644 --- a/voice.go +++ b/voice.go @@ -3,196 +3,10 @@ package dmr import ( "errors" "fmt" - "strings" "github.com/pd0mz/go-dmr/crc/quadres_16_7" - "github.com/pd0mz/go-dmr/fec" ) -// Priority Levels -const ( - NoPriority uint8 = iota - Priority1 - Priority2 - Priority3 -) - -// PriorityName is a map of priority level to string. -var PriorityName = map[uint8]string{ - NoPriority: "no priority", - Priority1: "priority 1", - Priority2: "priority 2", - Priority3: "priority 3", -} - -// ServiceOptions as per DMR part 2, section 7.2.1. -type ServiceOptions struct { - // Emergency service - Emergency bool - // Not defined in document - Privacy bool - // Broadcast service (only defined in group calls) - Broadcast bool - // Open Voice Call Mode - OpenVoiceCallMode bool - // Priority 3 (0b11) is the highest priority - Priority uint8 -} - -// Byte packs the service options to a single byte. -func (so *ServiceOptions) Byte() byte { - var b byte - if so.Emergency { - b |= B00000001 - } - if so.Privacy { - b |= B00000010 - } - if so.Broadcast { - b |= B00010000 - } - if so.OpenVoiceCallMode { - b |= B00100000 - } - b |= (so.Priority << 6) - return b -} - -// String representatation of the service options. -func (so *ServiceOptions) String() string { - var part = []string{} - if so.Emergency { - part = append(part, "emergency") - } - if so.Privacy { - part = append(part, "privacy") - } - if so.Broadcast { - part = append(part, "broadcast") - } - if so.OpenVoiceCallMode { - part = append(part, "Open Voice Call Mode") - } - part = append(part, fmt.Sprintf("%s (%d)", PriorityName[so.Priority], so.Priority)) - return strings.Join(part, ", ") -} - -// ParseServiceOptions parses the service options byte. -func ParseServiceOptions(data byte) ServiceOptions { - return ServiceOptions{ - Emergency: (data & B00000001) > 0, - Privacy: (data & B00000010) > 0, - Broadcast: (data & B00010000) > 0, - OpenVoiceCallMode: (data & B00100000) > 0, - Priority: (data & B11000000) >> 6, - } -} - -// Full Link Control Opcode -const ( - GroupVoiceChannelUser uint8 = 0x00 // B000000 - UnitToUnitVoiceChannelUser uint8 = 0x03 // B000011 -) - -// LC is a Link Control message. -type LC struct { - CallType uint8 - Opcode uint8 - FeatureSetID uint8 - ServiceOptions ServiceOptions - DstID uint32 - SrcID uint32 -} - -// Bytes packs the Link Control message to bytes. -func (lc *LC) Bytes() []byte { - var fclo uint8 - switch lc.CallType { - case CallTypeGroup: - fclo = GroupVoiceChannelUser - break - case CallTypePrivate: - fclo = UnitToUnitVoiceChannelUser - break - } - - return []byte{ - fclo, - lc.FeatureSetID, - lc.ServiceOptions.Byte(), - uint8(lc.DstID >> 16), - uint8(lc.DstID >> 8), - uint8(lc.DstID), - uint8(lc.SrcID >> 16), - uint8(lc.SrcID >> 8), - uint8(lc.SrcID), - } -} - -func (lc *LC) String() string { - return fmt.Sprintf("call type %s, feature set id %d, %d->%d, service options %s", - CallTypeName[lc.CallType], lc.FeatureSetID, lc.SrcID, lc.DstID, lc.ServiceOptions.String()) -} - -// ParseLC parses a packed Link Control message. -func ParseLC(data []byte) (*LC, error) { - if data == nil { - return nil, errors.New("dmr/lc: data can't be nil") - } - if len(data) != 9 { - return nil, fmt.Errorf("dmr/lc: expected 9 LC bytes, got %d", len(data)) - } - - if data[0]&B10000000 > 0 { - return nil, errors.New("dmr/lc: protect flag is not 0") - } - - var ( - ct uint8 - fclo = data[0] & B00111111 - ) - switch fclo { - case GroupVoiceChannelUser: - ct = CallTypeGroup - break - case UnitToUnitVoiceChannelUser: - ct = CallTypePrivate - break - default: - return nil, fmt.Errorf("dmr/lc: unknown FCLO %06b (%d)", fclo, fclo) - } - - return &LC{ - CallType: ct, - FeatureSetID: data[1], - ServiceOptions: ParseServiceOptions(data[2]), - DstID: uint32(data[3])<<16 | uint32(data[4])<<8 | uint32(data[5]), - SrcID: uint32(data[6])<<16 | uint32(data[7])<<8 | uint32(data[8]), - }, nil -} - -// ParseFullLC parses a packed Link Control message and checks/corrects the Reed-Solomon check data. -func ParseFullLC(data []byte) (*LC, error) { - if data == nil { - return nil, errors.New("dmr/full lc: data can't be nil") - } - if len(data) != 12 { - return nil, fmt.Errorf("dmr/full lc: expected 12 bytes, got %d", len(data)) - } - - syndrome := &fec.RS_12_9_Poly{} - if err := fec.RS_12_9_CalcSyndrome(data, syndrome); err != nil { - return nil, err - } - if !fec.RS_12_9_CheckSyndrome(syndrome) { - if _, err := fec.RS_12_9_Correct(data, syndrome); err != nil { - return nil, err - } - } - - return ParseLC(data[:9]) -} - // EMB LCSS fragments. const ( SingleFragment uint8 = iota