- uint16_t block;
- uint16_t this_len;
- uint16_t this_off;
-
- ao_flash_setup();
- if (pos >= FLASH_DATA_SIZE || pos + len > FLASH_DATA_SIZE)
- return 0;
- while (len) {
-
- /* Compute portion of transfer within
- * a single block
- */
- this_off = (uint16_t) pos & FLASH_BLOCK_MASK;
- this_len = FLASH_BLOCK_SIZE - this_off;
- block = (uint16_t) (pos >> FLASH_BLOCK_SHIFT);
- if (this_len > len)
- this_len = len;
-
- /* Transfer the data */
- ao_mutex_get(&ao_flash_mutex); {
- if (this_len != FLASH_BLOCK_SIZE)
- ao_flash_fill(block);
- else {
- ao_flash_flush_internal();
- ao_flash_block = block;
- }
- memcpy(ao_flash_data + this_off, buf, this_len);
- ao_flash_block_dirty = 1;
- } ao_mutex_put(&ao_flash_mutex);
-
- /* See how much is left */
- buf += this_len;
- len -= this_len;
- pos += this_len;
- }
+ uint16_t block = (uint16_t) (pos >> ao_flash_block_shift);
+
+ /* Transfer the data */
+ ao_mutex_get(&ao_flash_mutex); {
+ if (len != ao_flash_block_size)
+ ao_flash_fill(block);
+ else {
+ ao_flash_flush_internal();
+ ao_flash_block = block;
+ }
+ memcpy(ao_flash_data + (uint16_t) (pos & ao_flash_block_mask),
+ buf,
+ len);
+ ao_flash_block_dirty = 1;
+ } ao_mutex_put(&ao_flash_mutex);