uint32_t blocking_flash_word_write(uint32_t * const p_dst, uint32_t data)
{
uint32_t err_code;
do{
err_code = sd_flash_write(p_dst, &data, 1);
} while(err_code == NRF_ERROR_BUSY);
return err_code;
}
/**@brief Function to store data to flash.
*
* @param[in] p_cmd The queue element associated with the operation.
*
* @retval NRF_SUCCESS Success. The request was sent to the SoftDevice.
* @retval Any error returned by the SoftDevice flash API.
*/
static __INLINE uint32_t store_execute(fs_cmd_t const * const p_cmd)
{
// Write in chunks if write-size is larger than FS_MAX_WRITE_SIZE.
fs_length_t const length = ((p_cmd->length_words - p_cmd->offset) < FS_MAX_WRITE_SIZE_WORDS) ?
(p_cmd->length_words - p_cmd->offset) : FS_MAX_WRITE_SIZE_WORDS;
return sd_flash_write((uint32_t*)p_cmd->p_addr + p_cmd->offset /* destination */,
(uint32_t*)p_cmd->p_src + p_cmd->offset /* source */,
length);
}
void writeBlockToFlash(uint32_t* to, uint32_t* from, int numWords)
{
uint8_t page = PAGE_FROM_ADDRESS(to);
if (page > LAST_PAGE || page < FIRST_PAGE) {
debug_log("Invalid block write address\r\n");
while(1);
}
else {
flashWorking = true;
sd_flash_write(to, from, numWords);
}
}
// Executes a store operation.
static uint32_t store_execute(fs_op_t const * const p_op)
{
uint16_t chunk_len;
if ((p_op->store.length_words - p_op->store.offset) < FS_MAX_WRITE_SIZE_WORDS)
{
chunk_len = p_op->store.length_words - p_op->store.offset;
}
else
{
chunk_len = FS_MAX_WRITE_SIZE_WORDS;
}
return sd_flash_write((uint32_t*)p_op->store.p_dest + p_op->store.offset,
(uint32_t*)p_op->store.p_src + p_op->store.offset,
chunk_len);
}
static otError sdFlashSingleWrite(uint32_t aAddress, const uint8_t *aData, uint32_t aSize)
{
uint32_t retval;
nrf_sdh_suspend();
do
{
sState = FLASH_STATE_PENDING;
retval = sd_flash_write((uint32_t *)aAddress, (uint32_t *)aData, aSize);
if (retval == NRF_SUCCESS)
{
break;
}
else if (retval == NRF_ERROR_BUSY)
{
sState = FLASH_STATE_WAITING_FOR_IDLE;
}
else
{
assert(false);
}
waitInState(FLASH_STATE_WAITING_FOR_IDLE);
} while (retval == NRF_ERROR_BUSY);
waitInState(FLASH_STATE_PENDING);
if (sState != FLASH_STATE_COMPLETE_SUCCESS)
{
retval = NRF_ERROR_INTERNAL;
}
sState = FLASH_STATE_IDLE;
nrf_sdh_resume();
return nrf5SdErrorToOtError(retval);
}
static void __HalFlashShootHead(void)
{
uint32_t ret;
uint32_t info;
if (opc[opc_begin].is_write == 1) {
ret = sd_flash_write((uint32_t *)((uint32_t)opc[opc_begin].pg_idx * 1024 + (uint32_t)opc[opc_begin].pg_offset), (uint32_t *)(opc[opc_begin].buf), (opc[opc_begin].len / 4));
} else {
ret = sd_flash_page_erase((uint32_t)opc[opc_begin].pg_idx);
}
if (ret == NRF_SUCCESS) {
opc_head_status = IN_PROGRESS;
} else if (ret == NRF_ERROR_BUSY) {
// Not good time
opc_head_status = BUSY_WAIT;
} else {
info = (uint32_t)opc[opc_begin].is_write << 24 | (uint32_t)opc[opc_begin].pg_idx << 16 | (uint32_t)opc[opc_begin].pg_offset;
__HalFlashRemoveHead();
persistent_record_error((uint8_t)ret, info);
}
}
/**@brief Function to store data to flash.
*
* @param[in] p_cmd The queue element associated with the operation.
*
* @retval NRF_SUCCESS Success. The request was sent to the SoftDevice.
* @retval Any error returned by the SoftDevice flash API.
*/
static ret_code_t store_execute(fs_cmd_t * const p_cmd)
{
ret_code_t ret;
uint16_t chunk_len;
if (p_cmd->store.length_words - p_cmd->store.offset < FS_MAX_WRITE_SIZE_WORDS)
{
chunk_len = p_cmd->store.length_words - p_cmd->store.offset;
}
else
{
chunk_len = FS_MAX_WRITE_SIZE_WORDS;
}
ret = sd_flash_write((uint32_t*)&p_cmd->store.p_dest[p_cmd->store.offset],
(uint32_t*)&p_cmd->store.p_src[p_cmd->store.offset],
chunk_len);
p_cmd->store.offset += chunk_len;
return ret;
}
int32_t flash_program_page(flash_t *obj, uint32_t address, const uint8_t *data, uint32_t size)
{
(void)(obj);
/* Return value defaults to error. */
uint32_t result = NRF_ERROR_BUSY;
/* Convert size to words. */
uint32_t words = size / sizeof(uint32_t);
if (NRF_HAL_SD_IS_ENABLED()) {
/* Setup stop watch for timeout. */
uint32_t start_us = lp_ticker_read();
uint32_t now_us = start_us;
/* Retry if flash is busy until timeout is reached. */
while (((now_us - start_us) < (words * WORD_WRITE_TIMEOUT_US)) &&
(result == NRF_ERROR_BUSY)) {
result = sd_flash_write((uint32_t *) address, (const uint32_t *) data, words);
/* Read timeout timer. */
now_us = lp_ticker_read();
}
} else {
/* We will use *_words function to speed up flashing code. Word means 32bit -> 4B
* or sizeof(uint32_t).
*/
nrf_nvmc_write_words(address, (const uint32_t *) data, words);
result = NRF_SUCCESS;
}
/* Convert Nordic error code to mbed HAL error code. */
return (result == NRF_SUCCESS) ? 0 : -1;
}
/**
* @brief Routine called to actually issue the flash access request to the SoftDevice.
*/
static uint32_t process_cmd(void)
{
uint32_t retval;
uint32_t storage_addr;
cmd_queue_element_t * p_cmd;
retval = NRF_ERROR_FORBIDDEN;
p_cmd = &m_cmd_queue.cmd[m_cmd_queue.rp];
storage_addr = p_cmd->storage_addr.block_id;
if (p_cmd->op_code == PSTORAGE_CLEAR_OP_CODE)
{
// Calculate page number before copy.
uint32_t page_number;
page_number = ((storage_addr / PSTORAGE_FLASH_PAGE_SIZE) +
m_round_val);
retval = sd_flash_page_erase(page_number);
if (NRF_SUCCESS == retval)
{
m_round_val++;
}
}
else if (p_cmd->op_code == PSTORAGE_STORE_OP_CODE)
{
uint32_t size;
uint8_t * p_data_addr = p_cmd->p_data_addr;
p_data_addr += m_round_val;
storage_addr += (p_cmd->offset + m_round_val);
size = p_cmd->size - m_round_val;
if (size < SOC_MAX_WRITE_SIZE)
{
retval = sd_flash_write(((uint32_t *)storage_addr),
(uint32_t *)p_data_addr,
size / sizeof(uint32_t));
}
else
{
retval = sd_flash_write(((uint32_t *)storage_addr),
(uint32_t *)p_data_addr,
SOC_MAX_WRITE_SIZE / sizeof(uint32_t));
}
if (retval == NRF_SUCCESS)
{
m_round_val += SOC_MAX_WRITE_SIZE;
}
}
else
{
// Should never reach here.
}
if (retval == NRF_SUCCESS)
{
m_cmd_queue.flash_access = true;
}
return retval;
}
/**
* @brief Routine called to actually issue the flash access request to the SoftDevice.
*
* @retval NRF_SUCCESS on success, else an error code indicating reason for failure.
*/
static uint32_t cmd_process(void)
{
uint32_t retval;
uint32_t storage_addr;
cmd_queue_element_t * p_cmd;
retval = NRF_ERROR_FORBIDDEN;
p_cmd = &m_cmd_queue.cmd[m_cmd_queue.rp];
storage_addr = p_cmd->storage_addr.block_id;
switch (p_cmd->op_code)
{
case PSTORAGE_STORE_OP_CODE:
{
uint32_t size;
uint32_t offset;
uint8_t * p_data_addr = p_cmd->p_data_addr;
offset = (m_round_val * SOC_MAX_WRITE_SIZE);
size = p_cmd->size - offset;
p_data_addr += offset;
storage_addr += (p_cmd->offset + offset);
if (size < SOC_MAX_WRITE_SIZE)
{
retval = sd_flash_write(((uint32_t *)storage_addr),
(uint32_t *)p_data_addr,
size / sizeof(uint32_t));
}
else
{
retval = sd_flash_write(((uint32_t *)storage_addr),
(uint32_t *)p_data_addr,
SOC_MAX_WRITE_SIZE / sizeof(uint32_t));
}
}
break;
case PSTORAGE_CLEAR_OP_CODE:
{
// Calculate page number before clearing.
uint32_t page_number;
pstorage_size_t block_size =
m_app_table[p_cmd->storage_addr.module_id].block_size;
pstorage_size_t block_count =
m_app_table[p_cmd->storage_addr.module_id].block_count;
pstorage_block_t base_address =
m_app_table[p_cmd->storage_addr.module_id].base_id;
// If the whole module should be cleared.
if (((base_address == storage_addr) && (block_size * block_count == p_cmd->size)) ||
(p_cmd->storage_addr.module_id == RAW_MODE_APP_ID))
{
page_number = ((storage_addr / PSTORAGE_FLASH_PAGE_SIZE) + m_round_val);
retval = sd_flash_page_erase(page_number);
}
// If one block is to be erased.
else
{
page_number = (storage_addr / PSTORAGE_FLASH_PAGE_SIZE);
uint32_t head_word_size = (
storage_addr -
(page_number * PSTORAGE_FLASH_PAGE_SIZE)
) / sizeof(uint32_t);
uint32_t tail_word_size = (
((page_number + 1) * PSTORAGE_FLASH_PAGE_SIZE) -
(storage_addr + p_cmd->size)
) / sizeof(uint32_t);
retval = swap_state_process(p_cmd,
page_number,
head_word_size,
tail_word_size);
}
}
break;
case PSTORAGE_UPDATE_OP_CODE:
{
uint32_t page_number = (storage_addr / PSTORAGE_FLASH_PAGE_SIZE);
uint32_t head_word_size = (
storage_addr + p_cmd->offset -
(page_number * PSTORAGE_FLASH_PAGE_SIZE)
) / sizeof(uint32_t);
uint32_t tail_word_size = (
((page_number + 1) * PSTORAGE_FLASH_PAGE_SIZE) -
(storage_addr + p_cmd->offset + p_cmd->size)
) / sizeof(uint32_t);
retval = swap_state_process(p_cmd, page_number, head_word_size, tail_word_size);
}
break;
default:
// Should never reach here.
break;
}
if (retval == NRF_SUCCESS)
{
m_cmd_queue.flash_access = true;
}
return retval;
}
/** @brief Function for handling flash accesses when using swap.
*
* __________________________________________________________
* | Page |
* |________________________________________________________|
* | head | affected body (to be updated or cleared) | tail |
* |______|__________________________________________|______|
*
* @param[in] p_cmd Queue element being processed.
* @param[in] page_number The affected page number.
* @param[in] head_word_size Size of the head in number of words.
* @param[in] tail_word_size Size of the tail in number of words.
*
* @retval NRF_SUCCESS on success, else an error code indicating reason for failure.
*/
static uint32_t swap_state_process(cmd_queue_element_t * p_cmd,
uint32_t page_number,
uint32_t head_word_size,
uint32_t tail_word_size)
{
uint32_t retval = NRF_ERROR_INTERNAL;
// Adjust entry point to state machine if needed. When we update has no head or tail its
// no need for using the swap.
if (m_swap_state == STATE_INIT)
{
if ((head_word_size == 0) && (tail_word_size == 0))
{
// Only skip swap usage if the new data fills a whole flash page.
m_swap_state = STATE_DATA_ERASE;
}
else
{
// Else start backing up application data to swap.
m_swap_state = STATE_DATA_TO_SWAP_WRITE;
}
}
switch (m_swap_state)
{
case STATE_DATA_TO_SWAP_WRITE:
// Backup previous content into swap page.
retval = sd_flash_write((uint32_t *)(PSTORAGE_SWAP_ADDR),
(uint32_t *)(page_number * PSTORAGE_FLASH_PAGE_SIZE),
PSTORAGE_FLASH_PAGE_SIZE / sizeof(uint32_t));
if (retval == NRF_SUCCESS)
{
m_swap_state = STATE_DATA_ERASE;
}
break;
case STATE_DATA_ERASE:
// Clear the application data page.
retval = sd_flash_page_erase(page_number);
if (retval == NRF_SUCCESS)
{
if (head_word_size == 0)
{
if (tail_word_size == 0)
{
if (p_cmd->op_code == PSTORAGE_CLEAR_OP_CODE)
{
m_swap_state = STATE_COMPLETE;
}
else
{
m_swap_state = STATE_NEW_BODY_WRITE;
}
}
else
{
m_swap_state = STATE_TAIL_RESTORE;
}
}
else
{
m_swap_state = STATE_HEAD_RESTORE;
}
}
break;
case STATE_HEAD_RESTORE:
// Restore head from swap to application data page.
retval = sd_flash_write((uint32_t *)(page_number * PSTORAGE_FLASH_PAGE_SIZE),
(uint32_t *)PSTORAGE_SWAP_ADDR,
head_word_size);
if (retval == NRF_SUCCESS)
{
if (tail_word_size == 0)
{
if (p_cmd->op_code == PSTORAGE_CLEAR_OP_CODE)
{
m_swap_state = STATE_SWAP_ERASE;
}
else
{
m_swap_state = STATE_NEW_BODY_WRITE;
}
}
else
{
m_swap_state = STATE_TAIL_RESTORE;
}
}
break;
case STATE_TAIL_RESTORE:
// Restore tail from swap to application data page.
retval = sd_flash_write((uint32_t *)((page_number * PSTORAGE_FLASH_PAGE_SIZE) +
(head_word_size * sizeof(uint32_t)) +
p_cmd->size),
(uint32_t *)(PSTORAGE_SWAP_ADDR +
(head_word_size * sizeof(uint32_t)) +
p_cmd->size),
tail_word_size);
if (retval == NRF_SUCCESS)
{
if (p_cmd->op_code == PSTORAGE_CLEAR_OP_CODE)
{
m_swap_state = STATE_SWAP_ERASE;
}
else
{
m_swap_state = STATE_NEW_BODY_WRITE;
}
}
break;
case STATE_NEW_BODY_WRITE:
// Write new data (body) to application data page.
retval = sd_flash_write((uint32_t *)((page_number * PSTORAGE_FLASH_PAGE_SIZE) +
(head_word_size * sizeof(uint32_t))),
(uint32_t *)p_cmd->p_data_addr,
p_cmd->size / sizeof(uint32_t));
if (retval == NRF_SUCCESS)
{
if ((head_word_size == 0) && (tail_word_size == 0))
{
m_swap_state = STATE_COMPLETE;
}
else
{
m_swap_state = STATE_SWAP_ERASE;
}
}
break;
case STATE_SWAP_ERASE:
// Clear the swap page for subsequent use.
retval = sd_flash_page_erase(PSTORAGE_SWAP_ADDR / PSTORAGE_FLASH_PAGE_SIZE);
if (retval == NRF_SUCCESS)
{
m_swap_state = STATE_COMPLETE;
}
break;
default:
break;
}
return retval;
}
/**
* @brief Function for processing of commands and issuing flash access request to the SoftDevice.
*
* @return The return value received from SoftDevice.
*/
static uint32_t cmd_process(void)
{
uint32_t retval;
uint32_t storage_addr;
cmd_queue_element_t * p_cmd;
retval = NRF_ERROR_FORBIDDEN;
p_cmd = &m_cmd_queue.cmd[m_cmd_queue.rp];
storage_addr = p_cmd->storage_addr.block_id;
switch (p_cmd->op_code)
{
case PSTORAGE_STORE_OP_CODE:
{
uint32_t size;
uint32_t offset;
uint8_t * p_data_addr = p_cmd->p_data_addr;
offset = (m_round_val * SOC_MAX_WRITE_SIZE);
size = p_cmd->size - offset;
p_data_addr += offset;
storage_addr += (p_cmd->offset + offset);
if (size < SOC_MAX_WRITE_SIZE)
{
retval = sd_flash_write(((uint32_t *)storage_addr),
(uint32_t *)p_data_addr,
size / sizeof(uint32_t));
}
else
{
retval = sd_flash_write(((uint32_t *)storage_addr),
(uint32_t *)p_data_addr,
SOC_MAX_WRITE_SIZE / sizeof(uint32_t));
}
}
break;
case PSTORAGE_CLEAR_OP_CODE:
{
uint32_t page_number;
page_number = ((storage_addr / PSTORAGE_FLASH_PAGE_SIZE) +
m_round_val);
retval = sd_flash_page_erase(page_number);
}
break;
default:
// Should never reach here.
break;
}
if (retval == NRF_SUCCESS)
{
m_cmd_queue.flash_access = true;
}
return retval;
}
