diff --git a/controlblock.go b/controlblock.go
index 7244bcc..8206ceb 100644
--- a/controlblock.go
+++ b/controlblock.go
@@ -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]),
+	}
+
+	if crc != cb.CRC {
+		return nil, fmt.Errorf("dmr: control block CRC error (%#04x != %#04x)", crc, cb.CRC)
 	}
 
-	switch cb.CBSKO {
-	case CBSKOOutboundActivation:
+	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 {
diff --git a/controlblock_test.go b/controlblock_test.go
new file mode 100644
index 0000000..0538f62
--- /dev/null
+++ b/controlblock_test.go
@@ -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())
+	}
+}
diff --git a/crc.go b/crc.go
new file mode 100644
index 0000000..f797e0b
--- /dev/null
+++ b/crc.go
@@ -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
+		}
+	}
+}
diff --git a/crc_test.go b/crc_test.go
new file mode 100644
index 0000000..8a8c4ca
--- /dev/null
+++ b/crc_test.go
@@ -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)
+		}
+	}
+}
diff --git a/data.go b/data.go
index db45e53..5832ca3 100644
--- a/data.go
+++ b/data.go
@@ -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),
+	}
+}
diff --git a/data_encoding.go b/data_encoding.go
new file mode 100644
index 0000000..76dfa8c
--- /dev/null
+++ b/data_encoding.go
@@ -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{}}
+}
diff --git a/data_test.go b/data_test.go
new file mode 100644
index 0000000..04aaa84
--- /dev/null
+++ b/data_test.go
@@ -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))))
+	}
+}
diff --git a/dataheader.go b/dataheader.go
index 33a37ca..e9a717e 100644
--- a/dataheader.go
+++ b/dataheader.go
@@ -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 DataHeaderData interface {
+	String() string
+	Write([]byte) error
 }
 
-type DataHeaderCommon struct {
+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)
+}
+
+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 UnconfirmedDataHeader struct {
-	Common                 DataHeaderCommon
+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)
+}
+
+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 ResponseDataHeader struct {
-	Common         DataHeaderCommon
+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)
+}
+
+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 ProprietaryDataHeader struct {
-	Common         DataHeaderCommon
+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)
+}
+
+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 ShortDataDefinedDataHeader struct {
-	Common         DataHeaderCommon
+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)
 	}
 
-	if proprietary {
-		return ProprietaryDataHeader{
-			Common: DataHeaderCommon{
-				ServiceAccessPoint: (header[0] & B11110000) >> 4,
-				PacketFormat:       (header[0] & B00001111),
-				CRC:                ccrc,
-			},
-			ManufacturerID: header[1],
-		}, nil
+	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,
 	}
 
-	common := DataHeaderCommon{
-		CRC: ccrc,
-	}
-	if err := common.Parse(header); err != nil {
-		return nil, err
-	}
+	if proprietary {
+		h.Data = ProprietaryData{
+			ManufacturerID: data[1] & B01111111,
+		}
 
-	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)
+	} 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)
+		}
 	}
+
+	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
-		}
-	}
-}
diff --git a/dataheader_test.go b/dataheader_test.go
new file mode 100644
index 0000000..02fa8bb
--- /dev/null
+++ b/dataheader_test.go
@@ -0,0 +1 @@
+package dmr
diff --git a/dmr.go b/dmr.go
index 23f233e..04e2799 100644
--- a/dmr.go
+++ b/dmr.go
@@ -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")
diff --git a/homebrew/homebrew.go b/homebrew/homebrew.go
index 66ea8dd..46a6465 100644
--- a/homebrew/homebrew.go
+++ b/homebrew/homebrew.go
@@ -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
diff --git a/ipsc/packet.go b/ipsc/packet.go
index 83781a2..934aca4 100644
--- a/ipsc/packet.go
+++ b/ipsc/packet.go
@@ -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",
diff --git a/packet.go b/packet.go
index b1126a2..1633dda 100644
--- a/packet.go
+++ b/packet.go
@@ -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)",
diff --git a/repeater.go b/repeater.go
index 91660af..65df4f6 100644
--- a/repeater.go
+++ b/repeater.go
@@ -5,6 +5,7 @@ type Repeater interface {
 	Active() bool
 	Close() error
 	ListenAndServe() error
+	Send(*Packet) error
 
 	GetPacketFunc() PacketFunc
 	SetPacketFunc(PacketFunc)
diff --git a/terminal/terminal.go b/terminal/terminal.go
index 190cdc7..9e52981 100644
--- a/terminal/terminal.go
+++ b/terminal/terminal.go
@@ -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
+}
+
+func (t *Terminal) voiceCallEnd(p *dmr.Packet) error {
+	slot := t.slot[p.Timeslot]
 
-	log.Debugf("[%d->%d] data call started", slot.srcID, slot.dstID)
+	if t.state != voiceCallActive {
+		return nil
+	}
 
+	slot.voice.streamID = 0
+	t.state = idle
+	t.debugf(p, "voice call ended")
 	return nil
 }
 
-func (t *Terminal) handlePacket(r dmr.Repeater, p *dmr.Packet) error {
-	var err error
-	if p.DstID != t.ID {
-		//log.Debugf("[%d->%d] (%s, %#04b): ignored, not sent to me", p.SrcID, p.DstID, dmr.DataTypeName[p.DataType], p.DataType)
-		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
+		}
 	}
 
-	switch p.DataType {
-	case dmr.VoiceBurstA, dmr.VoiceBurstB, dmr.VoiceBurstC, dmr.VoiceBurstD, dmr.VoiceBurstE, dmr.VoiceBurstF:
-		return nil
-	case dmr.CBSK:
+	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 {
+	// 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
 	}
 
-	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
 }
diff --git a/trellis/trellis.go b/trellis/trellis.go
index 7479ce9..2757b82 100644
--- a/trellis/trellis.go
+++ b/trellis/trellis.go
@@ -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 {