common/blkdev-common.c

#ifdef USE_HOSTCC

#include <stdlib.h>
#include <errno.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "blkdev.h"
#ifdef DEBUG
#define debug(...)			printf(...)
#else
#define debug(...)				do { } while(0)
#endif
#define led_sequence_updater(x)		do { } while(0)
#else

#include <common.h>
#include <led_control.h>
#include <blkdev.h>

#endif


static struct cp_flash_desc *flash_layout;

static int init_flashes(void);
static int _append_blkdev(struct cp_blkdev *list, struct cp_blkdev *blkdev);


int partition_erase(struct cp_part * part)
{
	if (!part) {
		return -EINVAL;
	}

	return part->blkdev->block_erase(part->blkdev, part->start, part->size);
}
/* offset is number of bytes from start of partition
 * length is the number of bytes in data
 * data is the data
 */
ssize_t partition_write(struct cp_part * part, size_t offset, size_t length, const uint8_t* data)
{

	debug("%s(part=%p, offset=%zu, length=%zu, data=%p)\n", __func__, part, offset,length, data);

	if(!part || !part->blkdev)
		return -EINVAL;

	/* First, we need to figure out how many blocks we'll need to write using the blkdev */
	if (length == 0 || data == NULL)
		return 0;

	size_t block_start_offset = 0;
	int blocks_read = 0;
	size_t blocks_to_write = 0;
	size_t blocks_written = 0;
	/*size_t pad_len = padding - length; */
	size_t bytes_written = 0;
	size_t part_start_byte = part->start * (part->blkdev->sector_size);
	size_t data_write_start = part_start_byte + offset;

	led_sequence_updater(FLASH_WRITE_SEQUENCE);
	block_start_offset = data_write_start/part->blkdev->sector_size;
	/* Determine the offset within the start block we'll be writing */
	offset = data_write_start - block_start_offset * part->blkdev->sector_size;


	debug("Writing to partition %s will start with block %zu, offset by %zu bytes\n", part->name, block_start_offset, offset); 

	uint8_t * sector_buffer = malloc(sizeof(uint8_t)* part->blkdev->sector_size);

	uint32_t bytes_copied = 0;
	if (offset > 0) {
		led_sequence_updater(FLASH_WRITE_SEQUENCE);
		if ((blocks_read = part->blkdev->block_read(part->blkdev, block_start_offset, 1, sector_buffer)) != 1) {
			printf("Expected to read %d blocks, read %d instead\n", 1, blocks_read);
		}
		bytes_copied = MIN(length, (part->blkdev->sector_size - offset));
		memcpy(sector_buffer + offset, (uint8_t*)data, bytes_copied );

		debug("Copied from data(%p) to sector_buffer(%p) %zu bytes\n", data, sector_buffer, bytes_copied);
		debug("reducing length by %zu bytes to %zu; advancing data from %p to %p\n",  offset, length-bytes_copied, data, data+bytes_copied);
		length -= bytes_copied;
		data += bytes_copied;
		led_sequence_updater(FLASH_WRITE_SEQUENCE);
		block_start_offset += part->blkdev->block_write(part->blkdev, block_start_offset, 1, sector_buffer);
		bytes_written += bytes_copied;
	}
	while (length >= part->blkdev->sector_size) {
		led_sequence_updater(FLASH_WRITE_SEQUENCE);
		blocks_to_write = MIN((length / part->blkdev->sector_size), MAX_IO_CHUNK_SIZE);
		blocks_written = part->blkdev->block_write(part->blkdev, block_start_offset, blocks_to_write, data);
		block_start_offset += blocks_written;
		data += part->blkdev->sector_size * blocks_written;
		length -= part->blkdev->sector_size * blocks_written;
		bytes_copied +=  part->blkdev->sector_size * blocks_written;
	}

	if (length > 0 ) {
		led_sequence_updater(FLASH_WRITE_SEQUENCE);
		part->blkdev->block_read(part->blkdev, block_start_offset, 1, sector_buffer);
		memcpy(sector_buffer, data, length);
		led_sequence_updater(FLASH_WRITE_SEQUENCE);
		part->blkdev->block_write(part->blkdev, block_start_offset, 1, sector_buffer);
	}
	free(sector_buffer);
	led_sequence_updater(FLASH_WRITE_SEQUENCE);
	return bytes_copied;

}

