From 3fe41e10ef9ab7a09996df024969451b9f6a89dc Mon Sep 17 00:00:00 2001 From: Wijnand Modderman-Lenstra Date: Sat, 19 Dec 2015 00:00:52 +0100 Subject: [PATCH] Checkpoint --- controlblock.go | 68 +++++++------- crc.go | 26 ++---- data.go | 35 +------ dataheader.go | 127 ++++++++++++++++++++----- dataheader_test.go | 224 +++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 372 insertions(+), 108 deletions(-) diff --git a/controlblock.go b/controlblock.go index 8206ceb..ad36a98 100644 --- a/controlblock.go +++ b/controlblock.go @@ -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)) diff --git a/crc.go b/crc.go index f797e0b..f4e8b91 100644 --- a/crc.go +++ b/crc.go @@ -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 -) - +// G(x) = x^9+x^6+x^4+x^3+1 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 } } } diff --git a/data.go b/data.go index 5832ca3..f2cc79b 100644 --- a/data.go +++ b/data.go @@ -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, diff --git a/dataheader.go b/dataheader.go index e9a717e..c7ac1da 100644 --- a/dataheader.go +++ b/dataheader.go @@ -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, diff --git a/dataheader_test.go b/dataheader_test.go index 02fa8bb..19562a0 100644 --- a/dataheader_test.go +++ b/dataheader_test.go @@ -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") + } +}