mkimage: fit: add support to encrypt image with aes
authorPhilippe Reynes <philippe.reynes@softathome.com>
Wed, 18 Dec 2019 17:25:41 +0000 (18:25 +0100)
committerTom Rini <trini@konsulko.com>
Fri, 17 Jan 2020 15:15:49 +0000 (10:15 -0500)
This commit add the support of encrypting image with aes
in mkimage. To enable the ciphering, a node cipher with
a reference to a key and IV (Initialization Vector) must
be added to the its file. Then mkimage add the encrypted
image to the FIT and add the key and IV to the u-boot
device tree.

Signed-off-by: Philippe Reynes <philippe.reynes@softathome.com>
Kconfig
common/Makefile
common/image-cipher.c [new file with mode: 0644]
common/image-fit.c
include/image.h
include/u-boot/aes.h [new file with mode: 0644]
lib/aes/aes-encrypt.c [new file with mode: 0644]
tools/Makefile
tools/fit_image.c
tools/image-host.c

diff --git a/Kconfig b/Kconfig
index d9be0daf23a19ff60d141d656e332fc17808a5ff..caae4aecbdfb29d012381c6f7d096ac1b4ff6064 100644 (file)
--- a/Kconfig
+++ b/Kconfig
@@ -383,6 +383,14 @@ config FIT_ENABLE_RSASSA_PSS_SUPPORT
          Enable this to support the pss padding algorithm as described
          in the rfc8017 (https://tools.ietf.org/html/rfc8017).
 
+config FIT_CIPHER
+       bool "Enable ciphering data in a FIT uImages"
+       depends on DM
+       select AES
+       help
+         Enable the feature of data ciphering/unciphering in the tool mkimage
+         and in the u-boot support of the FIT image.
+
 config FIT_VERBOSE
        bool "Show verbose messages when FIT images fail"
        help
index 029cc0f2ce6b408ac79524cb744fc064c81b87e5..5f62b8d0b136afdcd8ae58485d3dedb9a5c94641 100644 (file)
@@ -113,6 +113,7 @@ obj-$(CONFIG_$(SPL_TPL_)OF_LIBFDT) += image-fdt.o
 obj-$(CONFIG_$(SPL_TPL_)FIT) += image-fit.o
 obj-$(CONFIG_$(SPL_)MULTI_DTB_FIT) += boot_fit.o common_fit.o
 obj-$(CONFIG_$(SPL_TPL_)FIT_SIGNATURE) += image-sig.o
+obj-$(CONFIG_$(SPL_TPL_)FIT_CIPHER) += image-cipher.o
 obj-$(CONFIG_IO_TRACE) += iotrace.o
 obj-y += memsize.o
 obj-y += stdio.o
diff --git a/common/image-cipher.c b/common/image-cipher.c
new file mode 100644 (file)
index 0000000..ab8b45b
--- /dev/null
@@ -0,0 +1,63 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 2019, Softathome
+ */
+
+#ifdef USE_HOSTCC
+#include "mkimage.h"
+#include <time.h>
+#else
+#include <common.h>
+#include <malloc.h>
+DECLARE_GLOBAL_DATA_PTR;
+#endif /* !USE_HOSTCC*/
+#include <image.h>
+#include <uboot_aes.h>
+#include <u-boot/aes.h>
+
+struct cipher_algo cipher_algos[] = {
+       {
+               .name = "aes128",
+               .key_len = AES128_KEY_LENGTH,
+               .iv_len  = AES_BLOCK_LENGTH,
+#if IMAGE_ENABLE_ENCRYPT
+               .calculate_type = EVP_aes_128_cbc,
+#endif
+               .encrypt = image_aes_encrypt,
+               .add_cipher_data = image_aes_add_cipher_data
+       },
+       {
+               .name = "aes192",
+               .key_len = AES192_KEY_LENGTH,
+               .iv_len  = AES_BLOCK_LENGTH,
+#if IMAGE_ENABLE_ENCRYPT
+               .calculate_type = EVP_aes_192_cbc,
+#endif
+               .encrypt = image_aes_encrypt,
+               .add_cipher_data = image_aes_add_cipher_data
+       },
+       {
+               .name = "aes256",
+               .key_len = AES256_KEY_LENGTH,
+               .iv_len  = AES_BLOCK_LENGTH,
+#if IMAGE_ENABLE_ENCRYPT
+               .calculate_type = EVP_aes_256_cbc,
+#endif
+               .encrypt = image_aes_encrypt,
+               .add_cipher_data = image_aes_add_cipher_data
+       }
+};
+
+struct cipher_algo *image_get_cipher_algo(const char *full_name)
+{
+       int i;
+       const char *name;
+
+       for (i = 0; i < ARRAY_SIZE(cipher_algos); i++) {
+               name = cipher_algos[i].name;
+               if (!strncmp(name, full_name, strlen(name)))
+                       return &cipher_algos[i];
+       }
+
+       return NULL;
+}
index 231612ff5f01a85c8b2efdf4c9d47dace10eccc1..6b50ceba1ab1bf0ea4913952e550e6e23d03ccda 100644 (file)
@@ -1080,6 +1080,33 @@ static int fit_image_hash_get_ignore(const void *fit, int noffset, int *ignore)
        return 0;
 }
 
