93 lines
2.2 KiB
Go
93 lines
2.2 KiB
Go
package eq2net
|
|
|
|
// CRC16 table for CCITT polynomial (0x1021)
|
|
var crc16Table [256]uint16
|
|
|
|
func init() {
|
|
// Initialize CRC16 lookup table
|
|
for i := 0; i < 256; i++ {
|
|
crc := uint16(i << 8)
|
|
for j := 0; j < 8; j++ {
|
|
if (crc & 0x8000) != 0 {
|
|
crc = (crc << 1) ^ 0x1021
|
|
} else {
|
|
crc = crc << 1
|
|
}
|
|
}
|
|
crc16Table[i] = crc
|
|
}
|
|
}
|
|
|
|
// CRC16 calculates the CRC16-CCITT checksum with a key
|
|
func CRC16(data []byte, length int, key uint32) uint16 {
|
|
if length <= 0 || len(data) < length {
|
|
return 0
|
|
}
|
|
|
|
// Mix the key into initial CRC value
|
|
crc := uint16(0xFFFF)
|
|
keyBytes := []byte{
|
|
byte(key),
|
|
byte(key >> 8),
|
|
byte(key >> 16),
|
|
byte(key >> 24),
|
|
}
|
|
|
|
// Process key bytes first
|
|
for _, b := range keyBytes {
|
|
tableIndex := (uint8(crc>>8) ^ b) & 0xFF
|
|
crc = (crc << 8) ^ crc16Table[tableIndex]
|
|
}
|
|
|
|
// Process data
|
|
for i := 0; i < length; i++ {
|
|
tableIndex := (uint8(crc>>8) ^ data[i]) & 0xFF
|
|
crc = (crc << 8) ^ crc16Table[tableIndex]
|
|
}
|
|
|
|
return crc ^ 0xFFFF
|
|
}
|
|
|
|
// ValidateCRC checks if a packet has a valid CRC
|
|
func ValidateCRC(buffer []byte, key uint32) bool {
|
|
if len(buffer) < 3 {
|
|
return false
|
|
}
|
|
|
|
// Check for CRC-exempt packets
|
|
if len(buffer) >= 2 && buffer[0] == 0x00 {
|
|
switch buffer[1] {
|
|
case byte(OPSessionRequest), byte(OPSessionResponse), byte(OPOutOfSession):
|
|
return true // Session packets don't require CRC
|
|
}
|
|
}
|
|
|
|
// Check for combined application packets (also CRC-exempt)
|
|
if len(buffer) >= 4 && buffer[2] == 0x00 && buffer[3] == 0x19 {
|
|
return true
|
|
}
|
|
|
|
// Calculate CRC for the packet (excluding last 2 CRC bytes)
|
|
dataLen := len(buffer) - 2
|
|
calculatedCRC := CRC16(buffer, dataLen, key)
|
|
|
|
// Extract packet CRC (big-endian in last 2 bytes)
|
|
packetCRC := uint16(buffer[dataLen])<<8 | uint16(buffer[dataLen+1])
|
|
|
|
// Valid if no CRC present (0) or CRCs match
|
|
return packetCRC == 0 || calculatedCRC == packetCRC
|
|
}
|
|
|
|
// AppendCRC adds CRC to the end of a packet
|
|
func AppendCRC(buffer []byte, key uint32) []byte {
|
|
// Calculate CRC for current buffer
|
|
crc := CRC16(buffer, len(buffer), key)
|
|
|
|
// Append CRC in big-endian format
|
|
result := make([]byte, len(buffer)+2)
|
|
copy(result, buffer)
|
|
result[len(buffer)] = byte(crc >> 8)
|
|
result[len(buffer)+1] = byte(crc)
|
|
|
|
return result
|
|
} |