cr50: add support for hardware HMAC and one-shot SHA

1. Hardware HMAC implementation is added for key lengths <= 32 bytes and
   more than 64 bytes. Keys between 32 and 64 bytes use hybrid approach.
2. HMAC DRBG performance increased even more from 520us to 320us per
   32  bytes.
3. Added support for one-shot SHA operation which is a bit faster than
   livestream mode when message length is known beforehand.
4. Image size impact - +216 bytes.
5. Added opportunities to enable keyladder code to use some common
   primitives like dcrypto_fifo_load() instead of it's own versions.
6. Added new console command hmac activated with CRYPTO_TEST=1 to
   test all paths (hw, sw, hybrid for HMAC) for SHA256, SHA1 and
   HMAC SHA256. Due to size of test vectors, you should choose one at
   a time to test. Also, since HMAC is used by DRBG, DRBG tests are also
   relevant.

BUG=b:195092622
TEST=make CRYPTO_TEST=1; 'hmac' command in console, tests/tpmtest.py

Signed-off-by: Vadim Sukhomlinov <sukhomlinov@google.com>
Change-Id: Icb3d8a9d0f3bd0509eb72993d5835584bc14640b
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3116570
Reviewed-by: Vadim Sukhomlinov <sukhomlinov@chromium.org>
Reviewed-by: Andrey Pronin <apronin@chromium.org>
Tested-by: Vadim Sukhomlinov <sukhomlinov@chromium.org>
Auto-Submit: Vadim Sukhomlinov <sukhomlinov@chromium.org>
Commit-Queue: Vadim Sukhomlinov <sukhomlinov@chromium.org>

4 files changed, 939 insertions, 90 deletions

diff --git a/board/cr50/build.mk b/board/cr50/build.mk
index b221c4f050..7e83e05405 100644
--- a/board/cr50/build.mk
+++ b/board/cr50/build.mk
@@ -19,7 +19,8 @@ ifeq ($(BOARD_MK_INCLUDED_ONCE),)
 # List of variables which can be defined in the environment or set in the make
 # command line.
 ENV_VARS := CR50_DEV CRYPTO_TEST H1_RED_BOARD U2F_TEST RND_TEST DRBG_TEST\
-	    ECDSA_TEST DCRYPTO_TEST P256_BIN_TEST
+	    ECDSA_TEST DCRYPTO_TEST P256_BIN_TEST SHA1_TEST SHA256_TEST\
+	    HMAC_SHA256_TEST
 
 ifneq ($(CRYPTO_TEST),)
 CPPFLAGS += -DCRYPTO_TEST_SETUP
@@ -48,6 +49,18 @@ ifneq ($(P256_BIN_TEST),)
 CPPFLAGS += -DP256_BIN_TEST=1
 endif
 
+ifneq ($(SHA1_TEST),)
+CPPFLAGS += -DSHA1_TEST=1
+endif
+
+ifneq ($(SHA256_TEST),)
+CPPFLAGS += -DSHA256_TEST=1
+endif
+
+ifneq ($(HMAC_SHA256_TEST),)
+CPPFLAGS += -DHMAC_SHA256_TEST=1
+endif
+
 endif
 
 
diff --git a/board/cr50/dcrypto/dcrypto_regs.h b/board/cr50/dcrypto/dcrypto_regs.h
new file mode 100644
index 0000000000..85ea57bc95
--- /dev/null
+++ b/board/cr50/dcrypto/dcrypto_regs.h
@@ -0,0 +1,82 @@
+/* Copyright 2021 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+#ifndef __EC_FIPS_MODULE_REGS_H
+#define __EC_FIPS_MODULE_REGS_H
+
+/**
+ * This header file contains H1 crypto device register tables defined
+ * as structs. This allows more efficient code generation compared to
+ * using GREG() macro. The root cause is that with GREG compiler can't
+ * always deduce that next register address can be calculated from
+ * previous one already loaded in register by adding a small constant,
+ * thus producing inefficient code to load address first and spill
+ * registers. Access made as struct like:
+ *
+ * static volatile struct keymgr_sha *reg_keymgr_sha =
+ *      (void *)(GC_KEYMGR_BASE_ADDR + GC_KEYMGR_SHA_CFG_MSGLEN_LO_OFFSET);
+ *
+ * reg_keymgr_sha->itop = 0;
+ * reg_keymgr_sha->trig = GC_KEYMGR_SHA_TRIG_TRIG_RESET_MASK;
+ *
+ * becomes more compact and efficient.
+ */
+#include "common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include "registers.h"
+
+
+
+/**
+ * SHA/HMAC part of KEYMGR starting offset 0x400
+ */
+struct keymgr_sha {
+	uint32_t msglen_lo; /* KEYMGR_SHA_CFG_MSGLEN_LO 0x400 */
+	uint32_t msglen_hi; /* KEYMGR_SHA_CFG_MSGLEN_HI 0x404 */
+	uint32_t cfg_en; /* KEYMGR_SHA_CFG_EN 0x408 */
+	uint32_t wr_en; /* KEYMGR_SHA_CFG_WR_EN 0x40c */
+	uint32_t trig; /* KEYMGR_SHA_TRIG 0x410 */
+	const uint32_t _pad1[11];
+	union {
+		uint32_t fifo_u32; /* KEYMGR_SHA_INPUT_FIFO 0x440 */
+		uint8_t fifo_u8; /* KEYMGR_SHA_INPUT_FIFO 0x440 */
+	};
+
+	uint32_t h[8]; /* KEYMGR_SHA_STS_H0 .. H7 */
+	uint32_t key[8]; /* KEYMGR_SHA_KEY_W0 .. W7 */
+	const uint32_t sts; /* KEYMGR_SHA_STS */
+	const uint32_t itcr; /* KEYMGR_SHA_ITCR */
+	uint32_t itop; /* KEYMGR_SHA_ITOP */
+	uint32_t use_hidden_key; /* KEYMGR_SHA_USE_HIDDEN_KEY */
+	uint32_t use_cert; /* KEYMGR_SHA_USE_CERT */
+	uint32_t cert_override; /* KEYMGR_SHA_CERT_OVERRIDE */
+	uint32_t rand_stall; /* KEYMGR_SHA_RAND_STALL_CTL */
+	uint32_t count_state; /* KEYMGR_SHA_EXECUTE_COUNT_STATE */
+	uint32_t count_max; /* KEYMGR_SHA_EXECUTE_COUNT_MAX */
+	uint32_t revoke_ctrl[3]; /* KEYMGR_CERT_REVOKE_CTRL0 .. CTRL3 */
+};
+
+BUILD_ASSERT(offsetof(struct keymgr_sha, trig) ==
+	     GC_KEYMGR_SHA_TRIG_OFFSET - GC_KEYMGR_SHA_CFG_MSGLEN_LO_OFFSET);
+
+BUILD_ASSERT(offsetof(struct keymgr_sha, fifo_u32) ==
+	     GC_KEYMGR_SHA_INPUT_FIFO_OFFSET -
+		     GC_KEYMGR_SHA_CFG_MSGLEN_LO_OFFSET);
+
+BUILD_ASSERT(offsetof(struct keymgr_sha, h) ==
+	     GC_KEYMGR_SHA_STS_H0_OFFSET - GC_KEYMGR_SHA_CFG_MSGLEN_LO_OFFSET);
+
+BUILD_ASSERT(offsetof(struct keymgr_sha, rand_stall) ==
+	     GC_KEYMGR_SHA_RAND_STALL_CTL_OFFSET -
+		     GC_KEYMGR_SHA_CFG_MSGLEN_LO_OFFSET);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __EC_FIPS_MODULE_REGS_H */
+
diff --git a/board/cr50/dcrypto/internal.h b/board/cr50/dcrypto/internal.h
index ef092f6fb5..ea9056cefb 100644
--- a/board/cr50/dcrypto/internal.h
+++ b/board/cr50/dcrypto/internal.h
@@ -10,6 +10,7 @@
 
 #include "common.h"
 #include "crypto_common.h"
+
 #include "util.h"
 
 #include "hmacsha2.h"
