#if 0 to block out code sections

本文介绍了如何在C语言中利用#if0预处理器指令来屏蔽或临时禁用代码块,以避免注释引起的编译问题。相比于使用块注释,#if0提供了一种更安全的方法,可以更容易地找到并恢复被屏蔽的代码,并且支持嵌套。此外,文章还提到了使用特定宏定义作为条件注释的替代方案,以便在将来查找和管理被屏蔽的代码。

reference:C Language Tutorial => #if 0 to block out code sectionsLearn C Language - #if 0 to block out code sectionshttps://riptutorial.com/c/example/5407/sharpif-0-to-block-out-code-sections

Example#

If there are sections of code that you are considering removing or want to temporarily disable, you can comment it out with a block comment.

/* Block comment around whole function to keep it from getting used.
 * What's even the purpose of this function?
int myUnusedFunction(void)
{
    int i = 5;
    return i;
}
*/

However, if the source code you have surrounded with a block comment has block style comments in the source, the ending */ of the existing block comments can cause your new block comment to be invalid and cause compilation problems.

/* Block comment around whole function to keep it from getting used.
 * What's even the purpose of this function?
int myUnusedFunction(void)
{
    int i = 5;

    /* Return 5 */
    return i;
}
*/ 

In the previous example, the last two lines of the function and the last '*/' are seen by the compiler, so it would compile with errors. A safer method is to use an #if 0 directive around the code you want to block out.

#if 0
/* #if 0 evaluates to false, so everything between here and the #endif are
 * removed by the preprocessor. */
int myUnusedFunction(void)
{
    int i = 5;
    return i;
}
#endif

A benefit with this is that when you want to go back and find the code, it's much easier to do a search for "#if 0" than searching all your comments.

Another very important benefit is that you can nest commenting out code with #if 0. This cannot be done with comments.

An alternative to using #if 0 is to use a name that will not be #defined but is more descriptive of why the code is being blocked out. For instance if there is a function that seems to be useless dead code you might use #if defined(POSSIBLE_DEAD_CODE) or #if defined(FUTURE_CODE_REL_020201) for code needed once other functionality is in place or something similar. Then when going back through to remove or enable that source, those sections of source are easy to find.

