Checkpoint

pull/1/head
Wijnand Modderman-Lenstra 9 years ago
parent 599d42dca9
commit 3fe41e10ef

@ -23,6 +23,40 @@ type ControlBlock struct {
Data ControlBlockData
}
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 (cb *ControlBlock) String() string {
if cb.Data == nil {
return fmt.Sprintf("CSBK, last %t, %d->%d, unknown (opcode %d)",
@ -196,40 +230,6 @@ func (d *Preamble) Write(data []byte) error {
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 {
return nil, fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))

@ -1,13 +1,6 @@
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++ {
@ -22,7 +15,7 @@ func crc9(crc *uint16, b uint8, bits int) {
(*crc)++
}
if xor {
(*crc) ^= crc9poly
(*crc) ^= 0x0059
}
v >>= 1
}
@ -35,21 +28,22 @@ func crc9end(crc *uint16, bits int) {
// Limit the number of shift registers to 9.
*crc &= 0x01ff
if xor {
(*crc) ^= crc9poly
(*crc) ^= 0x0059
}
}
}
// G(x) = x^16+x^12+x^5+1
func crc16(crc *uint16, b byte) {
var v uint8 = 0x80
for i := 0; i < 8; i++ {
xor := ((*crc) & 0x8000) > 0
xor := ((*crc) & 0x8000) != 0
(*crc) <<= 1
if b&v > 0 {
(*crc)++
}
if xor {
(*crc) ^= crc16poly
(*crc) ^= 0x1021
}
v >>= 1
}
@ -57,10 +51,10 @@ func crc16(crc *uint16, b byte) {
func crc16end(crc *uint16) {
for i := 0; i < 16; i++ {
xor := ((*crc) & 0x8000) > 0
xor := ((*crc) & 0x8000) != 0
(*crc) <<= 1
if xor {
(*crc) ^= crc16poly
(*crc) ^= 0x1021
}
}
}
@ -74,7 +68,7 @@ func crc32(crc *uint32, b byte) {
(*crc)++
}
if xor {
(*crc) ^= crc32poly
(*crc) ^= 0x04c11db7
}
v >>= 1
}
@ -85,7 +79,7 @@ func crc32end(crc *uint32) {
xor := ((*crc) & 0x80000000) > 0
(*crc) <<= 1
if xor {
(*crc) ^= crc32poly
(*crc) ^= 0x04c11db7
}
}
}

@ -35,10 +35,7 @@ func ParseDataBlock(data []byte, dataType uint8, confirmed bool) (*DataBlock, er
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]))
copy(db.Data, data[2:2+db.Length])
for _, block := range db.Data {
crc9(&crc, block, 8)
@ -56,7 +53,6 @@ func ParseDataBlock(data []byte, dataType uint8, confirmed bool) (*DataBlock, er
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])
}
@ -239,8 +235,6 @@ func CombineDataBlocks(blocks []*DataBlock) (*DataFragment, error) {
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
}
@ -279,33 +273,6 @@ func ParseMessageData(data []byte, ddFormat uint8, nullTerminated bool) (string,
}
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,

