rts-sim-testing-service/third_party/message/can_bus.go

package message

import "github.com/snksoft/crc"

/////////////////////CAN串行总线 bit 流处理 ////////////////////////

// CanBits 可以在CAN总线上传输的bit流
// 按bit位来存储数据
type CanBits struct {
	Data         []byte //bits
	LastBitsLen  int    //Data中最后一个字节中从高位到低位存储数据的有效位数，值范围[1,8],8-LastBitsLen则为最后一个字节中低位剩余的空位数量
	readRowIndex int    //行读指针，Data中的一个字节为一行，从0开始；当前值表示当前可读位置
	readColIndex int    //列读指针,字节中的bit位为列，从7开始到0，字节中最左侧bit为最高位编号7；当前值表示当前可读位置;当值为-1时表示已经读结束
}

// CanBusData CAN总线上传输的数据
type CanBusData interface {
	GetData() []byte
	GetLastRowBitsLen() int
}
type CanBitsReader interface {
	// ReadByte 读取一个字节
	ReadByte() (byte, bool)
	// ReadBits 读取bits,一次最多8bits
	ReadBits(bitsLen int) (byte, bool)
}
type CanBitsWriter interface {
	// AddByte 添加一整个字节bit数据
	AddByte(bits byte) bool
	// AddBits 添加bit数据，bitsLen-bits字节中有效位数，有效位向低位对齐
	AddBits(bits byte, bitsLen int) bool
	//GetReader 获取CanBits reader
	GetReader() CanBitsReader
}

func NewCanBitsWriter(cap int) CanBitsWriter {
	return &CanBits{LastBitsLen: 8, Data: make([]byte, 0, cap)}
}
func NewCanBitsReader(data []byte, lastRowBitsLen int) CanBitsReader {
	return &CanBits{
		LastBitsLen:  lastRowBitsLen,
		Data:         data,
		readRowIndex: 0,
		readColIndex: 7,
	}
}
func (b *CanBits) GetData() []byte {
	return b.Data
}
func (b *CanBits) GetLastRowBitsLen() int {
	return b.LastBitsLen
}
func (b *CanBits) GetReader() CanBitsReader {
	if len(b.Data) > 0 {
		b.readRowIndex = 0
		b.readColIndex = 7
	} else {
		b.readRowIndex = -1
	}
	return b
}

// ReadByte 读取一个字节
func (b *CanBits) ReadByte() (byte, bool) {
	return b.ReadBits(8)
}

// ReadBits 读取bits,一次最多8bits
func (b *CanBits) ReadBits(bitsLen int) (byte, bool) {
	if bitsLen < 1 || bitsLen > 8 {
		return 0, false
	}
	//当前可读行可读bits数
	if !b.isReadEnd() {
		rowBitsLen := b.curRowCanReadLen()
		if bitsLen <= rowBitsLen { //当前行有足够的bits
			sb := b.Data[b.readRowIndex]
			sb >>= b.readColIndex - bitsLen + 1
			b.toNextReadPos(bitsLen)
			return sb & b.getCanBitsMask(bitsLen), true
		} else { //当前行没有有足够的bits
			sb1 := b.Data[b.readRowIndex]
			rt1 := sb1 >> (b.readColIndex - rowBitsLen + 1)
			rt1 <<= bitsLen - rowBitsLen
			b.toNextReadPos(rowBitsLen)
			if !b.isReadEnd() {
				readBitsLen := bitsLen - rowBitsLen
				if b.curRowCanReadLen() >= readBitsLen {
					sb2 := b.Data[b.readRowIndex]
					sb2 >>= b.readColIndex - readBitsLen + 1
					b.toNextReadPos(readBitsLen)
					return (rt1 | sb2) & b.getCanBitsMask(bitsLen), true
				}
			}
		}
	}
	//
	return 0, false
}

// 是否读到最后一行
func (b *CanBits) isReadInLastRow() bool {
	return b.readRowIndex == len(b.Data)-1
}

// 当前可读行可读bits数
func (b *CanBits) curRowCanReadLen() int {
	if b.isReadInLastRow() {
		return b.LastBitsLen - (7 - b.readColIndex)
	} else {
		return b.readColIndex + 1
	}
}
func (b *CanBits) toNextReadPos(hadReadRowLen int) {
	lastRow := b.isReadInLastRow()
	b.readColIndex -= hadReadRowLen
	if !lastRow {
		if b.readColIndex < 0 {
			b.readRowIndex++
			b.readColIndex = 7
		}
	}
}

// 是否读结束
func (b *CanBits) isReadEnd() bool {
	return b.isReadInLastRow() && (7-b.readColIndex >= b.LastBitsLen) || b.readRowIndex < 0
}

