rts-sim-testing-service/third_party/message/rssp_code.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)&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
}