rssp
This commit is contained in:
xzb
parent
31a394aecc
commit
2d0d98fe22
161
third_party/message/rssp.go
vendored
161
third_party/message/rssp.go
vendored
@ -19,10 +19,6 @@ type RsspHead struct {
|
||||
Da uint16
|
||||
}
|
||||
|
||||
const (
|
||||
RsspCrc16GX uint32 = 0b1_0000_1000_0001_0001 //生成多项式 G(X)=X16+X11+X4+1
|
||||
)
|
||||
|
||||
func (h *RsspHead) Type() RsspType {
|
||||
return h.Mc
|
||||
}
|
||||
@ -89,7 +85,7 @@ func (r *RsspRsd) Encode() []byte {
|
||||
//用户数据包
|
||||
data = append(data, r.Sad...)
|
||||
//报文尾-CRC16
|
||||
r.Crc16 = uint16(NewCrc(uint64(RsspCrc16GX), 17, data).Generate())
|
||||
r.Crc16 = RsspCrc16(data)
|
||||
data = binary.LittleEndian.AppendUint16(data, r.Crc16)
|
||||
//
|
||||
return data
|
||||
@ -141,7 +137,7 @@ func (r *RsspSse) Encode() []byte {
|
||||
data = binary.LittleEndian.AppendUint32(data, r.SeqEnq1)
|
||||
data = binary.LittleEndian.AppendUint32(data, r.SeqEnq2)
|
||||
//报文尾-CRC16
|
||||
r.Crc16 = uint16(NewCrc(uint64(RsspCrc16GX), 17, data).Generate())
|
||||
r.Crc16 = RsspCrc16(data)
|
||||
data = binary.LittleEndian.AppendUint16(data, r.Crc16)
|
||||
return data
|
||||
}
|
||||
@ -192,7 +188,7 @@ func (r *RsspSsr) Encode() []byte {
|
||||
data = binary.LittleEndian.AppendUint32(data, r.Tic2)
|
||||
data = append(data, r.Dvn)
|
||||
//报文尾-CRC16
|
||||
r.Crc16 = uint16(NewCrc(uint64(RsspCrc16GX), 17, data).Generate())
|
||||
r.Crc16 = RsspCrc16(data)
|
||||
data = binary.LittleEndian.AppendUint16(data, r.Crc16)
|
||||
return data
|
||||
}
|
||||
@ -238,7 +234,7 @@ const (
|
||||
// ParseRsspPack 解析RSSP数据包
|
||||
func ParseRsspPack(pack []byte) (Rssper, error) {
|
||||
// pack 进行CRC16循环冗余校验，检测整个包的完整性
|
||||
gCrc16 := uint16(NewCrc(uint64(RsspCrc16GX), 17, pack[0:len(pack)-2]).Generate())
|
||||
gCrc16 := RsspCrc16(pack[0 : len(pack)-2])
|
||||
pCrc16 := binary.LittleEndian.Uint16(pack[len(pack)-2 : len(pack)])
|
||||
if gCrc16 != pCrc16 {
|
||||
return nil, fmt.Errorf("ParseRsspPack 整个数据包CRC16校验未通过")
|
||||
@ -262,152 +258,3 @@ func ParseRsspPack(pack []byte) (Rssper, error) {
|
||||
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
|
||||
}
|
||||
|
||||
191
third_party/message/rssp_crc.go
vendored
Normal file
191
third_party/message/rssp_crc.go
vendored
Normal file
@ -0,0 +1,191 @@
|
||||
package message
|
||||
|
||||
//rssp 协议中crc校验，查表法实现
|
||||
|
||||
// Crc16Table G(x)=X16+X11+X4+1，计算初始值为0
|
||||
var crc16Table []uint32 = nil
|
||||
var crc32C1Table []uint32 = nil
|
||||
var crc32C2Table []uint32 = nil
|
||||
|
||||
const (
|
||||
RsspCrc16GX uint32 = 0b1_0000_1000_0001_0001 //生成多项式 G(X)=X16+X11+X4+1
|
||||
RsspCrc32C1 uint32 = 0x100d4e63 //安全通道1 CRC32生成多项式
|
||||
RsspCrc32C2 uint32 = 0x8ce56011 //安全通道1 CRC32生成多项式
|
||||
)
|
||||
|
||||
func CreateRsspCrcTable() {
|
||||
if crc16Table == nil {
|
||||
var table = make([]uint32, 0, 256)
|
||||
for bt := 0x00; bt <= 0xff; bt++ {
|
||||
table = append(table, Lookup(uint32(bt), RsspCrc16GX, 16, false))
|
||||
}
|
||||
crc16Table = table
|
||||
}
|
||||
if crc32C1Table == nil {
|
||||
var table = make([]uint32, 0, 256)
|
||||
for bt := 0x00; bt <= 0xff; bt++ {
|
||||
table = append(table, Lookup(uint32(bt), RsspCrc32C1, 32, false))
|
||||
}
|
||||
crc32C1Table = table
|
||||
}
|
||||
if crc32C2Table == nil {
|
||||
var table = make([]uint32, 0, 256)
|
||||
for bt := 0x00; bt <= 0xff; bt++ {
|
||||
table = append(table, Lookup(uint32(bt), RsspCrc32C2, 32, false))
|
||||
}
|
||||
crc32C2Table = table
|
||||
}
|
||||
}
|
||||
func RsspCrc16(data []byte) uint16 {
|
||||
return uint16(CrcTableBased(data, 16, 0, false, false, 0, crc16Table))
|
||||
}
|
||||
func RsspC1Crc32(data []byte) uint32 {
|
||||
return CrcTableBased(data, 32, 0, false, false, 0, crc32C1Table)
|
||||
}
|
||||
func RsspC2Crc32(data []byte) uint32 {
|
||||
return CrcTableBased(data, 32, 0, false, false, 0, crc32C2Table)
|
||||
}
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// 反转(0b00001010->0b01010000)
|
||||
func reflect(data uint32, width int) uint32 {
|
||||
var register1 uint32 = 0
|
||||
var significant_mask uint32 = 0xffffffff >> (32 - width)
|
||||
var register_msb_mask uint32 = 1 << (width - 1)
|
||||
var register_lsb_mask uint32 = 1
|
||||
|
||||
for i := 0; i < width; i++ {
|
||||
need_or := (data>>i)®ister_lsb_mask == register_lsb_mask
|
||||
if need_or {
|
||||
register1 |= register_msb_mask >> i
|
||||
}
|
||||
}
|
||||
return register1 & significant_mask
|
||||
}
|
||||
|
||||
// Lookup 计算单个数值的crc
|
||||
func Lookup(data uint32,
|
||||
polynomial uint32,
|
||||
width int,
|
||||
input_reflected bool) uint32 {
|
||||
var register1 uint32 = 0
|
||||
var significant_mask uint32 = 0xffffffff >> (32 - width)
|
||||
var register_msb_mask uint32 = 1 << (width - 1)
|
||||
var register_lsb_mask uint32 = 1
|
||||
var byte_msb_mask uint32 = 0x80
|
||||
var byte_lsb_mask uint32 = 1
|
||||
|
||||
if input_reflected {
|
||||
polynomial = reflect(polynomial, width)
|
||||
}
|
||||
|
||||
for i := 0; i < 1+(width/8); i++ {
|
||||
var byteData uint32 = 0
|
||||
|
||||
if i < 1 {
|
||||
byteData = data
|
||||
}
|
||||
|
||||
for j := 0; j < 8; j++ {
|
||||
need_xor := false
|
||||
|
||||
if input_reflected {
|
||||
need_xor = (register1 & register_lsb_mask) == register_lsb_mask
|
||||
register1 >>= 1
|
||||
|
||||
need_or := (byteData & byte_lsb_mask) == byte_lsb_mask
|
||||
byteData >>= 1
|
||||
|
||||
if need_or {
|
||||
register1 |= register_msb_mask
|
||||
}
|
||||
} else {
|
||||
need_xor = (register1 & register_msb_mask) == register_msb_mask
|
||||
register1 <<= 1
|
||||
|
||||
need_or := (byteData & byte_msb_mask) == byte_msb_mask
|
||||
byteData <<= 1
|
||||
|
||||
if need_or {
|
||||
register1 |= register_lsb_mask
|
||||
}
|
||||
}
|
||||
|
||||
if need_xor {
|
||||
register1 ^= polynomial
|
||||
}
|
||||
}
|
||||
}
|
||||
return register1 & significant_mask
|
||||
}
|
||||
|
||||
// CrcTableBased 查表法计算字节数组的crc
|
||||
func CrcTableBased(
|
||||
data []byte,
|
||||
width int,
|
||||
initial_value uint32,
|
||||
input_reflected bool,
|
||||
result_reflected bool,
|
||||
final_xor_value uint32,
|
||||
table []uint32) uint32 {
|
||||
//
|
||||
length := len(data)
|
||||
var register1 uint32 = initial_value
|
||||
var significant_mask uint32 = 0xffffffff >> (32 - width)
|
||||
var register_lsb_mask uint32 = 0xff
|
||||
|
||||
if input_reflected {
|
||||
register1 = reflect(register1, width)
|
||||
}
|
||||
|
||||
for i := 0; i < length; i++ {
|
||||
var byteData = uint32(data[i])
|
||||
var shift_out uint32 = 0
|
||||
|
||||
if input_reflected {
|
||||
shift_out = register1 & register_lsb_mask
|
||||
register1 = (register1 >> 8) ^ table[shift_out^byteData]
|
||||
} else {
|
||||
shift_out = (register1 >> (width - 8)) & register_lsb_mask
|
||||
register1 = (register1 << 8) ^ table[shift_out^byteData]
|
||||
}
|
||||
}
|
||||
|
||||
if input_reflected != result_reflected {
|
||||
register1 = reflect(register1, width)
|
||||
}
|
||||
return (register1 ^ final_xor_value) & significant_mask
|
||||
}
|
||||
|
||||
// MSB优先(字节中高位bit即最左侧bit优先)
|
||||
func CrcBitOrientedMsbFirst(data []byte, polynomial uint32, width int) uint32 {
|
||||
var register1 uint32 = 0
|
||||
var significant_mask uint32 = 0xffffffff >> (32 - width)
|
||||
var register_msb_mask uint32 = 1 << (width - 1)
|
||||
var register_lsb_mask uint32 = 1
|
||||
var byte_msb_mask uint32 = 0x80
|
||||
length := len(data)
|
||||
|
||||
for i := 0; i < length+(width/8); i++ {
|
||||
var byteData uint32 = 0
|
||||
if i < length {
|
||||
byteData = uint32(data[i])
|
||||
}
|
||||
for j := 0; j < 8; j++ {
|
||||
need_xor := (register1 & register_msb_mask) == register_msb_mask
|
||||
register1 <<= 1
|
||||
|
||||
need_or := (byteData & byte_msb_mask) == byte_msb_mask
|
||||
byteData <<= 1
|
||||
|
||||
if need_or {
|
||||
register1 |= register_lsb_mask
|
||||
}
|
||||
if need_xor {
|
||||
register1 ^= polynomial
|
||||
}
|
||||
}
|
||||
}
|
||||
return register1 & significant_mask
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user