Support custom mode pages
[SCSI2SD.git] / software / SCSI2SD / src / mode.c
index 5cdf5de..c362878 100755 (executable)
@@ -197,6 +197,33 @@ 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
+{\r
+       int found = 0;\r
+       int cfgIdx = 0;\r
+       while ((cfgIdx < sizeof(cfg->modePages) + 2) &&\r
+               (cfg->modePages[cfgIdx + 1] != 0)\r
+               )\r
+       {\r
+               int pageSize = cfg->modePages[cfgIdx + 1] + 2;\r
+               int dataPageCode = cfg->modePages[cfgIdx] & 0x3f;\r
+               if ((dataPageCode == pageCode) ||\r
+                       (pageCode == 0x3f)\r
+                       )\r
+               {\r
+                       pageIn(pc, *idx, &cfg->modePages[cfgIdx], pageSize);\r
+                       *idx += pageSize;\r
+                       found = 1;\r
+                       if (pageCode != 0x3f)\r
+                       {\r
+                               break;\r
+                       }\r
+               }\r
+               cfgIdx += pageSize;\r
+       }\r
+       return found;\r
+}\r
+\r
 static void doModeSense(\r
        int sixByteCmd, int dbd, int pc, int pageCode, int allocLength)\r
 {\r
@@ -305,6 +332,12 @@ static void doModeSense(
 \r
        int pageFound = 0;\r
 \r
+       if (scsiDev.target->cfg->modePages[1] != 0)\r
+       {\r
+               pageFound = useCustomPages(scsiDev.target->cfg, pc, pageCode, &idx);\r
+               pageCode = 0xFF; // dodgy, skip rest of logic\r
+       }\r
+\r
        if (pageCode == 0x01 || pageCode == 0x3F)\r
        {\r
                pageFound = 1;\r