static int flash_nrf5_write(struct device *dev, off_t addr, const void *data, size_t len) { uint32_t addr_word; uint32_t tmp_word; void *data_word; uint32_t remaining = len; uint32_t count = 0; if (!is_addr_valid(addr, len)) { return -EINVAL; } /* Start with a word-aligned address and handle the offset */ addr_word = addr & ~0x3; /* If not aligned, read first word, update and write it back */ if (!is_aligned_32(addr)) { tmp_word = *(uint32_t *)(addr_word); count = sizeof(uint32_t) - (addr & 0x3); if (count > len) { count = len; } memcpy((uint8_t *)&tmp_word + (addr & 0x3), data, count); nvmc_wait_ready(); *(uint32_t *)addr_word = tmp_word; addr_word = addr + count; remaining -= count; } /* Write all the 4-byte aligned data */ data_word = (void *) data + count; while (remaining >= sizeof(uint32_t)) { nvmc_wait_ready(); *(uint32_t *)addr_word = *(uint32_t *)data_word; addr_word += sizeof(uint32_t); data_word += sizeof(uint32_t); remaining -= sizeof(uint32_t); } /* Write remaining data */ if (remaining) { tmp_word = *(uint32_t *)(addr_word); memcpy((uint8_t *)&tmp_word, data_word, remaining); nvmc_wait_ready(); *(uint32_t *)addr_word = tmp_word; } nvmc_wait_ready(); return 0; }
static int write_op(void *context) { struct flash_context *w_ctx = context; u32_t addr_word; u32_t tmp_word; u32_t count; #if defined(CONFIG_SOC_FLASH_NRF_RADIO_SYNC) u32_t ticks_begin = 0; u32_t ticks_diff; u32_t i = 1; if (w_ctx->enable_time_limit) { ticks_begin = ticker_ticks_now_get(); } #endif /* CONFIG_SOC_FLASH_NRF_RADIO_SYNC */ /* Start with a word-aligned address and handle the offset */ addr_word = (u32_t)w_ctx->flash_addr & ~0x3; /* If not aligned, read first word, update and write it back */ if (!is_aligned_32(w_ctx->flash_addr)) { tmp_word = *(u32_t *)(addr_word); count = sizeof(u32_t) - (w_ctx->flash_addr & 0x3); if (count > w_ctx->len) { count = w_ctx->len; } memcpy((u8_t *)&tmp_word + (w_ctx->flash_addr & 0x3), (void *)w_ctx->data_addr, count); nvmc_wait_ready(); *(u32_t *)addr_word = tmp_word; shift_write_context(count, w_ctx); #if defined(CONFIG_SOC_FLASH_NRF_RADIO_SYNC) if (w_ctx->enable_time_limit) { ticks_diff = ticker_ticks_diff_get(ticker_ticks_now_get(), ticks_begin); if (2 * ticks_diff > HAL_TICKER_US_TO_TICKS(w_ctx->slot)) { nvmc_wait_ready(); return FLASH_OP_ONGOING; } } #endif /* CONFIG_SOC_FLASH_NRF_RADIO_SYNC */ } /* Write all the 4-byte aligned data */ while (w_ctx->len >= sizeof(u32_t)) { nvmc_wait_ready(); *(u32_t *)w_ctx->flash_addr = UNALIGNED_GET((u32_t *)w_ctx->data_addr); shift_write_context(sizeof(u32_t), w_ctx); #if defined(CONFIG_SOC_FLASH_NRF_RADIO_SYNC) i++; if (w_ctx->enable_time_limit) { ticks_diff = ticker_ticks_diff_get(ticker_ticks_now_get(), ticks_begin); if (ticks_diff + ticks_diff/i > HAL_TICKER_US_TO_TICKS(w_ctx->slot)) { nvmc_wait_ready(); return FLASH_OP_ONGOING; } } #endif /* CONFIG_SOC_FLASH_NRF_RADIO_SYNC */ } /* Write remaining data */ if (w_ctx->len) { tmp_word = *(u32_t *)(w_ctx->flash_addr); memcpy((u8_t *)&tmp_word, (void *)w_ctx->data_addr, w_ctx->len); nvmc_wait_ready(); *(u32_t *)w_ctx->flash_addr = tmp_word; shift_write_context(w_ctx->len, w_ctx); } nvmc_wait_ready(); return FLASH_OP_DONE; }