// AddByte 添加一整个字节bit数据
func (b *CanBits) AddByte(bits byte) bool {
	return b.AddBits(bits, 8)
}

// AddBits 添加bit数据，bitsLen-bits字节中有效位数，有效位向低位对齐
func (b *CanBits) AddBits(bits byte, bitsLen int) bool {
	if bitsLen < 1 || bitsLen > 8 {
		return false
	}
	if len(b.Data) <= 0 {
		b.LastBitsLen = 8
	}
	//
	bits &= b.getCanBitsMask(bitsLen)
	//
	lastByteHavePos := b.LastBitsLen < 8
	if lastByteHavePos {
		lastBytePosWidth := 8 - b.LastBitsLen //最后一个字节低位剩余可用bit位
		if bitsLen <= lastBytePosWidth {      //最后一个字节有足够的空位
			bits <<= lastBytePosWidth - bitsLen
			b.Data[len(b.Data)-1] |= bits
			b.LastBitsLen += bitsLen
		} else { //最后一个字节没有足够的空位
			lb := bits >> (bitsLen - lastBytePosWidth)
			b.Data[len(b.Data)-1] |= lb //至此最后一个字节中的位存满
			//
			nb := bits << (8 - (bitsLen - lastBytePosWidth))
			b.Data = append(b.Data, nb)
			b.LastBitsLen = bitsLen - lastBytePosWidth
		}
	} else {
		bits <<= 8 - bitsLen
		b.Data = append(b.Data, bits)
		b.LastBitsLen = bitsLen
	}
	return true
}
func (b *CanBits) getCanBitsMask(bitsLen int) byte {
	switch bitsLen {
	case 1:
		return CAN_BITS_LEN1
	case 2:
		return CAN_BITS_LEN2
	case 3:
		return CAN_BITS_LEN3
	case 4:
		return CAN_BITS_LEN4
	case 5:
		return CAN_BITS_LEN5
	case 6:
		return CAN_BITS_LEN6
	case 7:
		return CAN_BITS_LEN7
	case 8:
		return CAN_BITS_LEN8
	default:
		panic("bitsLen<1||bitsLen>8")
	}
}

const (
	CAN_BITS_LEN1 = byte(0b0000_0001)
	CAN_BITS_LEN2 = byte(0b0000_0011)
	CAN_BITS_LEN3 = byte(0b0000_0111)
	CAN_BITS_LEN4 = byte(0b0000_1111)
	CAN_BITS_LEN5 = byte(0b0001_1111)
	CAN_BITS_LEN6 = byte(0b0011_1111)
	CAN_BITS_LEN7 = byte(0b0111_1111)
	CAN_BITS_LEN8 = byte(0b1111_1111)
)

///////////////////// CRC ////////////////////////

// const (
// 	CAN_CRC16_ATPREQ = 0x11021
// 	CAN_CRC32        = 0x04C11DB7
// )

var (
	CAN_CRC16 = crc.CCITT
	CAN_CRC32 = &crc.Parameters{Width: 32, Polynomial: 0x4A503DF1, Init: 0x00FF0000, ReflectIn: true, ReflectOut: true, FinalXor: 0xFFFFFFFF}
)

func Can_Crc16(data []byte) uint16 {
	return uint16(crc.CalculateCRC(CAN_CRC16, data))
}

func Can_Crc32(data []byte) uint32 {
	return uint32(crc.CalculateCRC(CAN_CRC32, data))
}

// var (
// 	crc16AtpReqTable []uint32 = nil
// 	crc32CanTable    []uint32 = nil
// )

// func CreateCanCrcTable() {
// 	if crc16AtpReqTable == nil {
// 		crc16AtpReqTable = CreateCrcTable(CAN_CRC16_ATPREQ, 16, false)
// 	}
// 	if crc32CanTable == nil {
// 		crc32CanTable = CreateCrcTable(CAN_CRC32, 32, false)
// 	}
// }
// func calculateAtpReqCrc16(data []byte) uint16 {
// 	crc := CrcTableBased(data, 16, 0, false, false, 0, crc16AtpReqTable)
// 	return uint16(crc)
// }
// func calculateCanCrc32(data []byte) uint32 {
// 	crc := CrcTableBased(data, 32, 0xFF_FF_FF_FF, false, false, 0, crc32CanTable)
// 	return crc
// }

// // CRC32A的校验范围是：报文+时间戳A
// func calculateDataRspCrc32A(data []byte) uint32 {
// 	return calculateCanCrc32(data)
// }

// // CRC32B的校验范围是：报文+时间戳B
// func calculateDataRspCrc32B(data []byte) uint32 {
// 	return calculateCanCrc32(data)
// }
// func calculateDataRspCrc32C(data []byte) uint32 {
// 	return calculateCanCrc32(data)
// }