go-dmr/terminal/terminal.go

package terminal

import (
	"encoding/hex"
	"errors"
	"fmt"
	"strings"
	"time"

	"github.com/op/go-logging"
	"github.com/pd0mz/go-dmr"
	"github.com/pd0mz/go-dmr/bptc"
	"github.com/pd0mz/go-dmr/lc"
	"github.com/pd0mz/go-dmr/trellis"
	"github.com/pd0mz/go-dmr/vbptc"
)

var log = logging.MustGetLogger("dmr/terminal")

const (
	idle uint8 = iota
	dataCallActive
	voiceCallActive
)

const VoiceFrameDuration = time.Millisecond * 60

type Slot struct {
	call struct {
		start time.Time
		end   time.Time
	}
	dstID, srcID uint32
	dataType     uint8
	data         struct {
		packetHeaderValid bool
		blocks            []*dmr.DataBlock
		blocksExpected    int
		blocksReceived    int
		header            *dmr.DataHeader
	}
	voice struct {
		lastFrame uint8
		streamID  uint32
	}
	selectiveAckRequestsSent int
	rxSequence               int
	fullMessageBlocks        int
	embeddedSignalling       *vbptc.VBPTC
	last                     struct {
		packetReceived time.Time
	}
}

func NewSlot() *Slot {
	s := &Slot{}

	// Expecting 8 rows of variable length BPTC coded embedded LC data.
	// It will contain 77 data bits (without the Hamming (16,11) checksums
	// and the last row of parity bits).
	s.embeddedSignalling = vbptc.New(8)

	return s
}

type VoiceFrameFunc func(*dmr.Packet, []byte)

type Terminal struct {
	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 {
	t := &Terminal{
		ID:       id,
		Call:     call,
		Repeater: r,
		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?!")
	}
	if slot.data.header.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++
	t.debugf(p, "data block %d/%d", slot.data.blocksReceived, slot.data.blocksExpected)
	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.Data.(*dmr.ResponseData)
		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.Data.(*dmr.ResponseData); 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.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.ServiceAccessPoint {
	case dmr.ServiceAccessPointShortData:
		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.Data.(*dmr.ShortDataDefinedData); ok {
			ddformat = sdd.DDFormat
		}
		t.debugf(p, "bytes %d, format %s (%d)", size, dmr.DDFormatName[ddformat], ddformat)
		break

	default:
		t.warningf(p, "service accesspoint not implemented")
	}

	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]

	if t.state != dataCallActive {
		return nil
	}

	slot.data.packetHeaderValid = false
	t.state = idle
	t.debugf(p, "data call ended")
	return nil
}

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 t.state == dataCallActive {
			if err := t.dataCallEnd(p); err != nil {
				return err
			}
		}
	}

	slot.data.packetHeaderValid = false
	slot.call.start = time.Now()
	slot.call.end = time.Time{}
	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]

	if t.state != voiceCallActive {
		return nil
	}

	slot.voice.streamID = 0
	t.state = idle
	t.debugf(p, "voice call ended")
	return nil
}

func (t *Terminal) voiceCallStart(p *dmr.Packet) error {
	slot := t.slot[p.Timeslot]

	if slot.dstID != p.DstID || slot.srcID != p.SrcID {
		if err := t.voiceCallEnd(p); err != nil {
			return err
		}
	}

	slot.voice.streamID = p.StreamID
	t.state = voiceCallActive

	t.debugf(p, "voice call started")
	return nil
}

