3 files changed, 59 insertions, 4 deletions

diff --git a/src/crc.rs b/src/crc.rs
new file mode 100644
index 0000000..cfcf982
--- /dev/null
+++ b/src/crc.rs
@@ -0,0 +1,40 @@
+// Copyright 2019 Robin Krahl <robin.krahl@ireas.org>
+// SPDX-License-Identifier: GPL-3.0-or-later
+
+use hal::stm32::CRC;
+
+pub struct Crc {
+    crc: CRC,
+}
+
+impl Crc {
+    pub fn new(crc: CRC) -> Self {
+        Crc { crc }
+    }
+
+    pub fn get(&self, data: &[u8]) -> u32 {
+        self.reset();
+        data.chunks(4)
+            .map(slice_to_u32)
+            .for_each(|val| self.write(val));
+        self.read()
+    }
+
+    fn write(&self, val: u32) {
+        self.crc.dr.write(|w| w.dr().bits(val));
+    }
+
+    fn read(&self) -> u32 {
+        self.crc.dr.read().dr().bits()
+    }
+
+    fn reset(&self) {
+        self.crc.cr.write(|w| w.reset().reset());
+    }
+}
+
+fn slice_to_u32(data: &[u8]) -> u32 {
+    data.iter()
+        .enumerate()
+        .fold(0, |acc, (idx, val)| acc + (u32::from(*val) << (idx * 8)))
+}
diff --git a/src/device.rs b/src/device.rs
index 0df14ae..fc119d5 100644
--- a/src/device.rs
+++ b/src/device.rs
@@ -7,6 +7,7 @@ use usb_device::bus::{UsbBus, UsbBusAllocator};
 use usb_device::device::{UsbDevice, UsbDeviceBuilder, UsbVidPid};
 
 use crate::commands::{CommandId, COMMAND_LEN};
+use crate::crc::Crc;
 use crate::hid::{HidDevice, Protocol, ReportType, Subclass};
 use crate::util::TryFrom;
 
@@ -85,14 +86,18 @@ impl Response {
 }
 
 pub struct Nitrokey {
+    crc: Crc,
     request: Option<Request>,
+    request_crc: u32,
     buf: [u8; REPORT_LEN],
 }
 
 impl Nitrokey {
-    pub fn new() -> Self {
+    pub fn new(crc: Crc) -> Self {
         Nitrokey {
+            crc,
             request: None,
+            request_crc: 0,
             buf: [0; REPORT_LEN],
         }
     }
@@ -118,12 +123,15 @@ impl HidDevice for Nitrokey {
 
         if let Some(ref request) = self.request {
             let mut response = Response::new(DeviceStatus::Ok, request.command_id, request.crc);
-            response.command_status =
+            response.command_status = if self.request_crc == request.crc {
                 if let Ok(command_id) = CommandId::try_from(request.command_id) {
                     command_id.execute(&request.data, &mut response.data)
                 } else {
                     CommandStatus::UnknownCommand
-                };
+                }
+            } else {
+                CommandStatus::WrongCrc
+            };
             // TODO: calculate actual CRC
             response.crc = 1; // libnitrokey accepts any non-zero value
 
@@ -146,6 +154,8 @@ impl HidDevice for Nitrokey {
         } else {
             let (request, len) = ssmarshal::deserialize(data).map_err(|_| ())?;
             assert!(len == REPORT_LEN);
+            // the last four bytes are ignored as they contain the CRC
+            self.request_crc = self.crc.get(&data[..REPORT_LEN - 4]);
             self.request = Some(request);
             Ok(())
         }
diff --git a/src/main.rs b/src/main.rs
index caf7fdf..7e364ab 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -10,6 +10,7 @@ extern crate panic_halt;
 mod util;
 
 mod commands;
+mod crc;
 mod device;
 mod hid;
 
@@ -18,6 +19,7 @@ use hal::prelude::*;
 use stm32f103xx_usb::UsbBus;
 use usb_device::class::UsbClass;
 
+use crate::crc::Crc;
 use crate::device::Nitrokey;
 use crate::hid::HidClass;
 
@@ -25,6 +27,9 @@ use crate::hid::HidClass;
 fn main() -> ! {
     let p = hal::stm32::Peripherals::take().unwrap();
 
+    p.RCC.ahbenr.modify(|_, w| w.crcen().set_bit());
+    let crc = Crc::new(p.CRC);
+
     let mut flash = p.FLASH.constrain();
     let mut rcc = p.RCC.constrain();
 
@@ -40,7 +45,7 @@ fn main() -> ! {
     let mut gpioa = p.GPIOA.split(&mut rcc.apb2);
 
     let usb_bus = UsbBus::usb_with_reset(p.USB, &mut rcc.apb1, &clocks, &mut gpioa.crh, gpioa.pa12);
-    let mut usb_class = HidClass::new(Nitrokey::new(), &usb_bus);
+    let mut usb_class = HidClass::new(Nitrokey::new(crc), &usb_bus);
     let mut usb_dev = device::create_usb_device(&usb_bus);
     usb_dev.force_reset().expect("USB reset failed");