From 39487df4de8aadb22ccdeb984a5ac8c74080905b Mon Sep 17 00:00:00 2001
From: Kimmo Lindholm <kimmo@eke.fi>
Date: Thu, 12 Oct 2017 00:04:41 +0300
Subject: SCSI: Respond to unit serial number inquiry

---
 os/hal/include/hal_usb_msd.h |  3 ++-
 os/hal/src/hal_usb_msd.c     | 22 +++++++++++++++++++++-
 os/various/lib_scsi.c        |  7 ++++++-
 os/various/lib_scsi.h        | 15 +++++++++++++++
 4 files changed, 44 insertions(+), 3 deletions(-)

diff --git a/os/hal/include/hal_usb_msd.h b/os/hal/include/hal_usb_msd.h
index fcc2cf2..0fe03e4 100644
--- a/os/hal/include/hal_usb_msd.h
+++ b/os/hal/include/hal_usb_msd.h
@@ -172,7 +172,8 @@ extern "C" {
   void msdObjectInit(USBMassStorageDriver *msdp);
   void msdStart(USBMassStorageDriver *msdp, USBDriver *usbp,
                 BaseBlockDevice *blkdev, uint8_t *blkbuf,
-                const scsi_inquiry_response_t *scsi_inquiry_response);
+                const scsi_inquiry_response_t *scsi_inquiry_response,
+                const scsi_unit_serial_number_inquiry_response_t *serialInquiry);
   void msdStop(USBMassStorageDriver *msdp);
   bool msd_request_hook(USBDriver *usbp);
 #ifdef __cplusplus
diff --git a/os/hal/src/hal_usb_msd.c b/os/hal/src/hal_usb_msd.c
index 6cc5386..564bad0 100644
--- a/os/hal/src/hal_usb_msd.c
+++ b/os/hal/src/hal_usb_msd.c
@@ -84,6 +84,19 @@ static const scsi_inquiry_response_t default_scsi_inquiry_response = {
     {'v',CH_KERNEL_MAJOR+'0','.',CH_KERNEL_MINOR+'0'}
 };
 
+/**
+ * @brief   Hardcoded default SCSI unit serial number inquiry response structure.
+ */
+static const scsi_unit_serial_number_inquiry_response_t default_scsi_unit_serial_number_inquiry_response =
+{
+    0x00,
+    0x80,
+    0x00,
+    0x08,
+    "00000000"
+};
+
+
 /*===========================================================================*/
 /* Driver local functions.                                                   */
 /*===========================================================================*/
@@ -373,7 +386,8 @@ void msdStop(USBMassStorageDriver *msdp) {
  */
 void msdStart(USBMassStorageDriver *msdp, USBDriver *usbp,
               BaseBlockDevice *blkdev, uint8_t *blkbuf,
-              const scsi_inquiry_response_t *inquiry) {
+              const scsi_inquiry_response_t *inquiry,
+              const scsi_unit_serial_number_inquiry_response_t *serialInquiry) {
 
   osalDbgCheck((msdp != NULL) && (usbp != NULL)
               && (blkdev != NULL) && (blkbuf != NULL));
@@ -393,6 +407,12 @@ void msdStart(USBMassStorageDriver *msdp, USBDriver *usbp,
   else {
     msdp->scsi_config.inquiry_response = inquiry;
   }
+  if (NULL == serialInquiry) {
+    msdp->scsi_config.unit_serial_number_inquiry_response = &default_scsi_unit_serial_number_inquiry_response;
+  }
+  else {
+    msdp->scsi_config.unit_serial_number_inquiry_response = serialInquiry;
+  }
   msdp->scsi_config.blkbuf = blkbuf;
   msdp->scsi_config.blkdev = blkdev;
   msdp->scsi_config.transport = &msdp->scsi_transport;
diff --git a/os/various/lib_scsi.c b/os/various/lib_scsi.c
index 55aeb7e..1ece954 100644
--- a/os/various/lib_scsi.c
+++ b/os/various/lib_scsi.c
@@ -175,7 +175,12 @@ static bool cmd_ignored(SCSITarget *scsip, const uint8_t *cmd) {
  */
 static bool inquiry(SCSITarget *scsip, const uint8_t *cmd) {
 
-  if ((cmd[1] & 0b11) || cmd[2] != 0) {
+  if ((cmd[1] & 0b1) && cmd[2] == 0x80) {
+    /* Unit serial number page */
+    return transmit_data(scsip, (const uint8_t *)scsip->config->unit_serial_number_inquiry_response,
+                                sizeof(scsi_unit_serial_number_inquiry_response_t));
+  }
+  else if ((cmd[1] & 0b11) || cmd[2] != 0) {
     set_sense(scsip, SCSI_SENSE_KEY_ILLEGAL_REQUEST,
                      SCSI_ASENSE_INVALID_FIELD_IN_CDB,
                      SCSI_ASENSEQ_NO_QUALIFIER);
diff --git a/os/various/lib_scsi.h b/os/various/lib_scsi.h
index 97badb0..8384ae3 100644
--- a/os/various/lib_scsi.h
+++ b/os/various/lib_scsi.h
@@ -132,6 +132,17 @@ typedef struct PACKED_VAR {
   uint8_t productRev[4];
 } scsi_inquiry_response_t;
 
+/**
+ * @brief   Represents SCSI unit serial number inquiry response structure.
+ * @details See SCSI specification.
+ */
+typedef struct PACKED_VAR {
+  uint8_t peripheral;
+  uint8_t page_code;
+  uint8_t reserved;
+  uint8_t page_length;
+  uint8_t serianNumber[8];
+} scsi_unit_serial_number_inquiry_response_t;
 /**
  * @brief   Represents SCSI mode sense (6) request structure.
  * @details See SCSI specification.
@@ -225,6 +236,10 @@ typedef struct {
    * @brief   Pointer to SCSI inquiry response object.
    */
   const scsi_inquiry_response_t *inquiry_response;
+  /**
+   * @brief   Pointer to SCSI unit serial number inquiry response object.
+   */
+  const scsi_unit_serial_number_inquiry_response_t *unit_serial_number_inquiry_response;
 } SCSITargetConfig;
 
 /**
-- 
cgit v1.2.3