297 lines
7.3 KiB
Go
297 lines
7.3 KiB
Go
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 ( //CRC生成多项式
|
||
RsspCrc16GX uint32 = 0b1_0000_1000_0001_0001 //生成多项式 G(X)=X16+X11+X4+1
|
||
RsspCrc32C1 uint32 = 0x100d4e63 //安全通道1 CRC32生成多项式
|
||
RsspCrc32C2 uint32 = 0x8ce56011 //安全通道1 CRC32生成多项式
|
||
)
|
||
const ( //时间戳生成多项式
|
||
RsspTsC1 uint32 = 0x0fc22f87
|
||
RsspTsC2 uint32 = 0xc3e887e1
|
||
)
|
||
const (
|
||
SCW_C1 = uint32(0xae390b5a) //SCW常
|
||
SCW_C2 = uint32(0xc103589c) //SCW常
|
||
)
|
||
|
||
// 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
|
||
}
|