cr50: update FIPS initialization logic to match security policy

FIPS security policy was updated to move U2F key management out of scope
as it doesn't add anything from certification standpoint on L1, but
greatly complicates design and requires flash operations to be in the
FIPS module boundary.

This change aligns FIPS initialization flow with security policy:
 1) Checking of U2F key type is removed and would be handled during
    U2F command processing to choose approved / not-approved key gen.
 2) FIPS module is always in approved mode when self-integrity tests,
    known-answer tests and TRNG power-up tests succeeds.
 3) Implementation of console command and TPM2 command moved out of
    FIPS boundary.

BUG=b:134594373
TEST=make BOARD=cr50 [CRYPTO_TEST=1]; console commands

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

10 files changed, 341 insertions, 374 deletions

diff --git a/board/cr50/board.c b/board/cr50/board.c
index 80ec7b623e..dfa05b8ef9 100644
--- a/board/cr50/board.c
+++ b/board/cr50/board.c
@@ -185,20 +185,6 @@ int board_get_ccd_rec_lid_pin(void)
 	return board_properties & BOARD_CCD_REC_LID_PIN_MASK;
 }
 
-bool board_fips_power_up_done(void)
-{
-	return !!(GREG32(PMU, PWRDN_SCRATCH22) == BOARD_FIPS_POWERUP_DONE);
-}
-
-/* Set status of FIPS power-up tests. */
-void board_set_fips_policy_test(bool asserted)
-{
-	/* Enable writing to the long life register */
-	if (asserted)
-		GREG32(PMU, PWRDN_SCRATCH22) = BOARD_FIPS_POWERUP_DONE;
-	else
-		GREG32(PMU, PWRDN_SCRATCH22) = 0;
-}
 
 /* Get header address of the backup RW copy. */
 const struct SignedHeader *get_other_rw_addr(void)
@@ -1573,44 +1559,6 @@ static uint32_t get_properties(void)
 	return properties;
 }
 
