Added SCSI MODE SENSE report to DiscImageChef.Device.Report.

DiscImageChef.Device.Report/CMakeLists.txt

@@ -7,5 +7,5 @@ find_package(LibXml2)
 
 include_directories(${LIBXML2_INCLUDE_DIR})
 
-add_executable(DiscImageChef_Device_Report main.c scsi.c scsi.h main.h ata.h ata.c atapi.c atapi.h atapi_report.c atapi_report.h identify_decode.c identify_decode.h scsi_report.c scsi_report.h inquiry_decode.c inquiry_decode.h)
+add_executable(DiscImageChef_Device_Report main.c scsi.c scsi.h main.h ata.h ata.c atapi.c atapi.h atapi_report.c atapi_report.h identify_decode.c identify_decode.h scsi_report.c scsi_report.h inquiry_decode.c inquiry_decode.h scsi_mode.h)
 target_link_libraries(DiscImageChef_Device_Report ${LIBXML2_LIBRARIES})

DiscImageChef.Device.Report/scsi.c

@@ -8,8 +8,12 @@
 #include <sys/ioctl.h>
 #include <errno.h>
 #include <string.h>
+#include <stdint.h>
 #include "scsi.h"
 
+#define FALSE 0
+#define TRUE 1
+
 int SendScsiCommand(int fd, void *cdb, unsigned char cdb_len, unsigned char *buffer, unsigned int buffer_len, unsigned char **senseBuffer, int direction)
 {
     if(buffer == NULL || cdb == NULL)
@@ -64,3 +68,198 @@ int Inquiry(int fd, unsigned char **buffer, unsigned char **senseBuffer)
 
     return error;
 }
