src/drivers/storage/flash.c

/*
 * Copyright 2016 Google Inc.
 *
 * See file CREDITS for list of people who contributed to this
 * project.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but without any warranty; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 * MA 02111-1307 USA
 */

#include <assert.h>
#include <stdio.h>
#include <string.h>

#include "base/algorithm.h"
#include "base/container_of.h"
#include "base/die.h"
#include "base/xalloc.h"
#include "drivers/storage/flash.h"

static int flash_storage_read(StorageOps *me, void *buffer,
			      uint64_t offset, size_t size)
{
	FlashStorage *storage = container_of(me, FlashStorage, ops);

	void *result = flash_read(storage->flash, offset, size);
	if (!result)
		return 1;

	memcpy(buffer, result, size);
	return 0;
}

static int flash_storage_write(StorageOps *me, const void *buffer,
			       uint64_t offset, size_t size)
{
	FlashStorage *storage = container_of(me, FlashStorage, ops);
	if (offset + size > storage->flash->size(storage->flash)) {
		printf("Write beyond end of flash storage.\n");
		return 1;
	}

	const int64_t erase_size = flash_erase_size(storage->flash);
	if (erase_size < 0)
		return 1;
	// Make sure the erase size is a power of 2.
	assert(((erase_size - 1) & erase_size) == 0);
	const uint64_t erase_mask = ~((uint64_t)erase_size - 1);

	// Round the original offset down to an erase boundary.
	uint64_t aligned_offset = offset & erase_mask;
	// Calculate how much below the original offset that is.
	uint64_t extra_low = offset - aligned_offset;
	// Add that into the original size.
	uint64_t aligned_size = size + extra_low;
	// Now round the size up so that it's a multiple of the erase size.
	aligned_size = (aligned_size + ~erase_mask) & erase_mask;

	uint64_t input_pos = 0;

	// Process one flash sector at a time.
	for (uint64_t pos = aligned_offset;
	     pos < aligned_offset + aligned_size;
	     pos += erase_size) {

		// Read the existing contents.
		uint8_t *existing = flash_read(storage->flash, pos, erase_size);
		if (!existing)
			return 1;
		// Set up a pointer to data to write to this sector.
		uint8_t *new_data = (uint8_t *)buffer + input_pos;
		// Figure out how many bytes long that is.
		int bytes = MIN(erase_size - extra_low, size - input_pos);

		// Loop until we would hit the end of either buffer.
		for (int i = 0; i < bytes; i++) {
			// We'll assume bits can change from 1 to 0 but not the
			// other way.
			uint8_t original = existing[i + extra_low];
			uint8_t new = new_data[i];
			if ((original & new) != new) {
				// A straight overwrite isn't possible, so
				// we need to erase this sector.
				if (flash_erase(storage->flash,
						pos, erase_size))
					return 1;
				// We already erased, so we can stop now.
				break;
			}
		}

		// Prepare the chunk of data to write.
		memcpy(existing + extra_low, new_data, bytes);
		uint8_t *write_buf = existing;
		int write_size = erase_size;
		uint64_t write_pos = pos;

		// We can skip over bytes that are all ones on either end.
		while (write_size && write_buf[write_size - 1] == 0xff)
			write_size--;
		while (write_size && write_buf[0] == 0xff) {
			write_buf++;
			write_size--;
			write_pos++;
		}

		// Actually do the write.
		if (flash_write(storage->flash, write_buf,
				write_pos, write_size)) {
			return 1;
		}

		// Update our position in the input buffer.
		input_pos += bytes;
		// The start is no longer misaligned.
		extra_low = 0;
	}

	return 0;
}

static int flash_storage_size(StorageOps *me)
{
	FlashStorage *storage = container_of(me, FlashStorage, ops);
	return flash_size(storage->flash);
}

FlashStorage *new_flash_storage(FlashOps *flash)
{
	FlashStorage *storage = xzalloc(sizeof(*storage));

	storage->ops.read = &flash_storage_read;
	storage->ops.write = &flash_storage_write;
	storage->ops.size = &flash_storage_size;

	storage->flash = flash;

	return storage;
}