Tegra2: implement Warmboot code and lp0_vec

board/tegra2/common/crypto/crypto.c

Index: board/tegra2/common/crypto/crypto.c
diff --git a/board/tegra2/common/crypto/crypto.c b/board/tegra2/common/crypto/crypto.c
new file mode 100755
index 0000000000000000000000000000000000000000..85811bbf3c1eddc2a77d7757e4b0e5d80b5972a0
--- /dev/null
+++ b/board/tegra2/common/crypto/crypto.c
@@ -0,0 +1,328 @@
+/*
+ *  (C) Copyright 2010
+ *  NVIDIA Corporation <www.nvidia.com>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+/*
+ * crypto.c - Cryptography support
+ */
+
+#include <common.h>
+#include <asm/arch/nvcommon.h>
+#include <asm/arch/nvboot_error.h>
+#include "crypto.h"
+#include "nvaes_ref.h"
+
+/* Local function declarations */
+static void
+apply_cbc_chain_data(NvU8 *cbc_chain_data, NvU8 *src, NvU8 *dst);
+
+static NvBootError
+determine_crypto_ops(security_mode security, NvBool *encrypt, NvBool *sign);
+
+static void
+generate_key_schedule(NvU8 *key, NvU8 *key_schedule, NvBool encrypt_data);
+
+static void
+encrypt_object( NvU8	*key_schedule,
+		NvU8	*src,
+		NvU8	*dst,
+		NvU32	num_aes_blocks);
+
+static NvBootError
+encrypt_and_sign(NvU8		*key,
+		 security_mode	security,
+		 NvU8		*src,
+		 NvU32		length,
+		 NvU8		*sig_dst);
+
+NvBool enable_debug_crypto = NV_FALSE;
+
+/* Implementation */
+static NvU8 zero_key[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+static void
+print_vector(char *name, NvU32 num_bytes, NvU8 *data)
+{
+	NvU32 i;
+
+	printf("%s [%d] @0x%08x", name, num_bytes, (NvU32)data);
+	for (i=0; i<num_bytes; i++) {
+		if ( i % 16 == 0 )
+			printf(" = ");
+		printf("%02x", data[i]);
+		if ( (i+1) % 16 != 0 )
+			printf(" ");
+	}
+	printf("\n");
+}
+
+
+static void
+apply_cbc_chain_data(NvU8 *cbc_chain_data, NvU8 *src, NvU8 *dst)
+{
+	int i;
+
+	for (i = 0; i < 16; i++) {
+		*dst++ = *src++ ^ *cbc_chain_data++;
+	}
+}
+
+static NvBootError
+determine_crypto_ops(security_mode security, NvBool *encrypt, NvBool *sign)
+{
+	NvBootError Error = NvBootError_Success;
+
+	switch (security) {
+	case security_mode_Plaintext:
+		*encrypt = NV_FALSE;
+		*sign = NV_FALSE;
+		break;
+
+	case security_mode_Checksum:
+		*encrypt = NV_FALSE;
+		*sign = NV_TRUE;
+		break;
+
+	case security_mode_Encrypted:
+		*encrypt = NV_TRUE;
+		*sign = NV_TRUE;
+		break;
+
+	default:
+		printf("determine_crypto_ops: security mode not set.\n");
+		Error = NvBootError_InvalidParameter;
+	}
+
+	return Error;
+}
+
+static void
+generate_key_schedule(NvU8 *key, NvU8 *key_schedule, NvBool encrypt_data)
+{
+	/* Expand the key to produce a key schedule. */
+	if (encrypt_data == NV_TRUE) {
+		/* Expand the provided key. */
+		nv_aes_expand_key(key, key_schedule);
+	} else {
+		/*
+		 * The only need for a key is for signing/checksum purposes, so
+		 * expand a key of 0's.
+		 */
+		nv_aes_expand_key(zero_key, key_schedule);
+	}
+}
+
+static void
+encrypt_object(NvU8	*key_schedule,
+		NvU8	*src,
+		NvU8	*dst,
+		NvU32	num_aes_blocks)
+{
+	NvU32 i;
+	NvU8 *cbc_chain_data;
+	NvU8  tmp_data[KEY_LENGTH];
+
+	cbc_chain_data = zero_key; /* Convenient array of 0's for IV */
+
+	for (i = 0; i < num_aes_blocks; i++) {
+		if (enable_debug_crypto) {
+			printf("encrypt_object: block %d of %d\n", i,
+							num_aes_blocks);
+			print_vector("AES Src", KEY_LENGTH, src);
+		}
+
+		/* Apply the chain data */
+		apply_cbc_chain_data(cbc_chain_data, src, tmp_data);
+
+		if (enable_debug_crypto) print_vector("AES Xor", KEY_LENGTH,
+							tmp_data);
+
+		/* encrypt the AES block */
+		nv_aes_encrypt(tmp_data, key_schedule, dst);
+
+		if (enable_debug_crypto)
+			print_vector("AES Dst", KEY_LENGTH, dst);
+		/* Update pointers for next loop. */
+		cbc_chain_data = dst;
+		src += KEY_LENGTH;
+		dst += KEY_LENGTH;
+	}
+}
+
+static void
+left_shift_vector(NvU8	*In,
+		  NvU8	*Out,
+		  NvU32	Size)
+{
+	NvU32 i;
+	NvU8 Carry = 0;
+
+	for (i=0; i<Size; i++) {
+		NvU32 j = Size-1-i;
+
+		Out[j] = (In[j] << 1) | Carry;
+		Carry = In[j] >> 7; /* get most significant bit */
+	}
+}
+
+static void
+sign_objext(
+	NvU8	*key,
+	NvU8	*key_schedule,
+	NvU8	*src,
+	NvU8	*dst,
+	NvU32	num_aes_blocks)
+{
+	NvU32 i;
+	NvU8 *cbc_chain_data;
+
+	NvU8	L[KEY_LENGTH];
+	NvU8	K1[KEY_LENGTH];
+	NvU8	tmp_data[KEY_LENGTH];
+
+	cbc_chain_data = zero_key; /* Convenient array of 0's for IV */
+
+	/* compute K1 constant needed by AES-CMAC calculation */
+
+	for (i=0; i<KEY_LENGTH; i++)
+		tmp_data[i] = 0;
+
+	encrypt_object(key_schedule, tmp_data, L, 1);
+
+	if (enable_debug_crypto)
+		print_vector("AES(key, nonce)", KEY_LENGTH, L);
+
+	left_shift_vector(L, K1, sizeof(L));
+
+	if (enable_debug_crypto)
+		print_vector("L", KEY_LENGTH, L);
+
+	if ( (L[0] >> 7) != 0 ) /* get MSB of L */
+		K1[KEY_LENGTH-1] ^= AES_CMAC_CONST_RB;
+
+	if (enable_debug_crypto)
+		print_vector("K1", KEY_LENGTH, K1);
+
+	/* compute the AES-CMAC value */
+	for (i = 0; i < num_aes_blocks; i++) {
+		/* Apply the chain data */
+		apply_cbc_chain_data(cbc_chain_data, src, tmp_data);
+
+		/* for the final block, XOR K1 into the IV */
+		if ( i == num_aes_blocks-1 )
+			apply_cbc_chain_data(tmp_data, K1, tmp_data);
+
+		/* encrypt the AES block */
+		nv_aes_encrypt(tmp_data, key_schedule, (NvU8*)dst);
+
+		if (enable_debug_crypto) {
+			printf("sign_objext: block %d of %d\n", i,
+							num_aes_blocks);
+			print_vector("AES-CMAC Src", KEY_LENGTH, src);
+			print_vector("AES-CMAC Xor", KEY_LENGTH, tmp_data);
+			print_vector("AES-CMAC Dst", KEY_LENGTH, (NvU8*)dst);
+		}
+
+		/* Update pointers for next loop. */
+		cbc_chain_data = (NvU8*)dst;
+		src += KEY_LENGTH;
+	}
+
+	if (enable_debug_crypto)
+		print_vector("AES-CMAC Hash", KEY_LENGTH, (NvU8*)dst);
+}
+
+static NvBootError
+encrypt_and_sign(NvU8		*key,
+		security_mode	security,
+		NvU8		*src,
+		NvU32		length,
+		NvU8		*sig_dst)
+{
+	NvBool	encrypt_data;
+	NvBool	sign_data;
+	NvU32	num_aes_blocks;
+	NvU8	key_schedule[4*NVAES_STATECOLS*(NVAES_ROUNDS+1)];
+	NvBootError Error;
+
+	Error = determine_crypto_ops(security, &encrypt_data, &sign_data);
+	if (Error != NvBootError_Success)
+		return Error;
+
+	if (enable_debug_crypto) {
+		printf("encrypt_and_sign: Length = %d\n", length);
+		print_vector("AES key", KEY_LENGTH, key);
+	}
+
+	generate_key_schedule(key, key_schedule, encrypt_data);
+
+	num_aes_blocks = ICEIL(length, KEY_LENGTH);
+
+	if (encrypt_data == NV_TRUE) {
+		if (enable_debug_crypto)
+			printf("encrypt_and_sign: begin encryption\n");
+
+		/* Perform this in place, resulting in src being encrypted. */
+		encrypt_object(key_schedule, src, src, num_aes_blocks);
+
+		if (enable_debug_crypto)
+			printf("encrypt_and_sign: end encryption\n");
+	}
+
+	if (sign_data == NV_TRUE) {
+		if (enable_debug_crypto)
+			printf("encrypt_and_sign: begin signing\n");
+
+		/* encrypt the data, overwriting the result in signature. */
+		sign_objext(key, key_schedule, src, sig_dst, num_aes_blocks);
+
+		if (enable_debug_crypto)
+			printf("encrypt_and_sign: end signing\n");
+	}
+
+	return Error;
+}
+
+NvBootError
+encrypt_and_sign_block(NvU8	*key,
+		security_mode	security,
+		NvU8		*block_image,
+		NvU32		block_len,
+		NvU8		*signature)
+{
+	NvBootError e;
+
+	e = encrypt_and_sign(key, security, block_image, block_len, signature);
+	return e;
+}
+
+NvBootError
+sign_data_block(NvU8 *source,
+		NvU32 length,
+		NvU8 *signature)
+{
+	return encrypt_and_sign_block(zero_key,
+				security_mode_Checksum,
+				source,
+				length,
+				signature);
+}