struct st_page_header * st_read_page_header(xptr_t p, struct st_page_header * header) { char * c; READ_PAGE(p); p = GET_PAGE_ADDRESS(p); c = (char *) XADDR(p); uint16_t trie_count; sptr_t all_trie_len; header->page = p; c += ST_PAGE_HEADER_OFFSET; CAST_AND_READ(trie_count, c); CAST_AND_READ(all_trie_len, c); header->trie_count = trie_count; header->tries = (sptr_t *) c; c += trie_count * sizeof(sptr_t); header->trie_offset = c - ((char *) XADDR(p)); header->data_end = header->trie_offset + all_trie_len; header->free_space = PAGE_SIZE - header->data_end; return header; }
xptr_t st_write_split_state(struct st_tmp_trie * new_state) { struct state_descriptor dsc; struct st_page_header newpg; xptr_t parent_state = new_state->old_state; char * buf = new_state->buf2; size_t buflen = new_state->len2; int lower_edge = new_state->hint_edge; char * state = (char *) XADDR(parent_state); READ_PAGE(parent_state); state = read_state(state, &dsc); int n = dsc.edge_count; xptr_t * candidates = (xptr_t *) malloc(sizeof(xptr_t) * n); xptr_t c; xptr_t result; for (int i = 0; i < n; ++i) { struct state_descriptor tmp; read_state(state + dsc.pointers[i], &tmp); if ((tmp.flags & STATE_LONG_JUMP) > 0) { candidates[i] = tmp.long_jump; } else { candidates[i] = XNULL; } } c = select_candidate(candidates, n, buflen + sizeof(sptr_t)); //it's important to search candidate basing on full size of changes not only on string length. free(candidates); if (c == XNULL) { // U_ASSERT(false); st_markup_page(&newpg, 1, true); memcpy((char *) XADDR(newpg.page) + newpg.trie_offset, buf, buflen); newpg.data_end = newpg.trie_offset + buflen; st_write_page_header(&newpg); } else { c = GET_PAGE_ADDRESS(c); char * page_start = (char *) XADDR(c); st_read_page_header(c, &newpg); WRITE_PAGE(c); /* Add new trie to the end of the trie array */ newpg.trie_count++; memmove_ABd(page_start + newpg.trie_offset, page_start + newpg.data_end, sizeof(sptr_t)); newpg.tries[newpg.trie_count - 1] = newpg.data_end - newpg.trie_offset; newpg.data_end += sizeof(sptr_t); newpg.trie_offset += sizeof(sptr_t); memcpy(page_start + newpg.data_end, buf, buflen); newpg.data_end += buflen; st_write_page_header(&newpg); } result = newpg.page + newpg.trie_offset - sizeof(sptr_t); return result; }
struct st_page_header * st_write_page_header(struct st_page_header * header) { char * c; WRITE_PAGE(header->page); c = (char *) XADDR(GET_PAGE_ADDRESS(header->page)); uint16_t trie_count = header->trie_count; sptr_t all_trie_len = header->data_end - header->trie_offset; c += ST_PAGE_HEADER_OFFSET; CAST_AND_WRITE(c, trie_count); CAST_AND_WRITE(c, all_trie_len); return header; }
static char * st_fix_parent_pointers(xptr_t parent_state, xptr_t split_page, char * trie_array_base) { struct st_page_header ph; struct state_descriptor dsc; st_read_page_header(parent_state, &ph); char * base = (char*) XADDR(ph.page) + ph.trie_offset; char * state = base; char * next_state; char * end = (char*) XADDR(ph.page) + ph.data_end; while (state < end) { next_state = read_state(state, &dsc); if ((dsc.long_jump != XNULL) && (GET_PAGE_ADDRESS(dsc.long_jump) == split_page)) { tries[((char *) XADDR(dsc.long_jump) - trie_array_base) / sizeof(sptr_t)].parent_offset = state - base; } state = next_state; } return base; }
/************************************************************************************************** * @fn sbCmnd * * @brief Act on the SB command and received buffer. * * input parameters * * @param sbCmd - Received SBL command. * @param payload_len - Length of command payload * * output parameters * * None. * * @return TRUE to indicate that the SB_ENABLE_CMD command was successful; FALSE otherwise. ************************************************************************************************** */ static uint8 sbCmnd(uint8 sbCmd, uint32 payload_len) { uint32 firstAddr; uint32 lastAddr; uint32 operationLength; uint32 writeLength; uint32 respPayloadLen = 0; uint32 pageNumber; uint32 i; uint32 actual_number_of_data_bytes_to_send; uint8 paddingLength; uint8 rsp = SB_SUCCESS; uint8 imageEnabledSuccessfully = FALSE; uint8 *pBuf; pBuf = sbBuf; switch (sbCmd) { case SB_HANDSHAKE_CMD: /* Mark all pages as not-deleted-yet */ memset(pageDeleted, 0, sizeof(pageDeleted)); UINT32_TO_BUF_LITTLE_ENDIAN(pBuf, SB_BOOTLOADER_REVISION); *pBuf++ = SB_DEVICE_TYPE_2538; UINT32_TO_BUF_LITTLE_ENDIAN(pBuf, SB_RW_BUF_LEN ); UINT32_TO_BUF_LITTLE_ENDIAN(pBuf, SB_DEVICE_PAGE_SIZE); respPayloadLen = pBuf - sbBuf; break; case SB_WRITE_CMD: firstAddr = BUF_TO_UINT32_LITTLE_ENDIAN(pBuf); operationLength = BUF_TO_UINT32_LITTLE_ENDIAN(pBuf); /* The payload_len includes the addr_offset * and the operationLength fields. The value * (pBuf - sbBuf) gives the number of bytes * used by those firelds. The remaining bytes * are the actual data bytes to be written. */ writeLength = payload_len - (pBuf - sbBuf); lastAddr = firstAddr + operationLength - 1; if ((firstAddr < FLASH_BASE) || (lastAddr > CC2538_CODE_FLASH_END_ADDRESS) || (writeLength > operationLength)) { rsp = SB_FAILURE; break; } /* Before writing to a flash page for the first time during a bootloading session, the * page must be erased. The following section makes sure that every page being written * to have already been erased, otherwise, it erases it (before writing to it). * Note that the write command may span over more than a single page. */ for (pageNumber = GET_PAGE_NUMBER(firstAddr); pageNumber <= GET_PAGE_NUMBER(lastAddr); pageNumber++) { if (!IS_PAGE_ERASED(pageNumber)) { if (FlashMainPageErase(GET_PAGE_ADDRESS(pageNumber)) != 0) { rsp = SB_FAILURE; break; } MARK_PAGE_ERASED(pageNumber); } } /* Note that the start address (firstAddr) and the byte count (writeLength) must be * word aligned. The start address is expected to be already aligned (by the SBL server), * since aligning it here would require padding the buffer's start, which would require * shifting the buffer content (as the buffer is passesd as (uint32_t *pui32Data) so it * should be aligned by itself. The byte count is aligned below. */ paddingLength = ((4 - (writeLength & 0x00000003)) % 4); for (i = 0; i < paddingLength; i++) { pBuf[writeLength + i] = 0xFF; } writeLength += paddingLength; /* If the page was successfully erased (or was previously erased), perform the write action. * Note that pBuf must point to a uint32-aligned address, as required by FlashMainPageProgram(). * This is the case now (the prefixing field are total of 8 bytes), and _sbBuf is 32bit aligned. */ if ((rsp == SB_SUCCESS) && (writeLength > 0) && (FlashMainPageProgram((uint32_t *)(pBuf), firstAddr, writeLength) != 0)) { rsp = SB_FAILURE; } break; case SB_READ_CMD: firstAddr = BUF_TO_UINT32_LITTLE_ENDIAN(pBuf); operationLength = BUF_TO_UINT32_LITTLE_ENDIAN(pBuf); lastAddr = firstAddr + operationLength - 1; if ((firstAddr < FLASH_BASE) || (lastAddr > CC2538_CODE_FLASH_END_ADDRESS) || (operationLength > sizeof(_sbBuf))) { rsp = SB_FAILURE; break; } #if !MT_SYS_OSAL_NV_READ_CERTIFICATE_DATA #if (HAL_IMG_A_BEG > HAL_NV_START_ADDR) #warning This check assumes NV PAGES located at the end of the program flash memory #endif if (GET_PAGE_NUMBER(lastAddr) >= HAL_NV_PAGE_BEG) { rsp = SB_FAILURE; break; } #endif /* If the end of the buffer is made only of 0xFF characters, no need to * send them. Find out the number of bytes that needs to be sent: */ for (actual_number_of_data_bytes_to_send = operationLength; (actual_number_of_data_bytes_to_send > 0) && ((*(uint8 *)(firstAddr + actual_number_of_data_bytes_to_send - 1)) == 0xFF); actual_number_of_data_bytes_to_send--); /* As a future upgrade, memcopy can be avoided. Instead, * may pass a pointer to the actual flash address */ (void)memcpy(pBuf, (const void *)firstAddr, actual_number_of_data_bytes_to_send); respPayloadLen = (pBuf - sbBuf) + actual_number_of_data_bytes_to_send; break; case SB_ENABLE_CMD: if (enableImg()) { imageEnabledSuccessfully = TRUE; } else { rsp = SB_VALIDATE_FAILED; } break; default: break; } sbResp(sbCmd, rsp, respPayloadLen); return imageEnabledSuccessfully; }
static xptr_t st_split_promote_root(struct st_page_header * root_page_hdr, int parent_free_space, xptr_t parent_position, int cpage) { struct state_descriptor dsc; xptr_t rtp = XNULL, ltp = XNULL; int total_length; int break_point; int n; int root_length; READ_PAGE(root_page_hdr->page); read_state(get_root_state(root_page_hdr), &dsc); n = dsc.edge_count; total_length = build_segments(dsc.p + dsc.len, dsc.pointers, source, tries, n); for (int i = 0; i < n; i++) { tries[i].id = i; } root_length = dsc.len + n * (sizeof(xptr_t) + sizeof(flags_t)); if (root_length <= parent_free_space) { char * buf = root_buffer; struct st_tmp_trie trie = {}; struct st_page_header * pghdr = root_page_hdr; memcpy(buf, dsc.p, dsc.len); ltp = root_page_hdr->page; st_read_page_header(GET_PAGE_ADDRESS(parent_position), pghdr); trie.state = (char *) XADDR(parent_position) - (char *) XADDR(pghdr->page) - pghdr->trie_offset; trie.state_len = sizeof(xptr_t) + sizeof(flags_t); trie.buf = buf; trie.len = root_length; trie.buf2 = NULL; st_new_state_write(pghdr, &trie); READ_PAGE(parent_position); read_state((char *) XADDR(parent_position), &dsc); } else { // TODO: PROMOTE ROOT! U_ASSERT(cpage==0); parent_position = root_page_hdr->page; root_page_hdr->data_end = root_page_hdr->trie_offset + root_length; } break_point = write_segments(tries, n, total_length, <p, &rtp, source); WRITE_PAGE(root_page_hdr->page); (* (flags_t *) dsc.p) |= STATE_SPLIT_POINT; for (int i = 0; i < n; i++) { dsc.pointers[tries[i].id] = i * (sizeof(xptr_t) + sizeof(flags_t)); } write_jump_list(&dsc, tries, ltp, 0, break_point); if (rtp != XNULL) { write_jump_list(&dsc, tries, rtp, break_point, n); } st_write_page_header(root_page_hdr); return ltp; }