diff --git a/drivers/st/io/io_stm32image.c b/drivers/st/io/io_stm32image.c new file mode 100644 index 000000000..e6798e047 --- /dev/null +++ b/drivers/st/io/io_stm32image.c @@ -0,0 +1,384 @@ +/* + * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static uintptr_t backend_dev_handle; +static uintptr_t backend_image_spec; +static uint32_t *stm32_img; +static uint8_t first_lba_buffer[MAX_LBA_SIZE] __aligned(4); +static struct stm32image_part_info *current_part; + +/* STM32 Image driver functions */ +static int stm32image_dev_open(const uintptr_t init_params, + io_dev_info_t **dev_info); +static int stm32image_partition_open(io_dev_info_t *dev_info, + const uintptr_t spec, io_entity_t *entity); +static int stm32image_partition_size(io_entity_t *entity, size_t *length); +static int stm32image_partition_read(io_entity_t *entity, uintptr_t buffer, + size_t length, size_t *length_read); +static int stm32image_partition_close(io_entity_t *entity); +static int stm32image_dev_init(io_dev_info_t *dev_info, + const uintptr_t init_params); +static int stm32image_dev_close(io_dev_info_t *dev_info); + +/* Identify the device type as a virtual driver */ +static io_type_t device_type_stm32image(void) +{ + return IO_TYPE_STM32IMAGE; +} + +static const io_dev_connector_t stm32image_dev_connector = { + .dev_open = stm32image_dev_open +}; + +static const io_dev_funcs_t stm32image_dev_funcs = { + .type = device_type_stm32image, + .open = stm32image_partition_open, + .size = stm32image_partition_size, + .read = stm32image_partition_read, + .close = stm32image_partition_close, + .dev_init = stm32image_dev_init, + .dev_close = stm32image_dev_close, +}; + +static io_dev_info_t stm32image_dev_info = { + .funcs = &stm32image_dev_funcs, + .info = (uintptr_t)0, +}; + +static struct stm32image_device_info stm32image_dev; + +static int get_part_idx_by_binary_type(uint32_t binary_type) +{ + int i; + + for (i = 0; i < STM32_PART_NUM; i++) { + if (stm32image_dev.part_info[i].binary_type == binary_type) { + return i; + } + } + + return -EINVAL; +} + +/* Open a connection to the STM32IMAGE device */ +static int stm32image_dev_open(const uintptr_t init_params, + io_dev_info_t **dev_info) +{ + int i; + struct stm32image_device_info *device_info = + (struct stm32image_device_info *)init_params; + + assert(dev_info != NULL); + *dev_info = (io_dev_info_t *)&stm32image_dev_info; + + stm32image_dev.device_size = device_info->device_size; + stm32image_dev.lba_size = device_info->lba_size; + + for (i = 0; i < STM32_PART_NUM; i++) { + memcpy(stm32image_dev.part_info[i].name, + device_info->part_info[i].name, MAX_PART_NAME_SIZE); + stm32image_dev.part_info[i].part_offset = + device_info->part_info[i].part_offset; + stm32image_dev.part_info[i].bkp_offset = + device_info->part_info[i].bkp_offset; + } + + return 0; +} + +/* Do some basic package checks */ +static int stm32image_dev_init(io_dev_info_t *dev_info, + const uintptr_t init_params) +{ + int result; + + if ((backend_dev_handle != 0U) || (backend_image_spec != 0U)) { + ERROR("STM32 Image io supports only one session\n"); + return -ENOMEM; + } + + /* Obtain a reference to the image by querying the platform layer */ + result = plat_get_image_source(STM32_IMAGE_ID, &backend_dev_handle, + &backend_image_spec); + if (result != 0) { + ERROR("STM32 image error (%i)\n", result); + return -EINVAL; + } + + return result; +} + +/* Close a connection to the STM32 Image device */ +static int stm32image_dev_close(io_dev_info_t *dev_info) +{ + backend_dev_handle = 0U; + backend_image_spec = 0U; + stm32_img = NULL; + + return 0; +} + +/* Open a partition */ +static int stm32image_partition_open(io_dev_info_t *dev_info, + const uintptr_t spec, io_entity_t *entity) +{ + const struct stm32image_part_info *partition_spec; + int idx; + + assert(entity != NULL); + + partition_spec = (struct stm32image_part_info *)spec; + assert(partition_spec != NULL); + + idx = get_part_idx_by_binary_type(partition_spec->binary_type); + if ((idx < 0) || (idx > STM32_PART_NUM)) { + ERROR("Wrong partition index (%d)\n", idx); + return -EINVAL; + } + + current_part = &stm32image_dev.part_info[idx]; + stm32_img = (uint32_t *)¤t_part->part_offset; + + return 0; +} + +/* Return the size of a partition */ +static int stm32image_partition_size(io_entity_t *entity, size_t *length) +{ + int result; + uintptr_t backend_handle; + size_t bytes_read; + boot_api_image_header_t *header = + (boot_api_image_header_t *)first_lba_buffer; + + assert(entity != NULL); + assert(length != NULL); + + /* Attempt to access the image */ + result = io_open(backend_dev_handle, backend_image_spec, + &backend_handle); + + if (result < 0) { + ERROR("%s: io_open (%i)\n", __func__, result); + return result; + } + + /* Reset magic header value */ + header->magic = 0; + + while (header->magic == 0U) { + result = io_seek(backend_handle, IO_SEEK_SET, *stm32_img); + if (result != 0) { + ERROR("%s: io_seek (%i)\n", __func__, result); + break; + } + + result = io_read(backend_handle, (uintptr_t)header, + MAX_LBA_SIZE, (size_t *)&bytes_read); + if (result != 0) { + ERROR("%s: io_read (%i)\n", __func__, result); + break; + } + + if ((header->magic != BOOT_API_IMAGE_HEADER_MAGIC_NB) || + (header->binary_type != current_part->binary_type) || + (header->image_length >= stm32image_dev.device_size)) { + WARN("%s: partition %s wrong header\n", + __func__, current_part->name); + + /* Header not correct, check next offset for backup */ + *stm32_img += current_part->bkp_offset; + if (*stm32_img > stm32image_dev.device_size) { + /* No backup found, end of device reached */ + WARN("Out of memory\n"); + result = -ENOMEM; + break; + } + header->magic = 0; + } + } + + io_close(backend_handle); + + if (result != 0) { + return result; + } + + *length = header->image_length; + + INFO("STM32 Image size : %i\n", *length); + + return 0; +} + +static int check_header(boot_api_image_header_t *header, uintptr_t buffer) +{ + uint32_t i; + uint32_t img_checksum = 0; + + /* + * Check header/payload validity: + * - Header magic + * - Header version + * - Payload checksum + */ + if (header->magic != BOOT_API_IMAGE_HEADER_MAGIC_NB) { + ERROR("Header magic\n"); + return -EINVAL; + } + + if (header->header_version != BOOT_API_HEADER_VERSION) { + ERROR("Header version\n"); + return -EINVAL; + } + + for (i = 0; i < header->image_length; i++) { + img_checksum += *(uint8_t *)(buffer + i); + } + + if (header->payload_checksum != img_checksum) { + ERROR("Checksum: 0x%x (awaited: 0x%x)\n", img_checksum, + header->payload_checksum); + return -EINVAL; + } + + return 0; +} + +/* Read data from a partition */ +static int stm32image_partition_read(io_entity_t *entity, uintptr_t buffer, + size_t length, size_t *length_read) +{ + int result = 0, offset, local_length = 0; + uint8_t *local_buffer = (uint8_t *)buffer; + boot_api_image_header_t *header = + (boot_api_image_header_t *)first_lba_buffer; + uintptr_t backend_handle; + + assert(entity != NULL); + assert(buffer != 0U); + assert(length_read != NULL); + + *length_read = 0U; + + while (*length_read == 0U) { + if (header->magic != BOOT_API_IMAGE_HEADER_MAGIC_NB) { + /* Check for backup as image is corrupted */ + *stm32_img += current_part->bkp_offset; + if (*stm32_img >= stm32image_dev.device_size) { + /* End of device reached */ + result = -ENOMEM; + break; + } + + local_buffer = (uint8_t *)buffer; + + result = stm32image_partition_size(entity, &length); + if (result != 0) { + break; + } + } + + /* Part of image already loaded with the header */ + memcpy(local_buffer, (uint8_t *)first_lba_buffer + + sizeof(boot_api_image_header_t), + MAX_LBA_SIZE - sizeof(boot_api_image_header_t)); + local_buffer += MAX_LBA_SIZE - sizeof(boot_api_image_header_t); + offset = MAX_LBA_SIZE; + + /* New image length to be read */ + local_length = round_up(length - + ((MAX_LBA_SIZE) - + sizeof(boot_api_image_header_t)), + stm32image_dev.lba_size); + + if ((header->load_address != 0U) && + (header->load_address != buffer)) { + ERROR("Wrong load address\n"); + panic(); + } + + result = io_open(backend_dev_handle, backend_image_spec, + &backend_handle); + + if (result != 0) { + ERROR("%s: io_open (%i)\n", __func__, result); + break; + } + + result = io_seek(backend_handle, IO_SEEK_SET, + *stm32_img + offset); + + if (result != 0) { + ERROR("%s: io_seek (%i)\n", __func__, result); + *length_read = 0; + io_close(backend_handle); + break; + } + + result = io_read(backend_handle, (uintptr_t)local_buffer, + local_length, length_read); + + /* Adding part of size already read from header */ + *length_read += MAX_LBA_SIZE - sizeof(boot_api_image_header_t); + + if (result != 0) { + ERROR("%s: io_read (%i)\n", __func__, result); + *length_read = 0; + io_close(backend_handle); + break; + } + + result = check_header(header, buffer); + if (result != 0) { + ERROR("Header check failed\n"); + *length_read = 0; + header->magic = 0; + io_close(backend_handle); + break; + } + + io_close(backend_handle); + } + + return result; +} + +/* Close a partition */ +static int stm32image_partition_close(io_entity_t *entity) +{ + current_part = NULL; + + return 0; +} + +/* Register the stm32image driver with the IO abstraction */ +int register_io_dev_stm32image(const io_dev_connector_t **dev_con) +{ + int result; + + assert(dev_con != NULL); + + result = io_register_device(&stm32image_dev_info); + if (result == 0) { + *dev_con = &stm32image_dev_connector; + } + + return result; +} diff --git a/include/drivers/st/io_stm32image.h b/include/drivers/st/io_stm32image.h new file mode 100644 index 000000000..b66821960 --- /dev/null +++ b/include/drivers/st/io_stm32image.h @@ -0,0 +1,32 @@ +/* + * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef IO_STM32IMAGE_H +#define IO_STM32IMAGE_H + +#include +#include + +#define MAX_LBA_SIZE 512 +#define MAX_PART_NAME_SIZE (EFI_NAMELEN + 1) +#define STM32_PART_NUM (PLAT_PARTITION_MAX_ENTRIES - STM32_TF_A_COPIES) + +struct stm32image_part_info { + char name[MAX_PART_NAME_SIZE]; + uint32_t binary_type; + uintptr_t part_offset; + uint32_t bkp_offset; +}; + +struct stm32image_device_info { + struct stm32image_part_info part_info[STM32_PART_NUM]; + uint32_t device_size; + uint32_t lba_size; +}; + +int register_io_dev_stm32image(const io_dev_connector_t **dev_con); + +#endif /* IO_STM32IMAGE_H */