kaeltemeister/go-dmr
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Checkpoint

Browse source
This commit is contained in:
Wijnand Modderman-Lenstra 2015-12-18 15:30:16 +01:00
parent cca6817956
commit 4f2b16dfcb
16 changed files with 1561 additions and 226 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

143
controlblock.go
View file

@ -3,31 +3,45 @@ package dmr
import ( import (
"errors" "errors"
"fmt" "fmt"
"strings"
) )
// Control Block Options // Control Block Opcode
const ( const (
CBSKOOutboundActivation = B00111000 OutboundActivationOpcode = B00111000
CBSKOUnitToUnitVoiceServiceRequest = B00000100 UnitToUnitVoiceServiceRequestOpcode = B00000100
CBSKOUnitToUnitVoiceServiceAnswerResponse = B00000101 UnitToUnitVoiceServiceAnswerResponseOpcode = B00000101
CBSKONegativeAcknowledgeResponse = B00100100 NegativeAcknowledgeResponseOpcode = B00100100
CBSKOPreamble = B00111101 PreambleOpcode = B00111101
) )
type ControlBlock struct { type ControlBlock struct {
CRC uint16
Last bool Last bool
CBSKO uint8 Opcode uint8
SrcID, DstID uint32 SrcID, DstID uint32
Data ControlBlockData Data ControlBlockData
} }
func (cb *ControlBlock) String() string {
if cb.Data == nil {
return fmt.Sprintf("CSBK, last %t, %d->%d, unknown (opcode %d)",
cb.Last, cb.SrcID, cb.DstID, cb.Opcode)
}
return fmt.Sprintf("CSBK, last %t, %d->%d, %s (opcode %d)",
cb.Last, cb.SrcID, cb.DstID, cb.Data.String(), cb.Opcode)
}
type ControlBlockData interface { type ControlBlockData interface {
String() string
Write([]byte) error Write([]byte) error
Parse([]byte) error Parse([]byte) error
} }
type OutboundActivation struct{} type OutboundActivation struct{}
func (d *OutboundActivation) String() string { return "outbound activation" }
func (d *OutboundActivation) Parse(data []byte) error { func (d *OutboundActivation) Parse(data []byte) error {
if len(data) != InfoSize { if len(data) != InfoSize {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data)) return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
@ -39,7 +53,7 @@ func (d *OutboundActivation) Write(data []byte) error {
if len(data) != InfoSize { if len(data) != InfoSize {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data)) return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
} }
data[0] |= B00111000 data[0] |= OutboundActivationOpcode
return nil return nil
} }
@ -47,6 +61,10 @@ type UnitToUnitVoiceServiceRequest struct {
Options uint8 Options uint8
} }
func (d *UnitToUnitVoiceServiceRequest) String() string {
return fmt.Sprintf("unit to unit voice service request, options %d", d.Options)
}
func (d *UnitToUnitVoiceServiceRequest) Parse(data []byte) error { func (d *UnitToUnitVoiceServiceRequest) Parse(data []byte) error {
if len(data) != InfoSize { if len(data) != InfoSize {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data)) return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
@ -59,7 +77,7 @@ func (d *UnitToUnitVoiceServiceRequest) Write(data []byte) error {
if len(data) != InfoSize { if len(data) != InfoSize {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data)) return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
} }
data[0] |= B00000100 data[0] |= UnitToUnitVoiceServiceRequestOpcode
data[2] = d.Options data[2] = d.Options
return nil return nil
} }
@ -71,6 +89,10 @@ type UnitToUnitVoiceServiceAnswerResponse struct {
Response uint8 Response uint8
} }
func (d *UnitToUnitVoiceServiceAnswerResponse) String() string {
return fmt.Sprintf("unit to unit voice service answer response, options %d, response %d", d.Options, d.Response)
}
func (d *UnitToUnitVoiceServiceAnswerResponse) Parse(data []byte) error { func (d *UnitToUnitVoiceServiceAnswerResponse) Parse(data []byte) error {
if len(data) != InfoSize { if len(data) != InfoSize {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data)) return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
@ -84,7 +106,7 @@ func (d *UnitToUnitVoiceServiceAnswerResponse) Write(data []byte) error {
if len(data) != InfoSize { if len(data) != InfoSize {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data)) return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
} }
data[0] |= B00000101 data[0] |= UnitToUnitVoiceServiceAnswerResponseOpcode
data[2] = d.Options data[2] = d.Options
data[3] = d.Response data[3] = d.Response
return nil return nil
@ -98,6 +120,10 @@ type NegativeAcknowledgeResponse struct {
Reason uint8 Reason uint8
} }
func (d *NegativeAcknowledgeResponse) String() string {
return fmt.Sprintf("negative ACK response, source %t, service %d, reason %d", d.SourceType, d.ServiceType, d.Reason)
}
func (d *NegativeAcknowledgeResponse) Parse(data []byte) error { func (d *NegativeAcknowledgeResponse) Parse(data []byte) error {
if len(data) != InfoSize { if len(data) != InfoSize {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data)) return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
@ -112,7 +138,7 @@ func (d *NegativeAcknowledgeResponse) Write(data []byte) error {
if len(data) != InfoSize { if len(data) != InfoSize {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data)) return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
} }
data[0] |= B00100110 data[0] |= NegativeAcknowledgeResponseOpcode
data[2] = d.ServiceType data[2] = d.ServiceType
if d.SourceType { if d.SourceType {
data[2] |= B01000000 data[2] |= B01000000
@ -123,13 +149,27 @@ func (d *NegativeAcknowledgeResponse) Write(data []byte) error {
var _ (ControlBlockData) = (*NegativeAcknowledgeResponse)(nil) var _ (ControlBlockData) = (*NegativeAcknowledgeResponse)(nil)
type ControlBlockPreamble struct { type Preamble struct {
DataFollows bool DataFollows bool
DstIsGroup bool DstIsGroup bool
Blocks uint8 Blocks uint8
} }
func (d *ControlBlockPreamble) Parse(data []byte) error { func (d *Preamble) String() string {
var part = []string{"preamble"}
if d.DataFollows {
part = append(part, "data folllows")
}
if d.DstIsGroup {
part = append(part, "group")
} else {
part = append(part, "unit")
}
part = append(part, fmt.Sprintf("%d blocks", d.Blocks))
return strings.Join(part, ", ")
}
func (d *Preamble) Parse(data []byte) error {
if len(data) != InfoSize { if len(data) != InfoSize {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data)) return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
} }
@ -139,11 +179,11 @@ func (d *ControlBlockPreamble) Parse(data []byte) error {
return nil return nil
} }
func (d *ControlBlockPreamble) Write(data []byte) error { func (d *Preamble) Write(data []byte) error {
if len(data) != InfoSize { if len(data) != InfoSize {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data)) return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
} }
data[0] |= B00100110 data[0] |= PreambleOpcode
if d.DataFollows { if d.DataFollows {
data[2] |= B10000000 data[2] |= B10000000
} }
@ -154,7 +194,41 @@ func (d *ControlBlockPreamble) Write(data []byte) error {
return nil return nil
} }
var _ (ControlBlockData) = (*ControlBlockPreamble)(nil) var _ (ControlBlockData) = (*Preamble)(nil)
func (cb *ControlBlock) Bytes() ([]byte, error) {
var data = make([]byte, InfoSize)
if err := cb.Data.Write(data); err != nil {
return nil, err
}
if cb.Last {
data[0] |= B10000000
}
data[4] = uint8(cb.DstID >> 16)
data[5] = uint8(cb.DstID >> 8)
data[6] = uint8(cb.DstID)
data[7] = uint8(cb.SrcID >> 16)
data[8] = uint8(cb.SrcID >> 8)
data[9] = uint8(cb.SrcID)
// Calculate CRC16
for i := 0; i < 10; i++ {
crc16(&cb.CRC, data[i])
}
crc16end(&cb.CRC)
// Inverting according to the inversion polynomial.
cb.CRC = ^cb.CRC
// Applying CRC mask, see DMR AI spec. page 143.
cb.CRC ^= 0xa5a5
data[10] = uint8(cb.CRC >> 8)
data[11] = uint8(cb.CRC)
return data, nil
}
func ParseControlBlock(data []byte) (*ControlBlock, error) { func ParseControlBlock(data []byte) (*ControlBlock, error) {
if len(data) != InfoSize { if len(data) != InfoSize {
@ -162,40 +236,55 @@ func ParseControlBlock(data []byte) (*ControlBlock, error) {
} }
// Calculate CRC16 // Calculate CRC16
var crc uint16
for i := 0; i < 10; i++ {
crc16(&crc, data[i])
}
crc16end(&crc)
// Inverting according to the inversion polynomial
crc = ^crc
// Applying CRC mask, see DMR AI spec. page 143.
crc ^= 0xa5a5
// Check packet // Check packet
if data[0]&B01000000 > 0 { if data[0]&B01000000 > 0 {
return nil, errors.New("dmr: CBSK protect flag is set") return nil, errors.New("dmr: CSBK protect flag is set")
} }
if data[1] != 0 { if data[1] != 0 {
return nil, errors.New("dmr: CBSK feature set ID is set") return nil, errors.New("dmr: CSBK feature set ID is set")
} }
cb := &ControlBlock{ cb := &ControlBlock{
CRC: uint16(data[10])<<8 | uint16(data[11]),
Last: (data[0] & B10000000) > 0, Last: (data[0] & B10000000) > 0,
CBSKO: (data[0] & B00111111), Opcode: (data[0] & B00111111),
DstID: uint32(data[4])<<16 | uint32(data[5])<<8 | uint32(data[6]), DstID: uint32(data[4])<<16 | uint32(data[5])<<8 | uint32(data[6]),
SrcID: uint32(data[7])<<16 | uint32(data[8])<<8 | uint32(data[9]), SrcID: uint32(data[7])<<16 | uint32(data[8])<<8 | uint32(data[9]),
} }
switch cb.CBSKO { if crc != cb.CRC {
case CBSKOOutboundActivation: return nil, fmt.Errorf("dmr: control block CRC error (%#04x != %#04x)", crc, cb.CRC)
}
switch cb.Opcode {
case OutboundActivationOpcode:
cb.Data = &OutboundActivation{} cb.Data = &OutboundActivation{}
break break
case CBSKOUnitToUnitVoiceServiceRequest: case UnitToUnitVoiceServiceRequestOpcode:
cb.Data = &UnitToUnitVoiceServiceRequest{} cb.Data = &UnitToUnitVoiceServiceRequest{}
break break
case CBSKOUnitToUnitVoiceServiceAnswerResponse: case UnitToUnitVoiceServiceAnswerResponseOpcode:
cb.Data = &UnitToUnitVoiceServiceAnswerResponse{} cb.Data = &UnitToUnitVoiceServiceAnswerResponse{}
break break
case CBSKONegativeAcknowledgeResponse: case NegativeAcknowledgeResponseOpcode:
cb.Data = &NegativeAcknowledgeResponse{} cb.Data = &NegativeAcknowledgeResponse{}
break break
case CBSKOPreamble: case PreambleOpcode:
cb.Data = &ControlBlockPreamble{} cb.Data = &Preamble{}
break break
default: default:
return nil, fmt.Errorf("dmr: unknown CBSKO %#02x (%#06b)", cb.CBSKO, cb.CBSKO) return nil, fmt.Errorf("dmr: unknown CSBK opcode %#02x (%#06b)", cb.Opcode, cb.Opcode)
} }
if err := cb.Data.Parse(data); err != nil { if err := cb.Data.Parse(data); err != nil {

144
controlblock_test.go Normal file
View file

@ -0,0 +1,144 @@
package dmr
import "testing"
func testCSBK(want *ControlBlock, t *testing.T) *ControlBlock {
want.SrcID = 2042214
want.DstID = 2043044
data, err := want.Bytes()
if err != nil {
t.Fatalf("encode failed: %v", err)
}
test, err := ParseControlBlock(data)
if err != nil {
t.Fatalf("decode failed: %v", err)
}
if test.SrcID != want.SrcID || test.DstID != want.DstID {
t.Fatal("decode failed, ID wrong")
}
return test
}
func TestCSBKOutboundActivation(t *testing.T) {
want := &ControlBlock{
Opcode: OutboundActivationOpcode,
Data: &OutboundActivation{},
}
test := testCSBK(want, t)
_, ok := test.Data.(*OutboundActivation)
switch {
case !ok:
t.Fatalf("decode failed: expected UnitToUnitVoiceServiceRequest, got %T", test.Data)
default:
t.Logf("decode: %s", test.String())
}
}
func TestCSBKUnitToUnitVoiceServiceRequest(t *testing.T) {
want := &ControlBlock{
Opcode: UnitToUnitVoiceServiceRequestOpcode,
Data: &UnitToUnitVoiceServiceRequest{
Options: 0x2a,
},
}
test := testCSBK(want, t)
d, ok := test.Data.(*UnitToUnitVoiceServiceRequest)
switch {
case !ok:
t.Fatalf("decode failed: expected UnitToUnitVoiceServiceRequest, got %T", test.Data)
case d.Options != 0x2a:
t.Fatalf("decode failed, options wrong")
default:
t.Logf("decode: %s", test.String())
}
}
func TestCSBKUnitToUnitVoiceServiceAnswerResponse(t *testing.T) {
want := &ControlBlock{
Opcode: UnitToUnitVoiceServiceAnswerResponseOpcode,
Data: &UnitToUnitVoiceServiceAnswerResponse{
Options: 0x17,
Response: 0x2a,
},
}
test := testCSBK(want, t)
d, ok := test.Data.(*UnitToUnitVoiceServiceAnswerResponse)
switch {
case !ok:
t.Fatalf("decode failed: expected UnitToUnitVoiceServiceAnswerResponse, got %T", test.Data)
case d.Response != 0x2a:
t.Fatalf("decode failed, response wrong")
case d.Options != 0x17:
t.Fatalf("decode failed, options wrong")
default:
t.Logf("decode: %s", test.String())
}
}
func TestCSBKNegativeAcknowledgeResponse(t *testing.T) {
want := &ControlBlock{
Opcode: NegativeAcknowledgeResponseOpcode,
Data: &NegativeAcknowledgeResponse{
ServiceType: 0x01,
Reason: 0x02,
},
}
test := testCSBK(want, t)
d, ok := test.Data.(*NegativeAcknowledgeResponse)
switch {
case !ok:
t.Fatalf("decode failed: expected NegativeAcknowledgeResponse, got %T", test.Data)
case d.ServiceType != 0x01:
t.Fatalf("decode failed, service type wrong")
case d.Reason != 0x02:
t.Fatalf("decode failed, reason wrong")
default:
t.Logf("decode: %s", test.String())
}
}
func TestCSBKPreamble(t *testing.T) {
want := &ControlBlock{
Opcode: PreambleOpcode,
Data: &Preamble{
DataFollows: true,
DstIsGroup: false,
Blocks: 0x10,
},
}
test := testCSBK(want, t)
d, ok := test.Data.(*Preamble)
switch {
case !ok:
t.Fatalf("decode failed: expected Preamble, got %T", test.Data)
case !d.DataFollows:
t.Fatalf("decode failed, data follows wrong")
case d.DstIsGroup:
t.Fatalf("decode failed, dst is group wrong")
case d.Blocks != 0x10:
t.Fatalf("decode failed, blocks wrong")
default:
t.Logf("decode: %s", test.String())
}
}

91
crc.go Normal file
View file

@ -0,0 +1,91 @@
package dmr
const (
// G(x) = x^9+x^6+x^4+x^3+1
crc9poly = 0x59
// G(x) = x^16+x^12+x^5+1
crc16poly = 0x1021
crc32poly = 0x04c11db7
)
func crc9(crc *uint16, b uint8, bits int) {
var v uint8 = 0x80
for i := 0; i < 8-bits; i++ {
v >>= 1
}
for i := 0; i < 8; i++ {
xor := (*crc)&0x0100 > 0
(*crc) <<= 1
// Limit the number of shift registers to 9.
*crc &= 0x01ff
if b&v > 0 {
(*crc)++
}
if xor {
(*crc) ^= crc9poly
}
v >>= 1
}
}
func crc9end(crc *uint16, bits int) {
for i := 0; i < bits; i++ {
xor := (*crc)&0x100 > 0
(*crc) <<= 1
// Limit the number of shift registers to 9.
*crc &= 0x01ff
if xor {
(*crc) ^= crc9poly
}
}
}
func crc16(crc *uint16, b byte) {
var v uint8 = 0x80
for i := 0; i < 8; i++ {
xor := ((*crc) & 0x8000) > 0
(*crc) <<= 1
if b&v > 0 {
(*crc)++
}
if xor {
(*crc) ^= crc16poly
}
v >>= 1
}
}
func crc16end(crc *uint16) {
for i := 0; i < 16; i++ {
xor := ((*crc) & 0x8000) > 0
(*crc) <<= 1
if xor {
(*crc) ^= crc16poly
}
}
}
func crc32(crc *uint32, b byte) {
var v uint8 = 0x80
for i := 0; i < 8; i++ {
xor := ((*crc) & 0x80000000) > 0
(*crc) <<= 1
if b&v > 0 {
(*crc)++
}
if xor {
(*crc) ^= crc32poly
}
v >>= 1
}
}
func crc32end(crc *uint32) {
for i := 0; i < 32; i++ {
xor := ((*crc) & 0x80000000) > 0
(*crc) <<= 1
if xor {
(*crc) ^= crc32poly
}
}
}

60
crc_test.go Normal file
View file

@ -0,0 +1,60 @@
package dmr
import "testing"
func TestCRC9(t *testing.T) {
tests := map[uint16][]byte{
0x0000: []byte{},
0x0100: []byte{0x00, 0x01},
0x0179: []byte("hello world"),
}
for want, test := range tests {
var crc uint16
for _, b := range test {
crc9(&crc, b, 8)
}
crc9end(&crc, 8)
if crc != want {
t.Fatalf("crc9 %v failed: %#04x != %#04x", test, crc, want)
}
}
}
func TestCRC16(t *testing.T) {
tests := map[uint16][]byte{
0x0000: []byte{},
0x1021: []byte{0x00, 0x01},
0x3be4: []byte("hello world"),
}
for want, test := range tests {
var crc uint16
for _, b := range test {
crc16(&crc, b)
}
crc16end(&crc)
if crc != want {
t.Fatalf("crc16 %v failed: %#04x != %#04x", test, crc, want)
}
}
}
func TestCRC32(t *testing.T) {
tests := map[uint32][]byte{
0x00000000: []byte{},
0x04c11db7: []byte{0x00, 0x01},
0x737af2ae: []byte("hello world"),
}
for want, test := range tests {
var crc uint32
for _, b := range test {
crc32(&crc, b)
}
crc32end(&crc)
if crc != want {
t.Fatalf("crc32 %v failed: %#08x != %#08x", test, crc, want)
}
}
}

321
data.go
View file

@ -1,9 +1,328 @@
package dmr package dmr
import (
"bytes"
"errors"
"fmt"
"golang.org/x/text/encoding"
"golang.org/x/text/encoding/charmap"
"golang.org/x/text/encoding/unicode"
)
const (
// n_DFragMax, see DMR AI spec. page 163.
MaxPacketFragmentSize = 1500
)
type DataBlock struct { type DataBlock struct {
Serial uint8 Serial uint8
CRC uint16 CRC uint16
OK bool OK bool
Data [24]byte Data []byte
Length uint8 Length uint8
} }
func ParseDataBlock(data []byte, dataType uint8, confirmed bool) (*DataBlock, error) {
var (
crc uint16
db = &DataBlock{
Length: dataBlockLength(dataType, confirmed),
}
)
if confirmed {
db.Serial = data[0] >> 1
db.CRC = uint16(data[0]&B00000001)<<8 | uint16(data[1])
db.Data = make([]byte, db.Length)
copy(db.Data, data[2:db.Length])
//log.Debugf("data block serial: %#02x (%#07b), crc: %#02x (%#09b), length: %d, confirmed: %t:", db.Serial, db.Serial, db.CRC, db.CRC, db.Length, confirmed)
//log.Debug(hex.Dump(data[2 : 2+db.Length]))
for _, block := range db.Data {
crc9(&crc, block, 8)
}
crc9(&crc, db.Serial, 7)
crc9end(&crc, 8)
// Inverting according to the inversion polynomial.
crc = ^crc
crc &= 0x01ff
// Applying CRC mask, see DMR AI spec. page 143
crc ^= 0x01ff
// FIXME(pd0mz): this is not working
if crc != db.CRC {
return nil, fmt.Errorf("dmr: block CRC error (%#04x != %#04x)", crc, db.CRC)
}
log.Debugf("dmr: data block CRC passed %#04x", crc)
} else {
copy(db.Data, data[:db.Length])
}
db.OK = true
return db, nil
}
func (db *DataBlock) Bytes(dataType uint8, confirmed bool) []byte {
var (
size = dataBlockLength(dataType, confirmed)
data = make([]byte, size)
)
if confirmed {
for _, block := range db.Data {
crc9(&db.CRC, block, 8)
}
crc9(&db.CRC, db.Serial, 7)
crc9end(&db.CRC, 8)
// Inverting according to the inversion polynomial.
db.CRC = ^db.CRC
db.CRC &= 0x01ff
// Applying CRC mask, see DMR AI spec. page 143
db.CRC ^= 0x01ff
data[0] = (db.Serial << 1) | (uint8(db.CRC>>8) & 0x01)
data[1] = uint8(db.CRC)
copy(data[2:], db.Data)
} else {
copy(data, db.Data)
}
return data
}
func dataBlockLength(dataType uint8, confirmed bool) uint8 {
var size uint8
switch dataType {
case Data:
size = 22
break
case Rate12Data:
size = 10
break
case Rate34Data:
size = 16
break
default:
return 0
}
if !confirmed {
return size + 2
}
return size
}
type DataFragment struct {
Data []byte
Stored int
Needed int
CRC uint32
}
func (df *DataFragment) DataBlocks(dataType uint8, confirm bool) ([]*DataBlock, error) {
df.Stored = len(df.Data)
if df.Stored > MaxPacketFragmentSize {
df.Stored = MaxPacketFragmentSize
}
// See DMR AI spec. page. 73. for block sizes.
var size = int(dataBlockLength(dataType, confirm))
df.Needed = (df.Stored + size - 1) / size
// Leave enough room for the 4 bytes CRC32
if (df.Needed*size)-df.Stored < 4 {
df.Needed++
}
// Calculate fragment CRC32
for i := 0; i < (df.Needed*size)-4; i += 2 {
if i+1 < df.Stored {
crc32(&df.CRC, df.Data[i+1])
} else {
crc32(&df.CRC, 0)
}
if i < df.Stored {
crc32(&df.CRC, df.Data[i])
} else {
crc32(&df.CRC, 0)
}
}
crc32end(&df.CRC)
var (
blocks = make([]*DataBlock, df.Needed)
stored int
)
for i := range blocks {
block := &DataBlock{
Serial: uint8(i % 128),
Length: dataBlockLength(dataType, confirm),
}
block.Data = make([]byte, block.Length)
store := int(block.Length)
if df.Stored-stored < store {
store = df.Stored - store
}
copy(block.Data, df.Data[stored:stored+store])
stored += store
if i == (df.Needed - 1) {
block.Data[block.Length-1] = uint8(df.CRC >> 24)
block.Data[block.Length-2] = uint8(df.CRC >> 16)
block.Data[block.Length-3] = uint8(df.CRC >> 8)
block.Data[block.Length-4] = uint8(df.CRC)
}
// Calculate block CRC9
block.CRC = 0
for _, b := range block.Data {
crc9(&block.CRC, b, 8)
}
crc9(&block.CRC, block.Serial, 7)
crc9end(&block.CRC, 8)
// Inverting according to the inversion polynomial
block.CRC = ^block.CRC
block.CRC &= 0x01ff
// Applying CRC mask, see DMR AI spec. page 143
block.CRC ^= 0x01ff
blocks[i] = block
}
return blocks, nil
}
func CombineDataBlocks(blocks []*DataBlock) (*DataFragment, error) {
if blocks == nil || len(blocks) == 0 {
return nil, errors.New("dmr: no data blocks to combine")
}
f := &DataFragment{
Data: make([]byte, MaxPacketFragmentSize),
}
for i, block := range blocks {
if block.Length == 0 {
continue
}
if i < (len(blocks) - 1) {
if f.Stored+int(block.Length) < len(f.Data) {
copy(f.Data[f.Stored:], block.Data[:block.Length])
f.Stored += int(block.Length)
}
} else {
if f.Stored+int(block.Length)-4 < len(f.Data) {
copy(f.Data[f.Stored:], block.Data[:block.Length])
f.Stored += int(block.Length)
}
f.CRC = 0
f.CRC |= uint32(block.Data[block.Length-4])
f.CRC |= uint32(block.Data[block.Length-3]) << 8
f.CRC |= uint32(block.Data[block.Length-2]) << 16
f.CRC |= uint32(block.Data[block.Length-1]) << 24
}
}
var crc uint32
for i := 0; i < f.Stored-4; i += 2 {
crc32(&crc, f.Data[i+1])
crc32(&crc, f.Data[i])
}
crc32end(&crc)
if crc != f.CRC {
return nil, fmt.Errorf("dmr: fragment CRC error (%#08x != %#08x)", crc, f.CRC)
}
log.Debugf("dmr: data fragment CRC passed %#08x", crc)
return f, nil
}
var encodingMap map[uint8]encoding.Encoding
func BuildMessageData(msg string, ddFormat uint8, nullTerminated bool) ([]byte, error) {
if e, ok := encodingMap[ddFormat]; ok {
enc := e.NewEncoder()
data, err := enc.Bytes([]byte(msg))
if err != nil {
return nil, err
}
if nullTerminated {
data = append(data, []byte{0x00, 0x00}...)
}
return data, nil
}
return nil, fmt.Errorf("dmr: encoding %s text is not supported", DDFormatName[ddFormat])
}
func ParseMessageData(data []byte, ddFormat uint8, nullTerminated bool) (string, error) {
if e, ok := encodingMap[ddFormat]; ok {
dec := e.NewDecoder()
str, err := dec.Bytes(data)
if err != nil {
return "", err
}
if nullTerminated {
if idx := bytes.IndexByte(str, 0x00); idx >= 0 {
str = str[:idx]
}
}
return string(str), nil
}
return "", fmt.Errorf("dmr: decoding %s text is not supported", DDFormatName[ddFormat])
}
func init() {
/*
DDFormatBinary: "binary",
DDFormatBCD: "BCD",
DDFormat7BitChar: "7-bit characters",
DDFormat8BitISO8859_1: "8-bit ISO 8859-1",
DDFormat8BitISO8859_2: "8-bit ISO 8859-2",
DDFormat8BitISO8859_3: "8-bit ISO 8859-3",
DDFormat8BitISO8859_4: "8-bit ISO 8859-4",
DDFormat8BitISO8859_5: "8-bit ISO 8859-5",
DDFormat8BitISO8859_6: "8-bit ISO 8859-6",
DDFormat8BitISO8859_7: "8-bit ISO 8859-7",
DDFormat8BitISO8859_8: "8-bit ISO 8859-8",
DDFormat8BitISO8859_9: "8-bit ISO 8859-9",
DDFormat8BitISO8859_10: "8-bit ISO 8859-10",
DDFormat8BitISO8859_11: "8-bit ISO 8859-11",
DDFormat8BitISO8859_13: "8-bit ISO 8859-13",
DDFormat8BitISO8859_14: "8-bit ISO 8859-14",
DDFormat8BitISO8859_15: "8-bit ISO 8859-15",
DDFormat8BitISO8859_16: "8-bit ISO 8859-16",
DDFormatUTF8: "UTF-8",
DDFormatUTF16: "UTF-16",
DDFormatUTF16BE: "UTF-16 big endian",
DDFormatUTF16LE: "UTF-16 little endian",
DDFormatUTF32: "UTF-32",
DDFormatUTF32BE: "UTF-32 big endian",
DDFormatUTF32LE: "UTF-32 little endian",
*/
encodingMap = map[uint8]encoding.Encoding{
DDFormatBinary: binaryEncoding{},
DDFormat8BitISO8859_2: charmap.ISO8859_2,
DDFormat8BitISO8859_3: charmap.ISO8859_3,
DDFormat8BitISO8859_4: charmap.ISO8859_4,
DDFormat8BitISO8859_5: charmap.ISO8859_5,
DDFormat8BitISO8859_6: charmap.ISO8859_6,
DDFormat8BitISO8859_7: charmap.ISO8859_7,
DDFormat8BitISO8859_8: charmap.ISO8859_8,
DDFormat8BitISO8859_10: charmap.ISO8859_10,
DDFormat8BitISO8859_13: charmap.ISO8859_13,
DDFormat8BitISO8859_14: charmap.ISO8859_14,
DDFormat8BitISO8859_15: charmap.ISO8859_15,
DDFormat8BitISO8859_16: charmap.ISO8859_16,
DDFormatUTF8: unicode.UTF8,
DDFormatUTF16: unicode.UTF16(unicode.LittleEndian, unicode.IgnoreBOM),
DDFormatUTF16BE: unicode.UTF16(unicode.BigEndian, unicode.IgnoreBOM),
DDFormatUTF16LE: unicode.UTF16(unicode.LittleEndian, unicode.IgnoreBOM),
}
}

33
data_encoding.go Normal file
View file

@ -0,0 +1,33 @@
package dmr
import (
"golang.org/x/text/encoding"
"golang.org/x/text/transform"
)
type binaryCoder struct{ transform.NopResetter }
func (e binaryCoder) Transform(dst, src []byte, atEOF bool) (nDst, nSrc int, err error) {
// The decoder can only make the input larger, not smaller.
n := len(src)
if len(dst) < n {
err = transform.ErrShortDst
n = len(dst)
atEOF = false
} else {
copy(dst[:n], src)
nDst = n
nSrc = n
}
return
}
type binaryEncoding struct{}
func (e binaryEncoding) NewDecoder() *encoding.Decoder {
return &encoding.Decoder{Transformer: binaryCoder{}}
}
func (e binaryEncoding) NewEncoder() *encoding.Encoder {
return &encoding.Encoder{Transformer: binaryCoder{}}
}

93
data_test.go Normal file
View file

@ -0,0 +1,93 @@
package dmr
import (
"bytes"
"encoding/hex"
"fmt"
"sort"
"testing"
)
func TestDataBlock(t *testing.T) {
want := &DataBlock{
Serial: 123,
Data: []byte{0x17, 0x2a},
Length: 2,
}
data := want.Bytes(Rate34Data, true)
if data == nil {
t.Fatal("encode failed")
}
size := int(dataBlockLength(Rate34Data, true))
if len(data) != size {
t.Fatalf("encode failed: expected %d bytes, got %d", size, len(data))
}
// Decoding is tested in the DataFragment test
}
func TestDataFragment(t *testing.T) {
msg, err := BuildMessageData("CQCQCQ PD0MZ", DDFormatUTF16, true)
if err != nil {
t.Fatalf("build message failed: %v", err)
}
want := &DataFragment{Data: msg}
blocks, err := want.DataBlocks(Rate34Data, true)
if err != nil {
t.Fatalf("encode failed: %v", err)
}
if blocks == nil {
t.Fatal("encode failed: blocks is nil")
}
if len(blocks) != 2 {
t.Fatalf("encode failed: expected 2 blocks, got %d", len(blocks))
}
for i, block := range blocks {
t.Log(fmt.Sprintf("block %02d:\n%s", i, hex.Dump(block.Data)))
}
test, err := CombineDataBlocks(blocks)
if err != nil {
t.Fatalf("decode failed: %v", err)
}
if !bytes.Equal(test.Data[:len(want.Data)], want.Data) {
t.Log(fmt.Sprintf("want:\n%s", hex.Dump(want.Data)))
t.Log(fmt.Sprintf("got:\n%s", hex.Dump(test.Data)))
t.Fatal("decode failed: data is wrong")
}
}
func TestMessage(t *testing.T) {
msg := "CQCQCQ PD0MZ"
var encodings = []int{}
for i := range encodingMap {
encodings = append(encodings, int(i))
}
sort.Sort(sort.IntSlice(encodings))
for _, i := range encodings {
e := encodingMap[uint8(i)]
n := DDFormatName[uint8(i)]
t.Logf("testing %s encoding", n)
enc := e.NewDecoder()
str, err := enc.String(msg)
if err != nil {
t.Fatalf("error encoding to %s: %v", n, err)
}
dec := e.NewDecoder()
out, err := dec.String(str)
if err != nil {
t.Fatalf("error decoding from %s: %v", n, err)
}
t.Log(fmt.Sprintf("encoder:\n%s", hex.Dump([]byte(str))))
t.Log(fmt.Sprintf("decoder:\n%s", hex.Dump([]byte(out))))
}
}

386
dataheader.go
View file

@ -22,6 +22,21 @@ const (
PacketFormatProprietaryData // 0b1111 PacketFormatProprietaryData // 0b1111
) )
// Service Access Point
const (
ServiceAccessPointUDT uint8 = iota // 0b0000
_ // 0b0001
ServiceAccessPointTCPIPHeaderCompression // 0b0010
ServiceAccessPointUDPIPHeaderCompression // 0b0011
ServiceAccessPointIPBasedPacketData // 0b0100
ServiceAccessPointARP // 0b0101
_ // 0b0110
_ // 0b0111
_ // 0b1000
ServiceAccessPointProprietaryData // 0b1001
ServiceAccessPointShortData // 0b1010
)
// Response Data Header Response Type // Response Data Header Response Type
const ( const (
ResponseTypeACK uint8 = iota ResponseTypeACK uint8 = iota
@ -131,53 +146,95 @@ var DDFormatName = map[uint8]string{
DDFormatUTF32LE: "UTF-32 little endian", DDFormatUTF32LE: "UTF-32 little endian",
} }
type DataHeader interface { // http://www.etsi.org/images/files/DMRcodes/dmrs-mfid.xls
CommonHeader() DataHeaderCommon var ManufacturerName = map[uint8]string{
0x00: "Reserved",
0x01: "Reserved",
0x02: "Reserved",
0x03: "Reserved",
0x04: "Flyde Micro Ltd.",
0x05: "PROD-EL SPA",
0x06: "Trident Datacom DBA Trident Micro Systems",
0x07: "RADIODATA GmbH",
0x08: "HYT science tech",
0x09: "ASELSAN Elektronik Sanayi ve Ticaret A.S.",
0x0a: "Kirisun Communications Co. Ltd",
0x0b: "DMR Association Ltd.",
0x10: "Motorola Ltd.",
0x13: "EMC S.p.A. (Electronic Marketing Company)",
0x1c: "EMC S.p.A. (Electronic Marketing Company)",
0x20: "JVCKENWOOD Corporation",
0x33: "Radio Activity Srl",
0x3c: "Radio Activity Srl",
0x58: "Tait Electronics Ltd",
0x68: "HYT science tech",
0x77: "Vertex Standard",
} }
type DataHeaderCommon struct { type DataHeaderData interface {
String() string
Write([]byte) error
}
type DataHeader struct {
PacketFormat uint8 PacketFormat uint8
DstIsGroup bool DstIsGroup bool
ResponseRequested bool ResponseRequested bool
HeaderCompression bool
ServiceAccessPoint uint8 ServiceAccessPoint uint8
DstID uint32 DstID uint32
SrcID uint32 SrcID uint32
CRC uint16 CRC uint16
Data DataHeaderData
} }
func (h *DataHeaderCommon) Parse(header []byte) error { type UDTData struct {
h.PacketFormat = header[0] & 0xf
h.DstIsGroup = (header[0] & 0x80) > 0
h.ResponseRequested = (header[0] & 0x40) > 0
h.ServiceAccessPoint = (header[1] & 0xf0) >> 4
h.DstID = uint32(header[2])<<16 | uint32(header[3])<<8 | uint32(header[4])
h.SrcID = uint32(header[5])<<16 | uint32(header[6])<<8 | uint32(header[7])
return nil
}
type UDTDataHeader struct {
Common DataHeaderCommon
Format uint8 Format uint8
PadNibble uint8 PadNibble uint8
AppendedBlocks uint8 AppendedBlocks uint8
SupplementaryFlag bool SupplementaryFlag bool
OPCode uint8 Opcode uint8
} }
func (h UDTDataHeader) CommonHeader() DataHeaderCommon { return h.Common } func (d UDTData) String() string {
return fmt.Sprintf("UDT, format %s (%d), pad nibble %d, appended blocks %d, supplementary %t, opcode %d",
UDTFormatName[d.Format], d.Format, d.PadNibble, d.AppendedBlocks, d.SupplementaryFlag, d.Opcode)
}
type UnconfirmedDataHeader struct { func (d UDTData) Write(data []byte) error {
Common DataHeaderCommon data[1] |= (d.Format & B00001111)
data[8] = (d.AppendedBlocks & B00000011) | (d.PadNibble << 3)
data[9] = (d.Opcode & B00111111)
if d.SupplementaryFlag {
data[9] |= B10000000
}
return nil
}
type UnconfirmedData struct {
PadOctetCount uint8 PadOctetCount uint8
FullMessage bool FullMessage bool
BlocksToFollow uint8 BlocksToFollow uint8
FragmentSequenceNumber uint8 FragmentSequenceNumber uint8
} }
func (h UnconfirmedDataHeader) CommonHeader() DataHeaderCommon { return h.Common } func (d UnconfirmedData) String() string {
return fmt.Sprintf("unconfirmed, pad octet %d, full %t, blocks %d, sequence %d",
d.PadOctetCount, d.FullMessage, d.BlocksToFollow, d.FragmentSequenceNumber)
}
type ConfirmedDataHeader struct { func (d UnconfirmedData) Write(data []byte) error {
Common DataHeaderCommon data[0] |= d.PadOctetCount & B00010000
data[1] |= d.PadOctetCount & B00001111
data[8] = d.BlocksToFollow & B01111111
if d.FullMessage {
data[8] |= B10000000
}
data[9] = d.FragmentSequenceNumber & B00001111
return nil
}
type ConfirmedData struct {
PadOctetCount uint8 PadOctetCount uint8
FullMessage bool FullMessage bool
BlocksToFollow uint8 BlocksToFollow uint8
@ -186,27 +243,58 @@ type ConfirmedDataHeader struct {
FragmentSequenceNumber uint8 FragmentSequenceNumber uint8
} }
func (h ConfirmedDataHeader) CommonHeader() DataHeaderCommon { return h.Common } func (d ConfirmedData) String() string {
return fmt.Sprintf("confirmed, pad octet %d, full %t, blocks %d, resync %t, send sequence %d, sequence %d",
d.PadOctetCount, d.FullMessage, d.BlocksToFollow, d.Resync, d.SendSequenceNumber, d.FragmentSequenceNumber)
}
type ResponseDataHeader struct { func (d ConfirmedData) Write(data []byte) error {
Common DataHeaderCommon data[0] |= d.PadOctetCount & B00010000
data[1] |= d.PadOctetCount & B00001111
data[8] = d.BlocksToFollow & B01111111
if d.FullMessage {
data[8] |= B10000000
}
data[9] = (d.FragmentSequenceNumber&B00000111)<<0 | (d.SendSequenceNumber&B00000111)<<4
if d.Resync {
data[9] |= B10000000
}
return nil
}
type ResponseData struct {
BlocksToFollow uint8 BlocksToFollow uint8
Class uint8 Class uint8
Type uint8 Type uint8
Status uint8 Status uint8
} }
func (h ResponseDataHeader) CommonHeader() DataHeaderCommon { return h.Common } func (d ResponseData) String() string {
return fmt.Sprintf("response, blocks %d, class %d, type %s (%d), status %d",
d.BlocksToFollow, d.Class, ResponseTypeName[d.Type], d.Type, d.Status)
}
type ProprietaryDataHeader struct { func (d ResponseData) Write(data []byte) error {
Common DataHeaderCommon data[8] = d.BlocksToFollow & B01111111
data[9] = (d.Status&B00000111)<<0 | (d.Type&B00000111)<<3 | (d.Class&B00000011)<<6
return nil
}
type ProprietaryData struct {
ManufacturerID uint8 ManufacturerID uint8
} }
func (h ProprietaryDataHeader) CommonHeader() DataHeaderCommon { return h.Common } func (d ProprietaryData) String() string {
return fmt.Sprintf("proprietary, manufacturer %s (%d)",
ManufacturerName[d.ManufacturerID], d.ManufacturerID)
}
type ShortDataRawDataHeader struct { func (d ProprietaryData) Write(data []byte) error {
Common DataHeaderCommon data[1] = (d.ManufacturerID & B01111111)
return nil
}
type ShortDataRawData struct {
AppendedBlocks uint8 AppendedBlocks uint8
SrcPort uint8 SrcPort uint8
DstPort uint8 DstPort uint8
@ -215,10 +303,26 @@ type ShortDataRawDataHeader struct {
BitPadding uint8 BitPadding uint8
} }
func (h ShortDataRawDataHeader) CommonHeader() DataHeaderCommon { return h.Common } func (d ShortDataRawData) String() string {
return fmt.Sprintf("short data raw, blocks %d, src port %d, dst port %d, rsync %t, full %t, padding %d",
d.AppendedBlocks, d.SrcPort, d.DstPort, d.Resync, d.FullMessage, d.BitPadding)
}
type ShortDataDefinedDataHeader struct { func (d ShortDataRawData) Write(data []byte) error {
Common DataHeaderCommon data[0] |= d.AppendedBlocks & B00110000
data[1] |= d.AppendedBlocks & B00001111
data[8] = (d.SrcPort&B00000111)<<5 | (d.DstPort&B00000111)<<2
if d.Resync {
data[8] |= B00000010
}
if d.FullMessage {
data[8] |= B00000001
}
data[9] = d.BitPadding
return nil
}
type ShortDataDefinedData struct {
AppendedBlocks uint8 AppendedBlocks uint8
DDFormat uint8 DDFormat uint8
Resync bool Resync bool
@ -226,133 +330,135 @@ type ShortDataDefinedDataHeader struct {
BitPadding uint8 BitPadding uint8
} }
func (h ShortDataDefinedDataHeader) CommonHeader() DataHeaderCommon { return h.Common } func (d ShortDataDefinedData) String() string {
return fmt.Sprintf("short data defined, blocks %d, dd format %s (%d), resync %t, full %t, padding %d",
d.AppendedBlocks, DDFormatName[d.DDFormat], d.DDFormat, d.Resync, d.FullMessage, d.BitPadding)
}
func ParseDataHeader(header []byte, proprietary bool) (DataHeader, error) { func (d ShortDataDefinedData) Write(data []byte) error {
if len(header) != 12 { data[0] |= d.AppendedBlocks & B00110000
return nil, fmt.Errorf("header must be 12 bytes, got %d", len(header)) data[1] |= d.AppendedBlocks & B00001111
data[8] = (d.DDFormat & B00111111) << 2
if d.Resync {
data[8] |= B00000010
}
if d.FullMessage {
data[8] |= B00000001
}
data[9] = d.BitPadding
return nil
}
var _ (DataHeaderData) = (*ShortDataDefinedData)(nil)
func ParseDataHeader(data []byte, proprietary bool) (*DataHeader, error) {
if len(data) != 12 {
return nil, fmt.Errorf("data must be 12 bytes, got %d", len(data))
} }
var ( var (
ccrc = (uint16(header[10]) << 8) | uint16(header[11]) ccrc = (uint16(data[10]) << 8) | uint16(data[11])
hcrc = dataHeaderCRC(header) hcrc = dataHeaderCRC(data)
) )
if ccrc != hcrc { if ccrc != hcrc {
return nil, fmt.Errorf("header CRC mismatch, %#04x != %#04x", ccrc, hcrc) return nil, fmt.Errorf("data CRC mismatch, %#04x != %#04x", ccrc, hcrc)
}
h := &DataHeader{
DstIsGroup: (data[0] & B10000000) > 0,
ResponseRequested: (data[0] & B01000000) > 0,
HeaderCompression: (data[0] & B00100000) > 0,
PacketFormat: (data[0] & B00001111),
ServiceAccessPoint: (data[1] & B11110000) >> 4,
DstID: uint32(data[2])<<16 | uint32(data[3])<<8 | uint32(data[4]),
SrcID: uint32(data[5])<<16 | uint32(data[6])<<8 | uint32(data[7]),
CRC: ccrc,
} }
if proprietary { if proprietary {
return ProprietaryDataHeader{ h.Data = ProprietaryData{
Common: DataHeaderCommon{ ManufacturerID: data[1] & B01111111,
ServiceAccessPoint: (header[0] & B11110000) >> 4,
PacketFormat: (header[0] & B00001111),
CRC: ccrc,
},
ManufacturerID: header[1],
}, nil
} }
common := DataHeaderCommon{ } else {
CRC: ccrc, switch h.PacketFormat {
}
if err := common.Parse(header); err != nil {
return nil, err
}
switch common.PacketFormat {
case PacketFormatUDT: case PacketFormatUDT:
return UDTDataHeader{ h.Data = UDTData{
Common: common, Format: (data[1] & B00001111),
Format: (header[1] & B00001111), PadNibble: (data[8] & B11111000) >> 3,
PadNibble: (header[8] & B11111000) >> 3, AppendedBlocks: (data[8] & B00000011),
AppendedBlocks: (header[8] & B00000011), SupplementaryFlag: (data[9] & B10000000) > 0,
SupplementaryFlag: (header[9] & B10000000) > 0, Opcode: (data[9] & B00111111),
OPCode: (header[9] & B00111111),
}, nil
case PacketFormatResponse:
return ResponseDataHeader{
Common: common,
BlocksToFollow: (header[8] & B01111111),
Class: (header[9] & B11000000) >> 6,
Type: (header[9] & B00111000) >> 3,
Status: (header[9] & B00000111),
}, nil
case PacketFormatUnconfirmedData:
return UnconfirmedDataHeader{
Common: common,
PadOctetCount: (header[0] & B00010000) | (header[1] & B00001111),
FullMessage: (header[8] & B10000000) > 0,
BlocksToFollow: (header[8] & B01111111),
FragmentSequenceNumber: (header[9] & B00001111),
}, nil
case PacketFormatConfirmedData:
return ConfirmedDataHeader{
Common: common,
PadOctetCount: (header[0] & B00010000) | (header[1] & B00001111),
FullMessage: (header[8] & B10000000) > 0,
BlocksToFollow: (header[8] & B01111111),
Resync: (header[9] & B10000000) > 0,
SendSequenceNumber: (header[9] & B01110000) >> 4,
FragmentSequenceNumber: (header[9] & B00001111),
}, nil
case PacketFormatShortDataRaw:
return ShortDataRawDataHeader{
Common: common,
AppendedBlocks: (header[0] & B00110000) | (header[1] & B00001111),
SrcPort: (header[8] & B11100000) >> 5,
DstPort: (header[8] & B00011100) >> 2,
Resync: (header[8] & B00000010) > 0,
FullMessage: (header[8] & B00000001) > 0,
BitPadding: (header[9]),
}, nil
case PacketFormatShortDataDefined:
return ShortDataDefinedDataHeader{
Common: common,
AppendedBlocks: (header[0] & B00110000) | (header[1] & B00001111),
DDFormat: (header[8] & B11111100) >> 2,
Resync: (header[8] & B00000010) > 0,
FullMessage: (header[8] & B00000001) > 0,
BitPadding: (header[9]),
}, nil
default:
return nil, fmt.Errorf("dmr: unknown data header packet format %#02x (%d)", common.PacketFormat, common.PacketFormat)
} }
break
case PacketFormatResponse:
h.Data = ResponseData{
BlocksToFollow: (data[8] & B01111111),
Class: (data[9] & B11000000) >> 6,
Type: (data[9] & B00111000) >> 3,
Status: (data[9] & B00000111),
}
break
case PacketFormatUnconfirmedData:
h.Data = UnconfirmedData{
PadOctetCount: (data[0] & B00010000) | (data[1] & B00001111),
FullMessage: (data[8] & B10000000) > 0,
BlocksToFollow: (data[8] & B01111111),
FragmentSequenceNumber: (data[9] & B00001111),
}
break
case PacketFormatConfirmedData:
h.Data = ConfirmedData{
PadOctetCount: (data[0] & B00010000) | (data[1] & B00001111),
FullMessage: (data[8] & B10000000) > 0,
BlocksToFollow: (data[8] & B01111111),
Resync: (data[9] & B10000000) > 0,
SendSequenceNumber: (data[9] & B01110000) >> 4,
FragmentSequenceNumber: (data[9] & B00001111),
}
break
case PacketFormatShortDataRaw:
h.Data = ShortDataRawData{
AppendedBlocks: (data[0] & B00110000) | (data[1] & B00001111),
SrcPort: (data[8] & B11100000) >> 5,
DstPort: (data[8] & B00011100) >> 2,
Resync: (data[8] & B00000010) > 0,
FullMessage: (data[8] & B00000001) > 0,
BitPadding: (data[9]),
}
break
case PacketFormatShortDataDefined:
h.Data = ShortDataDefinedData{
AppendedBlocks: (data[0] & B00110000) | (data[1] & B00001111),
DDFormat: (data[8] & B11111100) >> 2,
Resync: (data[8] & B00000010) > 0,
FullMessage: (data[8] & B00000001) > 0,
BitPadding: (data[9]),
}
break
default:
return nil, fmt.Errorf("dmr: unknown data data packet format %#02x (%d)", h.PacketFormat, h.PacketFormat)
}
}
return h, nil
} }
func dataHeaderCRC(header []byte) uint16 { func dataHeaderCRC(data []byte) uint16 {
var crc uint16 var crc uint16
if len(header) < 10 { if len(data) < 10 {
return crc return crc
} }
for i := 0; i < 10; i++ { for i := 0; i < 10; i++ {
crc16(&crc, header[i]) crc16(&crc, data[i])
} }
crc16end(&crc) crc16end(&crc)
return (^crc) ^ 0xcccc return (^crc) ^ 0xcccc
} }
func crc16(crc *uint16, b byte) {
var v = uint8(0x80)
for i := 0; i < 8; i++ {
xor := ((*crc) & 0x8000) > 0
(*crc) <<= 1
if b&v > 0 {
(*crc)++
}
if xor {
(*crc) ^= 0x1021
}
v >>= 1
}
}
func crc16end(crc *uint16) {
for i := 0; i < 16; i++ {
xor := ((*crc) & 0x8000) > 0
(*crc) <<= 1
if xor {
(*crc) ^= 0x1021
}
}
}

1
dataheader_test.go Normal file
View file

@ -0,0 +1 @@
package dmr

4
dmr.go
View file

@ -1,2 +1,6 @@
// Package dmr implements various protocols for interfacing Digital Mobile Radio repeaters and base stations. // Package dmr implements various protocols for interfacing Digital Mobile Radio repeaters and base stations.
package dmr package dmr
import "github.com/op/go-logging"
var log = logging.MustGetLogger("dmr/terminal")

72
homebrew/homebrew.go
View file

@ -63,6 +63,7 @@ var (
AuthTimeout = time.Second * 5 AuthTimeout = time.Second * 5
PingInterval = time.Second * 5 PingInterval = time.Second * 5
PingTimeout = time.Second * 15 PingTimeout = time.Second * 15
SendInterval = time.Millisecond * 30
) )
// Peer is a remote repeater that also speaks the Homebrew protocol // Peer is a remote repeater that also speaks the Homebrew protocol
@ -110,8 +111,11 @@ type Homebrew struct {
conn *net.UDPConn conn *net.UDPConn
closed bool closed bool
id []byte id []byte
mutex *sync.Mutex last time.Time // Record last received frame time
mutex *sync.Mutex // Mutex for manipulating peer list or send queue
rxtx *sync.Mutex // Mutex for when receiving data or sending data
stop chan bool stop chan bool
queue []*dmr.Packet
} }
// New creates a new Homebrew repeater // New creates a new Homebrew repeater
@ -131,6 +135,7 @@ func New(config *RepeaterConfiguration, addr *net.UDPAddr) (*Homebrew, error) {
PeerID: make(map[uint32]*Peer), PeerID: make(map[uint32]*Peer),
id: packRepeaterID(config.ID), id: packRepeaterID(config.ID),
mutex: &sync.Mutex{}, mutex: &sync.Mutex{},
queue: make([]*dmr.Packet, 0),
} }
if h.conn, err = net.ListenUDP("udp", addr); err != nil { if h.conn, err = net.ListenUDP("udp", addr); err != nil {
return nil, errors.New("homebrew: " + err.Error()) return nil, errors.New("homebrew: " + err.Error())
@ -242,6 +247,21 @@ func (h *Homebrew) ListenAndServe() error {
return nil return nil
} }
// Send a packet to the peers. Will block until the packet is sent.
func (h *Homebrew) Send(p *dmr.Packet) error {
h.rxtx.Lock()
defer h.rxtx.Unlock()
data := BuildData(p, h.Config.ID)
for _, peer := range h.getPeers() {
if err := h.WriteToPeer(data, peer); err != nil {
return err
}
}
return nil
}
func (h *Homebrew) GetPacketFunc() dmr.PacketFunc { func (h *Homebrew) GetPacketFunc() dmr.PacketFunc {
return h.pf return h.pf
} }
@ -522,12 +542,20 @@ func (h *Homebrew) handleAuth(peer *Peer) error {
} }
func (h *Homebrew) handlePacket(p *dmr.Packet, peer *Peer) error { func (h *Homebrew) handlePacket(p *dmr.Packet, peer *Peer) error {
h.rxtx.Lock()
defer h.rxtx.Unlock()
// Record last received time
h.last = time.Now()
// Offload packet to handle callback
if peer.PacketReceived != nil { if peer.PacketReceived != nil {
return peer.PacketReceived(h, p) return peer.PacketReceived(h, p)
} }
if h.pf == nil { if h.pf == nil {
return errors.New("homebrew: no PacketReceived func defined to handle DMR packet") return errors.New("homebrew: no PacketReceived func defined to handle DMR packet")
} }
return h.pf(h, p) return h.pf(h, p)
} }
@ -666,6 +694,44 @@ func packRepeaterID(id uint32) []byte {
return []byte(fmt.Sprintf("%08X", id)) return []byte(fmt.Sprintf("%08X", id))
} }
// BuildData converts DMR packet format to Homebrew packet format.
func BuildData(p *dmr.Packet, repeaterID uint32) []byte {
var data = make([]byte, 53)
copy(data[:4], DMRData)
data[4] = p.Sequence
data[5] = uint8(p.SrcID >> 16)
data[6] = uint8(p.SrcID >> 8)
data[7] = uint8(p.SrcID)
data[8] = uint8(p.DstID >> 16)
data[9] = uint8(p.DstID >> 8)
data[10] = uint8(p.DstID)
data[11] = uint8(repeaterID >> 24)
data[12] = uint8(repeaterID >> 16)
data[13] = uint8(repeaterID >> 8)
data[14] = uint8(repeaterID)
data[15] = p.Timeslot | (p.CallType << 1)
data[16] = uint8(p.StreamID >> 24)
data[17] = uint8(p.StreamID >> 16)
data[18] = uint8(p.StreamID >> 8)
data[19] = uint8(p.StreamID)
copy(data[20:], p.Data)
switch p.DataType {
case dmr.VoiceBurstB, dmr.VoiceBurstC, dmr.VoiceBurstD, dmr.VoiceBurstE, dmr.VoiceBurstF:
data[15] |= (0x00 << 2)
data[15] |= (p.DataType - dmr.VoiceBurstA) << 4
break
case dmr.VoiceBurstA:
data[15] |= (0x01 << 2)
break
default:
data[15] |= (0x02 << 2)
data[15] |= (p.DataType) << 4
}
return data
}
// ParseData converts Homebrew packet format to DMR packet format. // ParseData converts Homebrew packet format to DMR packet format.
func ParseData(data []byte) (*dmr.Packet, error) { func ParseData(data []byte) (*dmr.Packet, error) {
if len(data) != 53 { if len(data) != 53 {
@ -686,8 +752,12 @@ func ParseData(data []byte) (*dmr.Packet, error) {
switch (data[15] >> 2) & 0x03 { switch (data[15] >> 2) & 0x03 {
case 0x00, 0x01: // voice (B-F), voice sync (A) case 0x00, 0x01: // voice (B-F), voice sync (A)
p.DataType = dmr.VoiceBurstA + (data[15] >> 4) p.DataType = dmr.VoiceBurstA + (data[15] >> 4)
break
case 0x02: // data sync case 0x02: // data sync
p.DataType = (data[15] >> 4) p.DataType = (data[15] >> 4)
break
default: // unknown/unused
return nil, errors.New("homebrew: unexpected frame type 0b11")
} }
return p, nil return p, nil

2
ipsc/packet.go
View file

@ -44,7 +44,7 @@ var (
0x0000: "unknown", 0x0000: "unknown",
0x1111: "voice LC header", 0x1111: "voice LC header",
0x2222: "terminator with LC", 0x2222: "terminator with LC",
0x3333: "CBSK", 0x3333: "CSBK",
0x4444: "data header", 0x4444: "data header",
0x5555: "rate 1/2 data", 0x5555: "rate 1/2 data",
0x6666: "rate 3/4 data", 0x6666: "rate 3/4 data",

8
packet.go
View file

@ -5,7 +5,7 @@ const (
PrivacyIndicator uint8 = iota // Privacy Indicator information in a standalone burst PrivacyIndicator uint8 = iota // Privacy Indicator information in a standalone burst
VoiceLC // Indicates the beginning of voice transmission, carries addressing information VoiceLC // Indicates the beginning of voice transmission, carries addressing information
TerminatorWithLC // Indicates the end of transmission, carries LC information TerminatorWithLC // Indicates the end of transmission, carries LC information
CBSK // Carries a control block CSBK // Carries a control block
MultiBlockControl // Header for multi-block control MultiBlockControl // Header for multi-block control
MultiBlockControlContinuation // Follow-on blocks for multi-block control MultiBlockControlContinuation // Follow-on blocks for multi-block control
Data // Carries addressing and numbering of packet data blocks Data // Carries addressing and numbering of packet data blocks
@ -26,12 +26,12 @@ var DataTypeName = map[uint8]string{
PrivacyIndicator: "privacy indicator", PrivacyIndicator: "privacy indicator",
VoiceLC: "voice LC", VoiceLC: "voice LC",
TerminatorWithLC: "terminator with LC", TerminatorWithLC: "terminator with LC",
CBSK: "control block", CSBK: "control block",
MultiBlockControl: "multi-block control", MultiBlockControl: "multi-block control",
MultiBlockControlContinuation: "multi-block control follow-on", MultiBlockControlContinuation: "multi-block control follow-on",
Data: "data", Data: "data",
Rate12Data: "rate 1/2 packet data", Rate12Data: "rate ½ packet data",
Rate34Data: "rate 3/4 packet data", Rate34Data: "rate ¾ packet data",
Idle: "idle", Idle: "idle",
VoiceBurstA: "voice (burst A)", VoiceBurstA: "voice (burst A)",
VoiceBurstB: "voice (burst B)", VoiceBurstB: "voice (burst B)",

1
repeater.go
View file

@ -5,6 +5,7 @@ type Repeater interface {
Active() bool Active() bool
Close() error Close() error
ListenAndServe() error ListenAndServe() error
Send(*Packet) error
GetPacketFunc() PacketFunc GetPacketFunc() PacketFunc
SetPacketFunc(PacketFunc) SetPacketFunc(PacketFunc)

401
terminal/terminal.go
View file

@ -2,7 +2,9 @@ package terminal
import ( import (
"encoding/hex" "encoding/hex"
"errors"
"fmt" "fmt"
"strings"
"time" "time"
"github.com/op/go-logging" "github.com/op/go-logging"
@ -16,9 +18,11 @@ var log = logging.MustGetLogger("dmr/terminal")
const ( const (
idle uint8 = iota idle uint8 = iota
dataCallActive dataCallActive
voideCallActive voiceCallActive
) )
const VoiceFrameDuration = time.Millisecond * 60
type Slot struct { type Slot struct {
call struct { call struct {
start time.Time start time.Time
@ -28,28 +32,41 @@ type Slot struct {
dataType uint8 dataType uint8
data struct { data struct {
packetHeaderValid bool packetHeaderValid bool
blocks [64]dmr.DataBlock blocks []*dmr.DataBlock
blocksExpected uint8 blocksExpected int
blocksReceived uint8 blocksReceived int
header dmr.DataHeader
}
voice struct {
lastFrame uint8
streamID uint32
} }
selectiveAckRequestsSent int selectiveAckRequestsSent int
rxSequence int rxSequence int
fullMessageBlocks uint8 fullMessageBlocks int
last struct { last struct {
packetReceived time.Time packetReceived time.Time
} }
} }
func NewSlot() Slot { func NewSlot() *Slot {
return Slot{} return &Slot{}
} }
type VoiceFrameFunc func(*dmr.Packet, []byte)
type Terminal struct { type Terminal struct {
ID uint32 ID uint32
Call string Call string
CallMap map[uint32]string
Repeater dmr.Repeater Repeater dmr.Repeater
slot []Slot TalkGroup []uint32
SoftwareDelay bool
accept map[uint32]bool
slot []*Slot
state uint8 state uint8
vff VoiceFrameFunc
} }
func New(id uint32, call string, r dmr.Repeater) *Terminal { func New(id uint32, call string, r dmr.Repeater) *Terminal {
@ -57,12 +74,222 @@ func New(id uint32, call string, r dmr.Repeater) *Terminal {
ID: id, ID: id,
Call: call, Call: call,
Repeater: r, Repeater: r,
slot: []Slot{NewSlot(), NewSlot()}, slot: []*Slot{NewSlot(), NewSlot(), NewSlot()},
accept: map[uint32]bool{id: true},
} }
r.SetPacketFunc(t.handlePacket) r.SetPacketFunc(t.handlePacket)
return t return t
} }
func (t *Terminal) SetTalkGroups(tg []uint32) {
t.accept = map[uint32]bool{t.ID: true}
if tg != nil {
for _, id := range tg {
t.accept[id] = true
}
}
}
func (t *Terminal) SetVoiceFrameFunc(f VoiceFrameFunc) {
t.vff = f
}
func (t *Terminal) Send(p *dmr.Packet) error {
return t.Repeater.Send(p)
}
func (t *Terminal) fmt(p *dmr.Packet, f string) string {
var fp = []string{
fmt.Sprintf("[slot %d][%02x][",
p.Timeslot+1,
p.Sequence),
}
if t.CallMap != nil {
if call, ok := t.CallMap[p.SrcID]; ok {
fp = append(fp, fmt.Sprintf("%-6s->", call))
} else {
fp = append(fp, fmt.Sprintf("%-6d->", p.SrcID))
}
if call, ok := t.CallMap[p.DstID]; ok {
fp = append(fp, fmt.Sprintf("%-6s] ", call))
} else {
fp = append(fp, fmt.Sprintf("%-6d] ", p.DstID))
}
} else {
fp = append(fp, fmt.Sprintf("%-6d->%-6d] ", p.SrcID, p.DstID))
}
fp = append(fp, f)
return strings.Join(fp, "")
}
func (t *Terminal) debugf(p *dmr.Packet, f string, v ...interface{}) {
ff := t.fmt(p, f)
if len(v) > 0 {
log.Debugf(ff, v...)
} else {
log.Debug(ff)
}
}
func (t *Terminal) infof(p *dmr.Packet, f string, v ...interface{}) {
ff := t.fmt(p, f)
if len(v) > 0 {
log.Infof(ff, v...)
} else {
log.Info(ff)
}
}
func (t *Terminal) warningf(p *dmr.Packet, f string, v ...interface{}) {
ff := t.fmt(p, f)
if len(v) > 0 {
log.Warningf(ff, v...)
} else {
log.Warning(ff)
}
}
func (t *Terminal) errorf(p *dmr.Packet, f string, v ...interface{}) {
ff := t.fmt(p, f)
if len(v) > 0 {
log.Errorf(ff, v...)
} else {
log.Error(ff)
}
}
func (t *Terminal) dataBlock(p *dmr.Packet, db *dmr.DataBlock) error {
slot := t.slot[p.Timeslot]
if slot.data.header == nil {
return errors.New("terminal: logic error, header is nil?!")
}
h := slot.data.header.CommonHeader()
if h.ResponseRequested {
// Only confirmed data blocks have serial numbers stored in them.
if int(db.Serial) < len(slot.data.blocks) {
slot.data.blocks[db.Serial] = db
} else {
t.warningf(p, "data block %d out of bounds (%d >= %d)", db.Serial, db.Serial, len(slot.data.blocks))
return nil
}
} else {
slot.data.blocks[slot.data.blocksReceived] = db
}
slot.data.blocksReceived++
if slot.data.blocksReceived == slot.data.blocksExpected {
return t.dataBlockAssemble(p)
}
return nil
}
func (t *Terminal) dataBlockAssemble(p *dmr.Packet) error {
slot := t.slot[p.Timeslot]
var (
errorsFound bool
selective = make([]bool, len(slot.data.blocks))
)
for i := 0; i < slot.fullMessageBlocks; i++ {
if slot.data.blocks[i] == nil || !slot.data.blocks[i].OK {
selective[i] = true
errorsFound = true
}
}
if errorsFound {
_, responseOk := slot.data.header.(*dmr.ResponseDataHeader)
switch {
case responseOk:
t.debugf(p, "found erroneous blocks, not sending out ACK for response")
return nil
case slot.selectiveAckRequestsSent > 25:
t.warningf(p, "found erroneous blocks, max selective ACK reached")
return nil
default:
//t.sendSelectiveAck()
return nil
}
}
fragment, err := dmr.CombineDataBlocks(slot.data.blocks)
if err != nil {
return err
}
if fragment.Stored > 0 {
// Response with data blocks? That must be a selective ACK
if _, ok := slot.data.header.(*dmr.ResponseDataHeader); ok {
// FIXME(pd0mz): deal with this shit
return nil
}
if err := t.dataBlockComplete(p, fragment); err != nil {
return err
}
// If we are not waiting for an ack, then the data session ended
if !slot.data.header.CommonHeader().ResponseRequested {
return t.dataCallEnd(p)
}
}
return nil
}
func (t *Terminal) dataBlockComplete(p *dmr.Packet, f *dmr.DataFragment) error {
slot := t.slot[p.Timeslot]
var (
data []byte
size int
ddformat = dmr.DDFormatUTF16
)
switch slot.data.header.CommonHeader().ServiceAccessPoint {
case dmr.ServiceAccessPointIPBasedPacketData:
t.debugf(p, "SAP IP based packet data (not implemented)")
break
case dmr.ServiceAccessPointShortData:
t.debugf(p, "SAP short data")
data = f.Data[2:] // Hytera has a 2 byte pre-padding
size = f.Stored - 2 - 4 // Leave out the CRC
if sdd, ok := slot.data.header.(*dmr.ShortDataDefinedDataHeader); ok {
ddformat = sdd.DDFormat
}
break
}
if data == nil || size == 0 {
t.warningf(p, "no data in message")
return nil
}
message, err := dmr.ParseMessageData(data[:size], ddformat, true)
if err != nil {
return err
}
t.infof(p, "message %q", message)
return nil
}
func (t *Terminal) callEnd(p *dmr.Packet) error {
switch t.state {
case dataCallActive:
return t.dataCallEnd(p)
case voiceCallActive:
return t.voiceCallEnd(p)
}
return nil
}
func (t *Terminal) dataCallEnd(p *dmr.Packet) error { func (t *Terminal) dataCallEnd(p *dmr.Packet) error {
slot := t.slot[p.Timeslot] slot := t.slot[p.Timeslot]
@ -70,8 +297,9 @@ func (t *Terminal) dataCallEnd(p *dmr.Packet) error {
return nil return nil
} }
log.Debugf("[%d->%d] data call ended", slot.srcID, slot.dstID) slot.data.packetHeaderValid = false
t.state = idle
t.debugf(p, "data call ended")
return nil return nil
} }
@ -79,10 +307,12 @@ func (t *Terminal) dataCallStart(p *dmr.Packet) error {
slot := t.slot[p.Timeslot] slot := t.slot[p.Timeslot]
if slot.dstID != p.DstID || slot.srcID != p.SrcID || slot.dataType != p.DataType { if slot.dstID != p.DstID || slot.srcID != p.SrcID || slot.dataType != p.DataType {
if t.state == dataCallActive {
if err := t.dataCallEnd(p); err != nil { if err := t.dataCallEnd(p); err != nil {
return err return err
} }
} }
}
slot.data.packetHeaderValid = false slot.data.packetHeaderValid = false
slot.call.start = time.Now() slot.call.start = time.Now()
@ -90,30 +320,51 @@ func (t *Terminal) dataCallStart(p *dmr.Packet) error {
slot.dstID = p.DstID slot.dstID = p.DstID
slot.srcID = p.SrcID slot.srcID = p.SrcID
t.state = dataCallActive t.state = dataCallActive
t.debugf(p, "data call started")
return nil
}
log.Debugf("[%d->%d] data call started", slot.srcID, slot.dstID) func (t *Terminal) voiceCallEnd(p *dmr.Packet) error {
slot := t.slot[p.Timeslot]
if t.state != voiceCallActive {
return nil
}
slot.voice.streamID = 0
t.state = idle
t.debugf(p, "voice call ended")
return nil
}
func (t *Terminal) voiceCallStart(p *dmr.Packet) error {
slot := t.slot[p.Timeslot]
if slot.dstID != p.DstID || slot.srcID != p.SrcID {
if err := t.voiceCallEnd(p); err != nil {
return err
}
}
slot.voice.streamID = p.StreamID
t.state = voiceCallActive
t.debugf(p, "voice call started")
return nil return nil
} }
func (t *Terminal) handlePacket(r dmr.Repeater, p *dmr.Packet) error { func (t *Terminal) handlePacket(r dmr.Repeater, p *dmr.Packet) error {
var err error // Ignore packets not addressed to us or any of the talk groups we monitor
if p.DstID != t.ID { if !t.accept[p.DstID] {
//log.Debugf("[%d->%d] (%s, %#04b): ignored, not sent to me", p.SrcID, p.DstID, dmr.DataTypeName[p.DataType], p.DataType) //log.Debugf("[%d->%d] (%s, %#04b): ignored, not sent to me", p.SrcID, p.DstID, dmr.DataTypeName[p.DataType], p.DataType)
return nil return nil
} }
switch p.DataType { var err error
case dmr.VoiceBurstA, dmr.VoiceBurstB, dmr.VoiceBurstC, dmr.VoiceBurstD, dmr.VoiceBurstE, dmr.VoiceBurstF:
return nil
case dmr.CBSK:
return nil
}
log.Debugf("[%d->%d] (%s, %#04b): sent to me", p.SrcID, p.DstID, dmr.DataTypeName[p.DataType], p.DataType)
t.debugf(p, dmr.DataTypeName[p.DataType])
switch p.DataType { switch p.DataType {
case dmr.CBSK: case dmr.CSBK:
err = t.handleControlBlock(p) err = t.handleControlBlock(p)
break break
case dmr.Data: case dmr.Data:
@ -121,12 +372,22 @@ func (t *Terminal) handlePacket(r dmr.Repeater, p *dmr.Packet) error {
break break
case dmr.Rate34Data: case dmr.Rate34Data:
err = t.handleRate34Data(p) err = t.handleRate34Data(p)
break
case dmr.VoiceBurstA, dmr.VoiceBurstB, dmr.VoiceBurstC, dmr.VoiceBurstD, dmr.VoiceBurstE, dmr.VoiceBurstF:
err = t.handleVoice(p)
break
case dmr.VoiceLC:
return nil
case dmr.TerminatorWithLC:
err = t.handleTerminatorWithLC(p)
return nil
default: default:
log.Debug(hex.Dump(p.Data)) log.Debug(hex.Dump(p.Data))
return nil
} }
if err != nil { if err != nil {
log.Errorf("handle packet error: %v", err) t.errorf(p, "handle packet error: %v", err)
} }
return err return err
@ -136,6 +397,11 @@ func (t *Terminal) handleControlBlock(p *dmr.Packet) error {
slot := t.slot[p.Timeslot] slot := t.slot[p.Timeslot]
slot.last.packetReceived = time.Now() slot.last.packetReceived = time.Now()
// This ends both data and voice calls
if err := t.callEnd(p); err != nil {
return err
}
var ( var (
bits = p.InfoBits() bits = p.InfoBits()
data = make([]byte, 12) data = make([]byte, 12)
@ -149,7 +415,7 @@ func (t *Terminal) handleControlBlock(p *dmr.Packet) error {
return err return err
} }
log.Debugf("[%d->%d] control block %T", cb.SrcID, cb.DstID, cb.Data) t.debugf(p, cb.String())
return nil return nil
} }
@ -158,6 +424,11 @@ func (t *Terminal) handleData(p *dmr.Packet) error {
slot := t.slot[p.Timeslot] slot := t.slot[p.Timeslot]
slot.last.packetReceived = time.Now() slot.last.packetReceived = time.Now()
// This ends voice calls (if any)
if err := t.voiceCallEnd(p); err != nil {
return err
}
var ( var (
bits = p.InfoBits() bits = p.InfoBits()
data = make([]byte, 12) data = make([]byte, 12)
@ -177,29 +448,43 @@ func (t *Terminal) handleData(p *dmr.Packet) error {
slot.selectiveAckRequestsSent = 0 slot.selectiveAckRequestsSent = 0
slot.rxSequence = 0 slot.rxSequence = 0
c := h.CommonHeader() t.debugf(p, "data header: %T", h)
log.Debugf("[%d->%d] data header %T", c.SrcID, c.DstID, h)
switch ht := h.(type) { switch ht := h.(type) {
case dmr.ShortDataDefinedDataHeader: case dmr.ShortDataDefinedDataHeader:
if ht.FullMessage { if ht.FullMessage {
slot.data.blocks = [64]dmr.DataBlock{} slot.fullMessageBlocks = int(ht.AppendedBlocks)
slot.fullMessageBlocks = ht.AppendedBlocks slot.data.blocks = make([]*dmr.DataBlock, slot.fullMessageBlocks)
log.Debugf("[%d->%d] expecting %d data blocks", c.SrcID, c.DstID, slot.fullMessageBlocks) t.debugf(p, "expecting %d data block", slot.fullMessageBlocks)
} }
slot.data.blocksExpected = ht.AppendedBlocks slot.data.blocksExpected = int(ht.AppendedBlocks)
return t.dataCallStart(p) err = t.dataCallStart(p)
break
default: default:
log.Warningf("unhandled data header %T", h) t.warningf(p, "unhandled data header %T", h)
return nil
} }
return nil if err == nil {
slot.data.header = h
slot.data.packetHeaderValid = true
}
return err
} }
func (t *Terminal) handleRate34Data(p *dmr.Packet) error { func (t *Terminal) handleRate34Data(p *dmr.Packet) error {
slot := t.slot[p.Timeslot] slot := t.slot[p.Timeslot]
slot.last.packetReceived = time.Now() slot.last.packetReceived = time.Now()
if t.state != dataCallActive {
t.debugf(p, "no data call in process, ignoring rate ¾ data")
return nil
}
if slot.data.header == nil {
t.warningf(p, "got rate ¾ data, but no data header stored")
return nil
}
var ( var (
bits = p.InfoBits() bits = p.InfoBits()
data = make([]byte, 18) data = make([]byte, 18)
@ -208,7 +493,51 @@ func (t *Terminal) handleRate34Data(p *dmr.Packet) error {
if err := trellis.Decode(bits, data); err != nil { if err := trellis.Decode(bits, data); err != nil {
return err return err
} }
fmt.Print(hex.Dump(data))
db, err := dmr.ParseDataBlock(data, dmr.Rate34Data, slot.data.header.CommonHeader().ResponseRequested)
if err != nil {
return err
}
return t.dataBlock(p, db)
}
func (t *Terminal) handleTerminatorWithLC(p *dmr.Packet) error {
// This ends both data and voice calls
return t.callEnd(p)
}
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 {
// Only accept voice frames from the same stream
t.debugf(p, "ignored frame, active stream id: %#08x, this stream id: %#08x", slot.voice.streamID, p.StreamID)
return nil
}
default:
t.voiceCallStart(p)
break
}
if t.vff != nil {
t.vff(p, bits)
if t.SoftwareDelay {
delta := time.Now().Sub(slot.last.packetReceived)
if delta < VoiceFrameDuration {
delay := VoiceFrameDuration - delta
time.Sleep(delay)
t.debugf(p, "software delay of %s", delay)
}
}
}
return nil return nil
} }

5
trellis/trellis.go
View file

@ -3,7 +3,6 @@ package trellis
import ( import (
"errors" "errors"
"fmt" "fmt"
"log"
"github.com/pd0mz/go-dmr" "github.com/pd0mz/go-dmr"
) )
@ -30,10 +29,6 @@ var (
} }
) )
func init() {
log.Printf("interleave matrix has %d points\n", len(interleaveMatrix))
}
// Decode is a convenience function that takes 196 Info bits and decodes them to 18 bytes (144 bits) binary using Trellis decoding. // Decode is a convenience function that takes 196 Info bits and decodes them to 18 bytes (144 bits) binary using Trellis decoding.
func Decode(bits []byte, bytes []byte) error { func Decode(bits []byte, bytes []byte) error {
if bytes == nil { if bytes == nil {