From 1b75ca78f883016979588fc7f83c318119aa2a2c Mon Sep 17 00:00:00 2001 From: Natalia Portillo Date: Sat, 16 Dec 2017 20:02:16 +0000 Subject: [PATCH] Added SCSI MODE SENSE report to DiscImageChef.Device.Report. --- DiscImageChef.Device.Report/CMakeLists.txt | 2 +- DiscImageChef.Device.Report/scsi.c | 199 +++++++++++ DiscImageChef.Device.Report/scsi.h | 30 +- DiscImageChef.Device.Report/scsi_mode.h | 42 +++ DiscImageChef.Device.Report/scsi_report.c | 390 ++++++++++++++++++++- 5 files changed, 657 insertions(+), 6 deletions(-) create mode 100644 DiscImageChef.Device.Report/scsi_mode.h diff --git a/DiscImageChef.Device.Report/CMakeLists.txt b/DiscImageChef.Device.Report/CMakeLists.txt index b385d9eeb..3a3bf19ac 100644 --- a/DiscImageChef.Device.Report/CMakeLists.txt +++ b/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}) \ No newline at end of file diff --git a/DiscImageChef.Device.Report/scsi.c b/DiscImageChef.Device.Report/scsi.c index 04c7d31dc..6411109fd 100644 --- a/DiscImageChef.Device.Report/scsi.c +++ b/DiscImageChef.Device.Report/scsi.c @@ -8,8 +8,12 @@ #include #include #include +#include #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) @@ -62,5 +66,200 @@ int Inquiry(int fd, unsigned char **buffer, unsigned char **senseBuffer) cdb[4] = pagesLength; error = SendScsiCommand(fd, &cdb, cmd_len, *buffer, pagesLength, senseBuffer, SG_DXFER_FROM_DEV); + 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; } \ No newline at end of file diff --git a/DiscImageChef.Device.Report/scsi.h b/DiscImageChef.Device.Report/scsi.h index 7bf2dd3a5..20ee30897 100644 --- a/DiscImageChef.Device.Report/scsi.h +++ b/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 diff --git a/DiscImageChef.Device.Report/scsi_mode.h b/DiscImageChef.Device.Report/scsi_mode.h new file mode 100644 index 000000000..eb1f9e6e7 --- /dev/null +++ b/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 + +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 diff --git a/DiscImageChef.Device.Report/scsi_report.c b/DiscImageChef.Device.Report/scsi_report.c index a71ea6411..cdda20dda 100644 --- a/DiscImageChef.Device.Report/scsi_report.c +++ b/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); // 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); // 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); // - xmlTextWriterEndElement(xmlWriter); - xmlTextWriterEndElement(xmlWriter); + // 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"); // + 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); // + } + + xmlTextWriterEndElement(xmlWriter); // +} + +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; } \ No newline at end of file