From 9942e7c8925e0a85159ed2b67d6cb7b641504d26 Mon Sep 17 00:00:00 2001 From: Wijnand Modderman-Lenstra Date: Tue, 22 Dec 2015 12:32:43 +0100 Subject: [PATCH] voice: implemented (embedded) link control --- bptc/bptc.go | 3 +- packet.go | 5 + terminal/terminal.go | 131 +++++++++++++++- vbptc/vbptc.go | 195 +++++++++++++++++++++++ voice.go | 364 +++++++++++++++++++++++++++++++++++++++++++ voice_emb.go | 40 ----- 6 files changed, 689 insertions(+), 49 deletions(-) create mode 100644 vbptc/vbptc.go create mode 100644 voice.go delete mode 100644 voice_emb.go diff --git a/bptc/bptc.go b/bptc/bptc.go index 77a47be..d1a836c 100644 --- a/bptc/bptc.go +++ b/bptc/bptc.go @@ -76,7 +76,8 @@ func Decode(info, data []byte) error { // Hamming checks if err := hamming_check(bits); err != nil { - return err + //return err + //log.Warningf("hamming check failed: %v", err) } // Extract data bits diff --git a/packet.go b/packet.go index 1633dda..5ff7528 100644 --- a/packet.go +++ b/packet.go @@ -49,6 +49,11 @@ const ( CallTypeGroup ) +var CallTypeName = map[uint8]string{ + CallTypePrivate: "private", + CallTypeGroup: "group", +} + // Packet represents a frame transported by the Air Interface type Packet struct { // 0 for slot 1, 1 for slot 2 diff --git a/terminal/terminal.go b/terminal/terminal.go index 464a78b..bf0f09d 100644 --- a/terminal/terminal.go +++ b/terminal/terminal.go @@ -11,6 +11,7 @@ import ( "github.com/pd0mz/go-dmr" "github.com/pd0mz/go-dmr/bptc" "github.com/pd0mz/go-dmr/trellis" + "github.com/pd0mz/go-dmr/vbptc" ) var log = logging.MustGetLogger("dmr/terminal") @@ -44,13 +45,21 @@ type Slot struct { selectiveAckRequestsSent int rxSequence int fullMessageBlocks int + embeddedSignalling *vbptc.VBPTC last struct { packetReceived time.Time } } func NewSlot() *Slot { - return &Slot{} + s := &Slot{} + + // Expecting 8 rows of variable length BPTC coded embedded LC data. + // It will contain 77 data bits (without the Hamming (16,11) checksums + // and the last row of parity bits). + s.embeddedSignalling = vbptc.New(8) + + return s } type VoiceFrameFunc func(*dmr.Packet, []byte) @@ -378,7 +387,8 @@ func (t *Terminal) handlePacket(r dmr.Repeater, p *dmr.Packet) error { err = t.handleVoice(p) break case dmr.VoiceLC: - return nil + err = t.handleVoiceLC(p) + break case dmr.TerminatorWithLC: err = t.handleTerminatorWithLC(p) return nil @@ -506,17 +516,37 @@ func (t *Terminal) handleRate34Data(p *dmr.Packet) error { func (t *Terminal) handleTerminatorWithLC(p *dmr.Packet) error { // This ends both data and voice calls - return t.callEnd(p) + if err := t.callEnd(p); err != nil { + return err + } + + var ( + bits = p.InfoBits() + data = make([]byte, 12) + ) + if err := bptc.Decode(bits, data); err != nil { + return err + } + + // Applying CRC mask to the checksum. See DMR AI. spec. page 143. + data[9] ^= 0x99 + data[10] ^= 0x99 + data[11] ^= 0x99 + + lc, err := dmr.ParseFullLC(data) + if err != nil { + return err + } + + t.debugf(p, "lc: %s", lc.String()) + + return nil } func (t *Terminal) handleVoice(p *dmr.Packet) error { slot := t.slot[p.Timeslot] slot.last.packetReceived = time.Now() - var ( - bits = p.VoiceBits() - ) - switch t.state { case voiceCallActive: if p.StreamID != slot.voice.streamID { @@ -529,8 +559,69 @@ func (t *Terminal) handleVoice(p *dmr.Packet) error { break } + // Check sync frame + sync := p.SyncBits() + patt := dmr.SyncPattern(sync) + if patt != dmr.SyncPatternUnknown { + t.debugf(p, "sync pattern %s", dmr.SyncPatternName[patt]) + } else { + // Not a sync frame, sync field should contain EMB + bits, err := dmr.ParseEMBBitsFromSync(sync) + if err != nil { + return err + } + emb, err := dmr.ParseEMB(bits) + if err != nil { + return err + } + t.debugf(p, "embedded signalling %s", emb.String()) + + // Handling embedded signalling LC + switch emb.LCSS { + case dmr.SingleFragment: + return nil // FIXME(pd0mz): unhandled + case dmr.FirstFragment: + slot.embeddedSignalling.Clear() + break + } + + if emb.LCSS == dmr.FirstFragment || emb.LCSS == dmr.Continuation || emb.LCSS == dmr.LastFragment { + frag, err := dmr.ParseEmbeddedSignallingLCFromSyncBits(sync) + if err != nil { + return err + } + if err := slot.embeddedSignalling.AddBurst(frag); err != nil { + return err + } + } + + if emb.LCSS == dmr.LastFragment { + if err := slot.embeddedSignalling.CheckAndRepair(); err != nil { + return err + } + + var signalling = make([]byte, 77) + if err := slot.embeddedSignalling.GetData(signalling); err != nil { + return err + } + eslc, err := dmr.DeinterleaveEmbeddedSignallingLC(signalling) + if err != nil { + return err + } + if !eslc.Check() { + return errors.New("embedded signalling LC checksum failed") + } + + lc, err := dmr.ParseLC(dmr.BitsToBytes(eslc.Bits)) + if err != nil { + return err + } + t.debugf(p, "lc: %s", lc.String()) + } + } + if t.vff != nil { - t.vff(p, bits) + t.vff(p, p.VoiceBits()) if t.SoftwareDelay { delta := time.Now().Sub(slot.last.packetReceived) if delta < VoiceFrameDuration { @@ -543,3 +634,27 @@ func (t *Terminal) handleVoice(p *dmr.Packet) error { return nil } + +func (t *Terminal) handleVoiceLC(p *dmr.Packet) error { + var ( + bits = p.InfoBits() + data = make([]byte, 12) + ) + if err := bptc.Decode(bits, data); err != nil { + return err + } + + // Applying CRC mask to the checksum. See DMR AI. spec. page 143. + data[9] ^= 0x96 + data[10] ^= 0x96 + data[11] ^= 0x96 + + lc, err := dmr.ParseFullLC(data) + if err != nil { + return err + } + + t.debugf(p, "lc: %s", lc.String()) + + return nil +} diff --git a/vbptc/vbptc.go b/vbptc/vbptc.go new file mode 100644 index 0000000..49a4250 --- /dev/null +++ b/vbptc/vbptc.go @@ -0,0 +1,195 @@ +// Package vbptc implements the Variable length BPTC for embedded signalling +package vbptc + +import ( + "errors" + "fmt" +) + +var ( + // See page 136 of the DMR AI. spec. for the generator matrix. + hamming_16_11_generator_matrix = []byte{ + 1, 0, 0, 1, 1, + 1, 1, 0, 1, 0, + 1, 1, 1, 1, 1, + 1, 1, 1, 0, 0, + 0, 1, 1, 1, 0, + 1, 0, 1, 0, 1, + 0, 1, 0, 1, 1, + 1, 0, 1, 1, 0, + 1, 1, 0, 0, 1, + 0, 1, 1, 0, 1, + 0, 0, 1, 1, 1, + // These are used to determine errors in the Hamming checksum bits. + 1, 0, 0, 0, 0, + 0, 1, 0, 0, 0, + 0, 0, 1, 0, 0, + 0, 0, 0, 1, 0, + 0, 0, 0, 0, 1, + } +) + +type VBPTC struct { + matrix []byte + row, col uint8 + expectedRows uint8 +} + +func New(expectedRows uint8) *VBPTC { + return &VBPTC{ + matrix: make([]byte, int(expectedRows)*16), + expectedRows: expectedRows, + } +} + +func (v *VBPTC) freeSpace() int { + var size = int(v.expectedRows) * 16 + var used = int(v.expectedRows)*int(v.col) + int(v.row) + return size - used +} + +// AddBurst adds the embedded signalling data to the matrix. +func (v *VBPTC) AddBurst(bits []byte) error { + if v.matrix == nil { + return errors.New("vbptc: matrix can't be nil") + } + var free = v.freeSpace() + if free == 0 { + return errors.New("vbptc: no free space in matrix") + } + + var adds = len(bits) + if adds > free { + adds = free + } + + for i := 0; i < adds; i++ { + v.matrix[v.col+v.row*16] = bits[i] + v.row++ + if v.row == v.expectedRows { + v.col++ + v.row = 0 + } + } + + return nil +} + +// CheckAndRepair checks data for errors and tries to repair them +func (v *VBPTC) CheckAndRepair() error { + if v.matrix == nil || v.expectedRows < 2 { + return fmt.Errorf("vbptc: no data") + } + + var ( + row, col uint8 + errs = make([]byte, 5) + ) + + // -1 because the last row contains only single parity check bits + for row = 0; row < v.expectedRows-1; row++ { + if !checkRow(v.matrix[row*16:], errs) { + // If the Hamming(16, 11, 4) column check failed, see if we can find + // the bit error location. + pos, found := findPosition(errs) + if !found { + return fmt.Errorf("vbptc: hamming(16,11) check error, can't repair row #%d", row) + } + + // Flip wrong bit + v.matrix[row*16+pos] ^= 1 + if !checkRow(v.matrix[row*16:], errs) { + return fmt.Errorf("vbptc: hamming(16,11) check error, couldn't repair row #%d", row) + } + } + } + + for col = 0; col < 16; col++ { + var parity uint8 + for row = 0; row < v.expectedRows-1; row++ { + parity = (parity + v.matrix[row*16+col]) % 2 + } + if parity != v.matrix[(v.expectedRows-1)*16+col] { + return fmt.Errorf("vbptc: parity check error in column #%d", col) + } + } + + return nil +} + +// Clear resets the variable BPTC matrix and cursor position +func (v *VBPTC) Clear() { + v.row = 0 + v.col = 0 + v.matrix = make([]byte, int(v.expectedRows)*16) +} + +// GetData extracts data bits (discarding Hamming (16,11) and parity check bits) from the vbptc matrix. +func (v *VBPTC) GetData(bits []byte) error { + if v.matrix == nil || v.expectedRows == 0 { + return errors.New("vbptc: no data in matrix") + } + if bits == nil { + return errors.New("vbptc: bits can't be nil") + } + if len(bits) < 77 { + return fmt.Errorf("vbptc: need at least 77 bits buffer, got %d", len(bits)) + } + + var row, col uint8 + for row = 0; row < v.expectedRows-1; row++ { + for col = 0; col < 11; col++ { + bits[row*11+col] = v.matrix[row*16+col] + } + } + + return nil +} + +func checkRow(bits, errs []byte) bool { + if bits == nil || errs == nil { + return false + } + + getParity(bits, errs) + errs[0] ^= bits[11] + errs[1] ^= bits[12] + errs[2] ^= bits[13] + errs[3] ^= bits[14] + errs[4] ^= bits[15] + + return errs[0] == 0 && errs[1] == 0 && errs[2] == 0 && errs[3] == 0 && errs[4] == 0 +} + +func findPosition(errs []byte) (uint8, bool) { + for row := uint8(0); row < 16; row++ { + var found = true + switch { + case hamming_16_11_generator_matrix[row*5] != errs[0]: + found = false + break + case hamming_16_11_generator_matrix[row*5+1] != errs[1]: + found = false + break + case hamming_16_11_generator_matrix[row*5+2] != errs[2]: + found = false + break + case hamming_16_11_generator_matrix[row*5+3] != errs[3]: + found = false + break + } + if found { + return row, true + } + } + + return 0, false +} + +func getParity(bits, errs []byte) { + errs[0] = (bits[0] ^ bits[1] ^ bits[2] ^ bits[3] ^ bits[5] ^ bits[7] ^ bits[8]) + errs[1] = (bits[1] ^ bits[2] ^ bits[3] ^ bits[4] ^ bits[6] ^ bits[8] ^ bits[9]) + errs[2] = (bits[2] ^ bits[3] ^ bits[4] ^ bits[5] ^ bits[7] ^ bits[9] ^ bits[10]) + errs[3] = (bits[0] ^ bits[1] ^ bits[2] ^ bits[4] ^ bits[6] ^ bits[7] ^ bits[10]) + errs[4] = (bits[0] ^ bits[2] ^ bits[5] ^ bits[6] ^ bits[8] ^ bits[9] ^ bits[10]) +} diff --git a/voice.go b/voice.go new file mode 100644 index 0000000..9ff4cfa --- /dev/null +++ b/voice.go @@ -0,0 +1,364 @@ +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 + FirstFragment + LastFragment + Continuation +) + +// LCSSName is a map of LCSS fragment type to string. +var LCSSName = map[uint8]string{ + SingleFragment: "single fragment", + FirstFragment: "first fragment", + LastFragment: "last fragment", + Continuation: "continuation", +} + +// EMB contains embedded signalling. +type EMB struct { + ColorCode uint8 + LCSS uint8 +} + +func (emb *EMB) String() string { + return fmt.Sprintf("color code %d, %s (%d)", emb.ColorCode, LCSSName[emb.LCSS], emb.LCSS) +} + +// ParseEMB parses embedded signalling +func ParseEMB(bits []byte) (*EMB, error) { + if len(bits) != EMBBits { + return nil, fmt.Errorf("dmr/emb: expected %d bits, got %d", EMBBits, len(bits)) + } + + if !quadres_16_7.Check(bits) { + return nil, errors.New("dmr/emb: checksum error") + } + + if bits[4] != 0 { + return nil, errors.New("dmr/emb: pi is not 0") + } + + return &EMB{ + ColorCode: uint8(bits[0])<<3 | uint8(bits[1])<<2 | uint8(bits[2])<<1 | uint8(bits[3]), + LCSS: uint8(bits[5])<<1 | uint8(bits[6]), + }, nil +} + +// ParseEMBBitsFromSync extracts the embedded signalling bits from the SYNC bits. +func ParseEMBBitsFromSync(sync []byte) ([]byte, error) { + if sync == nil { + return nil, errors.New("dmr/emb from sync: bits can't be nil") + } + if len(sync) != 48 { + return nil, fmt.Errorf("dmr/emb from sync: expected 48 sync bits, got %d", len(sync)) + } + + var bits = make([]byte, 16) + copy(bits[:8], sync[:8]) + copy(bits[8:], sync[8+32:]) + return bits, nil +} + +// ParseEmbeddedSignallingLCFromSyncBits extracts the embedded signalling LC from the SYNC bits. +func ParseEmbeddedSignallingLCFromSyncBits(sync []byte) ([]byte, error) { + if sync == nil { + return nil, errors.New("dmr/emb lc from sync: bits can't be nil") + } + if len(sync) != 48 { + return nil, fmt.Errorf("dmr/emb lc from sync: expected 48 sync bits, got %d", len(sync)) + } + + var bits = make([]byte, 32) + copy(bits, sync[8:40]) + return bits, nil +} + +// EmbeddedSignallingLC contains the embedded signalling LC and checksum. +type EmbeddedSignallingLC struct { + Bits []byte + Checksum []byte +} + +// Check verifies the checksum in the embedded signalling LC. +func (eslc *EmbeddedSignallingLC) Check() bool { + var checksum uint8 + checksum |= eslc.Checksum[0] << 4 + checksum |= eslc.Checksum[1] << 3 + checksum |= eslc.Checksum[2] << 2 + checksum |= eslc.Checksum[3] << 1 + checksum |= eslc.Checksum[4] << 0 + + var data = BitsToBytes(eslc.Bits) + var verify uint16 + for _, b := range data { + verify += uint16(b) + } + + var calculated = uint8(verify % 31) + return calculated == checksum +} + +// Interleave packs the embedded signalling LC to interleaved bits. +func (eslc *EmbeddedSignallingLC) Interleave() []byte { + var bits = make([]byte, 77) + var j int + for i := range bits { + switch i { + case 32: + bits[i] = eslc.Checksum[0] + break + case 43: + bits[i] = eslc.Checksum[1] + break + case 54: + bits[i] = eslc.Checksum[2] + break + case 65: + bits[i] = eslc.Checksum[3] + break + case 76: + bits[i] = eslc.Checksum[4] + break + default: + bits[i] = eslc.Bits[j] + j++ + } + } + + return bits +} + +// DeinterleaveEmbeddedSignallingLC deinterleaves the embedded signalling LC bits. +func DeinterleaveEmbeddedSignallingLC(bits []byte) (*EmbeddedSignallingLC, error) { + if bits == nil { + return nil, errors.New("dmr/emb lc deinterleave: bits can't be nil") + } + if len(bits) != 77 { + return nil, fmt.Errorf("dmr/emb lc deinterleave: expected 77 bits, got %d", len(bits)) + } + + var eslc = &EmbeddedSignallingLC{ + Bits: make([]byte, 72), + Checksum: make([]byte, 5), + } + var j int + for i, b := range bits { + switch i { + case 32: + eslc.Checksum[0] = b + break + case 43: + eslc.Checksum[1] = b + break + case 54: + eslc.Checksum[2] = b + break + case 65: + eslc.Checksum[3] = b + break + case 76: + eslc.Checksum[4] = b + break + default: + eslc.Bits[j] = b + j++ + } + } + + return eslc, nil +} diff --git a/voice_emb.go b/voice_emb.go deleted file mode 100644 index aba5fe8..0000000 --- a/voice_emb.go +++ /dev/null @@ -1,40 +0,0 @@ -package dmr - -import ( - "errors" - "fmt" - - "github.com/pd0mz/go-dmr/crc/quadres_16_7" -) - -// EMB LCSS fragments -const ( - SingleFragment uint8 = iota - FirstFragment - LastFragment - Continuation -) - -type EMB struct { - ColorCode uint8 - LCSS uint8 -} - -func ParseEMB(bits []byte) (*EMB, error) { - if len(bits) != EMBBits { - return nil, fmt.Errorf("dmr/emb: expected %d bits, got %d", EMBBits, len(bits)) - } - - if !quadres_16_7.Check(bits) { - return nil, errors.New("dmr/emb: checksum error") - } - - if bits[4] != 0 { - return nil, errors.New("dmr/emb: pi is not 0") - } - - return &EMB{ - ColorCode: uint8(bits[0])<<3 | uint8(bits[1])<<2 | uint8(bits[2])<<1 | uint8(bits[3]), - LCSS: uint8(bits[5])<<1 | uint8(bits[6]), - }, nil -}