int flash_store_contiguous_blocks_write_common()

in core/flash/flash_store_contiguous_blocks.c [64:251]

• 155 lines of code
• 33 McCabe index (conditional complexity)

int flash_store_contiguous_blocks_write_common (const struct flash_store_contiguous_blocks *flash,
	int id, const uint8_t *data, size_t length, const uint8_t *extra_data, size_t extra_length)
{
	int base_offset;
	int offset;
	int status;

	base_offset = id * flash->state->block_size;
	if (flash->decreasing) {
		base_offset = -base_offset;
	}
	offset = base_offset;

	status = flash_sector_erase_region (flash->flash, flash->base_addr + base_offset,
		flash->state->block_size);
	if (status != 0) {
		return status;
	}

#ifdef FLASH_STORE_SUPPORT_NO_PARTIAL_PAGE_WRITE
	if (flash->state->page_buffer) {
		/* It is necessary to ensure that no page is written more than once.  Internal buffering is
		 * necessary in three cases:
		 * 1. The first page of data for variable length storage, since that holds the data header.
		 * 2. The last page of data for storage that appends extra data to the end, but only in
		 * cases where the stored data does not align to page boundaries.
		 * 3. The entire amount of data (header, data, and extra data) fits into the first page.
		 *
		 * Data that is naturally aligned to full pages is not buffered and is written directly from
		 * the source data. */
		struct flash_store_header header = {
			.header_len = FLASH_STORE_HEADER_LENGTH,
			.marker = FLASH_STORE_HEADER_MARKER,
			.length = length
		};
		size_t header_len = (!flash->variable) ? 0 :
				(flash->state->old_header) ? sizeof (header.length) : sizeof (header);
		size_t first = flash->state->page_size - header_len;
		size_t remain = FLASH_REGION_OFFSET (length + header_len, flash->state->page_size);
		size_t write_extra = flash->state->page_size - remain;
		size_t middle = (length > remain) ? (length - remain) : length;
		bool extra_first = false;

		if (flash->variable) {
			if (middle > first) {
				middle -= first;
			}
			else {
				if (middle < first) {
					/* All the data fits into the first flash page. */
					extra_first = true;
				}
				first = middle;
				middle = 0;
			}
		}
		else {
			if (middle < first) {
				middle = 0;
			}
			first = 0;
		}

		if (extra_length == 0) {
			if (!extra_first) {
				middle += remain;
			}
			remain = 0;
			write_extra = 0;
		}
		else if (write_extra > extra_length) {
			write_extra = extra_length;
		}

		/* Write full pages of source data. */
		offset += first + header_len;

		if (middle != 0) {
			status = flash_write_and_verify (flash->flash, flash->base_addr + offset, &data[first],
				middle);
			if (status != 0) {
				return status;
			}

			offset += middle;
		}

		/* Write the additional data appended to the end. */
		if (!extra_first && (extra_length != 0)) {
			platform_mutex_lock (&flash->state->lock);
			memcpy (flash->state->page_buffer, &data[length - remain], remain);
			memcpy (&flash->state->page_buffer[remain], extra_data, write_extra);

			status = flash_write_and_verify (flash->flash, flash->base_addr + offset,
				flash->state->page_buffer, remain + write_extra);
			platform_mutex_unlock (&flash->state->lock);
			if (status != 0) {
				return status;
			}

			offset += remain + write_extra;
		}
		else {
			offset += write_extra;
		}

		if (write_extra < extra_length) {
			status = flash_write_and_verify (flash->flash, flash->base_addr + offset,
				&extra_data[write_extra], extra_length - write_extra);
			if (status != 0) {
				return status;
			}
		}

		/* For variable storage, write the first page with the data header. */
		if (first != 0) {
			platform_mutex_lock (&flash->state->lock);
			if (!flash->state->old_header) {
				memcpy (flash->state->page_buffer, (uint8_t*) &header, sizeof (header));
			}
			else {
				memcpy (flash->state->page_buffer, (uint8_t*) &header.length,
					sizeof (header.length));
			}
			memcpy (&flash->state->page_buffer[header_len], data, first);
			if (extra_first) {
				memcpy (&flash->state->page_buffer[header_len + first], extra_data, write_extra);
				header_len += write_extra;
			}

			status = flash_write_and_verify (flash->flash, flash->base_addr + base_offset,
				flash->state->page_buffer, first + header_len);
			platform_mutex_unlock (&flash->state->lock);
			if (status != 0) {
				return status;
			}
		}
	}
	else
#endif
	{
		/* Each page can be written multiple times without erasing. */
		if (flash->variable) {
			if (!flash->state->old_header) {
				offset += FLASH_STORE_HEADER_LENGTH;
			}
			else {
				offset += sizeof (uint16_t);
			}
		}

		status = flash_write_and_verify (flash->flash, flash->base_addr + offset, data, length);
		if (status != 0) {
			return status;
		}

		if (extra_data) {
			offset += length;
			status = flash_write_and_verify (flash->flash, flash->base_addr + offset, extra_data,
				extra_length);
			if (status != 0) {
				return status;
			}
		}

		if (flash->variable) {
			struct flash_store_header header = {
				.header_len = FLASH_STORE_HEADER_LENGTH,
				.marker = FLASH_STORE_HEADER_MARKER,
				.length = length
			};

			if (!flash->state->old_header) {
				status = flash_write_and_verify (flash->flash, flash->base_addr + base_offset,
					(uint8_t*) &header, sizeof (header));
			}
			else {
				status = flash_write_and_verify (flash->flash, flash->base_addr + base_offset,
					(uint8_t*) &header.length, sizeof (header.length));
			}
			if (status != 0) {
				return status;
			}
		}
	}

	return 0;
}