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