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