3.5 Edit the Script 1. Launch a text or code editor to create a new JavaScript file. 2. Review the script one function at a time. There are four functions that must be implemented in the script to support solicited ethernet communications. • onProfileLoad: Retrieves driver metadata • onValidateTag: Verifies the address and data type created in the configuration or any dynamic tags created in an OPC client are valid for the end device connected • onTagsRequest: Builds a packet of bytes to transmit to the device across the wire. • onData: Interprets the response from the device and updates tag values or indicates if the read or write operation was successful based on the data in the response. Note: onTagsRequest and onData can do much more then described in this example. These functions can be used to communicate with many kinds of protocols. For more information view the Profile Library Plugin Help documentation. 3. Build out the script one function at a time, use the following information to edit the script. Required function: onProfileLoad The onProfileLoad function is the first of these functions called by the driver. It retrieves driver metadata, identifying the interface between the script and the driver by specifying the version of Universal Device Driver with which it was created as well as the mode. For more information on the mode please view the Profile Library plug-in help. Note: The only supported version is 2.0. Any other value is rejected, leading to failure of all subsequent functions. Any exception thrown out of any of the “framework” functions is caught and results in failure of the current operation. An exception thrown out of: • onProfileLoad causes all subsequent operations to fail until corrected • onValidateTag causes the tag address to be treated as “invalid” • onTagsRequest causes the read or write operation on the current tag to fail • onData causes the read or write operation on the current tag to fail Below is the entire onProfileLoad function: function onProfileLoad() { return { version: “2.0”, mode: “Client” }; } Required function: onValidateTag The onValidateTag script function is to validate the address syntax of a tag and the data type, which is central to communicating with a device. In the case of a Modbus device, this function ensures that an address is a holding register in the supported range. If desired, add logic to this function to modify various tag fields, such as providing a valid default data type,r modifying an address format to enforce consistency among tag addresses, or assigning a bulkId to group specific tags together. For the onValidateTag function in this Modbus example, review the sections: www.ptc.com 6 ©2021-2023 PTC, Inc. All Rights Reserved. // Validate the address is a holding register in the supported range let tagAddress = info.tag.address; try { let numericAddress = parseInt(tagAddress, 10); if (numericAddress < MINREGISTERADDRESS || numericAddress > MAXREGISTERADDRESS || isNaN(numericAddress)) { info.tag.valid = false; return info.tag; } // If grouping tags into bulks, assign bulkId now. // Otherwise, the next bulkId is assigned by default. let bulkId = Math.floor((numericAddress - MINREGISTERADDRESS)/BULKREGISTERCOUNT); info.tag.bulkId = bulkId; log(`Modbus Ethernet onValidateTag: Bulk register count ${BULKREGISTERCOUNT}, address ${tagAddress}, bulkId ${info.tag.bulkId}`, VERBOSE_LOGGING); info.tag.valid = true; return info.tag; } catch (e) { // Use log to provide helpful information that can assist with error resolution log(`Unexpected error (onValidateTag): ${e.message}`, VERBOSE_LOGGING); info.tag.valid = false; return info.tag; } The code above offers a look at the JavaScript object info that the driver provides to the script writer. This object is meant to hold data to be exchanged between the script and the driver. It checks the address received from the driver (info.tag.address) and verifies it is in the expected range for a Modbus holding register as defined by constants MINREGISTERADDRESS, MAXREGISTERADDRESS. If it’s not in that range, fail the tag being added by setting the valid field of the tag to false: info.tag.valid = false. The script also defines the bulkId field for each tag. The register in the address along with the BULKREGISTERCOUNT constant facilitates assigning the bulkId that allows blocking together consecutive registers. Once the tags are blocked together, the Universal Device driver will then provide them in the tags object passed to the onTagsRequest and onData functions. // Provide a valid default data type based on register // Note: "Default" is an invalid data type let validDataTypes = {"3": "Word", "4": "Word"} if (info.tag.dataType === "Default") { let registerChar = info.tag.address.charAt(0); info.tag.dataType = validDataTypes[registerChar]; } /* www.ptc.com 7 ©2021-2023 PTC, Inc. All Rights Reserved. * The regular expression to compare address to. * ^4 starts with '4' * 0* find zero or more occurrences of '0' * 1$ ends with '1' */ let addressRegex = /^40*1$/; // Correct a "semi-correct" tag address (e.g. 401 or 400001 --> 40001) with regex if (addressRegex.test(info.tag.address)) { info.tag.address = "40001"; } The above code provides examples of logic to modify various tag fields. The first code block resets the data type if Default is initially selected. While Default is a Kepware server data type, it is an invalid return value for a tag data type (i.e., info.tag.dataType). As such, provide an appropriate and valid data type based on the register if the data type is set as Default. The second code block uses a regex to recognize semi-correct addresses and modify them accordingly. In the above implementation, this logic adjusts tag addresses with too few or too many zeros; for example, ‘401’ or ‘400001` is changed to ‘40001’. Below is the entire onValidateTag function: function onValidateTag(info) { // Provide a valid default data type based on register // Note: "Default" is an invalid data type let validDataTypes = {"3": "Long", "4": "DWord"} if (info.tag.dataType === "Default") { let registerChar = info.tag.address.charAt(0); info.tag.dataType = validDataTypes[registerChar]; } /* * The regular expression to compare address to. * ^4 starts with '4' * 0* find zero or more occurrences of '0' * 1$ ends with '1' */ let addressRegex = /^40*1$/; // Correct a "semi-correct" tag address (e.g. 401 or 400001 --> 40001) with regex if (addressRegex.test(info.tag.address)) { info.tag.address = "40001"; } // Validate the address is a holding register in the supported range let tagAddress = info.tag.address; try { www.ptc.com 8 ©2021-2023 PTC, Inc. All Rights Reserved. let numericAddress = parseInt(tagAddress, 10); if (numericAddress < MINREGISTERADDRESS || numericAddress > MAXREGISTERADDRESS || isNaN(numericAddress)) { info.tag.valid = false; return info.tag; } // If grouping tags into bulks, assign bulkId now. // Otherwise, the next bulkId is assigned by default. let bulkId = Math.floor((numericAddress - MINREGISTERADDRESS)/BULKREGISTERCOUNT); info.tag.bulkId = bulkId; log(`Modbus Ethernet onValidateTag: Bulk register count ${BULKREGISTERCOUNT}, address ${tagAddress}, bulkId ${info.tag.bulkId}`, VERBOSE_LOGGING); info.tag.valid = true; return info.tag; } catch (e) { // Use log to provide helpful information that can assist with error resolution log(`Unexpected error (onValidateTag): ${e.message}`, VERBOSE_LOGGING); info.tag.valid = false; return info.tag; } } Required function: onTagsRequest The onTagsRequest script function builds a packet of bytes that is sent to the target Modbus device. In the example implementation, the onTagsRequest function makes use of two helper functions to build action-specific packet: BuildReadMessage and BuildWriteMessage: function onTagsRequest(info) { let action = "Fail"; if (info.type === "Read") { let readData = BuildReadMessage(info.tags); // Evaluate if the data was successfully built if (readData.length === 12) { action = "Receive"; } return { action: action, data: readData }; } else if (info.type === "Write") { SENTWRITEDATA = BuildWriteMessage(info.tags); // Evaluate if the data was successfully built www.ptc.com 9 ©2021-2023 PTC, Inc. All Rights Reserved. if (SENTWRITEDATA.length === 12) { action = "Receive"; } return { action: action, data: SENTWRITEDATA }; } } Unlike the onTagsRequest function, these helper functions are not required; they help make the script more manageable. Let’s dive into these helper functions now. Helper Function: BuildReadMessage This function builds into the packet the function code for a Modbus read to ensure that the read is on the appropriate address(es). Most of the Modbus-specific pieces of this snippet are documented in code comments with the important parts called out. The Modbus protocol supports blocking / bulk read and write functionality. The Universal Device Driver supports blocking tags for reads but does not support blocking tags for writes. The tags parameter is an array containing at least one tag element. If, in onValidateTag, the script assigned the same bulkId to more than one tag, then those tags sharing a bulkId are included in the array when the request type is Read. function BuildReadMessage (tags) { // This should never happen, but it's best practice if (tags.length === 0) { throw "No tags were requested for read request."; } // Sort the Modbus registers low to high let registers = []; for(let i=0; i<tags.length; i++) { registers[i] = parseInt(tags[i].address, 10); } registers.sort (sortNumber); // Find the lowest register, and the number of registers required to read the whole block let first = registers[0]; let count = registers[registers.length - 1] - first + 1; // Get the zero-based register index to make the request first -= 40001; The code above checks the tags component of the JavaScript object info (i.e. info.tags). This component holds an array of tags. Each tag has an address used to build a request packet for a read. The beginning of this section of code ensures that the driver has given a tag to build a request packet. If the length of the tags array is zero, it exits the function because there's no reason for the driver to build a request packet if no tag – and in turn, no address – is provided. www.ptc.com 10 ©2021-2023 PTC, Inc. All Rights Reserved. // Update the transaction ID in the stateful transaction object if (TXID === undefined) { TXID = 0; } else { TXID++; } JavaScript is not a strongly typed language, making it possible to modify a variable's type or composition at runtime. This is something to take advantage of within the BuildReadMessage function. The above code snippet updates the value of a global variable TXID, which represents a transaction ID exchanged between the script and the driver. Use this global variable to keep track of the number of times it is building packets to transmit to the device. It's important to keep track of this because the transaction ID is a necessary part of the Modbus protocol, as seen in the next step. TXID is stateful between transactions because it is shared between the script and driver and maintains state across transactions. Every time this function is called, the transaction ID value maintains the state it was the last time it was changed at runtime. // Build the Modbus Ethernet data let data = // ----Transaction ID------|-Protocol--|---Length--|Server|-Fxn-| [hiByte(TXID), loByte(TXID), 0x00, 0x00, 0x00, 0x06, 0x00, 0x03, ------Starting Address-------|-------Register count--------| hiByte(first), loByte(first), hiByte(count), loByte(count)] The above shows the packet being constructed. It is an array of bytes to be sent to the Modbus device. The code comments the different parts of the packet that are defined in the Modbus protocol; for instance, the TXID described earlier is used in the protocol as the top two bytes. Note: Only bytes are currently supported for the data array. Below is the entire BuildReadMessage function: function BuildReadMessage (tags) { // This should never happen, but it's best practice if (tags.length === 0) { throw "No tags were requested for read request."; } // Sort the Modbus registers low to high let registers = []; for(let i=0; i<tags.length; i++) { registers[i] = parseInt(tags[i].address, 10); } registers.sort (sortNumber); // Find the lowest register, and the number of registers required to read the whole block let first = registers[0]; let count = registers[registers.length - 1] - first + 1; // Get the zero-based register index to make the request first -= 40001; www.ptc.com 11 ©2021-2023 PTC, Inc. All Rights Reserved. // Initialize or update the transaction ID in the stateful transaction object if (TXID === undefined) { TXID = 0; } else { TXID++; } // Build the Modbus Ethernet data let data = // ----Transaction ID------|-Protocol--|---Length--|Server|-Fxn-|------Starting Address--- - [hiByte(TXID), loByte(TXID), 0x00, 0x00, 0x00, 0x06, 0x00, 0x03, hiByte(first), ---|-------Register count--------| loByte(first), hiByte(count), loByte(count)] return data; } Helper Function: BuildWriteMessage The BuildWriteMessage function is similar to the BuildReadMessage function in that it assists with building an array of bytes to send the device. However, this function facilitates writing a value to, rather than reading a value from, a Modbus device. Note: Not all devices support writes. If the target device does support writes, the BuildWriteMessage function – in conjunction with the ParseWriteMessage function – provides an example of how to implement this functionality. // This should never happen but it's best practice if (tags.length === 0) { throw "No tags were requested for write request."; } // Sort the Modbus registers low to high let register = parseInt(tags[0].address, 10); register -= 40001; // Get the value to write which is located in the first // element in the tags[n].value object let value = parseInt(tags[0].value); The code above assigns the integer value of the tag address to the variable register. Additionally, is assigns the value of the first tag value to the variable value since KEPServerEX only allows single writes. // Build the Modbus Ethernet data let data = // ----Transaction ID-----|-Protocol--|---Length--|Server|-Fxn-| www.ptc.com 12 ©2021-2023 PTC, Inc. All Rights Reserved. [ hiByte(TXID), loByte(TXID), 0x00, 0x00, 0x00, 0x06, 0x00, 0x06, --------Starting Address----------|-------value to write--------| hiByte(register), loByte(register), hiByte(value), loByte(value) ]; return data; The above shows how to build up a write packet, which is very similar to building a read packet within the BuildReadMessage function. Required function: onData The onData script function parses the array of bytes received from a Modbus device. In the example implementation, as was the case with the onTagsRequest function, the onData function uses two helper functions to parse responses from a Modbus device: ParseReadMessage and ParseWriteMessage: function onData(info) { let action = ACTIONFAILURE; if (info.type === "Read") { let tags = ParseReadMessage(info.tags, info.data); // Evaluate if the data was successfully parsed from the packet if (tags[0].value != null || tags[0].quality != null) { action = ACTIONCOMPLETE; } return { action: action, tags: tags }; } else if (info.type === "Write") { action = ParseWriteMessage(info.data); return { action: action, tags: info.tags }; } } Helper Function: ParseReadMessage This function's purpose is to parse an incoming packet into a tag value to update the respective tag in the server. The incoming packet is passed to the script via the JavaScript object information as the returned byte array is contained in its data component (i.e. info.data). The function determines what information is important based on the protocol specification and extracts the value for the tag/address. This value is assigned to the value field of the tag (e.g. info.tags[0].value) and then returned from the function, which is how the tag is updated in the server. www.ptc.com 13 ©2021-2023 PTC, Inc. All Rights Reserved. function ParseReadMessage(tags, data) { // This should never happen but it's best practice if (tags.length === 0) { throw "No tags were requested for read request."; } log(`Modbus Ethernet ParseReadMessage: data ${JSON.stringify(data)}`, VERBOSE_LOGGING); // Convert the string addresses to integers (eg 40001) let registers = []; for(let i=0; i < tags.length; i++) { registers[i] = parseInt(tags[i].address, 10); } // Find the lowest numbered register - this is the starting address let startingAddress = Array.min (registers); // MBE Response values start here: let offset = 9; // Enough bytes? if (data.length < offset + 2 * registers.length) { // Iterate the registers and set the quality of each tag to bad for (let i = 0; i < registers.length; ++i) { // Log message only once for this response if (i === 0) { if (data.length === offset){ log(`Modbus Ethernet ParseReadMessage: Device returned an error code ${data[7]}, ${data[8]}`); } else { log(`Modbus Ethernet ParseReadMessage: Invalid response from device`); } } tags[i].quality = "Bad"; } } The code above performs error checking and gathering some information about the transaction. If the number of bytes in the response is not the number of bytes expected, then the script sets the quality of each tag to Bad. If the response appears to include an error code from the device, then the script provides that information in the message passed to the log function. Otherwise, the script logs a message indicating an invalid response from the device. The result is an updated tags component of the JavaScript object info to be shared with the driver and ultimately used to update the tag qualities in the server. // Iterate the registers and lookup the response value for each for (let i = 0; i < registers.length; ++i) { // Calculate the index of this register's value in the response buffer let index = registers[i] - startingAddress; // Extract it from the response buffer www.ptc.com 14 ©2021-2023 PTC, Inc. All Rights Reserved. let hi = data[2*index + offset]; let lo = data[2*index + offset + 1]; tags[i].value = (wordFromBytes (hi, lo)); } return tags; The code above extracts the value returned from the device and assigns it to the appropriate tag to be used to update the tag value in the server. The result is an updated tags component of the JavaScript object info to be shared with the driver and ultimately used to update the tag values in the server. Below is the entire ParseReadMessage function. function ParseReadMessage(tags, data) { // This should never happen but it's best practice if (tags.length === 0) { throw "No tags were requested for read request."; } log(`Modbus Ethernet ParseReadMessage: data ${JSON.stringify(data)}`, VERBOSE_LOGGING); // Convert the string addresses to integers (eg 40001) let registers = []; for(let i=0; i < tags.length; i++) { registers[i] = parseInt(tags[i].address, 10); } // Find the lowest numbered register - this is the starting address let startingAddress = Array.min (registers); // MBE Response values start here: let offset = 9; // Enough bytes? if (data.length < offset + 2 * registers.length) { // Iterate the registers and set the quality of each tag to bad for (let i = 0; i < registers.length; ++i) { // Log message only once for this response if (i === 0) { if (data.length === offset){ log(`Modbus Ethernet ParseReadMessage: Device returned an error code ${data[7]}, ${data[8]}`); } else { log(`Modbus Ethernet ParseReadMessage: Invalid response from device`); } } tags[i].quality = "Bad"; } www.ptc.com 15 ©2021-2023 PTC, Inc. All Rights Reserved. } else { // Iterate the registers and lookup the response value for each. // Assigning the quality of the tag is optional. If undefined, Good is assumed. for (let i = 0; i < registers.length; ++i) { // Calc the index of this register's value in the response buffer let index = registers[i] - startingAddress; // Extract the value from the response buffer let hi = data[2*index + offset]; let lo = data[2*index + offset + 1]; tags[i].value = (wordFromBytes (hi, lo)); } } return tags; } Helper Function: ParseWriteMessage The purpose of the ParseWriteMessage function is to determine if the write was successful. Most devices respond that the request was received and executed. In the case of Modbus, the response echoes the request, which makes it possible to compare the returned message with the sent message that was saved in the global variable SENTWRITEDATA. Below is the entire ParseWriteMessage function: function ParseWriteMessage(data) { // Modbus echoes a write request so if the data sent // does not match the data received, then the write fails SENTWRITEDATA.forEach((e1) => data.forEach((e2) => { if (e1 !== e2) { return "Fail"; } })); return "Complete"; } 以上分析总结出重点
09-30
3.5 Edit the Script 1. Launch a text or code editor to create a new JavaScript file. 2. Review the script one function at a time. There are four functions that must be implemented in the script to support solicited ethernet communications. • onProfileLoad: Retrieves driver metadata • onValidateTag: Verifies the address and data type created in the configuration or any dynamic tags created in an OPC client are valid for the end device connected • onTagsRequest: Builds a packet of bytes to transmit to the device across the wire. • onData: Interprets the response from the device and updates tag values or indicates if the read or write operation was successful based on the data in the response. Note: onTagsRequest and onData can do much more then described in this example. These functions can be used to communicate with many kinds of protocols. For more information view the Profile Library Plugin Help documentation. 3. Build out the script one function at a time, use the following information to edit the script. Required function: onProfileLoad The onProfileLoad function is the first of these functions called by the driver. It retrieves driver metadata, identifying the interface between the script and the driver by specifying the version of Universal Device Driver with which it was created as well as the mode. For more information on the mode please view the Profile Library plug-in help. Note: The only supported version is 2.0. Any other value is rejected, leading to failure of all subsequent functions. Any exception thrown out of any of the “framework” functions is caught and results in failure of the current operation. An exception thrown out of: • onProfileLoad causes all subsequent operations to fail until corrected • onValidateTag causes the tag address to be treated as “invalid” • onTagsRequest causes the read or write operation on the current tag to fail • onData causes the read or write operation on the current tag to fail Below is the entire onProfileLoad function: function onProfileLoad() { return { version: “2.0”, mode: “Client” }; } Required function: onValidateTag The onValidateTag script function is to validate the address syntax of a tag and the data type, which is central to communicating with a device. In the case of a Modbus device, this function ensures that an address is a holding register in the supported range. If desired, add logic to this function to modify various tag fields, such as providing a valid default data type,r modifying an address format to enforce consistency among tag addresses, or assigning a bulkId to group specific tags together. For the onValidateTag function in this Modbus example, review the sections: // Validate the address is a holding register in the supported range let tagAddress = info.tag.address; try { let numericAddress = parseInt(tagAddress, 10); if (numericAddress < MINREGISTERADDRESS || numericAddress > MAXREGISTERADDRESS || isNaN(numericAddress)) { info.tag.valid = false; return info.tag; } // If grouping tags into bulks, assign bulkId now. // Otherwise, the next bulkId is assigned by default. let bulkId = Math.floor((numericAddress - MINREGISTERADDRESS)/BULKREGISTERCOUNT); info.tag.bulkId = bulkId; log(`Modbus Ethernet onValidateTag: Bulk register count ${BULKREGISTERCOUNT}, address ${tagAddress}, bulkId ${info.tag.bulkId}`, VERBOSE_LOGGING); info.tag.valid = true; return info.tag; } catch (e) { // Use log to provide helpful information that can assist with error resolution log(`Unexpected error (onValidateTag): ${e.message}`, VERBOSE_LOGGING); info.tag.valid = false; return info.tag; } The code above offers a look at the JavaScript object info that the driver provides to the script writer. This object is meant to hold data to be exchanged between the script and the driver. It checks the address received from the driver (info.tag.address) and verifies it is in the expected range for a Modbus holding register as defined by constants MINREGISTERADDRESS, MAXREGISTERADDRESS. If it’s not in that range, fail the tag being added by setting the valid field of the tag to false: info.tag.valid = false. The script also defines the bulkId field for each tag. The register in the address along with the BULKREGISTERCOUNT constant facilitates assigning the bulkId that allows blocking together consecutive registers. Once the tags are blocked together, the Universal Device driver will then provide them in the tags object passed to the onTagsRequest and onData functions. // Provide a valid default data type based on register // Note: "Default" is an invalid data type let validDataTypes = {"3": "Word", "4": "Word"} if (info.tag.dataType === "Default") { let registerChar = info.tag.address.charAt(0); info.tag.dataType = validDataTypes[registerChar]; } /* * The regular expression to compare address to. * ^4 starts with '4' * 0* find zero or more occurrences of '0' * 1$ ends with '1' */ let addressRegex = /^40*1$/; // Correct a "semi-correct" tag address (e.g. 401 or 400001 --> 40001) with regex if (addressRegex.test(info.tag.address)) { info.tag.address = "40001"; } The above code provides examples of logic to modify various tag fields. The first code block resets the data type if Default is initially selected. While Default is a Kepware server data type, it is an invalid return value for a tag data type (i.e., info.tag.dataType). As such, provide an appropriate and valid data type based on the register if the data type is set as Default. The second code block uses a regex to recognize semi-correct addresses and modify them accordingly. In the above implementation, this logic adjusts tag addresses with too few or too many zeros; for example, ‘401’ or ‘400001` is changed to ‘40001’. Below is the entire onValidateTag function: function onValidateTag(info) { // Provide a valid default data type based on register // Note: "Default" is an invalid data type let validDataTypes = {"3": "Long", "4": "DWord"} if (info.tag.dataType === "Default") { let registerChar = info.tag.address.charAt(0); info.tag.dataType = validDataTypes[registerChar]; } /* * The regular expression to compare address to. * ^4 starts with '4' * 0* find zero or more occurrences of '0' * 1$ ends with '1' */ let addressRegex = /^40*1$/; // Correct a "semi-correct" tag address (e.g. 401 or 400001 --> 40001) with regex if (addressRegex.test(info.tag.address)) { info.tag.address = "40001"; } // Validate the address is a holding register in the supported range let tagAddress = info.tag.address; try {let numericAddress = parseInt(tagAddress, 10); if (numericAddress < MINREGISTERADDRESS || numericAddress > MAXREGISTERADDRESS || isNaN(numericAddress)) { info.tag.valid = false; return info.tag; } // If grouping tags into bulks, assign bulkId now. // Otherwise, the next bulkId is assigned by default. let bulkId = Math.floor((numericAddress - MINREGISTERADDRESS)/BULKREGISTERCOUNT); info.tag.bulkId = bulkId; log(`Modbus Ethernet onValidateTag: Bulk register count ${BULKREGISTERCOUNT}, address ${tagAddress}, bulkId ${info.tag.bulkId}`, VERBOSE_LOGGING); info.tag.valid = true; return info.tag; } catch (e) { // Use log to provide helpful information that can assist with error resolution log(`Unexpected error (onValidateTag): ${e.message}`, VERBOSE_LOGGING); info.tag.valid = false; return info.tag; } } Required function: onTagsRequest The onTagsRequest script function builds a packet of bytes that is sent to the target Modbus device. In the example implementation, the onTagsRequest function makes use of two helper functions to build action-specific packet: BuildReadMessage and BuildWriteMessage: function onTagsRequest(info) { let action = "Fail"; if (info.type === "Read") { let readData = BuildReadMessage(info.tags); // Evaluate if the data was successfully built if (readData.length === 12) { action = "Receive"; } return { action: action, data: readData }; } else if (info.type === "Write") { SENTWRITEDATA = BuildWriteMessage(info.tags); // Evaluate if the data was successfully built if (SENTWRITEDATA.length === 12) { action = "Receive"; } return { action: action, data: SENTWRITEDATA }; } } Unlike the onTagsRequest function, these helper functions are not required; they help make the script more manageable. Let’s dive into these helper functions now. Helper Function: BuildReadMessage This function builds into the packet the function code for a Modbus read to ensure that the read is on the appropriate address(es). Most of the Modbus-specific pieces of this snippet are documented in code comments with the important parts called out. The Modbus protocol supports blocking / bulk read and write functionality. The Universal Device Driver supports blocking tags for reads but does not support blocking tags for writes. The tags parameter is an array containing at least one tag element. If, in onValidateTag, the script assigned the same bulkId to more than one tag, then those tags sharing a bulkId are included in the array when the request type is Read. function BuildReadMessage (tags) { // This should never happen, but it's best practice if (tags.length === 0) { throw "No tags were requested for read request."; } // Sort the Modbus registers low to high let registers = []; for(let i=0; i<tags.length; i++) { registers[i] = parseInt(tags[i].address, 10); } registers.sort (sortNumber); // Find the lowest register, and the number of registers required to read the whole block let first = registers[0]; let count = registers[registers.length - 1] - first + 1; // Get the zero-based register index to make the request first -= 40001; The code above checks the tags component of the JavaScript object info (i.e. info.tags). This component holds an array of tags. Each tag has an address used to build a request packet for a read. The beginning of this section of code ensures that the driver has given a tag to build a request packet. If the length of the tags array is zero, it exits the function because there's no reason for the driver to build a request packet if no tag – and in turn, no address – is provided. // Update the transaction ID in the stateful transaction object if (TXID === undefined) { TXID = 0; } else { TXID++; } JavaScript is not a strongly typed language, making it possible to modify a variable's type or composition at runtime. This is something to take advantage of within the BuildReadMessage function. The above code snippet updates the value of a global variable TXID, which represents a transaction ID exchanged between the script and the driver. Use this global variable to keep track of the number of times it is building packets to transmit to the device. It's important to keep track of this because the transaction ID is a necessary part of the Modbus protocol, as seen in the next step. TXID is stateful between transactions because it is shared between the script and driver and maintains state across transactions. Every time this function is called, the transaction ID value maintains the state it was the last time it was changed at runtime. // Build the Modbus Ethernet data let data = // ----Transaction ID------|-Protocol--|---Length--|Server|-Fxn-| [hiByte(TXID), loByte(TXID), 0x00, 0x00, 0x00, 0x06, 0x00, 0x03, ------Starting Address-------|-------Register count--------| hiByte(first), loByte(first), hiByte(count), loByte(count)] The above shows the packet being constructed. It is an array of bytes to be sent to the Modbus device. The code comments the different parts of the packet that are defined in the Modbus protocol; for instance, the TXID described earlier is used in the protocol as the top two bytes. Note: Only bytes are currently supported for the data array. Below is the entire BuildReadMessage function: function BuildReadMessage (tags) { // This should never happen, but it's best practice if (tags.length === 0) { throw "No tags were requested for read request."; } // Sort the Modbus registers low to high let registers = []; for(let i=0; i<tags.length; i++) { registers[i] = parseInt(tags[i].address, 10); } registers.sort (sortNumber); // Find the lowest register, and the number of registers required to read the whole block let first = registers[0]; let count = registers[registers.length - 1] - first + 1; // Get the zero-based register index to make the request first -= 40001; www.ptc.com 11 ©2021-2023 PTC, Inc. All Rights Reserved. // Initialize or update the transaction ID in the stateful transaction object if (TXID === undefined) { TXID = 0; } else { TXID++; } // Build the Modbus Ethernet data let data = // ----Transaction ID------|-Protocol--|---Length--|Server|-Fxn-|------Starting Address--- - [hiByte(TXID), loByte(TXID), 0x00, 0x00, 0x00, 0x06, 0x00, 0x03, hiByte(first), ---|-------Register count--------| loByte(first), hiByte(count), loByte(count)] return data; } Helper Function: BuildWriteMessage The BuildWriteMessage function is similar to the BuildReadMessage function in that it assists with building an array of bytes to send the device. However, this function facilitates writing a value to, rather than reading a value from, a Modbus device. Note: Not all devices support writes. If the target device does support writes, the BuildWriteMessage function – in conjunction with the ParseWriteMessage function – provides an example of how to implement this functionality. // This should never happen but it's best practice if (tags.length === 0) { throw "No tags were requested for write request."; } // Sort the Modbus registers low to high let register = parseInt(tags[0].address, 10); register -= 40001; // Get the value to write which is located in the first // element in the tags[n].value object let value = parseInt(tags[0].value); The code above assigns the integer value of the tag address to the variable register. Additionally, is assigns the value of the first tag value to the variable value since KEPServerEX only allows single writes. // Build the Modbus Ethernet data let data = // ----Transaction ID-----|-Protocol--|---Length--|Server|-Fxn-| www.ptc.com 12 ©2021-2023 PTC, Inc. All Rights Reserved. [ hiByte(TXID), loByte(TXID), 0x00, 0x00, 0x00, 0x06, 0x00, 0x06, --------Starting Address----------|-------value to write--------| hiByte(register), loByte(register), hiByte(value), loByte(value) ]; return data; The above shows how to build up a write packet, which is very similar to building a read packet within the BuildReadMessage function. Required function: onData The onData script function parses the array of bytes received from a Modbus device. In the example implementation, as was the case with the onTagsRequest function, the onData function uses two helper functions to parse responses from a Modbus device: ParseReadMessage and ParseWriteMessage: function onData(info) { let action = ACTIONFAILURE; if (info.type === "Read") { let tags = ParseReadMessage(info.tags, info.data); // Evaluate if the data was successfully parsed from the packet if (tags[0].value != null || tags[0].quality != null) { action = ACTIONCOMPLETE; } return { action: action, tags: tags }; } else if (info.type === "Write") { action = ParseWriteMessage(info.data); return { action: action, tags: info.tags }; } } Helper Function: ParseReadMessage This function's purpose is to parse an incoming packet into a tag value to update the respective tag in the server. The incoming packet is passed to the script via the JavaScript object information as the returned byte array is contained in its data component (i.e. info.data). The function determines what information is important based on the protocol specification and extracts the value for the tag/address. This value is assigned to the value field of the tag (e.g. info.tags[0].value) and then returned from the function, which is how the tag is updated in the server. function ParseReadMessage(tags, data) { // This should never happen but it's best practice if (tags.length === 0) { throw "No tags were requested for read request."; } log(`Modbus Ethernet ParseReadMessage: data ${JSON.stringify(data)}`, VERBOSE_LOGGING); // Convert the string addresses to integers (eg 40001) let registers = []; for(let i=0; i < tags.length; i++) { registers[i] = parseInt(tags[i].address, 10); } // Find the lowest numbered register - this is the starting address let startingAddress = Array.min (registers); // MBE Response values start here: let offset = 9; // Enough bytes? if (data.length < offset + 2 * registers.length) { // Iterate the registers and set the quality of each tag to bad for (let i = 0; i < registers.length; ++i) { // Log message only once for this response if (i === 0) { if (data.length === offset){ log(`Modbus Ethernet ParseReadMessage: Device returned an error code ${data[7]}, ${data[8]}`); } else { log(`Modbus Ethernet ParseReadMessage: Invalid response from device`); } } tags[i].quality = "Bad"; } } The code above performs error checking and gathering some information about the transaction. If the number of bytes in the response is not the number of bytes expected, then the script sets the quality of each tag to Bad. If the response appears to include an error code from the device, then the script provides that information in the message passed to the log function. Otherwise, the script logs a message indicating an invalid response from the device. The result is an updated tags component of the JavaScript object info to be shared with the driver and ultimately used to update the tag qualities in the server. // Iterate the registers and lookup the response value for each for (let i = 0; i < registers.length; ++i) { // Calculate the index of this register's value in the response buffer let index = registers[i] - startingAddress; // Extract it from the response buffer www.ptc.com 14 ©2021-2023 PTC, Inc. All Rights Reserved. let hi = data[2*index + offset]; let lo = data[2*index + offset + 1]; tags[i].value = (wordFromBytes (hi, lo)); } return tags; The code above extracts the value returned from the device and assigns it to the appropriate tag to be used to update the tag value in the server. The result is an updated tags component of the JavaScript object info to be shared with the driver and ultimately used to update the tag values in the server. Below is the entire ParseReadMessage function. function ParseReadMessage(tags, data) { // This should never happen but it's best practice if (tags.length === 0) { throw "No tags were requested for read request."; } log(`Modbus Ethernet ParseReadMessage: data ${JSON.stringify(data)}`, VERBOSE_LOGGING); // Convert the string addresses to integers (eg 40001) let registers = []; for(let i=0; i < tags.length; i++) { registers[i] = parseInt(tags[i].address, 10); } // Find the lowest numbered register - this is the starting address let startingAddress = Array.min (registers); // MBE Response values start here: let offset = 9; // Enough bytes? if (data.length < offset + 2 * registers.length) { // Iterate the registers and set the quality of each tag to bad for (let i = 0; i < registers.length; ++i) { // Log message only once for this response if (i === 0) { if (data.length === offset){ log(`Modbus Ethernet ParseReadMessage: Device returned an error code ${data[7]}, ${data[8]}`); } else { log(`Modbus Ethernet ParseReadMessage: Invalid response from device`); } } tags[i].quality = "Bad"; } www.ptc.com 15 ©2021-2023 PTC, Inc. All Rights Reserved. } else { // Iterate the registers and lookup the response value for each. // Assigning the quality of the tag is optional. If undefined, Good is assumed. for (let i = 0; i < registers.length; ++i) { // Calc the index of this register's value in the response buffer let index = registers[i] - startingAddress; // Extract the value from the response buffer let hi = data[2*index + offset]; let lo = data[2*index + offset + 1]; tags[i].value = (wordFromBytes (hi, lo)); } } return tags; } Helper Function: ParseWriteMessage The purpose of the ParseWriteMessage function is to determine if the write was successful. Most devices respond that the request was received and executed. In the case of Modbus, the response echoes the request, which makes it possible to compare the returned message with the sent message that was saved in the global variable SENTWRITEDATA. Below is the entire ParseWriteMessage function: function ParseWriteMessage(data) { // Modbus echoes a write request so if the data sent // does not match the data received, then the write fails SENTWRITEDATA.forEach((e1) => data.forEach((e2) => { if (e1 !== e2) { return "Fail"; } })); return "Complete"; } 把3.5以上各个代码段,整理成一段完整的代码,这段代码必须有合理性,可直接复制出来能运行的代码。
09-30
// SPDX-License-Identifier: GPL-2.0+ /* * Freescale i.MX23/i.MX28 SB image generator * * Copyright (C) 2012-2013 Marek Vasut <marex@denx.de> */ #ifdef CONFIG_MXS #include <errno.h> #include <fcntl.h> #include <stdio.h> #include <string.h> #include <u-boot/crc.h> #include <unistd.h> #include <limits.h> #include <openssl/evp.h> #include "imagetool.h" #include "mxsimage.h" #include "pbl_crc32.h" #include <image.h> /* * OpenSSL 1.1.0 and newer compatibility functions: * https://wiki.openssl.org/index.php/1.1_API_Changes */ #if OPENSSL_VERSION_NUMBER < 0x10100000L || \ (defined(LIBRESSL_VERSION_NUMBER) && LIBRESSL_VERSION_NUMBER < 0x2070000fL) static void *OPENSSL_zalloc(size_t num) { void *ret = OPENSSL_malloc(num); if (ret != NULL) memset(ret, 0, num); return ret; } EVP_MD_CTX *EVP_MD_CTX_new(void) { return OPENSSL_zalloc(sizeof(EVP_MD_CTX)); } void EVP_MD_CTX_free(EVP_MD_CTX *ctx) { EVP_MD_CTX_cleanup(ctx); OPENSSL_free(ctx); } int EVP_CIPHER_CTX_reset(EVP_CIPHER_CTX *ctx) { return EVP_CIPHER_CTX_cleanup(ctx); } #endif /* * DCD block * |-Write to address command block * | 0xf00 == 0xf33d * | 0xba2 == 0xb33f * |-ORR address with mask command block * | 0xf00 |= 0x1337 * |-Write to address command block * | 0xba2 == 0xd00d * : */ #define SB_HAB_DCD_WRITE 0xccUL #define SB_HAB_DCD_CHECK 0xcfUL #define SB_HAB_DCD_NOOP 0xc0UL #define SB_HAB_DCD_MASK_BIT (1 << 3) #define SB_HAB_DCD_SET_BIT (1 << 4) /* Addr.n = Value.n */ #define SB_DCD_WRITE \ (SB_HAB_DCD_WRITE << 24) /* Addr.n &= ~Value.n */ #define SB_DCD_ANDC \ ((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT) /* Addr.n |= Value.n */ #define SB_DCD_ORR \ ((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT) /* (Addr.n & Value.n) == 0 */ #define SB_DCD_CHK_EQZ \ (SB_HAB_DCD_CHECK << 24) /* (Addr.n & Value.n) == Value.n */ #define SB_DCD_CHK_EQ \ ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT) /* (Addr.n & Value.n) != Value.n */ #define SB_DCD_CHK_NEQ \ ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_MASK_BIT) /* (Addr.n & Value.n) != 0 */ #define SB_DCD_CHK_NEZ \ ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT) /* NOP */ #define SB_DCD_NOOP \ (SB_HAB_DCD_NOOP << 24) struct sb_dcd_ctx { struct sb_dcd_ctx *dcd; uint32_t id; /* The DCD block. */ uint32_t *payload; /* Size of the whole DCD block. */ uint32_t size; /* Pointer to previous DCD command block. */ uint32_t *prev_dcd_head; }; /* * IMAGE * |-SECTION * | |-CMD * | |-CMD * | `-CMD * |-SECTION * | |-CMD * : : */ struct sb_cmd_list { char *cmd; size_t len; unsigned int lineno; }; struct sb_cmd_ctx { uint32_t size; struct sb_cmd_ctx *cmd; uint8_t *data; uint32_t length; struct sb_command payload; struct sb_command c_payload; }; struct sb_section_ctx { uint32_t size; /* Section flags */ unsigned int boot:1; struct sb_section_ctx *sect; struct sb_cmd_ctx *cmd_head; struct sb_cmd_ctx *cmd_tail; struct sb_sections_header payload; }; struct sb_image_ctx { unsigned int in_section:1; unsigned int in_dcd:1; /* Image configuration */ unsigned int display_progress:1; unsigned int silent_dump:1; char *input_filename; char *output_filename; char *cfg_filename; uint8_t image_key[16]; /* Number of section in the image */ unsigned int sect_count; /* Bootable section */ unsigned int sect_boot; unsigned int sect_boot_found:1; struct sb_section_ctx *sect_head; struct sb_section_ctx *sect_tail; struct sb_dcd_ctx *dcd_head; struct sb_dcd_ctx *dcd_tail; EVP_CIPHER_CTX *cipher_ctx; EVP_MD_CTX *md_ctx; uint8_t digest[32]; struct sb_key_dictionary_key sb_dict_key; struct sb_boot_image_header payload; }; /* * Instruction semantics: * NOOP * TAG [LAST] * LOAD address file * LOAD IVT address IVT_entry_point * FILL address pattern length * JUMP [HAB] address [r0_arg] * CALL [HAB] address [r0_arg] * MODE mode * For i.MX23, mode = USB/I2C/SPI1_FLASH/SPI2_FLASH/NAND_BCH * JTAG/SPI3_EEPROM/SD_SSP0/SD_SSP1 * For i.MX28, mode = USB/I2C/SPI2_FLASH/SPI3_FLASH/NAND_BCH * JTAG/SPI2_EEPROM/SD_SSP0/SD_SSP1 */ /* * AES libcrypto */ static int sb_aes_init(struct sb_image_ctx *ictx, uint8_t *iv, int enc) { EVP_CIPHER_CTX *ctx; int ret; /* If there is no init vector, init vector is all zeroes. */ if (!iv) iv = ictx->image_key; ctx = EVP_CIPHER_CTX_new(); ret = EVP_CipherInit(ctx, EVP_aes_128_cbc(), ictx->image_key, iv, enc); if (ret == 1) { EVP_CIPHER_CTX_set_padding(ctx, 0); ictx->cipher_ctx = ctx; } return ret; } static int sb_aes_crypt(struct sb_image_ctx *ictx, uint8_t *in_data, uint8_t *out_data, int in_len) { EVP_CIPHER_CTX *ctx = ictx->cipher_ctx; int ret, outlen; uint8_t *outbuf; outbuf = malloc(in_len); if (!outbuf) return -ENOMEM; memset(outbuf, 0, sizeof(in_len)); ret = EVP_CipherUpdate(ctx, outbuf, &outlen, in_data, in_len); if (!ret) { ret = -EINVAL; goto err; } if (out_data) memcpy(out_data, outbuf, outlen); err: free(outbuf); return ret; } static int sb_aes_deinit(EVP_CIPHER_CTX *ctx) { return EVP_CIPHER_CTX_reset(ctx); } static int sb_aes_reinit(struct sb_image_ctx *ictx, int enc) { int ret; EVP_CIPHER_CTX *ctx = ictx->cipher_ctx; struct sb_boot_image_header *sb_header = &ictx->payload; uint8_t *iv = sb_header->iv; ret = sb_aes_deinit(ctx); if (!ret) return ret; return sb_aes_init(ictx, iv, enc); } /* * Debug */ static void soprintf(struct sb_image_ctx *ictx, const char *fmt, ...) { va_list ap; if (ictx->silent_dump) return; va_start(ap, fmt); vfprintf(stdout, fmt, ap); va_end(ap); } /* * Code */ static time_t sb_get_timestamp(void) { struct tm time_2000 = { .tm_yday = 1, /* Jan. 1st */ .tm_year = 100, /* 2000 */ }; time_t seconds_to_2000 = mktime(&time_2000); time_t seconds_to_now = time(NULL); return seconds_to_now - seconds_to_2000; } static int sb_get_time(time_t time, struct tm *tm) { struct tm time_2000 = { .tm_yday = 1, /* Jan. 1st */ .tm_year = 0, /* 1900 */ }; const time_t seconds_to_2000 = mktime(&time_2000); const time_t seconds_to_now = seconds_to_2000 + time; struct tm *ret; ret = gmtime_r(&seconds_to_now, tm); return ret ? 0 : -EINVAL; } static void sb_encrypt_sb_header(struct sb_image_ctx *ictx) { EVP_MD_CTX *md_ctx = ictx->md_ctx; struct sb_boot_image_header *sb_header = &ictx->payload; uint8_t *sb_header_ptr = (uint8_t *)sb_header; /* Encrypt the header, compute the digest. */ sb_aes_crypt(ictx, sb_header_ptr, NULL, sizeof(*sb_header)); EVP_DigestUpdate(md_ctx, sb_header_ptr, sizeof(*sb_header)); } static void sb_encrypt_sb_sections_header(struct sb_image_ctx *ictx) { EVP_MD_CTX *md_ctx = ictx->md_ctx; struct sb_section_ctx *sctx = ictx->sect_head; struct sb_sections_header *shdr; uint8_t *sb_sections_header_ptr; const int size = sizeof(*shdr); while (sctx) { shdr = &sctx->payload; sb_sections_header_ptr = (uint8_t *)shdr; sb_aes_crypt(ictx, sb_sections_header_ptr, ictx->sb_dict_key.cbc_mac, size); EVP_DigestUpdate(md_ctx, sb_sections_header_ptr, size); sctx = sctx->sect; }; } static void sb_encrypt_key_dictionary_key(struct sb_image_ctx *ictx) { EVP_MD_CTX *md_ctx = ictx->md_ctx; sb_aes_crypt(ictx, ictx->image_key, ictx->sb_dict_key.key, sizeof(ictx->sb_dict_key.key)); EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key)); } static void sb_decrypt_key_dictionary_key(struct sb_image_ctx *ictx) { EVP_MD_CTX *md_ctx = ictx->md_ctx; EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key)); sb_aes_crypt(ictx, ictx->sb_dict_key.key, ictx->image_key, sizeof(ictx->sb_dict_key.key)); } static void sb_encrypt_tag(struct sb_image_ctx *ictx, struct sb_cmd_ctx *cctx) { EVP_MD_CTX *md_ctx = ictx->md_ctx; struct sb_command *cmd = &cctx->payload; sb_aes_crypt(ictx, (uint8_t *)cmd, (uint8_t *)&cctx->c_payload, sizeof(*cmd)); EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd)); } static int sb_encrypt_image(struct sb_image_ctx *ictx) { /* Start image-wide crypto. */ ictx->md_ctx = EVP_MD_CTX_new(); EVP_DigestInit(ictx->md_ctx, EVP_sha1()); /* * SB image header. */ sb_aes_init(ictx, NULL, 1); sb_encrypt_sb_header(ictx); /* * SB sections header. */ sb_encrypt_sb_sections_header(ictx); /* * Key dictionary. */ sb_aes_reinit(ictx, 1); sb_encrypt_key_dictionary_key(ictx); /* * Section tags. */ struct sb_cmd_ctx *cctx; struct sb_command *ccmd; struct sb_section_ctx *sctx = ictx->sect_head; while (sctx) { cctx = sctx->cmd_head; sb_aes_reinit(ictx, 1); while (cctx) { ccmd = &cctx->payload; sb_encrypt_tag(ictx, cctx); if (ccmd->header.tag == ROM_TAG_CMD) { sb_aes_reinit(ictx, 1); } else if (ccmd->header.tag == ROM_LOAD_CMD) { sb_aes_crypt(ictx, cctx->data, cctx->data, cctx->length); EVP_DigestUpdate(ictx->md_ctx, cctx->data, cctx->length); } cctx = cctx->cmd; } sctx = sctx->sect; }; /* * Dump the SHA1 of the whole image. */ sb_aes_reinit(ictx, 1); EVP_DigestFinal(ictx->md_ctx, ictx->digest, NULL); EVP_MD_CTX_free(ictx->md_ctx); sb_aes_crypt(ictx, ictx->digest, ictx->digest, sizeof(ictx->digest)); /* Stop the encryption session. */ sb_aes_deinit(ictx->cipher_ctx); return 0; } static int sb_load_file(struct sb_cmd_ctx *cctx, char *filename) { long real_size, roundup_size; uint8_t *data; long ret; unsigned long size; FILE *fp; if (!filename) { fprintf(stderr, "ERR: Missing filename!\n"); return -EINVAL; } fp = fopen(filename, "r"); if (!fp) goto err_open; ret = fseek(fp, 0, SEEK_END); if (ret < 0) goto err_file; real_size = ftell(fp); if (real_size < 0) goto err_file; ret = fseek(fp, 0, SEEK_SET); if (ret < 0) goto err_file; roundup_size = roundup(real_size, SB_BLOCK_SIZE); data = calloc(1, roundup_size); if (!data) goto err_file; size = fread(data, 1, real_size, fp); if (size != (unsigned long)real_size) goto err_alloc; cctx->data = data; cctx->length = roundup_size; fclose(fp); return 0; err_alloc: free(data); err_file: fclose(fp); err_open: fprintf(stderr, "ERR: Failed to load file \"%s\"\n", filename); return -EINVAL; } static uint8_t sb_command_checksum(struct sb_command *inst) { uint8_t *inst_ptr = (uint8_t *)inst; uint8_t csum = 0; unsigned int i; for (i = 0; i < sizeof(struct sb_command); i++) csum += inst_ptr[i]; return csum; } static int sb_token_to_long(char *tok, uint32_t *rid) { char *endptr; unsigned long id; if (tok[0] != '0' || tok[1] != 'x') { fprintf(stderr, "ERR: Invalid hexadecimal number!\n"); return -EINVAL; } tok += 2; errno = 0; id = strtoul(tok, &endptr, 16); if ((errno == ERANGE && id == ULONG_MAX) || (errno != 0 && id == 0)) { fprintf(stderr, "ERR: Value can't be decoded!\n"); return -EINVAL; } /* Check for 32-bit overflow. */ if (id > 0xffffffff) { fprintf(stderr, "ERR: Value too big!\n"); return -EINVAL; } if (endptr == tok) { fprintf(stderr, "ERR: Deformed value!\n"); return -EINVAL; } *rid = (uint32_t)id; return 0; } static int sb_grow_dcd(struct sb_dcd_ctx *dctx, unsigned int inc_size) { uint32_t *tmp; if (!inc_size) return 0; dctx->size += inc_size; tmp = realloc(dctx->payload, dctx->size); if (!tmp) return -ENOMEM; dctx->payload = tmp; /* Assemble and update the HAB DCD header. */ dctx->payload[0] = htonl((SB_HAB_DCD_TAG << 24) | (dctx->size << 8) | SB_HAB_VERSION); return 0; } static int sb_build_dcd(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd) { struct sb_dcd_ctx *dctx; char *tok; uint32_t id; int ret; dctx = calloc(1, sizeof(*dctx)); if (!dctx) return -ENOMEM; ret = sb_grow_dcd(dctx, 4); if (ret) goto err_dcd; /* Read DCD block number. */ tok = strtok(cmd->cmd, " "); if (!tok) { fprintf(stderr, "#%i ERR: DCD block without number!\n", cmd->lineno); ret = -EINVAL; goto err_dcd; } /* Parse the DCD block number. */ ret = sb_token_to_long(tok, &id); if (ret) { fprintf(stderr, "#%i ERR: Malformed DCD block number!\n", cmd->lineno); goto err_dcd; } dctx->id = id; /* * The DCD block is now constructed. Append it to the list. * WARNING: The DCD size is still not computed and will be * updated while parsing it's commands. */ if (!ictx->dcd_head) { ictx->dcd_head = dctx; ictx->dcd_tail = dctx; } else { ictx->dcd_tail->dcd = dctx; ictx->dcd_tail = dctx; } return 0; err_dcd: free(dctx->payload); free(dctx); return ret; } static int sb_build_dcd_block(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd, uint32_t type) { char *tok; uint32_t address, value, length; int ret; struct sb_dcd_ctx *dctx = ictx->dcd_tail; uint32_t *dcd; if (dctx->prev_dcd_head && (type != SB_DCD_NOOP) && ((dctx->prev_dcd_head[0] & 0xff0000ff) == type)) { /* Same instruction as before, just append it. */ ret = sb_grow_dcd(dctx, 8); if (ret) return ret; } else if (type == SB_DCD_NOOP) { ret = sb_grow_dcd(dctx, 4); if (ret) return ret; /* Update DCD command block pointer. */ dctx->prev_dcd_head = dctx->payload + dctx->size / sizeof(*dctx->payload) - 1; /* NOOP has only 4 bytes and no payload. */ goto noop; } else { /* * Either a different instruction block started now * or this is the first instruction block. */ ret = sb_grow_dcd(dctx, 12); if (ret) return ret; /* Update DCD command block pointer. */ dctx->prev_dcd_head = dctx->payload + dctx->size / sizeof(*dctx->payload) - 3; } dcd = dctx->payload + dctx->size / sizeof(*dctx->payload) - 2; /* * Prepare the command. */ tok = strtok(cmd->cmd, " "); if (!tok) { fprintf(stderr, "#%i ERR: Missing DCD address!\n", cmd->lineno); ret = -EINVAL; goto err; } /* Read DCD destination address. */ ret = sb_token_to_long(tok, &address); if (ret) { fprintf(stderr, "#%i ERR: Incorrect DCD address!\n", cmd->lineno); goto err; } tok = strtok(NULL, " "); if (!tok) { fprintf(stderr, "#%i ERR: Missing DCD value!\n", cmd->lineno); ret = -EINVAL; goto err; } /* Read DCD operation value. */ ret = sb_token_to_long(tok, &value); if (ret) { fprintf(stderr, "#%i ERR: Incorrect DCD value!\n", cmd->lineno); goto err; } /* Fill in the new DCD entry. */ dcd[0] = htonl(address); dcd[1] = htonl(value); noop: /* Update the DCD command block. */ length = dctx->size - ((dctx->prev_dcd_head - dctx->payload) * sizeof(*dctx->payload)); dctx->prev_dcd_head[0] = htonl(type | (length << 8)); err: return ret; } static int sb_build_section(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd) { struct sb_section_ctx *sctx; struct sb_sections_header *shdr; char *tok; uint32_t bootable = 0; uint32_t id; int ret; sctx = calloc(1, sizeof(*sctx)); if (!sctx) return -ENOMEM; /* Read section number. */ tok = strtok(cmd->cmd, " "); if (!tok) { fprintf(stderr, "#%i ERR: Section without number!\n", cmd->lineno); ret = -EINVAL; goto err_sect; } /* Parse the section number. */ ret = sb_token_to_long(tok, &id); if (ret) { fprintf(stderr, "#%i ERR: Malformed section number!\n", cmd->lineno); goto err_sect; } /* Read section's BOOTABLE flag. */ tok = strtok(NULL, " "); if (tok && (strlen(tok) == 8) && !strncmp(tok, "BOOTABLE", 8)) bootable = SB_SECTION_FLAG_BOOTABLE; sctx->boot = bootable; shdr = &sctx->payload; shdr->section_number = id; shdr->section_flags = bootable; /* * The section is now constructed. Append it to the list. * WARNING: The section size is still not computed and will * be updated while parsing it's commands. */ ictx->sect_count++; /* Mark that this section is bootable one. */ if (bootable) { if (ictx->sect_boot_found) { fprintf(stderr, "#%i WARN: Multiple bootable section!\n", cmd->lineno); } else { ictx->sect_boot = id; ictx->sect_boot_found = 1; } } if (!ictx->sect_head) { ictx->sect_head = sctx; ictx->sect_tail = sctx; } else { ictx->sect_tail->sect = sctx; ictx->sect_tail = sctx; } return 0; err_sect: free(sctx); return ret; } static int sb_build_command_nop(struct sb_image_ctx *ictx) { struct sb_section_ctx *sctx = ictx->sect_tail; struct sb_cmd_ctx *cctx; struct sb_command *ccmd; cctx = calloc(1, sizeof(*cctx)); if (!cctx) return -ENOMEM; ccmd = &cctx->payload; /* * Construct the command. */ ccmd->header.checksum = 0x5a; ccmd->header.tag = ROM_NOP_CMD; cctx->size = sizeof(*ccmd); /* * Append the command to the last section. */ if (!sctx->cmd_head) { sctx->cmd_head = cctx; sctx->cmd_tail = cctx; } else { sctx->cmd_tail->cmd = cctx; sctx->cmd_tail = cctx; } return 0; } static int sb_build_command_tag(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd) { struct sb_section_ctx *sctx = ictx->sect_tail; struct sb_cmd_ctx *cctx; struct sb_command *ccmd; char *tok; cctx = calloc(1, sizeof(*cctx)); if (!cctx) return -ENOMEM; ccmd = &cctx->payload; /* * Prepare the command. */ /* Check for the LAST keyword. */ tok = strtok(cmd->cmd, " "); if (tok && !strcmp(tok, "LAST")) ccmd->header.flags = ROM_TAG_CMD_FLAG_ROM_LAST_TAG; /* * Construct the command. */ ccmd->header.checksum = 0x5a; ccmd->header.tag = ROM_TAG_CMD; cctx->size = sizeof(*ccmd); /* * Append the command to the last section. */ if (!sctx->cmd_head) { sctx->cmd_head = cctx; sctx->cmd_tail = cctx; } else { sctx->cmd_tail->cmd = cctx; sctx->cmd_tail = cctx; } return 0; } static int sb_build_command_load(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd) { struct sb_section_ctx *sctx = ictx->sect_tail; struct sb_cmd_ctx *cctx; struct sb_command *ccmd; char *tok; int ret, is_ivt = 0, is_dcd = 0; uint32_t dest, dcd = 0; cctx = calloc(1, sizeof(*cctx)); if (!cctx) return -ENOMEM; ccmd = &cctx->payload; /* * Prepare the command. */ tok = strtok(cmd->cmd, " "); if (!tok) { fprintf(stderr, "#%i ERR: Missing LOAD address or 'IVT'!\n", cmd->lineno); ret = -EINVAL; goto err; } /* Check for "IVT" flag. */ if (!strcmp(tok, "IVT")) is_ivt = 1; if (!strcmp(tok, "DCD")) is_dcd = 1; if (is_ivt || is_dcd) { tok = strtok(NULL, " "); if (!tok) { fprintf(stderr, "#%i ERR: Missing LOAD address!\n", cmd->lineno); ret = -EINVAL; goto err; } } /* Read load destination address. */ ret = sb_token_to_long(tok, &dest); if (ret) { fprintf(stderr, "#%i ERR: Incorrect LOAD address!\n", cmd->lineno); goto err; } /* Read filename or IVT entrypoint or DCD block ID. */ tok = strtok(NULL, " "); if (!tok) { fprintf(stderr, "#%i ERR: Missing LOAD filename or IVT ep or DCD block ID!\n", cmd->lineno); ret = -EINVAL; goto err; } if (is_ivt) { /* Handle IVT. */ struct sb_ivt_header *ivt; uint32_t ivtep; ret = sb_token_to_long(tok, &ivtep); if (ret) { fprintf(stderr, "#%i ERR: Incorrect IVT entry point!\n", cmd->lineno); goto err; } ivt = calloc(1, sizeof(*ivt)); if (!ivt) { ret = -ENOMEM; goto err; } ivt->header = sb_hab_ivt_header(); ivt->entry = ivtep; ivt->self = dest; cctx->data = (uint8_t *)ivt; cctx->length = sizeof(*ivt); } else if (is_dcd) { struct sb_dcd_ctx *dctx = ictx->dcd_head; uint32_t dcdid; uint8_t *payload; uint32_t asize; ret = sb_token_to_long(tok, &dcdid); if (ret) { fprintf(stderr, "#%i ERR: Incorrect DCD block ID!\n", cmd->lineno); goto err; } while (dctx) { if (dctx->id == dcdid) break; dctx = dctx->dcd; } if (!dctx) { fprintf(stderr, "#%i ERR: DCD block %08x not found!\n", cmd->lineno, dcdid); goto err; } asize = roundup(dctx->size, SB_BLOCK_SIZE); payload = calloc(1, asize); if (!payload) { ret = -ENOMEM; goto err; } memcpy(payload, dctx->payload, dctx->size); cctx->data = payload; cctx->length = asize; /* Set the Load DCD flag. */ dcd = ROM_LOAD_CMD_FLAG_DCD_LOAD; } else { /* Regular LOAD of a file. */ ret = sb_load_file(cctx, tok); if (ret) { fprintf(stderr, "#%i ERR: Cannot load '%s'!\n", cmd->lineno, tok); goto err; } } if (cctx->length & (SB_BLOCK_SIZE - 1)) { fprintf(stderr, "#%i ERR: Unaligned payload!\n", cmd->lineno); } /* * Construct the command. */ ccmd->header.checksum = 0x5a; ccmd->header.tag = ROM_LOAD_CMD; ccmd->header.flags = dcd; ccmd->load.address = dest; ccmd->load.count = cctx->length; ccmd->load.crc32 = pbl_crc32(0, (const char *)cctx->data, cctx->length); cctx->size = sizeof(*ccmd) + cctx->length; /* * Append the command to the last section. */ if (!sctx->cmd_head) { sctx->cmd_head = cctx; sctx->cmd_tail = cctx; } else { sctx->cmd_tail->cmd = cctx; sctx->cmd_tail = cctx; } return 0; err: free(cctx); return ret; } static int sb_build_command_fill(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd) { struct sb_section_ctx *sctx = ictx->sect_tail; struct sb_cmd_ctx *cctx; struct sb_command *ccmd; char *tok; uint32_t address, pattern, length; int ret; cctx = calloc(1, sizeof(*cctx)); if (!cctx) return -ENOMEM; ccmd = &cctx->payload; /* * Prepare the command. */ tok = strtok(cmd->cmd, " "); if (!tok) { fprintf(stderr, "#%i ERR: Missing FILL address!\n", cmd->lineno); ret = -EINVAL; goto err; } /* Read fill destination address. */ ret = sb_token_to_long(tok, &address); if (ret) { fprintf(stderr, "#%i ERR: Incorrect FILL address!\n", cmd->lineno); goto err; } tok = strtok(NULL, " "); if (!tok) { fprintf(stderr, "#%i ERR: Missing FILL pattern!\n", cmd->lineno); ret = -EINVAL; goto err; } /* Read fill pattern address. */ ret = sb_token_to_long(tok, &pattern); if (ret) { fprintf(stderr, "#%i ERR: Incorrect FILL pattern!\n", cmd->lineno); goto err; } tok = strtok(NULL, " "); if (!tok) { fprintf(stderr, "#%i ERR: Missing FILL length!\n", cmd->lineno); ret = -EINVAL; goto err; } /* Read fill pattern address. */ ret = sb_token_to_long(tok, &length); if (ret) { fprintf(stderr, "#%i ERR: Incorrect FILL length!\n", cmd->lineno); goto err; } /* * Construct the command. */ ccmd->header.checksum = 0x5a; ccmd->header.tag = ROM_FILL_CMD; ccmd->fill.address = address; ccmd->fill.count = length; ccmd->fill.pattern = pattern; cctx->size = sizeof(*ccmd); /* * Append the command to the last section. */ if (!sctx->cmd_head) { sctx->cmd_head = cctx; sctx->cmd_tail = cctx; } else { sctx->cmd_tail->cmd = cctx; sctx->cmd_tail = cctx; } return 0; err: free(cctx); return ret; } static int sb_build_command_jump_call(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd, unsigned int is_call) { struct sb_section_ctx *sctx = ictx->sect_tail; struct sb_cmd_ctx *cctx; struct sb_command *ccmd; char *tok; uint32_t dest, arg = 0x0; uint32_t hab = 0; int ret; const char *cmdname = is_call ? "CALL" : "JUMP"; cctx = calloc(1, sizeof(*cctx)); if (!cctx) return -ENOMEM; ccmd = &cctx->payload; /* * Prepare the command. */ tok = strtok(cmd->cmd, " "); if (!tok) { fprintf(stderr, "#%i ERR: Missing %s address or 'HAB'!\n", cmd->lineno, cmdname); ret = -EINVAL; goto err; } /* Check for "HAB" flag. */ if (!strcmp(tok, "HAB")) { hab = is_call ? ROM_CALL_CMD_FLAG_HAB : ROM_JUMP_CMD_FLAG_HAB; tok = strtok(NULL, " "); if (!tok) { fprintf(stderr, "#%i ERR: Missing %s address!\n", cmd->lineno, cmdname); ret = -EINVAL; goto err; } } /* Read load destination address. */ ret = sb_token_to_long(tok, &dest); if (ret) { fprintf(stderr, "#%i ERR: Incorrect %s address!\n", cmd->lineno, cmdname); goto err; } tok = strtok(NULL, " "); if (tok) { ret = sb_token_to_long(tok, &arg); if (ret) { fprintf(stderr, "#%i ERR: Incorrect %s argument!\n", cmd->lineno, cmdname); goto err; } } /* * Construct the command. */ ccmd->header.checksum = 0x5a; ccmd->header.tag = is_call ? ROM_CALL_CMD : ROM_JUMP_CMD; ccmd->header.flags = hab; ccmd->call.address = dest; ccmd->call.argument = arg; cctx->size = sizeof(*ccmd); /* * Append the command to the last section. */ if (!sctx->cmd_head) { sctx->cmd_head = cctx; sctx->cmd_tail = cctx; } else { sctx->cmd_tail->cmd = cctx; sctx->cmd_tail = cctx; } return 0; err: free(cctx); return ret; } static int sb_build_command_jump(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd) { return sb_build_command_jump_call(ictx, cmd, 0); } static int sb_build_command_call(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd) { return sb_build_command_jump_call(ictx, cmd, 1); } static int sb_build_command_mode(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd) { struct sb_section_ctx *sctx = ictx->sect_tail; struct sb_cmd_ctx *cctx; struct sb_command *ccmd; char *tok; int ret; unsigned int i; uint32_t mode = 0xffffffff; cctx = calloc(1, sizeof(*cctx)); if (!cctx) return -ENOMEM; ccmd = &cctx->payload; /* * Prepare the command. */ tok = strtok(cmd->cmd, " "); if (!tok) { fprintf(stderr, "#%i ERR: Missing MODE boot mode argument!\n", cmd->lineno); ret = -EINVAL; goto err; } for (i = 0; i < ARRAY_SIZE(modetable); i++) { if (!strcmp(tok, modetable[i].name)) { mode = modetable[i].mode; break; } if (!modetable[i].altname) continue; if (!strcmp(tok, modetable[i].altname)) { mode = modetable[i].mode; break; } } if (mode == 0xffffffff) { fprintf(stderr, "#%i ERR: Invalid MODE boot mode argument!\n", cmd->lineno); ret = -EINVAL; goto err; } /* * Construct the command. */ ccmd->header.checksum = 0x5a; ccmd->header.tag = ROM_MODE_CMD; ccmd->mode.mode = mode; cctx->size = sizeof(*ccmd); /* * Append the command to the last section. */ if (!sctx->cmd_head) { sctx->cmd_head = cctx; sctx->cmd_tail = cctx; } else { sctx->cmd_tail->cmd = cctx; sctx->cmd_tail = cctx; } return 0; err: free(cctx); return ret; } static int sb_prefill_image_header(struct sb_image_ctx *ictx) { struct sb_boot_image_header *hdr = &ictx->payload; /* Fill signatures */ memcpy(hdr->signature1, "STMP", 4); memcpy(hdr->signature2, "sgtl", 4); /* SB Image version 1.1 */ hdr->major_version = SB_VERSION_MAJOR; hdr->minor_version = SB_VERSION_MINOR; /* Boot image major version */ hdr->product_version.major = htons(0x999); hdr->product_version.minor = htons(0x999); hdr->product_version.revision = htons(0x999); /* Boot image major version */ hdr->component_version.major = htons(0x999); hdr->component_version.minor = htons(0x999); hdr->component_version.revision = htons(0x999); /* Drive tag must be 0x0 for i.MX23 */ hdr->drive_tag = 0; hdr->header_blocks = sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE; hdr->section_header_size = sizeof(struct sb_sections_header) / SB_BLOCK_SIZE; hdr->timestamp_us = sb_get_timestamp() * 1000000; hdr->flags = ictx->display_progress ? SB_IMAGE_FLAG_DISPLAY_PROGRESS : 0; /* FIXME -- We support only default key */ hdr->key_count = 1; return 0; } static int sb_postfill_image_header(struct sb_image_ctx *ictx) { struct sb_boot_image_header *hdr = &ictx->payload; struct sb_section_ctx *sctx = ictx->sect_head; uint32_t kd_size, sections_blocks; EVP_MD_CTX *md_ctx; /* The main SB header size in blocks. */ hdr->image_blocks = hdr->header_blocks; /* Size of the key dictionary, which has single zero entry. */ kd_size = hdr->key_count * sizeof(struct sb_key_dictionary_key); hdr->image_blocks += kd_size / SB_BLOCK_SIZE; /* Now count the payloads. */ hdr->section_count = ictx->sect_count; while (sctx) { hdr->image_blocks += sctx->size / SB_BLOCK_SIZE; sctx = sctx->sect; } if (!ictx->sect_boot_found) { fprintf(stderr, "ERR: No bootable section selected!\n"); return -EINVAL; } hdr->first_boot_section_id = ictx->sect_boot; /* The n * SB section size in blocks. */ sections_blocks = hdr->section_count * hdr->section_header_size; hdr->image_blocks += sections_blocks; /* Key dictionary offset. */ hdr->key_dictionary_block = hdr->header_blocks + sections_blocks; /* Digest of the whole image. */ hdr->image_blocks += 2; /* Pointer past the dictionary. */ hdr->first_boot_tag_block = hdr->key_dictionary_block + kd_size / SB_BLOCK_SIZE; /* Compute header digest. */ md_ctx = EVP_MD_CTX_new(); EVP_DigestInit(md_ctx, EVP_sha1()); EVP_DigestUpdate(md_ctx, hdr->signature1, sizeof(struct sb_boot_image_header) - sizeof(hdr->digest)); EVP_DigestFinal(md_ctx, hdr->digest, NULL); EVP_MD_CTX_free(md_ctx); return 0; } static int sb_fixup_sections_and_tags(struct sb_image_ctx *ictx) { /* Fixup the placement of sections. */ struct sb_boot_image_header *ihdr = &ictx->payload; struct sb_section_ctx *sctx = ictx->sect_head; struct sb_sections_header *shdr; struct sb_cmd_ctx *cctx; struct sb_command *ccmd; uint32_t offset = ihdr->first_boot_tag_block; while (sctx) { shdr = &sctx->payload; /* Fill in the section TAG offset. */ shdr->section_offset = offset + 1; offset += shdr->section_size; /* Section length is measured from the TAG block. */ shdr->section_size--; /* Fixup the TAG command. */ cctx = sctx->cmd_head; while (cctx) { ccmd = &cctx->payload; if (ccmd->header.tag == ROM_TAG_CMD) { ccmd->tag.section_number = shdr->section_number; ccmd->tag.section_length = shdr->section_size; ccmd->tag.section_flags = shdr->section_flags; } /* Update the command checksum. */ ccmd->header.checksum = sb_command_checksum(ccmd); cctx = cctx->cmd; } sctx = sctx->sect; } return 0; } static int sb_parse_line(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd) { char *tok; char *line = cmd->cmd; char *rptr = NULL; int ret; /* Analyze the identifier on this line first. */ tok = strtok_r(line, " ", &rptr); if (!tok || (strlen(tok) == 0)) { fprintf(stderr, "#%i ERR: Invalid line!\n", cmd->lineno); return -EINVAL; } cmd->cmd = rptr; /* set DISPLAY_PROGRESS flag */ if (!strcmp(tok, "DISPLAYPROGRESS")) { ictx->display_progress = 1; return 0; } /* DCD */ if (!strcmp(tok, "DCD")) { ictx->in_section = 0; ictx->in_dcd = 1; sb_build_dcd(ictx, cmd); return 0; } /* Section */ if (!strcmp(tok, "SECTION")) { ictx->in_section = 1; ictx->in_dcd = 0; sb_build_section(ictx, cmd); return 0; } if (!ictx->in_section && !ictx->in_dcd) { fprintf(stderr, "#%i ERR: Data outside of a section!\n", cmd->lineno); return -EINVAL; } if (ictx->in_section) { /* Section commands */ if (!strcmp(tok, "NOP")) { ret = sb_build_command_nop(ictx); } else if (!strcmp(tok, "TAG")) { ret = sb_build_command_tag(ictx, cmd); } else if (!strcmp(tok, "LOAD")) { ret = sb_build_command_load(ictx, cmd); } else if (!strcmp(tok, "FILL")) { ret = sb_build_command_fill(ictx, cmd); } else if (!strcmp(tok, "JUMP")) { ret = sb_build_command_jump(ictx, cmd); } else if (!strcmp(tok, "CALL")) { ret = sb_build_command_call(ictx, cmd); } else if (!strcmp(tok, "MODE")) { ret = sb_build_command_mode(ictx, cmd); } else { fprintf(stderr, "#%i ERR: Unsupported instruction '%s'!\n", cmd->lineno, tok); return -ENOTSUP; } } else if (ictx->in_dcd) { char *lptr; uint32_t ilen = '1'; tok = strtok_r(tok, ".", &lptr); if (!tok || (strlen(tok) == 0) || (lptr && strlen(lptr) != 1)) { fprintf(stderr, "#%i ERR: Invalid line!\n", cmd->lineno); return -EINVAL; } if (lptr && (lptr[0] != '1' && lptr[0] != '2' && lptr[0] != '4')) { fprintf(stderr, "#%i ERR: Invalid instruction width!\n", cmd->lineno); return -EINVAL; } if (lptr) ilen = lptr[0] - '1'; /* DCD commands */ if (!strcmp(tok, "WRITE")) { ret = sb_build_dcd_block(ictx, cmd, SB_DCD_WRITE | ilen); } else if (!strcmp(tok, "ANDC")) { ret = sb_build_dcd_block(ictx, cmd, SB_DCD_ANDC | ilen); } else if (!strcmp(tok, "ORR")) { ret = sb_build_dcd_block(ictx, cmd, SB_DCD_ORR | ilen); } else if (!strcmp(tok, "EQZ")) { ret = sb_build_dcd_block(ictx, cmd, SB_DCD_CHK_EQZ | ilen); } else if (!strcmp(tok, "EQ")) { ret = sb_build_dcd_block(ictx, cmd, SB_DCD_CHK_EQ | ilen); } else if (!strcmp(tok, "NEQ")) { ret = sb_build_dcd_block(ictx, cmd, SB_DCD_CHK_NEQ | ilen); } else if (!strcmp(tok, "NEZ")) { ret = sb_build_dcd_block(ictx, cmd, SB_DCD_CHK_NEZ | ilen); } else if (!strcmp(tok, "NOOP")) { ret = sb_build_dcd_block(ictx, cmd, SB_DCD_NOOP); } else { fprintf(stderr, "#%i ERR: Unsupported instruction '%s'!\n", cmd->lineno, tok); return -ENOTSUP; } } else { fprintf(stderr, "#%i ERR: Unsupported instruction '%s'!\n", cmd->lineno, tok); return -ENOTSUP; } /* * Here we have at least one section with one command, otherwise we * would have failed already higher above. * * FIXME -- should the updating happen here ? */ if (ictx->in_section && !ret) { ictx->sect_tail->size += ictx->sect_tail->cmd_tail->size; ictx->sect_tail->payload.section_size = ictx->sect_tail->size / SB_BLOCK_SIZE; } return ret; } static int sb_load_cmdfile(struct sb_image_ctx *ictx) { struct sb_cmd_list cmd; int lineno = 1; FILE *fp; char *line = NULL; ssize_t rlen; size_t len; fp = fopen(ictx->cfg_filename, "r"); if (!fp) goto err_file; while ((rlen = getline(&line, &len, fp)) > 0) { memset(&cmd, 0, sizeof(cmd)); /* Strip the trailing newline. */ line[rlen - 1] = '\0'; cmd.cmd = line; cmd.len = rlen; cmd.lineno = lineno++; sb_parse_line(ictx, &cmd); } free(line); fclose(fp); return 0; err_file: fclose(fp); fprintf(stderr, "ERR: Failed to load file \"%s\"\n", ictx->cfg_filename); return -EINVAL; } static int sb_build_tree_from_cfg(struct sb_image_ctx *ictx) { int ret; ret = sb_load_cmdfile(ictx); if (ret) return ret; ret = sb_prefill_image_header(ictx); if (ret) return ret; ret = sb_postfill_image_header(ictx); if (ret) return ret; ret = sb_fixup_sections_and_tags(ictx); if (ret) return ret; return 0; } static int sb_verify_image_header(struct sb_image_ctx *ictx, FILE *fp, long fsize) { /* Verify static fields in the image header. */ struct sb_boot_image_header *hdr = &ictx->payload; const char *stat[2] = { "[PASS]", "[FAIL]" }; struct tm tm; int sz, ret = 0; unsigned char digest[20]; EVP_MD_CTX *md_ctx; unsigned long size; /* Start image-wide crypto. */ ictx->md_ctx = EVP_MD_CTX_new(); EVP_DigestInit(ictx->md_ctx, EVP_sha1()); soprintf(ictx, "---------- Verifying SB Image Header ----------\n"); size = fread(&ictx->payload, 1, sizeof(ictx->payload), fp); if (size != sizeof(ictx->payload)) { fprintf(stderr, "ERR: SB image header too short!\n"); return -EINVAL; } /* Compute header digest. */ md_ctx = EVP_MD_CTX_new(); EVP_DigestInit(md_ctx, EVP_sha1()); EVP_DigestUpdate(md_ctx, hdr->signature1, sizeof(struct sb_boot_image_header) - sizeof(hdr->digest)); EVP_DigestFinal(md_ctx, digest, NULL); EVP_MD_CTX_free(md_ctx); sb_aes_init(ictx, NULL, 1); sb_encrypt_sb_header(ictx); if (memcmp(digest, hdr->digest, 20)) ret = -EINVAL; soprintf(ictx, "%s Image header checksum: %s\n", stat[!!ret], ret ? "BAD" : "OK"); if (ret) return ret; if (memcmp(hdr->signature1, "STMP", 4) || memcmp(hdr->signature2, "sgtl", 4)) ret = -EINVAL; soprintf(ictx, "%s Signatures: '%.4s' '%.4s'\n", stat[!!ret], hdr->signature1, hdr->signature2); if (ret) return ret; if ((hdr->major_version != SB_VERSION_MAJOR) || ((hdr->minor_version != 1) && (hdr->minor_version != 2))) ret = -EINVAL; soprintf(ictx, "%s Image version: v%i.%i\n", stat[!!ret], hdr->major_version, hdr->minor_version); if (ret) return ret; ret = sb_get_time(hdr->timestamp_us / 1000000, &tm); soprintf(ictx, "%s Creation time: %02i:%02i:%02i %02i/%02i/%04i\n", stat[!!ret], tm.tm_hour, tm.tm_min, tm.tm_sec, tm.tm_mday, tm.tm_mon, tm.tm_year + 2000); if (ret) return ret; soprintf(ictx, "%s Product version: %x.%x.%x\n", stat[0], ntohs(hdr->product_version.major), ntohs(hdr->product_version.minor), ntohs(hdr->product_version.revision)); soprintf(ictx, "%s Component version: %x.%x.%x\n", stat[0], ntohs(hdr->component_version.major), ntohs(hdr->component_version.minor), ntohs(hdr->component_version.revision)); if (hdr->flags & ~SB_IMAGE_FLAGS_MASK) ret = -EINVAL; soprintf(ictx, "%s Image flags: %s\n", stat[!!ret], hdr->flags & SB_IMAGE_FLAG_DISPLAY_PROGRESS ? "Display_progress" : ""); if (ret) return ret; if (hdr->drive_tag != 0) ret = -EINVAL; soprintf(ictx, "%s Drive tag: %i\n", stat[!!ret], hdr->drive_tag); if (ret) return ret; sz = sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE; if (hdr->header_blocks != sz) ret = -EINVAL; soprintf(ictx, "%s Image header size (blocks): %i\n", stat[!!ret], hdr->header_blocks); if (ret) return ret; sz = sizeof(struct sb_sections_header) / SB_BLOCK_SIZE; if (hdr->section_header_size != sz) ret = -EINVAL; soprintf(ictx, "%s Section header size (blocks): %i\n", stat[!!ret], hdr->section_header_size); if (ret) return ret; soprintf(ictx, "%s Sections count: %i\n", stat[!!ret], hdr->section_count); soprintf(ictx, "%s First bootable section %i\n", stat[!!ret], hdr->first_boot_section_id); if (hdr->image_blocks != fsize / SB_BLOCK_SIZE) ret = -EINVAL; soprintf(ictx, "%s Image size (blocks): %i\n", stat[!!ret], hdr->image_blocks); if (ret) return ret; sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count; if (hdr->key_dictionary_block != sz) ret = -EINVAL; soprintf(ictx, "%s Key dict offset (blocks): %i\n", stat[!!ret], hdr->key_dictionary_block); if (ret) return ret; if (hdr->key_count != 1) ret = -EINVAL; soprintf(ictx, "%s Number of encryption keys: %i\n", stat[!!ret], hdr->key_count); if (ret) return ret; sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count; sz += hdr->key_count * sizeof(struct sb_key_dictionary_key) / SB_BLOCK_SIZE; if (hdr->first_boot_tag_block != (unsigned)sz) ret = -EINVAL; soprintf(ictx, "%s First TAG block (blocks): %i\n", stat[!!ret], hdr->first_boot_tag_block); if (ret) return ret; return 0; } static void sb_decrypt_tag(struct sb_image_ctx *ictx, struct sb_cmd_ctx *cctx) { EVP_MD_CTX *md_ctx = ictx->md_ctx; struct sb_command *cmd = &cctx->payload; sb_aes_crypt(ictx, (uint8_t *)&cctx->c_payload, (uint8_t *)&cctx->payload, sizeof(*cmd)); EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd)); } static int sb_verify_command(struct sb_image_ctx *ictx, struct sb_cmd_ctx *cctx, FILE *fp, unsigned long *tsize) { struct sb_command *ccmd = &cctx->payload; unsigned long size, asize; char *csum, *flag = ""; int ret; unsigned int i; uint8_t csn, csc = ccmd->header.checksum; ccmd->header.checksum = 0x5a; csn = sb_command_checksum(ccmd); ccmd->header.checksum = csc; if (csc == csn) ret = 0; else ret = -EINVAL; csum = ret ? "checksum BAD" : "checksum OK"; switch (ccmd->header.tag) { case ROM_NOP_CMD: soprintf(ictx, " NOOP # %s\n", csum); return ret; case ROM_TAG_CMD: if (ccmd->header.flags & ROM_TAG_CMD_FLAG_ROM_LAST_TAG) flag = "LAST"; soprintf(ictx, " TAG %s # %s\n", flag, csum); sb_aes_reinit(ictx, 0); return ret; case ROM_LOAD_CMD: soprintf(ictx, " LOAD addr=0x%08x length=0x%08x # %s\n", ccmd->load.address, ccmd->load.count, csum); cctx->length = ccmd->load.count; asize = roundup(cctx->length, SB_BLOCK_SIZE); cctx->data = malloc(asize); if (!cctx->data) return -ENOMEM; size = fread(cctx->data, 1, asize, fp); if (size != asize) { fprintf(stderr, "ERR: SB LOAD command payload too short!\n"); return -EINVAL; } *tsize += size; EVP_DigestUpdate(ictx->md_ctx, cctx->data, asize); sb_aes_crypt(ictx, cctx->data, cctx->data, asize); if (ccmd->load.crc32 != pbl_crc32(0, (const char *)cctx->data, asize)) { fprintf(stderr, "ERR: SB LOAD command payload CRC32 invalid!\n"); return -EINVAL; } return 0; case ROM_FILL_CMD: soprintf(ictx, " FILL addr=0x%08x length=0x%08x pattern=0x%08x # %s\n", ccmd->fill.address, ccmd->fill.count, ccmd->fill.pattern, csum); return 0; case ROM_JUMP_CMD: if (ccmd->header.flags & ROM_JUMP_CMD_FLAG_HAB) flag = " HAB"; soprintf(ictx, " JUMP%s addr=0x%08x r0_arg=0x%08x # %s\n", flag, ccmd->fill.address, ccmd->jump.argument, csum); return 0; case ROM_CALL_CMD: if (ccmd->header.flags & ROM_CALL_CMD_FLAG_HAB) flag = " HAB"; soprintf(ictx, " CALL%s addr=0x%08x r0_arg=0x%08x # %s\n", flag, ccmd->fill.address, ccmd->jump.argument, csum); return 0; case ROM_MODE_CMD: for (i = 0; i < ARRAY_SIZE(modetable); i++) { if (ccmd->mode.mode == modetable[i].mode) { soprintf(ictx, " MODE %s # %s\n", modetable[i].name, csum); break; } } fprintf(stderr, " MODE !INVALID! # %s\n", csum); return 0; } return ret; } static int sb_verify_commands(struct sb_image_ctx *ictx, struct sb_section_ctx *sctx, FILE *fp) { unsigned long size, tsize = 0; struct sb_cmd_ctx *cctx; int ret; sb_aes_reinit(ictx, 0); while (tsize < sctx->size) { cctx = calloc(1, sizeof(*cctx)); if (!cctx) return -ENOMEM; if (!sctx->cmd_head) { sctx->cmd_head = cctx; sctx->cmd_tail = cctx; } else { sctx->cmd_tail->cmd = cctx; sctx->cmd_tail = cctx; } size = fread(&cctx->c_payload, 1, sizeof(cctx->c_payload), fp); if (size != sizeof(cctx->c_payload)) { fprintf(stderr, "ERR: SB command header too short!\n"); return -EINVAL; } tsize += size; sb_decrypt_tag(ictx, cctx); ret = sb_verify_command(ictx, cctx, fp, &tsize); if (ret) return -EINVAL; } return 0; } static int sb_verify_sections_cmds(struct sb_image_ctx *ictx, FILE *fp) { struct sb_boot_image_header *hdr = &ictx->payload; struct sb_sections_header *shdr; unsigned int i; int ret; struct sb_section_ctx *sctx; unsigned long size; char *bootable = ""; soprintf(ictx, "----- Verifying SB Sections and Commands -----\n"); for (i = 0; i < hdr->section_count; i++) { sctx = calloc(1, sizeof(*sctx)); if (!sctx) return -ENOMEM; if (!ictx->sect_head) { ictx->sect_head = sctx; ictx->sect_tail = sctx; } else { ictx->sect_tail->sect = sctx; ictx->sect_tail = sctx; } size = fread(&sctx->payload, 1, sizeof(sctx->payload), fp); if (size != sizeof(sctx->payload)) { fprintf(stderr, "ERR: SB section header too short!\n"); return -EINVAL; } } size = fread(&ictx->sb_dict_key, 1, sizeof(ictx->sb_dict_key), fp); if (size != sizeof(ictx->sb_dict_key)) { fprintf(stderr, "ERR: SB key dictionary too short!\n"); return -EINVAL; } sb_encrypt_sb_sections_header(ictx); sb_aes_reinit(ictx, 0); sb_decrypt_key_dictionary_key(ictx); sb_aes_reinit(ictx, 0); sctx = ictx->sect_head; while (sctx) { shdr = &sctx->payload; if (shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) { sctx->boot = 1; bootable = " BOOTABLE"; } sctx->size = (shdr->section_size * SB_BLOCK_SIZE) + sizeof(struct sb_command); soprintf(ictx, "SECTION 0x%x%s # size = %i bytes\n", shdr->section_number, bootable, sctx->size); if (shdr->section_flags & ~SB_SECTION_FLAG_BOOTABLE) fprintf(stderr, " WARN: Unknown section flag(s) %08x\n", shdr->section_flags); if ((shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) && (hdr->first_boot_section_id != shdr->section_number)) { fprintf(stderr, " WARN: Bootable section does ID not match image header ID!\n"); } ret = sb_verify_commands(ictx, sctx, fp); if (ret) return ret; sctx = sctx->sect; } /* * FIXME IDEA: * check if the first TAG command is at sctx->section_offset */ return 0; } static int sb_verify_image_end(struct sb_image_ctx *ictx, FILE *fp, off_t filesz) { uint8_t digest[32]; unsigned long size; off_t pos; int ret; soprintf(ictx, "------------- Verifying image end -------------\n"); size = fread(digest, 1, sizeof(digest), fp); if (size != sizeof(digest)) { fprintf(stderr, "ERR: SB key dictionary too short!\n"); return -EINVAL; } pos = ftell(fp); if (pos != filesz) { fprintf(stderr, "ERR: Trailing data past the image!\n"); return -EINVAL; } /* Check the image digest. */ EVP_DigestFinal(ictx->md_ctx, ictx->digest, NULL); EVP_MD_CTX_free(ictx->md_ctx); /* Decrypt the image digest from the input image. */ sb_aes_reinit(ictx, 0); sb_aes_crypt(ictx, digest, digest, sizeof(digest)); /* Check all of 20 bytes of the SHA1 hash. */ ret = memcmp(digest, ictx->digest, 20) ? -EINVAL : 0; if (ret) soprintf(ictx, "[FAIL] Full-image checksum: BAD\n"); else soprintf(ictx, "[PASS] Full-image checksum: OK\n"); return ret; } static int sb_build_tree_from_img(struct sb_image_ctx *ictx) { long filesize; int ret; FILE *fp; if (!ictx->input_filename) { fprintf(stderr, "ERR: Missing filename!\n"); return -EINVAL; } fp = fopen(ictx->input_filename, "r"); if (!fp) goto err_open; ret = fseek(fp, 0, SEEK_END); if (ret < 0) goto err_file; filesize = ftell(fp); if (filesize < 0) goto err_file; ret = fseek(fp, 0, SEEK_SET); if (ret < 0) goto err_file; if (filesize < (signed)sizeof(ictx->payload)) { fprintf(stderr, "ERR: File too short!\n"); goto err_file; } if (filesize & (SB_BLOCK_SIZE - 1)) { fprintf(stderr, "ERR: The file is not aligned!\n"); goto err_file; } /* Load and verify image header */ ret = sb_verify_image_header(ictx, fp, filesize); if (ret) goto err_verify; /* Load and verify sections and commands */ ret = sb_verify_sections_cmds(ictx, fp); if (ret) goto err_verify; ret = sb_verify_image_end(ictx, fp, filesize); if (ret) goto err_verify; ret = 0; err_verify: soprintf(ictx, "-------------------- Result -------------------\n"); soprintf(ictx, "Verification %s\n", ret ? "FAILED" : "PASSED"); /* Stop the encryption session. */ sb_aes_deinit(ictx->cipher_ctx); fclose(fp); return ret; err_file: fclose(fp); err_open: fprintf(stderr, "ERR: Failed to load file \"%s\"\n", ictx->input_filename); return -EINVAL; } static void sb_free_image(struct sb_image_ctx *ictx) { struct sb_section_ctx *sctx = ictx->sect_head, *s_head; struct sb_dcd_ctx *dctx = ictx->dcd_head, *d_head; struct sb_cmd_ctx *cctx, *c_head; while (sctx) { s_head = sctx; c_head = sctx->cmd_head; while (c_head) { cctx = c_head; c_head = c_head->cmd; if (cctx->data) free(cctx->data); free(cctx); } sctx = sctx->sect; free(s_head); } while (dctx) { d_head = dctx; dctx = dctx->dcd; free(d_head->payload); free(d_head); } } /* * MXSSB-MKIMAGE glue code. */ static int mxsimage_check_image_types(uint8_t type) { if (type == IH_TYPE_MXSIMAGE) return EXIT_SUCCESS; else return EXIT_FAILURE; } static void mxsimage_set_header(void *ptr, struct stat *sbuf, int ifd, struct image_tool_params *params) { } int mxsimage_check_params(struct image_tool_params *params) { if (!params) return -1; if (!strlen(params->imagename)) { fprintf(stderr, "Error: %s - Configuration file not specified, it is needed for mxsimage generation\n", params->cmdname); return -1; } /* * Check parameters: * XIP is not allowed and verify that incompatible * parameters are not sent at the same time * For example, if list is required a data image must not be provided */ return (params->dflag && (params->fflag || params->lflag)) || (params->fflag && (params->dflag || params->lflag)) || (params->lflag && (params->dflag || params->fflag)) || (params->xflag) || !(strlen(params->imagename)); } static int mxsimage_verify_print_header(char *file, int silent) { int ret; struct sb_image_ctx ctx; memset(&ctx, 0, sizeof(ctx)); ctx.input_filename = file; ctx.silent_dump = silent; ret = sb_build_tree_from_img(&ctx); sb_free_image(&ctx); return ret; } char *imagefile; static int mxsimage_verify_header(unsigned char *ptr, int image_size, struct image_tool_params *params) { struct sb_boot_image_header *hdr; if (!ptr) return -EINVAL; hdr = (struct sb_boot_image_header *)ptr; /* * Check if the header contains the MXS image signatures, * if so, do a full-image verification. */ if (memcmp(hdr->signature1, "STMP", 4) || memcmp(hdr->signature2, "sgtl", 4)) return -EINVAL; imagefile = params->imagefile; return mxsimage_verify_print_header(params->imagefile, 1); } static void mxsimage_print_header(const void *hdr) { if (imagefile) mxsimage_verify_print_header(imagefile, 0); } static int sb_build_image(struct sb_image_ctx *ictx, struct image_type_params *tparams) { struct sb_boot_image_header *sb_header = &ictx->payload; struct sb_section_ctx *sctx; struct sb_cmd_ctx *cctx; struct sb_command *ccmd; struct sb_key_dictionary_key *sb_dict_key = &ictx->sb_dict_key; uint8_t *image, *iptr; /* Calculate image size. */ uint32_t size = sizeof(*sb_header) + ictx->sect_count * sizeof(struct sb_sections_header) + sizeof(*sb_dict_key) + sizeof(ictx->digest); sctx = ictx->sect_head; while (sctx) { size += sctx->size; sctx = sctx->sect; }; image = malloc(size); if (!image) return -ENOMEM; iptr = image; memcpy(iptr, sb_header, sizeof(*sb_header)); iptr += sizeof(*sb_header); sctx = ictx->sect_head; while (sctx) { memcpy(iptr, &sctx->payload, sizeof(struct sb_sections_header)); iptr += sizeof(struct sb_sections_header); sctx = sctx->sect; }; memcpy(iptr, sb_dict_key, sizeof(*sb_dict_key)); iptr += sizeof(*sb_dict_key); sctx = ictx->sect_head; while (sctx) { cctx = sctx->cmd_head; while (cctx) { ccmd = &cctx->payload; memcpy(iptr, &cctx->c_payload, sizeof(cctx->payload)); iptr += sizeof(cctx->payload); if (ccmd->header.tag == ROM_LOAD_CMD) { memcpy(iptr, cctx->data, cctx->length); iptr += cctx->length; } cctx = cctx->cmd; } sctx = sctx->sect; }; memcpy(iptr, ictx->digest, sizeof(ictx->digest)); iptr += sizeof(ictx->digest); /* Configure the mkimage */ tparams->hdr = image; tparams->header_size = size; return 0; } static int mxsimage_generate(struct image_tool_params *params, struct image_type_params *tparams) { int ret; struct sb_image_ctx ctx; /* Do not copy the U-Boot image! */ params->skipcpy = 1; memset(&ctx, 0, sizeof(ctx)); ctx.cfg_filename = params->imagename; ctx.output_filename = params->imagefile; ret = sb_build_tree_from_cfg(&ctx); if (ret) goto fail; ret = sb_encrypt_image(&ctx); if (!ret) ret = sb_build_image(&ctx, tparams); fail: sb_free_image(&ctx); return ret; } /* * mxsimage parameters */ U_BOOT_IMAGE_TYPE( mxsimage, "Freescale MXS Boot Image support", 0, NULL, mxsimage_check_params, mxsimage_verify_header, mxsimage_print_header, mxsimage_set_header, NULL, mxsimage_check_image_types, NULL, mxsimage_generate ); #endif 是legacyuimage吗
11-08
#include <ntifs.h> #include <ntddk.h> #include <ntimage.h> #define RELOC_FLAG64(ReInfo) ((ReInfo>>0x0C)==IMAGE_REL_BASED_DIR64) #define RELOC_FLAG RELOC_FLAG64 ULONG64 g_fnLoadLibrary ; ULONG64 g_fnGetProcAddress ; ULONG64 g_fnRtlAddFuntion ; void Log(const char* sz_info, bool is_error, ULONG err_code); EXTERN_C NTSTATUS MmCopyVirtualMemory( IN PEPROCESS FromProcess, IN CONST VOID* FromAddress, IN PEPROCESS ToProcess, OUT PVOID ToAddress, IN SIZE_T BufferSize, IN KPROCESSOR_MODE PreviousMode, OUT PSIZE_T NumberOfBytesCopied ); EXTERN_C NTSTATUS NTAPI ZwSetInformationProcess( __in HANDLE ProcessHandle, __in PROCESSINFOCLASS ProcessInformationClass, __in_bcount (ProcessInformationLength) PVOID ProcessInformation, __in ULONG ProcessInformationLength ); typedef PVOID HINSTANCE, HMODULE; using f_LoadLibraryA = HMODULE(_stdcall*)(const char* lpLibFileName); using f_GetProcAddress = PVOID(_stdcall*)(HMODULE hModule, LPCSTR lpProcName); using f_DLL_ENTRY_POINT = BOOLEAN(_stdcall*)(PVOID DllHandle, ULONG Reason, PVOID Reserved); using f_RtlAddFunctionTable = BOOLEAN(_stdcall*)(_IMAGE_RUNTIME_FUNCTION_ENTRY* FunctionTable, DWORD32 EntryCount, DWORD64 BaseAddress); struct Manual_Mapping_data { //用于重定位iat f_LoadLibraryA pLoadLibraryA; f_GetProcAddress pGetProcAddress; //x64专有 f_RtlAddFunctionTable pRtlAddFunctionTable; char* pBase; DWORD32 dwReadson; //设置为0线程开始时执行dll PVOID reserveParam; BOOLEAN bFirst;//只允许第一次系统调用进入 BOOLEAN bStart;//开始执行shellcode了 可以同步关闭instCallBack BOOLEAN bContinue;//用于继续执行 去掉pe头后执行dllmain size_t DllSize; }; void __stdcall InstruShellCode(Manual_Mapping_data* pData); #pragma pack(push) #pragma pack(1) struct shellcode_t { private: char padding[43]; public: uintptr_t manual_data; private: char pdding[47]; public: uintptr_t rip; uintptr_t shellcode; }; char g_instcall_shel1code[] = { 0x50, 0x51, 0x52, 0x53, 0x55, 0x56, 0x57, 0x41, 0x50, 0x41, 0x51, 0x41, 0x52, 0x41, 0x53, 0x41, 0x54, 0x41, 0x55, 0x41, 0x56, 0x41, 0x57, 0x48,0x89 ,0x25,0x4C,0x00,0x00,0x00, 0x48, 0x83, 0xEC, 0x38, 0x48,0x81,0xE4,0xF0,0xFF,0xFF,0xFF, 0x48,0xB9,0x11,0x11,0x11,0x11,0x11,0x11,0x11,0x11, 0xFF,0x15,0x29,0x00,0x00,0x00,0x90,0x90,0x90,0x90,0x90,0x90,0x90,0x90,//call 0x48, 0x8B, 0x25, 0x22,0x00,0x00,0x00, 0x41, 0x5F, 0x41, 0x5E, 0x41, 0x5D, 0x41, 0x5C, 0x41, 0x5B, 0x41, 0x5A, 0x41, 0x59, 0x41, 0x58, 0x5F, 0x5E, 0x5D, 0x5B, 0x5A, 0x59, 0x58, 0x41, 0xFF, 0xE2, 0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0 }; #pragma pack(pop) NTSTATUS inst_callback_set_callback(PVOID inst_callback) { NTSTATUS status = STATUS_SUCCESS; PVOID InstCallBack = inst_callback; PACCESS_TOKEN Token{ 0 }; PULONG TokenMask{ 0 }; Token = PsReferencePrimaryToken(IoGetCurrentProcess()); TokenMask = (PULONG)((ULONG_PTR)Token + 0x40);//取_TOKEN结构下Privileges TokenMask[0] |= 0x100000; TokenMask[1] |= 0x100000; TokenMask[2] |= 0x100000; status = ZwSetInformationProcess(NtCurrentProcess(), ProcessInstrumentationCallback, &InstCallBack, sizeof(PVOID)); if (!NT_SUCCESS(status)) { Log("failed to set instrcallback !", true, status); } else { Log("failed to set instrcallback SUCCESS!", false, 0); } return status; } void Log(const char* sz_info, bool is_error, ULONG err_code) { if (is_error) { DbgPrintEx(77, 0, "[instCallBack Error] : %s err_code:%x\n", sz_info, err_code); } else { DbgPrintEx(77, 0, "[instCallBack] : %s \n", sz_info); } } PUCHAR inst_callback_get_dll_memory(UNICODE_STRING* us_dll_path) { NTSTATUS status = STATUS_SUCCESS; HANDLE hFile = 0; OBJECT_ATTRIBUTES objattr = { 0 }; IO_STATUS_BLOCK IoStatusBlock = { 0 }; LARGE_INTEGER lainter = { 0 }; LARGE_INTEGER byteOffset = { 0 }; FILE_STANDARD_INFORMATION fileinfo = { 0 }; ULONG64 fileSize = 0; PUCHAR pDllMemory = 0; InitializeObjectAttributes(&objattr, us_dll_path, OBJ_CASE_INSENSITIVE, 0, 0); status = ZwCreateFile(&hFile, GENERIC_READ, &objattr, &IoStatusBlock, &lainter, FILE_ATTRIBUTE_NORMAL, FILE_SHARE_READ | FILE_SHARE_DELETE | FILE_SHARE_WRITE, FILE_OPEN, 0, 0, 0); if (!NT_SUCCESS(status)) { Log("failed to Create File!", true, status); return 0; } status = ZwQueryInformationFile(hFile, &IoStatusBlock, &fileinfo, sizeof(fileinfo), FileStandardInformation); fileSize = (ULONG)fileinfo.AllocationSize.QuadPart; if (!NT_SUCCESS(status)) { Log("failed to get File size!", true, status); return 0; } fileSize += 0x1000; fileSize = (ULONG64)PAGE_ALIGN(fileSize); //ExAllocatePoolWithTag pDllMemory = (PUCHAR)ExAllocatePoolWithTag(PagedPool, fileSize, 'Dllp'); //pDllmem = (PUCHAR)ExAllocatePool3(NonPagedPool, fileSize, 'Dllp', NULL, NULL);//NonPagedPool PagedPool RtlSecureZeroMemory(pDllMemory, fileSize); status = ZwReadFile(hFile, 0, 0, 0, &IoStatusBlock, pDllMemory, fileSize, &byteOffset, 0); ZwFlushBuffersFile(hFile, &IoStatusBlock); if (!NT_SUCCESS(status)) { ExFreePool(pDllMemory); ZwClose(hFile); Log("failed to read File context!", true, status); return 0; } ZwClose(hFile); return pDllMemory; } NTSTATUS inst_callback_alloc_memory(PUCHAR p_dll_memory, _Out_ PVOID* inst_callbak_addr, PVOID* p_manual_data) { PEPROCESS Process{ 0 }; IMAGE_NT_HEADERS* pNtHeader = nullptr; IMAGE_FILE_HEADER* pFileHeader = nullptr; IMAGE_OPTIONAL_HEADER* pOptheader = nullptr; NTSTATUS status = STATUS_SUCCESS; PVOID pManualMapData = 0,pShellCode = 0; char* pStartMapAdd = 0; size_t AllocSize = 0, RETSize; Manual_Mapping_data ManualMapData{ 0 }; if (reinterpret_cast<IMAGE_DOS_HEADER*>(p_dll_memory)->e_magic != 0x5A4D) { status = STATUS_INVALID_PARAMETER; Log("the dll is not valid pe structure\n", true, status); return status; } pNtHeader = (IMAGE_NT_HEADERS*)((ULONG_PTR)p_dll_memory + reinterpret_cast<IMAGE_DOS_HEADER*>(p_dll_memory)->e_lfanew); pFileHeader = &pNtHeader->FileHeader; pOptheader = &pNtHeader->OptionalHeader; if (pFileHeader->Machine != IMAGE_FILE_MACHINE_AMD64) { status = STATUS_INVALID_PARAMETER; Log("the dll is is x86 structure ,not support!\n", true, status); return status; } AllocSize = pOptheader->SizeOfImage; status = ZwAllocateVirtualMemory(NtCurrentProcess(), (PVOID*)&pStartMapAdd, 0, &AllocSize, MEM_COMMIT, PAGE_EXECUTE_READWRITE); if (!NT_SUCCESS(status)) { Log("failed to alloc Memory!", true, status); return status; } RtlSecureZeroMemory(pStartMapAdd, AllocSize); ManualMapData.dwReadson = 0; ManualMapData.pGetProcAddress = (f_GetProcAddress)g_fnGetProcAddress; ManualMapData.pLoadLibraryA = (f_LoadLibraryA)g_fnLoadLibrary; ManualMapData.pRtlAddFunctionTable = (f_RtlAddFunctionTable)g_fnRtlAddFuntion; ManualMapData.pBase = pStartMapAdd; ManualMapData.bContinue = false;//判断是否抹除了pe标识 ManualMapData.bFirst = true; ManualMapData.bStart = false; ManualMapData.DllSize = AllocSize; Process = IoGetCurrentProcess(); status = MmCopyVirtualMemory(Process, p_dll_memory, Process, pStartMapAdd, PAGE_SIZE, KernelMode, &RETSize); if (!NT_SUCCESS(status)) { Log("failed to load pe header!", true, status); return status; } IMAGE_SECTION_HEADER* pSectionHeadr = IMAGE_FIRST_SECTION(pNtHeader); for (size_t i = 0; i < pFileHeader->NumberOfSections; i++, pSectionHeadr++) { if (pSectionHeadr->SizeOfRawData) { status = MmCopyVirtualMemory(Process, p_dll_memory + pSectionHeadr->PointerToRawData, Process, pStartMapAdd + pSectionHeadr->VirtualAddress, pSectionHeadr->SizeOfRawData, KernelMode, &RETSize); if (!NT_SUCCESS(status)) { Log("failed to load pe header!", true, status); return status; } } } AllocSize = PAGE_SIZE; status = ZwAllocateVirtualMemory(NtCurrentProcess(), &pManualMapData, 0, &AllocSize, MEM_COMMIT, PAGE_EXECUTE_READWRITE); if (!NT_SUCCESS(status)) { Log("failed to alloc Memory for maunalmapdata!", true, status); return status; } RtlSecureZeroMemory(pManualMapData, AllocSize); status = MmCopyVirtualMemory(Process, &ManualMapData, Process, pManualMapData, sizeof(ManualMapData), KernelMode, &RETSize); if (!NT_SUCCESS(status)) { Log("failed to write Memory for maunalmapdata!", true, status); return status; } //重定位ShellCode status = ZwAllocateVirtualMemory(NtCurrentProcess(), &pShellCode, 0, &AllocSize, MEM_COMMIT, PAGE_EXECUTE_READWRITE); if (!NT_SUCCESS(status)) { Log("failed to alloc Memory for shellcode!", true, status); return status; } RtlSecureZeroMemory(pShellCode, AllocSize); status = MmCopyVirtualMemory(Process, InstruShellCode, Process, pShellCode, AllocSize, KernelMode, &RETSize); if (!NT_SUCCESS(status)) { Log("failed to write Memory for shellcode!", true, status); return status; } //ShellCode shellcode_t shell_code; memset(&shell_code, 0, sizeof shell_code); memcpy(&shell_code, &g_instcall_shel1code, sizeof shellcode_t); shell_code.manual_data = (UINT64)pManualMapData; shell_code.rip = (UINT64)pShellCode; status = ZwAllocateVirtualMemory(NtCurrentProcess(), inst_callbak_addr, 0, &AllocSize, MEM_COMMIT, PAGE_EXECUTE_READWRITE); if (!NT_SUCCESS(status)) { Log("failed to alloc Memory for instrcall shellcode!", true, status); return status; } RtlSecureZeroMemory(*inst_callbak_addr, AllocSize); status = MmCopyVirtualMemory(Process, &shell_code, Process, *inst_callbak_addr, sizeof shell_code, KernelMode, &RETSize); if (!NT_SUCCESS(status)) { Log("failed to write Memory for instrcall shellcode!", true, status); return status; } *p_manual_data = pManualMapData; return status; } NTSTATUS inst_callback_inject(HANDLE process_id, UNICODE_STRING* us_dll_path) { NTSTATUS status = STATUS_SUCCESS; PEPROCESS pEhtreadyx = NULL; KAPC_STATE apc_state; PUCHAR pDllMem = 0; PVOID InstCallBack = 0, pManualMapData = 0; status = PsLookupProcessByProcessId(process_id, &pEhtreadyx); if (!NT_SUCCESS(status)) { Log("failed to get process!", true, status); status = STATUS_UNSUCCESSFUL; return status; } KeStackAttachProcess(pEhtreadyx, &apc_state); while (true) { pDllMem = inst_callback_get_dll_memory(us_dll_path); if (!pDllMem) { status = STATUS_UNSUCCESSFUL; break; } status = inst_callback_alloc_memory(pDllMem, &InstCallBack, &pManualMapData); if (!NT_SUCCESS(status))break; //设置instrecallback status = inst_callback_set_callback(InstCallBack); break; } if (pManualMapData && MmIsAddressValid(pManualMapData)) { __try { while (1) { if (((Manual_Mapping_data*)pManualMapData)->bStart)break; } } __except (1) { Log("process exit! 1", true, 0); ObDereferenceObject(pEhtreadyx); KeUnstackDetachProcess(&apc_state); return status; } } inst_callback_set_callback(0); if (pManualMapData && MmIsAddressValid(pManualMapData) && PsLookupProcessByProcessId(process_id, &pEhtreadyx) != STATUS_PENDING) { __try { *(PUCHAR)((((Manual_Mapping_data*)pManualMapData))->pBase) = 0; ((Manual_Mapping_data*)pManualMapData)->bContinue = true; } __except (1) { Log("process exit!", true, 0); } } ObDereferenceObject(pEhtreadyx); KeUnstackDetachProcess(&apc_state); if (pDllMem && MmIsAddressValid(pDllMem))ExFreePool(pDllMem); return status; } void __stdcall InstruShellCode(Manual_Mapping_data* pData) { if (!pData->bFirst)return; pData->bFirst = false; pData->bStart = true; char* pBase = pData->pBase; auto* pOptionHeader = &reinterpret_cast<IMAGE_NT_HEADERS*>(pBase + reinterpret_cast<IMAGE_DOS_HEADER*> ((uintptr_t)pBase)->e_lfanew)->OptionalHeader; char* LocationDelta = pBase - pOptionHeader->ImageBase;//差值 if (LocationDelta) { if (pOptionHeader->DataDirectory[IMAGE_DIRECTORY_ENTRY_BASERELOC].Size) { auto* pRelocData = reinterpret_cast<IMAGE_BASE_RELOCATION*> (pBase + pOptionHeader->DataDirectory[IMAGE_DIRECTORY_ENTRY_BASERELOC].VirtualAddress); auto* pRelocEnd = reinterpret_cast<IMAGE_BASE_RELOCATION*>(reinterpret_cast<uintptr_t>(pRelocData) + pOptionHeader->DataDirectory[IMAGE_DIRECTORY_ENTRY_BASERELOC].Size); while (pRelocData< pRelocEnd && pRelocData->SizeOfBlock) { UINT64 AmountOfEntries = (pRelocData->SizeOfBlock - sizeof(IMAGE_BASE_RELOCATION)) / sizeof(short); unsigned short* pRelativeInfo = reinterpret_cast<unsigned short*>(pRelocData + 1); for (UINT64 i = 0; i != AmountOfEntries;++pRelativeInfo) { if (RELOC_FLAG(*pRelativeInfo)) { //计算出需要重定位的地址 auto* pPatch = reinterpret_cast<UINT_PTR*>(pBase + pRelocData->VirtualAddress + ((*pRelativeInfo) & 0xFFF)); //手动重定位 *pPatch += reinterpret_cast<UINT_PTR>(LocationDelta); } } pRelocData = reinterpret_cast<IMAGE_BASE_RELOCATION*>(reinterpret_cast<char*>(pRelocData) + pRelocData->SizeOfBlock); } } } if (pOptionHeader->DataDirectory[IMAGE_DIRECTORY_ENTRY_IMPORT].Size) { IMAGE_IMPORT_DESCRIPTOR* pImportDescr = reinterpret_cast<IMAGE_IMPORT_DESCRIPTOR*> (pBase+pOptionHeader->DataDirectory[IMAGE_DIRECTORY_ENTRY_IMPORT].VirtualAddress); while (pImportDescr->Name) { HMODULE hDll = pData->pLoadLibraryA(pBase + pImportDescr->Name); //INT IAT //int ULONG_PTR* pInt = (ULONG_PTR*)(pBase + pImportDescr->OriginalFirstThunk); ULONG_PTR* pIat = (ULONG_PTR*)(pBase + pImportDescr->FirstThunk); //IAT if (!pInt)pInt = pIat; for (; *pIat; ++pIat, ++pInt) { if (IMAGE_SNAP_BY_ORDINAL(*pInt)) { *pIat = (ULONG_PTR)pData->pGetProcAddress(hDll, (char*)(*pInt & 0xffff)); } else { IMAGE_IMPORT_BY_NAME* pImport = (IMAGE_IMPORT_BY_NAME*)(pBase + *pInt); *pIat = (ULONG_PTR)pData->pGetProcAddress(hDll, pImport->Name); } } pImportDescr++; } } #define DLL_PROCESS_ATTACH 1 if (pOptionHeader->DataDirectory[IMAGE_DIRECTORY_ENTRY_TLS].Size) { auto* pTLS = reinterpret_cast<IMAGE_TLS_DIRECTORY*> (pBase + pOptionHeader->DataDirectory[IMAGE_DIRECTORY_ENTRY_TLS].VirtualAddress); auto* pCallBack = reinterpret_cast<PIMAGE_TLS_CALLBACK*>(pTLS->AddressOfCallBacks); for (;pCallBack && *pCallBack; ++pCallBack) { (*pCallBack)(pBase, DLL_PROCESS_ATTACH, nullptr); } } auto excep = pOptionHeader->DataDirectory[IMAGE_DIRECTORY_ENTRY_EXCEPTION]; if (excep.Size) { pData->pRtlAddFunctionTable((_IMAGE_RUNTIME_FUNCTION_ENTRY*) (pBase + excep.VirtualAddress), excep.Size / sizeof(_IMAGE_RUNTIME_FUNCTION_ENTRY),(DWORD64)pBase); } while (!pData->bContinue); //手动调用dllmain ((f_DLL_ENTRY_POINT)(pBase+pOptionHeader->AddressOfEntryPoint))(pBase, DLL_PROCESS_ATTACH, 0); } 注入dll会触发c000005异常
08-26
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值