+/**
+ * fit_image_cipher_get_algo - get cipher algorithm name
+ * @fit: pointer to the FIT format image header
+ * @noffset: cipher node offset
+ * @algo: double pointer to char, will hold pointer to the algorithm name
+ *
+ * fit_image_cipher_get_algo() finds cipher algorithm property in a given
+ * cipher node. If the property is found its data start address is returned
+ * to the caller.
+ *
+ * returns:
+ *     0, on success
+ *     -1, on failure
+ */
+int fit_image_cipher_get_algo(const void *fit, int noffset, char **algo)
+{
+       int len;
+
+       *algo = (char *)fdt_getprop(fit, noffset, FIT_ALGO_PROP, &len);
+       if (!*algo) {
+               fit_get_debug(fit, noffset, FIT_ALGO_PROP, len);
+               return -1;
+       }
+
+       return 0;
+}
+
 ulong fit_get_end(const void *fit)
 {
        return map_to_sysmem((void *)(fit + fdt_totalsize(fit)));
index 4a280b78e748ebe49f4c74a4ab60b160ec8b37e2..bb8abe566fe7f94850c5fe4d3d0e4b72636e1713 100644 (file)
@@ -930,6 +930,10 @@ int booti_setup(ulong image, ulong *relocated_addr, ulong *size,
 #define FIT_IGNORE_PROP                "uboot-ignore"
 #define FIT_SIG_NODENAME       "signature"
 
+/* cipher node */
+#define FIT_CIPHER_NODENAME    "cipher"
+#define FIT_ALGO_PROP          "algo"
+
 /* image node */
 #define FIT_DATA_PROP          "data"
 #define FIT_DATA_POSITION_PROP "data-position"
@@ -1028,6 +1032,10 @@ int fit_image_hash_get_value(const void *fit, int noffset, uint8_t **value,
 
 int fit_set_timestamp(void *fit, int noffset, time_t timestamp);
 
+int fit_cipher_data(const char *keydir, void *keydest, void *fit,
+                   const char *comment, int require_keys,
+                   const char *engine_id, const char *cmdname);
+
 /**
  * fit_add_verification_data() - add verification data to FIT image nodes
  *
@@ -1138,6 +1146,7 @@ struct image_sign_info {
        const char *require_keys;       /* Value for 'required' property */
        const char *engine_id;          /* Engine to use for signing */
 };
+
 #endif /* Allow struct image_region to always be defined for rsa.h */
 
 /* A part of an image, used for hashing */
@@ -1310,6 +1319,60 @@ static inline int fit_image_check_target_arch(const void *fdt, int node)
 #endif
 }
 
+/*
+ * At present we only support ciphering on the host, and unciphering on the
+ * device
+ */
+#if defined(USE_HOSTCC)
+# if defined(CONFIG_FIT_CIPHER)
+#  define IMAGE_ENABLE_ENCRYPT 1
+#  define IMAGE_ENABLE_DECRYPT 1
+#  include <openssl/evp.h>
+# else
+#  define IMAGE_ENABLE_ENCRYPT 0
+#  define IMAGE_ENABLE_DECRYPT 0
+# endif
+#else
+# define IMAGE_ENABLE_ENCRYPT  0
+# define IMAGE_ENABLE_DECRYPT  CONFIG_IS_ENABLED(FIT_CIPHER)
+#endif
+
+/* Information passed to the ciphering routines */
+struct image_cipher_info {
+       const char *keydir;             /* Directory containing keys */
+       const char *keyname;            /* Name of key to use */
+       const char *ivname;             /* Name of IV to use */
+       const void *fit;                /* Pointer to FIT blob */
+       int node_noffset;               /* Offset of the cipher node */
+       const char *name;               /* Algorithm name */
+       struct cipher_algo *cipher;     /* Cipher algorithm information */
+       const void *fdt_blob;           /* FDT containing key and IV */
+       const void *key;                /* Value of the key */
+       const void *iv;                 /* Value of the IV */
+       size_t size_unciphered;         /* Size of the unciphered data */
+};
+
+struct cipher_algo {
+       const char *name;               /* Name of algorithm */
+       int key_len;                    /* Length of the key */
+       int iv_len;                     /* Length of the IV */
+
+#if IMAGE_ENABLE_ENCRYPT
+       const EVP_CIPHER * (*calculate_type)(void);
+#endif
+
+       int (*encrypt)(struct image_cipher_info *info,
+                      const unsigned char *data, int data_len,
+                      unsigned char **cipher, int *cipher_len);
+
+       int (*add_cipher_data)(struct image_cipher_info *info,
+                              void *keydest);
+};
+
+int fit_image_cipher_get_algo(const void *fit, int noffset, char **algo);
+
+struct cipher_algo *image_get_cipher_algo(const char *full_name);
+
 #ifdef CONFIG_FIT_VERBOSE
 #define fit_unsupported(msg)   printf("! %s:%d " \
                                "FIT images not supported for '%s'\n", \
diff --git a/include/u-boot/aes.h b/include/u-boot/aes.h
new file mode 100644 (file)
index 0000000..4fb2cb7
--- /dev/null
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 2019, Softathome
+ */
+
+#ifndef _AES_H
+#define _AES_H
+
+#include <errno.h>
+#include <image.h>
+
+#if IMAGE_ENABLE_ENCRYPT
+int image_aes_encrypt(struct image_cipher_info *info,
+                     const unsigned char *data, int size,
+                     unsigned char **cipher, int *cipher_len);
+int image_aes_add_cipher_data(struct image_cipher_info *info, void *keydest);
+#else
+int image_aes_encrypt(struct image_cipher_info *info,
+                     const unsigned char *data, int size,
+                     unsigned char **cipher, int *cipher_len)
+{
+       return -ENXIO;
+}
+
+int image_aes_add_cipher_data(struct image_cipher_info *info, void *keydest)
+{
+       return -ENXIO;
+}
+#endif /* IMAGE_ENABLE_ENCRYPT */
+
+#endif
diff --git a/lib/aes/aes-encrypt.c b/lib/aes/aes-encrypt.c
new file mode 100644 (file)
index 0000000..de00a83
--- /dev/null
@@ -0,0 +1,136 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 2019,Softathome
+ */
+#include "mkimage.h"
+#include <stdio.h>
+#include <string.h>
+#include <image.h>
+#include <time.h>
+#include <openssl/bn.h>
+#include <openssl/rsa.h>
+#include <openssl/pem.h>
+#include <openssl/err.h>
+#include <openssl/ssl.h>
+#include <openssl/evp.h>
+#include <openssl/engine.h>
+#include <uboot_aes.h>
+
+#if OPENSSL_VERSION_NUMBER >= 0x10000000L
+#define HAVE_ERR_REMOVE_THREAD_STATE
+#endif
+
+int image_aes_encrypt(struct image_cipher_info *info,
+                     unsigned char *data, int size,
+                     unsigned char **cipher, int *cipher_len)
+{
+       EVP_CIPHER_CTX *ctx;
+       unsigned char *buf = NULL;
+       int buf_len, len, ret = 0;
+
+       /* create and initialise the context */
+       ctx = EVP_CIPHER_CTX_new();
+       if (!ctx) {
+               printf("Can't create context\n");
+               return -1;
+       }
+
+       /* allocate a buffer for the result */
+       buf = malloc(size + AES_BLOCK_LENGTH);
+       if (!buf) {
+               printf("Can't allocate memory to encrypt\n");
+               ret = -1;
+               goto out;
+       }
+
+       if (EVP_EncryptInit_ex(ctx, info->cipher->calculate_type(),
+                              NULL, info->key, info->iv) != 1) {
+               printf("Can't init encryption\n");
+               ret = -1;
+               goto out;
+       }
+
+       if (EVP_EncryptUpdate(ctx, buf, &len, data, size) != 1) {
+               printf("Can't encrypt data\n");
+               ret = -1;
+               goto out;
+       }
+
+       buf_len = len;
+
+       if (EVP_EncryptFinal_ex(ctx, buf + len, &len) != 1) {
+               printf("Can't finalise the encryption\n");
+               ret = -1;
+               goto out;
+       }
+
+       buf_len += len;
+
+       *cipher = buf;
+       *cipher_len = buf_len;
+
+ out:
+       EVP_CIPHER_CTX_free(ctx);
+       return ret;
+}
+
+int image_aes_add_cipher_data(struct image_cipher_info *info, void *keydest)
+{
+       int parent, node;
+       char name[128];
+       int ret = 0;
+
+       /* Either create or overwrite the named cipher node */
+       parent = fdt_subnode_offset(keydest, 0, FIT_CIPHER_NODENAME);
+       if (parent == -FDT_ERR_NOTFOUND) {
+               parent = fdt_add_subnode(keydest, 0, FIT_CIPHER_NODENAME);
+               if (parent < 0) {
+                       ret = parent;
+                       if (ret != -FDT_ERR_NOSPACE) {
+                               fprintf(stderr,
+                                       "Couldn't create cipher node: %s\n",
+                                       fdt_strerror(parent));
+                       }
+               }
+       }
+       if (ret)
+               goto done;
+
+       /* Either create or overwrite the named key node */
+       snprintf(name, sizeof(name), "key-%s-%s-%s",
+                info->name, info->keyname, info->ivname);
+       node = fdt_subnode_offset(keydest, parent, name);
+       if (node == -FDT_ERR_NOTFOUND) {
+               node = fdt_add_subnode(keydest, parent, name);
+               if (node < 0) {
+                       ret = node;
+                       if (ret != -FDT_ERR_NOSPACE) {
+                               fprintf(stderr,
+                                       "Could not create key subnode: %s\n",
+                                       fdt_strerror(node));
+                       }
+               }
+       } else if (node < 0) {
+               fprintf(stderr, "Cannot select keys parent: %s\n",
+                       fdt_strerror(node));
+               ret = node;
+       }
+
+       if (!ret)
+               ret = fdt_setprop(keydest, node, "iv",
+                                 info->iv, info->cipher->iv_len);
+
+       if (!ret)
+               ret = fdt_setprop(keydest, node, "key",
+                                 info->key, info->cipher->key_len);
+
+       if (!ret)
+               ret = fdt_setprop_u32(keydest, node, "key-len",
+                                     info->cipher->key_len);
+
+done:
+       if (ret)
+               ret = ret == -FDT_ERR_NOSPACE ? -ENOSPC : -EIO;
+
+       return ret;
+}
index 345bc84e48db32173c69457d360acf8bf9638c58..051127a6150118a5f18d81ae2bcce35c34cb19bb 100644 (file)
@@ -59,6 +59,7 @@ hostprogs-$(CONFIG_CMD_BOOTEFI_SELFTEST) += file2include
 
 FIT_OBJS-$(CONFIG_FIT) := fit_common.o fit_image.o image-host.o common/image-fit.o
 FIT_SIG_OBJS-$(CONFIG_FIT_SIGNATURE) := common/image-sig.o
+FIT_CIPHER_OBJS-$(CONFIG_FIT_CIPHER) := common/image-cipher.o
 
 # The following files are synced with upstream DTC.
 # Use synced versions from scripts/dtc/libfdt/.
@@ -75,6 +76,9 @@ RSA_OBJS-$(CONFIG_FIT_SIGNATURE) := $(addprefix lib/rsa/, \
                                        rsa-sign.o rsa-verify.o rsa-checksum.o \
                                        rsa-mod-exp.o)
 
+AES_OBJS-$(CONFIG_FIT_CIPHER) := $(addprefix lib/aes/, \
+                                       aes-encrypt.o)
+
 ROCKCHIP_OBS = lib/rc4.o rkcommon.o rkimage.o rksd.o rkspi.o
 
 # common objs for dumpimage and mkimage
@@ -82,6 +86,7 @@ dumpimage-mkimage-objs := aisimage.o \
                        atmelimage.o \
                        $(FIT_OBJS-y) \
                        $(FIT_SIG_OBJS-y) \
+                       $(FIT_CIPHER_OBJS-y) \
                        common/bootm.o \
                        lib/crc32.o \
                        default_image.o \
@@ -116,7 +121,8 @@ dumpimage-mkimage-objs := aisimage.o \
                        gpimage.o \
                        gpimage-common.o \
                        mtk_image.o \
-                       $(RSA_OBJS-y)
+                       $(RSA_OBJS-y) \
+                       $(AES_OBJS-y)
 
 dumpimage-objs := $(dumpimage-mkimage-objs) dumpimage.o
 mkimage-objs   := $(dumpimage-mkimage-objs) mkimage.o
@@ -137,6 +143,12 @@ HOST_EXTRACFLAGS   += -DCONFIG_FIT_SIGNATURE
 HOST_EXTRACFLAGS       += -DCONFIG_FIT_SIGNATURE_MAX_SIZE=$(CONFIG_FIT_SIGNATURE_MAX_SIZE)
 endif
 
+ifdef CONFIG_FIT_CIPHER
+# This affects include/image.h, but including the board config file
+# is tricky, so manually define this options here.
+HOST_EXTRACFLAGS       += -DCONFIG_FIT_CIPHER
+endif
+
 ifdef CONFIG_SYS_U_BOOT_OFFS
 HOSTCFLAGS_kwbimage.o += -DCONFIG_SYS_U_BOOT_OFFS=$(CONFIG_SYS_U_BOOT_OFFS)
 endif
index 6aa4b1c7330a8cd040b34bd5301a529a0b927ce1..dd61a816c93f65c141326f28ad9eea0028ed7488 100644 (file)
@@ -58,6 +58,14 @@ static int fit_add_file_data(struct image_tool_params *params, size_t size_inc,
                ret = fit_set_timestamp(ptr, 0, time);
        }
 
+       if (!ret) {
+               ret = fit_cipher_data(params->keydir, dest_blob, ptr,
+                                     params->comment,
+                                     params->require_keys,
+                                     params->engine_id,
+                                     params->cmdname);
+       }
+
        if (!ret) {
                ret = fit_add_verification_data(params->keydir, dest_blob, ptr,
                                                params->comment,
@@ -74,7 +82,6 @@ static int fit_add_file_data(struct image_tool_params *params, size_t size_inc,
 err_keydest:
        munmap(ptr, sbuf.st_size);
        close(tfd);
-
        return ret;
 }
 
@@ -621,6 +628,62 @@ err_no_fd:
        return ret;
 }
 
+static int copyfile(const char *src, const char *dst)
+{
+       int fd_src = -1, fd_dst = -1;
+       void *buf = NULL;
+       ssize_t size;
+       size_t count;
+       int ret = -1;
+
+       fd_src = open(src, O_RDONLY);
+       if (fd_src < 0) {
+               printf("Can't open file %s (%s)\n", src, strerror(errno));
+               goto out;
+       }
+
+       fd_dst = open(dst, O_WRONLY | O_CREAT, 0700);
+       if (fd_dst < 0) {
+               printf("Can't open file %s (%s)\n", dst, strerror(errno));
+               goto out;
+       }
+
+       buf = malloc(512);
+       if (!buf) {
+               printf("Can't allocate buffer to copy file\n");
+               goto out;
+       }
+
+       while (1) {
+               size = read(fd_src, buf, 512);
+               if (size < 0) {
+                       printf("Can't read file %s\n", src);
+                       goto out;
+               }
+               if (!size)
+                       break;
+
+               count = size;
+               size = write(fd_dst, buf, count);
+               if (size < 0) {
+                       printf("Can't write file %s\n", dst);
+                       goto out;
+               }
+       }
+
+       ret = 0;
+
+ out:
+       if (fd_src >= 0)
+               close(fd_src);
+       if (fd_dst >= 0)
+               close(fd_dst);
+       if (buf)
+               free(buf);
+
+       return ret;
+}
+
 /**
  * fit_handle_file - main FIT file processing function
  *
@@ -636,6 +699,7 @@ err_no_fd:
 static int fit_handle_file(struct image_tool_params *params)
 {
        char tmpfile[MKIMAGE_MAX_TMPFILE_LEN];
+       char bakfile[MKIMAGE_MAX_TMPFILE_LEN + 4] = {0};
        char cmd[MKIMAGE_MAX_DTC_CMDLINE_LEN];
        size_t size_inc;
        int ret;
@@ -670,6 +734,7 @@ static int fit_handle_file(struct image_tool_params *params)
                snprintf(cmd, sizeof(cmd), "cp \"%s\" \"%s\"",
                         params->imagefile, tmpfile);
        }
+
        if (*cmd && system(cmd) == -1) {
                fprintf (stderr, "%s: system(%s) failed: %s\n",
                                params->cmdname, cmd, strerror(errno));
@@ -681,6 +746,14 @@ static int fit_handle_file(struct image_tool_params *params)
        if (ret)
                goto err_system;
 
+       /*
+        * Copy the tmpfile to bakfile, then in the following loop
+        * we copy bakfile to tmpfile. So we always start from the
+        * beginning.
+        */
+       sprintf(bakfile, "%s%s", tmpfile, ".bak");
+       rename(tmpfile, bakfile);
+
        /*
         * Set hashes for images in the blob. Unfortunately we may need more
         * space in either FDT, so keep trying until we succeed.
@@ -692,6 +765,11 @@ static int fit_handle_file(struct image_tool_params *params)
         * steps of this loop is enough to sign with several keys.
         */
        for (size_inc = 0; size_inc < 64 * 1024; size_inc += 1024) {
+               if (copyfile(bakfile, tmpfile) < 0) {
+                       printf("Can't copy %s to %s\n", bakfile, tmpfile);
+                       ret = -EIO;
+                       break;
+               }
                ret = fit_add_file_data(params, size_inc, tmpfile);
                if (!ret || ret != -ENOSPC)
                        break;
@@ -715,13 +793,16 @@ static int fit_handle_file(struct image_tool_params *params)
                                params->cmdname, tmpfile, params->imagefile,
                                strerror (errno));
                unlink (tmpfile);
+               unlink(bakfile);
                unlink (params->imagefile);
                return EXIT_FAILURE;
        }
+       unlink(bakfile);
        return EXIT_SUCCESS;
 
 err_system:
        unlink(tmpfile);
+       unlink(bakfile);
        return -1;
 }
 