@@ -39,11 +40,6 @@ extern "C" {
 #define CHAR_BIT 8
 #endif
 
-enum sha_mode {
-	SHA1_MODE = 0,
-	SHA256_MODE = 1
-};
-
 /*
  * Use this structure to avoid alignment problems with input and output
  * pointers.
@@ -57,6 +53,9 @@ int dcrypto_grab_sha_hw(void);
 void dcrypto_release_sha_hw(void);
 #endif
 
+/* Load data into KEYMGR SHA FIFO. */
+void dcrypto_sha_fifo_load(const void *data, size_t n);
+
 /*
  * BIGNUM.
  */
@@ -352,6 +351,12 @@ static inline uint64_t rol64(uint64_t value, int bits)
 /* Define machine word alignment mask. */
 #define WORD_MASK (sizeof(uintptr_t) - 1)
 
+/* return true if pointer is not word aligned. */
+static inline bool is_not_aligned(const void *ptr)
+{
+	return (uintptr_t)ptr & WORD_MASK;
+}
+
 /**
  * @brief Launders the machine register sized value `val`.
  *
diff --git a/board/cr50/dcrypto/sha_hw.c b/board/cr50/dcrypto/sha_hw.c
index 82f5c6dacf..7d70d71c86 100644
--- a/board/cr50/dcrypto/sha_hw.c
+++ b/board/cr50/dcrypto/sha_hw.c
@@ -5,7 +5,13 @@
 #include "dcrypto.h"
 #include "fips.h"
 #include "internal.h"
-#include "registers.h"
+#include "dcrypto_regs.h"
+
+/**
+ * Define KEYMGR SHA/HMAC access structure.
+ */
+static volatile struct keymgr_sha *reg_keymgr_sha =
+	(void *)(GC_KEYMGR_BASE_ADDR + GC_KEYMGR_SHA_CFG_MSGLEN_LO_OFFSET);
 
 #ifdef SECTION_IS_RO
 /* RO is single threaded. */
@@ -47,116 +53,197 @@ void dcrypto_release_sha_hw(void)
 
 #endif /* ! SECTION_IS_RO */
 
-static void dcrypto_sha_wait(enum sha_mode mode, uint32_t *digest)
+/* Stall frequency for SHA/HMAC engine. */
+enum keymgr_stall_freq {
+	KEYMGR_STALL_FREQ_50 = 0,
+	KEYMGR_STALL_FREQ_25 = 1,
+	KEYMGR_STALL_FREQ_12 = 2,
+	KEYMGR_STALL_FREQ_6 = 3,
+};
+
+#define KEYMGR_STALL(freq)                            \
+	(GC_KEYMGR_SHA_RAND_STALL_CTL_STALL_EN_MASK | \
+	 (freq << GC_KEYMGR_SHA_RAND_STALL_CTL_FREQ_LSB))
+
+/* Ready to use settings for SHA_RAND_STALL_CTL register. */
+enum keymgr_stall_cfg {
+	KEYMGR_STALL_DISABLED = 0,
+	KEYMGR_STALL_50 = KEYMGR_STALL(KEYMGR_STALL_FREQ_50),
+	KEYMGR_STALL_25 = KEYMGR_STALL(KEYMGR_STALL_FREQ_25),
+	KEYMGR_STALL_12 = KEYMGR_STALL(KEYMGR_STALL_FREQ_12),
+	KEYMGR_STALL_6 = KEYMGR_STALL(KEYMGR_STALL_FREQ_6)
+};
+
+/* Ready to use configuration of KEYMGR SHA engine. */
+enum sha_cfg {
+	MODE_SHA256_LIVESTREAM = (GC_KEYMGR_SHA_CFG_EN_LIVESTREAM_MASK |
+				  GC_KEYMGR_SHA_CFG_EN_INT_EN_DONE_MASK),
+
+	MODE_SHA1_LIVESTREAM = (GC_KEYMGR_SHA_CFG_EN_LIVESTREAM_MASK |
+				GC_KEYMGR_SHA_CFG_EN_INT_EN_DONE_MASK |
+				GC_KEYMGR_SHA_CFG_EN_SHA1_MASK),
+
+	MODE_SHA256_ONESHOT = (GC_KEYMGR_SHA_CFG_EN_INT_EN_DONE_MASK),
+
+	MODE_SHA1_ONESHOT = (GC_KEYMGR_SHA_CFG_EN_INT_EN_DONE_MASK |
+			     GC_KEYMGR_SHA_CFG_EN_SHA1_MASK),
+
+	MODE_HMAC_SHA256_LIVESTREAM = (GC_KEYMGR_SHA_CFG_EN_LIVESTREAM_MASK |
+				       GC_KEYMGR_SHA_CFG_EN_INT_EN_DONE_MASK |
+				       GC_KEYMGR_SHA_CFG_EN_HMAC_MASK)
+};
+
+static void dcrypto_sha_wait(uint32_t *digest, size_t digest_words)
 {
-	int i;
-	const int digest_len = (mode == SHA1_MODE) ? SHA1_DIGEST_SIZE :
-							   SHA256_DIGEST_SIZE;
-
-	/* Stop LIVESTREAM mode. */
-	GREG32(KEYMGR, SHA_TRIG) = GC_KEYMGR_SHA_TRIG_TRIG_STOP_MASK;
+	/* Stop LIVESTREAM mode (if active). */
+	reg_keymgr_sha->trig = GC_KEYMGR_SHA_TRIG_TRIG_STOP_MASK;
 
 	/* Wait for SHA DONE interrupt. */
-	while (!GREG32(KEYMGR, SHA_ITOP))
+	while (!reg_keymgr_sha->itop)
 		;
 
-	/* Read out final digest. */
-	for (i = 0; i < digest_len / 4; ++i)
-		*digest++ = GR_KEYMGR_SHA_HASH(i);
-	dcrypto_release_sha_hw();
+	/**
+	 * Read out final digest. Note, we unroll loop for
+	 * 2 cases - SHA1 and SHA256, defaulting to SHA1 to remove
+	 * extra check. This function is only called with
+	 * SHA256_DIGEST_WORDS or SHA1_DIGEST_WORDS. Such unrolling
+	 * is cheap, as each read/write instruction is just 2 bytes.
+	 */
+	switch (digest_words) {
+	case SHA256_DIGEST_WORDS:
+		digest[5] = reg_keymgr_sha->h[5];
+		digest[6] = reg_keymgr_sha->h[6];
+		digest[7] = reg_keymgr_sha->h[7];
+		/* Fall through */
+	default: /* SHA1_DIGEST_WORDS */
+		digest[0] = reg_keymgr_sha->h[0];
+		digest[1] = reg_keymgr_sha->h[1];
+		digest[2] = reg_keymgr_sha->h[2];
+		digest[3] = reg_keymgr_sha->h[3];
+		digest[4] = reg_keymgr_sha->h[4];
+	}
+
+	/* Clear interrupt status. */
+	reg_keymgr_sha->itop = 0;
+
+	/* Destroy any leftovers in SHA engine. */
+	reg_keymgr_sha->trig = GC_KEYMGR_SHA_TRIG_TRIG_RESET_MASK;
 }
 
+/* Wrapper for HW SHA/HMAC to match vtable function signature. */
 static void dcrypto_sha_update(union hash_ctx *unused, const void *data,
 			       size_t n)
 {
+	dcrypto_sha_fifo_load(data, n);
+}
+
+void dcrypto_sha_fifo_load(const void *data, size_t n)
+{
 	const uint8_t *bp = (const uint8_t *)data;
+	const uint8_t *bp_end = bp + n;
+	const uint32_t *wp_end = (uint32_t *)((uintptr_t)bp_end & ~WORD_MASK);
 	const uint32_t *wp;
 
 	/* Feed unaligned start bytes. */
-	while (n != 0 && ((uint32_t)bp & 3)) {
-		GREG8(KEYMGR, SHA_INPUT_FIFO) = *bp++;
-		n -= 1;
-	}
+	while (is_not_aligned(bp) && bp < bp_end)
+		reg_keymgr_sha->fifo_u8 = *bp++;
 
-	/* Feed groups of aligned words. */
 	wp = (uint32_t *)bp;
-	while (n >= 8 * 4) {
-		GREG32(KEYMGR, SHA_INPUT_FIFO) = *wp++;
-		GREG32(KEYMGR, SHA_INPUT_FIFO) = *wp++;
-		GREG32(KEYMGR, SHA_INPUT_FIFO) = *wp++;
-		GREG32(KEYMGR, SHA_INPUT_FIFO) = *wp++;
-		GREG32(KEYMGR, SHA_INPUT_FIFO) = *wp++;
-		GREG32(KEYMGR, SHA_INPUT_FIFO) = *wp++;
-		GREG32(KEYMGR, SHA_INPUT_FIFO) = *wp++;
-		GREG32(KEYMGR, SHA_INPUT_FIFO) = *wp++;
-		n -= 8 * 4;
+
+	/* Feed groups of aligned words. */
+	while (wp + 4 < wp_end) {
+		reg_keymgr_sha->fifo_u32 = *wp++;
+		reg_keymgr_sha->fifo_u32 = *wp++;
+		reg_keymgr_sha->fifo_u32 = *wp++;
+		reg_keymgr_sha->fifo_u32 = *wp++;
 	}
+
 	/* Feed individual aligned words. */
-	while (n >= 4) {
-		GREG32(KEYMGR, SHA_INPUT_FIFO) = *wp++;
-		n -= 4;
-	}
+	while (wp < wp_end)
+		reg_keymgr_sha->fifo_u32 = *wp++;
 
 	/* Feed remaining bytes. */
 	bp = (uint8_t *)wp;
-	while (n != 0) {
-		GREG8(KEYMGR, SHA_INPUT_FIFO) = *bp++;
-		n -= 1;
-	}
+	while (bp < bp_end)
+		reg_keymgr_sha->fifo_u8 = *bp++;
 }
 
-static void dcrypto_sha_init(enum sha_mode mode)
+static void dcrypto_load_hmac_key(uint32_t *hmac_key)
 {
-	int val;
+	volatile uint32_t *reg_key = reg_keymgr_sha->key;
 
-	/* Stop LIVESTREAM mode, in case final() was not called. */
-	GREG32(KEYMGR, SHA_TRIG) = GC_KEYMGR_SHA_TRIG_TRIG_STOP_MASK;
+	/* Set that we want key from KEY_W0..W7. */
+	reg_keymgr_sha->use_hidden_key = 0;
+	reg_keymgr_sha->use_cert = 0;
+
+	/* Only SHA2-256 is supported for HMAC. */
+	for (size_t i = 0; i < SHA256_DIGEST_WORDS; ++i)
+		*reg_key++ = *hmac_key++;
+}
+
+/**
+ * Program SHA/HMAC in specified mode and set random stalls to
+ * harden against side-channel attacks.
+ *
+ * @param mode SHA/HMAC operation mode
+ * @param stall configured random stalls
+ * @param len length of message for one-shot mode (ignored in livestream)
+ */
+static void dcrypto_sha_init(enum sha_cfg mode, enum keymgr_stall_cfg stall,
+			     size_t len)
+{
 	/* Clear interrupt status. */
-	GREG32(KEYMGR, SHA_ITOP) = 0;
-
-	/* Enable streaming mode. */
-	val = GC_KEYMGR_SHA_CFG_EN_LIVESTREAM_MASK;
-	/* Enable SHA DONE interrupt. */
-	val |= GC_KEYMGR_SHA_CFG_EN_INT_EN_DONE_MASK;
-	/* Select SHA mode. */
-	if (mode == SHA1_MODE)
-		val |= GC_KEYMGR_SHA_CFG_EN_SHA1_MASK;
-	GREG32(KEYMGR, SHA_CFG_EN) = val;
-
-	/* Turn off random nops (which are enabled by default). */
-	GWRITE_FIELD(KEYMGR, SHA_RAND_STALL_CTL, STALL_EN, 0);
-	/* Configure random nop percentage at 12%. */
-	GWRITE_FIELD(KEYMGR, SHA_RAND_STALL_CTL, FREQ, 2);
-	/* Now turn on random nops. */
-	GWRITE_FIELD(KEYMGR, SHA_RAND_STALL_CTL, STALL_EN, 1);
+	reg_keymgr_sha->itop = 0;
+
+	/* Reset logic. */
+	reg_keymgr_sha->trig = GC_KEYMGR_SHA_TRIG_TRIG_RESET_MASK;
+
+	/* Load data length, ignored in livestream mode. */
+	reg_keymgr_sha->msglen_lo = len;
+	reg_keymgr_sha->msglen_hi = 0;
+
+	/* Turn off random nops (which are enabled by default) to reprogram. */
+	reg_keymgr_sha->rand_stall = 0;
+
+	/* Configure and enable random nop percentage. */
+	reg_keymgr_sha->rand_stall = stall;
+
+	reg_keymgr_sha->cfg_en = mode;
 
 	/* Start SHA engine. */
-	GREG32(KEYMGR, SHA_TRIG) = GC_KEYMGR_SHA_TRIG_TRIG_GO_MASK;
+	reg_keymgr_sha->trig = GC_KEYMGR_SHA_TRIG_TRIG_GO_MASK;
 }
 
 static const struct sha1_digest *dcrypto_sha1_final(union hash_ctx *ctx)
 {
-	dcrypto_sha_wait(SHA1_MODE, ctx->sha1.digest.b32);
+	dcrypto_sha_wait(ctx->sha1.digest.b32, SHA1_DIGEST_WORDS);
+	dcrypto_release_sha_hw();
 	return &ctx->sha1.digest;
 }
 
 static const struct sha256_digest *dcrypto_sha256_final(union hash_ctx *ctx)
 {
-	dcrypto_sha_wait(SHA256_MODE, ctx->sha256.digest.b32);
+	dcrypto_sha_wait(ctx->sha256.digest.b32, SHA256_DIGEST_WORDS);
+	dcrypto_release_sha_hw();
 	return &ctx->sha256.digest;
 }
 
 static const union sha_digests *dcrypto_sha256_final_as_hash(
 	union hash_ctx *const ctx) __alias(dcrypto_sha256_final);
+
 static const union sha_digests *dcrypto_sha1_final_as_hash(
 	union hash_ctx *const ctx) __alias(dcrypto_sha1_final);
 
-static void dcrypto_sha_hash(enum sha_mode mode, const uint8_t *data, size_t n,
-			     uint32_t *digest)
+static const union sha_digests *dcrypto_hmac_final(union hmac_ctx *const ctx)
 {
-	dcrypto_sha_init(mode);
-	dcrypto_sha_update(NULL, data, n);
-	dcrypto_sha_wait(mode, digest);
+	size_t i;
+
+	dcrypto_sha_wait(ctx->hmac_sha256.hash.digest.b32, SHA256_DIGEST_WORDS);
+	/* Destroy loaded key after use. */
+	for (i = 0; i < SHA256_DIGEST_WORDS; ++i)
+		reg_keymgr_sha->key[i] = 0;
+	dcrypto_release_sha_hw();
+	return (union sha_digests *)&ctx->hmac_sha256.hash.digest;
 }
 
 /* Requires dcrypto_grab_sha_hw() to be called first. */
@@ -170,7 +257,7 @@ static void dcrypto_sha1_init(union hash_ctx *ctx)
 	};
 
 	ctx->f = &hw_sha1_vtab;
-	dcrypto_sha_init(SHA1_MODE);
+	dcrypto_sha_init(MODE_SHA1_LIVESTREAM, KEYMGR_STALL_6, 0);
 }
 
 static void dcrypto_sha256_init(union hash_ctx *ctx)
