usb_packet: calculate and check CRC5/CRC16

In case CRC validation fails, an crc5-err/crc16-err annotation is issued,
instead of crc5-ok/crc16-ok, and the error is noted in the annotation
text.

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.