-/**
- * NVMEM variable name for FIPS config. This is complementary for FWMP policy
- * and used primarily for lab testing where FWMP would be complicated.
- */
-static const uint8_t k_fips_config = NVMEM_VAR_FIPS_CONFIG;
-void board_set_local_fips_policy(bool asserted)
-{
-	setvar(&k_fips_config, sizeof(k_fips_config), (uint8_t *)&asserted,
-	       sizeof(asserted));
-}
-
-static bool board_get_local_fips_policy(void)
-{
-	const struct tuple *t;
-	bool fips;
-
-	t = getvar(&k_fips_config, sizeof(k_fips_config));
-	fips = (t) ? tuple_val(t)[0] : false;
-	freevar(t);
-
-	return fips;
-}
-
-bool board_fips_enforced(void)
-{
-	/**
-	 * combined flag which caches fips state and the fact it was cached
-	 * bit 7 is set when bit 0 contains fips status
-	 */
-	static uint8_t fips_state;
-
-	if (!(fips_state & 128)) {
-		fips_state = board_fwmp_fips_mode_enabled() ||
-			     board_get_local_fips_policy();
-		fips_state |= 128;
-	}
-	return !!(fips_state & 1);
-}
 
 static void init_board_properties(void)
 {
diff --git a/board/cr50/board.h b/board/cr50/board.h
index 9519306be4..7a73753260 100644
--- a/board/cr50/board.h
+++ b/board/cr50/board.h
@@ -93,8 +93,10 @@
 /* Also use the cr50 as a second factor authentication */
 #define CONFIG_U2F
 
-#undef CONFIG_FIPS_RSA2048
-#undef CONFIG_FIPS_SW_HMAC_DRBG
+/* Additional FIPS KAT tests. */
+#define CONFIG_FIPS_RSA2048
+#define CONFIG_FIPS_SW_HMAC_DRBG
+#define CONFIG_FIPS_AES_CBC_256
 
 /* USB configuration */
 #define CONFIG_USB
@@ -252,7 +254,6 @@ enum nvmem_vars {
 	NVMEM_VAR_U2F_SALT,
 	NVMEM_VAR_CCD_CONFIG,
 	NVMEM_VAR_G2F_SALT,
-	NVMEM_VAR_FIPS_CONFIG,
 
 	NVMEM_VARS_COUNT
 };
@@ -330,16 +331,6 @@ int board_has_ec_cr50_comm_support(void);
 int board_id_is_mismatched(void);
 /* Allow for deep sleep to be enabled on AP shutdown */
 int board_deep_sleep_allowed(void);
-/* indicates completion of power-up tests earlier */
-bool board_fips_power_up_done(void);
-
-/**
- * Set status of FIPS power-up tests on wake from sleep
- *
- * @param asserted: 0 power-up tests should run on resume, otherwise can be
- * skipped
- */
-void board_set_fips_policy_test(bool asserted);
 
 void power_button_record(void);
 
@@ -365,12 +356,6 @@ void board_reboot_ec_deferred(int usec_delay);
 void board_closed_loop_reset(void);
 int board_wipe_tpm(int reset_required);
 int board_is_first_factory_boot(void);
-int board_fwmp_fips_mode_enabled(void);
-
-/* set FIPS policy for board in NVRAM (independent of FWMP) */
-void board_set_local_fips_policy(bool asserted);
-/* return non zero if FIPS mode enforced in FWMP or NVRAM */
-bool board_fips_enforced(void);
 
 int usb_i2c_board_enable(void);
 void usb_i2c_board_disable(void);
diff --git a/board/cr50/build.mk b/board/cr50/build.mk
index 1656cf24c1..ed45ca2963 100644
--- a/board/cr50/build.mk
+++ b/board/cr50/build.mk
@@ -92,6 +92,9 @@ fips-${CONFIG_FIPS_UTIL} += dcrypto/util.o
 
 custom-board-ro_objs-${CONFIG_FIPS_UTIL} = $(BDIR)/dcrypto/util.o
 
+# FIPS console and TPM2 commands are outside FIPS module
+board-y += fips_cmd.o
+
 board-y += tpm2/NVMem.o
 board-y += tpm2/aes.o
 board-y += tpm2/ecc.o
diff --git a/board/cr50/fips.c b/board/cr50/fips.c
index 436e1588ae..9fd72d28c8 100644
--- a/board/cr50/fips.c
+++ b/board/cr50/fips.c
@@ -24,9 +24,6 @@
 
 #define CPRINTS(format, args...) cprints(CC_SYSTEM, format, ## args)
 
-/* FIPS mode is temporarily disabled. */
-#define FIPS_MODE_ENABLED 0
-
 /**
  * Combined FIPS status & global FIPS error.
  * default value is  = FIPS_UNINITIALIZED
@@ -39,85 +36,18 @@ enum fips_status fips_status(void)
 	return _fips_status;
 }
 
+#ifdef CRYPTO_TEST_SETUP
 /* Flag to simulate specific error condition in power-up tests. */
 uint8_t fips_break_cmd;
-
-void fips_set_status(enum fips_status status)
-{
-	/**
-	 * if FIPS error took place, drop indication of FIPS approved mode.
-	 * Next cycle of sleep will power-cycle HW crypto components, so any
-	 * soft-errors will be recovered. In case of hard errors it
-	 * will be detected again.
-	 */
-	/* accumulate status */
-	_fips_status |= status;
-
-	status = _fips_status;
-	/* if we have error, require power up tests on resume */
-	if (status & FIPS_ERROR_MASK)
-		board_set_fips_policy_test(false);
-}
-
-bool fips_mode(void)
-{
-	return (_fips_status & FIPS_MODE_ACTIVE);
-}
-
-#if FIPS_MODE_ENABLED
-static const uint8_t k_salt = NVMEM_VAR_G2F_SALT;
-
-/* Can't include TPM2 headers, so just define constant locally. */
-#define HR_NV_INDEX (1U << 24)
-
-/* Wipe old U2F keys. */
-static void u2f_zeroize(void)
-{
-	const uint32_t u2fobjs[] = { TPM_HIDDEN_U2F_KEK | HR_NV_INDEX,
-				     TPM_HIDDEN_U2F_KH_SALT | HR_NV_INDEX, 0 };
-	/* Delete NVMEM_VAR_G2F_SALT. */
-	setvar(&k_salt, sizeof(k_salt), NULL, 0);
-	/* Remove U2F keys and wipe all deleted objects. */
-	nvmem_erase_tpm_data_selective(u2fobjs);
-}
+#else
+/* For production set it to zero, so check is eliminated. */
+#define fips_break_cmd 0
 #endif
 
-/**
- * Return current status for U2F keys:
- * false - U2F keys require zeroization.
- * true - U2F keys are missing or created in FIPS mode.
- */
-static bool fips_u2f_compliant(void)
+static inline bool fips_is_no_crypto_error(void)
 {
-/* Until U2F key gen switch to new code, don't enable FIPS mode. */
-#if FIPS_MODE_ENABLED
-	uint8_t val_len = 0;
-	const struct tuple *t_salt;
-
-	/**
-	 * We are in FIPS mode if and only if:
-	 * 1) U2F keys were created in FIPS compliant way (board_fips_enforced)
-	 * 2) OR U2F keys weren't previously created
-	 */
-	if (board_fips_enforced())
-		return true;
-
-	/* FIPS mode wasn't enforced, so check presence of U2F keys */
-	t_salt = getvar(&k_salt, sizeof(k_salt));
-	if (t_salt) {
-		val_len = t_salt->val_len;
-		freevar(t_salt);
-	}
-	/* If none of keys is present - we are in FIPS mode. */
-	if (!val_len && !read_tpm_nvmem_size(TPM_HIDDEN_U2F_KEK) &&
-	    !read_tpm_nvmem_size(TPM_HIDDEN_U2F_KH_SALT)) {
-		/* Apparently, board FIPS mode wasn't set yet, so set it. */
-		board_set_local_fips_policy(true);
-		return true;
-	}
-#endif
-	/* we still have old U2F keys, so not in FIPS until zeroized */
-	return false;
+	return (_fips_status &
+	       (FIPS_ERROR_MASK & (~FIPS_FATAL_SELF_INTEGRITY))) == 0;
 }
 
 /* Return true if crypto can be used (no failures detected). */
@@ -130,8 +60,7 @@ bool fips_crypto_allowed(void)
 	 * TODO(b/138578318): remove ignoring of FIPS_FATAL_SELF_INTEGRITY.
 	 */
 	return ((_fips_status & FIPS_POWER_UP_TEST_DONE) &&
-		!(_fips_status &
-		  (FIPS_ERROR_MASK & (~FIPS_FATAL_SELF_INTEGRITY))));
+		fips_is_no_crypto_error());
 }
 
 void fips_throw_err(enum fips_status err)
