Implement Send Diagnostic

erichelgeson · May 24, 2022 · 0451d13 · 0451d13
1 parent 3c25845
commit 0451d13
Showing 1 changed file with 21 additions and 0 deletions.
diff --git a/src/BlueSCSI.cpp b/src/BlueSCSI.cpp
@@ -1468,6 +1468,24 @@ byte onReadBuffer(byte mode, uint32_t allocLength)
   }
 }
 
+/*
+ * On Send Diagnostic
+ */
+byte onSendDiagnostic(byte flags)
+{
+  int self_test = flags & 0x4;
+  LOGN("-SendDiagnostic");
+  LOGHEXN(flags);
+  if(self_test) {
+    // Don't actually do a test, we're good.
+    return SCSI_STATUS_GOOD;
+  } else {
+    m_senseKey = SCSI_SENSE_ILLEGAL_REQUEST;
+    m_addition_sense = SCSI_ASC_INVALID_FIELD_IN_CDB;
+    return SCSI_STATUS_CHECK_CONDITION;
+  }
+}
+
 /*
  * MsgIn2.
  */
@@ -1718,6 +1736,9 @@ void loop()
     LOGN("[ReadBuffer]");
     m_sts |= onReadBuffer(cmd[1] & 7, ((uint32_t)cmd[6] << 16) | ((uint32_t)cmd[7] << 8) | cmd[8]);
     break;
+  case SCSI_SEND_DIAG:
+    m_sts |= onSendDiagnostic(cmd[1]);
+    break;
   default:
     LOGN("[*Unknown]");
     m_sts |= SCSI_STATUS_CHECK_CONDITION;