diff --git a/drivers/io/io_block.c b/drivers/io/io_block.c new file mode 100644 index 0000000..198b723 --- /dev/null +++ b/drivers/io/io_block.c @@ -0,0 +1,412 @@ +/* + * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +typedef struct { + io_block_dev_spec_t *dev_spec; + uintptr_t base; + size_t file_pos; + size_t size; +} block_dev_state_t; + +#define is_power_of_2(x) ((x != 0) && ((x & (x - 1)) == 0)) + +io_type_t device_type_block(void); + +static int block_open(io_dev_info_t *dev_info, const uintptr_t spec, + io_entity_t *entity); +static int block_seek(io_entity_t *entity, int mode, ssize_t offset); +static int block_read(io_entity_t *entity, uintptr_t buffer, size_t length, + size_t *length_read); +static int block_write(io_entity_t *entity, const uintptr_t buffer, + size_t length, size_t *length_written); +static int block_close(io_entity_t *entity); +static int block_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info); +static int block_dev_close(io_dev_info_t *dev_info); + +static const io_dev_connector_t block_dev_connector = { + .dev_open = block_dev_open +}; + +static const io_dev_funcs_t block_dev_funcs = { + .type = device_type_block, + .open = block_open, + .seek = block_seek, + .size = NULL, + .read = block_read, + .write = block_write, + .close = block_close, + .dev_init = NULL, + .dev_close = block_dev_close, +}; + +static block_dev_state_t state_pool[MAX_IO_BLOCK_DEVICES]; +static io_dev_info_t dev_info_pool[MAX_IO_BLOCK_DEVICES]; + +/* Track number of allocated block state */ +static unsigned int block_dev_count; + +io_type_t device_type_block(void) +{ + return IO_TYPE_BLOCK; +} + +/* Locate a block state in the pool, specified by address */ +static int find_first_block_state(const io_block_dev_spec_t *dev_spec, + unsigned int *index_out) +{ + int result = -ENOENT; + for (int index = 0; index < MAX_IO_BLOCK_DEVICES; ++index) { + /* dev_spec is used as identifier since it's unique */ + if (state_pool[index].dev_spec == dev_spec) { + result = 0; + *index_out = index; + break; + } + } + return result; +} + +/* Allocate a device info from the pool and return a pointer to it */ +static int allocate_dev_info(io_dev_info_t **dev_info) +{ + int result = -ENOMEM; + assert(dev_info != NULL); + + if (block_dev_count < MAX_IO_BLOCK_DEVICES) { + unsigned int index = 0; + result = find_first_block_state(NULL, &index); + assert(result == 0); + /* initialize dev_info */ + dev_info_pool[index].funcs = &block_dev_funcs; + dev_info_pool[index].info = (uintptr_t)&state_pool[index]; + *dev_info = &dev_info_pool[index]; + ++block_dev_count; + } + + return result; +} + + +/* Release a device info to the pool */ +static int free_dev_info(io_dev_info_t *dev_info) +{ + int result; + unsigned int index = 0; + block_dev_state_t *state; + assert(dev_info != NULL); + + state = (block_dev_state_t *)dev_info->info; + result = find_first_block_state(state->dev_spec, &index); + if (result == 0) { + /* free if device info is valid */ + memset(state, 0, sizeof(block_dev_state_t)); + memset(dev_info, 0, sizeof(io_dev_info_t)); + --block_dev_count; + } + + return result; +} + +static int block_open(io_dev_info_t *dev_info, const uintptr_t spec, + io_entity_t *entity) +{ + block_dev_state_t *cur; + io_block_spec_t *region; + + assert((dev_info->info != (uintptr_t)NULL) && + (spec != (uintptr_t)NULL) && + (entity->info == (uintptr_t)NULL)); + + region = (io_block_spec_t *)spec; + cur = (block_dev_state_t *)dev_info->info; + assert(((region->offset % cur->dev_spec->block_size) == 0) && + ((region->length % cur->dev_spec->block_size) == 0)); + + cur->base = region->offset; + cur->size = region->length; + cur->file_pos = 0; + + entity->info = (uintptr_t)cur; + return 0; +} + +/* parameter offset is relative address at here */ +static int block_seek(io_entity_t *entity, int mode, ssize_t offset) +{ + block_dev_state_t *cur; + + assert(entity->info != (uintptr_t)NULL); + + cur = (block_dev_state_t *)entity->info; + assert((offset >= 0) && (offset < cur->size)); + + switch (mode) { + case IO_SEEK_SET: + cur->file_pos = offset; + break; + case IO_SEEK_CUR: + cur->file_pos += offset; + break; + default: + return -EINVAL; + } + assert(cur->file_pos < cur->size); + return 0; +} + +static int block_read(io_entity_t *entity, uintptr_t buffer, size_t length, + size_t *length_read) +{ + block_dev_state_t *cur; + io_block_spec_t *buf; + io_block_ops_t *ops; + size_t aligned_length, skip, count, left, padding, block_size; + int lba; + + assert(entity->info != (uintptr_t)NULL); + cur = (block_dev_state_t *)entity->info; + ops = &(cur->dev_spec->ops); + buf = &(cur->dev_spec->buffer); + block_size = cur->dev_spec->block_size; + assert((length <= cur->size) && + (length > 0) && + (ops->read != 0)); + + skip = cur->file_pos % block_size; + aligned_length = ((skip + length) + (block_size - 1)) & + ~(block_size - 1); + padding = aligned_length - (skip + length); + left = aligned_length; + do { + lba = (cur->file_pos + cur->base) / block_size; + if (left >= buf->length) { + /* Since left is larger, it's impossible to padding. */ + if (skip) { + /* + * The beginning address (file_pos) isn't + * aligned with block size, we need to use + * block buffer to read block. Since block + * device is always relied on DMA operation. + */ + count = ops->read(lba, buf->offset, + buf->length); + } else { + count = ops->read(lba, buffer, buf->length); + } + assert(count == buf->length); + cur->file_pos += count - skip; + if (skip) { + /* + * Since it's not aligned with block size, + * block buffer is used to store data. + */ + memcpy((void *)buffer, + (void *)(buf->offset + skip), + count - skip); + } + left = left - (count - skip); + } else { + if (skip || padding) { + /* + * The beginning address (file_pos) isn't + * aligned with block size, we have to read + * full block by block buffer instead. + * The size isn't aligned with block size. + * Use block buffer to avoid overflow. + */ + count = ops->read(lba, buf->offset, left); + } else + count = ops->read(lba, buffer, left); + assert(count == left); + left = left - (skip + padding); + cur->file_pos += left; + if (skip || padding) { + /* + * Since it's not aligned with block size, + * block buffer is used to store data. + */ + memcpy((void *)buffer, + (void *)(buf->offset + skip), + left); + } + /* It's already the last block operation */ + left = 0; + } + skip = cur->file_pos % block_size; + } while (left > 0); + *length_read = length; + + return 0; +} + +static int block_write(io_entity_t *entity, const uintptr_t buffer, + size_t length, size_t *length_written) +{ + block_dev_state_t *cur; + io_block_spec_t *buf; + io_block_ops_t *ops; + size_t aligned_length, skip, count, left, padding, block_size; + int lba; + + assert(entity->info != (uintptr_t)NULL); + cur = (block_dev_state_t *)entity->info; + ops = &(cur->dev_spec->ops); + buf = &(cur->dev_spec->buffer); + block_size = cur->dev_spec->block_size; + assert((length <= cur->size) && + (length > 0) && + (ops->read != 0) && + (ops->write != 0)); + + skip = cur->file_pos % block_size; + aligned_length = ((skip + length) + (block_size - 1)) & + ~(block_size - 1); + padding = aligned_length - (skip + length); + left = aligned_length; + do { + lba = (cur->file_pos + cur->base) / block_size; + if (left >= buf->length) { + /* Since left is larger, it's impossible to padding. */ + if (skip) { + /* + * The beginning address (file_pos) isn't + * aligned with block size, we need to use + * block buffer to write block. Since block + * device is always relied on DMA operation. + */ + count = ops->read(lba, buf->offset, + buf->length); + assert(count == buf->length); + memcpy((void *)(buf->offset + skip), + (void *)buffer, + count - skip); + count = ops->write(lba, buf->offset, + buf->length); + } else + count = ops->write(lba, buffer, buf->length); + assert(count == buf->length); + cur->file_pos += count - skip; + left = left - (count - skip); + } else { + if (skip || padding) { + /* + * The beginning address (file_pos) isn't + * aligned with block size, we need to avoid + * poluate data in the beginning. Reading and + * skipping the beginning is the only way. + * The size isn't aligned with block size. + * Use block buffer to avoid overflow. + */ + count = ops->read(lba, buf->offset, left); + assert(count == left); + memcpy((void *)(buf->offset + skip), + (void *)buffer, + left - skip - padding); + count = ops->write(lba, buf->offset, left); + } else + count = ops->write(lba, buffer, left); + assert(count == left); + cur->file_pos += left - (skip + padding); + /* It's already the last block operation */ + left = 0; + } + skip = cur->file_pos % block_size; + } while (left > 0); + *length_written = length; + return 0; +} + +static int block_close(io_entity_t *entity) +{ + entity->info = (uintptr_t)NULL; + return 0; +} + +static int block_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info) +{ + block_dev_state_t *cur; + io_block_spec_t *buffer; + io_dev_info_t *info; + size_t block_size; + int result; + + assert(dev_info != NULL); + result = allocate_dev_info(&info); + if (result) + return -ENOENT; + + cur = (block_dev_state_t *)info->info; + /* dev_spec is type of io_block_dev_spec_t. */ + cur->dev_spec = (io_block_dev_spec_t *)dev_spec; + buffer = &(cur->dev_spec->buffer); + block_size = cur->dev_spec->block_size; + assert((block_size > 0) && + (is_power_of_2(block_size) != 0) && + ((buffer->offset % block_size) == 0) && + ((buffer->length % block_size) == 0)); + + *dev_info = info; /* cast away const */ + (void)block_size; + (void)buffer; + return 0; +} + +static int block_dev_close(io_dev_info_t *dev_info) +{ + return free_dev_info(dev_info); +} + +/* Exported functions */ + +/* Register the Block driver with the IO abstraction */ +int register_io_dev_block(const io_dev_connector_t **dev_con) +{ + int result; + + assert(dev_con != NULL); + + /* + * Since dev_info isn't really used in io_register_device, always + * use the same device info at here instead. + */ + result = io_register_device(&dev_info_pool[0]); + if (result == 0) + *dev_con = &block_dev_connector; + return result; +} diff --git a/include/drivers/io/io_block.h b/include/drivers/io/io_block.h new file mode 100644 index 0000000..ebf43cd --- /dev/null +++ b/include/drivers/io/io_block.h @@ -0,0 +1,52 @@ +/* + * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef __IO_BLOCK_H__ +#define __IO_BLOCK_H__ + +#include + +/* block devices ops */ +typedef struct io_block_ops { + size_t (*read)(int lba, uintptr_t buf, size_t size); + size_t (*write)(int lba, const uintptr_t buf, size_t size); +} io_block_ops_t; + +typedef struct io_block_dev_spec { + io_block_spec_t buffer; + io_block_ops_t ops; + size_t block_size; +} io_block_dev_spec_t; + +struct io_dev_connector; + +int register_io_dev_block(const struct io_dev_connector **dev_con); + +#endif /* __IO_BLOCK_H__ */ diff --git a/include/drivers/io/io_storage.h b/include/drivers/io/io_storage.h index 970ab2c..243f688 100644 --- a/include/drivers/io/io_storage.h +++ b/include/drivers/io/io_storage.h @@ -44,6 +44,7 @@ IO_TYPE_SEMIHOSTING, IO_TYPE_MEMMAP, IO_TYPE_FIRMWARE_IMAGE_PACKAGE, + IO_TYPE_BLOCK, IO_TYPE_MAX } io_type_t;