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

149
controlblock.go
View file

@ -3,31 +3,45 @@ package dmr
import (
"errors"
"fmt"
"strings"
)
// Control Block Options
// Control Block Opcode
const (
CBSKOOutboundActivation = B00111000
CBSKOUnitToUnitVoiceServiceRequest = B00000100
CBSKOUnitToUnitVoiceServiceAnswerResponse = B00000101
CBSKONegativeAcknowledgeResponse = B00100100
CBSKOPreamble = B00111101
OutboundActivationOpcode = B00111000
UnitToUnitVoiceServiceRequestOpcode = B00000100
UnitToUnitVoiceServiceAnswerResponseOpcode = B00000101
NegativeAcknowledgeResponseOpcode = B00100100
PreambleOpcode = B00111101
)
type ControlBlock struct {
CRC uint16
Last bool
CBSKO uint8
Opcode uint8
SrcID, DstID uint32
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 {
String() string
Write([]byte) error
Parse([]byte) error
}
type OutboundActivation struct{}
func (d *OutboundActivation) String() string { return "outbound activation" }
func (d *OutboundActivation) Parse(data []byte) error {
if len(data) != InfoSize {
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 {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
}
data[0] |= B00111000
data[0] |= OutboundActivationOpcode
return nil
}
@ -47,6 +61,10 @@ type UnitToUnitVoiceServiceRequest struct {
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 {
if len(data) != InfoSize {
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 {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
}
data[0] |= B00000100
data[0] |= UnitToUnitVoiceServiceRequestOpcode
data[2] = d.Options
return nil
}
@ -71,6 +89,10 @@ type UnitToUnitVoiceServiceAnswerResponse struct {
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 {
if len(data) != InfoSize {
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 {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
}
data[0] |= B00000101
data[0] |= UnitToUnitVoiceServiceAnswerResponseOpcode
data[2] = d.Options
data[3] = d.Response
return nil
@ -98,6 +120,10 @@ type NegativeAcknowledgeResponse struct {
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 {
if len(data) != InfoSize {
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 {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
}
data[0] |= B00100110
data[0] |= NegativeAcknowledgeResponseOpcode
data[2] = d.ServiceType
if d.SourceType {
data[2] |= B01000000
@ -123,13 +149,27 @@ func (d *NegativeAcknowledgeResponse) Write(data []byte) error {
var _ (ControlBlockData) = (*NegativeAcknowledgeResponse)(nil)
type ControlBlockPreamble struct {
type Preamble struct {
DataFollows bool
DstIsGroup bool
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 {
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
}
func (d *ControlBlockPreamble) Write(data []byte) error {
func (d *Preamble) Write(data []byte) error {
if len(data) != InfoSize {
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
}
data[0] |= B00100110
data[0] |= PreambleOpcode
if d.DataFollows {
data[2] |= B10000000
}
@ -154,7 +194,41 @@ func (d *ControlBlockPreamble) Write(data []byte) error {
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) {
if len(data) != InfoSize {
@ -162,40 +236,55 @@ func ParseControlBlock(data []byte) (*ControlBlock, error) {
}
// 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
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 {
return nil, errors.New("dmr: CBSK feature set ID is set")
return nil, errors.New("dmr: CSBK feature set ID is set")
}
cb := &ControlBlock{
Last: (data[0] & B10000000) > 0,
CBSKO: (data[0] & B00111111),
DstID: uint32(data[4])<<16 | uint32(data[5])<<8 | uint32(data[6]),
SrcID: uint32(data[7])<<16 | uint32(data[8])<<8 | uint32(data[9]),
CRC: uint16(data[10])<<8 | uint16(data[11]),
Last: (data[0] & B10000000) > 0,
Opcode: (data[0] & B00111111),
DstID: uint32(data[4])<<16 | uint32(data[5])<<8 | uint32(data[6]),
SrcID: uint32(data[7])<<16 | uint32(data[8])<<8 | uint32(data[9]),
}
switch cb.CBSKO {
case CBSKOOutboundActivation:
if crc != cb.CRC {
return nil, fmt.Errorf("dmr: control block CRC error (%#04x != %#04x)", crc, cb.CRC)
}
switch cb.Opcode {
case OutboundActivationOpcode:
cb.Data = &OutboundActivation{}
break
case CBSKOUnitToUnitVoiceServiceRequest:
case UnitToUnitVoiceServiceRequestOpcode:
cb.Data = &UnitToUnitVoiceServiceRequest{}
break
case CBSKOUnitToUnitVoiceServiceAnswerResponse:
case UnitToUnitVoiceServiceAnswerResponseOpcode:
cb.Data = &UnitToUnitVoiceServiceAnswerResponse{}
break
case CBSKONegativeAcknowledgeResponse:
case NegativeAcknowledgeResponseOpcode:
cb.Data = &NegativeAcknowledgeResponse{}
break
case CBSKOPreamble:
cb.Data = &ControlBlockPreamble{}
case PreambleOpcode:
cb.Data = &Preamble{}
break
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 {

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
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 {
Serial uint8
CRC uint16
OK bool
Data [24]byte
Data []byte
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))))
}
}

390
dataheader.go
View file

@ -22,6 +22,21 @@ const (
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
const (
ResponseTypeACK uint8 = iota
@ -131,53 +146,95 @@ var DDFormatName = map[uint8]string{
DDFormatUTF32LE: "UTF-32 little endian",
}
type DataHeader interface {
CommonHeader() DataHeaderCommon
// http://www.etsi.org/images/files/DMRcodes/dmrs-mfid.xls
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
DstIsGroup bool
ResponseRequested bool
HeaderCompression bool
ServiceAccessPoint uint8
DstID uint32
SrcID uint32
CRC uint16
Data DataHeaderData
}
func (h *DataHeaderCommon) Parse(header []byte) error {
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
type UDTData struct {
Format uint8
PadNibble uint8
AppendedBlocks uint8
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 {
Common DataHeaderCommon
func (d UDTData) Write(data []byte) error {
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
FullMessage bool
BlocksToFollow 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 {
Common DataHeaderCommon
func (d UnconfirmedData) Write(data []byte) error {
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
FullMessage bool
BlocksToFollow uint8
@ -186,27 +243,58 @@ type ConfirmedDataHeader struct {
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 {
Common DataHeaderCommon
func (d ConfirmedData) Write(data []byte) error {
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
Class uint8
Type 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 {
Common DataHeaderCommon
func (d ResponseData) Write(data []byte) error {
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
}
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 {
Common DataHeaderCommon
func (d ProprietaryData) Write(data []byte) error {
data[1] = (d.ManufacturerID & B01111111)
return nil
}
type ShortDataRawData struct {
AppendedBlocks uint8
SrcPort uint8
DstPort uint8
@ -215,10 +303,26 @@ type ShortDataRawDataHeader struct {
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 {
Common DataHeaderCommon
func (d ShortDataRawData) Write(data []byte) error {
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
DDFormat uint8
Resync bool
@ -226,133 +330,135 @@ type ShortDataDefinedDataHeader struct {
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) {
if len(header) != 12 {
return nil, fmt.Errorf("header must be 12 bytes, got %d", len(header))
func (d ShortDataDefinedData) Write(data []byte) error {
data[0] |= d.AppendedBlocks & B00110000
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 (
ccrc = (uint16(header[10]) << 8) | uint16(header[11])
hcrc = dataHeaderCRC(header)
ccrc = (uint16(data[10]) << 8) | uint16(data[11])
hcrc = dataHeaderCRC(data)
)
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 {
return ProprietaryDataHeader{
Common: DataHeaderCommon{
ServiceAccessPoint: (header[0] & B11110000) >> 4,
PacketFormat: (header[0] & B00001111),
CRC: ccrc,
},
ManufacturerID: header[1],
}, nil
h.Data = ProprietaryData{
ManufacturerID: data[1] & B01111111,
}
} else {
switch h.PacketFormat {
case PacketFormatUDT:
h.Data = UDTData{
Format: (data[1] & B00001111),
PadNibble: (data[8] & B11111000) >> 3,
AppendedBlocks: (data[8] & B00000011),
SupplementaryFlag: (data[9] & B10000000) > 0,
Opcode: (data[9] & B00111111),
}
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)
}
}
common := DataHeaderCommon{
CRC: ccrc,
}
if err := common.Parse(header); err != nil {
return nil, err
}
switch common.PacketFormat {
case PacketFormatUDT:
return UDTDataHeader{
Common: common,
Format: (header[1] & B00001111),
PadNibble: (header[8] & B11111000) >> 3,
AppendedBlocks: (header[8] & B00000011),
SupplementaryFlag: (header[9] & B10000000) > 0,
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)
}
return h, nil
}
func dataHeaderCRC(header []byte) uint16 {
func dataHeaderCRC(data []byte) uint16 {
var crc uint16
if len(header) < 10 {
if len(data) < 10 {
return crc
}
for i := 0; i < 10; i++ {
crc16(&crc, header[i])
crc16(&crc, data[i])
}
crc16end(&crc)
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
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
PingInterval = time.Second * 5
PingTimeout = time.Second * 15
SendInterval = time.Millisecond * 30
)
// Peer is a remote repeater that also speaks the Homebrew protocol
@ -110,8 +111,11 @@ type Homebrew struct {
conn *net.UDPConn
closed bool
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
queue []*dmr.Packet
}
// New creates a new Homebrew repeater
@ -131,6 +135,7 @@ func New(config *RepeaterConfiguration, addr *net.UDPAddr) (*Homebrew, error) {
PeerID: make(map[uint32]*Peer),
id: packRepeaterID(config.ID),
mutex: &sync.Mutex{},
queue: make([]*dmr.Packet, 0),
}
if h.conn, err = net.ListenUDP("udp", addr); err != nil {
return nil, errors.New("homebrew: " + err.Error())
@ -242,6 +247,21 @@ func (h *Homebrew) ListenAndServe() error {
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 {
return h.pf
}
@ -522,12 +542,20 @@ func (h *Homebrew) handleAuth(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 {
return peer.PacketReceived(h, p)
}
if h.pf == nil {
return errors.New("homebrew: no PacketReceived func defined to handle DMR packet")
}
return h.pf(h, p)
}
@ -666,6 +694,44 @@ func packRepeaterID(id uint32) []byte {
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.
func ParseData(data []byte) (*dmr.Packet, error) {
if len(data) != 53 {
@ -686,8 +752,12 @@ func ParseData(data []byte) (*dmr.Packet, error) {
switch (data[15] >> 2) & 0x03 {
case 0x00, 0x01: // voice (B-F), voice sync (A)
p.DataType = dmr.VoiceBurstA + (data[15] >> 4)
break
case 0x02: // data sync
p.DataType = (data[15] >> 4)
break
default: // unknown/unused
return nil, errors.New("homebrew: unexpected frame type 0b11")
}
return p, nil

2
ipsc/packet.go
View file

@ -44,7 +44,7 @@ var (
0x0000: "unknown",
0x1111: "voice LC header",
0x2222: "terminator with LC",
0x3333: "CBSK",
0x3333: "CSBK",
0x4444: "data header",
0x5555: "rate 1/2 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
VoiceLC // Indicates the beginning of voice transmission, carries addressing 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
MultiBlockControlContinuation // Follow-on blocks for multi-block control
Data // Carries addressing and numbering of packet data blocks
@ -26,12 +26,12 @@ var DataTypeName = map[uint8]string{
PrivacyIndicator: "privacy indicator",
VoiceLC: "voice LC",
TerminatorWithLC: "terminator with LC",
CBSK: "control block",
CSBK: "control block",
MultiBlockControl: "multi-block control",
MultiBlockControlContinuation: "multi-block control follow-on",
Data: "data",
Rate12Data: "rate 1/2 packet data",
Rate34Data: "rate 3/4 packet data",
Rate12Data: "rate ½ packet data",
Rate34Data: "rate ¾ packet data",
Idle: "idle",
VoiceBurstA: "voice (burst A)",
VoiceBurstB: "voice (burst B)",

1
repeater.go
View file

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

413
terminal/terminal.go
View file

@ -2,7 +2,9 @@ package terminal
import (
"encoding/hex"
"errors"
"fmt"
"strings"
"time"
"github.com/op/go-logging"
@ -16,9 +18,11 @@ var log = logging.MustGetLogger("dmr/terminal")
const (
idle uint8 = iota
dataCallActive
voideCallActive
voiceCallActive
)
const VoiceFrameDuration = time.Millisecond * 60
type Slot struct {
call struct {
start time.Time
@ -28,28 +32,41 @@ type Slot struct {
dataType uint8
data struct {
packetHeaderValid bool
blocks [64]dmr.DataBlock
blocksExpected uint8
blocksReceived uint8
blocks []*dmr.DataBlock
blocksExpected int
blocksReceived int
header dmr.DataHeader
}
voice struct {
lastFrame uint8
streamID uint32
}
selectiveAckRequestsSent int
rxSequence int
fullMessageBlocks uint8
fullMessageBlocks int
last struct {
packetReceived time.Time
}
}
func NewSlot() Slot {
return Slot{}
func NewSlot() *Slot {
return &Slot{}
}
type VoiceFrameFunc func(*dmr.Packet, []byte)
type Terminal struct {
ID uint32
Call string
Repeater dmr.Repeater
slot []Slot
state uint8
ID uint32
Call string
CallMap map[uint32]string
Repeater dmr.Repeater
TalkGroup []uint32
SoftwareDelay bool
accept map[uint32]bool
slot []*Slot
state uint8
vff VoiceFrameFunc
}
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,
Call: call,
Repeater: r,
slot: []Slot{NewSlot(), NewSlot()},
slot: []*Slot{NewSlot(), NewSlot(), NewSlot()},
accept: map[uint32]bool{id: true},
}
r.SetPacketFunc(t.handlePacket)
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 {
slot := t.slot[p.Timeslot]
@ -70,8 +297,9 @@ func (t *Terminal) dataCallEnd(p *dmr.Packet) error {
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
}
@ -79,8 +307,10 @@ func (t *Terminal) dataCallStart(p *dmr.Packet) error {
slot := t.slot[p.Timeslot]
if slot.dstID != p.DstID || slot.srcID != p.SrcID || slot.dataType != p.DataType {
if err := t.dataCallEnd(p); err != nil {
return err
if t.state == dataCallActive {
if err := t.dataCallEnd(p); err != nil {
return err
}
}
}
@ -90,30 +320,51 @@ func (t *Terminal) dataCallStart(p *dmr.Packet) error {
slot.dstID = p.DstID
slot.srcID = p.SrcID
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
}
func (t *Terminal) handlePacket(r dmr.Repeater, p *dmr.Packet) error {
var err error
if p.DstID != t.ID {
// Ignore packets not addressed to us or any of the talk groups we monitor
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)
return nil
}
switch p.DataType {
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)
var err error
t.debugf(p, dmr.DataTypeName[p.DataType])
switch p.DataType {
case dmr.CBSK:
case dmr.CSBK:
err = t.handleControlBlock(p)
break
case dmr.Data:
@ -121,12 +372,22 @@ func (t *Terminal) handlePacket(r dmr.Repeater, p *dmr.Packet) error {
break
case dmr.Rate34Data:
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:
log.Debug(hex.Dump(p.Data))
return nil
}
if err != nil {
log.Errorf("handle packet error: %v", err)
t.errorf(p, "handle packet error: %v", err)
}
return err
@ -136,6 +397,11 @@ func (t *Terminal) handleControlBlock(p *dmr.Packet) error {
slot := t.slot[p.Timeslot]
slot.last.packetReceived = time.Now()
// This ends both data and voice calls
if err := t.callEnd(p); err != nil {
return err
}
var (
bits = p.InfoBits()
data = make([]byte, 12)
@ -149,7 +415,7 @@ func (t *Terminal) handleControlBlock(p *dmr.Packet) error {
return err
}
log.Debugf("[%d->%d] control block %T", cb.SrcID, cb.DstID, cb.Data)
t.debugf(p, cb.String())
return nil
}
@ -158,6 +424,11 @@ func (t *Terminal) handleData(p *dmr.Packet) error {
slot := t.slot[p.Timeslot]
slot.last.packetReceived = time.Now()
// This ends voice calls (if any)
if err := t.voiceCallEnd(p); err != nil {
return err
}
var (
bits = p.InfoBits()
data = make([]byte, 12)
@ -177,29 +448,43 @@ func (t *Terminal) handleData(p *dmr.Packet) error {
slot.selectiveAckRequestsSent = 0
slot.rxSequence = 0
c := h.CommonHeader()
log.Debugf("[%d->%d] data header %T", c.SrcID, c.DstID, h)
t.debugf(p, "data header: %T", h)
switch ht := h.(type) {
case dmr.ShortDataDefinedDataHeader:
if ht.FullMessage {
slot.data.blocks = [64]dmr.DataBlock{}
slot.fullMessageBlocks = ht.AppendedBlocks
log.Debugf("[%d->%d] expecting %d data blocks", c.SrcID, c.DstID, slot.fullMessageBlocks)
slot.fullMessageBlocks = int(ht.AppendedBlocks)
slot.data.blocks = make([]*dmr.DataBlock, slot.fullMessageBlocks)
t.debugf(p, "expecting %d data block", slot.fullMessageBlocks)
}
slot.data.blocksExpected = ht.AppendedBlocks
return t.dataCallStart(p)
slot.data.blocksExpected = int(ht.AppendedBlocks)
err = t.dataCallStart(p)
break
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 {
slot := t.slot[p.Timeslot]
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 (
bits = p.InfoBits()
data = make([]byte, 18)
@ -208,7 +493,51 @@ func (t *Terminal) handleRate34Data(p *dmr.Packet) error {
if err := trellis.Decode(bits, data); err != nil {
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
}

5
trellis/trellis.go
View file

@ -3,7 +3,6 @@ package trellis
import (
"errors"
"fmt"
"log"
"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.
func Decode(bits []byte, bytes []byte) error {
if bytes == nil {