@@ -147,6 +76,49 @@ void fips_throw_err(enum fips_status err)
 }
 
 /**
+ * Set status of FIPS power-up tests on wake from sleep. We don't want to
+ * run lengthy KAT & power-up tests on every wake-up, so need to 'cache'
+ * result in long life register which content persists during sleep mode.
+ *
+ * @param asserted: false power-up tests should run on resume, otherwise
+ * can be skipped.
+ */
+static void fips_set_power_up(bool asserted)
+{
+	/* Enable writing to the long life register */
+	if (asserted)
+		GREG32(PMU, PWRDN_SCRATCH22) = BOARD_FIPS_POWERUP_DONE;
+	else
+		GREG32(PMU, PWRDN_SCRATCH22) = 0;
+}
+
+/**
+ * Return true if FIPS KAT tests completed successfully after waking up
+ * from sleep mode which clears RAM.
+ */
+static bool fips_is_power_up_done(void)
+{
+	return !!(GREG32(PMU, PWRDN_SCRATCH22) == BOARD_FIPS_POWERUP_DONE);
+}
+
+void fips_set_status(enum fips_status status)
+{
+	/**
+	 * if FIPS error took place, drop indication of FIPS approved mode.
+	 * Next cycle of sleep will power-cycle HW crypto components, so any
+	 * soft-errors will be recovered. In case of hard errors it
+	 * will be detected again.
+	 */
+	/* Accumulate status (errors). */
+	_fips_status |= status;
+
+	status = _fips_status;
+	/* if we have error, require power up tests on resume. */
+	if (status & FIPS_ERROR_MASK)
+		fips_set_power_up(false);
+}
+
+/**
  * Test vectors for Known-Answer Tests (KATs) and driving functions.
  */
 
@@ -420,6 +392,7 @@ static bool fips_ecdsa_verify_kat(void)
 	return !(fips_break_cmd == FIPS_BREAK_ECDSA) && (passed == 0);
 }
 
+#ifdef CONFIG_FIPS_AES_CBC_256
 #define AES_BLOCK_LEN 16
 
 /* Known-answer test for AES-256 encrypt/decrypt. */
@@ -457,12 +430,13 @@ static bool fips_aes256_kat(void)
 	return !(fips_break_cmd == FIPS_BREAK_AES256) &&
 	       (memcmp(kat_aes128_msg, dec, AES_BLOCK_LEN) == 0);
 }
