Allow custom VPD pages

diff --git a/software/SCSI2SD/src/inquiry.c b/software/SCSI2SD/src/inquiry.c
index 99d5618f092c3a4473cbae33aba73a3b7a9458b9..bc85904cbb827b66f2d7cb076d21950775ca0bcd 100755 (executable)
--- a/software/SCSI2SD/src/inquiry.c
+++ b/software/SCSI2SD/src/inquiry.c
@@ -93,6 +93,37 @@ static const uint8 AscImpOperatingDefinition[] =
 'S','C','S','I','-','2'\r
 };\r
 \r
+static void useCustomVPD(const TargetConfig* cfg, int pageCode)\r
+{\r
+       int cfgIdx = 0;\r
+       int found = 0;\r
+       while ((cfgIdx < sizeof(cfg->vpd) - 4) &&\r
+               (cfg->vpd[cfgIdx + 3] != 0)\r
+               )\r
+       {\r
+               int pageSize = cfg->vpd[cfgIdx + 3] + 4;\r
+               int dataPageCode = cfg->vpd[cfgIdx + 1];\r
+               if (dataPageCode == pageCode)\r
+               {\r
+                       memcpy(scsiDev.data, &(cfg->vpd[cfgIdx]), pageSize);\r
+                       scsiDev.dataLen = pageSize;\r
+                       scsiDev.phase = DATA_IN;\r
+                       found = 1;\r
+                       break;\r
+               }\r
+               cfgIdx += pageSize;\r
+       }\r
+\r
+       if (!found)\r
+       {\r
+               // error.\r
+               scsiDev.status = CHECK_CONDITION;\r
+               scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
+               scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
+               scsiDev.phase = STATUS;\r
+       }\r
+}\r
+\r
 void scsiInquiry()\r
 {\r
        uint8 evpd = scsiDev.cdb[1] & 1; // enable vital product data.\r
@@ -127,6 +158,10 @@ void scsiInquiry()
                        scsiDev.phase = DATA_IN;\r
                }\r
        }\r
+       else if (scsiDev.target->cfg->vpd[3] != 0)\r
+       {\r
+               useCustomVPD(scsiDev.target->cfg, pageCode);\r
+       }\r
        else if (pageCode == 0x00)\r
        {\r
                memcpy(scsiDev.data, SupportedVitalPages, sizeof(SupportedVitalPages));\r

diff --git a/software/SCSI2SD/src/mode.c b/software/SCSI2SD/src/mode.c
index c3628780b0f131d042d4086bad1f56fb7815d00a..ee9fa56e57885026ee130060799f0c71bbcce0fd 100755 (executable)
--- a/software/SCSI2SD/src/mode.c
+++ b/software/SCSI2SD/src/mode.c
@@ -197,11 +197,11 @@ static void pageIn(int pc, int dataIdx, const uint8* pageData, int pageLen)
        }\r
 }\r
 \r
-static int useCustomPages(TargetConfig* cfg, int pc, int pageCode, int* idx)\r
+static int useCustomPages(const TargetConfig* cfg, int pc, int pageCode, int* idx)\r
 {\r
        int found = 0;\r
        int cfgIdx = 0;\r
-       while ((cfgIdx < sizeof(cfg->modePages) + 2) &&\r
+       while ((cfgIdx < sizeof(cfg->modePages) - 2) &&\r
                (cfg->modePages[cfgIdx + 1] != 0)\r
                )\r
        {\r

diff --git a/software/include/scsi2sd.h b/software/include/scsi2sd.h
index 54f6bb972a62b2eb58162cbd17911092b5afe7d6..fdd43b8ea77fabbf70d527a38ab0a042eb1f361f 100755 (executable)
--- a/software/include/scsi2sd.h
+++ b/software/include/scsi2sd.h
@@ -154,7 +154,8 @@ typedef struct __attribute__((packed))
        uint8_t reserved[960]; // Pad out to 1024 bytes for main section.
 
        uint8_t modePages[1024];
-       uint8_t unused[2048]; // Total size is 4k.
+       uint8_t vpd[1024];
+       uint8_t unused[1024]; // Total size is 4k.
 } TargetConfig;
 
 typedef struct __attribute__((packed))

diff --git a/software/scsi2sd-util/ConfigUtil.cc b/software/scsi2sd-util/ConfigUtil.cc
index 2074ad59f3c1034c5ef44357f3954c3f00f03bdc..3c3cdd015364ae08e410475c47ed237aa5c04c20 100644 (file)
--- a/software/scsi2sd-util/ConfigUtil.cc
+++ b/software/scsi2sd-util/ConfigUtil.cc
@@ -90,7 +90,7 @@ namespace
        {
                std::vector<uint8_t> result;
                int i = 0;
-               while (i < sizeof(cfg.modePages) + 2)
+               while (i < sizeof(cfg.modePages) - 2)
                {
                        int pageLen = cfg.modePages[i+1];
                        if (pageLen == 0) break;
@@ -102,6 +102,23 @@ namespace
                }
                return result;
        }
+
+       std::vector<uint8_t> getVPDPages(const TargetConfig& cfg)
+       {
+               std::vector<uint8_t> result;
+               int i = 0;
+               while (i < sizeof(cfg.vpd) - 4)
+               {
+                       int pageLen = cfg.vpd[i+3];
+                       if (pageLen == 0) break;
+                       std::copy(
+                               &cfg.vpd[i],
+                               &cfg.vpd[i+pageLen+4],
+                               std::back_inserter(result));
+                       i += pageLen + 4;
+               }
+               return result;
+       }
 }
 
 BoardConfig
@@ -217,6 +234,7 @@ ConfigUtil::toXML(const TargetConfig& config)
 {
        std::stringstream s;
        std::vector<uint8_t> modePages(getModePages(config));
+       std::vector<uint8_t> vpd(getVPDPages(config));
 
        s <<
                "<SCSITarget id=\"" <<
@@ -299,6 +317,13 @@ ConfigUtil::toXML(const TargetConfig& config)
                                wxBase64Encode(&modePages[0], modePages.size())) <<
                                "\n" <<
                "       </modePages>\n" <<
+               "\n" <<
+               "       <!-- Custom inquiry VPD pages, base64 encoded, up to 1024 bytes.-->\n" <<
+               "       <vpd>\n" <<
+                       (vpd.size() == 0 ? "" :
+                               wxBase64Encode(&vpd[0], vpd.size())) <<
+                               "\n" <<
+               "       </vpd>\n" <<
                "</SCSITarget>\n";
 
        return s.str();
@@ -511,6 +536,15 @@ parseTarget(wxXmlNode* node)
                        size_t len = std::min(buf.GetDataLen(), sizeof(result.modePages));
                        memcpy(result.modePages, buf.GetData(), len);
                }
+               else if (child->GetName() == "vpd")
+               {
+                       wxMemoryBuffer buf =
+                               wxBase64Decode(child->GetNodeContent(), wxBase64DecodeMode_SkipWS);
+                       size_t len = std::min(buf.GetDataLen(), sizeof(result.vpd));
+                       memcpy(result.vpd, buf.GetData(), len);
+               }
+
+
 
                child = child->GetNext();
        }