@@ -184,7 +271,7 @@ static void dcrypto_sha256_init(union hash_ctx *ctx)
 	};
 
 	ctx->f = &HW_SHA256_VTAB;
-	dcrypto_sha_init(SHA256_MODE);
+	dcrypto_sha_init(MODE_SHA256_LIVESTREAM, KEYMGR_STALL_6, 0);
 }
 
 /**
@@ -225,24 +312,44 @@ void SHA256_hw_init(struct sha256_ctx *ctx)
 		SHA256_sw_init(ctx);
 }
 
+/* SHA1/256/HMAC SHA256 one-shot mode primitive. */
+static void dcrypto_sha_oneshot(enum sha_cfg mode, const void *data, size_t n,
+				uint32_t *digest, size_t digest_words)
+{
+	/**
+	 * Load message length in one-shot mode. This way engine can start
+	 * processing data blocks earlier than in livestream mode.
+	 */
+	if (n != 0) { /* One-shot only works for non-empty data. */
+		dcrypto_sha_init(mode, KEYMGR_STALL_6, n);
+
+		dcrypto_sha_fifo_load(data, n);
+	} else /* Due to bug in SHA engine, force livestream for this case. */
+		dcrypto_sha_init(mode | GC_KEYMGR_SHA_CFG_EN_LIVESTREAM_MASK,
+				 KEYMGR_STALL_6, 0);
+	dcrypto_sha_wait(digest, digest_words);
+}
+
 const struct sha1_digest *SHA1_hw_hash(const void *data, size_t n,
-					struct sha1_digest *digest)
+				       struct sha1_digest *digest)
 {
-	if (dcrypto_grab_sha_hw())
-		/* dcrypto_sha_wait() will release the hw. */
-		dcrypto_sha_hash(SHA1_MODE, data, n,  digest->b32);
-	else
+	if (dcrypto_grab_sha_hw()) {
+		dcrypto_sha_oneshot(MODE_SHA1_ONESHOT, data, n, digest->b32,
+				    SHA1_DIGEST_WORDS);
+		dcrypto_release_sha_hw();
+	} else
 		SHA1_sw_hash(data, n, digest);
 	return digest;
 }
 
 const struct sha256_digest *SHA256_hw_hash(const void *data, size_t n,
-					  struct sha256_digest *digest)
+					   struct sha256_digest *digest)
 {
-	if (dcrypto_grab_sha_hw())
-		/* dcrypto_sha_wait() will release the hw. */
-		dcrypto_sha_hash(SHA256_MODE, data, n, digest->b32);
-	else
+	if (dcrypto_grab_sha_hw()) {
+		dcrypto_sha_oneshot(MODE_SHA256_ONESHOT, data, n, digest->b32,
+				    SHA256_DIGEST_WORDS);
+		dcrypto_release_sha_hw();
+	} else
 		SHA256_sw_hash(data, n, digest);
 	return digest;
 }
@@ -251,16 +358,658 @@ const struct sha256_digest *SHA256_hw_hash(const void *data, size_t n,
 const uint8_t *DCRYPTO_SHA1_hash(const void *data, size_t n, uint8_t *digest)
 	__alias(SHA1_hw_hash);
 
-/* TODO(b/195092622): initialize HW HMAC instead. */
+const struct sha256_digest *HMAC_SHA256_hw_final(struct hmac_sha256_ctx *ctx)
+{
+	return HMAC_SHA256_final(ctx);
+}
+
 void HMAC_SHA256_hw_init(struct hmac_sha256_ctx *ctx, const void *key,
-			      size_t len)
+			 size_t len)
+{
+	/* VTable for Hardware HMAC implementation. */
+	static const struct hash_vtable HW_HMAC_SHA256_VTAB = {
+		dcrypto_sha256_init,	      dcrypto_sha_update,
+		dcrypto_sha256_final_as_hash, dcrypto_hmac_final,
+		SHA256_DIGEST_SIZE,	      SHA256_BLOCK_SIZE,
+		sizeof(struct sha256_ctx)
+	};
+
+	struct sha256_digest padded_key;
+
+	/**
+	 * HW HMAC only supports key length <= 256 bits. However,
+	 * large keys are hashed back to 256 bits.
+	 */
+	if ((len > SHA256_DIGEST_SIZE) && (len <= SHA256_BLOCK_SIZE)) {
+		/* Try a hybrid path with HW SHA2 and software HMAC. */
+		SHA256_hw_init(&ctx->hash);
+		HMAC_sw_init((union hmac_ctx *)ctx, key, len);
+		return;
+	}
+
+	if (!dcrypto_grab_sha_hw()) {
+		/* Fallback to software path. */
+		SHA256_sw_init(&ctx->hash);
+		HMAC_sw_init((union hmac_ctx *)ctx, key, len);
+		return;
+	}
+
+	if (len > SHA256_BLOCK_SIZE) {
+		/* For long keys make key 256 bit. */
+		dcrypto_sha_oneshot(MODE_SHA256_ONESHOT, key, len,
+				    padded_key.b32, SHA256_DIGEST_WORDS);
+	} else {
+		/**
+		 * len <= SHA256_DIGEST_SIZE, so zero pad HMAC key
+		 * before sending to HW.
+		 */
+		memset(padded_key.b32, 0, sizeof(padded_key));
+		memcpy(padded_key.b32, key, len);
+	}
+
+	ctx->hash.f = &HW_HMAC_SHA256_VTAB;
+
+	dcrypto_load_hmac_key(padded_key.b32);
+	dcrypto_sha_init(MODE_HMAC_SHA256_LIVESTREAM, KEYMGR_STALL_25, 0);
+
+	always_memset(padded_key.b32, 0, sizeof(padded_key));
+}
+
+#ifdef CRYPTO_TEST_SETUP
+/**
+ * To use hmac command for testing, set one or many configuration variables:
+ * SHA1_TEST - SHA1 test vectors
+ * SHA256_TEST - SHA2 test vectors
+ * HMAC_SHA256_TEST - HMAC SHA256 test vectors.
+ *
+ * It will test all software and hardware implementations available.
+ */
+#include "console.h"
+#include "common.h"
+#include "endian.h"
+
+#define CPRINTS(format, args...) cprints(CC_SYSTEM, format, ##args)
+
+struct hash_test {
+	size_t len;
+	uint8_t *c;
+	uint8_t *d;
+};
+
+struct hmac_test {
+	size_t klen;
+	uint8_t *k;
+	size_t len;
+	uint8_t *c;
+	uint8_t *d;
+};
+
+union kdata32 {
+	uint8_t bytes[32];
+	uint32_t words[8];
+};
+
+#ifndef SHA1_TEST
+#define SHA1_TEST 0
+#endif
+
+#ifndef SHA256_TEST
+#define SHA256_TEST 0
+#endif
+
+#ifndef HMAC_SHA256_TEST
+#define HMAC_SHA256_TEST 0
+#endif
+
+#if SHA1_TEST || SHA256_TEST || HMAC_SHA256_TEST
+static const char *smemcmp(const void *a, const void *b, size_t len)
 {
-	SHA256_hw_init(&ctx->hash);
-	HMAC_sw_init((union hmac_ctx *)ctx, key, len);
+	return (memcmp(a, b, len) == 0) ? "passed" : "NOT passed!";
 }
