Checkpoint
Wijnand Modderman-Lenstra
9 years ago
parent
caa595110d
commit
cca6817956
@ -0,0 +1,206 @@
|
|||||||
|
package dmr
|
||||||
|
|
||||||
|
import (
|
||||||
|
"errors"
|
||||||
|
"fmt"
|
||||||
|
)
|
||||||
|
|
||||||
|
// Control Block Options
|
||||||
|
const (
|
||||||
|
CBSKOOutboundActivation = B00111000
|
||||||
|
CBSKOUnitToUnitVoiceServiceRequest = B00000100
|
||||||
|
CBSKOUnitToUnitVoiceServiceAnswerResponse = B00000101
|
||||||
|
CBSKONegativeAcknowledgeResponse = B00100100
|
||||||
|
CBSKOPreamble = B00111101
|
||||||
|
)
|
||||||
|
|
||||||
|
type ControlBlock struct {
|
||||||
|
Last bool
|
||||||
|
CBSKO uint8
|
||||||
|
SrcID, DstID uint32
|
||||||
|
Data ControlBlockData
|
||||||
|
}
|
||||||
|
|
||||||
|
type ControlBlockData interface {
|
||||||
|
Write([]byte) error
|
||||||
|
Parse([]byte) error
|
||||||
|
}
|
||||||
|
|
||||||
|
type OutboundActivation struct{}
|
||||||
|
|
||||||
|
func (d *OutboundActivation) Parse(data []byte) error {
|
||||||
|
if len(data) != InfoSize {
|
||||||
|
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
|
||||||
|
}
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
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
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
type UnitToUnitVoiceServiceRequest struct {
|
||||||
|
Options uint8
|
||||||
|
}
|
||||||
|
|
||||||
|
func (d *UnitToUnitVoiceServiceRequest) Parse(data []byte) error {
|
||||||
|
if len(data) != InfoSize {
|
||||||
|
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
|
||||||
|
}
|
||||||
|
d.Options = data[2]
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
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[2] = d.Options
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
var _ (ControlBlockData) = (*UnitToUnitVoiceServiceRequest)(nil)
|
||||||
|
|
||||||
|
type UnitToUnitVoiceServiceAnswerResponse struct {
|
||||||
|
Options uint8
|
||||||
|
Response uint8
|
||||||
|
}
|
||||||
|
|
||||||
|
func (d *UnitToUnitVoiceServiceAnswerResponse) Parse(data []byte) error {
|
||||||
|
if len(data) != InfoSize {
|
||||||
|
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
|
||||||
|
}
|
||||||
|
d.Options = data[2]
|
||||||
|
d.Response = data[3]
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
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[2] = d.Options
|
||||||
|
data[3] = d.Response
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
var _ (ControlBlockData) = (*UnitToUnitVoiceServiceAnswerResponse)(nil)
|
||||||
|
|
||||||
|
type NegativeAcknowledgeResponse struct {
|
||||||
|
SourceType bool
|
||||||
|
ServiceType uint8
|
||||||
|
Reason uint8
|
||||||
|
}
|
||||||
|
|
||||||
|
func (d *NegativeAcknowledgeResponse) Parse(data []byte) error {
|
||||||
|
if len(data) != InfoSize {
|
||||||
|
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
|
||||||
|
}
|
||||||
|
d.SourceType = (data[2] & B01000000) > 0
|
||||||
|
d.ServiceType = (data[2] & B00011111)
|
||||||
|
d.Reason = data[3]
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
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[2] = d.ServiceType
|
||||||
|
if d.SourceType {
|
||||||
|
data[2] |= B01000000
|
||||||
|
}
|
||||||
|
data[3] = d.Reason
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
var _ (ControlBlockData) = (*NegativeAcknowledgeResponse)(nil)
|
||||||
|
|
||||||
|
type ControlBlockPreamble struct {
|
||||||
|
DataFollows bool
|
||||||
|
DstIsGroup bool
|
||||||
|
Blocks uint8
|
||||||
|
}
|
||||||
|
|
||||||
|
func (d *ControlBlockPreamble) Parse(data []byte) error {
|
||||||
|
if len(data) != InfoSize {
|
||||||
|
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
|
||||||
|
}
|
||||||
|
d.DataFollows = (data[2] & B10000000) > 0
|
||||||
|
d.DstIsGroup = (data[2] & B01000000) > 0
|
||||||
|
d.Blocks = data[3]
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
func (d *ControlBlockPreamble) Write(data []byte) error {
|
||||||
|
if len(data) != InfoSize {
|
||||||
|
return fmt.Errorf("dmr: expected %d info bytes, got %d", InfoSize, len(data))
|
||||||
|
}
|
||||||
|
data[0] |= B00100110
|
||||||
|
if d.DataFollows {
|
||||||
|
data[2] |= B10000000
|
||||||
|
}
|
||||||
|
if d.DstIsGroup {
|
||||||
|
data[2] |= B01000000
|
||||||
|
}
|
||||||
|
data[3] = d.Blocks
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
var _ (ControlBlockData) = (*ControlBlockPreamble)(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))
|
||||||
|
}
|
||||||
|
|
||||||
|
// Calculate CRC16
|
||||||
|
|
||||||
|
// Check packet
|
||||||
|
if data[0]&B01000000 > 0 {
|
||||||
|
return nil, errors.New("dmr: CBSK protect flag is set")
|
||||||
|
}
|
||||||
|
if data[1] != 0 {
|
||||||
|
return nil, errors.New("dmr: CBSK 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]),
|
||||||
|
}
|
||||||
|
|
||||||
|
switch cb.CBSKO {
|
||||||
|
case CBSKOOutboundActivation:
|
||||||
|
cb.Data = &OutboundActivation{}
|
||||||
|
break
|
||||||
|
case CBSKOUnitToUnitVoiceServiceRequest:
|
||||||
|
cb.Data = &UnitToUnitVoiceServiceRequest{}
|
||||||
|
break
|
||||||
|
case CBSKOUnitToUnitVoiceServiceAnswerResponse:
|
||||||
|
cb.Data = &UnitToUnitVoiceServiceAnswerResponse{}
|
||||||
|
break
|
||||||
|
case CBSKONegativeAcknowledgeResponse:
|
||||||
|
cb.Data = &NegativeAcknowledgeResponse{}
|
||||||
|
break
|
||||||
|
case CBSKOPreamble:
|
||||||
|
cb.Data = &ControlBlockPreamble{}
|
||||||
|
break
|
||||||
|
default:
|
||||||
|
return nil, fmt.Errorf("dmr: unknown CBSKO %#02x (%#06b)", cb.CBSKO, cb.CBSKO)
|
||||||
|
}
|
||||||
|
|
||||||
|
if err := cb.Data.Parse(data); err != nil {
|
||||||
|
return nil, err
|
||||||
|
}
|
||||||
|
|
||||||
|
return cb, nil
|
||||||
|
}
|
||||||
@ -0,0 +1,9 @@
|
|||||||
|
package dmr
|
||||||
|
|
||||||
|
type DataBlock struct {
|
||||||
|
Serial uint8
|
||||||
|
CRC uint16
|
||||||
|
OK bool
|
||||||
|
Data [24]byte
|
||||||
|
Length uint8
|
||||||
|
}
|
||||||
@ -0,0 +1,214 @@
|
|||||||
|
package terminal
|
||||||
|
|
||||||
|
import (
|
||||||
|
"encoding/hex"
|
||||||
|
"fmt"
|
||||||
|
"time"
|
||||||
|
|
||||||
|
"github.com/op/go-logging"
|
||||||
|
"github.com/pd0mz/go-dmr"
|
||||||
|
"github.com/pd0mz/go-dmr/bptc"
|
||||||
|
"github.com/pd0mz/go-dmr/trellis"
|
||||||
|
)
|
||||||
|
|
||||||
|
var log = logging.MustGetLogger("dmr/terminal")
|
||||||
|
|
||||||
|
const (
|
||||||
|
idle uint8 = iota
|
||||||
|
dataCallActive
|
||||||
|
voideCallActive
|
||||||
|
)
|
||||||
|
|
||||||
|
type Slot struct {
|
||||||
|
call struct {
|
||||||
|
start time.Time
|
||||||
|
end time.Time
|
||||||
|
}
|
||||||
|
dstID, srcID uint32
|
||||||
|
dataType uint8
|
||||||
|
data struct {
|
||||||
|
packetHeaderValid bool
|
||||||
|
blocks [64]dmr.DataBlock
|
||||||
|
blocksExpected uint8
|
||||||
|
blocksReceived uint8
|
||||||
|
}
|
||||||
|
selectiveAckRequestsSent int
|
||||||
|
rxSequence int
|
||||||
|
fullMessageBlocks uint8
|
||||||
|
last struct {
|
||||||
|
packetReceived time.Time
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
func NewSlot() Slot {
|
||||||
|
return Slot{}
|
||||||
|
}
|
||||||
|
|
||||||
|
type Terminal struct {
|
||||||
|
ID uint32
|
||||||
|
Call string
|
||||||
|
Repeater dmr.Repeater
|
||||||
|
slot []Slot
|
||||||
|
state uint8
|
||||||
|
}
|
||||||
|
|
||||||
|
func New(id uint32, call string, r dmr.Repeater) *Terminal {
|
||||||
|
t := &Terminal{
|
||||||
|
ID: id,
|
||||||
|
Call: call,
|
||||||
|
Repeater: r,
|
||||||
|
slot: []Slot{NewSlot(), NewSlot()},
|
||||||
|
}
|
||||||
|
r.SetPacketFunc(t.handlePacket)
|
||||||
|
return t
|
||||||
|
}
|
||||||
|
|
||||||
|
func (t *Terminal) dataCallEnd(p *dmr.Packet) error {
|
||||||
|
slot := t.slot[p.Timeslot]
|
||||||
|
|
||||||
|
if t.state != dataCallActive {
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
log.Debugf("[%d->%d] data call ended", slot.srcID, slot.dstID)
|
||||||
|
|
||||||
|
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 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
|
||||||
|
|
||||||
|
log.Debugf("[%d->%d] data call started", slot.srcID, slot.dstID)
|
||||||
|
|
||||||
|
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
|
||||||
|
}
|
||||||
|
|
||||||
|
switch p.DataType {
|
||||||
|
case dmr.VoiceBurstA, dmr.VoiceBurstB, dmr.VoiceBurstC, dmr.VoiceBurstD, dmr.VoiceBurstE, dmr.VoiceBurstF:
|
||||||
|
return nil
|
||||||
|
case dmr.CBSK:
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
log.Debugf("[%d->%d] (%s, %#04b): sent to me", p.SrcID, p.DstID, dmr.DataTypeName[p.DataType], p.DataType)
|
||||||
|
|
||||||
|
switch p.DataType {
|
||||||
|
case dmr.CBSK:
|
||||||
|
err = t.handleControlBlock(p)
|
||||||
|
break
|
||||||
|
case dmr.Data:
|
||||||
|
err = t.handleData(p)
|
||||||
|
break
|
||||||
|
case dmr.Rate34Data:
|
||||||
|
err = t.handleRate34Data(p)
|
||||||
|
default:
|
||||||
|
log.Debug(hex.Dump(p.Data))
|
||||||
|
}
|
||||||
|
|
||||||
|
if err != nil {
|
||||||
|
log.Errorf("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()
|
||||||
|
|
||||||
|
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
|
||||||
|
}
|
||||||
|
|
||||||
|
log.Debugf("[%d->%d] control block %T", cb.SrcID, cb.DstID, cb.Data)
|
||||||
|
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
func (t *Terminal) handleData(p *dmr.Packet) error {
|
||||||
|
slot := t.slot[p.Timeslot]
|
||||||
|
slot.last.packetReceived = time.Now()
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
c := h.CommonHeader()
|
||||||
|
log.Debugf("[%d->%d] data header %T", c.SrcID, c.DstID, 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.data.blocksExpected = ht.AppendedBlocks
|
||||||
|
return t.dataCallStart(p)
|
||||||
|
default:
|
||||||
|
log.Warningf("unhandled data header %T", h)
|
||||||
|
}
|
||||||
|
|
||||||
|
return nil
|
||||||
|
}
|
||||||
|
|
||||||
|
func (t *Terminal) handleRate34Data(p *dmr.Packet) error {
|
||||||
|
slot := t.slot[p.Timeslot]
|
||||||
|
slot.last.packetReceived = time.Now()
|
||||||
|
|
||||||
|
var (
|
||||||
|
bits = p.InfoBits()
|
||||||
|
data = make([]byte, 18)
|
||||||
|
)
|
||||||
|
|
||||||
|
if err := trellis.Decode(bits, data); err != nil {
|
||||||
|
return err
|
||||||
|
}
|
||||||
|
fmt.Print(hex.Dump(data))
|
||||||
|
|
||||||
|
return nil
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue