C++ (Cpp) is_aligned_32 Examples

Example #1

File: soc_flash_nrf5.c Project: sunkaizhu/zephyr

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

Example #2

File: soc_flash_nrf.c Project: kraj/zephyr

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