diff --git a/src/security/tpm/tspi/tspi.c b/src/security/tpm/tspi/tspi.c
index 966b8b7c77..795016e725 100644
--- a/src/security/tpm/tspi/tspi.c
+++ b/src/security/tpm/tspi/tspi.c
@@ -210,11 +210,28 @@ uint32_t tpm_extend_pcr(int pcr, enum vb2_hash_algorithm digest_algo,
 			uint8_t *digest, size_t digest_len, const char *name)
 {
 	uint32_t result;
+	uint16_t algorithm = 0;
 
 	if (!digest)
 		return TPM_E_IOERROR;
 
-	result = tlcl_extend(pcr, digest, NULL);
+#if CONFIG(TPM2)
+	switch (digest_algo) {
+	case VB2_HASH_SHA1:
+		algorithm = TPM_ALG_SHA1;
+		break;
+	case VB2_HASH_SHA256:
+		algorithm = TPM_ALG_SHA256;
+		break;
+	case VB2_HASH_SHA512:
+		algorithm = TPM_ALG_SHA512;
+		break;
+	default:
+		return TPM_E_HASH_ERROR;
+	}
+#endif
+
+	result = tlcl_extend(pcr, algorithm, digest, digest_len, NULL);
 	if (result != TPM_SUCCESS)
 		return result;
 
diff --git a/src/security/tpm/tss.h b/src/security/tpm/tss.h
index 336935d911..c1ba234c2c 100644
--- a/src/security/tpm/tss.h
+++ b/src/security/tpm/tss.h
@@ -184,7 +184,8 @@ uint32_t tlcl_lock_nv_write(uint32_t index);
 /**
  * Perform a TPM_Extend.
  */
-uint32_t tlcl_extend(int pcr_num, const uint8_t *in_digest,
+uint32_t tlcl_extend(int pcr_num,  uint16_t algorithm,
+		     const uint8_t *in_digest, size_t in_digest_len,
 		     uint8_t *out_digest);
 
 /**
diff --git a/src/security/tpm/tss/tcg-1.2/tss.c b/src/security/tpm/tss/tcg-1.2/tss.c
index b11d6a3d16..4c980d0d79 100644
--- a/src/security/tpm/tss/tcg-1.2/tss.c
+++ b/src/security/tpm/tss/tcg-1.2/tss.c
@@ -341,7 +341,8 @@ uint32_t tlcl_set_global_lock(void)
 	return tlcl_write(TPM_NV_INDEX0, (uint8_t *) &x, 0);
 }
 
-uint32_t tlcl_extend(int pcr_num, const uint8_t *in_digest,
+uint32_t tlcl_extend(int pcr_num,  uint16_t algorithm,
+		     const uint8_t *in_digest, size_t in_digest_len,
 		     uint8_t *out_digest)
 {
 	struct s_tpm_extend_cmd cmd;
@@ -350,8 +351,11 @@ uint32_t tlcl_extend(int pcr_num, const uint8_t *in_digest,
 
 	memcpy(&cmd, &tpm_extend_cmd, sizeof(cmd));
 	to_tpm_uint32(cmd.buffer + tpm_extend_cmd.pcrNum, pcr_num);
-	memcpy(cmd.buffer + cmd.inDigest, in_digest, kPcrDigestLength);
 
+	if (in_digest_len != kPcrDigestLength)
+		return TPM_E_HASH_ERROR;
+
+	memcpy(cmd.buffer + cmd.inDigest, in_digest, kPcrDigestLength);
 	result = tlcl_send_receive(cmd.buffer, response, sizeof(response));
 	if (result != TPM_SUCCESS)
 		return result;
diff --git a/src/security/tpm/tss/tcg-2.0/tss.c b/src/security/tpm/tss/tcg-2.0/tss.c
index 16e40fe569..ac43549d84 100644
--- a/src/security/tpm/tss/tcg-2.0/tss.c
+++ b/src/security/tpm/tss/tcg-2.0/tss.c
@@ -127,21 +127,47 @@ uint32_t tlcl_assert_physical_presence(void)
 }
 
 /*
- * The caller will provide the digest in a 32 byte buffer, let's consider it a
- * sha256 digest.
+ * The caller will provide the pcr index, digest algorithm and
+ * a byte buffer to extend into the TPM.
  */
-uint32_t tlcl_extend(int pcr_num, const uint8_t *in_digest,
+uint32_t tlcl_extend(int pcr_num, uint16_t algorithm,
+		     const uint8_t *in_digest, size_t in_digest_len,
 		     uint8_t *out_digest)
 {
 	struct tpm2_pcr_extend_cmd pcr_ext_cmd;
 	struct tpm2_response *response;
+	uint16_t algorithm_size;
 
 	pcr_ext_cmd.pcrHandle = HR_PCR + pcr_num;
 	pcr_ext_cmd.digests.count = 1;
-	pcr_ext_cmd.digests.digests[0].hashAlg = TPM_ALG_SHA256;
-	memcpy(pcr_ext_cmd.digests.digests[0].digest.sha256, in_digest,
-	       sizeof(pcr_ext_cmd.digests.digests[0].digest.sha256));
+	pcr_ext_cmd.digests.digests[0].hashAlg = algorithm;
+	algorithm_size = tlcl_get_hash_size_from_algo(algorithm);
 
+	if (algorithm_size == 0)
+		return TPM_E_HASH_ERROR;
+
+	if (in_digest_len != algorithm_size)
+		return TPM_E_HASH_ERROR;
+
+	switch (algorithm) {
+	case TPM_ALG_SHA1:
+		memcpy(pcr_ext_cmd.digests.digests[0].digest.sha1, in_digest, in_digest_len);
+		break;
+	case TPM_ALG_SHA256:
+		memcpy(pcr_ext_cmd.digests.digests[0].digest.sha256, in_digest, in_digest_len);
+		break;
+	case TPM_ALG_SHA384:
+		memcpy(pcr_ext_cmd.digests.digests[0].digest.sha384, in_digest, in_digest_len);
+		break;
+	case TPM_ALG_SHA512:
+		memcpy(pcr_ext_cmd.digests.digests[0].digest.sha512, in_digest, in_digest_len);
+		break;
+	case TPM_ALG_SM3_256:
+		memcpy(pcr_ext_cmd.digests.digests[0].digest.sm3_256, in_digest, in_digest_len);
+		break;
+	default:
+		return TPM_E_HASH_ERROR;
+	}
 	response = tpm_process_command(TPM2_PCR_Extend, &pcr_ext_cmd);
 
 	printk(BIOS_INFO, "%s: response is %x\n",
diff --git a/src/security/vboot/tpm_common.c b/src/security/vboot/tpm_common.c
index 0a211c57d4..1db7189d7a 100644
--- a/src/security/vboot/tpm_common.c
+++ b/src/security/vboot/tpm_common.c
@@ -46,7 +46,7 @@ vb2_error_t vboot_extend_pcr(struct vb2_context *ctx, int pcr,
 	switch (which_digest) {
 	/* SHA1 of (devmode|recmode|keyblock) bits */
 	case BOOT_MODE_PCR:
-		return tpm_extend_pcr(pcr, VB2_HASH_SHA256, buffer, size,
+		return tpm_extend_pcr(pcr, VB2_HASH_SHA1, buffer, size,
 				      TPM_PCR_BOOT_MODE);
 	 /* SHA256 of HWID */
 	case HWID_DIGEST_PCR:
diff --git a/src/vendorcode/eltan/security/mboot/mboot.c b/src/vendorcode/eltan/security/mboot/mboot.c
index c5523a5fd8..499d35299c 100644
--- a/src/vendorcode/eltan/security/mboot/mboot.c
+++ b/src/vendorcode/eltan/security/mboot/mboot.c
@@ -150,7 +150,8 @@ int mboot_hash_extend_log(uint64_t flags, uint8_t *hashData, uint32_t hashDataLe
 	printk(BIOS_DEBUG, "%s: SHA256 Hash Digest:\n", __func__);
 	mboot_print_buffer(digest->digest.sha256, VB2_SHA256_DIGEST_SIZE);
 
-	return (tlcl_extend(newEventHdr->pcrIndex, (uint8_t *)&(newEventHdr->digest), NULL));
+	return (tlcl_extend(newEventHdr->pcrIndex, newEventHdr->digest.digests[0].hashAlg,
+			(uint8_t *)&(newEventHdr->digest), hashDataLen, NULL));
 }
 
 /*