rts-sim-testing-service/third_party/message/rssp_code.go
walker 62b44a0cc6 添加UDP网络延时记录器
动力学和半实物接口添加UDP网络监控功能
2024-01-22 13:10:10 +08:00

324 lines
8.7 KiB
Go
Raw Permalink Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

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)&register_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
}