ssize_t partition_read(struct cp_part * part, size_t offset, size_t length, uint8_t * data)
{
	ssize_t bytes_read = 0;
	if ( length == 0 || data == NULL || part == NULL || part->blkdev == NULL)
		return -EINVAL;

	size_t blocks_to_read = 0;
	size_t blocks_read = 0;
	size_t data_read_start = (part->start * part->blkdev->sector_size) + offset;
	size_t block_start_offset = data_read_start/part->blkdev->sector_size;
	uint8_t * sector_buffer = (uint8_t*) malloc (sizeof(uint8_t)* part->blkdev->sector_size);

	debug("%s(part=0x%p, offset=%zu, length=%zu, data=0x%p)\n", __func__, part, offset, length, data);
	offset = data_read_start - block_start_offset * part->blkdev->sector_size;
	debug("Reading partition %s will start at block %zu offset by %zu bytes\n", part->name, block_start_offset, offset); 
	if (offset > 0) {
		led_sequence_updater(FLASH_READ_SEQUENCE);
		block_start_offset += part->blkdev->block_read(part->blkdev, block_start_offset, 1, sector_buffer);
		bytes_read = MIN(length, part->blkdev->sector_size - offset);
		memcpy(data, sector_buffer + offset,  bytes_read);
		length -= bytes_read;
		data += bytes_read;
	}

	while (length >= part->blkdev->sector_size) {
		led_sequence_updater(FLASH_READ_SEQUENCE);
		blocks_to_read = MIN((length / part->blkdev->sector_size), MAX_IO_CHUNK_SIZE);
		debug("Reading %zu blocks\n", blocks_to_read);
		blocks_read = part->blkdev->block_read(part->blkdev, block_start_offset, blocks_to_read, data);
		if (blocks_read != blocks_to_read) {
			debug("Warning, expected to read %zu blocks, read %zu instead\n", blocks_to_read, blocks_read);
		}
		if (blocks_read == 0)
			break;
		block_start_offset += blocks_read;
		data += part->blkdev->sector_size * blocks_read;
		length -= part->blkdev->sector_size * blocks_read;
		bytes_read +=  part->blkdev->sector_size * blocks_read;
	}
	if (length > 0) {
		led_sequence_updater(FLASH_READ_SEQUENCE);
		block_start_offset += part->blkdev->block_read(part->blkdev, block_start_offset, 1, sector_buffer);
		memcpy(data, sector_buffer, length);
		bytes_read += length;
	}
	free(sector_buffer);
	led_sequence_updater(FLASH_READ_SEQUENCE);
	return bytes_read;
}

int flash_desc_init_complete() 
{
	return (!!!flash_layout);
}
static int _append_blkdev(struct cp_blkdev * blkdev_list, struct cp_blkdev * blkdev)
{

	if (blkdev_list == NULL) {
		printf("_append_blkdev passed empty blkdev_list\n");
		return -EINVAL;
	}
	if (blkdev_list->next == NULL) {
		blkdev_list->next = blkdev;
		return 0;
	}
	return _append_blkdev(blkdev_list->next, blkdev);
}

int register_blkdev(struct cp_blkdev *blkdev)
{
	int ret = 0;

	if (blkdev == NULL) {
		printf("Attempt to register NULL blkdev\n");
		return -EINVAL;
	}
	if (!flash_layout) {
		ret = init_flashes();
		if (ret != 0) {
			printf("Failed to init flashes\n");
			return ret;
		}
	}
	if (flash_layout->blkdevs == NULL) {
		flash_layout->blkdevs = blkdev;
		return 0;
	}
	return _append_blkdev(flash_layout->blkdevs, blkdev);
}

