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>

diff --git a/drivers/st/io/io_stm32image.c b/drivers/st/io/io_stm32image.c
new file mode 100644 (file)
index 0000000..e6798e0
--- /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 <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;
+}

diff --git a/include/drivers/st/io_stm32image.h b/include/drivers/st/io_stm32image.h
new file mode 100644 (file)
index 0000000..b668219
--- /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 <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 */