+#endif
 
 #ifdef CONFIG_FIPS_RSA2048
 /* Known-answer test for RSA 2048. */
 static bool fips_rsa2048_verify_kat(void)
 {
-	uint8_t digest[SHA256_DIGEST_SIZE];
+	struct sha256_digest digest;
 	static const uint32_t pub[64] = {
 		0xf8729219, 0x2b42fc45, 0xfe6f4397, 0xa6ba59df, 0x4ce45ab8,
 		0x4be044ea, 0xdade58ec, 0xf871ada6, 0x3a6355a1, 0x43739940,
@@ -546,16 +520,16 @@ static bool fips_rsa2048_verify_kat(void)
 
 	int passed;
 
-	DCRYPTO_SHA256_hash(msg, sizeof(msg), digest);
-	passed = DCRYPTO_rsa_verify(&rsa, digest, sizeof(digest), sig,
+	SHA256_hw_hash(msg, sizeof(msg), &digest);
+	passed = DCRYPTO_rsa_verify(&rsa, digest.b8, sizeof(digest), sig,
 				    sizeof(sig), PADDING_MODE_PKCS1,
 				    HASH_SHA256);
 	if (!passed)
 		return false;
-	DCRYPTO_SHA256_hash(bad_msg, sizeof(bad_msg), digest);
+	SHA256_hw_hash(bad_msg, sizeof(bad_msg), &digest);
 
 	/* now signature should fail */
-	return !DCRYPTO_rsa_verify(&rsa, digest, sizeof(digest), sig,
+	return !DCRYPTO_rsa_verify(&rsa, digest.b8, sizeof(digest), sig,
 				   sizeof(sig), PADDING_MODE_PKCS1,
 				   HASH_SHA256);
 }
@@ -604,16 +578,11 @@ static bool fips_self_integrity(void)
 	return DCRYPTO_equals(fips_integrity.b8, digest.b8, sizeof(digest));
 }
 
-/**
- * FIPS Power-up known-answer tests.
- * Single point of initialization for all FIPS-compliant
- * cryptography. Responsible for KATs, TRNG testing, and signalling a
- * fatal error.
- * @return FIPS_POWERON_TEST_ERROR if memory allocation error took place
- */
-#define FIPS_POWERON_TEST_ERROR -2ULL
+/* Duration of FIPS tests. */
+uint64_t fips_last_kat_test_duration;
+
 #define FIPS_KAT_STACK_SIZE 2048
-static uint64_t fips_power_up_tests(void)
+void fips_power_up_tests(void)
 {
 	char *stack_buf;
 	void *stack;
@@ -645,11 +614,14 @@ static uint64_t fips_power_up_tests(void)
 		if (!call_on_stack(stack, &fips_ecdsa_verify_kat))
 			_fips_status |= FIPS_FATAL_ECDSA;
 
-		if (!call_on_stack(stack, &fips_aes256_kat))
-			_fips_status |= FIPS_FATAL_AES256;
 		if (!call_on_stack(stack, &fips_hmac_drbg_kat))
 			_fips_status |= FIPS_FATAL_HMAC_DRBG;
 
+#ifdef CONFIG_FIPS_AES_CBC_256
+		if (!call_on_stack(stack, &fips_aes256_kat))
+			_fips_status |= FIPS_FATAL_AES256;
+#endif
+
 #ifdef CONFIG_FIPS_RSA2048
 		/* RSA KAT adds 30ms and not used for U2F */
 		if (!call_on_stack(stack, &fips_rsa2048_verify_kat))
@@ -667,7 +639,7 @@ static uint64_t fips_power_up_tests(void)
 		if (!call_on_stack(stack, &fips_hmac_sha256_kat))
 			_fips_status |= FIPS_FATAL_HMAC_SHA256;
 #ifdef CONFIG_FIPS_SW_HMAC_DRBG
-		/* SW HMAC DRBG adds 40ms and not used for U2F */
+		/* SW HMAC DRBG adds 30ms and not used for U2F */
 		if (!call_on_stack(stack, &fips_hmac_drbg_kat))
 			_fips_status |= FIPS_FATAL_HMAC_DRBG;
 #endif
@@ -676,9 +648,10 @@ static uint64_t fips_power_up_tests(void)
 
 		/* Second call to TRNG warm-up. */
 		fips_trng_startup(1);
-		/* if no errors, set not to run tests on wake from sleep. */
-		if (!(_fips_status & FIPS_ERROR_MASK))
-			board_set_fips_policy_test(true);
+
+		/* If no errors, set not to run tests on wake from sleep. */
+		if (fips_is_no_crypto_error())
+			fips_set_power_up(true);
 		else /* write combined error to flash log */
 			flash_log_add_event(FE_LOG_FIPS_FAILURE,
 					    sizeof(_fips_status),
@@ -686,39 +659,15 @@ static uint64_t fips_power_up_tests(void)
 		/* Set the bit that power-up tests completed, even if failed. */
 		_fips_status |= FIPS_POWER_UP_TEST_DONE;
 	} else
-		return FIPS_POWERON_TEST_ERROR;
+		_fips_status |= FIPS_FATAL_OTHER;
 
-	return get_time().val - starttime;
-}
-
-/* Print on console current FIPS mode. */
-static void fips_print_mode(void)
-{
-	if (_fips_status == FIPS_UNINITIALIZED)
-		CPRINTS("FIPS mode not initialized");
-	else if (_fips_status & FIPS_ERROR_MASK)
-		CPRINTS("FIPS error code 0x%08x, not-approved", _fips_status);
-	else
-		CPRINTS("Running in FIPS 140-2 %s mode",
-			((_fips_status & FIPS_MODE_ACTIVE) &&
-			 (_fips_status & FIPS_POWER_UP_TEST_DONE)) ?
-				"approved" :
-				"not-approved");
-}
-
-/* Print time it took tests to run or print error message. */
-static void fips_print_test_time(uint64_t time)
-{
-	if (time == FIPS_POWERON_TEST_ERROR)
-		CPRINTS("FIPS test failed to run");
-	else if (time != -1ULL)
-		CPRINTS("FIPS power-up tests completed in %llu", time);
+	fips_last_kat_test_duration = get_time().val - starttime;
 }
 
 /* Initialize FIPS mode. Executed during power-up and resume from sleep. */
 static void fips_power_on(void)
 {
-	uint64_t testtime = -1ULL;
+	fips_last_kat_test_duration = -1ULL;
 	/* make sure on power-on / resume it's cleared */
 	_fips_status = FIPS_UNINITIALIZED;
 
@@ -727,164 +676,16 @@ static void fips_power_on(void)
 	 * for some reason, run them now. Board FIPS KAT status will
 	 * be updated by fips_power_up_tests() if all tests pass.
 	 */
-	if (!board_fips_power_up_done())
-		testtime = fips_power_up_tests();
+	if (!fips_is_power_up_done())
+		fips_power_up_tests();
 	else	/* tests were already completed before sleep */
 		_fips_status |= FIPS_POWER_UP_TEST_DONE;
 
 	/* Check if we can set FIPS-approved mode. */
-	if (fips_u2f_compliant())
+	if (fips_crypto_allowed())
 		fips_set_status(FIPS_MODE_ACTIVE);
 
-	/* Once FIPS power-up tests completed we can enable console output. */
-	console_enable_output();
-
-	fips_print_test_time(testtime);
-	fips_print_mode();
 }
 
 /* FIPS initialization is last init hook, HOOK_PRIO_FIPS > HOOK_PRIO_LAST */
-DECLARE_HOOK(HOOK_INIT, fips_power_on, HOOK_PRIO_FIPS);
-
-/* Switch FIPS status. */
-void fips_set_policy(bool active)
-{
-#ifndef CR50_DEV
-	/* in Production mode never disable FIPS once enabled. */
-	if (!active)
-		return;
-#endif
-	/* Do nothing if there is no change. */
-	if (!(!active ^ !(_fips_status & FIPS_MODE_ACTIVE)))
-		return;
-/* Temporarily prevent switch to FIPS mode until U2F key gen is ready. */
-#if FIPS_MODE_ENABLED
-	/* Update local board FIPS flag. */
-	board_set_local_fips_policy(active);
-	CPRINTS("FIPS policy set to %d", active);
-	cflush();
-	u2f_zeroize();
-
-#ifdef CR50_DEV
-	if (!active) {
-		uint8_t random[32];
-		/* Create fake u2f keys old style */
-		fips_trng_bytes(random, sizeof(random));
-		setvar(&k_salt, sizeof(k_salt), random, sizeof(random));
-
-		fips_trng_bytes(random, sizeof(random));
-		write_tpm_nvmem_hidden(TPM_HIDDEN_U2F_KEK, sizeof(random),
-				       random, 1);
-		fips_trng_bytes(random, sizeof(random));
-		write_tpm_nvmem_hidden(TPM_HIDDEN_U2F_KH_SALT, sizeof(random),
-				       random, 1);
-	}
-#endif
-#endif
-	system_reset(EC_RESET_FLAG_SECURITY);
-}
-
-/* Console command 'fips' to report and change status, run tests */
-static int cmd_fips_status(int argc, char **argv)
-{
-	fips_print_mode();
-	ccprints("FIPS crypto allowed: %u, u2f compliant: %u, "
-		 "board power up done: %u, board enforced: %u, fwmp : %u",
-		 fips_crypto_allowed(), fips_u2f_compliant(),
-		 board_fips_power_up_done(), board_fips_enforced(),
-		 board_fwmp_fips_mode_enabled());
-
-	cflush();
-
-	if (argc == 2) {
-		if (!strncmp(argv[1], "on", 2))
-			fips_set_policy(true);
-		else if (!strncmp(argv[1], "test", 4)) {
-			fips_print_test_time(fips_power_up_tests());
-			fips_print_mode();
-		}
-#ifdef CR50_DEV
-		else if (!strncmp(argv[1], "off", 3))
-			fips_set_policy(false);
-		else if (!strncmp(argv[1], "trng", 4))
-			fips_break_cmd = FIPS_BREAK_TRNG;
-		else if (!strncmp(argv[1], "sha", 3))
-			fips_break_cmd = FIPS_BREAK_SHA256;
-#endif
-	}
-	return 0;
-}
-
-DECLARE_SAFE_CONSOLE_COMMAND(fips, cmd_fips_status,
-#ifdef CR50_DEV
-	"[on | off | test | trng | sha]",
-#else
-	"[on | test]",
-#endif
-	"Report or change FIPS status, run tests, simulate errors");
-
-/**
- * Vendor command implementation to report & change status, run tests.
- * Command structure:
- *
- * field     |    size  |                  note
- * =========================================================================
- * op        |    1     | 0 - get status, 1 - set FIPS ON (remove old U2F)
- *           |          | 2 - run tests, 3 .. 8 - simulate errors
- */
-static enum vendor_cmd_rc fips_cmd(enum vendor_cmd_cc code, void *buf,
-				    size_t input_size, size_t *response_size)
-{
-	uint8_t *cmd = buf;
-	uint32_t fips_reverse;
-
-	*response_size = 0;
-	if (input_size != 1)
-		return VENDOR_RC_BOGUS_ARGS;
-
-	switch ((enum fips_cmd)*cmd) {
-	case FIPS_CMD_GET_STATUS:
-		fips_reverse = htobe32(_fips_status);
-		memcpy(buf, &fips_reverse, sizeof(fips_reverse));
-		*response_size = sizeof(fips_reverse);
-		break;
-	case FIPS_CMD_ON:
-		fips_set_policy(true); /* we can reboot here... */
-		break;
-	case FIPS_CMD_TEST:
-		fips_power_up_tests();
-		fips_reverse = htobe32(_fips_status);
-		memcpy(buf, &fips_reverse, sizeof(fips_reverse));
-		*response_size = sizeof(fips_reverse);
-		break;
-#ifdef CR50_DEV
-	case FIPS_CMD_BREAK_TRNG:
-		fips_break_cmd = FIPS_BREAK_TRNG;
-		break;
-	case FIPS_CMD_BREAK_SHA256:
-		fips_break_cmd = FIPS_BREAK_SHA256;
-		break;
-	case FIPS_CMD_BREAK_HMAC_SHA256:
-		fips_break_cmd = FIPS_BREAK_HMAC_SHA256;
-		break;
-	case FIPS_CMD_BREAK_HMAC_DRBG:
-		fips_break_cmd = FIPS_BREAK_HMAC_DRBG;
-		break;
-	case FIPS_CMD_BREAK_ECDSA:
-		fips_break_cmd = FIPS_BREAK_ECDSA;
-		break;
-	case FIPS_CMD_BREAK_AES256:
-		fips_break_cmd = FIPS_BREAK_AES256;
-		break;
-	case FIPS_CMD_NO_BREAK:
-		fips_break_cmd = FIPS_NO_BREAK;
-		break;
-#endif
-	default:
-		return VENDOR_RC_BOGUS_ARGS;
-	}
-
-	return VENDOR_RC_SUCCESS;
-}
-
-DECLARE_VENDOR_COMMAND(VENDOR_CC_FIPS_CMD, fips_cmd);
+DECLARE_HOOK(HOOK_INIT, fips_power_on, HOOK_PRIO_INIT_FIPS);
diff --git a/board/cr50/fips.h b/board/cr50/fips.h
index 4a7eef2f0e..a9017e46a9 100644
--- a/board/cr50/fips.h
+++ b/board/cr50/fips.h
@@ -27,8 +27,12 @@ enum fips_status {
 	FIPS_FATAL_HMAC_SHA256 = 1 << 5,
 	FIPS_FATAL_HMAC_DRBG = 1 << 6,
 	FIPS_FATAL_ECDSA = 1 << 7,
+#ifdef CONFIG_FIPS_RSA2048
 	FIPS_FATAL_RSA2048 = 1 << 8,
+#endif
+#ifdef CONFIG_FIPS_AES_CBC_256
 	FIPS_FATAL_AES256 = 1 << 9,
+#endif
 	FIPS_FATAL_SELF_INTEGRITY = 1 << 10,
 	FIPS_FATAL_OTHER = 1 << 15,
 	FIPS_ERROR_MASK = 0xffff,
@@ -43,9 +47,20 @@ enum fips_break {
 	FIPS_BREAK_HMAC_SHA256 = 3,
 	FIPS_BREAK_HMAC_DRBG = 4,
 	FIPS_BREAK_ECDSA = 5,
-	FIPS_BREAK_AES256 = 6
+#ifdef CONFIG_FIPS_AES_CBC_256
+	FIPS_BREAK_AES256 = 6,
+#endif
+#ifdef CONFIG_FIPS_RSA2048
+	FIPS_BREAK_RSA2048 = 7,
+#endif
 };
+
+#ifdef CRYPTO_TEST_SETUP
 extern uint8_t fips_break_cmd;
+#endif
+
+/* Duration of last FIPS KAT / Power-up tests. */
+extern uint64_t fips_last_kat_test_duration;
 
 /* Command codes for VENDOR_CC_FIPS_CMD. */
 enum fips_cmd {
@@ -57,8 +72,13 @@ enum fips_cmd {
 	FIPS_CMD_BREAK_HMAC_SHA256 = 5,
 	FIPS_CMD_BREAK_HMAC_DRBG = 6,
 	FIPS_CMD_BREAK_ECDSA = 7,
+#ifdef CONFIG_FIPS_AES_CBC_256
 	FIPS_CMD_BREAK_AES256 = 8,
-	FIPS_CMD_NO_BREAK = 9
+#endif
+#ifdef CONFIG_FIPS_RSA2048
+	FIPS_CMD_BREAK_RSA2048 = 9,
+#endif
+	FIPS_CMD_NO_BREAK = 10
 };
 
 /* These symbols defined in core/cortex-m/ec.lds.S. */
@@ -68,9 +88,6 @@ extern uint8_t __fips_module_end;
 /* Return current FIPS status of operations. */
 enum fips_status fips_status(void);
 
-/* return true if in FIPS-approved mode. */
-bool fips_mode(void);
-
 /**
  * Crypto is enabled when either FIPS mode is not enforced,
  * or if it is enforced and in good health
@@ -89,10 +106,14 @@ void fips_set_status(enum fips_status status);
 void fips_throw_err(enum fips_status err);
 
 /**
- * Switch FIPS status, zeroize keys if needed. For Production it's a one way
- * to 'FIPS on'. For development board it allows creation of non-FIPS keys.
+ * FIPS Power-up and known-answer tests.
+ * Single point of initialization for all FIPS-compliant
+ * cryptography. Responsible for KATs, TRNG testing, and signalling a
+ * fatal error.
+ *
+ * Set FIPS status globally as a result.
  */
-void fips_set_policy(bool active);
+void fips_power_up_tests(void);
 
 #ifdef __cplusplus
 }
diff --git a/board/cr50/fips_cmd.c b/board/cr50/fips_cmd.c
new file mode 100644
index 0000000000..304e23a0da
--- /dev/null
+++ b/board/cr50/fips_cmd.c
@@ -0,0 +1,215 @@
+/* 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.
+ */
+
+#include "builtin/endian.h"
+#include "console.h"
+#include "dcrypto.h"
+#include "ec_commands.h"
+#include "extension.h"
+#include "fips.h"
+#include "fips_rand.h"
+#include "flash_log.h"
+#include "hooks.h"
+#include "new_nvmem.h"
+#include "nvmem.h"
+#include "nvmem_vars.h"
+#include "registers.h"
+#include "scratch_reg1.h"
+#include "shared_mem.h"
+#include "system.h"
+#include "tpm_nvmem_ops.h"
+#include "u2f_impl.h"
+
+#define CPRINTS(format, args...) cprints(CC_SYSTEM, format, ##args)
+
+/* Print on console current FIPS mode. */
+static void fips_print_mode(void)
+{
+	enum fips_status status = fips_status();
+
+	if (status == FIPS_UNINITIALIZED)
+		CPRINTS("FIPS mode not initialized");
+	else if (status & FIPS_ERROR_MASK)
+		CPRINTS("FIPS error code 0x%08x, not-approved", status);
+	else
+		CPRINTS("Running in FIPS 140-2 %s mode",
+			((status & FIPS_MODE_ACTIVE) &&
+			 (status & FIPS_POWER_UP_TEST_DONE)) ?
+				"approved" :
+				      "not-approved");
+}
+
+/* Print time it took tests to run or print error message. */
+static void fips_print_test_time(void)
+{
+	if (fips_last_kat_test_duration != -1ULL)
+		CPRINTS("FIPS power-up tests completed in %llu",
+			fips_last_kat_test_duration);
+}
+
+static void fips_print_status(void)
+{
+	/* Once FIPS power-up tests completed we can enable console output. */
+	console_enable_output();
+
+	fips_print_test_time();
+	fips_print_mode();
+}
+DECLARE_HOOK(HOOK_INIT, fips_print_status, HOOK_PRIO_INIT_PRINT_FIPS_STATUS);
+
+#ifdef CRYPTO_TEST_SETUP
+static const uint8_t k_salt = NVMEM_VAR_G2F_SALT;
+
+/* Can't include TPM2 headers, so just define constant locally. */
+#define HR_NV_INDEX (1U << 24)
+
+/* Wipe old U2F keys. */
+static void u2f_zeroize_non_fips(void)
+{
+	const uint32_t u2fobjs[] = { TPM_HIDDEN_U2F_KEK | HR_NV_INDEX,
+				     TPM_HIDDEN_U2F_KH_SALT | HR_NV_INDEX, 0 };
+	/* Delete NVMEM_VAR_G2F_SALT. */
+	setvar(&k_salt, sizeof(k_salt), NULL, 0);
+	/* Remove U2F keys and wipe all deleted objects. */
+	nvmem_erase_tpm_data_selective(u2fobjs);
+}
+
+/* Set U2F keys to old or new version. */
+static void fips_set_u2f_keys(bool active)
+{
+	if (!active) {
+		/* Old version. */
+		uint8_t random[32];
+		/* Create fake u2f keys old style */
+		fips_trng_bytes(random, sizeof(random));
+		setvar(&k_salt, sizeof(k_salt), random, sizeof(random));
+
+		fips_trng_bytes(random, sizeof(random));
+		write_tpm_nvmem_hidden(TPM_HIDDEN_U2F_KEK, sizeof(random),
+				       random, 1);
+		fips_trng_bytes(random, sizeof(random));
+		write_tpm_nvmem_hidden(TPM_HIDDEN_U2F_KH_SALT, sizeof(random),
+				       random, 1);
+	} else {
+		/**
+		 * TODO(sukhomlinov): Implement new key generation after merging
+		 * https://crrev.com/c/3034852 and adding FIPS key gen.
+		 */
+		u2f_zeroize_non_fips();
+	}
+	system_reset(EC_RESET_FLAG_SECURITY);
+}
+#endif
+
+/* Console command 'fips' to report and change status, run tests */
+static int cmd_fips_status(int argc, char **argv)
+{
+	fips_print_mode();
+	fips_print_test_time();
+	CPRINTS("FIPS crypto allowed: %u", fips_crypto_allowed());
+
+	cflush();
+
+	if (argc == 2) {
+		if (!strncmp(argv[1], "test", 4)) {
+			fips_power_up_tests();
+			fips_print_test_time();
+			fips_print_mode();
+		}
+#ifdef CRYPTO_TEST_SETUP
+		else if (!strncmp(argv[1], "new", 3))
+			fips_set_u2f_keys(true); /* we can reboot here... */
+		else if (!strncmp(argv[1], "old", 3))
+			fips_set_u2f_keys(false); /* we can reboot here... */
+		else if (!strncmp(argv[1], "trng", 4))
+			fips_break_cmd = FIPS_BREAK_TRNG;
+		else if (!strncmp(argv[1], "sha", 3))
+			fips_break_cmd = FIPS_BREAK_SHA256;
+#endif
+	}
+	return 0;
+}
+
+DECLARE_SAFE_CONSOLE_COMMAND(
+	fips, cmd_fips_status,
+#ifdef CRYPTO_TEST_SETUP
+	"[test | new | old | trng | sha]",
+	"Report FIPS status, switch U2F key, run tests, simulate errors");
+#else
+	"[test]", "Report FIPS status, run tests");
+#endif
+
+/**
+ * Vendor command implementation to report & change status, run tests.
+ * Command structure:
+ *
+ * field     |    size  |                  note
+ * =========================================================================
+ * op        |    1     | 0 - get status, 1 - set FIPS ON (remove old U2F)
+ *           |          | 2 - run tests, 3 .. 8 - simulate errors
+ */
+static enum vendor_cmd_rc fips_cmd(enum vendor_cmd_cc code, void *buf,
+				   size_t input_size, size_t *response_size)
+{
+	uint8_t *cmd = buf;
+	uint32_t fips_reverse;
+
+	*response_size = 0;
+	if (input_size != 1)
+		return VENDOR_RC_BOGUS_ARGS;
+
+	switch ((enum fips_cmd) *cmd) {
+	case FIPS_CMD_GET_STATUS:
+		fips_reverse = htobe32(fips_status());
+		memcpy(buf, &fips_reverse, sizeof(fips_reverse));
+		*response_size = sizeof(fips_reverse);
+		break;
+	case FIPS_CMD_TEST:
+		fips_power_up_tests();
+		fips_reverse = htobe32(fips_status());
+		memcpy(buf, &fips_reverse, sizeof(fips_reverse));
+		*response_size = sizeof(fips_reverse);
+		break;
+#ifdef CRYPTO_TEST_SETUP
+	case FIPS_CMD_ON:
+		fips_set_u2f_keys(true); /* we can reboot here... */
+		break;
+	case FIPS_CMD_BREAK_TRNG:
+		fips_break_cmd = FIPS_BREAK_TRNG;
+		break;
+	case FIPS_CMD_BREAK_SHA256:
+		fips_break_cmd = FIPS_BREAK_SHA256;
+		break;
+	case FIPS_CMD_BREAK_HMAC_SHA256:
+		fips_break_cmd = FIPS_BREAK_HMAC_SHA256;
+		break;
+	case FIPS_CMD_BREAK_HMAC_DRBG:
+		fips_break_cmd = FIPS_BREAK_HMAC_DRBG;
+		break;
+	case FIPS_CMD_BREAK_ECDSA:
+		fips_break_cmd = FIPS_BREAK_ECDSA;
+		break;
+#ifdef CONFIG_FIPS_AES_CBC_256
+	case FIPS_CMD_BREAK_AES256:
+		fips_break_cmd = FIPS_BREAK_AES256;
+		break;
+#endif /* CONFIG_FIPS_AES_CBC_256 */
+#ifdef CONFIG_FIPS_RSA2048
+	case FIPS_CMD_BREAK_RSA2048:
+		fips_break_cmd = FIPS_BREAK_RSA2048;
+		break;
+#endif /* CONFIG_FIPS_RSA2048 */
+	case FIPS_CMD_NO_BREAK:
+		fips_break_cmd = FIPS_NO_BREAK;
+		break;
+#endif /* CRYPTO_TEST_SETUP */
+	default:
+		return VENDOR_RC_BOGUS_ARGS;
+	}
+
+	return VENDOR_RC_SUCCESS;
+}
+
+DECLARE_VENDOR_COMMAND(VENDOR_CC_FIPS_CMD, fips_cmd);
diff --git a/board/cr50/fips_rand.c b/board/cr50/fips_rand.c
index 2b2f796767..a214f9196e 100644
--- a/board/cr50/fips_rand.c
+++ b/board/cr50/fips_rand.c
@@ -181,9 +181,11 @@ static struct rand_result read_rand(void)
 	uint32_t empty_count = 0;
 	uint32_t reset_count = 0;
 
+#ifdef CRYPTO_TEST_SETUP
 	/* Do we need to simulate error? */
 	if (fips_break_cmd == FIPS_BREAK_TRNG)
 		return (struct rand_result){ .random_value = 0, .valid = true };
+#endif
 
 	/**
 	 * make sure we never hang in the loop - try at max 1
diff --git a/board/cr50/wp.c b/board/cr50/wp.c
index 7b8f02b24d..e24332c3d4 100644
--- a/board/cr50/wp.c
+++ b/board/cr50/wp.c
@@ -386,7 +386,6 @@ int board_wipe_tpm(int reset_required)
  */
 #define FWMP_HASH_SIZE		    32
 #define FWMP_DEV_DISABLE_CCD_UNLOCK BIT(6)
-#define FWMP_DEV_FIPS_MODE	    BIT(7)
 #define FIRMWARE_FLAG_DEV_MODE      0x02
 
 struct RollbackSpaceFirmware {
@@ -472,19 +471,6 @@ int board_fwmp_allows_unlock(void)
 #endif
 }
 
-int board_fwmp_fips_mode_enabled(void)
-{
-	struct RollbackSpaceFirmware fw;
-
-	if (TPM_READ_SUCCESS ==
-	    read_tpm_nvmem(FIRMWARE_NV_INDEX, sizeof(fw), &fw)) {
-		return !!(fw.flags & FWMP_DEV_FIPS_MODE);
-	}
-
-	/* If not found or other error, assume fips mode is disabled */
-	return 0;
-}
-
 int board_vboot_dev_mode_enabled(void)
 {
 	struct RollbackSpaceFirmware fw;
diff --git a/include/config.h b/include/config.h
index 7f334bd5dc..bc183476ac 100644
--- a/include/config.h
+++ b/include/config.h
@@ -4885,9 +4885,11 @@
 #define CONFIG_CRC8
 #endif
 
-/* Run RSA 2048 known-answer test (+30 ms) */
+/* Don't run RSA 2048 known-answer test (+30 ms). */
 #undef CONFIG_FIPS_RSA2048
-/* Run software HMAC_DRBG-SHA256 known-answer test (+40 ms) */
+/* Don't run software HMAC_DRBG-SHA256 known-answer test (+30 ms). */
 #undef CONFIG_FIPS_SW_HMAC_DRBG
+/* Don't run AES CBC 256 test (not used for U2F anymore). */
+#undef CONFIG_FIPS_AES_CBC_256
 
 #endif  /* __CROS_EC_CONFIG_H */
diff --git a/include/hooks.h b/include/hooks.h
index 0142ab62a0..1002f23d83 100644
--- a/include/hooks.h
+++ b/include/hooks.h
@@ -15,7 +15,6 @@ enum hook_priority {
 	HOOK_PRIO_FIRST = 1,       /* Highest priority */
 	HOOK_PRIO_DEFAULT = 5000,  /* Default priority */
 	HOOK_PRIO_LAST = 9999,     /* Lowest priority */
-	HOOK_PRIO_FIPS = 10000,    /* FIPS init executes last */
 
 	/* Specific hook vales for HOOK_INIT */
 	/* DMA inits before ADC, I2C, SPI */
@@ -68,6 +67,11 @@ enum hook_priority {
 	HOOK_PRIO_TEMP_SENSOR = 6000,
 	/* After all sensors have been polled */
 	HOOK_PRIO_TEMP_SENSOR_DONE = HOOK_PRIO_TEMP_SENSOR + 1,
+
+	/* Once devices are initialized, run FIPS tests and print status. */
+	HOOK_PRIO_INIT_FIPS = 9997,    /* FIPS init executes last */
+	HOOK_PRIO_INIT_PRINT_FIPS_STATUS = 9998,    /* Status of FIPS tests. */
+
 };
 
 enum hook_type {