static int init_flashes(void)
{
	if (flash_layout) {
		return 0;
	}
	flash_layout = (struct cp_flash_desc *) malloc(sizeof(struct cp_flash_desc));
	if (flash_layout == NULL) {
		printf(" Warning, failed to allocate flash_layout!\n");
		return -ENOMEM;
	}
	if (memset(flash_layout, 0, sizeof(struct cp_flash_desc)) == NULL) {
		printf("Warning memset failed to zero flash_layout!\n");
		return -EINVAL;
	}
	return 0;
}

struct cp_part *get_partition(const char *part_name)
{
	if (!part_name)
		return NULL;

	if (flash_layout == NULL) {
		return NULL;
	}
	if (flash_layout->blkdevs == NULL) {
		return NULL;
	}

	struct cp_blkdev *blkdev = flash_layout->blkdevs;

	while (blkdev) {
		struct cp_part *current = blkdev->partitions;

		while (current) {
			if (strncmp(part_name, current->name, MAX_PART_NAME_LENGTH) == 0)
				return current;
			current = current->next;
		}
		blkdev = blkdev->next;
	}
	return NULL;
}

static void insert_partition(struct cp_blkdev *blkdev, struct cp_part *part)
{
	struct cp_part *current = blkdev->partitions;

	if (blkdev->partitions == NULL) {
		blkdev->partitions = part;
		return;
	}

	while (current->next) {
		current = current->next;
	}
	current->next = part;

}
void print_partition(struct cp_part *part)
{
	if (part == NULL) {
		return;
	}
	/* Partition Name, start byte, end byte */
	printf("%32s\t%32s\t0x%08zX-0x%08zX\n", part->blkdev->name,  part->name, part->start*part->blkdev->sector_size, (part->start + part->size)*part->blkdev->sector_size);

}

/* Start and size are in blocks */
int add_partition_to_blkdev(struct cp_blkdev *blkdev, const char *name, uint32_t start, uint32_t size)
{
	if (!flash_layout || !name) {
		return -EINVAL;
	}

	struct cp_part *part = get_partition(name);

	if (part != NULL) {
		printf("Partition name %s already exists in blkdev %s!\n", name, part->blkdev->name);
		return -EEXIST;
	}

	part = (struct cp_part *) malloc(sizeof(struct cp_part));
	if (part == NULL) {
		return -ENOMEM;
	}
	memset(part, 0, sizeof(struct cp_part));

	strncpy(part->name, name, MAX_PART_NAME_LENGTH);
	part->start = start;
	part->size = size;
	insert_partition(blkdev, part);
	part->blkdev = blkdev;
	debug("Partition %s added to %s with start block at 0x%zX and size=0x%zX blocks (%zX bytes)\n", part->name, blkdev->name, part->start, part->size, PARTITION_SIZE(part));

	return 0;
}

void print_partitions(void)
{
	if (!flash_layout) {
		return;
	}
	printf("Blockdev\tName\t\t\tStart\tEnd\tSize\n"); 
	struct cp_blkdev * blkdev = flash_layout->blkdevs;
	while (blkdev) {
		struct cp_part *  part = blkdev->partitions;
		while(part) {
			printf("%s\t%s\t\t\t%zu\t%zu\t%zu\n", blkdev->name, part->name, part->start, part->start + part->size, part->size);
			part = part->next;
		}
		blkdev = blkdev->next;
	}
}

void partition_free(struct cp_part *partition)
{
	if (partition == NULL) {
		return;
	}
	if (partition->next) {
		if (partition->blkdev == partition->next->blkdev) {
			partition_free(partition->next);
			partition->next = NULL;
		}
	}
	free(partition);
}

void blkdev_free(struct cp_blkdev *blkdev)
{
	if (blkdev == NULL)
		return;

	if (blkdev->partitions) {
		partition_free(blkdev->partitions);
		blkdev->partitions = NULL;
	}

	if (blkdev->next) {
		blkdev_free(blkdev->next);
		blkdev->next = NULL;
	}

	free(blkdev);

}

void deinit_flashes(void)
{
	if (flash_layout == NULL) {
		return;
	}
	if (flash_layout->blkdevs) {
		blkdev_free(flash_layout->blkdevs);
		flash_layout->blkdevs = NULL;
	}
	free(flash_layout);
	return;
}