stm32mp1: add an IO to read STM32IMAGE binaries

This IO is required to read binaries with STM32 header.
This header is added with the stm32image tool.
Signed-off-by: Yann Gautier <yann.gautier@st.com>

drivers/st/io/io_stm32image.c

+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <boot_api.h>
+#include <debug.h>
+#include <errno.h>
+#include <io_driver.h>
+#include <io_stm32image.h>
+#include <io_storage.h>
+#include <platform.h>
+#include <platform_def.h>
+#include <stdint.h>
+#include <string.h>
+#include <utils.h>
+
+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 *)&current_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;
+}

include/drivers/st/io_stm32image.h

+/*
+ * 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 <io_driver.h>
+#include <partition.h>
+
+#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 */