12 files changed, 834 insertions, 0 deletions

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 3086e5c..64d58a6 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -1475,6 +1475,7 @@ source "board/vscom/baltos/Kconfig"
 source "board/woodburn/Kconfig"
 source "board/work-microwave/work_92105/Kconfig"
 source "board/xilinx/Kconfig"
+source "board/xilinx/zynq/Kconfig"
 source "board/xilinx/zynqmp/Kconfig"
 source "board/zipitz2/Kconfig"
 
diff --git a/arch/arm/mach-zynq/include/mach/hardware.h b/arch/arm/mach-zynq/include/mach/hardware.h
index f69cf00..3ff3c10 100644
--- a/arch/arm/mach-zynq/include/mach/hardware.h
+++ b/arch/arm/mach-zynq/include/mach/hardware.h
@@ -20,6 +20,7 @@
 #define ZYNQ_EFUSE_BASEADDR		0xF800D000
 #define ZYNQ_USB_BASEADDR0		0xE0002000
 #define ZYNQ_USB_BASEADDR1		0xE0003000
+#define ZYNQ_OCM_BASEADDR		0xFFFC0000
 
 /* Bootmode setting values */
 #define ZYNQ_BM_MASK		0x7
diff --git a/board/xilinx/zynq/Kconfig b/board/xilinx/zynq/Kconfig
new file mode 100644
index 0000000..d6f4063
--- /dev/null
+++ b/board/xilinx/zynq/Kconfig
@@ -0,0 +1,33 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Copyright (c) 2018, Xilinx, Inc.
+
+if ARCH_ZYNQ
+
+config CMD_ZYNQ
+	bool "Enable Zynq specific commands"
+	default y
+	help
+	  Enables Zynq specific commands.
+
+config CMD_ZYNQ_AES
+	bool "Enable zynq aes command for decryption of encrypted images"
+	depends on CMD_ZYNQ
+	depends on FPGA_ZYNQPL
+	help
+	  Decrypts the encrypted image present in source address
+	  and places the decrypted image at destination address.
+
+config CMD_ZYNQ_RSA
+	bool "Enable zynq rsa command for loading secure images"
+	default y
+	depends on CMD_ZYNQ
+	depends on CMD_ZYNQ_AES
+	help
+	  Enabling this will support zynq secure image verification.
+	  The secure image is a xilinx specific BOOT.BIN with
+	  either authentication or encryption or both encryption
+	  and authentication feature enabled while generating
+	  BOOT.BIN using Xilinx bootgen tool.
+
+endif
diff --git a/board/xilinx/zynq/Makefile b/board/xilinx/zynq/Makefile
index 03ad5f0..e7645be 100644
--- a/board/xilinx/zynq/Makefile
+++ b/board/xilinx/zynq/Makefile
@@ -26,6 +26,11 @@ $(warning Put custom ps7_init_gpl.c/h to board/xilinx/zynq/custom_hw_platform/))
 endif
 endif
 
+ifndef CONFIG_SPL_BUILD
+obj-$(CONFIG_CMD_ZYNQ) += cmds.o
+obj-$(CONFIG_CMD_ZYNQ_RSA) += bootimg.o
+endif
+
 obj-$(CONFIG_SPL_BUILD) += $(init-objs)
 
 # Suppress "warning: function declaration isn't a prototype"
