aaru-dps/Aaru
1
0
Fork 0
mirror of https://github.com/aaru-dps/Aaru.git synced 2025-12-16 19:24:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Natalia Portillo
2017-12-16 20:02:16 +00:00
parent 8930336221
commit 1b75ca78f8
5 changed files with 657 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
DiscImageChef.Device.Report/CMakeLists.txt
View File

@@ -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})

199
DiscImageChef.Device.Report/scsi.c
View File

@@ -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;
}

30
DiscImageChef.Device.Report/scsi.h
View File

@@ -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

42
DiscImageChef.Device.Report/scsi_mode.h Normal file
View File

@@ -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

386
DiscImageChef.Device.Report/scsi_report.c
View File

@@ -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;
}