From 8dc9a838f5807bf95b5146a108f450e615ee59dd Mon Sep 17 00:00:00 2001
From: Michael McMaster <michael@codesrc.com>
Date: Tue, 19 Jan 2016 20:27:31 +1000
Subject: [PATCH] Allow custom VPD pages

---
 software/SCSI2SD/src/inquiry.c      | 35 ++++++++++++++++++++++++++++
 software/SCSI2SD/src/mode.c         |  4 ++--
 software/include/scsi2sd.h          |  3 ++-
 software/scsi2sd-util/ConfigUtil.cc | 36 ++++++++++++++++++++++++++++-
 4 files changed, 74 insertions(+), 4 deletions(-)

diff --git a/software/SCSI2SD/src/inquiry.c b/software/SCSI2SD/src/inquiry.c
index 99d5618..bc85904 100755
--- 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'
 };
 
+static void useCustomVPD(const TargetConfig* cfg, int pageCode)
+{
+	int cfgIdx = 0;
+	int found = 0;
+	while ((cfgIdx < sizeof(cfg->vpd) - 4) &&
+		(cfg->vpd[cfgIdx + 3] != 0)
+		)
+	{
+		int pageSize = cfg->vpd[cfgIdx + 3] + 4;
+		int dataPageCode = cfg->vpd[cfgIdx + 1];
+		if (dataPageCode == pageCode)
+		{
+			memcpy(scsiDev.data, &(cfg->vpd[cfgIdx]), pageSize);
+			scsiDev.dataLen = pageSize;
+			scsiDev.phase = DATA_IN;
+			found = 1;
+			break;
+		}
+		cfgIdx += pageSize;
+	}
+
+	if (!found)
+	{
+		// error.
+		scsiDev.status = CHECK_CONDITION;
+		scsiDev.target->sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.phase = STATUS;
+	}
+}
+
 void scsiInquiry()
 {
 	uint8 evpd = scsiDev.cdb[1] & 1; // enable vital product data.
@@ -127,6 +158,10 @@ void scsiInquiry()
 			scsiDev.phase = DATA_IN;
 		}
 	}
+	else if (scsiDev.target->cfg->vpd[3] != 0)
+	{
+		useCustomVPD(scsiDev.target->cfg, pageCode);
+	}
 	else if (pageCode == 0x00)
 	{
 		memcpy(scsiDev.data, SupportedVitalPages, sizeof(SupportedVitalPages));
diff --git a/software/SCSI2SD/src/mode.c b/software/SCSI2SD/src/mode.c
index c362878..ee9fa56 100755
--- 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)
 	}
 }
 
-static int useCustomPages(TargetConfig* cfg, int pc, int pageCode, int* idx)
+static int useCustomPages(const TargetConfig* cfg, int pc, int pageCode, int* idx)
 {
 	int found = 0;
 	int cfgIdx = 0;
-	while ((cfgIdx < sizeof(cfg->modePages) + 2) &&
+	while ((cfgIdx < sizeof(cfg->modePages) - 2) &&
 		(cfg->modePages[cfgIdx + 1] != 0)
 		)
 	{
diff --git a/software/include/scsi2sd.h b/software/include/scsi2sd.h
index 54f6bb9..fdd43b8 100755
--- 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 2074ad5..3c3cdd0 100644
--- 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();
 	}