324 lines
8.7 KiB
Go
324 lines
8.7 KiB
Go
package message
|
||
|
||
import "github.com/snksoft/crc"
|
||
|
||
//rssp 协议中crc校验，查表法实现
|
||
|
||
// // Crc16Table G(x)=X16+X11+X4+1，计算初始值为0
|
||
// var crc16Table []uint32 = nil
|
||
// var crc32C1Table []uint32 = nil
|
||
// var crc32C2Table []uint32 = nil
|
||
|
||
const (
|
||
// SCW常量
|
||
RSSP_I_C1_SCW uint32 = 0xae390b5a
|
||
RSSP_I_C2_SCW uint32 = 0xc103589c
|
||
//时间戳生成多项式
|
||
RSSP_I_C1_TS uint32 = 0x0fc22f87
|
||
RSSP_I_C2_TS uint32 = 0xc3e887e1
|
||
)
|
||
|
||
var (
|
||
// crc16多项式为G(x)=X16+X11+X4+1
|
||
RSSP_I_CRC16 = &crc.Parameters{Width: 16, Polynomial: 0x0811, Init: 0x0, ReflectIn: false, ReflectOut: false, FinalXor: 0x0}
|
||
// 通道1 crc32多项式为0x100d4e63
|
||
RSSP_I_C1_CRC32 = &crc.Parameters{Width: 32, Polynomial: 0x100d4e63, Init: 0x0, ReflectIn: false, ReflectOut: false, FinalXor: 0x0}
|
||
// 通道2 crc32多项式为0x8ce56011
|
||
RSSP_I_C2_CRC32 = &crc.Parameters{Width: 32, Polynomial: 0x8ce56011, Init: 0x0, ReflectIn: false, ReflectOut: false, FinalXor: 0x0}
|
||
)
|
||
|
||
// Rssp_I_Crc16计算
|
||
func Rssp_I_Crc16(data []byte) uint16 {
|
||
return uint16(crc.CalculateCRC(RSSP_I_CRC16, data))
|
||
}
|
||
|
||
// 通道1的crc32
|
||
func Rssp_I_Crc32C1(data []byte) uint32 {
|
||
return uint32(crc.CalculateCRC(RSSP_I_C1_CRC32, data))
|
||
}
|
||
|
||
// 通道2的crc32
|
||
func Rssp_I_Crc32C2(data []byte) uint32 {
|
||
return uint32(crc.CalculateCRC(RSSP_I_C2_CRC32, data))
|
||
}
|
||
|
||
// const ( //CRC生成多项式
|
||
// RsspCrc16GX uint32 = 0b1_0000_1000_0001_0001 //生成多项式 G(X)=X16+X11+X4+1
|
||
// RsspCrc32C1 uint32 = 0x100d4e63 //安全通道1 CRC32生成多项式
|
||
// RsspCrc32C2 uint32 = 0x8ce56011 //安全通道1 CRC32生成多项式
|
||
// )
|
||
|
||
// // InitRsspCrcTable 初始化RSSP协议中需要的CRC表
|
||
// func InitRsspCrcTable() {
|
||
// if crc16Table == nil {
|
||
// crc16Table = CreateCrcTable(RsspCrc16GX, 16, false)
|
||
// }
|
||
// if crc32C1Table == nil {
|
||
// crc32C1Table = CreateCrcTable(RsspCrc32C1, 32, false)
|
||
// }
|
||
// if crc32C2Table == nil {
|
||
// crc32C2Table = CreateCrcTable(RsspCrc32C2, 32, false)
|
||
// }
|
||
// }
|
||
// 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)
|
||
// }
|
||
|
||
// // CreateCrcTable 创建CRC表，支持8、16、32位
|
||
// func CreateCrcTable(polynomial uint32, width int, input_reflected bool) []uint32 {
|
||
// var table = make([]uint32, 0, 256)
|
||
// for bt := 0x00; bt <= 0xff; bt++ {
|
||
// table = append(table, lookup(uint32(bt), polynomial, width, input_reflected))
|
||
// }
|
||
// return table
|
||
// }
|
||
|
||
/////////////////////////////////////////////////////////////////////////////
|
||
|
||
// // 反转(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
|
||
// }
|
||
|
||
// // 计算单个数值的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
|
||
// }
|
||
|
||
// func Crc(data []byte,
|
||
// polynomial uint32,
|
||
// width int,
|
||
// initial_value uint32,
|
||
// input_reflected bool,
|
||
// result_reflected bool,
|
||
// final_xor_value 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 = uint32(data[i])
|
||
// var shift_out uint32 = 0
|
||
// var value uint32 = 0
|
||
|
||
// if input_reflected {
|
||
// shift_out = register1 & register_lsb_mask
|
||
// value = lookup(shift_out^byteData, polynomial, width, input_reflected)
|
||
// register1 = (register1 >> 8) ^ value
|
||
// } else {
|
||
// shift_out = (register1 >> (width - 8)) & register_lsb_mask
|
||
// value = lookup(shift_out^byteData, polynomial, width, input_reflected)
|
||
// register1 = (register1 << 8) ^ value
|
||
// }
|
||
// }
|
||
|
||
// if input_reflected != result_reflected {
|
||
// register1 = reflect(register1, width)
|
||
// }
|
||
// return (register1 ^ final_xor_value) & significant_mask
|
||
// }
|
||
|
||
//////////////////////////LFSR//////////////////////////////
|
||
|
||
// 线性反馈移位寄存器
|
||
type RsspLFSR struct {
|
||
polynomial uint32 //生成多项式
|
||
width int //寄存器宽度
|
||
register uint32 //寄存器
|
||
initValue uint32 //初始值
|
||
rightShift bool //true-右移，false-左移
|
||
}
|
||
|
||
func NewRsspLFSR(polynomial uint32, width int, init_value uint32, rightShift bool) *RsspLFSR {
|
||
var wd_msb_mask uint32 = 1 << (width - 1)
|
||
var wd_mask uint32 = 0xffffffff >> (32 - width)
|
||
init_value &= wd_mask
|
||
polynomial &= wd_mask
|
||
polynomial = (polynomial >> 1) | wd_msb_mask
|
||
//
|
||
return &RsspLFSR{polynomial: polynomial, width: width, register: init_value, initValue: init_value, rightShift: rightShift}
|
||
}
|
||
func (r *RsspLFSR) move() {
|
||
r.move0(r.rightShift)
|
||
}
|
||
func (r *RsspLFSR) MoveRight() *RsspLFSR {
|
||
r.move0(true)
|
||
return r
|
||
}
|
||
func (r *RsspLFSR) MoveLeft() *RsspLFSR {
|
||
r.move0(false)
|
||
return r
|
||
}
|
||
func (r *RsspLFSR) move0(rightShift bool) {
|
||
var significant_mask uint32 = 0xffffffff >> (32 - r.width)
|
||
//
|
||
r.register &= significant_mask
|
||
cb := r.register & r.polynomial & significant_mask
|
||
out := bitXor(cb, r.width)
|
||
if rightShift {
|
||
r.register >>= 1
|
||
r.register = r.register | (out << (r.width - 1))
|
||
} else {
|
||
r.register <<= 1
|
||
r.register = r.register | out
|
||
}
|
||
r.register &= significant_mask
|
||
}
|
||
|
||
func (r *RsspLFSR) GetAndMove() uint32 {
|
||
rt := r.register
|
||
r.move()
|
||
return rt
|
||
}
|
||
func (r *RsspLFSR) Get() uint32 {
|
||
return r.register
|
||
}
|
||
func (r *RsspLFSR) Reset() {
|
||
r.register = r.initValue
|
||
}
|
||
|
||
// return 0 或 1
|
||
func bitXor(data uint32, width int) uint32 {
|
||
var v uint32 = 0
|
||
var lsb_mask uint32 = 1
|
||
for i := 0; i < width; i++ {
|
||
v ^= data >> i
|
||
}
|
||
return v & lsb_mask
|
||
}
|
||
|
||
// ///////////////////////////////////////////////////////
|
||
const RsspSnMax = uint32(4294967295)
|
||
|
||
type RsspSn struct {
|
||
sn uint32 //顺序序列号，最大值4294967295
|
||
initValue uint32
|
||
}
|
||
|
||
func NewRsspSn(initValue uint32) *RsspSn {
|
||
return &RsspSn{sn: initValue, initValue: initValue}
|
||
}
|
||
func (s *RsspSn) GetAndAdd() uint32 {
|
||
if s.sn < RsspSnMax {
|
||
s.sn++
|
||
return s.sn
|
||
} else {
|
||
s.sn = s.initValue
|
||
return s.sn
|
||
}
|
||
}
|
||
func (s *RsspSn) Get() uint32 {
|
||
return s.sn
|
||
}
|