diff options
| -rw-r--r-- | decoders/usb_packet/pd.py | 47 |
1 files changed, 43 insertions, 4 deletions
diff --git a/decoders/usb_packet/pd.py b/decoders/usb_packet/pd.py index 54c5303..8a8503e 100644 --- a/decoders/usb_packet/pd.py +++ b/decoders/usb_packet/pd.py @@ -142,6 +142,35 @@ def bitstr_to_num(bitstr): l.reverse() return int(''.join(l), 2) +def reverse_number(num, count): + out = list(count * '0') + for i in range(0, count): + if (num >> i & 1): + out[i] = '1'; + return int(''.join(out), 2) + +def calc_crc5(bitstr): + poly5 = 0x25 + crc5 = 0x1f + for bit in bitstr: + crc5 <<= 1 + if (int(bit) != (crc5 >> 5)): + crc5 ^= poly5 + crc5 &= 0x1f + crc5 ^= 0x1f + return reverse_number(crc5, 5) + +def calc_crc16(bitstr): + poly16 = 0x18005 + crc16 = 0xffff + for bit in bitstr: + crc16 <<= 1 + if (int(bit) != (crc16 >> 16)): + crc16 ^= poly16 + crc16 &= 0xffff + crc16 ^= 0xffff + return reverse_number(crc16, 16) + class Decoder(srd.Decoder): api_version = 2 id = 'usb_packet' @@ -283,9 +312,14 @@ class Decoder(srd.Decoder): # Bits[27:31]: CRC5 crc5 = bitstr_to_num(packet[27:31 + 1]) + crc5_calc = calc_crc5(packet[16:27]) self.ss, self.es = self.bits[27][1], self.bits[31][2] - self.putpb(['CRC5', crc5]) - self.putb([6, ['CRC5: 0x%02X' % crc5, 'CRC5', 'C']]) + if (crc5 == crc5_calc): + self.putpb(['CRC5', crc5]) + self.putb([6, ['CRC5: 0x%02X' % crc5, 'CRC5', 'C']]) + else: + self.putpb(['CRC5 ERROR', crc5]) + self.putb([7, ['CRC5 ERROR: 0x%02X' % crc5, 'CRC5 ERR', 'CE', 'C']]) self.packet.append(crc5) elif pidname in ('DATA0', 'DATA1', 'DATA2', 'MDATA'): # Bits[16:packetlen-16]: Data @@ -310,9 +344,14 @@ class Decoder(srd.Decoder): # Bits[packetlen-16:packetlen]: CRC16 crc16 = bitstr_to_num(packet[-16:]) + crc16_calc = calc_crc16(packet[16:-16]) self.ss, self.es = self.bits[-16][1], self.bits[-1][2] - self.putpb(['CRC16', crc16]) - self.putb([9, ['CRC16: 0x%04X' % crc16, 'CRC16', 'C']]) + if (crc16 == crc16_calc): + self.putpb(['CRC16', crc16]) + self.putb([9, ['CRC16: 0x%04X' % crc16, 'CRC16', 'C']]) + else: + self.putpb(['CRC16 ERROR', crc16]) + self.putb([10, ['CRC16 ERROR: 0x%04X' % crc16, 'CRC16 ERR', 'CE', 'C']]) self.packet.append(crc16) elif pidname in ('ACK', 'NAK', 'STALL', 'NYET', 'ERR'): pass # Nothing to do, these only have SYNC+PID+EOP fields. |