diff --git a/board/xilinx/zynq/bootimg.c b/board/xilinx/zynq/bootimg.c
new file mode 100644
index 0000000..56d69cd
--- /dev/null
+++ b/board/xilinx/zynq/bootimg.c
@@ -0,0 +1,143 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 Xilinx, Inc.
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/sys_proto.h>
+#include <u-boot/md5.h>
+#include <zynq_bootimg.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define ZYNQ_IMAGE_PHDR_OFFSET		0x09C
+#define ZYNQ_IMAGE_FSBL_LEN_OFFSET	0x040
+#define ZYNQ_PART_HDR_CHKSUM_WORD_COUNT	0x0F
+#define ZYNQ_PART_HDR_WORD_COUNT	0x10
+#define ZYNQ_MAXIMUM_IMAGE_WORD_LEN	0x40000000
+#define MD5_CHECKSUM_SIZE	16
+
+struct headerarray {
+	u32 fields[16];
+};
+
+/*
+ * Check whether the given partition is last partition or not
+ */
+static int zynq_islastpartition(struct headerarray *head)
+{
+	int index;
+
+	debug("%s\n", __func__);
+	if (head->fields[ZYNQ_PART_HDR_CHKSUM_WORD_COUNT] != 0xFFFFFFFF)
+		return -1;
+
+	for (index = 0; index < ZYNQ_PART_HDR_WORD_COUNT - 1; index++) {
+		if (head->fields[index] != 0x0)
+			return -1;
+	}
+
+	return 0;
+}
+
+/*
+ * Get the partition count from the partition header
+ */
+int zynq_get_part_count(struct partition_hdr *part_hdr_info)
+{
+	u32 count;
+	struct headerarray *hap;
+
+	debug("%s\n", __func__);
+
+	for (count = 0; count < ZYNQ_MAX_PARTITION_NUMBER; count++) {
+		hap = (struct headerarray *)&part_hdr_info[count];
+		if (zynq_islastpartition(hap) != -1)
+			break;
+	}
+
+	return count;
+}
+
+/*
+ * Get the partition info of all the partitions available.
+ */
+int zynq_get_partition_info(u32 image_base_addr, u32 *fsbl_len,
+			    struct partition_hdr *part_hdr)
+{
+	u32 parthdroffset;
+
+	*fsbl_len = *((u32 *)(image_base_addr + ZYNQ_IMAGE_FSBL_LEN_OFFSET));
+
+	parthdroffset = *((u32 *)(image_base_addr + ZYNQ_IMAGE_PHDR_OFFSET));
+
+	parthdroffset += image_base_addr;
+
+	memcpy(part_hdr, (u32 *)parthdroffset,
+	       (sizeof(struct partition_hdr) * ZYNQ_MAX_PARTITION_NUMBER));
+
+	return 0;
+}
+
+/*
+ * Check whether the partition header is valid or not
+ */
+int zynq_validate_hdr(struct partition_hdr *header)
+{
+	struct headerarray *hap;
+	u32 index;
+	u32 checksum;
+
+	debug("%s\n", __func__);
+
+	hap = (struct headerarray *)header;
+
+	for (index = 0; index < ZYNQ_PART_HDR_WORD_COUNT; index++) {
+		if (hap->fields[index])
+			break;
+	}
+	if (index == ZYNQ_PART_HDR_WORD_COUNT)
+		return -1;
+
+	checksum = 0;
+	for (index = 0; index < ZYNQ_PART_HDR_CHKSUM_WORD_COUNT; index++)
+		checksum += hap->fields[index];
+
+	checksum ^= 0xFFFFFFFF;
+
+	if (hap->fields[ZYNQ_PART_HDR_CHKSUM_WORD_COUNT] != checksum) {
+		printf("Error: Checksum 0x%8.8x != 0x%8.8x\n",
+		       checksum, hap->fields[ZYNQ_PART_HDR_CHKSUM_WORD_COUNT]);
+		return -1;
+	}
+
+	if (header->imagewordlen > ZYNQ_MAXIMUM_IMAGE_WORD_LEN) {
+		printf("INVALID_PARTITION_LENGTH\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+/*
+ * Validate the partition by calculationg the md5 checksum for the
+ * partition and compare with checksum present in checksum offset of
+ * partition
+ */
+int zynq_validate_partition(u32 start_addr, u32 len, u32 chksum_off)
+{
+	u8 checksum[MD5_CHECKSUM_SIZE];
+	u8 calchecksum[MD5_CHECKSUM_SIZE];
+
+	memcpy(&checksum[0], (u32 *)chksum_off, MD5_CHECKSUM_SIZE);
+
+	md5_wd((u8 *)start_addr, len, &calchecksum[0], 0x10000);
+
+	if (!memcmp(checksum, calchecksum, MD5_CHECKSUM_SIZE))
+		return 0;
+
+	printf("Error: Partition DataChecksum\n");
+	return -1;
+}
diff --git a/board/xilinx/zynq/cmds.c b/board/xilinx/zynq/cmds.c
new file mode 100644
index 0000000..0b2a817
--- /dev/null
+++ b/board/xilinx/zynq/cmds.c
@@ -0,0 +1,513 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 Xilinx, Inc.
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/sys_proto.h>
+#include <malloc.h>
+#include <u-boot/md5.h>
+#include <u-boot/rsa.h>
+#include <u-boot/rsa-mod-exp.h>
+#include <u-boot/sha256.h>
+#include <zynqpl.h>
+#include <fpga.h>
+#include <zynq_bootimg.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#ifdef CONFIG_CMD_ZYNQ_RSA
+
+#define ZYNQ_EFUSE_RSA_ENABLE_MASK	0x400
+#define ZYNQ_ATTRIBUTE_PL_IMAGE_MASK		0x20
+#define ZYNQ_ATTRIBUTE_CHECKSUM_TYPE_MASK	0x7000
+#define ZYNQ_ATTRIBUTE_RSA_PRESENT_MASK		0x8000
+#define ZYNQ_ATTRIBUTE_RSA_PART_OWNER_MASK	0x30000
+
+#define ZYNQ_RSA_MODULAR_SIZE			256
+#define ZYNQ_RSA_MODULAR_EXT_SIZE		256
+#define ZYNQ_RSA_EXPO_SIZE			64
+#define ZYNQ_RSA_SPK_SIGNATURE_SIZE		256
+#define ZYNQ_RSA_PARTITION_SIGNATURE_SIZE	256
+#define ZYNQ_RSA_SIGNATURE_SIZE			0x6C0
+#define ZYNQ_RSA_HEADER_SIZE			4
+#define ZYNQ_RSA_MAGIC_WORD_SIZE		60
+#define ZYNQ_RSA_PART_OWNER_UBOOT		1
+#define ZYNQ_RSA_ALIGN_PPK_START		64
+
+#define WORD_LENGTH_SHIFT	2
+
+static u8 *ppkmodular;
+static u8 *ppkmodularex;
+
+struct zynq_rsa_public_key {
+	uint len;		/* Length of modulus[] in number of u32 */
+	u32 n0inv;		/* -1 / modulus[0] mod 2^32 */
+	u32 *modulus;	/* modulus as little endian array */
+	u32 *rr;		/* R^2 as little endian array */
+};
+
+static struct zynq_rsa_public_key public_key;
+
+static struct partition_hdr part_hdr[ZYNQ_MAX_PARTITION_NUMBER];
+
+/*
+ * Extract the primary public key components from already autheticated FSBL
+ */
+static void zynq_extract_ppk(u32 fsbl_len)
+{
+	u32 padsize;
+	u8 *ppkptr;
+
+	debug("%s\n", __func__);
+
+	/*
+	 * Extract the authenticated PPK from OCM i.e at end of the FSBL
+	 */
+	ppkptr = (u8 *)(fsbl_len + ZYNQ_OCM_BASEADDR);
+	padsize = ((u32)ppkptr % ZYNQ_RSA_ALIGN_PPK_START);
+	if (padsize)
+		ppkptr += (ZYNQ_RSA_ALIGN_PPK_START - padsize);
+
+	ppkptr += ZYNQ_RSA_HEADER_SIZE;
+
+	ppkptr += ZYNQ_RSA_MAGIC_WORD_SIZE;
+
+	ppkmodular = (u8 *)ppkptr;
+	ppkptr += ZYNQ_RSA_MODULAR_SIZE;
+	ppkmodularex = (u8 *)ppkptr;
+	ppkptr += ZYNQ_RSA_MODULAR_EXT_SIZE;
+}
+
+/*
+ * Calculate the inverse(-1 / modulus[0] mod 2^32 ) for the PPK
+ */
+static u32 zynq_calc_inv(void)
+{
+	u32 modulus = public_key.modulus[0];
+	u32 tmp = BIT(1);
+	u32 inverse;
+
+	inverse = modulus & BIT(0);
+
+	while (tmp) {
+		inverse *= 2 - modulus * inverse;
+		tmp *= tmp;
+	}
+
+	return ~(inverse - 1);
+}
+
+/*
+ * Recreate the signature by padding the bytes and verify with hash value
+ */
+static int zynq_pad_and_check(u8 *signature, u8 *hash)
+{
+	u8 padding[] = {0x30, 0x31, 0x30, 0x0D, 0x06, 0x09, 0x60, 0x86, 0x48,
+			0x01, 0x65, 0x03, 0x04, 0x02, 0x01, 0x05, 0x00, 0x04,
+			0x20};
+	u8 *pad_ptr = signature + 256;
+	u32 pad = 202;
+	u32 ii;
+
+	/*
+	 * Re-Create PKCS#1v1.5 Padding
+	 * MSB  ----------------------------------------------------LSB
+	 * 0x0 || 0x1 || 0xFF(for 202 bytes) || 0x0 || T_padding || SHA256 Hash
+	 */
+	if (*--pad_ptr != 0 || *--pad_ptr != 1)
+		return -1;
+
+	for (ii = 0; ii < pad; ii++) {
+		if (*--pad_ptr != 0xFF)
+			return -1;
+	}
+
+	if (*--pad_ptr != 0)
+		return -1;
+
+	for (ii = 0; ii < sizeof(padding); ii++) {
+		if (*--pad_ptr != padding[ii])
+			return -1;
+	}
+
+	for (ii = 0; ii < 32; ii++) {
+		if (*--pad_ptr != hash[ii])
+			return -1;
+	}
+	return 0;
+}
+
+/*
+ * Verify and extract the hash value from signature using the public key
+ * and compare it with calculated hash value.
+ */
+static int zynq_rsa_verify_key(const struct zynq_rsa_public_key *key,
+			       const u8 *sig, const u32 sig_len, const u8 *hash)
+{
+	int status;
+	void *buf;
+
+	if (!key || !sig || !hash)
+		return -1;
+
+	if (sig_len != (key->len * sizeof(u32))) {
+		printf("Signature is of incorrect length %d\n", sig_len);
+		return -1;
+	}
+
+	/* Sanity check for stack size */
+	if (sig_len > ZYNQ_RSA_SPK_SIGNATURE_SIZE) {
+		printf("Signature length %u exceeds maximum %d\n", sig_len,
+		       ZYNQ_RSA_SPK_SIGNATURE_SIZE);
+		return -1;
+	}
+
+	buf = malloc(sig_len);
+	if (!buf)
+		return -1;
+
+	memcpy(buf, sig, sig_len);
+
+	status = zynq_pow_mod((u32 *)key, (u32 *)buf);
+	if (status == -1) {
+		free(buf);
+		return status;
+	}
+
+	status = zynq_pad_and_check((u8 *)buf, (u8 *)hash);
+
+	free(buf);
+	return status;
+}
+
+/*
+ * Authenticate the partition
+ */
+static int zynq_authenticate_part(u8 *buffer, u32 size)
+{
+	u8 hash_signature[32];
+	u8 *spk_modular;
+	u8 *spk_modular_ex;
+	u8 *signature_ptr;
+	u32 status;
+
+	debug("%s\n", __func__);
+
+	signature_ptr = (u8 *)(buffer + size - ZYNQ_RSA_SIGNATURE_SIZE);
+
+	signature_ptr += ZYNQ_RSA_HEADER_SIZE;
+
+	signature_ptr += ZYNQ_RSA_MAGIC_WORD_SIZE;
+
+	ppkmodular = (u8 *)signature_ptr;
+	signature_ptr += ZYNQ_RSA_MODULAR_SIZE;
+	ppkmodularex = signature_ptr;
+	signature_ptr += ZYNQ_RSA_MODULAR_EXT_SIZE;
+	signature_ptr += ZYNQ_RSA_EXPO_SIZE;
+
+	sha256_csum_wd((const unsigned char *)signature_ptr,
+		       (ZYNQ_RSA_MODULAR_EXT_SIZE + ZYNQ_RSA_EXPO_SIZE +
+		       ZYNQ_RSA_MODULAR_SIZE),
+		       (unsigned char *)hash_signature, 0x1000);
+
+	spk_modular = (u8 *)signature_ptr;
+	signature_ptr += ZYNQ_RSA_MODULAR_SIZE;
+	spk_modular_ex = (u8 *)signature_ptr;
+	signature_ptr += ZYNQ_RSA_MODULAR_EXT_SIZE;
+	signature_ptr += ZYNQ_RSA_EXPO_SIZE;
+
+	public_key.len = ZYNQ_RSA_MODULAR_SIZE / sizeof(u32);
+	public_key.modulus = (u32 *)ppkmodular;
+	public_key.rr = (u32 *)ppkmodularex;
+	public_key.n0inv = zynq_calc_inv();
+
+	status = zynq_rsa_verify_key(&public_key, signature_ptr,
+				     ZYNQ_RSA_SPK_SIGNATURE_SIZE,
+				     hash_signature);
+	if (status)
+		return status;
+
+	signature_ptr += ZYNQ_RSA_SPK_SIGNATURE_SIZE;
+
+	sha256_csum_wd((const unsigned char *)buffer,
+		       (size - ZYNQ_RSA_PARTITION_SIGNATURE_SIZE),
+		       (unsigned char *)hash_signature, 0x1000);
+
+	public_key.len = ZYNQ_RSA_MODULAR_SIZE / sizeof(u32);
+	public_key.modulus = (u32 *)spk_modular;
+	public_key.rr = (u32 *)spk_modular_ex;
+	public_key.n0inv = zynq_calc_inv();
+
+	return zynq_rsa_verify_key(&public_key, (u8 *)signature_ptr,
+				   ZYNQ_RSA_PARTITION_SIGNATURE_SIZE,
+				   (u8 *)hash_signature);
+}
+
+/*
+ * Parses the partition header and verfies the authenticated and
+ * encrypted image.
+ */
+static int zynq_verify_image(u32 src_ptr)
+{
+	u32 silicon_ver, image_base_addr, status;
+	u32 partition_num = 0;
+	u32 efuseval, srcaddr, size, fsbl_len;
+	struct partition_hdr *hdr_ptr;
+	u32 part_data_len, part_img_len, part_attr;
+	u32 part_load_addr, part_dst_addr, part_chksum_offset;
+	u32 part_start_addr, part_total_size, partitioncount;
+	bool encrypt_part_flag = false;
+	bool part_chksum_flag = false;
+	bool signed_part_flag = false;
+
+	image_base_addr = src_ptr;
+
+	silicon_ver = zynq_get_silicon_version();
+
+	/* RSA not supported in silicon versions 1.0 and 2.0 */
+	if (silicon_ver == 0 || silicon_ver == 1)
+		return -1;
+
+	zynq_get_partition_info(image_base_addr, &fsbl_len,
+				&part_hdr[0]);
+
+	/* Extract ppk if efuse was blown Otherwise return error */
+	efuseval = readl(&efuse_base->status);
+	if (!(efuseval & ZYNQ_EFUSE_RSA_ENABLE_MASK))
+		return -1;
+
+	zynq_extract_ppk(fsbl_len);
+
+	partitioncount = zynq_get_part_count(&part_hdr[0]);
+
+	/*
+	 * As the first two partitions are related to fsbl,
+	 * we can ignore those two in bootimage and the below
+	 * code doesn't need to validate it as fsbl is already
+	 * done by now
+	 */
+	if (partitioncount <= 2 ||
+	    partitioncount > ZYNQ_MAX_PARTITION_NUMBER)
+		return -1;
+
+	while (partition_num < partitioncount) {
+		if (((part_hdr[partition_num].partitionattr &
+		   ZYNQ_ATTRIBUTE_RSA_PART_OWNER_MASK) >> 16) !=
+		   ZYNQ_RSA_PART_OWNER_UBOOT) {
+			printf("UBOOT is not Owner for partition %d\n",
+			       partition_num);
+			partition_num++;
+			continue;
+		}
+		hdr_ptr = &part_hdr[partition_num];
+		status = zynq_validate_hdr(hdr_ptr);
+		if (status)
+			return status;
+
+		part_data_len = hdr_ptr->datawordlen;
+		part_img_len = hdr_ptr->imagewordlen;
+		part_attr = hdr_ptr->partitionattr;
+		part_load_addr = hdr_ptr->loadaddr;
+		part_chksum_offset = hdr_ptr->checksumoffset;
+		part_start_addr = hdr_ptr->partitionstart;
+		part_total_size = hdr_ptr->partitionwordlen;
+
+		if (part_data_len != part_img_len) {
+			debug("Encrypted\n");
+			encrypt_part_flag = true;
+		}
+
+		if (part_attr & ZYNQ_ATTRIBUTE_CHECKSUM_TYPE_MASK)
+			part_chksum_flag = true;
+
+		if (part_attr & ZYNQ_ATTRIBUTE_RSA_PRESENT_MASK) {
+			debug("RSA Signed\n");
+			signed_part_flag = true;
+			size = part_total_size << WORD_LENGTH_SHIFT;
+		} else {
+			size = part_img_len;
+		}
+
+		if (!signed_part_flag && !part_chksum_flag) {
+			printf("Partition not signed & no chksum\n");
+			partition_num++;
+			continue;
+		}
+
+		srcaddr = image_base_addr +
+			  (part_start_addr << WORD_LENGTH_SHIFT);
+
+		/*
+		 * This validation is just for PS DDR.
+		 * TODO: Update this for PL DDR check as well.
+		 */
+		if (part_load_addr < gd->bd->bi_dram[0].start &&
+		    ((part_load_addr + part_data_len) >
+		    (gd->bd->bi_dram[0].start +
+		     gd->bd->bi_dram[0].size))) {
+			printf("INVALID_LOAD_ADDRESS_FAIL\n");
+			return -1;
+		}
+
+		if (part_attr & ZYNQ_ATTRIBUTE_PL_IMAGE_MASK)
+			part_load_addr = srcaddr;
+		else
+			memcpy((u32 *)part_load_addr, (u32 *)srcaddr,
+			       size);
+
+		if (part_chksum_flag) {
+			part_chksum_offset = image_base_addr +
+					     (part_chksum_offset <<
+					     WORD_LENGTH_SHIFT);
+			status = zynq_validate_partition(part_load_addr,
+							 (part_total_size <<
+							  WORD_LENGTH_SHIFT),
+							 part_chksum_offset);
+			if (status != 0) {
+				printf("PART_CHKSUM_FAIL\n");
+				return -1;
+			}
+			debug("Partition Validation Done\n");
+		}
+
+		if (signed_part_flag) {
+			status = zynq_authenticate_part((u8 *)part_load_addr,
+							size);
+			if (status != 0) {
+				printf("AUTHENTICATION_FAIL\n");
+				return -1;
+			}
+			debug("Authentication Done\n");
+		}
+
+		if (encrypt_part_flag) {
+			debug("DECRYPTION\n");
+
+			part_dst_addr = part_load_addr;
+
+			if (part_attr & ZYNQ_ATTRIBUTE_PL_IMAGE_MASK) {
+				partition_num++;
+				continue;
+			}
+
+			status = zynq_decrypt_load(part_load_addr,
+						   part_img_len,
+						   part_dst_addr,
+						   part_data_len);
+			if (status != 0) {
+				printf("DECRYPTION_FAIL\n");
+				return -1;
+			}
+		}
+		partition_num++;
+	}
+
+	return 0;
+}
+
+static int do_zynq_rsa(cmd_tbl_t *cmdtp, int flag, int argc,
+		       char * const argv[])
+{
+	u32 src_ptr;
+	char *endp;
+
+	src_ptr = simple_strtoul(argv[2], &endp, 16);
+	if (*argv[2] == 0 || *endp != 0)
+		return CMD_RET_USAGE;
+	if (zynq_verify_image(src_ptr))
+		return CMD_RET_FAILURE;
+
+	return CMD_RET_SUCCESS;
+}
+#endif
+
+#ifdef CONFIG_CMD_ZYNQ_AES
+static int zynq_decrypt_image(cmd_tbl_t *cmdtp, int flag, int argc,
+			      char * const argv[])
+{
+	char *endp;
+	u32 srcaddr, srclen, dstaddr, dstlen;
+	int status;
+
+	srcaddr = simple_strtoul(argv[2], &endp, 16);
+	if (*argv[2] == 0 || *endp != 0)
+		return CMD_RET_USAGE;
+	srclen = simple_strtoul(argv[3], &endp, 16);
+	if (*argv[3] == 0 || *endp != 0)
+		return CMD_RET_USAGE;
+	dstaddr = simple_strtoul(argv[4], &endp, 16);
+	if (*argv[4] == 0 || *endp != 0)
+		return CMD_RET_USAGE;
+	dstlen = simple_strtoul(argv[5], &endp, 16);
+	if (*argv[5] == 0 || *endp != 0)
+		return CMD_RET_USAGE;
+
+	/*
+	 * Roundup source and destination lengths to
+	 * word size
+	 */
+	if (srclen % 4)
+		srclen = roundup(srclen, 4);
+	if (dstlen % 4)
+		dstlen = roundup(dstlen, 4);
+
+	status = zynq_decrypt_load(srcaddr, srclen >> 2, dstaddr, dstlen >> 2);
+	if (status != 0)
+		return CMD_RET_FAILURE;
+
+	return CMD_RET_SUCCESS;
+}
+#endif
+
+static cmd_tbl_t zynq_commands[] = {
+#ifdef CONFIG_CMD_ZYNQ_RSA
+	U_BOOT_CMD_MKENT(rsa, 3, 1, do_zynq_rsa, "", ""),
+#endif
+#ifdef CONFIG_CMD_ZYNQ_AES
+	U_BOOT_CMD_MKENT(aes, 6, 1, zynq_decrypt_image, "", ""),
+#endif
+};
+
+static int do_zynq(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	cmd_tbl_t *zynq_cmd;
+	int ret;
+
+	if (!ARRAY_SIZE(zynq_commands)) {
+		puts("No zynq specific command enabled\n");
+		return CMD_RET_USAGE;
+	}
+
+	if (argc < 2)
+		return CMD_RET_USAGE;
+	zynq_cmd = find_cmd_tbl(argv[1], zynq_commands,
+				ARRAY_SIZE(zynq_commands));
+	if (!zynq_cmd || argc != zynq_cmd->maxargs)
+		return CMD_RET_USAGE;
+
+	ret = zynq_cmd->cmd(zynq_cmd, flag, argc, argv);
+
+	return cmd_process_error(zynq_cmd, ret);
+}
+
+static char zynq_help_text[] =
+	""
+#ifdef CONFIG_CMD_ZYNQ_RSA
+	"rsa <baseaddr>  - Verifies the authenticated and encrypted\n"
+	"                  zynq images and loads them back to load\n"
+	"                  addresses as specified in BOOT image(BOOT.BIN)\n"
+#endif
+#ifdef CONFIG_CMD_ZYNQ_AES
+	"aes <srcaddr> <srclen> <dstaddr> <dstlen>\n"
+	"                - Decrypts the encrypted image present in source\n"
+	"                  address and places the decrypted image at\n"
+	"                  destination address\n"
+#endif
+	;
+
+U_BOOT_CMD(zynq,	6,	0,	do_zynq,
+	   "Zynq specific commands", zynq_help_text
+);
diff --git a/configs/zynq_cse_qspi_defconfig b/configs/zynq_cse_qspi_defconfig
index 2425dfa..df6ebdc 100644
--- a/configs/zynq_cse_qspi_defconfig
+++ b/configs/zynq_cse_qspi_defconfig
@@ -7,6 +7,7 @@ CONFIG_DEBUG_UART_BASE=0x0
 CONFIG_DEBUG_UART_CLOCK=0
 CONFIG_SPL_STACK_R_ADDR=0x200000
 # CONFIG_ZYNQ_DDRC_INIT is not set
+# CONFIG_CMD_ZYNQ is not set
 CONFIG_DEFAULT_DEVICE_TREE="zynq-cse-qspi-single"
 CONFIG_DEBUG_UART=y
 CONFIG_DISTRO_DEFAULTS=y
diff --git a/drivers/fpga/zynqpl.c b/drivers/fpga/zynqpl.c
index fd37d18..6409d30 100644
--- a/drivers/fpga/zynqpl.c
+++ b/drivers/fpga/zynqpl.c
@@ -17,6 +17,7 @@
 
 #define DEVCFG_CTRL_PCFG_PROG_B		0x40000000
 #define DEVCFG_CTRL_PCFG_AES_EFUSE_MASK	0x00001000
+#define DEVCFG_CTRL_PCAP_RATE_EN_MASK	0x02000000
 #define DEVCFG_ISR_FATAL_ERROR_MASK	0x00740040
 #define DEVCFG_ISR_ERROR_FLAGS_MASK	0x00340840
 #define DEVCFG_ISR_RX_FIFO_OV		0x00040000
@@ -497,3 +498,47 @@ struct xilinx_fpga_op zynq_op = {
 	.loadfs = zynq_loadfs,
 #endif
 };
+
+#ifdef CONFIG_CMD_ZYNQ_AES
+/*
+ * Load the encrypted image from src addr and decrypt the image and
+ * place it back the decrypted image into dstaddr.
+ */
+int zynq_decrypt_load(u32 srcaddr, u32 srclen, u32 dstaddr, u32 dstlen)
+{
+	if (srcaddr < SZ_1M || dstaddr < SZ_1M) {
+		printf("%s: src and dst addr should be > 1M\n",
+		       __func__);
+		return FPGA_FAIL;
+	}
+
+	if (zynq_dma_xfer_init(BIT_NONE)) {
+		printf("%s: zynq_dma_xfer_init FAIL\n", __func__);
+		return FPGA_FAIL;
+	}
+
+	writel((readl(&devcfg_base->ctrl) | DEVCFG_CTRL_PCAP_RATE_EN_MASK),
+	       &devcfg_base->ctrl);
+
+	debug("%s: Source = 0x%08X\n", __func__, (u32)srcaddr);
+	debug("%s: Size = %zu\n", __func__, srclen);
+
+	/* flush(clean & invalidate) d-cache range buf */
+	flush_dcache_range((u32)srcaddr, (u32)srcaddr +
+			roundup(srclen << 2, ARCH_DMA_MINALIGN));
+	/*
+	 * Flush destination address range only if image is not
+	 * bitstream.
+	 */
+	flush_dcache_range((u32)dstaddr, (u32)dstaddr +
+			   roundup(dstlen << 2, ARCH_DMA_MINALIGN));
+
+	if (zynq_dma_transfer(srcaddr | 1, srclen, dstaddr | 1, dstlen))
+		return FPGA_FAIL;
+
+	writel((readl(&devcfg_base->ctrl) & ~DEVCFG_CTRL_PCAP_RATE_EN_MASK),
+	       &devcfg_base->ctrl);
+
+	return FPGA_SUCCESS;
+}
+#endif
diff --git a/include/u-boot/rsa-mod-exp.h b/include/u-boot/rsa-mod-exp.h
index 3253614..8a428c4 100644
--- a/include/u-boot/rsa-mod-exp.h
+++ b/include/u-boot/rsa-mod-exp.h
@@ -42,6 +42,10 @@ int rsa_mod_exp_sw(const uint8_t *sig, uint32_t sig_len,
 int rsa_mod_exp(struct udevice *dev, const uint8_t *sig, uint32_t sig_len,
 		struct key_prop *node, uint8_t *out);
 
+#if defined(CONFIG_CMD_ZYNQ_RSA)
+int zynq_pow_mod(u32 *keyptr, u32 *inout);
+#endif
+
 /**
  * struct struct mod_exp_ops - Driver model for RSA Modular Exponentiation
  *				operations
diff --git a/include/zynq_bootimg.h b/include/zynq_bootimg.h
new file mode 100644
index 0000000..c39c0bf
--- /dev/null
+++ b/include/zynq_bootimg.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2018 Xilinx, Inc.
+ */
+
+#ifndef _ZYNQ_BOOTIMG_H_
+#define _ZYNQ_BOOTIMG_H_
+
+#define ZYNQ_MAX_PARTITION_NUMBER	0xE
+
+struct partition_hdr {
+	u32 imagewordlen;	/* 0x0 */
+	u32 datawordlen;	/* 0x4 */
+	u32 partitionwordlen;	/* 0x8 */
+	u32 loadaddr;		/* 0xC */
+	u32 execaddr;		/* 0x10 */
+	u32 partitionstart;	/* 0x14 */
+	u32 partitionattr;	/* 0x18 */
+	u32 sectioncount;	/* 0x1C */
+	u32 checksumoffset;	/* 0x20 */
+	u32 pads1[1];
+	u32 acoffset;	/* 0x28 */
+	u32 pads2[4];
+	u32 checksum;		/* 0x3C */
+};
+
+int zynq_get_part_count(struct partition_hdr *part_hdr_info);
+int zynq_get_partition_info(u32 image_base_addr, u32 *fsbl_len,
+			    struct partition_hdr *part_hdr);
+int zynq_validate_hdr(struct partition_hdr *header);
+int zynq_validate_partition(u32 start_addr, u32 len, u32 chksum_off);
+
+#endif /* _ZYNQ_BOOTIMG_H_ */
diff --git a/include/zynqpl.h b/include/zynqpl.h
index cdfd8a2..766e691 100644
--- a/include/zynqpl.h
+++ b/include/zynqpl.h
@@ -11,6 +11,10 @@
 
 #include <xilinx.h>
 
+#ifdef CONFIG_CMD_ZYNQ_AES
+int zynq_decrypt_load(u32 srcaddr, u32 dstaddr, u32 srclen, u32 dstlen);
+#endif
+
 extern struct xilinx_fpga_op zynq_op;
 
 #define XILINX_ZYNQ_XC7Z007S	0x3
diff --git a/lib/rsa/rsa-mod-exp.c b/lib/rsa/rsa-mod-exp.c
index 031c710..420ab2e 100644
--- a/lib/rsa/rsa-mod-exp.c
+++ b/lib/rsa/rsa-mod-exp.c
@@ -300,3 +300,54 @@ int rsa_mod_exp_sw(const uint8_t *sig, uint32_t sig_len,
 
 	return 0;
 }
+
+#if defined(CONFIG_CMD_ZYNQ_RSA)
+/**
+ * zynq_pow_mod - in-place public exponentiation
+ *
+ * @keyptr:	RSA key
+ * @inout:	Big-endian word array containing value and result
+ * @return 0 on successful calculation, otherwise failure error code
+ *
+ * FIXME: Use pow_mod() instead of zynq_pow_mod()
+ *        pow_mod calculation required for zynq is bit different from
+ *        pw_mod above here, hence defined zynq specific routine.
+ */
+int zynq_pow_mod(u32 *keyptr, u32 *inout)
+{
+	u32 *result, *ptr;
+	uint i;
+	struct rsa_public_key *key;
+	u32 val[RSA2048_BYTES], acc[RSA2048_BYTES], tmp[RSA2048_BYTES];
+
+	key = (struct rsa_public_key *)keyptr;
+
+	/* Sanity check for stack size - key->len is in 32-bit words */
+	if (key->len > RSA_MAX_KEY_BITS / 32) {
+		debug("RSA key words %u exceeds maximum %d\n", key->len,
+		      RSA_MAX_KEY_BITS / 32);
+		return -EINVAL;
+	}
+
+	result = tmp;  /* Re-use location. */
+
+	for (i = 0, ptr = inout; i < key->len; i++, ptr++)
+		val[i] = *(ptr);
+
+	montgomery_mul(key, acc, val, key->rr);  /* axx = a * RR / R mod M */
+	for (i = 0; i < 16; i += 2) {
+		montgomery_mul(key, tmp, acc, acc); /* tmp = acc^2 / R mod M */
+		montgomery_mul(key, acc, tmp, tmp); /* acc = tmp^2 / R mod M */
+	}
+	montgomery_mul(key, result, acc, val);  /* result = XX * a / R mod M */
+
+	/* Make sure result < mod; result is at most 1x mod too large. */
+	if (greater_equal_modulus(key, result))
+		subtract_modulus(key, result);
+
+	for (i = 0, ptr = inout; i < key->len; i++, ptr++)
+		*ptr = result[i];
+
+	return 0;
+}
+#endif