uint32_t nrf_dfu_continue(uint32_t * p_enter_dfu_mode)
{
uint32_t ret_val;
nrf_dfu_bank_t * p_bank;
uint32_t src_addr = CODE_REGION_1_START;
NRF_LOG_INFO("Enter nrf_dfu_continue\r\n");
if (s_dfu_settings.bank_layout == NRF_DFU_BANK_LAYOUT_SINGLE )
{
p_bank = &s_dfu_settings.bank_0;
}
else if(s_dfu_settings.bank_current == NRF_DFU_CURRENT_BANK_0)
{
p_bank = &s_dfu_settings.bank_0;
}
else
{
p_bank = &s_dfu_settings.bank_1;
src_addr += align_to_page(s_dfu_settings.bank_0.image_size, CODE_PAGE_SIZE);
}
ret_val = nrf_dfu_continue_bank(p_bank, src_addr, p_enter_dfu_mode);
return ret_val;
}
static int fatelf_replace(const char *out, const char *fname,
const char *newobj)
{
const int fd = xopen(fname, O_RDONLY, 0755);
const int newfd = xopen(newobj, O_RDONLY, 0755);
FATELF_header *header = xread_fatelf_header(fname, fd);
const int idx = xfind_fatelf_record_by_elf(newobj, newfd, fname, header);
const int outfd = xopen(out, O_WRONLY | O_CREAT | O_TRUNC, 0755);
uint64_t offset = FATELF_DISK_FORMAT_SIZE(((int)header->num_records));
int i;
unlink_on_xfail = out;
// pad out some bytes for the header we'll write at the end...
xwrite_zeros(out, outfd, (size_t) offset);
for (i = 0; i < ((int) header->num_records); i++)
{
const uint64_t binary_offset = align_to_page(offset);
FATELF_record *rec = &header->records[i];
// append this binary to the final file, padded to page alignment.
xwrite_zeros(out, outfd, (size_t) (binary_offset - offset));
if (i == idx) // the thing we're replacing...
rec->size = xcopyfile(newobj, newfd, out, outfd);
else
xcopyfile_range(fname, fd, out, outfd, binary_offset, rec->size);
rec->offset = binary_offset;
offset = binary_offset + rec->size;
} // for
// Write the actual FatELF header now...
xwrite_fatelf_header(out, outfd, header);
xappend_junk(fname, fd, out, outfd);
xclose(out, outfd);
xclose(newobj, newfd);
xclose(fname, fd);
free(header);
unlink_on_xfail = NULL;
return 0; // success.
} // fatelf_replace
uint32_t nrf_dfu_find_cache(uint32_t size_req, bool dual_bank_only, uint32_t * p_address)
{
// TODO: Prevalidate p_address and p_bank
uint32_t free_size = DFU_REGION_TOTAL_SIZE - DFU_APP_DATA_RESERVED;
nrf_dfu_bank_t * p_bank;
NRF_LOG_INFO("Enter nrf_dfu_find_cache\r\n");
// Simple check if size requirement can me met
if(free_size < size_req)
{
NRF_LOG_INFO("No way to fit the new firmware on device\r\n");
return NRF_ERROR_NO_MEM;
}
NRF_LOG_INFO("Bank content\r\n");
NRF_LOG_INFO("Bank type: %d\r\n", s_dfu_settings.bank_layout);
NRF_LOG_INFO("Bank 0 code: 0x%02x: Size: %d\r\n", s_dfu_settings.bank_0.bank_code, s_dfu_settings.bank_0.image_size);
NRF_LOG_INFO("Bank 1 code: 0x%02x: Size: %d\r\n", s_dfu_settings.bank_1.bank_code, s_dfu_settings.bank_1.image_size);
// Setting bank_0 as candidate
p_bank = &s_dfu_settings.bank_0;
// Setting candidate address
(*p_address) = MAIN_APPLICATION_START_ADDR;
// Calculate free size
if (s_dfu_settings.bank_0.bank_code == NRF_DFU_BANK_VALID_APP)
{
// Valid app present.
NRF_LOG_INFO("free_size before bank select: %d\r\n", free_size);
free_size -= align_to_page(p_bank->image_size, CODE_PAGE_SIZE);
NRF_LOG_INFO("free_size: %d, size_req: %d\r\n", free_size, size_req);
// Check if we can fit the new in the free space or if removal of old app is required.
if(size_req > free_size)
{
// Not enough room in free space (bank_1)
if ((dual_bank_only))
{
NRF_LOG_INFO("Failure: dual bank restriction\r\n");
return NRF_ERROR_NO_MEM;
}
// Can only support single bank update, clearing old app.
s_dfu_settings.bank_layout = NRF_DFU_BANK_LAYOUT_SINGLE;
s_dfu_settings.bank_current = NRF_DFU_CURRENT_BANK_0;
p_bank = &s_dfu_settings.bank_0;
NRF_LOG_INFO("Enforcing single bank\r\n");
}
else
{
// Room in bank_1 for update
// Ensure we are using dual bank layout
s_dfu_settings.bank_layout = NRF_DFU_BANK_LAYOUT_DUAL;
s_dfu_settings.bank_current = NRF_DFU_CURRENT_BANK_1;
p_bank = &s_dfu_settings.bank_1;
// Set to first free page boundry after previous app
(*p_address) += align_to_page(s_dfu_settings.bank_0.image_size, CODE_PAGE_SIZE);
NRF_LOG_INFO("Using second bank\r\n");
}
}
else
{
// No valid app present. Promoting dual bank.
s_dfu_settings.bank_layout = NRF_DFU_BANK_LAYOUT_DUAL;
s_dfu_settings.bank_current = NRF_DFU_CURRENT_BANK_0;
p_bank = &s_dfu_settings.bank_0;
NRF_LOG_INFO("No previous, using bank 0\r\n");
}
// Set the bank-code to invalid, and reset size/CRC
memset(p_bank, 0, sizeof(nrf_dfu_bank_t));
// Store the Firmware size in the bank for continuations
p_bank->image_size = size_req;
return NRF_SUCCESS;
}
/** @brief Function to execute the continuation of a SoftDevice update.
*
* @param[in] src_addr Source address of the SoftDevice to copy from.
* @param[in] p_bank Pointer to the bank where the SoftDevice resides.
*
* @retval NRF_SUCCESS Continuation was successful.
* @retval NRF_ERROR_INVALID_LENGTH Invalid len
* @retval NRF_ERROR_NO_MEM if UICR.NRFFW[1] is not set (i.e. is 0xFFFFFFFF).
* @retval NRF_ERROR_INVALID_PARAM if an invalid command is given.
* @retval NRF_ERROR_INTERNAL indicates that the contents of the memory blocks where not verified correctly after copying.
* @retval NRF_ERROR_NULL If the content of the memory blocks differs after copying.
*/
static uint32_t nrf_dfu_sd_continue_impl(uint32_t src_addr,
nrf_dfu_bank_t * p_bank)
{
uint32_t ret_val = NRF_SUCCESS;
uint32_t target_addr = SOFTDEVICE_REGION_START + s_dfu_settings.write_offset;
uint32_t length_left = align_to_page(s_dfu_settings.sd_size - s_dfu_settings.write_offset, CODE_PAGE_SIZE);
uint32_t split_size = align_to_page(length_left / 4, CODE_PAGE_SIZE);
NRF_LOG_INFO("Enter nrf_bootloader_dfu_sd_continue\r\n");
// This can be a continuation due to a power failure
src_addr += s_dfu_settings.write_offset;
if (s_dfu_settings.sd_size != 0 && s_dfu_settings.write_offset == s_dfu_settings.sd_size)
{
NRF_LOG_INFO("SD already copied\r\n");
return NRF_SUCCESS;
}
NRF_LOG_INFO("Updating SD. Old SD ver: 0x%04x\r\n", SD_FWID_GET(MBR_SIZE));
do
{
NRF_LOG_INFO("Copying [0x%08x-0x%08x] to [0x%08x-0x%08x]: Len: 0x%08x\r\n", src_addr, src_addr + split_size, target_addr, target_addr + split_size, split_size);
// Copy a chunk of the SD. Size in words
ret_val = nrf_dfu_mbr_copy_sd((uint32_t*)target_addr, (uint32_t*)src_addr, split_size);
if (ret_val != NRF_SUCCESS)
{
NRF_LOG_INFO("Failed to copy SD: target: 0x%08x, src: 0x%08x, len: 0x%08x\r\n", target_addr, src_addr, split_size);
return ret_val;
}
NRF_LOG_INFO("Finished copying [0x%08x-0x%08x] to [0x%08x-0x%08x]: Len: 0x%08x\r\n", src_addr, src_addr + split_size, target_addr, target_addr + split_size, split_size);
// Validate copy. Size in words
ret_val = nrf_dfu_mbr_compare((uint32_t*)target_addr, (uint32_t*)src_addr, split_size);
if (ret_val != NRF_SUCCESS)
{
NRF_LOG_INFO("Failed to Compare SD: target: 0x%08x, src: 0x%08x, len: 0x%08x\r\n", target_addr, src_addr, split_size);
return ret_val;
}
NRF_LOG_INFO("Validated 0x%08x-0x%08x to 0x%08x-0x%08x: Size: 0x%08x\r\n", src_addr, src_addr + split_size, target_addr, target_addr + split_size, split_size);
target_addr += split_size;
src_addr += split_size;
if (split_size > length_left)
{
length_left = 0;
}
else
{
length_left -= split_size;
}
NRF_LOG_INFO("Finished with the SD update.\r\n");
// Save the updated point of writes in case of power loss
s_dfu_settings.write_offset = s_dfu_settings.sd_size - length_left;
(void)nrf_dfu_settings_write(NULL);
}
while (length_left > 0);
return ret_val;
}
/*-----------------------------------------------------------------------------
DetourFunctionWithTrampoline
-----------------------------------------------------------------------------*/
int DetourFunctionWithTrampoline(void* pOriginalFunction, void* pTargetFunction, void* pTrampoline)
{
int pagesize = sysconf(_SC_PAGE_SIZE);
void* pOriginalFunctionPage = align_to_page(pOriginalFunction, pagesize);
void* pTrampolinePage = align_to_page(pTrampoline, pagesize);
void* pOriginalFunctionPage2;
void* pTrampolinePage2;
int mprotect_error = 0;
unsigned int cpysize = 0;
hde32s s;
DetourInfo* newInfo;
while (cpysize < 5)
{
cpysize += hde32_disasm((void*)((unsigned int)pOriginalFunction + cpysize), &s);
/* this would require reassembling at the trampoline location */
if (s.flags & F_RELATIVE)
{
printf("ERROR: DetourFunctionWithTrampoline - found function with relative addressing. Bailing out :(\n");
return -1;
}
}
/* check if the end of the areas that are about to be overwritten lie in a different page */
pOriginalFunctionPage2 = align_to_page((void*)((unsigned int)pOriginalFunction + sizeof(__uint32_t)), pagesize);
pTrampolinePage2 = align_to_page((void*)((unsigned int)pTrampoline + cpysize - 1), pagesize);
if (mprotect(pOriginalFunctionPage, pagesize, PROT_READ | PROT_WRITE | PROT_EXEC) == -1
|| mprotect(pTrampolinePage, pagesize, PROT_READ | PROT_WRITE | PROT_EXEC) == -1)
mprotect_error = 1;
/* if the end of the areas lie in a different page, mprotect needs to be called for that page too */
if (!mprotect_error && pOriginalFunctionPage2 != pOriginalFunctionPage)
{
if (mprotect(pOriginalFunctionPage2, pagesize, PROT_READ | PROT_WRITE | PROT_EXEC) == -1)
mprotect_error = 1;
}
if (!mprotect_error && pTrampolinePage2 != pTrampolinePage)
{
if (mprotect(pTrampolinePage2, pagesize, PROT_READ | PROT_WRITE | PROT_EXEC) == -1)
mprotect_error = 1;
}
if (mprotect_error)
{
printf("ERROR: DetourFunctionWithTrampoline - Couldn't set memory access rights - errno: %d (%s)\n", errno, strerror(errno));
return -1;
}
/* copy original code */
memcpy(pTrampoline, pOriginalFunction, cpysize);
/* assemble jmp instruction in the original function */
*(unsigned char*)pOriginalFunction = 0xE9;
/* addressing is relative to the program counter */
*(__uint32_t*)((unsigned int)pOriginalFunction + 1) = (__uint32_t)pTargetFunction - (__uint32_t)pOriginalFunction - 5;
/* assemble jmp instruction in the trampoline */
*(unsigned char*)((unsigned int)pTrampoline + cpysize) = 0xE9;
/* Still relative, jump over partial instructions (if any!) */
*(__uint32_t*)((unsigned int)pTrampoline + cpysize + 1) = (__uint32_t)pOriginalFunction + cpysize - (__uint32_t)pTrampoline - 5 - cpysize;
/* Add to detour list */
CheckDetourList();
/* Skip to the end of the list */
for (newInfo = DetourList; newInfo->nextDetour != (void*)0; newInfo = (DetourInfo*)newInfo->nextDetour) ;
/* Initialize */
newInfo->nextDetour = (DetourInfo*)malloc(sizeof(DetourInfo));
newInfo = (DetourInfo*)newInfo->nextDetour;
newInfo->nextDetour = (void*)0;
newInfo->pOriginalFunction = pOriginalFunction;
newInfo->OriginalOffset = 0;
newInfo->pTrampoline = pTrampoline;
newInfo->TrampolineSize = cpysize;
return 0;
}
bool is_small(SearchRange range) const {
auto ps = get_page_size();
auto b = align_to_page(reinterpret_cast<void const*>(page_->read_entry_at(range.begin)), ps);
auto e = align_to_page(reinterpret_cast<void const*>(page_->read_entry_at(range.end)), ps);
return b == e;
}