index 88b329502ca3a56e6a1349f3312cfab2e4117143..9483561bfa518487366a131d81d2c94d509081f1 100644 (file)
@@ -12,6 +12,7 @@
 #include <bootm.h>
 #include <image.h>
 #include <version.h>
+#include <uboot_aes.h>
 
 /**
  * fit_set_hash_value - set hash value in requested has node
@@ -268,6 +269,262 @@ static int fit_image_process_sig(const char *keydir, void *keydest,
        return 0;
 }
 
+static int fit_image_read_data(char *filename, unsigned char *data,
+                              int expected_size)
+{
+       struct stat sbuf;
+       int fd, ret = -1;
+       ssize_t n;
+
+       /* Open file */
+       fd = open(filename, O_RDONLY | O_BINARY);
+       if (fd < 0) {
+               printf("Can't open file %s (err=%d => %s)\n",
+                      filename, errno, strerror(errno));
+               return -1;
+       }
+
+       /* Compute file size */
+       if (fstat(fd, &sbuf) < 0) {
+               printf("Can't fstat file %s (err=%d => %s)\n",
+                      filename, errno, strerror(errno));
+               goto err;
+       }
+
+       /* Check file size */
+       if (sbuf.st_size != expected_size) {
+               printf("File %s don't have the expected size (size=%ld, expected=%d)\n",
+                      filename, sbuf.st_size, expected_size);
+               goto err;
+       }
+
+       /* Read data */
+       n = read(fd, data, sbuf.st_size);
+       if (n < 0) {
+               printf("Can't read file %s (err=%d => %s)\n",
+                      filename, errno, strerror(errno));
+               goto err;
+       }
+
+       /* Check that we have read all the file */
+       if (n != sbuf.st_size) {
+               printf("Can't read all file %s (read %ld bytes, expexted %ld)\n",
+                      filename, n, sbuf.st_size);
+               goto err;
+       }
+
+       ret = 0;
+
+err:
+       close(fd);
+       return ret;
+}
+
+static int fit_image_setup_cipher(struct image_cipher_info *info,
+                                 const char *keydir, void *fit,
+                                 const char *image_name, int image_noffset,
+                                 const char *node_name, int noffset)
+{
+       char *algo_name;
+       char filename[128];
+       int ret = -1;
+
+       if (fit_image_cipher_get_algo(fit, noffset, &algo_name)) {
+               printf("Can't get algo name for cipher '%s' in image '%s'\n",
+                      node_name, image_name);
+               goto out;
+       }
+
+       info->keydir = keydir;
+
+       /* Read the key name */
+       info->keyname = fdt_getprop(fit, noffset, "key-name-hint", NULL);
+       if (!info->keyname) {
+               printf("Can't get key name for cipher '%s' in image '%s'\n",
+                      node_name, image_name);
+               goto out;
+       }
+
+       /* Read the IV name */
+       info->ivname = fdt_getprop(fit, noffset, "iv-name-hint", NULL);
+       if (!info->ivname) {
+               printf("Can't get iv name for cipher '%s' in image '%s'\n",
+                      node_name, image_name);
+               goto out;
+       }
+
+       info->fit = fit;
+       info->node_noffset = noffset;
+       info->name = algo_name;
+
+       info->cipher = image_get_cipher_algo(algo_name);
+       if (!info->cipher) {
+               printf("Can't get algo for cipher '%s'\n", image_name);
+               goto out;
+       }
+
+       /* Read the key in the file */
+       snprintf(filename, sizeof(filename), "%s/%s%s",
+                info->keydir, info->keyname, ".bin");
+       info->key = malloc(info->cipher->key_len);
+       if (!info->key) {
+               printf("Can't allocate memory for key\n");
+               ret = -1;
+               goto out;
+       }
+       ret = fit_image_read_data(filename, (unsigned char *)info->key,
+                                 info->cipher->key_len);
+       if (ret < 0)
+               goto out;
+
+       /* Read the IV in the file */
+       snprintf(filename, sizeof(filename), "%s/%s%s",
+                info->keydir, info->ivname, ".bin");
+       info->iv = malloc(info->cipher->iv_len);
+       if (!info->iv) {
+               printf("Can't allocate memory for iv\n");
+               ret = -1;
+               goto out;
+       }
+       ret = fit_image_read_data(filename, (unsigned char *)info->iv,
+                                 info->cipher->iv_len);
+
+ out:
+       return ret;
+}
+
+int fit_image_write_cipher(void *fit, int image_noffset, int noffset,
+                          const void *data, size_t size,
+                          unsigned char *data_ciphered, int data_ciphered_len)
+{
+       int ret = -1;
+
+       /* Remove unciphered data */
+       ret = fdt_delprop(fit, image_noffset, FIT_DATA_PROP);
+       if (ret) {
+               printf("Can't remove data (err = %d)\n", ret);
+               goto out;
+       }
+
+       /* Add ciphered data */
+       ret = fdt_setprop(fit, image_noffset, FIT_DATA_PROP,
+                         data_ciphered, data_ciphered_len);
+       if (ret) {
+               printf("Can't add ciphered data (err = %d)\n", ret);
+               goto out;
+       }
+
+       /* add non ciphered data size */
+       ret = fdt_setprop_u32(fit, image_noffset, "data-size-unciphered", size);
+       if (ret) {
+               printf("Can't add unciphered data size (err = %d)\n", ret);
+               goto out;
+       }
+
+ out:
+       return ret;
+}
+
+static int
+fit_image_process_cipher(const char *keydir, void *keydest, void *fit,
+                        const char *image_name, int image_noffset,
+                        const char *node_name, int node_noffset,
+                        const void *data, size_t size,
+                        const char *cmdname)
+{
+       struct image_cipher_info info;
+       unsigned char *data_ciphered = NULL;
+       int data_ciphered_len;
+       int ret;
+
+       memset(&info, 0, sizeof(info));
+
+       ret = fit_image_setup_cipher(&info, keydir, fit, image_name,
+                                    image_noffset, node_name, node_noffset);
+       if (ret)
+               goto out;
+
+       ret = info.cipher->encrypt(&info, data, size,
+                                   &data_ciphered, &data_ciphered_len);
+       if (ret)
+               goto out;
+
+       /*
+        * Write the public key into the supplied FDT file; this might fail
+        * several times, since we try signing with successively increasing
+        * size values
+        */
+       if (keydest) {
+               ret = info.cipher->add_cipher_data(&info, keydest);
+               if (ret) {
+                       printf("Failed to add verification data for cipher '%s' in image '%s'\n",
+                              info.keyname, image_name);
+                       goto out;
+               }
+       }
+
+       ret = fit_image_write_cipher(fit, image_noffset, node_noffset,
+                                    data, size,
+                                    data_ciphered, data_ciphered_len);
+
+ out:
+       free(data_ciphered);
+       free((void *)info.key);
+       free((void *)info.iv);
+       return ret;
+}
+
+int fit_image_cipher_data(const char *keydir, void *keydest,
+                         void *fit, int image_noffset, const char *comment,
+                         int require_keys, const char *engine_id,
+                         const char *cmdname)
+{
+       const char *image_name;
+       const void *data;
+       size_t size;
+       int node_noffset;
+
+       /* Get image name */
+       image_name = fit_get_name(fit, image_noffset, NULL);
+       if (!image_name) {
+               printf("Can't get image name\n");
+               return -1;
+       }
+
+       /* Get image data and data length */
+       if (fit_image_get_data(fit, image_noffset, &data, &size)) {
+               printf("Can't get image data/size\n");
+               return -1;
+       }
+
+       /* Process all hash subnodes of the component image node */
+       for (node_noffset = fdt_first_subnode(fit, image_noffset);
+            node_noffset >= 0;
+            node_noffset = fdt_next_subnode(fit, node_noffset)) {
+               const char *node_name;
+               int ret = 0;
+
+               node_name = fit_get_name(fit, node_noffset, NULL);
+               if (!node_name) {
+                       printf("Can't get node name\n");
+                       return -1;
+               }
+
+               if (IMAGE_ENABLE_ENCRYPT && keydir &&
+                   !strncmp(node_name, FIT_CIPHER_NODENAME,
+                            strlen(FIT_CIPHER_NODENAME)))
+                       ret = fit_image_process_cipher(keydir, keydest,
+                                                      fit, image_name,
+                                                      image_noffset,
+                                                      node_name, node_noffset,
+                                                      data, size, cmdname);
+               if (ret)
+                       return ret;
+       }
+
+       return 0;
+}
+
 /**
  * fit_image_add_verification_data() - calculate/set verig. data for image node
  *
@@ -675,6 +932,41 @@ static int fit_config_add_verification_data(const char *keydir, void *keydest,
        return 0;
 }
 
+int fit_cipher_data(const char *keydir, void *keydest, void *fit,
+                   const char *comment, int require_keys,
+                   const char *engine_id, const char *cmdname)
+{
+       int images_noffset;
+       int noffset;
+       int ret;
+
+       /* Find images parent node offset */
+       images_noffset = fdt_path_offset(fit, FIT_IMAGES_PATH);
+       if (images_noffset < 0) {
+               printf("Can't find images parent node '%s' (%s)\n",
+                      FIT_IMAGES_PATH, fdt_strerror(images_noffset));
+               return images_noffset;
+       }
+
+       /* Process its subnodes, print out component images details */
+       for (noffset = fdt_first_subnode(fit, images_noffset);
+            noffset >= 0;
+            noffset = fdt_next_subnode(fit, noffset)) {
+               /*
+                * Direct child node of the images parent node,
+                * i.e. component image node.
+                */
+               ret = fit_image_cipher_data(keydir, keydest,
+                                           fit, noffset, comment,
+                                           require_keys, engine_id,
+                                           cmdname);
+               if (ret)
+                       return ret;
+       }
+
+       return 0;
+}
+
 int fit_add_verification_data(const char *keydir, void *keydest, void *fit,
                              const char *comment, int require_keys,
                              const char *engine_id, const char *cmdname)