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