diff --git a/src/BlueSCSI.cpp b/src/BlueSCSI.cpp
index 66c0539..5342757 100644
--- a/src/BlueSCSI.cpp
+++ b/src/BlueSCSI.cpp
@@ -1468,6 +1468,27 @@ 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 +1739,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;