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