Resend outgoing packet on invalid crc and when used all receiving retries

Signed-off-by: Szczepan Zalega <szczepan@nitrokey.com>

1 files changed, 66 insertions, 51 deletions

diff --git a/include/device_proto.h b/include/device_proto.h
index ff8bb67..2d39a04 100644
--- a/include/device_proto.h
+++ b/include/device_proto.h
@@ -218,61 +218,76 @@ namespace nitrokey {
 
               if (!outp.isValid()) throw std::runtime_error("Invalid outgoing packet");
 
-              status = dev.send(&outp);
-              if (status <= 0)
-                throw std::runtime_error(
-                    std::string("Device error while sending command ") +
-                    std::to_string((int) (status)));
-
-              std::this_thread::sleep_for(dev.get_send_receive_delay());
-
-              // FIXME make checks done in device:recv here
-              int retry = dev.get_retry_count();
-              while (retry-- > 0) {
-                status = dev.recv(&resp);
-
-                if (dev.get_device_model() == DeviceModel::STORAGE &&
-                    resp.command_id >= 0x20 &&
-//                    resp.command_id <= 0x20 + 26
-                    resp.command_id < 0x60
-                    ){
+              int retry = 0;
+              int sending_retry_counter = 3;
+              while (sending_retry_counter-->0) {
+                status = dev.send(&outp);
+                if (status <= 0)
+                  throw std::runtime_error(
+                      std::string("Device error while sending command ") +
+                      std::to_string((int) (status)));
+
+                std::this_thread::sleep_for(dev.get_send_receive_delay());
+
+                // FIXME make checks done in device:recv here
+                retry = dev.get_retry_count();
+                while (retry-- > 0) {
+                  status = dev.recv(&resp);
+
+                  if (dev.get_device_model() == DeviceModel::STORAGE &&
+                      resp.command_id >= 0x20 &&
+                      //                    resp.command_id <= 0x20 + 26
+                      resp.command_id < 0x60
+                      ) {
                     Log::instance()(std::string("Detected storage device cmd, status: ") +
-                                        std::to_string(resp.StorageStatus.Status_u8), Loglevel::DEBUG_L2);
-
-                  resp.last_command_status = 0;
-                  switch(resp.StorageStatus.Status_u8){
-                    case 0:
-                    case 1:
-                      resp.last_command_status = 0;
-                      resp.device_status = 0;
-                      break;
-                    case 2:
-                      resp.last_command_status = 0;
-                      resp.device_status = 1; //pro busy
-                      break;
-                    case 3:
-                    case 4:
-                      resp.last_command_status = 4;
-                      resp.device_status = 0;
-                      break;
-                  };
+                                    std::to_string(resp.StorageStatus.Status_u8), Loglevel::DEBUG_L2);
+
+                    resp.last_command_status = 0;
+                    switch (resp.StorageStatus.Status_u8) {
+                      case 0:
+                      case 1:
+                        resp.last_command_status = 0;
+                        resp.device_status = 0;
+                        break;
+                      case 2:
+                        resp.last_command_status = 0;
+                        resp.device_status = 1; //pro busy
+                        break;
+                      case 3:
+                      case 4:
+                        resp.last_command_status = 4;
+                        resp.device_status = 0;
+                        break;
+                    };
+                  }
+
+                  //SENDPASSWORD gives wrong CRC , for now rely on !=0 (TODO report)
+//                  if (resp.device_status == 0 && resp.last_command_crc == outp.crc && resp.isCRCcorrect()) break;
+                  if (resp.device_status == 0 && resp.last_command_crc == outp.crc && resp.isValid()) break;
+                  if (resp.device_status == 1 ) {
+                    retry++;
+                    Log::instance()("Status busy, not decresing retry counter: " + std::to_string(retry), Loglevel::DEBUG_L2);
+                  }
+                  Log::instance()(std::string("Retry status - dev status, equal crc, correct CRC: ")
+                                  + std::to_string(resp.device_status) + " " +
+                                  std::to_string(resp.last_command_crc == outp.crc) +
+                      " " + std::to_string(resp.isCRCcorrect()), Loglevel::DEBUG_L2);
+
+                  Log::instance()(
+                      "Device is not ready or received packet's last CRC is not equal to sent CRC packet, retrying...",
+                      Loglevel::DEBUG);
+                  Log::instance()("Invalid incoming HID packet:", Loglevel::DEBUG_L2);
+                  Log::instance()((std::string) (resp), Loglevel::DEBUG_L2);
+                  std::this_thread::sleep_for(dev.get_retry_timeout());
+                  continue;
                 }
-
-                dev.set_last_command_status(resp.last_command_status); // FIXME should be handled on device.recv
-
                 if (resp.device_status == 0 && resp.last_command_crc == outp.crc) break;
-                Log::instance()(std::string("Retry status: ")
-                                + std::to_string(resp.device_status) + " " +
-                    std::to_string(resp.last_command_crc==outp.crc), Loglevel::DEBUG_L2);
-
-                Log::instance()(
-                    "Device is not ready or received packet's last CRC is not equal to sent CRC packet, retrying...",
-                    Loglevel::DEBUG);
-                Log::instance()("Invalid incoming HID packet:", Loglevel::DEBUG_L2);
-                Log::instance()((std::string) (resp), Loglevel::DEBUG_L2);
-                std::this_thread::sleep_for(dev.get_retry_timeout());
-                continue;
+                Log::instance()(std::string("Resending (outer loop) "), Loglevel::DEBUG_L2);
+                Log::instance()(std::string("sending_retry_counter count: ") + std::to_string(sending_retry_counter), Loglevel::DEBUG);
               }
+
+              dev.set_last_command_status(resp.last_command_status); // FIXME should be handled on device.recv
+
               clear_packet(outp);
 
               if (status <= 0)