func (t *Terminal) handlePacket(r dmr.Repeater, p *dmr.Packet) error {
	// Ignore packets not addressed to us or any of the talk groups we monitor
	if false && !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
	}

	var err error

	t.warningf(p, "handle packet: %s", dmr.DataTypeName[p.DataType])
	//log.Debug(hex.Dump(p.Data))

	//
	switch p.DataType {
	case dmr.CSBK:
		err = t.handleControlBlock(p)
		break
	case dmr.Data:
		err = t.handleData(p)
		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:
		err = t.handleVoiceLC(p)
		break
	case dmr.TerminatorWithLC:
		err = t.handleTerminatorWithLC(p)
		return nil
	default:
		t.warningf(p, "unhandled packet: %s", dmr.DataTypeName[p.DataType])
		log.Debug(hex.Dump(p.Data))
		return nil
	}

	if err != nil {
		t.errorf(p, "handle packet error: %v", err)
	}

	return err
}

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)
	)

	if err := bptc.Decode(bits, data); err != nil {
		return err
	}
	cb, err := dmr.ParseControlBlock(data)
	if err != nil {
		return err
	}

	t.debugf(p, cb.String())

	return nil
}

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)
	)

	if err := bptc.Decode(bits, data); err != nil {
		return err
	}

	h, err := dmr.ParseDataHeader(data, false)
	if err != nil {
		return err
	}

	slot.data.packetHeaderValid = false
	slot.data.blocksReceived = 0
	slot.selectiveAckRequestsSent = 0
	slot.rxSequence = 0

	t.debugf(p, h.String())
	switch d := h.Data.(type) {
	case *dmr.ShortDataDefinedData:
		if d.FullMessage {
			slot.fullMessageBlocks = int(d.AppendedBlocks)
			slot.data.blocks = make([]*dmr.DataBlock, slot.fullMessageBlocks)
			t.debugf(p, "expecting %d data block", slot.fullMessageBlocks)
		}
		slot.data.blocksExpected = int(d.AppendedBlocks)
		err = t.dataCallStart(p)
		break

	default:
		t.warningf(p, "unhandled data header %T", h.Data)
		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)
	)

	if err := trellis.Decode(bits, data); err != nil {
		return err
	}

	db, err := dmr.ParseDataBlock(data, dmr.Rate34Data, slot.data.header.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
	if err := t.callEnd(p); err != nil {
		return err
	}

	var (
		bits = p.InfoBits()
		data = make([]byte, 12)
	)
	if err := bptc.Decode(bits, data); err != nil {
		return err
	}

	// Applying CRC mask to the checksum. See DMR AI. spec. page 143.
	data[9] ^= 0x99
	data[10] ^= 0x99
	data[11] ^= 0x99

	lc, err := lc.ParseFullLC(data)
	if err != nil {
		return err
	}

	t.debugf(p, "terminator with lc: %s", lc.String())

	return nil
}

func (t *Terminal) handleVoice(p *dmr.Packet) error {
	slot := t.slot[p.Timeslot]
	slot.last.packetReceived = time.Now()

	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
	}

	// Check sync frame
	sync := p.SyncBits()
	patt := dmr.SyncPattern(sync)
	if patt != dmr.SyncPatternUnknown {
		t.debugf(p, "sync pattern %s", dmr.SyncPatternName[patt])
	} else {
		// Not a sync frame, sync field should contain EMB
		bits, err := dmr.ParseEMBBitsFromSync(sync)
		if err != nil {
			return err
		}
		emb, err := dmr.ParseEMB(bits)
		if err != nil {
			return err
		}
		t.debugf(p, "embedded signalling %s", emb.String())

		// Handling embedded signalling LC
		switch emb.LCSS {
		case dmr.SingleFragment:
			return nil // FIXME(pd0mz): unhandled
		case dmr.FirstFragment:
			slot.embeddedSignalling.Clear()
			break
		}

		if emb.LCSS == dmr.FirstFragment || emb.LCSS == dmr.Continuation || emb.LCSS == dmr.LastFragment {
			frag, err := dmr.ParseEmbeddedSignallingLCFromSyncBits(sync)
			if err != nil {
				return err
			}
			if err := slot.embeddedSignalling.AddBurst(frag); err != nil {
				return err
			}
		}

		if emb.LCSS == dmr.LastFragment {
			if err := slot.embeddedSignalling.CheckAndRepair(); err != nil {
				return err
			}

			var signalling = make([]byte, 77)
			if err := slot.embeddedSignalling.GetData(signalling); err != nil {
				return err
			}
			eslc, err := dmr.DeinterleaveEmbeddedSignallingLC(signalling)
			if err != nil {
				return err
			}
			if !eslc.Check() {
				return errors.New("embedded signalling LC checksum failed")
			}

			lc, err := lc.ParseLC(dmr.BitsToBytes(eslc.Bits))
			if err != nil {
				return err
			}
			t.debugf(p, "voice embedded lc: %s", lc.String())
		}
	}

	if t.vff != nil {
		t.vff(p, p.VoiceBits())
		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
}

func (t *Terminal) handleVoiceLC(p *dmr.Packet) error {
	var (
		bits = p.InfoBits()
		data = make([]byte, 12)
	)
	if err := bptc.Decode(bits, data); err != nil {
		return err
	}

	// Applying CRC mask to the checksum. See DMR AI. spec. page 143.
	data[9] ^= 0x96
	data[10] ^= 0x96
	data[11] ^= 0x96

	lc, err := lc.ParseFullLC(data)
	if err != nil {
		return err
	}

	t.debugf(p, "voice header lc: %s", lc.String())

	return nil
}