public static byte[] EncodeMode10(DecodedMode mode, PeripheralDeviceTypes deviceType)
{
int modeSize = 0;
if (mode.Pages != null)
{
modeSize += mode.Pages.Sum(page => page.PageResponse.Length);
}
byte[] hdr = EncodeModeHeader10(mode.Header, deviceType);
modeSize += hdr.Length;
byte[] md = new byte[modeSize];
Array.Copy(hdr, 0, md, 0, hdr.Length);
if (mode.Pages == null)
{
return(md);
}
{
int offset = hdr.Length;
foreach (ModePage page in mode.Pages)
{
Array.Copy(page.PageResponse, 0, md, offset, page.PageResponse.Length);
offset += page.PageResponse.Length;
}
}
return(md);
}
public static DecodedMode?DecodeMode10(byte[] modeResponse, PeripheralDeviceTypes deviceType)
{
ModeHeader?hdr = DecodeModeHeader10(modeResponse, deviceType);
if (!hdr.HasValue)
{
return(null);
}
DecodedMode decoded = new DecodedMode {
Header = hdr.Value
};
bool longlba = (modeResponse[4] & 0x01) == 0x01;
int offset;
int blkDrLength = 0;
if (decoded.Header.BlockDescriptors != null)
{
blkDrLength = decoded.Header.BlockDescriptors.Length;
}
if (longlba)
{
offset = 8 + blkDrLength * 16;
}
else
{
offset = 8 + blkDrLength * 8;
}
int length = modeResponse[0] << 8;
length += modeResponse[1];
length += 2;
if (length != modeResponse.Length)
{
return(decoded);
}
List <ModePage> listpages = new List <ModePage>();
while (offset < modeResponse.Length)
{
bool isSubpage = (modeResponse[offset] & 0x40) == 0x40;
ModePage pg = new ModePage();
byte pageNo = (byte)(modeResponse[offset] & 0x3F);
if (pageNo == 0)
{
pg.PageResponse = new byte[modeResponse.Length - offset];
Array.Copy(modeResponse, offset, pg.PageResponse, 0, pg.PageResponse.Length);
pg.Page = 0;
pg.Subpage = 0;
offset += pg.PageResponse.Length;
}
else
{
if (isSubpage)
{
pg.PageResponse = new byte[(modeResponse[offset + 2] << 8) + modeResponse[offset + 3] + 4];
if (pg.PageResponse.Length + offset > modeResponse.Length)
{
return(decoded);
}
Array.Copy(modeResponse, offset, pg.PageResponse, 0, pg.PageResponse.Length);
pg.Page = (byte)(modeResponse[offset] & 0x3F);
pg.Subpage = modeResponse[offset + 1];
offset += pg.PageResponse.Length;
}
else
{
pg.PageResponse = new byte[modeResponse[offset + 1] + 2];
if (pg.PageResponse.Length + offset > modeResponse.Length)
{
return(decoded);
}
Array.Copy(modeResponse, offset, pg.PageResponse, 0, pg.PageResponse.Length);
pg.Page = (byte)(modeResponse[offset] & 0x3F);
pg.Subpage = 0;
offset += pg.PageResponse.Length;
}
}
listpages.Add(pg);
}
decoded.Pages = listpages.ToArray();
return(decoded);
}
public static DecodedMode?DecodeMode6(byte[] modeResponse, PeripheralDeviceTypes deviceType)
{
ModeHeader?hdr = DecodeModeHeader6(modeResponse, deviceType);
if (!hdr.HasValue)
{
return(null);
}
var decoded = new DecodedMode
{
Header = hdr.Value
};
int blkDrLength = 0;
if (decoded.Header.BlockDescriptors != null)
{
blkDrLength = decoded.Header.BlockDescriptors.Length;
}
int offset = 4 + (blkDrLength * 8);
int length = modeResponse[0] + 1;
if (length != modeResponse.Length)
{
return(decoded);
}
List <ModePage> listpages = new List <ModePage>();
while (offset < modeResponse.Length)
{
bool isSubpage = (modeResponse[offset] & 0x40) == 0x40;
var pg = new ModePage();
byte pageNo = (byte)(modeResponse[offset] & 0x3F);
if (pageNo == 0)
{
pg.PageResponse = new byte[modeResponse.Length - offset];
Array.Copy(modeResponse, offset, pg.PageResponse, 0, pg.PageResponse.Length);
pg.Page = 0;
pg.Subpage = 0;
offset += pg.PageResponse.Length;
}
else
{
if (isSubpage)
{
if (offset + 3 >= modeResponse.Length)
{
break;
}
pg.PageResponse = new byte[(modeResponse[offset + 2] << 8) + modeResponse[offset + 3] + 4];
int copyLen = pg.PageResponse.Length;
if (pg.PageResponse.Length + offset > modeResponse.Length)
{
copyLen = modeResponse.Length - offset;
}
Array.Copy(modeResponse, offset, pg.PageResponse, 0, copyLen);
pg.Page = (byte)(modeResponse[offset] & 0x3F);
pg.Subpage = modeResponse[offset + 1];
offset += pg.PageResponse.Length;
}
else
{
if (offset + 1 >= modeResponse.Length)
{
break;
}
pg.PageResponse = new byte[modeResponse[offset + 1] + 2];
int copyLen = pg.PageResponse.Length;
if (pg.PageResponse.Length + offset > modeResponse.Length)
{
copyLen = modeResponse.Length - offset;
}
Array.Copy(modeResponse, offset, pg.PageResponse, 0, copyLen);
pg.Page = (byte)(modeResponse[offset] & 0x3F);
pg.Subpage = 0;
offset += pg.PageResponse.Length;
}
}
listpages.Add(pg);
}
decoded.Pages = listpages.ToArray();
return(decoded);
}