+
+int PreventMediumRemoval(int fd, unsigned char **senseBuffer)
+{
+    return PreventAllowMediumRemoval(fd, senseBuffer, FALSE, TRUE);
+}
+
+int AllowMediumRemoval(int fd, unsigned char **senseBuffer)
+{
+    return PreventAllowMediumRemoval(fd, senseBuffer, FALSE, FALSE);
+}
+
+int PreventAllowMediumRemoval(int fd, unsigned char **senseBuffer, int persistent, int prevent)
+{
+    unsigned char cmd_len = 6;
+    char cdb[] = {SCSI_PREVENT_ALLOW_MEDIUM_REMOVAL, 0, 0, 0, 0, 0};
+    unsigned char *buffer = malloc(0);
+
+    if(prevent)
+        cdb[4] += 0x01;
+    if(persistent)
+        cdb[4] += 0x02;
+
+    int error = SendScsiCommand(fd, &cdb, cmd_len, buffer, 0, senseBuffer, SG_DXFER_NONE);
+
+    return error;
+}
+
+int LoadTray(int fd, unsigned char **senseBuffer)
+{
+    return StartStopUnit(fd, senseBuffer, FALSE, 0, 0, FALSE, TRUE, TRUE);
+}
+
+int EjectTray(int fd, unsigned char **senseBuffer)
+{
+    return StartStopUnit(fd, senseBuffer, FALSE, 0, 0, FALSE, TRUE, FALSE);
+}
+
+int StartUnit(int fd, unsigned char **senseBuffer)
+{
+return StartStopUnit(fd, senseBuffer, FALSE, 0, 0, FALSE, FALSE, TRUE);
+}
+
+int StopUnit(int fd, unsigned char **senseBuffer)
+{
+return StartStopUnit(fd, senseBuffer, FALSE, 0, 0, FALSE, FALSE, FALSE);
+}
+
+int StartStopUnit(int fd, unsigned char **senseBuffer, int immediate, uint8_t formatLayer, uint8_t powerConditions, int changeFormatLayer, int loadEject, int start)
+{
+    unsigned char cmd_len = 6;
+    char cdb[] = {SCSI_START_STOP_UNIT, 0, 0, 0, 0, 0};
+    unsigned char *buffer = malloc(0);
+
+    if(immediate)
+        cdb[1] += 0x01;
+    if(changeFormatLayer)
+    {
+        cdb[3] = (formatLayer & 0x03);
+        cdb[4] += 0x04;
+    }
+    else
+    {
+        if(loadEject)
+            cdb[4] += 0x02;
+        if(start)
+            cdb[4] += 0x01;
+    }
+    cdb[4] += ((powerConditions & 0x0F) << 4);
+
+    int error = SendScsiCommand(fd, &cdb, cmd_len, buffer, 0, senseBuffer, SG_DXFER_NONE);
+
+    return error;
+}
+
+int SpcPreventMediumRemoval(int fd, unsigned char **senseBuffer)
+{
+    return SpcPreventAllowMediumRemoval(fd, senseBuffer, 0x01);
+}
+
+int SpcAllowMediumRemoval(int fd, unsigned char **senseBuffer)
+{
+    return SpcPreventAllowMediumRemoval(fd, senseBuffer, 0x00);
+}
+
+int SpcPreventAllowMediumRemoval(int fd, unsigned char **senseBuffer, uint8_t preventMode)
+{
+    unsigned char cmd_len = 6;
+    char cdb[] = {SCSI_PREVENT_ALLOW_MEDIUM_REMOVAL, 0, 0, 0, 0, 0};
+    unsigned char *buffer = malloc(0);
+    cdb[4] = (preventMode & 0x03);
+
+    int error = SendScsiCommand(fd, &cdb, cmd_len, buffer, 0, senseBuffer, SG_DXFER_NONE);
+
+    return error;
+}
+
+int Load(int fd, unsigned char **senseBuffer)
+{
+return LoadUnload(fd, senseBuffer, FALSE, TRUE, FALSE, FALSE, FALSE);
+}
+
+int Unload(int fd, unsigned char **senseBuffer)
+{
+return LoadUnload(fd, senseBuffer, FALSE, FALSE, FALSE, FALSE, FALSE);
+}
+
+int LoadUnload(int fd, unsigned char **senseBuffer, int immediate, int load, int retense, int endOfTape, int hold)
+{
+    unsigned char cmd_len = 6;
+    char cdb[] = {SCSI_LOAD_UNLOAD, 0, 0, 0, 0, 0};
+    unsigned char *buffer = malloc(0);
+    if(immediate)
+        cdb[1] = 0x01;
+    if(load)
+        cdb[4] += 0x01;
+    if(retense)
+        cdb[4] += 0x02;
+    if(endOfTape)
+        cdb[4] += 0x04;
+    if(hold)
+        cdb[4] += 0x08;
+
+    int error = SendScsiCommand(fd, &cdb, cmd_len, buffer, 0, senseBuffer, SG_DXFER_NONE);
+
+    return error;
+}
+
+int ModeSense6(int fd, unsigned char **buffer, unsigned char **senseBuffer, int DBD, uint8_t pageControl, uint8_t pageCode, uint8_t subPageCode)
+{
+    unsigned char cmd_len = 6;
+    unsigned int buffer_len = 255;
+    *buffer = malloc(buffer_len);
+    memset(*buffer, 0, buffer_len);
+
+    unsigned char cdb[] = {SCSI_MODE_SENSE, 0, 0, 0, 0, 0};
+    if(DBD)
+        cdb[1] |= 0x08;
+    cdb[2] |= pageControl;
+    cdb[2] |= (pageCode & 0x3F);
+    cdb[3] = subPageCode;
+    cdb[4] = (uint8_t)(buffer_len & 0xFF);
+
+    int error = SendScsiCommand(fd, &cdb, cmd_len, *buffer, buffer_len, senseBuffer, SG_DXFER_FROM_DEV);
+
+    if(error)
+        return error;
+
+    buffer_len = (unsigned int)*(*buffer + 0) + 1;
+
+    free(*buffer);
+    *buffer = malloc(buffer_len);
+    memset(*buffer, 0, buffer_len);
+    cdb[4] = (uint8_t)(buffer_len & 0xFF);
+
+    error = SendScsiCommand(fd, &cdb, cmd_len, *buffer, buffer_len, senseBuffer, SG_DXFER_FROM_DEV);
+
+    return error;
+}
+
+int ModeSense10(int fd, unsigned char **buffer, unsigned char **senseBuffer, int LLBAA, int DBD, uint8_t pageControl, uint8_t pageCode, uint8_t subPageCode)
+{
+    unsigned char cmd_len = 10;
+    unsigned int buffer_len = 4096;
+    *buffer = malloc(buffer_len);
+    memset(*buffer, 0, buffer_len);
+
+    unsigned char cdb[] = {SCSI_MODE_SENSE_10, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+    if(LLBAA)
+        cdb[1] |= 0x10;
+    if(DBD)
+        cdb[1] |= 0x08;
+    cdb[2] |= pageControl;
+    cdb[2] |= (pageCode & 0x3F);
+    cdb[3] = subPageCode;
+    cdb[7] = (uint8_t)((buffer_len & 0xFF00) >> 8);
+    cdb[8] = (uint8_t)(buffer_len & 0xFF);
+    cdb[9] = 0;
+
+    int error = SendScsiCommand(fd, &cdb, cmd_len, *buffer, buffer_len, senseBuffer, SG_DXFER_FROM_DEV);
+
+    if(error)
+        return error;
+
+    buffer_len = (unsigned int)(*(*buffer + 0) << 8) + *(*buffer + 1) + 2;
+
+    free(*buffer);
+    *buffer = malloc(buffer_len);
+    memset(*buffer, 0, buffer_len);
+    cdb[7] = (uint8_t)((buffer_len & 0xFF00) >> 8);
+    cdb[8] = (uint8_t)(buffer_len & 0xFF);
+
+    error = SendScsiCommand(fd, &cdb, cmd_len, *buffer, buffer_len, senseBuffer, SG_DXFER_FROM_DEV);
+
+    return error;
+}

DiscImageChef.Device.Report/scsi.h

@@ -9,14 +9,42 @@
 
 int SendScsiCommand(int fd, void *cdb, unsigned char cdb_len, unsigned char *buffer, unsigned int buffer_len, unsigned char **senseBuffer, int direction);
 int Inquiry(int fd, unsigned char **buffer, unsigned char **senseBuffer);
-
+int PreventMediumRemoval(int fd, unsigned char **senseBuffer);
+int AllowMediumRemoval(int fd, unsigned char **senseBuffer);
+int PreventAllowMediumRemoval(int fd, unsigned char **senseBuffer, int persistent, int prevent);
+int LoadTray(int fd, unsigned char **senseBuffer);
+int EjectTray(int fd, unsigned char **senseBuffer);
+int StartUnit(int fd, unsigned char **senseBuffer);
+int StopUnit(int fd, unsigned char **senseBuffer);
+int StartStopUnit(int fd, unsigned char **senseBuffer, int immediate, uint8_t formatLayer, uint8_t powerConditions, int changeFormatLayer, int loadEject, int start);
+int SpcPreventMediumRemoval(int fd, unsigned char **senseBuffer);
+int SpcAllowMediumRemoval(int fd, unsigned char **senseBuffer);
+int SpcPreventAllowMediumRemoval(int fd, unsigned char **senseBuffer, uint8_t preventMode);
+int Load(int fd, unsigned char **senseBuffer);
+int Unload(int fd, unsigned char **senseBuffer);
+int LoadUnload(int fd, unsigned char **senseBuffer, int immediate, int load, int retense, int endOfTape, int hold);
+int ModeSense6(int fd, unsigned char **buffer, unsigned char **senseBuffer, int DBD, uint8_t pageControl, uint8_t pageCode, uint8_t subPageCode);
+int ModeSense10(int fd, unsigned char **buffer, unsigned char **senseBuffer, int LLBAA, int DBD, uint8_t pageContorl, uint8_t pageCode, uint8_t subPageCode);
 
 typedef enum
 {
     SCSI_INQUIRY = 0x12,
+    SCSI_START_STOP_UNIT = 0x1B,
+    SCSI_LOAD_UNLOAD = SCSI_START_STOP_UNIT,
+    SCSI_MODE_SENSE = 0x1A,
+    SCSI_PREVENT_ALLOW_MEDIUM_REMOVAL = 0x1E,
+    SCSI_MODE_SENSE_10 = 0x5A,
     SCSI_ATA_PASSTHROUGH_16 = 0x85
 } ScsiCommands;
 
+typedef enum
+{
+    MODE_PAGE_CURRENT = 0x00,
+    MODE_PAGE_CHANGEABLE = 0x40,
+    MODE_PAGE_DEFAULT = 0x80,
+    MODE_PAGE_SAVED = 0xC0
+} ScsiModeSensePageControl;
+
 // SCSI INQUIRY command response
 #pragma pack(1)
 typedef struct

DiscImageChef.Device.Report/scsi_mode.h

@@ -0,0 +1,42 @@
+//
+// Created by claunia on 16/12/17.
+//
+
+#ifndef DISCIMAGECHEF_DEVICE_REPORT_SCSI_MODE_H
+#define DISCIMAGECHEF_DEVICE_REPORT_SCSI_MODE_H
+
+#include <stdint.h>
+
+typedef struct
+{
+    uint8_t Density;
+    uint64_t Blocks;
+    uint32_t BlockLength;
+} BlockDescriptor;
+
+typedef struct
+{
+    uint8_t MediumType;
+    int WriteProtected;
+    BlockDescriptor BlockDescriptors[4096];
+    int descriptorsLength;
+    uint8_t Speed;
+    uint8_t BufferedMode;
+    int EBC;
+    int DPOFUA;
+    int decoded;
+} ModeHeader;
+
+typedef struct
+{
+    ModeHeader Header;
+    unsigned char *Pages[256][256];
+    size_t pageSizes[256][256];
+    int decoded;
+} DecodedMode;
+
+ModeHeader DecodeModeHeader6(unsigned char* modeResponse, uint8_t deviceType);
+ModeHeader DecodeModeHeader10(unsigned char* modeResponse, uint8_t deviceType);
+DecodedMode DecodeMode6(unsigned char* modeResponse, uint8_t deviceType);
+DecodedMode DecodeMode10(unsigned char* modeResponse, uint8_t deviceType);
+#endif //DISCIMAGECHEF_DEVICE_REPORT_SCSI_MODE_H

DiscImageChef.Device.Report/scsi_report.c

@@ -6,6 +6,7 @@
 #include "scsi_report.h"
 #include "scsi.h"
 #include "inquiry_decode.h"
+#include "scsi_mode.h"
 
 void ScsiReport(int fd, xmlTextWriterPtr xmlWriter)
 {
@@ -27,7 +28,7 @@ void ScsiReport(int fd, xmlTextWriterPtr xmlWriter)
         return;
     }
 
-    xmlTextWriterStartElement(xmlWriter, BAD_CAST DIC_SCSI_REPORT_ELEMENT);
+    xmlTextWriterStartElement(xmlWriter, BAD_CAST DIC_SCSI_REPORT_ELEMENT); // <SCSI>
     page_len = *(buffer + 4) + 5;
 
     ScsiInquiry *inquiry = malloc(sizeof(ScsiInquiry));
@@ -45,8 +46,8 @@ void ScsiReport(int fd, xmlTextWriterPtr xmlWriter)
 
         removable = (user_response == 'Y' || user_response == 'y');
     }
