rts-sim-testing-service/third_party/message/rssp.go

package message

import (
	"encoding/binary"
	"fmt"
)

//RSSP-1 V1.0 铁路信号安全通信协议

// RsspHead rssp报文头
type RsspHead struct {
	//报文头-协议交互类别(1Byte)
	Pic byte
	//报文头-报文类别(1Byte)
	Mc byte
	//报文头-源地址(2Byte)
	Sa uint16
	//报文头-目地址(2Byte)
	Da uint16
}

func (h *RsspHead) Type() RsspType {
	return h.Mc
}
func (h *RsspHead) decode(buf []byte) {
	ri := 0
	//报文头
	h.Pic = buf[ri]
	ri++
	h.Mc = buf[ri]
	ri++
	h.Sa = binary.LittleEndian.Uint16(buf[ri : ri+2])
	ri += 2
	h.Da = binary.LittleEndian.Uint16(buf[ri : ri+2])
	ri += 2
}
func (h *RsspHead) encode() []byte {
	data := make([]byte, 0, 6)
	//报文头
	data = append(data, h.Pic)
	data = append(data, h.Mc)
	data = binary.LittleEndian.AppendUint16(data, h.Sa)
	data = binary.LittleEndian.AppendUint16(data, h.Da)
	//
	return data
}

/////////////////////////////////////////////////

// RsspRsd 实时安全数据包
type RsspRsd struct {
	RsspHead
	//安全校验域-序列号(4Byte)
	Sn uint32
	//安全校验域-安全数据长度(2Byte)
	Sdl uint16
	//安全校验域-安全校验通道1(4Byte)
	Svc1 uint32
	//安全校验域-安全校验通道2(4Byte)
	Svc2 uint32
	//用户数据包-安全应用数据(总字节数480)
	Sad []byte
	//报文尾-CRC16(2Byte)
	Crc16 uint16
}

func (r *RsspRsd) Encode() []byte {
	data := make([]byte, 0, 6+14+len(r.Sad)+2)
	//报文头
	data = append(data, r.RsspHead.encode()...)
	//安全校验域
	data = binary.LittleEndian.AppendUint32(data, r.Sn)
	data = binary.LittleEndian.AppendUint16(data, r.Sdl)
	data = binary.LittleEndian.AppendUint32(data, r.Svc1)
	data = binary.LittleEndian.AppendUint32(data, r.Svc2)
	//用户数据包
	data = append(data, r.Sad...)
	//报文尾-CRC16
	r.Crc16 = uint16(NewCrc(uint64(RsspCrc16GX), 17, data).Generate())
	data = binary.LittleEndian.AppendUint16(data, r.Crc16)
	//
	return data
}
func (r *RsspRsd) Decode(buf []byte) error {
	//报文头
	r.RsspHead.decode(buf)
	ri := 6
	//安全校验域
	r.Sn = binary.LittleEndian.Uint32(buf[ri : ri+4])
	ri += 4
	r.Sdl = binary.LittleEndian.Uint16(buf[ri : ri+2])
	ri += 2
	r.Svc1 = binary.LittleEndian.Uint32(buf[ri : ri+4])
	ri += 4
	r.Svc2 = binary.LittleEndian.Uint32(buf[ri : ri+4])
	ri += 4
	//用户数据
	sadLen := int(r.Sdl) - 8
	r.Sad = buf[ri : ri+sadLen]
	ri += sadLen
	//报文尾
	r.Crc16 = binary.LittleEndian.Uint16(buf[ri : ri+2])
	//
	return nil
}

///////////////////////////////////////////////////////////////

// RsspSse 时序校正请求包
type RsspSse struct {
	RsspHead
	//安全校验域-序列号(4Byte)
	Sn uint32
	//安全校验域-时序校正请求通道1(4Byte)
	SeqEnq1 uint32
	//安全校验域-时序校正请求通道2(4Byte)
	SeqEnq2 uint32
	//报文尾-CRC16(2Byte)
	Crc16 uint16
}

