414 lines
9.5 KiB
Go
414 lines
9.5 KiB
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
|
||
}
|
||
|
||
const (
|
||
RsspCrc16GX uint32 = 0b1_0000_1000_0001_0001 //生成多项式 G(X)=X16+X11+X4+1
|
||
)
|
||
|
||
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
|
||
}
|
||
|
||
const (
|
||
CRC32_G_C1 = (uint64(0x01) << 32) | uint64(0x100d4e63) //crc32生成多项式
|
||
CRC32_G_C2 = (uint64(0x01) << 32) | uint64(0x8ce56011) //crc32生成多项式
|
||
SCW_C1 = uint32(0xae390b5a) //SCW常
|
||
SCW_C2 = uint32(0xc103589c) //SCW常
|
||
SJC_C1 = uint32(0x0fc22f87) //时间戳生成多项式
|
||
SJC_C2 = uint32(0xc3e887e1) //时间戳生成多项式
|
||
)
|
||
|
||
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循环冗余校验--移位寄存器///////////////////////////
|
||
|
||
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
|
||
}
|