-    xmlTextWriterStartElement(xmlWriter, BAD_CAST DIC_SCSI_INQUIRY_ELEMENT);
 
+    xmlTextWriterStartElement(xmlWriter, BAD_CAST DIC_SCSI_INQUIRY_ELEMENT); // <Inquiry>
     xmlTextWriterWriteFormatElement(xmlWriter, BAD_CAST "AccessControlCoordinator", "%s", inquiry->ACC ? "true" : "false");
     xmlTextWriterWriteFormatElement(xmlWriter, BAD_CAST "ACKRequests", "%s", inquiry->ACKREQQ ? "true" : "false");
     xmlTextWriterWriteFormatElement(xmlWriter, BAD_CAST "Address16", "%s", inquiry->Addr16 ? "true" : "false");
@@ -101,7 +102,388 @@ void ScsiReport(int fd, xmlTextWriterPtr xmlWriter)
     xmlTextWriterStartElement(xmlWriter, BAD_CAST "Data");
     xmlTextWriterWriteBase64(xmlWriter, buffer, 0, page_len);
     xmlTextWriterEndElement(xmlWriter);
+    xmlTextWriterEndElement(xmlWriter); // </Inquiry>
 
+    // TODO: EVPDs
+
+    if(removable)
+    {
+        if(inquiry->PeripheralDeviceType == 0x05) // MultiMediaDevice
+        {
+            AllowMediumRemoval(fd, &sense);
+            EjectTray(fd, &sense);
+        }
+        else if(inquiry->PeripheralDeviceType == 0x05) // SequentialAccess
+        {
+            SpcAllowMediumRemoval(fd, &sense);
+            printf("Asking drive to unload tape (can take a few minutes)...\n");
+            Unload(fd, &sense);
+        }
+        printf("Please remove any media from the device and press any key when it is out.\n");
+        scanf("%c");
+    }
+
+    int supportsMode6 = FALSE;
+    int supportsMode10 = FALSE;
+    int supportsModeSubpages = FALSE;
+    unsigned char* mode6Response = NULL;
+    unsigned char* mode10Response = NULL;
+
+    printf("Querying all mode pages and subpages using SCSI MODE SENSE (10)...\n");
+    error = ModeSense10(fd, &mode10Response, &sense, FALSE, TRUE, MODE_PAGE_DEFAULT, 0x3F, 0xFF);
+
+    if(error)
+    {
+        printf("Querying all mode pages using SCSI MODE SENSE (10)...");
+        error = ModeSense10(fd, &mode10Response, &sense, FALSE, TRUE, MODE_PAGE_DEFAULT, 0x3F, 0x00);
+        if(!error)
+            supportsMode10 = TRUE;
+    }
+    else
+    {
+        supportsMode10 = TRUE;
+        supportsModeSubpages = TRUE;
+    }
+
+    printf("Querying all mode pages and subpages using SCSI MODE SENSE (6)...\n");
+    error = ModeSense6(fd, &mode6Response, &sense, FALSE, MODE_PAGE_DEFAULT, 0x3F, 0xFF);
+    if(error)
+    {
+        printf("Querying all mode pages using SCSI MODE SENSE (6)...");
+        error = ModeSense6(fd, &mode6Response, &sense, FALSE, MODE_PAGE_DEFAULT, 0x3F, 0x00);
+        if(error)
+        {
+            printf("Querying SCSI MODE SENSE (6)...");
+            error = ModeSense6(fd, &mode6Response, &sense, FALSE, MODE_PAGE_DEFAULT, 0x00, 0x00);
+            if(!error)
+                supportsMode6 = TRUE;
+        }
+        else
+            supportsMode6 = TRUE;
+    }
+    else
+    {
+        supportsMode6 = TRUE;
+        supportsModeSubpages = TRUE;
+    }
+
+    xmlTextWriterWriteFormatElement(xmlWriter, BAD_CAST "SupportsModeSense6", "%s", supportsMode6 ? "true" : "false");
+    xmlTextWriterWriteFormatElement(xmlWriter, BAD_CAST "SupportsModeSense10", "%s", supportsMode10 ? "true" : "false");
+    xmlTextWriterWriteFormatElement(xmlWriter, BAD_CAST "SupportsModeSubpages", "%s", supportsModeSubpages ? "true" : "false");
+
+    if(supportsMode6)
+    {
+        xmlTextWriterStartElement(xmlWriter, BAD_CAST "ModeSense6Data");
+        xmlTextWriterWriteBase64(xmlWriter, mode6Response, 0, *(mode6Response + 0) + 1);
         xmlTextWriterEndElement(xmlWriter);
+    }
+
+    if(supportsMode10)
+    {
+        xmlTextWriterStartElement(xmlWriter, BAD_CAST "ModeSense10Data");
+        xmlTextWriterWriteBase64(xmlWriter, mode10Response, 0, (*(mode10Response + 0) << 8) + *(mode10Response + 1) + 2);
         xmlTextWriterEndElement(xmlWriter);
+    }
+
+    DecodedMode decMode;
+    memset(&decMode, 0, sizeof(DecodedMode));
+
+    if(supportsMode10)
+        decMode = DecodeMode10(mode10Response, inquiry->PeripheralDeviceType);
+    else if(supportsMode6)
+        decMode = DecodeMode6(mode6Response, inquiry->PeripheralDeviceType);
+
+    if(decMode.decoded)
+    {
+        int page, subpage;
+
+        xmlTextWriterStartElement(xmlWriter, BAD_CAST "ModeSense"); // <ModeSense>
+        xmlTextWriterWriteFormatElement(xmlWriter, BAD_CAST "BlankCheckEnabled", "%s", decMode.Header.EBC ? "true" : "false");
+        xmlTextWriterWriteFormatElement(xmlWriter, BAD_CAST "DPOandFUA", "%s", decMode.Header.DPOFUA ? "true" : "false");
+        xmlTextWriterWriteFormatElement(xmlWriter, BAD_CAST "WriteProtected", "%s", decMode.Header.WriteProtected ? "true" : "false");
+
+        if(decMode.Header.BufferedMode > 0)
+            xmlTextWriterWriteFormatElement(xmlWriter, BAD_CAST "BlankCheckEnabled", "%d", decMode.Header.BufferedMode);
+        if(decMode.Header.Speed > 0)
+            xmlTextWriterWriteFormatElement(xmlWriter, BAD_CAST "Speed", "%d", decMode.Header.Speed);
+
+        for(page = 0; page < 256; page++)
+        {
+            for(subpage = 0; subpage < 256; subpage++)
+            {
+                if(decMode.pageSizes[page][subpage] > 0 && decMode.Pages[page][subpage] != NULL)
+                {
+                    xmlTextWriterStartElement(xmlWriter, BAD_CAST "modePageType");
+                    xmlTextWriterWriteFormatAttribute(xmlWriter, BAD_CAST "page", "%d", page);
+                    xmlTextWriterWriteFormatAttribute(xmlWriter, BAD_CAST "subpage", "%d", subpage);
+                    xmlTextWriterWriteBase64(xmlWriter, decMode.Pages[page][subpage], 0, decMode.pageSizes[page][subpage]);
+                    xmlTextWriterEndElement(xmlWriter);
+
+                    if(page == 0x2A && subpage == 0x00)
+                    {
+                        // TODO: Decode CD-ROM page
+                    }
+                }
+            }
+        }
+        xmlTextWriterEndElement(xmlWriter); // </ModeSense>
+    }
+
+    xmlTextWriterEndElement(xmlWriter); // </SCSI>
+}
+
+ModeHeader DecodeModeHeader6(unsigned char* modeResponse, uint8_t deviceType)
+{
+    int i;
+    ModeHeader header;
+
+    if(modeResponse[3])
+    {
+        header.descriptorsLength = modeResponse[3] / 8;
+        for(i = 0; i < header.descriptorsLength; i++)
+        {
+            header.BlockDescriptors[i].Density = modeResponse[0 + i * 8 + 4];
+            header.BlockDescriptors[i].Blocks += (uint64_t)(modeResponse[1 + i * 8 + 4] << 16);
+            header.BlockDescriptors[i].Blocks += (uint64_t)(modeResponse[2 + i * 8 + 4] << 8);
+            header.BlockDescriptors[i].Blocks += modeResponse[3 + i * 8 + 4];
+            header.BlockDescriptors[i].BlockLength += (uint32_t)(modeResponse[5 + i * 8 + 4] << 16);
+            header.BlockDescriptors[i].BlockLength += (uint32_t)(modeResponse[6 + i * 8 + 4] << 8);
+            header.BlockDescriptors[i].BlockLength += modeResponse[7 + i * 8 + 4];
+        }
+    }
+
+    if(deviceType == 0x00 || deviceType == 0x05)
+    {
+        header.WriteProtected = ((modeResponse[2] & 0x80) == 0x80);
+        header.DPOFUA = ((modeResponse[2] & 0x10) == 0x10);
+    }
+
+    if(deviceType == 0x01)
+    {
+        header.WriteProtected = ((modeResponse[2] & 0x80) == 0x80);
+        header.Speed = (uint8_t)(modeResponse[2] & 0x0F);
+        header.BufferedMode = (uint8_t)((modeResponse[2] & 0x70) >> 4);
+    }
+
+    if(deviceType == 0x02)
+        header.BufferedMode = (uint8_t)((modeResponse[2] & 0x70) >> 4);
+
+    if(deviceType == 0x07)
+    {
+        header.WriteProtected = ((modeResponse[2] & 0x80) == 0x80);
+        header.EBC = ((modeResponse[2] & 0x01) == 0x01);
+        header.DPOFUA = ((modeResponse[2] & 0x10) == 0x10);
+    }
+
+    header.decoded = 1;
+
+    return header;
+}
+
+ModeHeader DecodeModeHeader10(unsigned char* modeResponse, uint8_t deviceType)
+{
+    uint16_t blockDescLength = (uint16_t)((modeResponse[6] << 8) + modeResponse[7]);
+    int i;
+    ModeHeader header;
+    header.MediumType = modeResponse[2];
+
+    int longLBA = (modeResponse[4] & 0x01) == 0x01;
+
+    if(blockDescLength > 0)
+    {
+        if(longLBA)
+        {
+            header.descriptorsLength = blockDescLength / 16;
+            for(i = 0; i < header.descriptorsLength; i++)
+            {
+                header.BlockDescriptors[i].Density = 0x00;
+                header.BlockDescriptors[i].Blocks = be64toh((uint64_t)(*modeResponse + 0 + i * 16 + 8));
+                header.BlockDescriptors[i].BlockLength += (uint32_t)(modeResponse[15 + i * 16 + 8] << 24);
+                header.BlockDescriptors[i].BlockLength += (uint32_t)(modeResponse[14 + i * 16 + 8] << 16);
+                header.BlockDescriptors[i].BlockLength += (uint32_t)(modeResponse[13 + i * 16 + 8] << 8);
+                header.BlockDescriptors[i].BlockLength += modeResponse[12 + i * 16 + 8];
+            }
+        }
+        else
+        {
+            header.descriptorsLength = blockDescLength / 8;
+            for(i = 0; i < header.descriptorsLength; i++)
+            {
+                if(deviceType != 0x00)
+                {
+                    header.BlockDescriptors[i].Density = modeResponse[0 + i * 8 + 8];
+                }
+                else
+                {
+                    header.BlockDescriptors[i].Density = 0x00;
+                    header.BlockDescriptors[i].Blocks += (uint64_t)(modeResponse[0 + i * 8 + 8] << 24);
+                }
+                header.BlockDescriptors[i].Blocks += (uint64_t)(modeResponse[1 + i * 8 + 8] << 16);
+                header.BlockDescriptors[i].Blocks += (uint64_t)(modeResponse[2 + i * 8 + 8] << 8);
+                header.BlockDescriptors[i].Blocks += modeResponse[3 + i * 8 + 8];
+                header.BlockDescriptors[i].BlockLength += (uint32_t)(modeResponse[5 + i * 8 + 8] << 16);
+                header.BlockDescriptors[i].BlockLength += (uint32_t)(modeResponse[6 + i * 8 + 8] << 8);
+                header.BlockDescriptors[i].BlockLength += modeResponse[7 + i * 8 + 8];
+            }
+        }
+    }
+
+    if(deviceType == 0x00 || deviceType == 0x05)
+    {
+        header.WriteProtected = ((modeResponse[3] & 0x80) == 0x80);
+        header.DPOFUA = ((modeResponse[3] & 0x10) == 0x10);
+    }
+
+    if(deviceType == 0x01)
+    {
+        header.WriteProtected = ((modeResponse[3] & 0x80) == 0x80);
+        header.Speed = (uint8_t)(modeResponse[3] & 0x0F);
+        header.BufferedMode = (uint8_t)((modeResponse[3] & 0x70) >> 4);
+    }
+
+    if(deviceType == 0x02)
+        header.BufferedMode = (uint8_t)((modeResponse[3] & 0x70) >> 4);
+
+    if(deviceType == 0x07)
+    {
+        header.WriteProtected = ((modeResponse[3] & 0x80) == 0x80);
+        header.EBC = ((modeResponse[3] & 0x01) == 0x01);
+        header.DPOFUA = ((modeResponse[3] & 0x10) == 0x10);
+    }
+
+    header.decoded = 1;
+
+    return header;
+}
+
+DecodedMode DecodeMode6(unsigned char* modeResponse, uint8_t deviceType)
+{
+    DecodedMode decoded;
+
+    ModeHeader hdr = DecodeModeHeader6(modeResponse, deviceType);
+    if(!hdr.decoded)
+        return decoded;
+
+    decoded.Header = hdr;
+    decoded.decoded = 1;
+
+    int offset = 4 + decoded.Header.descriptorsLength * 8;
+    int length = modeResponse[0] + 1;
+
+    while(offset < length)
+    {
+        int isSubpage = (modeResponse[offset] & 0x40) == 0x40;
+
+        uint8_t pageNo = (uint8_t)(modeResponse[offset] & 0x3F);
+        int subpage;
+
+        if(pageNo == 0)
+        {
+            decoded.pageSizes[0][0] = (size_t)(length - offset);
+            decoded.Pages[0][0] = malloc(decoded.pageSizes[0][0]);
+            memset(decoded.Pages[0][0], 0, decoded.pageSizes[0][0]);
+            memcpy(decoded.Pages[0][0], modeResponse + offset, decoded.pageSizes[0][0]);
+            offset += decoded.pageSizes[0][0];
+        }
+        else
+        {
+            if(isSubpage)
+            {
+                if(offset + 3 >= length)
+                    break;
+
+                pageNo = (uint8_t)(modeResponse[offset] & 0x3F);
+                subpage = modeResponse[offset + 1];
+                decoded.pageSizes[pageNo][subpage] = (size_t)((modeResponse[offset + 2] << 8) + modeResponse[offset + 3] + 4);
+                decoded.Pages[pageNo][subpage] = malloc(decoded.pageSizes[pageNo][subpage]);
+                memset(decoded.Pages[pageNo][subpage], 0, decoded.pageSizes[pageNo][subpage]);
+                memcpy(decoded.Pages[pageNo][subpage], modeResponse + offset, decoded.pageSizes[pageNo][subpage]);
+                offset += decoded.pageSizes[pageNo][subpage];
+            }
+            else
+            {
+                if(offset + 1 >= length)
+                    break;
+
+                pageNo = (uint8_t)(modeResponse[offset] & 0x3F);
+                decoded.pageSizes[pageNo][0] = (size_t)(modeResponse[offset + 1] + 2);
+                decoded.Pages[pageNo][0] = malloc(decoded.pageSizes[pageNo][0]);
+                memset(decoded.Pages[pageNo][0], 0, decoded.pageSizes[pageNo][0]);
+                memcpy(decoded.Pages[pageNo][0], modeResponse + offset, decoded.pageSizes[pageNo][0]);
+                offset += decoded.pageSizes[pageNo][0];
+            }
+        }
+    }
+
+    return decoded;
+}
+
+DecodedMode DecodeMode10(unsigned char* modeResponse, uint8_t deviceType)
+{
+    DecodedMode decodedMode;
+
+    decodedMode.Header = DecodeModeHeader10(modeResponse, deviceType);
+
+    if(!decodedMode.Header.decoded)
+        return decodedMode;
+
+    decodedMode.decoded = 1;
+
+    int longlba = (modeResponse[4] & 0x01) == 0x01;
+    int offset;
+
+    if(longlba)
+        offset = 8 + decodedMode.Header.descriptorsLength * 16;
+    else
+        offset = 8 + decodedMode.Header.descriptorsLength * 8;
+    int length = (modeResponse[0] << 8);
+    length += modeResponse[1];
+    length += 2;
+
+    while(offset < length)
+    {
+        int isSubpage = (modeResponse[offset] & 0x40) == 0x40;
+
+        uint8_t pageNo = (uint8_t)(modeResponse[offset] & 0x3F);
+        int subpage;
+
+        if(pageNo == 0)
+        {
+            decodedMode.pageSizes[0][0] = (size_t)(length - offset);
+            decodedMode.Pages[0][0] = malloc(decodedMode.pageSizes[0][0]);
+            memset(decodedMode.Pages[0][0], 0, decodedMode.pageSizes[0][0]);
+            memcpy(decodedMode.Pages[0][0], modeResponse + offset, decodedMode.pageSizes[0][0]);
+            offset += decodedMode.pageSizes[0][0];
+        }
+        else
+        {
+            if(isSubpage)
+            {
+                if(offset + 3 >= length)
+                    break;
+
+                pageNo = (uint8_t)(modeResponse[offset] & 0x3F);
+                subpage = modeResponse[offset + 1];
+                decodedMode.pageSizes[pageNo][subpage] = (size_t)((modeResponse[offset + 2] << 8) + modeResponse[offset + 3] + 4);
+                decodedMode.Pages[pageNo][subpage] = malloc(decodedMode.pageSizes[pageNo][subpage]);
+                memset(decodedMode.Pages[pageNo][subpage], 0, decodedMode.pageSizes[pageNo][subpage]);
+                memcpy(decodedMode.Pages[pageNo][subpage], modeResponse + offset, decodedMode.pageSizes[pageNo][subpage]);
+                offset += decodedMode.pageSizes[pageNo][subpage];
+            }
+            else
+            {
+                if(offset + 1 >= length)
+                    break;
+
+                pageNo = (uint8_t)(modeResponse[offset] & 0x3F);
+                decodedMode.pageSizes[pageNo][0] = (size_t)(modeResponse[offset + 1] + 2);
+                decodedMode.Pages[pageNo][0] = malloc(decodedMode.pageSizes[pageNo][0]);
+                memset(decodedMode.Pages[pageNo][0], 0, decodedMode.pageSizes[pageNo][0]);
+                memcpy(decodedMode.Pages[pageNo][0], modeResponse + offset, decodedMode.pageSizes[pageNo][0]);
+                offset += decodedMode.pageSizes[pageNo][0];
+            }
+        }
+    }
+
+    return decodedMode;
 }