func (r *RsspSse) Encode() []byte {
	data := make([]byte, 0, 20)
	//报文头
	data = append(data, r.RsspHead.encode()...)
	//安全校验域
	data = binary.LittleEndian.AppendUint32(data, r.Sn)
	data = binary.LittleEndian.AppendUint32(data, r.SeqEnq1)
	data = binary.LittleEndian.AppendUint32(data, r.SeqEnq2)
	//报文尾-CRC16
	r.Crc16 = uint16(NewCrc(uint64(RsspCrc16GX), 17, data).Generate())
	data = binary.LittleEndian.AppendUint16(data, r.Crc16)
	return data
}
func (r *RsspSse) Decode(buf []byte) error {
	//报文头
	r.RsspHead.decode(buf)
	//安全校验域
	ri := 6
	r.Sn = binary.LittleEndian.Uint32(buf[ri : ri+4])
	ri += 4
	r.SeqEnq1 = binary.LittleEndian.Uint32(buf[ri : ri+4])
	ri += 4
	r.SeqEnq2 = binary.LittleEndian.Uint32(buf[ri : ri+4])
	ri += 4
	//报文尾
	r.Crc16 = binary.LittleEndian.Uint16(buf[ri : ri+2])
	//
	return nil
}

/////////////////////////////////////////////////////////

// RsspSsr 时序校正应答包，用于回应SSE
type RsspSsr struct {
	RsspHead
	//安全校验域-应答方的序列号(4Byte)
	SrSn uint32
	//安全校验域-请求方的序列号(4Byte)
	SeSn uint32
	//安全校验域-时序初始化通道1(4Byte)
	Tic1 uint32
	//安全校验域-时序初始化通道2(4Byte)
	Tic2 uint32
	//安全校验域-数据版本号(1Byte)
	Dvn byte
	//报文尾-CRC16(2Byte)
	Crc16 uint16
}

func (r *RsspSsr) Encode() []byte {
	data := make([]byte, 0, 25)
	//报文头
	data = append(data, r.RsspHead.encode()...)
	//安全校验域
	data = binary.LittleEndian.AppendUint32(data, r.SrSn)
	data = binary.LittleEndian.AppendUint32(data, r.SeSn)
	data = binary.LittleEndian.AppendUint32(data, r.Tic1)
	data = binary.LittleEndian.AppendUint32(data, r.Tic2)
	data = append(data, r.Dvn)
	//报文尾-CRC16
	r.Crc16 = uint16(NewCrc(uint64(RsspCrc16GX), 17, data).Generate())
	data = binary.LittleEndian.AppendUint16(data, r.Crc16)
	return data
}
func (r *RsspSsr) Decode(buf []byte) error {
	//报文头
	r.RsspHead.decode(buf)
	//安全校验域
	ri := 6
	r.SrSn = binary.LittleEndian.Uint32(buf[ri : ri+4])
	ri += 4
	r.SeSn = binary.LittleEndian.Uint32(buf[ri : ri+4])
	ri += 4
	r.Tic1 = binary.LittleEndian.Uint32(buf[ri : ri+4])
	ri += 4
	r.Tic2 = binary.LittleEndian.Uint32(buf[ri : ri+4])
	ri += 4
	r.Dvn = buf[ri]
	ri += 1
	//报文尾
	r.Crc16 = binary.LittleEndian.Uint16(buf[ri : ri+2])
	//
	return nil
}

//////////////////////////////////////////////////////////////

type RsspCodec interface {
	Encode() []byte
	Decode(buf []byte) error
}
type Rssper interface {
	Type() RsspType
}
type RsspType = byte

const (
	RSD_A = RsspType(0x80)
	RSD_B = RsspType(0x81)
	SSE   = RsspType(0x90)
	SSR   = RsspType(0x91)
)

// ParseRsspPack 解析RSSP数据包
func ParseRsspPack(pack []byte) (Rssper, error) {
	// pack 进行CRC16循环冗余校验，检测整个包的完整性
	gCrc16 := uint16(NewCrc(uint64(RsspCrc16GX), 17, pack[0:len(pack)-2]).Generate())
	pCrc16 := binary.LittleEndian.Uint16(pack[len(pack)-2 : len(pack)])
	if gCrc16 != pCrc16 {
		return nil, fmt.Errorf("ParseRsspPack 整个数据包CRC16校验未通过")
	}
	//
	ph := &RsspHead{}
	ph.decode(pack)
	//
	var codec RsspCodec
	switch ph.Mc {
	case RSD_A | RSD_B:
		codec = &RsspRsd{}
	case SSE:
		codec = &RsspSse{}
	case SSR:
		codec = &RsspSsr{}
	default:
		return nil, fmt.Errorf("ParseRsspPack 无法识别的报文类型码[0x%x]", ph.Mc)
	}
	//
	e := codec.Decode(pack)
	return codec.(Rssper), e
}