@ -1,6 +1,9 @@
package dmr
import "fmt"
import (
"fmt"
"strings"
)
// Data Header Packet Format
const (
@ -37,17 +40,35 @@ const (
ServiceAccessPointShortData // 0b1010
)
// Response Data Header Response Type
var ServiceAccessPointName = map[uint8]string{
ServiceAccessPointUDT: "UDT",
ServiceAccessPointTCPIPHeaderCompression: "TCP/IP header compression",
ServiceAccessPointUDPIPHeaderCompression: "UDP/IP header compression",
ServiceAccessPointIPBasedPacketData: "IP based packet data",
ServiceAccessPointARP: "ARP",
ServiceAccessPointProprietaryData: "proprietary data",
ServiceAccessPointShortData: "short data",
}
// Response Data Header Response Type, encodes class and type
const (
ResponseTypeACK uint8 = iota
ResponseTypeIllegalFormat
ResponseTypePacketCRCFailed
ResponseTypeMemoryFull
ResponseTypeRecvFSVNOutOfSeq
ResponseTypeUndeliverable
ResponseTypeRecvPktOutOfSeq
ResponseTypeDisallowed
ResponseTypeSelectiveACK
_ uint8 = iota // Class 0b00, Type 0b000
ResponseTypeACK // Class 0b00, Type 0b001
_ // Class 0b00, Type 0b010
_ // Class 0b00, Type 0b011
_ // Class 0b00, Type 0b100
_ // Class 0b00, Type 0b101
_ // Class 0b00, Type 0b110
_ // Class 0b00, Type 0b111
ResponseTypeIllegalFormat // Class 0b01, Type 0b000
ResponseTypePacketCRCFailed // Class 0b01, Type 0b001
ResponseTypeMemoryFull // Class 0b01, Type 0b010
ResponseTypeRecvFSVNOutOfSeq // Class 0b01, Type 0b011
ResponseTypeUndeliverable // Class 0b01, Type 0b100
ResponseTypeRecvPktOutOfSeq // Class 0b01, Type 0b101
ResponseTypeDisallowed // Class 0b01, Type 0b110
_ // Class 0b01, Type 0b111
ResponseTypeSelectiveACK // Class 0b10, Type 0b000
)
var ResponseTypeName = map[uint8]string{
@ -188,6 +209,66 @@ type DataHeader struct {
Data DataHeaderData
}
func (h *DataHeader) Bytes() ([]byte, error) {
var data = make([]byte, 12)
data[0] = (h.PacketFormat & B00001111)
if h.DstIsGroup {
data[0] |= B10000000
}
if h.ResponseRequested {
data[0] |= B01000000
}
if h.HeaderCompression {
data[0] |= B00100000
}
data[1] = (h.ServiceAccessPoint & B00001111)
data[2] = uint8(h.DstID >> 16)
data[3] = uint8(h.DstID >> 8)
data[4] = uint8(h.DstID)
data[5] = uint8(h.SrcID >> 16)
data[6] = uint8(h.SrcID >> 8)
data[7] = uint8(h.SrcID)
if h.Data != nil {
if err := h.Data.Write(data); err != nil {
return nil, err
}
}
h.CRC = 0
for i := 0; i < 10; i++ {
crc16(&h.CRC, data[i])
}
crc16end(&h.CRC)
// Inverting according to the inversion polynomial.
h.CRC = ^h.CRC
// Applying CRC mask, see DMR AI spec. page 143.
h.CRC ^= 0xcccc
data[10] = uint8(h.CRC >> 8)
data[11] = uint8(h.CRC)
return data, nil
}
func (h DataHeader) String() string {
var part = []string{"data header"}
if h.DstIsGroup {
part = append(part, "group")
} else {
part = append(part, "unit")
}
part = append(part, fmt.Sprintf("response %t, sap %s (%d), %d->%d",
h.ResponseRequested, ServiceAccessPointName[h.ServiceAccessPoint], h.ServiceAccessPoint,
h.SrcID, h.DstID))
if h.Data != nil {
part = append(part, h.Data.String())
}
return strings.Join(part, ", ")
}
type UDTData struct {
Format uint8
PadNibble uint8
@ -264,19 +345,18 @@ func (d ConfirmedData) Write(data []byte) error {
type ResponseData struct {
BlocksToFollow uint8
Class uint8
Type uint8
ClassType uint8 // See ResponseType map above
Status uint8
}
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)
return fmt.Sprintf("response, blocks %d, type %s (%#02b %#03b), status %d",
d.BlocksToFollow, ResponseTypeName[d.ClassType], (d.ClassType >> 3), (d.ClassType & 0x07), d.Status)
}
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
data[9] = d.Status | d.ClassType<<3
return nil
}
@ -382,7 +462,7 @@ func ParseDataHeader(data []byte, proprietary bool) (*DataHeader, error) {
} else {
switch h.PacketFormat {
case PacketFormatUDT:
h.Data = UDTData{
h.Data = &UDTData{
Format: (data[1] & B00001111),
PadNibble: (data[8] & B11111000) >> 3,
AppendedBlocks: (data[8] & B00000011),
@ -392,16 +472,15 @@ func ParseDataHeader(data []byte, proprietary bool) (*DataHeader, error) {
break
case PacketFormatResponse:
h.Data = ResponseData{
h.Data = &ResponseData{
BlocksToFollow: (data[8] & B01111111),
Class: (data[9] & B11000000) >> 6,
Type: (data[9] & B00111000) >> 3,
ClassType: (data[9] & B11111000) >> 3,
Status: (data[9] & B00000111),
}
break
case PacketFormatUnconfirmedData:
h.Data = UnconfirmedData{
h.Data = &UnconfirmedData{
PadOctetCount: (data[0] & B00010000) | (data[1] & B00001111),
FullMessage: (data[8] & B10000000) > 0,
BlocksToFollow: (data[8] & B01111111),
@ -410,7 +489,7 @@ func ParseDataHeader(data []byte, proprietary bool) (*DataHeader, error) {
break
case PacketFormatConfirmedData:
h.Data = ConfirmedData{
h.Data = &ConfirmedData{
PadOctetCount: (data[0] & B00010000) | (data[1] & B00001111),
FullMessage: (data[8] & B10000000) > 0,
BlocksToFollow: (data[8] & B01111111),
@ -421,7 +500,7 @@ func ParseDataHeader(data []byte, proprietary bool) (*DataHeader, error) {
break
case PacketFormatShortDataRaw:
h.Data = ShortDataRawData{
h.Data = &ShortDataRawData{
AppendedBlocks: (data[0] & B00110000) | (data[1] & B00001111),
SrcPort: (data[8] & B11100000) >> 5,
DstPort: (data[8] & B00011100) >> 2,
@ -432,7 +511,7 @@ func ParseDataHeader(data []byte, proprietary bool) (*DataHeader, error) {
break
case PacketFormatShortDataDefined:
h.Data = ShortDataDefinedData{
h.Data = &ShortDataDefinedData{
AppendedBlocks: (data[0] & B00110000) | (data[1] & B00001111),
DDFormat: (data[8] & B11111100) >> 2,
Resync: (data[8] & B00000010) > 0,

@ -1 +1,225 @@
package dmr
import (
"encoding/hex"
"testing"
)
func testDataHeader(want *DataHeader, t *testing.T) *DataHeader {
want.SrcID = 2042214
want.DstID = 2043044
t.Logf("encode:\n%s", want.String())
data, err := want.Bytes()
if err != nil {
t.Fatalf("encode failed: %v", err)
}
t.Logf("encoded:\n%s", hex.Dump(data))
test, err := ParseDataHeader(data, false)
if err != nil {
t.Fatalf("decode failed: %v", err)
}
if test.SrcID != want.SrcID || test.DstID != want.DstID {
t.Fatal("decode failed, ID wrong")
}
t.Logf("decoded:\n%s", test.String())
return test
}
func TestDataHeaderUDT(t *testing.T) {
want := &DataHeader{
PacketFormat: PacketFormatUDT,
Data: &UDTData{
Format: UDTFormatIPAddress,
PadNibble: 2,
AppendedBlocks: 3,
Opcode: 4,
},
}
test := testDataHeader(want, t)
d, ok := test.Data.(*UDTData)
switch {
case !ok:
t.Fatalf("decode failed: expected UDTData, got %T", test.Data)
case d.Format != UDTFormatIPAddress:
t.Fatalf("decode failed: format wrong")
case d.PadNibble != 2:
t.Fatalf("decode failed: pad nibble wrong")
case d.AppendedBlocks != 3:
t.Fatalf("decode failed: appended blocks wrong")
case d.Opcode != 4:
t.Fatalf("decode failed: opcode wrong")
}
}
func TestDataHeaderResponse(t *testing.T) {
want := &DataHeader{
PacketFormat: PacketFormatResponse,
Data: &ResponseData{
BlocksToFollow: 0x10,
ClassType: ResponseTypeSelectiveACK,
},
}
test := testDataHeader(want, t)
d, ok := test.Data.(*ResponseData)
switch {
case !ok:
t.Fatalf("decode failed: expected ResponseData, got %T", test.Data)
case d.BlocksToFollow != 0x10:
t.Fatalf("decode failed: wrong blocks %d, expected 16", d.BlocksToFollow)
case d.ClassType != ResponseTypeSelectiveACK:
t.Fatalf("decode failed: wrong type %s (%d), expected selective ACK", ResponseTypeName[d.ClassType], d.ClassType)
}
}
func TestDataHeaderUnconfirmedData(t *testing.T) {
want := &DataHeader{
PacketFormat: PacketFormatUnconfirmedData,
Data: &UnconfirmedData{
PadOctetCount: 2,
FullMessage: true,
BlocksToFollow: 5,
FragmentSequenceNumber: 3,
},
}
test := testDataHeader(want, t)
d, ok := test.Data.(*UnconfirmedData)
switch {
case !ok:
t.Fatalf("decode failed: expected UnconfirmedData, got %T", test.Data)
case d.PadOctetCount != 2:
t.Fatalf("decode failed: pad octet count wrong")
case !d.FullMessage:
t.Fatalf("decode failed: full message bit wrong")
case d.BlocksToFollow != 5:
t.Fatalf("decode failed: blocks to follow wrong")
case d.FragmentSequenceNumber != 3:
t.Fatalf("decode failed: fragment sequence number wrong")
}
}
func TestDataHeaderConfirmedData(t *testing.T) {
want := &DataHeader{
PacketFormat: PacketFormatConfirmedData,
Data: &ConfirmedData{
PadOctetCount: 2,
FullMessage: true,
BlocksToFollow: 5,
SendSequenceNumber: 4,
FragmentSequenceNumber: 3,
},
}
test := testDataHeader(want, t)
d, ok := test.Data.(*ConfirmedData)
switch {
case !ok:
t.Fatalf("decode failed: expected ConfirmedData, got %T", test.Data)
case d.PadOctetCount != 2:
t.Fatalf("decode failed: pad octet count wrong")
case !d.FullMessage:
t.Fatalf("decode failed: full message bit wrong")
case d.Resync:
t.Fatalf("decode failed: resync bit wrong")
case d.BlocksToFollow != 5:
t.Fatalf("decode failed: blocks to follow wrong")
case d.SendSequenceNumber != 4:
t.Fatalf("decode failed: fragment sequence number wrong")
case d.FragmentSequenceNumber != 3:
t.Fatalf("decode failed: fragment sequence number wrong")
}
}
func TestDataHeaderShortDataRaw(t *testing.T) {
want := &DataHeader{
PacketFormat: PacketFormatShortDataRaw,
Data: &ShortDataRawData{
AppendedBlocks: 3,
SrcPort: 4,
DstPort: 5,
FullMessage: true,
BitPadding: 2,
},
}
test := testDataHeader(want, t)
d, ok := test.Data.(*ShortDataRawData)
switch {
case !ok:
t.Fatalf("decode failed: expected ShortDataRawData, got %T", test.Data)
case d.AppendedBlocks != 3:
t.Fatalf("decode failed: appended blocks wrong")
case d.SrcPort != 4:
t.Fatalf("decode failed: src port wrong")
case d.DstPort != 5:
t.Fatalf("decode failed: dst port wrong")
case d.Resync:
t.Fatalf("decode failed: rsync bit wrong")
case !d.FullMessage:
t.Fatalf("decode failed: full message bit wrong")
case d.BitPadding != 2:
t.Fatalf("decode failed: bit padding wrong")
}
}
func TestDataHeaderShortDataDefined(t *testing.T) {
want := &DataHeader{
PacketFormat: PacketFormatShortDataDefined,
Data: &ShortDataDefinedData{
AppendedBlocks: 3,
DDFormat: DDFormatUTF16,
FullMessage: true,
BitPadding: 2,
},
}
test := testDataHeader(want, t)
d, ok := test.Data.(*ShortDataDefinedData)
switch {
case !ok:
t.Fatalf("decode failed: expected ShortDataDefinedData, got %T", test.Data)
case d.AppendedBlocks != 3:
t.Fatalf("decode failed: appended blocks wrong")
case d.DDFormat != DDFormatUTF16:
t.Fatalf("decode failed: dd format wrong, expected UTF-16, got %d", DDFormatName[d.DDFormat])
case d.Resync:
t.Fatalf("decode failed: rsync bit wrong")
case !d.FullMessage:
t.Fatalf("decode failed: full message bit wrong")
case d.BitPadding != 2:
t.Fatalf("decode failed: bit padding wrong")
}
}

Loading…
Cancel
Save