+#endif
 
-const struct sha256_digest *
-HMAC_SHA256_hw_final(struct hmac_sha256_ctx *ctx)
+#if SHA1_TEST
+static int cmd_sha1_test(int argc, char *argv[])
 {
-	return HMAC_SHA256_final(ctx);
+	/**
+	 * Test vectors for SHA1, SHA256 from
+	 * https://csrc.nist.gov/projects/cryptographic-algorithm-validation-program/secure-hashing
+	 */
+	const struct hash_test sha1_v[] = {
+		{ .len = 0,
+		  .c = (uint8_t[]){},
+		  .d = (uint8_t[]){ 0xda, 0x39, 0xa3, 0xee, 0x5e, 0x6b, 0x4b,
+				    0x0d, 0x32, 0x55, 0xbf, 0xef, 0x95, 0x60,
+				    0x18, 0x90, 0xaf, 0xd8, 0x07, 0x09 } },
+		{ .len = 1,
+		  .c = (uint8_t[]){ 0x36 },
+		  .d = (uint8_t[]){ 0xc1, 0xdf, 0xd9, 0x6e, 0xea, 0x8c, 0xc2,
+				    0xb6, 0x27, 0x85, 0x27, 0x5b, 0xca, 0x38,
+				    0xac, 0x26, 0x12, 0x56, 0xe2, 0x78 } },
+		{ .len = 12,
+		  .c = (uint8_t[]){ 0x09, 0x38, 0xf2, 0xe2, 0xeb, 0xb6, 0x4f,
+				    0x8a, 0xf8, 0xbb, 0xfc, 0x91 },
+		  .d = (uint8_t[]){ 0x9f, 0x4e, 0x66, 0xb6, 0xce, 0xea, 0x40,
+				    0xdc, 0xf4, 0xb9, 0x16, 0x6c, 0x28, 0xf1,
+				    0xc8, 0x84, 0x74, 0x14, 0x1d, 0xa9 } },
+		{ .len = 20,
+		  .c = (uint8_t[]){ 0x63, 0xa3, 0xcc, 0x83, 0xfd, 0x1e, 0xc1,
+				    0xb6, 0x68, 0x0e, 0x99, 0x74, 0xa0, 0x51,
+				    0x4e, 0x1a, 0x9e, 0xce, 0xbb, 0x6a },
+		  .d = (uint8_t[]){ 0x8b, 0xb8, 0xc0, 0xd8, 0x15, 0xa9, 0xc6,
+				    0x8a, 0x1d, 0x29, 0x10, 0xf3, 0x9d, 0x94,
+				    0x26, 0x03, 0xd8, 0x07, 0xfb, 0xcc } },
+		{ .len = 32,
+		  .c = (uint8_t[]){ 0x03, 0x21, 0x79, 0x4b, 0x73, 0x94, 0x18,
+				    0xc2, 0x4e, 0x7c, 0x2e, 0x56, 0x52, 0x74,
+				    0x79, 0x1c, 0x4b, 0xe7, 0x49, 0x75, 0x2a,
+				    0xd2, 0x34, 0xed, 0x56, 0xcb, 0x0a, 0x63,
+				    0x47, 0x43, 0x0c, 0x6b },
+		  .d = (uint8_t[]){ 0xb8, 0x99, 0x62, 0xc9, 0x4d, 0x60, 0xf6,
+				    0xa3, 0x32, 0xfd, 0x60, 0xf6, 0xf0, 0x7d,
+				    0x4f, 0x03, 0x2a, 0x58, 0x6b, 0x76 } },
+		{ .len = 48,
+		  .c = (uint8_t[]){ 0x57, 0xe8, 0x96, 0x59, 0xd8, 0x78, 0xf3,
+				    0x60, 0xaf, 0x6d, 0xe4, 0x5a, 0x9a, 0x5e,
+				    0x37, 0x2e, 0xf4, 0x0c, 0x38, 0x49, 0x88,
+				    0xe8, 0x26, 0x40, 0xa3, 0xd5, 0xe4, 0xb7,
+				    0x6d, 0x2e, 0xf1, 0x81, 0x78, 0x0b, 0x9a,
+				    0x09, 0x9a, 0xc0, 0x6e, 0xf0, 0xf8, 0xa7,
+				    0xf3, 0xf7, 0x64, 0x20, 0x97, 0x20 },
+		  .d = (uint8_t[]){ 0xf6, 0x52, 0xf3, 0xb1, 0x54, 0x9f, 0x16,
+				    0x71, 0x0c, 0x74, 0x02, 0x89, 0x59, 0x11,
+				    0xe2, 0xb8, 0x6a, 0x9b, 0x2a, 0xee } },
+		{ .len = 55,
+		  .c = (uint8_t[]){ 0xec, 0x6b, 0x4a, 0x88, 0x71, 0x3d, 0xf2,
+				    0x7c, 0x0f, 0x2d, 0x02, 0xe7, 0x38, 0xb6,
+				    0x9d, 0xb4, 0x3a, 0xbd, 0xa3, 0x92, 0x13,
+				    0x17, 0x25, 0x9c, 0x86, 0x4c, 0x1c, 0x38,
+				    0x6e, 0x9a, 0x5a, 0x3f, 0x53, 0x3d, 0xc0,
+				    0x5f, 0x3b, 0xee, 0xb2, 0xbe, 0xc2, 0xaa,
+				    0xc8, 0xe0, 0x6d, 0xb4, 0xc6, 0xcb, 0x3c,
+				    0xdd, 0xcf, 0x69, 0x7e, 0x03, 0xd5 },
+		  .d = (uint8_t[]){ 0xa7, 0x27, 0x2e, 0x23, 0x08, 0x62, 0x2f,
+				    0xf7, 0xa3, 0x39, 0x46, 0x0a, 0xdc, 0x61,
+				    0xef, 0xd0, 0xea, 0x8d, 0xab, 0xdc } },
+		{ .len = 56,
+		  .c = (uint8_t[]){ 0x03, 0x21, 0x73, 0x6b, 0xeb, 0xa5, 0x78,
+				    0xe9, 0x0a, 0xbc, 0x1a, 0x90, 0xaa, 0x56,
+				    0x15, 0x7d, 0x87, 0x16, 0x18, 0xf6, 0xde,
+				    0x0d, 0x76, 0x4c, 0xc8, 0xc9, 0x1e, 0x06,
+				    0xc6, 0x8e, 0xcd, 0x3b, 0x9d, 0xe3, 0x82,
+				    0x40, 0x64, 0x50, 0x33, 0x84, 0xdb, 0x67,
+				    0xbe, 0xb7, 0xfe, 0x01, 0x22, 0x32, 0xda,
+				    0xca, 0xef, 0x93, 0xa0, 0x00, 0xfb, 0xa7 },
+		  .d = (uint8_t[]){ 0xae, 0xf8, 0x43, 0xb8, 0x69, 0x16, 0xc1,
+				    0x6f, 0x66, 0xc8, 0x4d, 0x83, 0xa6, 0x00,
+				    0x5d, 0x23, 0xfd, 0x00, 0x5c, 0x9e } },
+		{ .len = 163,
+		  .c = (uint8_t[]){ 0x7c, 0x9c, 0x67, 0x32, 0x3a, 0x1d, 0xf1,
+				    0xad, 0xbf, 0xe5, 0xce, 0xb4, 0x15, 0xea,
+				    0xef, 0x01, 0x55, 0xec, 0xe2, 0x82, 0x0f,
+				    0x4d, 0x50, 0xc1, 0xec, 0x22, 0xcb, 0xa4,
+				    0x92, 0x8a, 0xc6, 0x56, 0xc8, 0x3f, 0xe5,
+				    0x85, 0xdb, 0x6a, 0x78, 0xce, 0x40, 0xbc,
+				    0x42, 0x75, 0x7a, 0xba, 0x7e, 0x5a, 0x3f,
+				    0x58, 0x24, 0x28, 0xd6, 0xca, 0x68, 0xd0,
+				    0xc3, 0x97, 0x83, 0x36, 0xa6, 0xef, 0xb7,
+				    0x29, 0x61, 0x3e, 0x8d, 0x99, 0x79, 0x01,
+				    0x62, 0x04, 0xbf, 0xd9, 0x21, 0x32, 0x2f,
+				    0xdd, 0x52, 0x22, 0x18, 0x35, 0x54, 0x44,
+				    0x7d, 0xe5, 0xe6, 0xe9, 0xbb, 0xe6, 0xed,
+				    0xf7, 0x6d, 0x7b, 0x71, 0xe1, 0x8d, 0xc2,
+				    0xe8, 0xd6, 0xdc, 0x89, 0xb7, 0x39, 0x83,
+				    0x64, 0xf6, 0x52, 0xfa, 0xfc, 0x73, 0x43,
+				    0x29, 0xaa, 0xfa, 0x3d, 0xcd, 0x45, 0xd4,
+				    0xf3, 0x1e, 0x38, 0x8e, 0x4f, 0xaf, 0xd7,
+				    0xfc, 0x64, 0x95, 0xf3, 0x7c, 0xa5, 0xcb,
+				    0xab, 0x7f, 0x54, 0xd5, 0x86, 0x46, 0x3d,
+				    0xa4, 0xbf, 0xea, 0xa3, 0xba, 0xe0, 0x9f,
+				    0x7b, 0x8e, 0x92, 0x39, 0xd8, 0x32, 0xb4,
+				    0xf0, 0xa7, 0x33, 0xaa, 0x60, 0x9c, 0xc1,
+				    0xf8, 0xd4 },
+		  .d = (uint8_t[]){ 0xd8, 0xfd, 0x6a, 0x91, 0xef, 0x3b, 0x6c,
+				    0xed, 0x05, 0xb9, 0x83, 0x58, 0xa9, 0x91,
+				    0x07, 0xc1, 0xfa, 0xc8, 0xc8, 0x07 } }
+	};
+	for (size_t i = 0; i < ARRAY_SIZE(sha1_v); i++) {
+		struct sha1_digest sha1;
+		struct sha1_ctx ctx;
+
+		memset(&sha1, 0, sizeof(sha1));
+		SHA1_sw_hash(sha1_v[i].c, sha1_v[i].len, &sha1);
+
+		CPRINTS("SHA1_sw_hash test %zu, len=%zu, %s", i, sha1_v[i].len,
+			smemcmp(sha1.b8, sha1_v[i].d, SHA1_DIGEST_SIZE));
+
+		memset(&sha1, 0, sizeof(sha1));
+		SHA1_hw_hash(sha1_v[i].c, sha1_v[i].len, &sha1);
+		CPRINTS("SHA1_hw_hash test %zu, len=%zu, %s", i, sha1_v[i].len,
+			smemcmp(sha1.b8, sha1_v[i].d, SHA1_DIGEST_SIZE));
+
+		SHA1_hw_init(&ctx);
+		SHA1_update(&ctx, sha1_v[i].c, sha1_v[i].len);
+		CPRINTS("SHA1_hw_init test %zu, len=%zu, %s", i, sha1_v[i].len,
+			smemcmp(SHA1_final(&ctx)->b8, sha1_v[i].d,
+				SHA1_DIGEST_SIZE));
+		cflush();
+	}
+	return EC_SUCCESS;
+}
+DECLARE_SAFE_CONSOLE_COMMAND(sha1_test, cmd_sha1_test, NULL, NULL);
+#endif
+
+#if SHA256_TEST
+static int cmd_sha256_test(int argc, char *argv[])
+{
+	/**
+	 * Test vectors for SHA1, SHA256 from
+	 * https://csrc.nist.gov/projects/cryptographic-algorithm-validation-program/secure-hashing
+	 */
+
+	const struct hash_test sha256_v[] = {
+		{ .len = 0,
+		  .c = (uint8_t[]){},
+		  .d = (uint8_t[]){ 0xe3, 0xb0, 0xc4, 0x42, 0x98, 0xfc, 0x1c,
+				    0x14, 0x9a, 0xfb, 0xf4, 0xc8, 0x99, 0x6f,
+				    0xb9, 0x24, 0x27, 0xae, 0x41, 0xe4, 0x64,
+				    0x9b, 0x93, 0x4c, 0xa4, 0x95, 0x99, 0x1b,
+				    0x78, 0x52, 0xb8, 0x55 } },
+		{ .len = 1,
+		  .c = (uint8_t[]){ 0xd3 },
+		  .d = (uint8_t[]){ 0x28, 0x96, 0x9c, 0xdf, 0xa7, 0x4a, 0x12,
+				    0xc8, 0x2f, 0x3b, 0xad, 0x96, 0x0b, 0x0b,
+				    0x00, 0x0a, 0xca, 0x2a, 0xc3, 0x29, 0xde,
+				    0xea, 0x5c, 0x23, 0x28, 0xeb, 0xc6, 0xf2,
+				    0xba, 0x98, 0x02, 0xc1 } },
+		{ .len = 5,
+		  .c = (uint8_t[]){ 0xc2, 0x99, 0x20, 0x96, 0x82 },
+		  .d = (uint8_t[]){ 0xf0, 0x88, 0x7f, 0xe9, 0x61, 0xc9, 0xcd,
+				    0x3b, 0xea, 0xb9, 0x57, 0xe8, 0x22, 0x24,
+				    0x94, 0xab, 0xb9, 0x69, 0xb1, 0xce, 0x4c,
+				    0x65, 0x57, 0x97, 0x6d, 0xf8, 0xb0, 0xf6,
+				    0xd2, 0x0e, 0x91, 0x66 } },
+		{ .len = 10,
+		  .c = (uint8_t[]){ 0x65, 0x74, 0x61, 0x6f, 0x6e, 0x72, 0x69,
+				    0x73, 0x68, 0x64 },
+		  .d = (uint8_t[]){ 0xf5, 0x53, 0xcd, 0xb8, 0xcf, 0x1,	0xee,
+				    0x17, 0x9b, 0x93, 0xc9, 0x68, 0xc0, 0xea,
+				    0x40, 0x91, 0x6,  0xec, 0x8e, 0x11, 0x96,
+				    0xc8, 0x5d, 0x1c, 0xaf, 0x64, 0x22, 0xe6,
+				    0x50, 0x4f, 0x47, 0x57 } },
+		{ .len = 12,
+		  .c = (uint8_t[]){ 0x9b, 0xaf, 0x69, 0xcb, 0xa3, 0x17, 0xf4,
+				    0x22, 0xfe, 0x26, 0xa9, 0xa0 },
+		  .d = (uint8_t[]){ 0xfe, 0x56, 0x28, 0x7c, 0xd6, 0x57, 0xe4,
+				    0xaf, 0xc5, 0x0d, 0xba, 0x7a, 0x3a, 0x54,
+				    0xc2, 0xa6, 0x32, 0x4b, 0x88, 0x6b, 0xec,
+				    0xdc, 0xd1, 0xfa, 0xe4, 0x73, 0xb7, 0x69,
+				    0xe5, 0x51, 0xa0, 0x9b } },
+		{ .len = 20,
+		  .c = (uint8_t[]){ 0xc1, 0xef, 0x39, 0xce, 0xe5, 0x8e, 0x78,
+				    0xf6, 0xfc, 0xdc, 0x12, 0xe0, 0x58, 0xb7,
+				    0xf9, 0x02, 0xac, 0xd1, 0xa9, 0x3b },
+		  .d = (uint8_t[]){ 0x6d, 0xd5, 0x2b, 0x0d, 0x8b, 0x48, 0xcc,
+				    0x81, 0x46, 0xce, 0xbd, 0x02, 0x16, 0xfb,
+				    0xf5, 0xf6, 0xef, 0x7e, 0xea, 0xfc, 0x0f,
+				    0xf2, 0xff, 0x9d, 0x14, 0x22, 0xd6, 0x34,
+				    0x55, 0x55, 0xa1, 0x42 } },
+		{ .len = 28,
+		  .c = (uint8_t[]){ 0x07, 0x77, 0xfc, 0x1e, 0x1c, 0xa4, 0x73,
+				    0x04, 0xc2, 0xe2, 0x65, 0x69, 0x28, 0x38,
+				    0x10, 0x9e, 0x26, 0xaa, 0xb9, 0xe5, 0xc4,
+				    0xae, 0x4e, 0x86, 0x00, 0xdf, 0x4b, 0x1f },
+		  .d = (uint8_t[]){ 0xff, 0xb4, 0xfc, 0x03, 0xe0, 0x54, 0xf8,
+				    0xec, 0xbc, 0x31, 0x47, 0x0f, 0xc0, 0x23,
+				    0xbe, 0xdc, 0xd4, 0xa4, 0x06, 0xb9, 0xdd,
+				    0x56, 0xc7, 0x1d, 0xa1, 0xb6, 0x60, 0xdc,
+				    0xc4, 0x84, 0x2c, 0x65 } },
+		{ .len = 32,
+		  .c = (uint8_t[]){ 0x09, 0xfc, 0x1a, 0xcc, 0xc2, 0x30, 0xa2,
+				    0x05, 0xe4, 0xa2, 0x08, 0xe6, 0x4a, 0x8f,
+				    0x20, 0x42, 0x91, 0xf5, 0x81, 0xa1, 0x27,
+				    0x56, 0x39, 0x2d, 0xa4, 0xb8, 0xc0, 0xcf,
+				    0x5e, 0xf0, 0x2b, 0x95 },
+		  .d = (uint8_t[]){ 0x4f, 0x44, 0xc1, 0xc7, 0xfb, 0xeb, 0xb6,
+				    0xf9, 0x60, 0x18, 0x29, 0xf3, 0x89, 0x7b,
+				    0xfd, 0x65, 0x0c, 0x56, 0xfa, 0x07, 0x84,
+				    0x4b, 0xe7, 0x64, 0x89, 0x07, 0x63, 0x56,
+				    0xac, 0x18, 0x86, 0xa4 } },
+		{ .len = 48,
+		  .c = (uint8_t[]){ 0x4e, 0xef, 0x51, 0x07, 0x45, 0x9b, 0xdd,
+				    0xf8, 0xf2, 0x4f, 0xc7, 0x65, 0x6f, 0xd4,
+				    0x89, 0x6d, 0xa8, 0x71, 0x1d, 0xb5, 0x04,
+				    0x00, 0xc0, 0x16, 0x48, 0x47, 0xf6, 0x92,
+				    0xb8, 0x86, 0xce, 0x8d, 0x7f, 0x4d, 0x67,
+				    0x39, 0x50, 0x90, 0xb3, 0x53, 0x4e, 0xfd,
+				    0x7b, 0x0d, 0x29, 0x8d, 0xa3, 0x4b },
+		  .d = (uint8_t[]){ 0x7c, 0x5d, 0x14, 0xed, 0x83, 0xda, 0xb8,
+				    0x75, 0xac, 0x25, 0xce, 0x7f, 0xee, 0xd6,
+				    0xef, 0x83, 0x7d, 0x58, 0xe7, 0x9d, 0xc6,
+				    0x01, 0xfb, 0x3c, 0x1f, 0xca, 0x48, 0xd4,
+				    0x46, 0x4e, 0x8b, 0x83 } },
+		{ .len = 55,
+		  .c = (uint8_t[]){ 0x3e, 0xbf, 0xb0, 0x6d, 0xb8, 0xc3, 0x8d,
+				    0x5b, 0xa0, 0x37, 0xf1, 0x36, 0x3e, 0x11,
+				    0x85, 0x50, 0xaa, 0xd9, 0x46, 0x06, 0xe2,
+				    0x68, 0x35, 0xa0, 0x1a, 0xf0, 0x50, 0x78,
+				    0x53, 0x3c, 0xc2, 0x5f, 0x2f, 0x39, 0x57,
+				    0x3c, 0x04, 0xb6, 0x32, 0xf6, 0x2f, 0x68,
+				    0xc2, 0x94, 0xab, 0x31, 0xf2, 0xa3, 0xe2,
+				    0xa1, 0xa0, 0xd8, 0xc2, 0xbe, 0x51 },
+		  .d = (uint8_t[]){ 0x65, 0x95, 0xa2, 0xef, 0x53, 0x7a, 0x69,
+				    0xba, 0x85, 0x83, 0xdf, 0xbf, 0x7f, 0x5b,
+				    0xec, 0x0a, 0xb1, 0xf9, 0x3c, 0xe4, 0xc8,
+				    0xee, 0x19, 0x16, 0xef, 0xf4, 0x4a, 0x93,
+				    0xaf, 0x57, 0x49, 0xc4 } },
+		{ .len = 56,
+		  .c = (uint8_t[]){ 0x2d, 0x52, 0x44, 0x7d, 0x12, 0x44, 0xd2,
+				    0xeb, 0xc2, 0x86, 0x50, 0xe7, 0xb0, 0x56,
+				    0x54, 0xba, 0xd3, 0x5b, 0x3a, 0x68, 0xee,
+				    0xdc, 0x7f, 0x85, 0x15, 0x30, 0x6b, 0x49,
+				    0x6d, 0x75, 0xf3, 0xe7, 0x33, 0x85, 0xdd,
+				    0x1b, 0x00, 0x26, 0x25, 0x02, 0x4b, 0x81,
+				    0xa0, 0x2f, 0x2f, 0xd6, 0xdf, 0xfb, 0x6e,
+				    0x6d, 0x56, 0x1c, 0xb7, 0xd0, 0xbd, 0x7a },
+		  .d = (uint8_t[]){ 0xcf, 0xb8, 0x8d, 0x6f, 0xaf, 0x2d, 0xe3,
+				    0xa6, 0x9d, 0x36, 0x19, 0x5a, 0xce, 0xc2,
+				    0xe2, 0x55, 0xe2, 0xaf, 0x2b, 0x7d, 0x93,
+				    0x39, 0x97, 0xf3, 0x48, 0xe0, 0x9f, 0x6c,
+				    0xe5, 0x75, 0x83, 0x60 } },
+		{ .len = 63,
+		  .c = (uint8_t[]){ 0xe2, 0xf7, 0x6e, 0x97, 0x60, 0x6a, 0x87,
+				    0x2e, 0x31, 0x74, 0x39, 0xf1, 0xa0, 0x3f,
+				    0xcd, 0x92, 0xe6, 0x32, 0xe5, 0xbd, 0x4e,
+				    0x7c, 0xbc, 0x4e, 0x97, 0xf1, 0xaf, 0xc1,
+				    0x9a, 0x16, 0xfd, 0xe9, 0x2d, 0x77, 0xcb,
+				    0xe5, 0x46, 0x41, 0x6b, 0x51, 0x64, 0x0c,
+				    0xdd, 0xb9, 0x2a, 0xf9, 0x96, 0x53, 0x4d,
+				    0xfd, 0x81, 0xed, 0xb1, 0x7c, 0x44, 0x24,
+				    0xcf, 0x1a, 0xc4, 0xd7, 0x5a, 0xce, 0xeb },
+		  .d = (uint8_t[]){ 0x18, 0x04, 0x1b, 0xd4, 0x66, 0x50, 0x83,
+				    0x00, 0x1f, 0xba, 0x8c, 0x54, 0x11, 0xd2,
+				    0xd7, 0x48, 0xe8, 0xab, 0xbf, 0xdc, 0xdf,
+				    0xd9, 0x21, 0x8c, 0xb0, 0x2b, 0x68, 0xa7,
+				    0x8e, 0x7d, 0x4c, 0x23 } },
+		{ .len = 70,
+		  .c = (uint8_t[]){ 0xbc, 0xe5, 0x0c, 0xdf, 0xff, 0x84, 0x38,
+				    0x85, 0xd4, 0xf3, 0x64, 0xd6, 0x9f, 0x93,
+				    0xbf, 0x58, 0xa2, 0x32, 0x2c, 0x70, 0x7b,
+				    0x82, 0xe8, 0x78, 0xee, 0xc9, 0x6d, 0x11,
+				    0xe5, 0xdb, 0x97, 0xbb, 0xb5, 0x46, 0x06,
+				    0xa3, 0xa3, 0xcc, 0xc3, 0xbb, 0xa7, 0x16,
+				    0x26, 0x10, 0x70, 0xa6, 0xf7, 0x59, 0xa7,
+				    0x0e, 0xd3, 0xcb, 0x78, 0x5f, 0xd1, 0x35,
+				    0x4f, 0xe5, 0x66, 0x48, 0xdf, 0x11, 0x86,
+				    0x36, 0x69, 0xb7, 0x0c, 0x80, 0x3b, 0x7a },
+		  .d = (uint8_t[]){ 0x8c, 0xb8, 0x7d, 0x8a, 0x8e, 0x48, 0x10,
+				    0xcf, 0xa6, 0x07, 0x3f, 0xb2, 0x00, 0xde,
+				    0xe5, 0x0e, 0xf1, 0xab, 0x90, 0x08, 0xfc,
+				    0x9a, 0x18, 0x84, 0x25, 0x35, 0x0f, 0x00,
+				    0x62, 0xd6, 0xd5, 0xf2 } },
+
+		{ .len = 163,
+		  .c = (uint8_t[]){ 0x45, 0x11, 0x01, 0x25, 0x0e, 0xc6, 0xf2,
+				    0x66, 0x52, 0x24, 0x9d, 0x59, 0xdc, 0x97,
+				    0x4b, 0x73, 0x61, 0xd5, 0x71, 0xa8, 0x10,
+				    0x1c, 0xdf, 0xd3, 0x6a, 0xba, 0x3b, 0x58,
+				    0x54, 0xd3, 0xae, 0x08, 0x6b, 0x5f, 0xdd,
+				    0x45, 0x97, 0x72, 0x1b, 0x66, 0xe3, 0xc0,
+				    0xdc, 0x5d, 0x8c, 0x60, 0x6d, 0x96, 0x57,
+				    0xd0, 0xe3, 0x23, 0x28, 0x3a, 0x52, 0x17,
+				    0xd1, 0xf5, 0x3f, 0x2f, 0x28, 0x4f, 0x57,
+				    0xb8, 0x5c, 0x8a, 0x61, 0xac, 0x89, 0x24,
+				    0x71, 0x1f, 0x89, 0x5c, 0x5e, 0xd9, 0x0e,
+				    0xf1, 0x77, 0x45, 0xed, 0x2d, 0x72, 0x8a,
+				    0xbd, 0x22, 0xa5, 0xf7, 0xa1, 0x34, 0x79,
+				    0xa4, 0x62, 0xd7, 0x1b, 0x56, 0xc1, 0x9a,
+				    0x74, 0xa4, 0x0b, 0x65, 0x5c, 0x58, 0xed,
+				    0xfe, 0x0a, 0x18, 0x8a, 0xd2, 0xcf, 0x46,
+				    0xcb, 0xf3, 0x05, 0x24, 0xf6, 0x5d, 0x42,
+				    0x3c, 0x83, 0x7d, 0xd1, 0xff, 0x2b, 0xf4,
+				    0x62, 0xac, 0x41, 0x98, 0x00, 0x73, 0x45,
+				    0xbb, 0x44, 0xdb, 0xb7, 0xb1, 0xc8, 0x61,
+				    0x29, 0x8c, 0xdf, 0x61, 0x98, 0x2a, 0x83,
+				    0x3a, 0xfc, 0x72, 0x8f, 0xae, 0x1e, 0xda,
+				    0x2f, 0x87, 0xaa, 0x2c, 0x94, 0x80, 0x85,
+				    0x8b, 0xec },
+		  .d = (uint8_t[]){ 0x3c, 0x59, 0x3a, 0xa5, 0x39, 0xfd, 0xcd,
+				    0xae, 0x51, 0x6c, 0xdf, 0x2f, 0x15, 0x00,
+				    0x0f, 0x66, 0x34, 0x18, 0x5c, 0x88, 0xf5,
+				    0x05, 0xb3, 0x97, 0x75, 0xfb, 0x9a, 0xb1,
+				    0x37, 0xa1, 0x0a, 0xa2 } }
+	};
+	for (size_t i = 0; i < ARRAY_SIZE(sha256_v); i++) {
+		struct sha256_digest sha256;
+		struct sha256_ctx ctx;
+
+		memset(&sha256, 0, sizeof(sha256));
+		SHA256_sw_hash(sha256_v[i].c, sha256_v[i].len, &sha256);
+
+		CPRINTS("SHA256_sw_hash test %zu, len=%zu, %s", i,
+			sha256_v[i].len,
+			smemcmp(sha256.b8, sha256_v[i].d, SHA256_DIGEST_SIZE));
+
+		memset(&sha256, 0, sizeof(sha256));
+		SHA256_hw_hash(sha256_v[i].c, sha256_v[i].len, &sha256);
+		CPRINTS("SHA256_hw_hash test %zu, len=%zu, %s", i,
+			sha256_v[i].len,
+			smemcmp(sha256.b8, sha256_v[i].d, SHA256_DIGEST_SIZE));
+
+		SHA256_hw_init(&ctx);
+		SHA256_update(&ctx, sha256_v[i].c, sha256_v[i].len);
+		CPRINTS("SHA256_hw_init test %zu, len=%zu, %s", i,
+			sha256_v[i].len,
+			smemcmp(SHA256_final(&ctx)->b8, sha256_v[i].d,
+				SHA256_DIGEST_SIZE));
+
+		/* Test the path when sha hw is taken. */
+		dcrypto_grab_sha_hw();
+		memset(&sha256, 0, sizeof(sha256));
+		SHA256_hw_hash(sha256_v[i].c, sha256_v[i].len, &sha256);
+		CPRINTS("SHA256_hw_hash test (sw path) %zu, len=%zu, %s", i,
+			sha256_v[i].len,
+			smemcmp(sha256.b8, sha256_v[i].d, SHA256_DIGEST_SIZE));
+		dcrypto_release_sha_hw();
+		cflush();
+	}
+	return EC_SUCCESS;
+}
+DECLARE_SAFE_CONSOLE_COMMAND(sha256_test, cmd_sha256_test, NULL, NULL);
+#endif
+
+#if HMAC_SHA256_TEST
+static int cmd_hmac_sha256(int argc, char *argv[])
+{
+	const struct hmac_test hmac_sha256_v[] = {
+		{ .klen = 10,
+		  .k = (uint8_t[]){ 0x65, 0x74, 0x61, 0x6f, 0x6e, 0x72, 0x69,
+				    0x73, 0x68, 0x64 },
+		  .len = 11,
+		  .c = (uint8_t[]){ 0x53, 0x61, 0x6d, 0x70, 0x6c, 0x65, 0x20,
+				    0x74, 0x65, 0x78, 0x74 },
+		  .d = (uint8_t[]){ 0xe9, 0x17, 0xc1, 0x7b, 0x4c, 0x6b, 0x77,
+				    0xda, 0xd2, 0x30, 0x36, 0x02, 0xf5, 0x72,
+				    0x33, 0x87, 0x9f, 0xc6, 0x6e, 0x7b, 0x7e,
+				    0xa8, 0xea, 0xaa, 0x9f, 0xba, 0xee, 0x51,
+				    0xff, 0xda, 0x24, 0xf4 } },
+		{ .klen = 40,
+		  .k = (uint8_t[]){ 0x97, 0x79, 0xd9, 0x12, 0x06, 0x42, 0x79,
+				    0x7f, 0x17, 0x47, 0x02, 0x5d, 0x5b, 0x22,
+				    0xb7, 0xac, 0x60, 0x7c, 0xab, 0x08, 0xe1,
+				    0x75, 0x8f, 0x2f, 0x3a, 0x46, 0xc8, 0xbe,
+				    0x1e, 0x25, 0xc5, 0x3b, 0x8c, 0x6a, 0x8f,
+				    0x58, 0xff, 0xef, 0xa1, 0x76 },
+		  .len = 128,
+		  .c = (uint8_t[]){ 0xb1, 0x68, 0x9c, 0x25, 0x91, 0xea, 0xf3,
+				    0xc9, 0xe6, 0x60, 0x70, 0xf8, 0xa7, 0x79,
+				    0x54, 0xff, 0xb8, 0x17, 0x49, 0xf1, 0xb0,
+				    0x03, 0x46, 0xf9, 0xdf, 0xe0, 0xb2, 0xee,
+				    0x90, 0x5d, 0xcc, 0x28, 0x8b, 0xaf, 0x4a,
+				    0x92, 0xde, 0x3f, 0x40, 0x01, 0xdd, 0x9f,
+				    0x44, 0xc4, 0x68, 0xc3, 0xd0, 0x7d, 0x6c,
+				    0x6e, 0xe8, 0x2f, 0xac, 0xea, 0xfc, 0x97,
+				    0xc2, 0xfc, 0x0f, 0xc0, 0x60, 0x17, 0x19,
+				    0xd2, 0xdc, 0xd0, 0xaa, 0x2a, 0xec, 0x92,
+				    0xd1, 0xb0, 0xae, 0x93, 0x3c, 0x65, 0xeb,
+				    0x06, 0xa0, 0x3c, 0x9c, 0x93, 0x5c, 0x2b,
+				    0xad, 0x04, 0x59, 0x81, 0x02, 0x41, 0x34,
+				    0x7a, 0xb8, 0x7e, 0x9f, 0x11, 0xad, 0xb3,
+				    0x04, 0x15, 0x42, 0x4c, 0x6c, 0x7f, 0x5f,
+				    0x22, 0xa0, 0x03, 0xb8, 0xab, 0x8d, 0xe5,
+				    0x4f, 0x6d, 0xed, 0x0e, 0x3a, 0xb9, 0x24,
+				    0x5f, 0xa7, 0x95, 0x68, 0x45, 0x1d, 0xfa,
+				    0x25, 0x8e },
+		  .d = (uint8_t[]){ 0x76, 0x9f, 0x00, 0xd3, 0xe6, 0xa6, 0xcc,
+				    0x1f, 0xb4, 0x26, 0xa1, 0x4a, 0x4f, 0x76,
+				    0xc6, 0x46, 0x2e, 0x61, 0x49, 0x72, 0x6e,
+				    0x0d, 0xee, 0x0e, 0xc0, 0xcf, 0x97, 0xa1,
+				    0x66, 0x05, 0xac, 0x8b } },
+		{ .klen = 45,
+		  .k = (uint8_t[]){ 0xb7, 0x63, 0x26, 0x3d, 0xc4, 0xfc, 0x62,
+				    0xb2, 0x27, 0xcd, 0x3f, 0x6b, 0x4e, 0x9e,
+				    0x35, 0x8c, 0x21, 0xca, 0x03, 0x6c, 0xe3,
+				    0x96, 0xab, 0x92, 0x59, 0xc1, 0xbe, 0xdd,
+				    0x2f, 0x5c, 0xd9, 0x02, 0x97, 0xdc, 0x70,
+				    0x3c, 0x33, 0x6e, 0xca, 0x3e, 0x35, 0x8a,
+				    0x4d, 0x6d, 0xc5 },
+		  .len = 128,
+		  .c = (uint8_t[]){ 0x53, 0xcb, 0x09, 0xd0, 0xa7, 0x88, 0xe4,
+				    0x46, 0x6d, 0x01, 0x58, 0x8d, 0xf6, 0x94,
+				    0x5d, 0x87, 0x28, 0xd9, 0x36, 0x3f, 0x76,
+				    0xcd, 0x01, 0x2a, 0x10, 0x30, 0x8d, 0xad,
+				    0x56, 0x2b, 0x6b, 0xe0, 0x93, 0x36, 0x48,
+				    0x92, 0xe8, 0x39, 0x7a, 0x8d, 0x86, 0xf1,
+				    0xd8, 0x1a, 0x20, 0x96, 0xcf, 0xc8, 0xa1,
+				    0xbb, 0xb2, 0x6a, 0x1a, 0x75, 0x52, 0x5f,
+				    0xfe, 0xbf, 0xcf, 0x16, 0x91, 0x1d, 0xad,
+				    0xd0, 0x9e, 0x80, 0x2a, 0xa8, 0x68, 0x6a,
+				    0xcf, 0xd1, 0xe4, 0x52, 0x46, 0x20, 0x25,
+				    0x4a, 0x6b, 0xca, 0x18, 0xdf, 0xa5, 0x6e,
+				    0x71, 0x41, 0x77, 0x56, 0xe5, 0xa4, 0x52,
+				    0xfa, 0x9a, 0xe5, 0xae, 0xc5, 0xdc, 0x71,
+				    0x59, 0x1c, 0x11, 0x63, 0x0e, 0x9d, 0xef,
+				    0xec, 0x49, 0xa4, 0xec, 0xf8, 0x5a, 0x14,
+				    0xf6, 0x0e, 0xb8, 0x54, 0x65, 0x78, 0x99,
+				    0x97, 0x2e, 0xa5, 0xbf, 0x61, 0x59, 0xcb,
+				    0x95, 0x47 },
+		  .d = (uint8_t[]){ 0x73, 0x73, 0x01, 0xde, 0xa9, 0x3d, 0xb6,
+				    0xbc, 0xba, 0xdd, 0x7b, 0xf7, 0x96, 0x69,
+				    0x39, 0x61, 0x31, 0x7c, 0xa6, 0x80, 0xb3,
+				    0x80, 0x41, 0x6f, 0x12, 0xf4, 0x66, 0xf0,
+				    0x65, 0x26, 0xb3, 0x6b } },
+		{ .klen = 64,
+		  .k = (uint8_t[]){ 0x99, 0x28, 0x68, 0x50, 0x4d, 0x25, 0x64,
+				    0xc4, 0xfb, 0x47, 0xbc, 0xbd, 0x4a, 0xe4,
+				    0x82, 0xd8, 0xfb, 0x0e, 0x8e, 0x56, 0xd7,
+				    0xb8, 0x18, 0x64, 0xe6, 0x19, 0x86, 0xa0,
+				    0xe2, 0x56, 0x82, 0xda, 0xeb, 0x5b, 0x50,
+				    0x17, 0x7c, 0x09, 0x5e, 0xdc, 0x9e, 0x97,
+				    0x1d, 0xa9, 0x5c, 0x32, 0x10, 0xc3, 0x76,
+				    0xe7, 0x23, 0x36, 0x5a, 0xc3, 0x3d, 0x1b,
+				    0x4f, 0x39, 0x18, 0x17, 0xf4, 0xc3, 0x51,
+				    0x24 },
+		  .len = 128,
+		  .c = (uint8_t[]){ 0xed, 0x4f, 0x26, 0x9a, 0x88, 0x51, 0xeb,
+				    0x31, 0x54, 0x77, 0x15, 0x16, 0xb2, 0x72,
+				    0x28, 0x15, 0x52, 0x00, 0x77, 0x80, 0x49,
+				    0xb2, 0xdc, 0x19, 0x63, 0xf3, 0xac, 0x32,
+				    0xba, 0x46, 0xea, 0x13, 0x87, 0xcf, 0xbb,
+				    0x9c, 0x39, 0x15, 0x1a, 0x2c, 0xc4, 0x06,
+				    0xcd, 0xc1, 0x3c, 0x3c, 0x98, 0x60, 0xa2,
+				    0x7e, 0xb0, 0xb7, 0xfe, 0x8a, 0x72, 0x01,
+				    0xad, 0x11, 0x55, 0x2a, 0xfd, 0x04, 0x1e,
+				    0x33, 0xf7, 0x0e, 0x53, 0xd9, 0x7c, 0x62,
+				    0xf1, 0x71, 0x94, 0xb6, 0x61, 0x17, 0x02,
+				    0x8f, 0xa9, 0x07, 0x1c, 0xc0, 0xe0, 0x4b,
+				    0xd9, 0x2d, 0xe4, 0x97, 0x2c, 0xd5, 0x4f,
+				    0x71, 0x90, 0x10, 0xa6, 0x94, 0xe4, 0x14,
+				    0xd4, 0x97, 0x7a, 0xbe, 0xd7, 0xca, 0x6b,
+				    0x90, 0xba, 0x61, 0x2d, 0xf6, 0xc3, 0xd4,
+				    0x67, 0xcd, 0xed, 0x85, 0x03, 0x25, 0x98,
+				    0xa4, 0x85, 0x46, 0x80, 0x4f, 0x9c, 0xf2,
+				    0xec, 0xfe },
+		  .d = (uint8_t[]){ 0x2f, 0x83, 0x21, 0xf4, 0x16, 0xb9, 0xbb,
+				    0x24, 0x9f, 0x11, 0x3b, 0x13, 0xfc, 0x12,
+				    0xd7, 0x0e, 0x16, 0x68, 0xdc, 0x33, 0x28,
+				    0x39, 0xc1, 0x0d, 0xaa, 0x57, 0x17, 0x89,
+				    0x6c, 0xb7, 0x0d, 0xdf } },
+		{ .klen = 70,
+		  .k = (uint8_t[]){ 0xbc, 0xe5, 0x0c, 0xdf, 0xff, 0x84, 0x38,
+				    0x85, 0xd4, 0xf3, 0x64, 0xd6, 0x9f, 0x93,
+				    0xbf, 0x58, 0xa2, 0x32, 0x2c, 0x70, 0x7b,
+				    0x82, 0xe8, 0x78, 0xee, 0xc9, 0x6d, 0x11,
+				    0xe5, 0xdb, 0x97, 0xbb, 0xb5, 0x46, 0x06,
+				    0xa3, 0xa3, 0xcc, 0xc3, 0xbb, 0xa7, 0x16,
+				    0x26, 0x10, 0x70, 0xa6, 0xf7, 0x59, 0xa7,
+				    0x0e, 0xd3, 0xcb, 0x78, 0x5f, 0xd1, 0x35,
+				    0x4f, 0xe5, 0x66, 0x48, 0xdf, 0x11, 0x86,
+				    0x36, 0x69, 0xb7, 0x0c, 0x80, 0x3b, 0x7a },
+		  .len = 128,
+		  .c = (uint8_t[]){ 0x93, 0xb7, 0xef, 0x0e, 0x47, 0x0d, 0xdf,
+				    0xac, 0x6a, 0xef, 0x93, 0xc0, 0xdc, 0xd3,
+				    0x7b, 0x8f, 0x1c, 0x4b, 0xaf, 0x5e, 0xad,
+				    0xd9, 0x78, 0xe3, 0xbf, 0x05, 0x12, 0xfa,
+				    0x0b, 0xae, 0xb0, 0x99, 0xff, 0x9e, 0xc1,
+				    0x06, 0x1b, 0x61, 0x72, 0x47, 0x9b, 0x56,
+				    0x74, 0xdb, 0x56, 0x06, 0xff, 0xa7, 0xe6,
+				    0xb5, 0x17, 0x33, 0x09, 0x37, 0x0e, 0x16,
+				    0x47, 0x05, 0x4a, 0xaf, 0xd5, 0x90, 0x48,
+				    0x16, 0xba, 0xd5, 0xe1, 0x52, 0x30, 0x32,
+				    0xcc, 0xcd, 0x4d, 0x78, 0x65, 0x05, 0xe2,
+				    0x41, 0xac, 0x83, 0xa4, 0x84, 0x91, 0x11,
+				    0x89, 0x66, 0x6f, 0x28, 0x75, 0x53, 0xd6,
+				    0xa8, 0x16, 0x4e, 0x8d, 0xcb, 0x0c, 0x85,
+				    0xd7, 0x5c, 0x4e, 0x29, 0xf6, 0x24, 0xc9,
+				    0x7c, 0xee, 0xa6, 0x4a, 0x2c, 0x8b, 0x0c,
+				    0x9d, 0xdf, 0xa5, 0x60, 0xf7, 0x0f, 0xa3,
+				    0xff, 0x91, 0x18, 0x3e, 0x4b, 0x96, 0x8f,
+				    0x88, 0xa1 },
+		  .d = (uint8_t[]){ 0x37, 0xf9, 0xf3, 0x29, 0x18, 0x30, 0x82,
+				    0x10, 0x84, 0x9d, 0xfe, 0xbf, 0x8d, 0xd4,
+				    0x56, 0x80, 0x4b, 0xab, 0xd6, 0x84, 0x5a,
+				    0xf0, 0x72, 0x18, 0xf9, 0xd9, 0xbe, 0x9d,
+				    0xf9, 0x74, 0x3d, 0x55 } }
+	};
+
+	for (size_t i = 0; i < ARRAY_SIZE(hmac_sha256_v); i++) {
+		struct hmac_sha256_ctx ctx;
+
+		HMAC_SHA256_sw_init(&ctx, hmac_sha256_v[i].k,
+				    hmac_sha256_v[i].klen);
+		HMAC_SHA256_update(&ctx, hmac_sha256_v[i].c,
+				   hmac_sha256_v[i].len);
+		CPRINTS("HMAC SHA256_sw_init test %zu, klen=%zu, len=%zu, %s",
+			i, hmac_sha256_v[i].klen, hmac_sha256_v[i].len,
+			smemcmp(HMAC_SHA256_final(&ctx)->b8, hmac_sha256_v[i].d,
+				SHA256_DIGEST_SIZE));
+
+		HMAC_SHA256_hw_init(&ctx, hmac_sha256_v[i].k,
+				    hmac_sha256_v[i].klen);
+		HMAC_SHA256_update(&ctx, hmac_sha256_v[i].c,
+				   hmac_sha256_v[i].len);
+		CPRINTS("HMAC SHA256_hw_init test %zu, klen=%zu, len=%zu, %s",
+			i, hmac_sha256_v[i].klen, hmac_sha256_v[i].len,
+			smemcmp(HMAC_SHA256_final(&ctx)->b8, hmac_sha256_v[i].d,
+				SHA256_DIGEST_SIZE));
+
+		/* Test the path when sha hw is taken. */
+		dcrypto_grab_sha_hw();
+		HMAC_SHA256_hw_init(&ctx, hmac_sha256_v[i].k,
+				    hmac_sha256_v[i].klen);
+		HMAC_SHA256_update(&ctx, hmac_sha256_v[i].c,
+				   hmac_sha256_v[i].len);
+		CPRINTS("HMAC SHA256_hw_init(sw) test %zu, klen=%zu,"
+			" len=%zu, %s",
+			i, hmac_sha256_v[i].klen, hmac_sha256_v[i].len,
+			smemcmp(HMAC_SHA256_final(&ctx)->b8, hmac_sha256_v[i].d,
+				SHA256_DIGEST_SIZE));
+		dcrypto_release_sha_hw();
+		cflush();
+	}
+	return EC_SUCCESS;
 }
+DECLARE_SAFE_CONSOLE_COMMAND(hmac_sha256, cmd_hmac_sha256, NULL, NULL);
+#endif
+
+#endif