// //////////////////CRC循环冗余校验--移位寄存器///////////////////////////

const (
	RsspCrc16GX uint32 = 0b1_0000_1000_0001_0001 //生成多项式 G(X)=X16+X11+X4+1
)

type crc struct {
	//生成多项式，即二进制位数，如生成多项式X4+X3+1对应二进制11001共5位，生成的校验码长度为4
	g uint64
	//生成多项式二进制长度
	gl int
	//移位寄存器
	reg *sReg
	//消息数据
	m *crcBitPipe
	//补零
	b *crcBitPipe
}

// NewCrc CRC循环冗余校验
//
// g : 生成多项式
// gl : 生成多项式的长度即二进制位数,gl值为8的倍数加1
// m : 被校验的消息数据
func NewCrc(g uint64, gl int, m []byte) *crc {

	return &crc{g: g, gl: gl, m: NewCrcBitPipe(m), b: NewCrcBit0Pipe(gl - 1), reg: NewReg(gl)}
}
func (c *crc) canToReg() bool {
	if c.m.HasFlowBit() {
		return true
	}
	if c.b.HasFlowBit() {
		return true
	}
	return false
}

// 被校验数据尽可能入移位寄存器
func (c *crc) mToReg() {
	for c.reg.Glb() <= 0 { //寄存器左侧有0位
		if c.m.HasFlowBit() {
			c.reg.Ifr(c.m.FlowBit())
		} else if c.b.HasFlowBit() {
			c.reg.Ifr(c.b.FlowBit())
		} else {
			return
		}
	}
}

// Generate 生成CRC效验码
func (c *crc) Generate() uint64 {
	for c.canToReg() {
		c.mToReg()
		if c.reg.Glb() >= 1 {
			c.reg.Xor(c.g)
		} else {
			break
		}
	}
	return c.reg.RegV()
}

// 把字节数组包装成bit流,bit从byte左侧流出
type crcBitPipe struct {
	buf []byte
	bi  int //当前流出的字节在buf中的下标,[0,len(buf)-1]
	i   int //在当前流出的字节中，当前可流出的bit在字节中的位置，[7,0]
}

func NewCrcBitPipe(buf []byte) *crcBitPipe {
	return &crcBitPipe{buf: buf, bi: 0, i: 7}
}
func NewCrcBit0Pipe(n int) *crcBitPipe {
	y := n % 8
	z := n / 8
	yy := 0
	if y > 0 {
		yy = 1
	}
	cap := z + yy
	buf := make([]byte, 0, cap)
	for i := 0; i < cap; i++ {
		buf = append(buf, 0x00)
	}
	si := 7
	if y > 0 {
		si = y - 1
	}
	return &crcBitPipe{buf: buf, bi: 0, i: si}
}

// FlowBit 从左侧流出一个bit
// 正常返回值为0或1，流结束返回大于1的值
func (p *crcBitPipe) FlowBit() byte {
	if p.HasFlowBit() {
		rt := 0x01 & (p.buf[p.bi] >> p.i)
		p.i--
		if p.i < 0 {
			p.bi++
			p.i = 7
		}
		return rt
	}
	return 2
}

func (p *crcBitPipe) HasFlowBit() bool {
	return p.bi < len(p.buf) && p.i >= 0
}
func (p *crcBitPipe) Reset() *crcBitPipe {
	p.bi = 0
	p.i = 7
	return p
}

// 移位寄存器，最长64位
type sReg struct {
	m uint64 //寄存器存储
	l int    //寄存器长度
}

func NewReg(l int) *sReg {
	return &sReg{m: 0, l: l}
}

// Glb 寄存器最左侧bit位值
func (r *sReg) Glb() byte {
	return byte(0x01 & (r.m >> (r.l - 1)))
}

// Ifr 从寄存器右侧移入一个bit
func (r *sReg) Ifr(bit byte) *sReg {
	r.m = (r.m << 1) | uint64(bit)
	return r
}

// And 寄存器的值与v位与操作，结果存入寄存器
func (r *sReg) And(v uint64) *sReg {
	r.m = r.m & v
	return r
}
func (r *sReg) Xor(v uint64) *sReg {
	r.m = r.m ^ v
	return r
}

// RegV 获取寄存器中的值
func (r *sReg) RegV() uint64 {
	return r.m
}