Fixed MTP to work with TWRP

2025-09-10 01:12:45 -04:00 · 2018-06-19 23:16:04 +02:00 · 2018-06-19 23:16:04 +02:00 · f6dfaef42e
commit f6dfaef42e
50820 changed files with 20846062 additions and 0 deletions
--- a/drivers/crypto/fmp/Kconfig
+++ b/drivers/crypto/fmp/Kconfig
@ -0,0 +1,48 @@
+config FMP_UFS
+	tristate "Samsung EXYNOS FMP Driver for UFS"
+	depends on SCSI_UFSHCD && SCSI_UFSHCD_PLATFORM
+	---help---
+	  This selects EXYNOS FMP UFS driver.
+
+	  If you have a controller with this interface, say Y here.
+
+	  If unsure, say N.
+
+config FMP_MMC
+	tristate "Samsung EXYNOS FMP Driver for MMC"
+	depends on MMC_DW
+	---help---
+	  This selects EXYNOS FMP MMC driver.
+
+	  If you have a controller with this interface, say Y here.
+
+	  If unsure, say N.
+
+config FIPS_FMP
+	tristate "Samsung EXYNOS FMP Validation Driver for FIPS"
+	depends on FMP_UFS
+	---help---
+	  This selects Samsung EXYNOS FMP Validation driver for FIPS.
+
+	  If you have a controller with this interface, say Y here.
+
+	  If unsure, say N.
+
+choice
+	prompt "Option for self-test failure"
+	depends on FIPS_FMP
+	default NODE_FOR_SELFTEST_FAIL
+
+config NODE_FOR_SELFTEST_FAIL
+	bool "Set fips fmp node when self-test fails"
+	depends on FIPS_FMP
+	help
+	   This select that fips fmp node was set to zero when FMP self-test fails.
+
+config PANIC_FOR_SELFTEST_FAIL
+	bool "Panic when self-test fails"
+	depends on FIPS_FMP
+	help
+	   This select that kernel panic occurs when FMP self-test fails.
+
+endchoice
--- a/drivers/crypto/fmp/Makefile
+++ b/drivers/crypto/fmp/Makefile
@ -0,0 +1,14 @@
+# Exynos FMP makefile
+obj-$(CONFIG_FIPS_FMP) += first_file.o
+obj-$(CONFIG_FMP_UFS) += fmp_ufs.o
+obj-$(CONFIG_FIPS_FMP) += fmpdev.o fmp_integrity.o fmplib.o
+obj-$(CONFIG_FIPS_FMP) += hmac_fmp.o sha256_fmp.o
+obj-$(CONFIG_FIPS_FMP) += fmp_ufs_fips.o
+obj-$(CONFIG_UFS_FMP_ECRYPT_FS) += fmp_derive_iv.o
+obj-$(CONFIG_MMC_DW_FMP_ECRYPT_FS) += fmp_derive_iv.o
+obj-$(CONFIG_FIPS_FMP) += last_file.o
+
+obj-$(CONFIG_FMP_MMC) += fmp_mmc.o
+
+ccflags-$(CONFIG_FMP_UFS) := -Idrivers/scsi/ufs
+ccflags-$(CONFIG_FMP_MMC) := -Idrivers/mmc/host
--- a/drivers/crypto/fmp/first_file.c
+++ b/drivers/crypto/fmp/first_file.c
@ -0,0 +1,41 @@
+/*
+ * First file for Exynos FMP FIPS integrity check
+ *
+ * Copyright (C) 2015 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#include <linux/init.h>
+
+/* Keep this on top */
+static const char
+builtime_fmp_hmac[] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
+                          0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f};
+
+const int first_fmp_rodata = 10;
+int first_fmp_data = 20;
+
+
+void first_fmp_text(void) __attribute__((unused));
+void first_fmp_text(void)
+{
+}
+
+const char *get_builtime_fmp_hmac(void)
+{
+	return builtime_fmp_hmac;
+}
+
+void __init first_fmp_init(void) __attribute__((unused));
+void __init first_fmp_init(void)
+{
+}
+
+void __exit first_fmp_exit(void) __attribute__((unused));
+void __exit first_fmp_exit(void)
+{
+}
--- a/drivers/crypto/fmp/fmp_derive_iv.c
+++ b/drivers/crypto/fmp/fmp_derive_iv.c
@ -0,0 +1,148 @@
+/*
+ * Exynos FMP derive iv for eCryptfs
+ *
+ * Copyright (C) 2014 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#include <linux/crypto.h>
+#include <linux/scatterlist.h>
+#include <linux/errno.h>
+#include <linux/err.h>
+#include <linux/fs.h>
+
+#define FMP_MAX_IV_BYTES		16
+#define FMP_MAX_OFFSET_BYTES		16
+#define DEFAULT_HASH			"md5"
+#define SHA256_HASH			"sha256-fmp"
+
+static DEFINE_SPINLOCK(fmp_tfm_lock);
+
+int calculate_sha256(struct crypto_hash *hash_tfm, char *dst, char *src, int len)
+{
+	int ret = -1;
+	unsigned long flag;
+	struct scatterlist sg;
+	struct hash_desc desc = {
+		.tfm = hash_tfm,
+		.flags = 0
+	};
+
+	spin_lock_irqsave(&fmp_tfm_lock, flag);
+	sg_init_one(&sg, (u8 *)src, len);
+
+	if (!desc.tfm) {
+		desc.tfm = crypto_alloc_hash(SHA256_HASH, 0,
+					     CRYPTO_ALG_ASYNC);
+		if (IS_ERR(desc.tfm)) {
+			printk(KERN_ERR "%s: Error attemping to allocate crypto context\n", __func__);
+			goto out;
+		}
+		hash_tfm = desc.tfm;
+	}
+
+	ret = crypto_hash_init(&desc);
+	if (ret) {
+		printk(KERN_ERR
+		       "%s: Error initializing crypto hash; ret = [%d]\n",
+		       __func__, ret);
+		goto out;
+	}
+	ret = crypto_hash_update(&desc, &sg, len);
+	if (ret) {
+		printk(KERN_ERR
+		       "%s: Error updating crypto hash; ret = [%d]\n",
+		       __func__, ret);
+		goto out;
+	}
+	ret = crypto_hash_final(&desc, dst);
+	if (ret) {
+		printk(KERN_ERR
+		       "%s: Error finalizing crypto hash; ret = [%d]\n",
+		       __func__, ret);
+		goto out;
+	}
+
+out:
+	spin_unlock_irqrestore(&fmp_tfm_lock, flag);
+	return ret;
+}
+
+int calculate_md5(struct crypto_hash *hash_tfm, char *dst, char *src, int len)
+{
+	int ret = -1;
+	unsigned long flag;
+	struct scatterlist sg;
+	struct hash_desc desc = {
+		.tfm = hash_tfm,
+		.flags = 0
+	};
+
+	spin_lock_irqsave(&fmp_tfm_lock, flag);
+	sg_init_one(&sg, (u8 *)src, len);
+
+	if (!desc.tfm) {
+		desc.tfm = crypto_alloc_hash(DEFAULT_HASH, 0,
+					     CRYPTO_ALG_ASYNC);
+		if (IS_ERR(desc.tfm)) {
+			printk(KERN_ERR "%s: Error attemping to allocate crypto context\n", __func__);
+			goto out;
+		}
+		hash_tfm = desc.tfm;
+	}
+
+	ret = crypto_hash_init(&desc);
+	if (ret) {
+		printk(KERN_ERR
+		       "%s: Error initializing crypto hash; ret = [%d]\n",
+		       __func__, ret);
+		goto out;
+	}
+	ret = crypto_hash_update(&desc, &sg, len);
+	if (ret) {
+		printk(KERN_ERR
+		       "%s: Error updating crypto hash; ret = [%d]\n",
+		       __func__, ret);
+		goto out;
+	}
+	ret = crypto_hash_final(&desc, dst);
+	if (ret) {
+		printk(KERN_ERR
+		       "%s: Error finalizing crypto hash; ret = [%d]\n",
+		       __func__, ret);
+		goto out;
+	}
+
+out:
+	spin_unlock_irqrestore(&fmp_tfm_lock, flag);
+	return ret;
+}
+
+int file_enc_derive_iv(struct address_space *mapping, loff_t offset, char *extent_iv)
+{
+	char src[FMP_MAX_IV_BYTES + FMP_MAX_OFFSET_BYTES];
+	int ret;
+
+	memcpy(src, mapping->iv, FMP_MAX_IV_BYTES);
+	memset(src + FMP_MAX_IV_BYTES, 0, FMP_MAX_OFFSET_BYTES);
+	snprintf(src + FMP_MAX_IV_BYTES, FMP_MAX_OFFSET_BYTES, "%lld", offset);
+
+#ifdef CONFIG_CRYPTO_FIPS
+	if (mapping->cc_enable)
+		ret = calculate_sha256(mapping->hash_tfm, extent_iv, src, FMP_MAX_IV_BYTES + FMP_MAX_OFFSET_BYTES);
+	else
+#endif
+		ret = calculate_md5(mapping->hash_tfm, extent_iv, src, FMP_MAX_IV_BYTES + FMP_MAX_OFFSET_BYTES);
+	if (ret) {
+		printk(KERN_ERR "%s: Error attempting to compute generating IV for a page; ret = [%d]\n", __func__, ret);
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
--- a/drivers/crypto/fmp/fmp_derive_iv.h
+++ b/drivers/crypto/fmp/fmp_derive_iv.h
@ -0,0 +1,22 @@
+/*
+ * Exynos FMP derive iv for eCryptfs
+ *
+ * Copyright (C) 2015 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#ifndef _FMP_DERIVE_IV_H_
+#define _FMP_DERIVE_IV_H_
+
+#define SHA256_HASH_SIZE	32
+#define MD5_DIGEST_SIZE		16
+
+int calculate_sha256(struct crypto_hash *hash_tfm, char *dst, char *src, int len);
+int calculate_md5(struct crypto_hash *hash_tfm, char *dst, char *src, int len);
+int file_enc_derive_iv(struct address_space *mapping, loff_t offset, char *extent_iv);
+
+#endif
--- a/drivers/crypto/fmp/fmp_integrity.c
+++ b/drivers/crypto/fmp/fmp_integrity.c
@ -0,0 +1,283 @@
+/*
+ * Perform FIPS Integrity test on Kernel Crypto API
+ *
+ * At build time, hmac(sha256) of crypto code, avaiable in different ELF sections
+ * of vmlinux file, is generated. vmlinux file is updated with built-time hmac
+ * in a read-only data variable, so that it is available at run-time
+ *
+ * At run time, hmac(sha256) is again calculated using crypto bytes of a running
+ * At run time, hmac-fmp(sha256-fmp) is again calculated using crypto bytes of a running
+ * kernel.
+ * Run time hmac is compared to built time hmac to verify the integrity.
+ *
+ *
+ * Author : Rohit Kothari (r.kothari@samsung.com)
+ * Date	  : 11 Feb 2014
+ *
+ * Copyright (c) 2014 Samsung Electronics
+ *
+ */
+
+#include <linux/crypto.h>
+#include <linux/kallsyms.h>
+#include <linux/err.h>
+#include <linux/scatterlist.h>
+#include <linux/smc.h>
+#include "fmpdev_int.h" //for FIPS_FMP_FUNC_TEST macro
+
+//#define FIPS_DEBUG
+
+static const char *symtab[][3] =
+		{{".text",	"first_fmp_text",	"last_fmp_text"  },
+	      {".rodata",	"first_fmp_rodata",	"last_fmp_rodata"},
+	      {".init.text",	"first_fmp_init",	"last_fmp_init"  }};
+
+extern const char *get_builtime_fmp_hmac(void);
+
+#ifdef FIPS_DEBUG
+static int
+dump_bytes(const char *section_name, const char *first_symbol, const char *last_symbol)
+{
+	u8 *start_addr = (u8 *)kallsyms_lookup_name(first_symbol);
+	u8 *end_addr   = (u8 *)kallsyms_lookup_name(last_symbol);
+
+	if (!start_addr || !end_addr || start_addr >= end_addr) {
+		printk(KERN_ERR "FIPS(%s): Error Invalid Addresses in Section : %s, Start_Addr : %p , End_Addr : %p",
+                       __FUNCTION__, section_name, start_addr, end_addr);
+		return -1;
+	}
+
+	printk(KERN_INFO "FIPS CRYPTO RUNTIME : Section - %s, %s : %p, %s : %p \n", section_name, first_symbol, start_addr, last_symbol, end_addr);
+	print_hex_dump_bytes("FIPS CRYPTO RUNTIME : ", DUMP_PREFIX_NONE, start_addr, end_addr - start_addr);
+
+	return 0;
+}
+#endif
+
+static int query_symbol_addresses(const char *first_symbol, const char *last_symbol,
+				unsigned long *start_addr,unsigned long *end_addr)
+{
+	unsigned long start = kallsyms_lookup_name(first_symbol);
+	unsigned long end = kallsyms_lookup_name(last_symbol);
+
+#ifdef FIPS_DEBUG
+	printk(KERN_INFO "FIPS CRYPTO RUNTIME : %s : %p, %s : %p\n", first_symbol, (u8*)start, last_symbol, (u8*)end);
+#endif
+
+	if (!start || !end || start >= end) {
+		printk(KERN_ERR "FIPS(%s): Error Invalid Addresses.", __FUNCTION__);
+		return -1;
+	}
+
+	*start_addr = start;
+	*end_addr = end;
+
+	return 0;
+}
+
+static int init_hash(struct hash_desc *desc)
+{
+	struct crypto_hash *tfm = NULL;
+	int ret = -1;
+
+	/* Same as build time */
+	const unsigned char *key = "The quick brown fox jumps over the lazy dog";
+
+	tfm = crypto_alloc_hash("hmac-fmp(sha256-fmp)", 0, 0);
+	if (IS_ERR(tfm)) {
+		printk(KERN_ERR "FIPS(%s): integrity failed to allocate tfm %ld", __FUNCTION__, PTR_ERR(tfm));
+		return -1;
+	}
+
+	ret = crypto_hash_setkey (tfm, key, strlen(key));
+	if (ret) {
+		printk(KERN_ERR "FIPS(%s): fail at crypto_hash_setkey", __FUNCTION__);
+		return -1;
+	}
+
+	desc->tfm = tfm;
+	desc->flags = 0;
+
+	ret = crypto_hash_init(desc);
+	if (ret) {
+		printk(KERN_ERR "FIPS(%s): fail at crypto_hash_init", __FUNCTION__);
+		return -1;
+	}
+
+	return 0;
+}
+
+static int finalize_hash(struct hash_desc *desc, unsigned char *out, unsigned int out_size)
+{
+	int ret = -1;
+
+	if (!desc || !desc->tfm || !out || !out_size) {
+		printk(KERN_ERR "FIPS(%s): Invalid args", __FUNCTION__);
+		return ret;
+	}
+
+	if (crypto_hash_digestsize(desc->tfm) > out_size) {
+		printk(KERN_ERR "FIPS(%s): Not enough space for digest", __FUNCTION__);
+		return ret;
+	}
+
+	ret = crypto_hash_final(desc, out);
+	if (ret) {
+		printk(KERN_ERR "FIPS(%s): crypto_hash_final failed", __FUNCTION__);
+		return -1;
+	}
+
+	return 0;
+}
+
+static int update_hash(struct hash_desc *desc, unsigned char *start_addr, unsigned int size)
+{
+	struct scatterlist sg;
+	unsigned char *buf = NULL;
+	unsigned char *cur = NULL;
+	unsigned int bytes_remaining;
+	unsigned int bytes;
+	int ret = -1;
+#if FIPS_FMP_FUNC_TEST == 5
+	static int total = 0;
+#endif
+
+	buf = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	if (!buf) {
+		printk(KERN_ERR "FIPS(%s): kmalloc failed", __FUNCTION__);
+		return ret;
+	}
+
+	bytes_remaining = size;
+	cur = start_addr;
+
+	while (bytes_remaining > 0) {
+		if (bytes_remaining >= PAGE_SIZE)
+			bytes = PAGE_SIZE;
+		else
+			bytes = bytes_remaining;
+
+		memcpy(buf, cur, bytes);
+
+		sg_init_one(&sg, buf, bytes);
+
+#if FIPS_FMP_FUNC_TEST == 5
+		if (total == 0) {
+			printk(KERN_INFO "FIPS : Failing Integrity Test");
+			buf[bytes / 2] += 1;
+		}
+#endif
+
+		ret = crypto_hash_update(desc, &sg, bytes);
+		if (ret) {
+			printk(KERN_ERR "FIPS(%s): crypto_hash_update failed", __FUNCTION__);
+			kfree(buf);
+			buf = 0;
+			return -1;
+		}
+
+		cur += bytes;
+		bytes_remaining -= bytes;
+
+#if FIPS_FMP_FUNC_TEST == 5
+		total += bytes;
+#endif
+	}
+
+	if (buf) {
+		kfree(buf);
+		buf = 0;
+        }
+
+	return 0;
+}
+
+int do_fmp_fw_integrity_check(void)
+{
+	int err = 0;
+
+	err = exynos_smc(SMC_CMD_FMP, FMP_FW_INTEGRITY, 0, 0);
+	if (err) {
+		printk(KERN_ERR "Fail to check integrity for FMP F/W. err = 0x%x\n", err);
+		return -1;
+	}
+
+	return 0;
+}
+
+int do_fips_fmp_integrity_check(void)
+{
+	int i, rows, err;
+	unsigned long start_addr = 0;
+	unsigned long end_addr = 0;
+	unsigned char runtime_hmac[32];
+	struct hash_desc desc;
+	const char *builtime_hmac = 0;
+	unsigned int size = 0;
+
+	err = do_fmp_fw_integrity_check();
+	if (err) {
+		printk(KERN_ERR "FIPS(%s): FMP FW Integrity Check Failed", __FUNCTION__);
+		return -1;
+	}
+
+	err = init_hash(&desc);
+	if (err) {
+		printk(KERN_ERR "FIPS(%s): init_hash failed", __FUNCTION__);
+		return -1;
+	}
+
+	rows = (unsigned int)sizeof(symtab) / sizeof(symtab[0]);
+
+	for (i = 0; i < rows; i++) {
+		err = query_symbol_addresses(symtab[i][1], symtab[i][2], &start_addr, &end_addr);
+		if (err) {
+			printk (KERN_ERR "FIPS(%s): Error to get start / end addresses", __FUNCTION__);
+			crypto_free_hash(desc.tfm);
+			return -1;
+		}
+
+#ifdef FIPS_DEBUG
+		dump_bytes(symtab[i][0], symtab[i][1], symtab[i][2]);
+#endif
+		size = end_addr - start_addr;
+
+		err = update_hash(&desc, (unsigned char *)start_addr, size);
+		if (err) {
+			printk(KERN_ERR "FIPS(%s): Error to update hash", __FUNCTION__);
+			crypto_free_hash(desc.tfm);
+			return -1;
+		}
+	}
+
+	err = finalize_hash(&desc, runtime_hmac, sizeof(runtime_hmac));
+	if (err) {
+		printk(KERN_ERR "FIPS(%s): Error in finalize", __FUNCTION__);
+		crypto_free_hash(desc.tfm);
+		return -1;
+	}
+
+	crypto_free_hash(desc.tfm);
+	builtime_hmac = get_builtime_fmp_hmac();
+	if (!builtime_hmac) {
+		printk(KERN_ERR "FIPS(%s): Unable to retrieve builtime_hmac", __FUNCTION__);
+		return -1;
+	}
+
+#ifdef FIPS_DEBUG
+	print_hex_dump_bytes("FIPS CRYPTO RUNTIME : runtime hmac  = ", DUMP_PREFIX_NONE, runtime_hmac, sizeof(runtime_hmac));
+	print_hex_dump_bytes("FIPS CRYPTO RUNTIME : builtime_hmac = ", DUMP_PREFIX_NONE, builtime_hmac , sizeof(runtime_hmac));
+#endif
+
+	if (!memcmp(builtime_hmac, runtime_hmac, sizeof(runtime_hmac))) {
+		printk(KERN_INFO "FIPS: Integrity Check Passed");
+		return 0;
+	} else {
+		printk(KERN_ERR "FIPS(%s): Integrity Check Failed", __FUNCTION__);
+		return -1;
+	}
+
+	return -1;
+}
+
+EXPORT_SYMBOL_GPL(do_fips_fmp_integrity_check);
--- a/drivers/crypto/fmp/fmp_integrity.h
+++ b/drivers/crypto/fmp/fmp_integrity.h
@ -0,0 +1,17 @@
+/*
+ * Exynos FMP integrity header
+ *
+ * Copyright (C) 2015 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#ifndef __FMP_INTEGRITY_H__
+#define __FMP_INTEGRITY_H__
+
+int do_fips_fmp_integrity_check(void);
+
+#endif
--- a/drivers/crypto/fmp/fmp_mmc.c
+++ b/drivers/crypto/fmp/fmp_mmc.c
@ -0,0 +1,127 @@
+/*
+ * Exynos FMP MMC driver
+ *
+ * Copyright (C) 2015 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#include <linux/pagemap.h>
+#include <linux/mmc/dw_mmc.h>
+#include <linux/smc.h>
+
+#include "dw_mmc-exynos.h"
+#if defined(CONFIG_MMC_DW_FMP_ECRYPT_FS)
+#include "fmp_derive_iv.h"
+#endif
+
+extern volatile unsigned int disk_key_flag;
+extern spinlock_t disk_key_lock;
+
+#define byte2word(b0, b1, b2, b3) 	\
+		((unsigned int)(b0) << 24) | ((unsigned int)(b1) << 16) | ((unsigned int)(b2) << 8) | (b3)
+#define word_in(x, c)           byte2word(((unsigned char *)(x) + 4 * (c))[0], ((unsigned char *)(x) + 4 * (c))[1], \
+					((unsigned char *)(x) + 4 * (c))[2], ((unsigned char *)(x) + 4 * (c))[3])
+
+int fmp_map_sg(struct dw_mci *host, struct idmac_desc_64addr *desc, int idx,
+			uint32_t sector_key, uint32_t sector, struct mmc_data *data)
+{
+#if defined(CONFIG_MMC_DW_FMP_DM_CRYPT)
+	if ((sector_key & DW_MMC_ENCRYPTION_SECTOR_BEGIN) &&
+			(host->pdata->quirks & DW_MCI_QUIRK_USE_SMU)) { /* disk encryption */
+		int ret;
+
+		/* disk algorithm selector  */
+		IDMAC_SET_DAS(desc, AES_XTS);
+		desc->des2 |= IDMAC_DES2_DKL;
+
+		/* Disk IV */
+		desc->des28 = 0;
+		desc->des29 = 0;
+		desc->des30 = 0;
+		desc->des31 = htonl(sector);
+
+		/* Disk Enc Key, Tweak Key */
+		if (disk_key_flag) {
+			/* Disk Enc Key, Tweak Key */
+			ret = exynos_smc(SMC_CMD_FMP, FMP_KEY_SET, EMMC0_FMP, 0);
+			if (ret) {
+				printk(KERN_ERR "Failed to smc call for FMP key setting: %x\n", ret);
+				return ret;
+			}
+			spin_lock(&disk_key_lock);
+			disk_key_flag = 0;
+			spin_unlock(&disk_key_lock);
+		}
+	}
+#endif
+#if defined(CONFIG_MMC_DW_FMP_ECRYPT_FS)
+	if ((sector_key & DW_MMC_FILE_ENCRYPTION_SECTOR_BEGIN) &&
+			(host->pdata->quirks & DW_MCI_QUIRK_USE_SMU)) { /* file encryption */
+		int ret;
+		unsigned int aes_alg = 0;
+		unsigned int j;
+		unsigned int last_index = 0;
+		unsigned long last_inode = 0;
+#ifdef CONFIG_CRYPTO_FIPS
+		char extent_iv[SHA256_HASH_SIZE];
+#else
+		char extent_iv[MD5_DIGEST_SIZE];
+#endif
+		loff_t index;
+
+		/* File algorithm selector*/
+		if (!strncmp(sg_page(&data->sg[idx])->mapping->alg, "aes", sizeof("aes")))
+			aes_alg = AES_CBC;
+		else if (!strncmp(sg_page(&data->sg[idx])->mapping->alg, "aesxts", sizeof("aesxts")))
+			aes_alg = AES_XTS;
+		else {
+			printk(KERN_ERR "Invalid file algorithm: %s\n", sg_page(&data->sg[idx])->mapping->alg);
+			return -1;
+		}
+
+		IDMAC_SET_FAS(desc, aes_alg);
+
+		/* File enc key size */
+		switch (sg_page(&data->sg[idx])->mapping->key_length) {
+		case 16:
+			desc->des2 &= ~IDMAC_DES2_FKL;
+			break;
+		case 32:
+		case 64:
+			desc->des2 |= IDMAC_DES2_FKL;
+			break;
+		default:
+			printk(KERN_ERR "Invalid file key length: %lx\n", sg_page(&data->sg[idx])->mapping->key_length);
+			return -1;
+		}
+
+		index = sg_page(&data->sg[idx])->index;
+		if ((last_index != index) || (last_inode != sg_page(&data->sg[idx])->mapping->host->i_ino)) {
+			index = index - sg_page(&data->sg[idx])->mapping->sensitive_data_index;
+			ret = file_enc_derive_iv(sg_page(&data->sg[idx])->mapping, index, extent_iv);
+			if (ret) {
+				printk(KERN_ERR "Error attemping to derive IV\n");
+				return ret;
+			}
+		}
+		last_index = sg_page(&data->sg[idx])->index;
+		last_inode = sg_page(&data->sg[idx])->mapping->host->i_ino;
+
+		/* File IV */
+		desc->des8 = word_in(extent_iv, 3);
+		desc->des9 = word_in(extent_iv, 2);
+		desc->des10 = word_in(extent_iv, 1);
+		desc->des11 = word_in(extent_iv, 0);
+
+		/* File Enc key */
+		for (j = 0; j < sg_page(&data->sg[idx])->mapping->key_length >> 2; j++)
+			*(&(desc->des12) + j) =
+				word_in(sg_page(&data->sg[idx])->mapping->key, (sg_page(&data->sg[idx])->mapping->key_length >> 2) - (j + 1));
+	}
+#endif
+	return 0;
+}
--- a/drivers/crypto/fmp/fmp_ufs.c
+++ b/drivers/crypto/fmp/fmp_ufs.c
@ -0,0 +1,289 @@
+/*
+ * Exynos FMP UFS driver for FIPS
+ *
+ * Copyright (C) 2014 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/syscalls.h>
+#include <linux/smc.h>
+#include <ufshcd.h>
+#include <ufs-exynos.h>
+
+#if defined(CONFIG_UFS_FMP_ECRYPT_FS)
+#include <linux/pagemap.h>
+#include "fmp_derive_iv.h"
+#endif
+
+#if defined(CONFIG_FIPS_FMP)
+#include "fmpdev_info.h"
+#endif
+
+#include "fmpdev_int.h" //For FIPS_FMP_FUNC_TEST macro
+
+extern bool in_fmp_fips_err(void);
+extern volatile unsigned int disk_key_flag;
+extern spinlock_t disk_key_lock;
+extern void exynos_ufs_ctrl_hci_core_clk(struct exynos_ufs *ufs, bool en);
+
+#define byte2word(b0, b1, b2, b3) 	\
+		((unsigned int)(b0) << 24) | ((unsigned int)(b1) << 16) | ((unsigned int)(b2) << 8) | (b3)
+#define word_in(x, c)           byte2word(((unsigned char *)(x) + 4 * (c))[0], ((unsigned char *)(x) + 4 * (c))[1], \
+					((unsigned char *)(x) + 4 * (c))[2], ((unsigned char *)(x) + 4 * (c))[3])
+
+int fmp_map_sg(struct ufshcd_sg_entry *prd_table, struct scatterlist *sg,
+					uint32_t sector_key, uint32_t idx,
+					uint32_t sector)
+{
+#if defined(CONFIG_FIPS_FMP)
+	if (unlikely(in_fmp_fips_err())) {
+		printk(KERN_ERR "Fail to work fmp due to fips in error\n");
+		return -EPERM;
+	}
+#endif
+
+#if defined(CONFIG_UFS_FMP_DM_CRYPT)
+	/* Disk Encryption */
+	if (sector_key & UFS_ENCRYPTION_SECTOR_BEGIN) {
+		/* algorithm */
+#if defined(CONFIG_FIPS_FMP)
+		if (prd_table[idx].size > 0x1000) {
+			printk(KERN_ERR "Fail to FMP XTS due to invalid size \
+					for Disk Encrytion. size = 0x%x\n", prd_table[idx].size);
+			return -EINVAL;
+		}
+#endif
+		SET_DAS(&prd_table[idx], AES_XTS);
+		prd_table[idx].size |= DKL;
+
+		/* disk IV */
+		prd_table[idx].disk_iv0 = 0;
+		prd_table[idx].disk_iv1 = 0;
+		prd_table[idx].disk_iv2 = 0;
+		prd_table[idx].disk_iv3 = htonl(sector);
+
+		if (disk_key_flag) {
+			int ret;
+
+			if (disk_key_flag == 1)
+				printk(KERN_INFO "FMP disk encryption key is set\n");
+			else if (disk_key_flag == 2)
+				printk(KERN_INFO "FMP disk encryption key is set after clear\n");
+			ret = exynos_smc(SMC_CMD_FMP, FMP_KEY_SET, UFS_FMP, 0);
+			if (ret < 0)
+				panic("Fail to load FMP loadable firmware\n");
+			else if (ret) {
+				printk(KERN_ERR "Fail to smc call for FMP key setting. ret = 0x%x\n", ret);
+				return ret;
+			}
+
+			spin_lock(&disk_key_lock);
+			disk_key_flag = 0;
+			spin_unlock(&disk_key_lock);
+		}
+	}
+#endif
+
+#if defined(CONFIG_UFS_FMP_ECRYPT_FS)
+	/* File Encryption */
+	if (sector_key & UFS_FILE_ENCRYPTION_SECTOR_BEGIN) {
+		int ret;
+		unsigned int aes_alg, j;
+		loff_t index;
+#ifdef CONFIG_CRYPTO_FIPS
+		char extent_iv[SHA256_HASH_SIZE];
+#else
+		char extent_iv[MD5_DIGEST_SIZE];
+#endif
+
+		/* algorithm */
+		if (!strncmp(sg_page(sg)->mapping->alg, "aes", sizeof("aes")))
+			aes_alg = AES_CBC;
+		else if (!strncmp(sg_page(sg)->mapping->alg, "aesxts", sizeof("aesxts"))) {
+			aes_alg = AES_XTS;
+#if defined(CONFIG_FIPS_FMP)
+			if (prd_table[idx].size > 0x1000) {
+				printk(KERN_ERR "Fail to FMP XTS due to invalid size \
+						for File Encrytion. size = 0x%x\n", prd_table[idx].size);
+				return -EINVAL;
+			}
+#endif
+		} else {
+			printk(KERN_ERR "Invalid file encryption algorithm %s \n", sg_page(sg)->mapping->alg);
+			return -EINVAL;
+		}
+		SET_FAS(&prd_table[idx], aes_alg);
+
+		/* file encryption key size */
+		switch (sg_page(sg)->mapping->key_length) {
+		case 16:
+			prd_table[idx].size &= ~FKL;
+			break;
+		case 32:
+		case 64:
+			prd_table[idx].size |= FKL;
+			break;
+		default:
+			printk(KERN_ERR "Invalid file key length %x \n",
+							(unsigned int)sg_page(sg)->mapping->key_length);
+			return -EINVAL;
+		}
+
+		index = sg_page(sg)->index;
+		index = index - sg_page(sg)->mapping->sensitive_data_index;
+		ret = file_enc_derive_iv(sg_page(sg)->mapping, index, extent_iv);
+		if (ret) {
+			printk(KERN_ERR "Error attemping to derive IV. ret = %c\n", ret);
+			return -EINVAL;
+		}
+
+		/* File IV */
+		prd_table[idx].file_iv0 = word_in(extent_iv, 3);
+		prd_table[idx].file_iv1 = word_in(extent_iv, 2);
+		prd_table[idx].file_iv2 = word_in(extent_iv, 1);
+		prd_table[idx].file_iv3 = word_in(extent_iv, 0);
+
+		/* File Enc key*/
+		for (j = 0; j < sg_page(sg)->mapping->key_length >> 2; j++)
+			*(&prd_table[idx].file_enckey0 + j) =
+				word_in(sg_page(sg)->mapping->key, (sg_page(sg)->mapping->key_length >> 2) - (j + 1));
+	}
+#endif
+	return 0;
+}
+EXPORT_SYMBOL_GPL(fmp_map_sg);
+
+#if defined(CONFIG_FIPS_FMP)
+int fmp_map_sg_st(struct ufs_hba *hba, struct ufshcd_sg_entry *prd_table,
+					struct scatterlist *sg, uint32_t sector_key,
+					uint32_t idx, uint32_t sector)
+{
+	struct ufshcd_sg_entry *prd_table_st = hba->ucd_prdt_ptr_st;
+
+	if (sector_key == UFS_BYPASS_SECTOR_BEGIN) {
+		SET_FAS(&prd_table[idx], CLEAR);
+		SET_DAS(&prd_table[idx], CLEAR);
+		return 0;
+	}
+
+	/* algorithm */
+	if (hba->self_test_mode == XTS_MODE)
+		SET_FAS(&prd_table[idx], AES_XTS);
+	else if (hba->self_test_mode == CBC_MODE)
+		SET_FAS(&prd_table[idx], AES_CBC);
+	else {
+		printk(KERN_ERR "Invalid algorithm for FMP FIPS validation. mode = %d\n", hba->self_test_mode);
+		return -EINVAL;
+	}
+
+	if (prd_table_st->size == 32)
+		prd_table[idx].size |= FKL;
+
+	/* File IV */
+	prd_table[idx].file_iv0 = prd_table_st->file_iv0;
+	prd_table[idx].file_iv1 = prd_table_st->file_iv1;
+	prd_table[idx].file_iv2 = prd_table_st->file_iv2;
+	prd_table[idx].file_iv3 = prd_table_st->file_iv3;
+
+	/* enc key */
+	prd_table[idx].file_enckey0 = prd_table_st->file_enckey0;
+	prd_table[idx].file_enckey1 = prd_table_st->file_enckey1;
+	prd_table[idx].file_enckey2 = prd_table_st->file_enckey2;
+	prd_table[idx].file_enckey3 = prd_table_st->file_enckey3;
+	prd_table[idx].file_enckey4 = prd_table_st->file_enckey4;
+	prd_table[idx].file_enckey5 = prd_table_st->file_enckey5;
+	prd_table[idx].file_enckey6 = prd_table_st->file_enckey6;
+	prd_table[idx].file_enckey7 = prd_table_st->file_enckey7;
+
+	/* tweak key */
+	prd_table[idx].file_twkey0 = prd_table_st->file_twkey0;
+	prd_table[idx].file_twkey1 = prd_table_st->file_twkey1;
+	prd_table[idx].file_twkey2 = prd_table_st->file_twkey2;
+	prd_table[idx].file_twkey3 = prd_table_st->file_twkey3;
+	prd_table[idx].file_twkey4 = prd_table_st->file_twkey4;
+	prd_table[idx].file_twkey5 = prd_table_st->file_twkey5;
+	prd_table[idx].file_twkey6 = prd_table_st->file_twkey6;
+	prd_table[idx].file_twkey7 = prd_table_st->file_twkey7;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(fmp_map_sg_st);
+#endif
+
+#if defined(CONFIG_UFS_FMP_ECRYPT_FS)
+void fmp_clear_sg(struct ufshcd_lrb *lrbp)
+{
+	struct scatterlist *sg;
+	int sg_segments;
+	struct ufshcd_sg_entry *prd_table;
+	struct scsi_cmnd *cmd = lrbp->cmd;
+	int i;
+
+	sg_segments = scsi_sg_count(cmd);
+	if (sg_segments) {
+		prd_table = (struct ufshcd_sg_entry *)lrbp->ucd_prdt_ptr;
+		scsi_for_each_sg(cmd, sg, sg_segments, i) {
+			if (!((unsigned long)(sg_page(sg)->mapping) & 0x1) && \
+					sg_page(sg)->mapping && sg_page(sg)->mapping->key \
+					&& ((unsigned int)(sg_page(sg)->index) >= 2)) {
+				#if FIPS_FMP_FUNC_TEST == 6 //Key Zeroization
+				print_hex_dump (KERN_ERR, "FIPS FMP descriptor before zeroize:", DUMP_PREFIX_NONE, 16, 1, &prd_table[i].file_iv0, sizeof(__le32)*20, false  );
+				#endif
+
+				memset(&prd_table[i].file_iv0, 0x0, sizeof(__le32)*20);
+
+				#if FIPS_FMP_FUNC_TEST == 6 //Key Zeroization
+				print_hex_dump (KERN_ERR, "FIPS FMP descriptor after zeroize:", DUMP_PREFIX_NONE, 16, 1, &prd_table[i].file_iv0, sizeof(__le32)*20, false  );
+				#endif
+
+			}
+		}
+	}
+
+	return;
+}
+#endif
+
+#if defined(CONFIG_UFS_FMP_DM_CRYPT)
+int fmp_clear_disk_key(void)
+{
+	struct device_node *dev_node;
+	struct platform_device *pdev;
+	struct device *dev;
+	struct ufs_hba *hba;
+
+	dev_node = of_find_compatible_node(NULL, NULL, "samsung,exynos-ufs");
+	if (!dev_node) {
+		printk(KERN_ERR "Fail to find exynos ufs device node\n");
+		return -ENODEV;
+	}
+
+	pdev = of_find_device_by_node(dev_node);
+	if (!pdev) {
+		printk(KERN_ERR "Fail to find exynos ufs pdev\n");
+		return -ENODEV;
+	}
+
+	dev = &pdev->dev;
+	hba = dev_get_drvdata(dev);
+	if (!hba) {
+		printk(KERN_ERR "Fail to find hba from dev\n");
+		return -ENODEV;
+	}
+
+	spin_lock(&disk_key_lock);
+	disk_key_flag = 2;
+	spin_unlock(&disk_key_lock);
+
+	printk(KERN_INFO "FMP disk encryption key is clear\n");
+	exynos_ufs_ctrl_hci_core_clk(dev_get_platdata(hba->dev), false);
+	return exynos_smc(SMC_CMD_FMP, FMP_KEY_CLEAR, 0, 0);
+}
+#endif
--- a/drivers/crypto/fmp/fmp_ufs_fips.c
+++ b/drivers/crypto/fmp/fmp_ufs_fips.c
@ -0,0 +1,444 @@
+/*
+ * Exynos FMP UFS driver for FIPS
+ *
+ * Copyright (C) 2014 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/syscalls.h>
+#include <linux/file.h>
+#include <linux/buffer_head.h>
+#include <linux/mtd/mtd.h>
+#include <linux/kdev_t.h>
+#include <linux/smc.h>
+
+#include <scsi/scsi_device.h>
+#include <scsi/scsi_host.h>
+#include <scsi/scsi_cmnd.h>
+
+#include <ufshcd.h>
+
+#include "fmpdev_int.h"
+#include "../scsi_priv.h"
+
+#if defined(CONFIG_UFS_FMP_ECRYPT_FS)
+#include "fmp_derive_iv.h"
+#endif
+#if defined(CONFIG_FIPS_FMP)
+#include "fmpdev_info.h"
+#endif
+
+#define byte2word(b0, b1, b2, b3) 	\
+		((unsigned int)(b0) << 24) | ((unsigned int)(b1) << 16) | ((unsigned int)(b2) << 8) | (b3)
+#define get_word(x, c)	byte2word(((unsigned char *)(x) + 4 * (c))[0], ((unsigned char *)(x) + 4 * (c))[1], \
+			((unsigned char *)(x) + 4 * (c))[2], ((unsigned char *)(x) + 4 * (c))[3])
+
+#define SF_BLK_OFFSET	(5)
+#define MAX_SCAN_PART	(50)
+
+struct ufs_fmp_work {
+	struct Scsi_Host *host;
+	struct scsi_device *sdev;
+	struct block_device *bdev;
+	sector_t sector;
+	dev_t devt;
+};
+
+struct ufshcd_sg_entry *prd_table;
+struct ufshcd_sg_entry *ucd_prdt_ptr_st;
+
+static int ufs_fmp_init(struct device *dev, uint32_t mode)
+{
+	struct ufs_hba *hba;
+	struct ufs_fmp_work *work;
+	struct Scsi_Host *host;
+
+	work = dev_get_drvdata(dev);
+	if (!work) {
+		dev_err(dev, "Fail to get work from platform device\n");
+		return -ENODEV;
+	}
+	host = work->host;
+	hba = shost_priv(host);
+
+	ucd_prdt_ptr_st = kmalloc(sizeof(struct ufshcd_sg_entry), GFP_KERNEL);
+	if (!ucd_prdt_ptr_st) {
+		dev_err(dev, "Fail to alloc prdt ptr for self test\n");
+		return -ENOMEM;
+	}
+	hba->ucd_prdt_ptr_st = ucd_prdt_ptr_st;
+
+	return 0;
+}
+
+static int ufs_fmp_set_key(struct device *dev, uint32_t mode, uint8_t *key, uint32_t key_len)
+{
+	struct ufs_hba *hba;
+	struct ufs_fmp_work *work;
+	struct Scsi_Host *host;
+	struct ufshcd_sg_entry *prd_table;
+
+	work = dev_get_drvdata(dev);
+	if (!work) {
+		dev_err(dev, "Fail to get work from platform device\n");
+		return -ENODEV;
+	}
+
+	host = work->host;
+	hba = shost_priv(host);
+	if (!hba->ucd_prdt_ptr_st) {
+		dev_err(dev, "prdt ptr for fips is not allocated\n");
+		return -ENOMEM;
+	}
+
+	prd_table = hba->ucd_prdt_ptr_st;
+
+	if (mode == CBC_MODE) {
+		SET_FAS(prd_table, CBC_MODE);
+
+		switch (key_len) {
+		case 32:
+			prd_table->size = 32;
+			/* encrypt key */
+			prd_table->file_enckey0 = get_word(key, 7);
+			prd_table->file_enckey1 = get_word(key, 6);
+			prd_table->file_enckey2 = get_word(key, 5);
+			prd_table->file_enckey3 = get_word(key, 4);
+			prd_table->file_enckey4 = get_word(key, 3);
+			prd_table->file_enckey5 = get_word(key, 2);
+			prd_table->file_enckey6 = get_word(key, 1);
+			prd_table->file_enckey7 = get_word(key, 0);
+			break;
+		case 16:
+			prd_table->size = 16;
+			/* encrypt key */
+			prd_table->file_enckey0 = get_word(key, 3);
+			prd_table->file_enckey1 = get_word(key, 2);
+			prd_table->file_enckey2 = get_word(key, 1);
+			prd_table->file_enckey3 = get_word(key, 0);
+			prd_table->file_enckey4 = 0;
+			prd_table->file_enckey5 = 0;
+			prd_table->file_enckey6 = 0;
+			prd_table->file_enckey7 = 0;
+			break;
+		default:
+			dev_err(dev, "Invalid key length : %d\n", key_len);
+			return -EINVAL;
+		}
+	} else if (mode == XTS_MODE) {
+		SET_FAS(prd_table, XTS_MODE);
+
+		switch (key_len) {
+		case 64:
+			prd_table->size = 32;
+			/* encrypt key */
+			prd_table->file_enckey0 = get_word(key, 7);
+			prd_table->file_enckey1 = get_word(key, 6);
+			prd_table->file_enckey2 = get_word(key, 5);
+			prd_table->file_enckey3 = get_word(key, 4);
+			prd_table->file_enckey4 = get_word(key, 3);
+			prd_table->file_enckey5 = get_word(key, 2);
+			prd_table->file_enckey6 = get_word(key, 1);
+			prd_table->file_enckey7 = get_word(key, 0);
+
+			/* tweak key */
+			prd_table->file_twkey0 = get_word(key, 15);
+			prd_table->file_twkey1 = get_word(key, 14);
+			prd_table->file_twkey2 = get_word(key, 13);
+			prd_table->file_twkey3 = get_word(key, 12);
+			prd_table->file_twkey4 = get_word(key, 11);
+			prd_table->file_twkey5 = get_word(key, 10);
+			prd_table->file_twkey6 = get_word(key, 9);
+			prd_table->file_twkey7 = get_word(key, 8);
+			break;
+		case 32:
+			prd_table->size = 16;
+			/* encrypt key */
+			prd_table->file_enckey0 = get_word(key, 3);
+			prd_table->file_enckey1 = get_word(key, 2);
+			prd_table->file_enckey2 = get_word(key, 1);
+			prd_table->file_enckey3 = get_word(key, 0);
+			prd_table->file_enckey4 = 0;
+			prd_table->file_enckey5 = 0;
+			prd_table->file_enckey6 = 0;
+			prd_table->file_enckey7 = 0;
+
+			/* tweak key */
+			prd_table->file_twkey0 = get_word(key, 7);
+			prd_table->file_twkey1 = get_word(key, 6);
+			prd_table->file_twkey2 = get_word(key, 5);
+			prd_table->file_twkey3 = get_word(key, 4);
+			prd_table->file_twkey4 = 0;
+			prd_table->file_twkey5 = 0;
+			prd_table->file_twkey6 = 0;
+			prd_table->file_twkey7 = 0;
+			break;
+		default:
+			dev_err(dev, "Invalid key length : %d\n", key_len);
+			return -EINVAL;
+		}
+	} else if (mode == BYPASS_MODE) {
+		SET_FAS(prd_table, BYPASS_MODE);
+
+		/* enc key */
+		prd_table->file_enckey0 = 0;
+		prd_table->file_enckey1 = 0;
+		prd_table->file_enckey2 = 0;
+		prd_table->file_enckey3 = 0;
+		prd_table->file_enckey4 = 0;
+		prd_table->file_enckey5 = 0;
+		prd_table->file_enckey6 = 0;
+		prd_table->file_enckey7 = 0;
+
+		/* tweak key */
+		prd_table->file_twkey0 = 0;
+		prd_table->file_twkey1 = 0;
+		prd_table->file_twkey2 = 0;
+		prd_table->file_twkey3 = 0;
+		prd_table->file_twkey4 = 0;
+		prd_table->file_twkey5 = 0;
+		prd_table->file_twkey6 = 0;
+		prd_table->file_twkey7 = 0;
+	} else {
+		dev_err(dev, "Invalid mode : %d\n", mode);
+		return -EINVAL;
+	}
+
+	dev_info(dev, "%s: ufs fmp key set is done.\n", __func__);
+	return 0;
+}
+
+static int ufs_fmp_set_iv(struct device *dev, uint32_t mode, uint8_t *iv, uint32_t iv_len)
+{
+	struct ufs_hba *hba;
+	struct ufs_fmp_work *work;
+	struct Scsi_Host *host;
+	struct ufshcd_sg_entry *prd_table;
+
+	work = dev_get_drvdata(dev);
+	if (!work) {
+		dev_err(dev, "Fail to get work from platform device\n");
+		return -ENODEV;
+	}
+
+	host = work->host;
+	hba = shost_priv(host);
+	if (!hba->ucd_prdt_ptr_st) {
+		dev_err(dev, "prdt ptr for fips is not allocated\n");
+		return -ENOMEM;
+	}
+
+	prd_table = hba->ucd_prdt_ptr_st;
+
+	if (mode == CBC_MODE || mode == XTS_MODE) {
+		prd_table->file_iv0 = get_word(iv, 3);
+		prd_table->file_iv1 = get_word(iv, 2);
+		prd_table->file_iv2 = get_word(iv, 1);
+		prd_table->file_iv3 = get_word(iv, 0);
+	} else if (mode == BYPASS_MODE) {
+		prd_table->file_iv0 = 0;
+		prd_table->file_iv1 = 0;
+		prd_table->file_iv2 = 0;
+		prd_table->file_iv3 = 0;
+	} else {
+		dev_err(dev, "Invalid mode : %d\n", mode);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static dev_t find_devt_for_selftest(void)
+{
+	int i, idx = 0;
+	uint64_t size;
+	uint64_t size_list[MAX_SCAN_PART];
+	dev_t devt_list[MAX_SCAN_PART];
+	dev_t devt_scan, devt;
+	struct block_device *bdev;
+	fmode_t fmode = FMODE_WRITE | FMODE_READ;
+
+	for (i = 0; i < MAX_SCAN_PART; i++) {
+		devt_scan = blk_lookup_devt("sda", i);
+		bdev = blkdev_get_by_dev(devt_scan, fmode, NULL);
+		if (IS_ERR(bdev))
+			continue;
+		else {
+			size_list[idx++] = (uint64_t)i_size_read(bdev->bd_inode);
+			devt_list[idx++] = devt_scan;
+		}
+	}
+
+	if (!idx)
+		goto err;
+
+	for (i = 0; i < idx; i++) {
+		if (i == 0) {
+			size = size_list[i];
+			devt = devt_list[i];
+		} else {
+			if (size < size_list[i])
+				devt = devt_list[i];
+		}
+	}
+
+	return devt;
+
+err:
+	return (dev_t)0;
+}
+
+static int ufs_fmp_run(struct device *dev, uint32_t mode, uint8_t *data,
+			uint32_t len, uint32_t write)
+{
+	int ret = 0;
+	struct ufs_hba *hba;
+	struct ufs_fmp_work *work;
+	struct Scsi_Host *host;
+	static struct buffer_head *bh;
+
+	work = dev_get_drvdata(dev);
+	if (!work) {
+		dev_err(dev, "Fail to get work from platform device\n");
+		return -ENODEV;
+	}
+	host = work->host;
+	hba = shost_priv(host);
+	hba->self_test_mode = mode;
+
+	bh = __getblk(work->bdev, work->sector, FMP_BLK_SIZE);
+	if (!bh) {
+		dev_err(dev, "Fail to get block from bdev\n");
+		return -ENODEV;
+	}
+	hba->self_test_bh = bh;
+
+	get_bh(bh);
+	if (write == WRITE_MODE) {
+		memcpy(bh->b_data, data, len);
+		set_buffer_dirty(bh);
+		sync_dirty_buffer(bh);
+		if (buffer_req(bh) && !buffer_uptodate(bh)) {
+			dev_err(dev, "IO error syncing for FMP fips write\n");
+			ret = -EIO;
+			goto out;
+		}
+		memset(bh->b_data, 0, FMP_BLK_SIZE);
+	} else {
+		lock_buffer(bh);
+		bh->b_end_io = end_buffer_read_sync;
+		submit_bh(READ_SYNC, bh);
+		wait_on_buffer(bh);
+		if (unlikely(!buffer_uptodate(bh))) {
+			ret = -EIO;
+			goto out;
+		}
+		memcpy(data, bh->b_data, len);
+	}
+out:
+	hba->self_test_mode = 0;
+	hba->self_test_bh = NULL;
+	put_bh(bh);
+
+	return ret;
+}
+
+static int ufs_fmp_exit(void)
+{
+	if (ucd_prdt_ptr_st)
+		kfree(ucd_prdt_ptr_st);
+
+	return 0;
+}
+
+struct fips_fmp_ops fips_fmp_fops = {
+	.init = ufs_fmp_init,
+	.set_key = ufs_fmp_set_key,
+	.set_iv = ufs_fmp_set_iv,
+	.run = ufs_fmp_run,
+	.exit = ufs_fmp_exit,
+};
+
+int fips_fmp_init(struct device *dev)
+{
+	struct ufs_fmp_work *work;
+	struct device_node *dev_node;
+	struct platform_device *pdev_ufs;
+	struct device *dev_ufs;
+	struct ufs_hba *hba;
+	struct Scsi_Host *host;
+	struct inode *inode;
+	struct scsi_device *sdev;
+	struct super_block *sb;
+	unsigned long blocksize;
+	unsigned char blocksize_bits;
+
+	sector_t self_test_block;
+	fmode_t fmode = FMODE_WRITE | FMODE_READ;
+
+	work = kmalloc(sizeof(*work), GFP_KERNEL);
+	if (!work) {
+		dev_err(dev, "Fail to alloc fmp work buffer\n");
+		return -ENOMEM;
+	}
+
+	dev_node = of_find_compatible_node(NULL, NULL, "samsung,exynos-ufs");
+	if (!dev_node) {
+		dev_err(dev, "Fail to find exynos ufs device node\n");
+		goto out;
+	}
+
+	pdev_ufs = of_find_device_by_node(dev_node);
+	if (!pdev_ufs) {
+		dev_err(dev, "Fail to find exynos ufs pdev\n");
+		goto out;
+	}
+
+	dev_ufs = &pdev_ufs->dev;
+	hba = dev_get_drvdata(dev_ufs);
+	if (!hba) {
+		dev_err(dev, "Fail to find hba from dev\n");
+		goto out;
+	}
+
+	host = hba->host;
+	sdev = to_scsi_device(dev_ufs);
+	work->host = host;
+	work->sdev = sdev;
+
+	work->devt = find_devt_for_selftest();
+	if (!work->devt) {
+		dev_err(dev, "Fail to find devt for self test\n");
+		return -ENODEV;
+	}
+
+	work->bdev = blkdev_get_by_dev(work->devt, fmode, NULL);
+	if (IS_ERR(work->bdev)) {
+		dev_err(dev, "Fail to open block device\n");
+		return -ENODEV;
+	}
+	inode = work->bdev->bd_inode;
+	sb = inode->i_sb;
+	blocksize = sb->s_blocksize;
+	blocksize_bits = sb->s_blocksize_bits;
+	self_test_block = (i_size_read(inode) - (blocksize * SF_BLK_OFFSET)) >> blocksize_bits;
+	work->sector = self_test_block;
+
+	dev_set_drvdata(dev, work);
+
+	return 0;
+
+out:
+	if (work)
+		kfree(work);
+
+	return -ENODEV;
+}
+EXPORT_SYMBOL_GPL(fips_fmp_init);
--- a/drivers/crypto/fmp/fmpdev.c
+++ b/drivers/crypto/fmp/fmpdev.c
--- a/drivers/crypto/fmp/fmpdev.h
+++ b/drivers/crypto/fmp/fmpdev.h
@ -0,0 +1,22 @@
+/*
+ * Exynos FMP test driver for FIPS
+ *
+ * Copyright (C) 2015 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#ifndef _FIPS_FMP_H_
+#define _FIPS_FMP_H_
+
+#define FMP_DRV_VERSION "1.1"
+
+int fips_fmp_cipher_init(struct device *dev, uint8_t *enckey, uint8_t *twkey, uint32_t key_len, uint32_t mode);
+int fips_fmp_cipher_set_iv(struct device *dev, uint8_t *iv, uint32_t mode);
+int fips_fmp_cipher_run(struct device *dev, uint8_t *src, uint8_t *dst, uint32_t len, uint32_t mode, uint32_t enc);
+int fips_fmp_cipher_exit(struct device *dev);
+
+#endif
--- a/drivers/crypto/fmp/fmpdev_info.h
+++ b/drivers/crypto/fmp/fmpdev_info.h
@ -0,0 +1,133 @@
+/*
+ * Exynos FMP device information for FIPS
+ *
+ * Copyright (C) 2015 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#ifndef _FIPS_FMP_INFO_H_
+#define _FIPS_FMP_INFO_H_
+
+#define FMP_BLK_SIZE	(4096)
+
+#define BYPASS_MODE	0
+#define CBC_MODE	1
+#define XTS_MODE	2
+
+#define ENCRYPT		1
+#define DECRYPT		2
+
+#define WRITE_MODE	1
+#define READ_MODE	2
+
+/* API extensions for linux */
+#define FMP_HMAC_MAX_KEY_LEN	512
+#define FMP_CIPHER_MAX_KEY_LEN	64
+
+enum fmpdev_crypto_op_t {
+	FMP_AES_CBC = 1,
+	FMP_AES_XTS = 2,
+	FMP_SHA2_256_HMAC = 3,
+	FMP_SHA2_256 = 4,
+	FMPFW_SHA2_256_HMAC = 5,
+	FMPFW_SHA2_256 = 6,
+	FMP_ALGORITHM_ALL, /* Keep updated - see below */
+};
+#define	FMP_ALGORITHM_MAX	(FMP_ALGORITHM_ALL - 1)
+
+/* the maximum of the above */
+#define EALG_MAX_BLOCK_LEN	16
+
+/* Values for hashes/MAC */
+#define AALG_MAX_RESULT_LEN	64
+
+/* maximum length of verbose alg names (depends on CRYPTO_MAX_ALG_NAME) */
+#define FMPDEV_MAX_ALG_NAME	64
+
+#define DEFAULT_PREALLOC_PAGES	32
+
+/* input of FMPGSESSION */
+struct session_op {
+	__u32	cipher;		/* cryptodev_crypto_op_t */
+	__u32	mac;		/* cryptodev_crypto_op_t */
+
+	__u32	keylen;
+	__u8	__user *key;
+	__u32	mackeylen;
+	__u8	__user *mackey;
+
+	__u32	ses;		/* session identifier */
+};
+
+struct session_info_op {
+	__u32 ses;		/* session identifier */
+
+	/* verbose names for the requested ciphers */
+	struct alg_info {
+		char cra_name[FMPDEV_MAX_ALG_NAME];
+		char cra_driver_name[FMPDEV_MAX_ALG_NAME];
+	} cipher_info, hash_info;
+
+	__u16	alignmask;	/* alignment constraints */
+	__u32   flags;          /* SIOP_FLAGS_* */
+};
+
+/* If this flag is set then this algorithm uses
+ * a driver only available in kernel (software drivers,
+ * or drivers based on instruction sets do not set this flag).
+ *
+ * If multiple algorithms are involved (as in AEAD case), then
+ * if one of them is kernel-driver-only this flag will be set.
+ */
+#define SIOP_FLAG_KERNEL_DRIVER_ONLY 1
+
+#define	COP_ENCRYPT	0
+#define COP_DECRYPT	1
+
+/* input of FMPCRYPT */
+struct crypt_op {
+	__u32	ses;		/* session identifier */
+	__u16	op;		/* COP_ENCRYPT or COP_DECRYPT */
+	__u16	flags;		/* see COP_FLAG_* */
+	__u32	len;		/* length of source data */
+	__u8	__user *src;	/* source data */
+	__u8	__user *dst;	/* pointer to output data */
+	/* pointer to output data for hash/MAC operations */
+	__u8	__user *mac;
+	/* initialization vector for encryption operations */
+	__u8	__user *iv;
+
+	__u32 data_unit_len;
+	__u32 data_unit_seqnumber;
+
+	__u8 	__user *secondLastEncodedData;
+	__u8 	__user *thirdLastEncodedData;
+};
+
+#define COP_FLAG_NONE		(0 << 0) /* totally no flag */
+#define COP_FLAG_UPDATE		(1 << 0) /* multi-update hash mode */
+#define COP_FLAG_FINAL		(1 << 1) /* multi-update final hash mode */
+#define COP_FLAG_WRITE_IV	(1 << 2) /* update the IV during operation */
+#define COP_FLAG_NO_ZC		(1 << 3) /* do not zero-copy */
+#define COP_FLAG_AEAD_TLS_TYPE  (1 << 4) /* authenticate and encrypt using the
+                                          * TLS protocol rules */
+#define COP_FLAG_AEAD_SRTP_TYPE  (1 << 5) /* authenticate and encrypt using the
+                                           * SRTP protocol rules */
+#define COP_FLAG_RESET		(1 << 6) /* multi-update reset the state.
+                                          * should be used in combination
+                                          * with COP_FLAG_UPDATE */
+#define COP_FLAG_AES_CBC	(1 << 7)
+#define COP_FLAG_AES_XTS	(1 << 8)
+#define COP_FLAG_AES_CBC_MCT	(1 << 9)
+
+#define FMPGSESSION		_IOWR('c', 200, struct session_op)
+#define FMPFSESSION		_IOWR('c', 201, __u32)
+#define FMPGSESSIONINFO		_IOWR('c', 202, struct session_info_op)
+#define FMPCRYPT		_IOWR('c', 203, struct crypt_op)
+#define FMP_AES_CBC_MCT		_IOWR('c', 204, struct crypt_op)
+
+#endif
--- a/drivers/crypto/fmp/fmpdev_int.h
+++ b/drivers/crypto/fmp/fmpdev_int.h
@ -0,0 +1,247 @@
+/*
+ * Exynos FMP device header for FIPS
+ *
+ * Copyright (C) 2015 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#ifndef __FMPDEV_INT_H__
+#define __FMPDEV_INT_H__
+
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/fs.h>
+#include <linux/file.h>
+#include <linux/fdtable.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/scatterlist.h>
+#include <crypto/aead.h>
+
+#include "fmpdev_info.h"
+
+/* Used during FIPS Functional test with CMT Lab
+ * FIPS_FMP_FUNC_TEST 0 - Normal operation
+ * FIPS_FMP_FUNC_TEST 1 - Fail FMP H/W AES XTS Self test
+ * FIPS_FMP_FUNC_TEST 2 - Fail FMP H/W AES CBC Self test
+ * FIPS_FMP_FUNC_TEST 3 - Fail FMP DRV SHA256 Self test
+ * FIPS_FMP_FUNC_TEST 4 - Fail FMP DRV HMAC Self test
+ * FIPS_FMP_FUNC_TEST 5 - Fail FMP DRV Integrity test
+ * FIPS_FMP_FUNC_TEST 6 - Enable FMP Key zeroization logs
+ */
+#define FIPS_FMP_FUNC_TEST 0
+
+struct fcrypt {
+	struct list_head list;
+	struct mutex sem;
+};
+
+/* kernel-internal extension to struct crypt_op */
+struct kernel_crypt_op {
+	struct crypt_op cop;
+
+	int ivlen;
+	__u8 iv[EALG_MAX_BLOCK_LEN];
+
+	int digestsize;
+	uint8_t hash_output[AALG_MAX_RESULT_LEN];
+
+	struct task_struct *task;
+	struct mm_struct *mm;
+};
+
+struct todo_list_item {
+	struct list_head __hook;
+	struct kernel_crypt_op kcop;
+	int result;
+};
+
+struct locked_list {
+	struct list_head list;
+	struct mutex lock;
+};
+
+struct fmp_info {
+	int init_status;
+	struct device *dev;
+	struct fcrypt fcrypt;
+	struct locked_list free, todo, done;
+	int itemcount;
+	struct work_struct fmptask;
+	wait_queue_head_t user_waiter;
+};
+
+/* compatibility stuff */
+#ifdef CONFIG_COMPAT
+#include <linux/compat.h>
+
+/* input of FMPGSESSION */
+struct compat_session_op {
+	/* Specify either cipher or mac
+	 */
+	uint32_t	cipher;		/* cryptodev_crypto_op_t */
+	uint32_t	mac;		/* cryptodev_crypto_op_t */
+
+	uint32_t	keylen;
+	compat_uptr_t	key;		/* pointer to key data */
+	uint32_t	mackeylen;
+	compat_uptr_t	mackey;		/* pointer to mac key data */
+
+	uint32_t	ses;		/* session identifier */
+};
+
+/* input of FMPCRYPT */
+ struct compat_crypt_op {
+	uint32_t	ses;		/* session identifier */
+	uint16_t	op;		/* COP_ENCRYPT or COP_DECRYPT */
+	uint16_t	flags;		/* see COP_FLAG_* */
+	uint32_t	len;		/* length of source data */
+	compat_uptr_t	src;		/* source data */
+	compat_uptr_t	dst;		/* pointer to output data */
+	compat_uptr_t	mac;/* pointer to output data for hash/MAC operations */
+	compat_uptr_t	iv;/* initialization vector for encryption operations */
+	compat_uptr_t secondLastEncodedData;
+	compat_uptr_t thirdLastEncodedData;
+};
+
+#define COMPAT_FMPGSESSION    _IOWR('c', 100, struct compat_session_op)
+#define COMPAT_FMPCRYPT       _IOWR('c', 101, struct compat_crypt_op)
+#define COMPAT_FMP_AES_CBC_MCT	_IOWR('c', 102, struct compat_crypt_op)
+#endif
+
+/* the maximum of the above */
+#define EALG_MAX_BLOCK_LEN	16
+
+struct cipher_data {
+	int init; /* 0 uninitialized */
+	int blocksize;
+	int aead;
+	int stream;
+	int ivsize;
+	int alignmask;
+	struct {
+		/* block ciphers */
+		struct crypto_ablkcipher *s;
+		struct ablkcipher_request *request;
+
+		/* AEAD ciphers */
+		struct crypto_aead *as;
+		struct aead_request *arequest;
+
+		struct fmpdev_result *result;
+		uint8_t iv[EALG_MAX_BLOCK_LEN];
+	} async;
+	int mode;
+};
+
+/* Hash */
+struct sha256_fmpfw_info {
+	uint32_t input;
+	size_t input_len;
+	uint32_t output;
+	uint32_t step;
+};
+
+struct hmac_sha256_fmpfw_info {
+	struct sha256_fmpfw_info s;
+	uint32_t key;
+	size_t key_len;
+	uint32_t hmac_mode;
+	uint32_t dummy;
+};
+
+struct hash_data {
+	int init; /* 0 uninitialized */
+	int digestsize;
+	int alignmask;
+	struct {
+		struct crypto_ahash *s;
+		struct fmpdev_result *result;
+		struct ahash_request *request;
+	} async;
+	struct hmac_sha256_fmpfw_info *fmpfw_info;
+};
+
+struct fmpdev_result {
+	struct completion completion;
+	int err;
+};
+
+/* other internal structs */
+struct csession {
+	struct list_head entry;
+	struct mutex sem;
+	struct cipher_data cdata;
+	struct hash_data hdata;
+	uint32_t sid;
+	uint32_t alignmask;
+
+	unsigned int array_size;
+	unsigned int used_pages; /* the number of pages that are used */
+	/* the number of pages marked as writable (first are the readable) */
+	unsigned int readable_pages;
+	struct page **pages;
+	struct scatterlist *sg;
+};
+
+struct csession *fmp_get_session_by_sid(struct fcrypt *fcr, uint32_t sid);
+
+inline static void fmp_put_session(struct csession *ses_ptr)
+{
+	mutex_unlock(&ses_ptr->sem);
+}
+int adjust_sg_array(struct csession *ses, int pagecount);
+
+#define MAX_TAP		8
+#define XBUFSIZE	8
+
+struct cipher_testvec {
+	char *key;
+	char *iv;
+	char *input;
+	char *result;
+	unsigned short tap[MAX_TAP];
+	int np;
+	unsigned char also_non_np;
+	unsigned char klen;
+	unsigned short ilen;
+	unsigned short rlen;
+};
+
+struct hash_testvec {
+	/* only used with keyed hash algorithms */
+	char *key;
+	char *plaintext;
+	char *digest;
+	unsigned char tap[MAX_TAP];
+	unsigned short psize;
+	unsigned char np;
+	unsigned char ksize;
+};
+
+struct cipher_test_suite {
+	struct {
+		struct cipher_testvec *vecs;
+		unsigned int count;
+	} enc;
+};
+
+struct hash_test_suite {
+	struct hash_testvec *vecs;
+	unsigned int count;
+};
+
+struct fips_fmp_ops {
+	int (*init)(struct device *dev, uint32_t mode);
+	int (*set_key)(struct device *dev, uint32_t mode, uint8_t *key, uint32_t key_len);
+	int (*set_iv)(struct device *dev, uint32_t mode, uint8_t *iv, uint32_t iv_len);
+	int (*run)(struct device *dev, uint32_t mode, uint8_t *data, uint32_t len, uint32_t write);
+	int (*exit)(void);
+};
+
+#endif
--- a/drivers/crypto/fmp/fmplib.c
+++ b/drivers/crypto/fmp/fmplib.c
@ -0,0 +1,782 @@
+/*
+ * Exynos FMP libary for FIPS
+ *
+ * Copyright (C) 2015 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/dma-mapping.h>
+#include <linux/smc.h>
+
+#include <crypto/hash.h>
+#include <crypto/sha.h>
+
+#include <asm/cacheflush.h>
+
+#include "fmpdev_int.h"
+#include "fmpdev.h"
+
+#define BYPASS_MODE	0
+#define CBC_MODE	1
+#define XTS_MODE	2
+
+#define INIT		0
+#define UPDATE		1
+#define FINAL		2
+
+static void fmpdev_complete(struct crypto_async_request *req, int err)
+{
+	struct fmpdev_result *res = req->data;
+
+	if (err == -EINPROGRESS)
+		return;
+
+	res->err = err;
+	complete(&res->completion);
+}
+
+static inline int waitfor(struct fmp_info *info, struct fmpdev_result *cr,
+			ssize_t ret)
+{
+	struct device *dev = info->dev;
+
+	switch (ret) {
+	case 0:
+		break;
+	case -EINPROGRESS:
+	case -EBUSY:
+		wait_for_completion(&cr->completion);
+		/* At this point we known for sure the request has finished,
+		 * because wait_for_completion above was not interruptible.
+		 * This is important because otherwise hardware or driver
+		 * might try to access memory which will be freed or reused for
+		 * another request. */
+
+		if (unlikely(cr->err)) {
+			dev_err(dev, "error from async request: %d\n", cr->err);
+			return cr->err;
+		}
+
+		break;
+	default:
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * FMP library to call FMP driver
+ *
+ * This file is part of linux fmpdev
+ */
+
+int fmpdev_cipher_init(struct fmp_info *info, struct cipher_data *out,
+			const char *alg_name,
+			uint8_t *enckey, uint8_t *twkey, size_t keylen)
+{
+	int ret;
+	struct device *dev = info->dev;
+
+	memset(out, 0, sizeof(*out));
+
+	if (!strcmp(alg_name, "cbc(aes-fmp)"))
+		out->mode = CBC_MODE;
+	else if (!strcmp(alg_name, "xts(aes-fmp)"))
+		out->mode = XTS_MODE;
+	else {
+		dev_err(dev, "Invalid mode\n");
+		return -1;
+	}
+
+	out->blocksize = 16;
+	out->ivsize = 16;
+	ret = fips_fmp_cipher_init(dev, enckey, twkey, keylen, out->mode);
+	if (ret) {
+		dev_err(dev, "Fail to initialize fmp cipher\n");
+		return -1;
+	}
+
+	out->init = 1;
+	return 0;
+}
+
+void fmpdev_cipher_deinit(struct cipher_data *cdata)
+{
+	if (cdata->init)
+		cdata->init = 0;
+}
+
+int fmpdev_cipher_set_iv(struct fmp_info *info, struct cipher_data *cdata,
+			uint8_t *iv, size_t iv_size)
+{
+	int ret;
+	struct device *dev = info->dev;
+
+	ret = fips_fmp_cipher_set_iv(dev, iv, cdata->mode);
+	if (ret) {
+		dev_err(dev, "Fail to set fmp iv\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+int fmpdev_cipher_exit(struct fmp_info *info)
+{
+	int ret;
+	struct device *dev = info->dev;
+
+	ret = fips_fmp_cipher_exit(dev);
+	if (ret) {
+		dev_err(dev, "Fail to exit fmp\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int fmpdev_cipher_encrypt(struct fmp_info *info,
+		struct cipher_data *cdata,
+		struct scatterlist *src,
+		struct scatterlist *dst, size_t len)
+{
+	int ret;
+	struct device *dev = info->dev;
+
+	ret = fips_fmp_cipher_run(dev, sg_virt(src), sg_virt(dst),
+				len, cdata->mode, ENCRYPT);
+	if (ret) {
+		dev_err(dev, "Fail to encrypt using fmp\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int fmpdev_cipher_decrypt(struct fmp_info *info,
+		struct cipher_data *cdata,
+		struct scatterlist *src,
+		struct scatterlist *dst, size_t len)
+{
+	int ret;
+	struct device *dev = info->dev;
+
+	ret = fips_fmp_cipher_run(dev, sg_virt(src), sg_virt(dst),
+				len, cdata->mode, DECRYPT);
+	if (ret) {
+		dev_err(dev, "Fail to encrypt using fmp\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+/* Hash functions */
+int fmpfw_hash_init(struct fmp_info *info, struct hash_data *hdata,
+			const char *alg_name, int hmac_mode,
+			void *mackey, size_t mackeylen)
+{
+	int ret;
+	unsigned long addr;
+	struct device *dev = info->dev;
+	struct hmac_sha256_fmpfw_info *fmpfw_info;
+
+	fmpfw_info = (struct hmac_sha256_fmpfw_info *)kzalloc(sizeof(*fmpfw_info), GFP_KERNEL);
+	if (unlikely(!fmpfw_info)) {
+		dev_err(dev, "Fail to alloc fmpfw info\n");
+		ret = ENOMEM;
+		goto error_alloc_info;
+	}
+
+	if (hmac_mode != 0) {
+		fmpfw_info->s.step = INIT;
+		fmpfw_info->hmac_mode = 1;
+		fmpfw_info->key = (uint32_t)virt_to_phys(mackey);
+		__flush_dcache_area(mackey, mackeylen);
+		fmpfw_info->key_len = mackeylen;
+		__flush_dcache_area(fmpfw_info, sizeof(*fmpfw_info));
+		addr = virt_to_phys(fmpfw_info);
+		ret = exynos_smc(SMC_CMD_FMP, FMP_FW_HMAC_SHA2_TEST, (uint32_t)addr, 0);
+		if (unlikely(ret)) {
+			dev_err(dev, "Fail to smc call for FMPFW HMAC SHA256 init. ret = 0x%x\n", ret);
+			ret = -EFAULT;
+			goto error_hmac_smc;
+		}
+	} else {
+		fmpfw_info->s.step = INIT;
+		fmpfw_info->hmac_mode = 0;
+		fmpfw_info->key = 0;
+		fmpfw_info->key_len = 0;
+		__flush_dcache_area(fmpfw_info, sizeof(*fmpfw_info));
+		addr = virt_to_phys(fmpfw_info);
+
+		ret = exynos_smc(SMC_CMD_FMP, FMP_FW_SHA2_TEST, (uint32_t)addr, 0);
+		if (unlikely(ret)) {
+			dev_err(dev, "Fail to smc call for FMPFW SHA256 init. ret = 0x%x\n", ret);
+			ret = -EFAULT;
+			goto error_sha_smc;
+		}
+	}
+
+	hdata->digestsize = SHA256_DIGEST_SIZE;
+	hdata->alignmask = 0x0;
+	hdata->fmpfw_info = fmpfw_info;
+
+	hdata->async.result = kzalloc(sizeof(*hdata->async.result), GFP_KERNEL);
+	if (unlikely(!hdata->async.result)) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	init_completion(&hdata->async.result->completion);
+
+	hdata->init = 1;
+
+	return 0;
+
+error_hmac_smc:
+error_sha_smc:
+	kfree(fmpfw_info);
+error_alloc_info:
+	hdata->fmpfw_info = NULL;
+error:
+	kfree(hdata->async.result);
+	return ret;
+}
+
+void fmpfw_hash_deinit(struct hash_data *hdata)
+{
+	if (hdata->init) {
+		kfree(hdata->async.result);
+		hdata->init = 0;
+	}
+}
+
+ssize_t fmpfw_hash_update(struct fmp_info *info, struct hash_data *hdata,
+				struct scatterlist *sg, size_t len)
+{
+	int ret = 0;
+	unsigned long addr;
+	struct device *dev = info->dev;
+	struct hmac_sha256_fmpfw_info *fmpfw_info = hdata->fmpfw_info;
+
+	fmpfw_info->s.step = UPDATE;
+	fmpfw_info->s.input = (uint32_t)sg_phys(sg);
+	__flush_dcache_area(sg_virt(sg), len);
+	fmpfw_info->s.input_len = len;
+	__flush_dcache_area(fmpfw_info, sizeof(*fmpfw_info));
+	addr = virt_to_phys(fmpfw_info);
+
+	reinit_completion(&hdata->async.result->completion);
+	if (fmpfw_info->hmac_mode) {
+		ret = exynos_smc(SMC_CMD_FMP, FMP_FW_HMAC_SHA2_TEST, addr, 0);
+		if (unlikely(ret)) {
+			dev_err(dev, "Fail to smc call for FMPFW HMAC SHA256 update. ret = 0x%x\n", ret);
+			ret = -EFAULT;
+		}
+	} else {
+		ret = exynos_smc(SMC_CMD_FMP, FMP_FW_SHA2_TEST, addr, 0);
+		if (unlikely(ret)) {
+			dev_err(dev, "Fail to smc call for FMPFW SHA256 update. ret = 0x%x\n", ret);
+			ret = -EFAULT;
+		}
+	}
+
+	return waitfor(info, hdata->async.result, ret);
+}
+
+int fmpfw_hash_final(struct fmp_info *info, struct hash_data *hdata, void *output)
+{
+	int ret = 0;
+	unsigned long addr;
+	struct device *dev = info->dev;
+	struct hmac_sha256_fmpfw_info *fmpfw_info = hdata->fmpfw_info;
+
+	memset(output, 0, hdata->digestsize);
+	fmpfw_info->s.step = FINAL;
+	fmpfw_info->s.output = (uint32_t)virt_to_phys(output);
+	__flush_dcache_area(fmpfw_info, sizeof(*fmpfw_info));
+	addr = virt_to_phys(fmpfw_info);
+
+	reinit_completion(&hdata->async.result->completion);
+	__dma_unmap_area((void *)output, hdata->digestsize, DMA_FROM_DEVICE);
+	if (fmpfw_info->hmac_mode) {
+		ret = exynos_smc(SMC_CMD_FMP, FMP_FW_HMAC_SHA2_TEST, addr, 0);
+		if (unlikely(ret)) {
+			dev_err(dev, "Fail to smc call for FMPFW HMAC SHA256 final. ret = 0x%x\n", ret);
+			ret = -EFAULT;
+		}
+	} else {
+		ret = exynos_smc(SMC_CMD_FMP, FMP_FW_SHA2_TEST, addr, 0);
+		if (unlikely(ret)) {
+			dev_err(dev, "Fail to smc call for FMPFW SHA256 final. ret = 0x%x\n", ret);
+			ret = -EFAULT;
+		}
+	}
+	__dma_unmap_area((void *)output, hdata->digestsize, DMA_FROM_DEVICE);
+
+	if (fmpfw_info->hmac_mode)
+		dev_info(dev, "fmp fw hmac sha256 F/W final is done\n");
+	else
+		dev_info(dev, "fmp fw sha256 F/W final is done\n");
+
+	kfree(fmpfw_info);
+	return waitfor(info, hdata->async.result, ret);
+}
+
+int fmpdev_hash_init(struct fmp_info *info, struct hash_data *hdata,
+			const char *alg_name,
+			int hmac_mode, void *mackey, size_t mackeylen)
+{
+	int ret;
+	struct device *dev = info->dev;
+
+	hdata->fmpfw_info = NULL;
+	hdata->async.s = crypto_alloc_ahash(alg_name, 0, 0);
+	if (unlikely(IS_ERR(hdata->async.s))) {
+		dev_err(dev, "Failed to load transform for %s\n", alg_name);
+		return -EINVAL;
+	}
+
+	/* Copy the key from user and set to TFM. */
+	if (hmac_mode != 0) {
+		ret = crypto_ahash_setkey(hdata->async.s, mackey, mackeylen);
+		if (unlikely(ret)) {
+			dev_err(dev, "Setting hmac key failed for %s-%zu.\n",
+					alg_name, mackeylen*8);
+			ret = -EINVAL;
+			goto error;
+		}
+	}
+
+	hdata->digestsize = crypto_ahash_digestsize(hdata->async.s);
+	hdata->alignmask = crypto_ahash_alignmask(hdata->async.s);
+
+	hdata->async.result = kzalloc(sizeof(*hdata->async.result), GFP_KERNEL);
+	if (unlikely(!hdata->async.result)) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	init_completion(&hdata->async.result->completion);
+
+	hdata->async.request = ahash_request_alloc(hdata->async.s, GFP_KERNEL);
+	if (unlikely(!hdata->async.request)) {
+		dev_err(dev, "error allocating async crypto request\n");
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	ahash_request_set_callback(hdata->async.request,
+			CRYPTO_TFM_REQ_MAY_BACKLOG,
+			fmpdev_complete, hdata->async.result);
+
+	ret = crypto_ahash_init(hdata->async.request);
+	if (unlikely(ret)) {
+		dev_err(dev, "error in fmpdev_hash_init()\n");
+		goto error_request;
+	}
+
+	hdata->init = 1;
+	return 0;
+
+error_request:
+	ahash_request_free(hdata->async.request);
+error:
+	kfree(hdata->async.result);
+	crypto_free_ahash(hdata->async.s);
+	return ret;
+}
+
+
+void fmpdev_hash_deinit(struct hash_data *hdata)
+{
+	if (hdata->init) {
+		if (hdata->async.request)
+			ahash_request_free(hdata->async.request);
+		kfree(hdata->async.result);
+		if (hdata->async.s)
+			crypto_free_ahash(hdata->async.s);
+		hdata->init = 0;
+	}
+}
+
+int fmpdev_hash_reset(struct fmp_info *info, struct hash_data *hdata)
+{
+	int ret;
+	struct device *dev = info->dev;
+
+	ret = crypto_ahash_init(hdata->async.request);
+	if (unlikely(ret)) {
+		dev_err(dev, "error in crypto_hash_init()\n");
+		return ret;
+	}
+
+	return 0;
+
+}
+
+ssize_t fmpdev_hash_update(struct fmp_info *info, struct hash_data *hdata,
+				struct scatterlist *sg, size_t len)
+{
+	int ret;
+
+	reinit_completion(&hdata->async.result->completion);
+	ahash_request_set_crypt(hdata->async.request, sg, NULL, len);
+
+	ret = crypto_ahash_update(hdata->async.request);
+
+	return waitfor(info, hdata->async.result, ret);
+}
+
+int fmpdev_hash_final(struct fmp_info *info, struct hash_data *hdata, void *output)
+{
+	int ret;
+
+	reinit_completion(&hdata->async.result->completion);
+	ahash_request_set_crypt(hdata->async.request, NULL, output, 0);
+
+	ret = crypto_ahash_final(hdata->async.request);
+
+	return waitfor(info, hdata->async.result, ret);
+}
+
+static int fmp_n_crypt(struct fmp_info *info, struct csession *ses_ptr,
+		struct crypt_op *cop,
+		struct scatterlist *src_sg, struct scatterlist *dst_sg,
+		uint32_t len)
+{
+	int ret;
+	struct device *dev = info->dev;
+
+	if (cop->op == COP_ENCRYPT) {
+		if (ses_ptr->hdata.init != 0) {
+			if (!ses_ptr->hdata.fmpfw_info)
+				ret = fmpdev_hash_update(info, &ses_ptr->hdata,
+								src_sg, len);
+			else
+				ret = fmpfw_hash_update(info, &ses_ptr->hdata,
+								src_sg, len);
+			if (unlikely(ret))
+				goto out_err;
+		}
+
+		if (ses_ptr->cdata.init != 0) {
+			ret = fmpdev_cipher_encrypt(info, &ses_ptr->cdata,
+						src_sg, dst_sg, len);
+			if (unlikely(ret))
+				goto out_err;
+		}
+	} else {
+		if (ses_ptr->cdata.init != 0) {
+			ret = fmpdev_cipher_decrypt(info, &ses_ptr->cdata,
+						src_sg, dst_sg, len);
+			if (unlikely(ret))
+				goto out_err;
+		}
+
+		if (ses_ptr->hdata.init != 0) {
+			if (!ses_ptr->hdata.fmpfw_info)
+				ret = fmpdev_hash_update(info, &ses_ptr->hdata,
+								dst_sg, len);
+			else
+				ret = fmpfw_hash_update(info, &ses_ptr->hdata,
+								dst_sg, len);
+			if (unlikely(ret))
+				goto out_err;
+		}
+	}
+
+	return 0;
+out_err:
+	dev_err(dev, "FMP crypt failure: %d\n", ret);
+
+	return ret;
+}
+
+static int __fmp_run_std(struct fmp_info *info,
+		struct csession *ses_ptr, struct crypt_op *cop)
+{
+	char *data;
+	struct device *dev = info->dev;
+	char __user *src, *dst;
+	size_t nbytes, bufsize;
+	struct scatterlist sg;
+	int ret = 0;
+
+	nbytes = cop->len;
+	data = (char *)__get_free_page(GFP_KERNEL);
+	if (unlikely(!data)) {
+		dev_err(dev, "Error getting free page.\n");
+		return -ENOMEM;
+	}
+
+	bufsize = PAGE_SIZE < nbytes ? PAGE_SIZE : nbytes;
+
+	src = cop->src;
+	dst = cop->dst;
+
+	while (nbytes > 0) {
+		size_t current_len = nbytes > bufsize ? bufsize : nbytes;
+
+		if (unlikely(copy_from_user(data, src, current_len))) {
+			dev_err(dev, "Error copying %d bytes from user address %p\n",
+						(int)current_len, src);
+			ret = -EFAULT;
+			break;
+		}
+
+		sg_init_one(&sg, data, current_len);
+		ret = fmp_n_crypt(info, ses_ptr, cop, &sg, &sg, current_len);
+		if (unlikely(ret)) {
+			dev_err(dev, "fmp_n_crypt failed\n");
+			break;
+		}
+
+		if (ses_ptr->cdata.init != 0) {
+			if (unlikely(copy_to_user(dst, data, current_len))) {
+				dev_err(dev, "could not copy to user\n");
+				ret = -EFAULT;
+				break;
+			}
+		}
+
+		dst += current_len;
+		nbytes -= current_len;
+		src += current_len;
+	}
+	free_page((unsigned long)data);
+
+	return ret;
+}
+
+
+int fmp_run(struct fmp_info *info, struct fcrypt *fcr, struct kernel_crypt_op *kcop)
+{
+	struct device *dev = info->dev;
+	struct csession *ses_ptr;
+	struct crypt_op *cop = &kcop->cop;
+	int ret = -EINVAL;
+
+	if (unlikely(cop->op != COP_ENCRYPT && cop->op != COP_DECRYPT)) {
+		dev_err(dev, "invalid operation op=%u\n", cop->op);
+		return -EINVAL;
+	}
+
+	/* this also enters ses_ptr->sem */
+	ses_ptr = fmp_get_session_by_sid(fcr, cop->ses);
+	if (unlikely(!ses_ptr)) {
+		dev_err(dev, "invalid session ID=0x%08X\n", cop->ses);
+		return -EINVAL;
+	}
+
+	if ((ses_ptr->cdata.init != 0) && (cop->len > PAGE_SIZE)) {
+		dev_err(dev, "Invalid input length. len = %d\n", cop->len);
+		return -EINVAL;
+	}
+
+	if (ses_ptr->hdata.init != 0 && (cop->flags == 0 || cop->flags & COP_FLAG_RESET) && \
+				!ses_ptr->hdata.fmpfw_info) {
+		ret = fmpdev_hash_reset(info, &ses_ptr->hdata);
+		if (unlikely(ret)) {
+			dev_err(dev, "error in fmpdev_hash_reset()");
+			goto out_unlock;
+		}
+	}
+
+	if (ses_ptr->cdata.init != 0) {
+		int blocksize = ses_ptr->cdata.blocksize;
+
+		if (unlikely(cop->len % blocksize)) {
+			dev_err(dev,
+				"data size (%u) isn't a multiple "
+				"of block size (%u)\n",
+				cop->len, blocksize);
+			ret = -EINVAL;
+			goto out_unlock;
+		}
+
+		if (cop->flags == COP_FLAG_AES_CBC)
+			fmpdev_cipher_set_iv(info, &ses_ptr->cdata, kcop->iv, 16);
+		else if (cop->flags == COP_FLAG_AES_XTS)
+			fmpdev_cipher_set_iv(info, &ses_ptr->cdata, (uint8_t *)&cop->data_unit_seqnumber, 16);
+		else {
+			ret = -EINVAL;
+			goto out_unlock;
+		}
+	}
+
+	if (likely(cop->len)) {
+		ret = __fmp_run_std(info, ses_ptr, &kcop->cop);
+		if (unlikely(ret))
+			goto out_unlock;
+	}
+
+	if (ses_ptr->hdata.init != 0 &&
+		((cop->flags & COP_FLAG_FINAL) ||
+		   (!(cop->flags & COP_FLAG_UPDATE) || cop->len == 0))) {
+		if (!ses_ptr->hdata.fmpfw_info)
+			ret = fmpdev_hash_final(info, &ses_ptr->hdata, kcop->hash_output);
+		else
+			ret = fmpfw_hash_final(info, &ses_ptr->hdata, kcop->hash_output);
+		if (unlikely(ret)) {
+			dev_err(dev, "CryptoAPI failure: %d\n", ret);
+			goto out_unlock;
+		}
+		kcop->digestsize = ses_ptr->hdata.digestsize;
+	}
+
+out_unlock:
+	fmp_put_session(ses_ptr);
+	return ret;
+}
+
+int fmp_run_AES_CBC_MCT(struct fmp_info *info, struct fcrypt *fcr,
+			struct kernel_crypt_op *kcop)
+{
+	struct device *dev = info->dev;
+	struct csession *ses_ptr;
+	struct crypt_op *cop = &kcop->cop;
+	char **Ct = 0;
+	char **Pt = 0;
+	int ret = 0, k = 0;
+
+	if (unlikely(cop->op != COP_ENCRYPT && cop->op != COP_DECRYPT)) {
+		dev_err(dev, "invalid operation op=%u\n", cop->op);
+		return -EINVAL;
+	}
+
+	/* this also enters ses_ptr->sem */
+	ses_ptr = fmp_get_session_by_sid(fcr, cop->ses);
+	if (unlikely(!ses_ptr)) {
+		dev_err(dev, "invalid session ID=0x%08X\n", cop->ses);
+		return -EINVAL;
+	}
+
+	if (cop->len > PAGE_SIZE) {
+		dev_err(dev, "Invalid input length. len = %d\n", cop->len);
+		return -EINVAL;
+	}
+
+	if (ses_ptr->cdata.init != 0) {
+		int blocksize = ses_ptr->cdata.blocksize;
+
+		if (unlikely(cop->len % blocksize)) {
+			dev_err(dev,
+				"data size (%u) isn't a multiple "
+				"of block size (%u)\n",
+				cop->len, blocksize);
+			ret = -EINVAL;
+			goto out_unlock;
+		}
+
+		fmpdev_cipher_set_iv(info, &ses_ptr->cdata, kcop->iv, 16);
+	}
+
+	if (likely(cop->len)) {
+		if (cop->flags & COP_FLAG_AES_CBC_MCT) {
+		// do MCT here
+		char *data;
+	        char __user *src, *dst, *secondLast;
+	        struct scatterlist sg;
+	        size_t nbytes, bufsize;
+	        int ret = 0;
+	        int y = 0;
+
+	        nbytes = cop->len;
+	        data = (char *)__get_free_page(GFP_KERNEL);
+		if (unlikely(!data)) {
+			dev_err(dev, "Error getting free page.\n");
+			return -ENOMEM;
+		}
+
+		Pt = (char**)kmalloc(1000 * sizeof(char*), GFP_KERNEL);
+		for (k=0; k<1000; k++)
+		       Pt[k]= (char*)kmalloc(nbytes, GFP_KERNEL);
+
+		Ct = (char**)kmalloc(1000 * sizeof(char*), GFP_KERNEL);
+		for (k=0; k<1000; k++)
+		       Ct[k]= (char*)kmalloc(nbytes, GFP_KERNEL);
+
+	        bufsize = PAGE_SIZE < nbytes ? PAGE_SIZE : nbytes;
+
+	        src = cop->src;
+	        dst = cop->dst;
+	        secondLast = cop->secondLastEncodedData;
+
+		if (unlikely(copy_from_user(data, src, nbytes))) {
+			printk(KERN_ERR "Error copying %d bytes from user address %p.\n", (int)nbytes, src);
+		        ret = -EFAULT;
+		        goto out_err;
+	        }
+
+	        sg_init_one(&sg, data, nbytes);
+		for (y = 0; y < 1000; y++) {
+			memcpy(Pt[y], data, nbytes);
+			ret = fmp_n_crypt(info, ses_ptr, cop, &sg, &sg, nbytes);
+			memcpy(Ct[y], data, nbytes);
+
+			if (y == 998) {
+				if (unlikely(copy_to_user(secondLast, data, nbytes)))
+					printk(KERN_ERR "unable to copy second last data for AES_CBC_MCT\n");
+				else
+					printk(KERN_ERR "KAMAL copied secondlast data\n");
+			}
+
+			if( y == 0) {
+				memcpy(data, kcop->iv, kcop->ivlen);
+			} else {
+				if(y != 999)
+					memcpy(data, Ct[y-1], nbytes);
+			}
+
+			if (unlikely(ret)) {
+				printk(KERN_ERR "fmp_n_crypt failed.\n");
+				goto out_err;
+			}
+
+			if (cop->op == COP_ENCRYPT)
+				fmpdev_cipher_set_iv(info, &ses_ptr->cdata, Ct[y], 16);
+			else if (cop->op == COP_DECRYPT)
+				fmpdev_cipher_set_iv(info, &ses_ptr->cdata, Pt[y], 16);
+		} // for loop
+
+		if (ses_ptr->cdata.init != 0) {
+			if (unlikely(copy_to_user(dst, data, nbytes))) {
+				printk(KERN_ERR "could not copy to user.\n");
+				ret = -EFAULT;
+				goto out_err;
+			}
+		}
+
+		for (k=0; k<1000; k++) {
+			kfree(Ct[k]);
+			kfree(Pt[k]);
+		}
+
+		kfree(Ct);
+		kfree(Pt);
+		free_page((unsigned long)data);
+		} else
+			goto out_unlock;
+	}
+
+out_unlock:
+	fmp_put_session(ses_ptr);
+	return ret;
+
+out_err:
+	fmp_put_session(ses_ptr);
+	dev_info(dev, "CryptoAPI failure: %d\n", ret);
+
+	return ret;
+}
--- a/drivers/crypto/fmp/fmplib.h
+++ b/drivers/crypto/fmp/fmplib.h
@ -0,0 +1,36 @@
+/*
+ * Exynos FMP library header
+ *
+ * Copyright (C) 2015 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#ifndef __FMPLIB_H__
+#define __FMPLIB_H__
+
+int fmpdev_cipher_init(struct fmp_info *info, struct cipher_data *out,
+			const char *alg_name,
+			uint8_t *enckey, uint8_t *twkey, size_t keylen);
+void fmpdev_cipher_deinit(struct cipher_data *cdata);
+int fmpfw_hash_init(struct fmp_info *info, struct hash_data *hdata,
+			const char *alg_name, int hmac_mode,
+			void *mackey, size_t mackeylen);
+void fmpfw_hash_deinit(struct hash_data *hdata);
+int fmpdev_hash_init(struct fmp_info *info, struct hash_data *hdata,
+			const char *alg_name,
+			int hmac_mode, void *mackey, size_t mackeylen);
+void fmpdev_hash_deinit(struct hash_data *hdata);
+int fmpdev_hash_reset(struct fmp_info *info, struct hash_data *hdata);
+ssize_t fmpdev_hash_update(struct hash_data *hdata,
+				struct scatterlist *sg, size_t len);
+int fmpdev_hash_final(struct hash_data *hdata, void *output);
+int fmp_run(struct fmp_info *info, struct fcrypt *fcr, struct kernel_crypt_op *kcop);
+int fmpdev_cipher_exit(struct fmp_info *info);
+int fmp_run_AES_CBC_MCT(struct fmp_info *info, struct fcrypt *fcr,
+				struct kernel_crypt_op *kcop);
+
+#endif
--- a/drivers/crypto/fmp/hmac_fmp.c
+++ b/drivers/crypto/fmp/hmac_fmp.c
@ -0,0 +1,287 @@
+/*
+ * Cryptographic API.
+ *
+ * HMAC: Keyed-Hashing for Message Authentication (RFC2104).
+ *
+ * Copyright (c) 2002 James Morris <jmorris@intercode.com.au>
+ * Copyright (c) 2006 Herbert Xu <herbert@gondor.apana.org.au>
+ *
+ * The HMAC implementation is derived from USAGI.
+ * Copyright (c) 2002 Kazunori Miyazawa <miyazawa@linux-ipv6.org> / USAGI
+ *
+ * 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.
+ *
+ */
+
+#include <crypto/internal/hash.h>
+#include <crypto/scatterwalk.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/scatterlist.h>
+#include <linux/string.h>
+
+#if defined(CONFIG_FIPS_FMP)
+extern bool in_fmp_fips_err(void);
+#endif
+
+
+struct hmac_ctx {
+	struct crypto_shash *hash;
+};
+
+static inline void *align_ptr(void *p, unsigned int align)
+{
+	return (void *)ALIGN((unsigned long)p, align);
+}
+
+static inline struct hmac_ctx *hmac_ctx(struct crypto_shash *tfm)
+{
+	return align_ptr(crypto_shash_ctx_aligned(tfm) +
+			 crypto_shash_statesize(tfm) * 2,
+			 crypto_tfm_ctx_alignment());
+}
+
+static int hmac_setkey(struct crypto_shash *parent,
+		       const u8 *inkey, unsigned int keylen)
+{
+	int bs = crypto_shash_blocksize(parent);
+	int ds = crypto_shash_digestsize(parent);
+	int ss = crypto_shash_statesize(parent);
+	char *ipad = crypto_shash_ctx_aligned(parent);
+	char *opad = ipad + ss;
+	struct hmac_ctx *ctx = align_ptr(opad + ss,
+					 crypto_tfm_ctx_alignment());
+	struct crypto_shash *hash = ctx->hash;
+	struct {
+		struct shash_desc shash;
+		char ctx[crypto_shash_descsize(hash)];
+	} desc;
+	unsigned int i;
+
+	desc.shash.tfm = hash;
+	desc.shash.flags = crypto_shash_get_flags(parent) &
+			    CRYPTO_TFM_REQ_MAY_SLEEP;
+
+	if (keylen > bs) {
+		int err;
+
+		err = crypto_shash_digest(&desc.shash, inkey, keylen, ipad);
+		if (err)
+			return err;
+
+		keylen = ds;
+	} else
+		memcpy(ipad, inkey, keylen);
+
+	memset(ipad + keylen, 0, bs - keylen);
+	memcpy(opad, ipad, bs);
+
+	for (i = 0; i < bs; i++) {
+		ipad[i] ^= 0x36;
+		opad[i] ^= 0x5c;
+	}
+
+	return crypto_shash_init(&desc.shash) ?:
+	       crypto_shash_update(&desc.shash, ipad, bs) ?:
+	       crypto_shash_export(&desc.shash, ipad) ?:
+	       crypto_shash_init(&desc.shash) ?:
+	       crypto_shash_update(&desc.shash, opad, bs) ?:
+	       crypto_shash_export(&desc.shash, opad);
+}
+
+static int hmac_export(struct shash_desc *pdesc, void *out)
+{
+	struct shash_desc *desc = shash_desc_ctx(pdesc);
+
+	desc->flags = pdesc->flags & CRYPTO_TFM_REQ_MAY_SLEEP;
+
+	return crypto_shash_export(desc, out);
+}
+
+static int hmac_import(struct shash_desc *pdesc, const void *in)
+{
+	struct shash_desc *desc = shash_desc_ctx(pdesc);
+	struct hmac_ctx *ctx = hmac_ctx(pdesc->tfm);
+
+	desc->tfm = ctx->hash;
+	desc->flags = pdesc->flags & CRYPTO_TFM_REQ_MAY_SLEEP;
+
+	return crypto_shash_import(desc, in);
+}
+
+static int hmac_init(struct shash_desc *pdesc)
+{
+	return hmac_import(pdesc, crypto_shash_ctx_aligned(pdesc->tfm));
+}
+
+static int hmac_update(struct shash_desc *pdesc,
+		       const u8 *data, unsigned int nbytes)
+{
+	struct shash_desc *desc = shash_desc_ctx(pdesc);
+
+	desc->flags = pdesc->flags & CRYPTO_TFM_REQ_MAY_SLEEP;
+
+	return crypto_shash_update(desc, data, nbytes);
+}
+
+static int hmac_final(struct shash_desc *pdesc, u8 *out)
+{
+	struct crypto_shash *parent = pdesc->tfm;
+	int ds = crypto_shash_digestsize(parent);
+	int ss = crypto_shash_statesize(parent);
+	char *opad = crypto_shash_ctx_aligned(parent) + ss;
+	struct shash_desc *desc = shash_desc_ctx(pdesc);
+
+	desc->flags = pdesc->flags & CRYPTO_TFM_REQ_MAY_SLEEP;
+
+	printk(KERN_INFO "%s: fmp hmac sha256 S/W final is done.\n", __func__);
+
+	return crypto_shash_final(desc, out) ?:
+	       crypto_shash_import(desc, opad) ?:
+	       crypto_shash_finup(desc, out, ds, out);
+}
+
+static int hmac_finup(struct shash_desc *pdesc, const u8 *data,
+		      unsigned int nbytes, u8 *out)
+{
+
+	struct crypto_shash *parent = pdesc->tfm;
+	int ds = crypto_shash_digestsize(parent);
+	int ss = crypto_shash_statesize(parent);
+	char *opad = crypto_shash_ctx_aligned(parent) + ss;
+	struct shash_desc *desc = shash_desc_ctx(pdesc);
+
+	desc->flags = pdesc->flags & CRYPTO_TFM_REQ_MAY_SLEEP;
+
+	return crypto_shash_finup(desc, data, nbytes, out) ?:
+	       crypto_shash_import(desc, opad) ?:
+	       crypto_shash_finup(desc, out, ds, out);
+}
+
+static int hmac_init_tfm(struct crypto_tfm *tfm)
+{
+	struct crypto_shash *parent = __crypto_shash_cast(tfm);
+	struct crypto_shash *hash;
+	struct crypto_instance *inst = (void *)tfm->__crt_alg;
+	struct crypto_shash_spawn *spawn = crypto_instance_ctx(inst);
+	struct hmac_ctx *ctx = hmac_ctx(parent);
+
+	hash = crypto_spawn_shash(spawn);
+	if (IS_ERR(hash))
+		return PTR_ERR(hash);
+
+	parent->descsize = sizeof(struct shash_desc) +
+			   crypto_shash_descsize(hash);
+
+	ctx->hash = hash;
+	return 0;
+}
+
+static void hmac_exit_tfm(struct crypto_tfm *tfm)
+{
+	struct hmac_ctx *ctx = hmac_ctx(__crypto_shash_cast(tfm));
+	crypto_free_shash(ctx->hash);
+}
+
+static int hmac_create(struct crypto_template *tmpl, struct rtattr **tb)
+{
+	struct shash_instance *inst;
+	struct crypto_alg *alg;
+	struct shash_alg *salg;
+	int err;
+	int ds;
+	int ss;
+
+#if defined(CONFIG_FIPS_FMP)
+	if (unlikely(in_fmp_fips_err())) {
+		printk(KERN_ERR "FMP HMAC create failed due to fips error\n");
+		return -EPERM;
+	}
+#endif
+
+	err = crypto_check_attr_type(tb, CRYPTO_ALG_TYPE_SHASH);
+	if (err)
+		return err;
+
+	salg = shash_attr_alg(tb[1], 0, 0);
+	if (IS_ERR(salg))
+		return PTR_ERR(salg);
+
+	err = -EINVAL;
+	ds = salg->digestsize;
+	ss = salg->statesize;
+	alg = &salg->base;
+	if (ds > alg->cra_blocksize ||
+	    ss < alg->cra_blocksize)
+		goto out_put_alg;
+
+	inst = shash_alloc_instance("hmac-fmp", alg);
+	err = PTR_ERR(inst);
+	if (IS_ERR(inst))
+		goto out_put_alg;
+
+	err = crypto_init_shash_spawn(shash_instance_ctx(inst), salg,
+				      shash_crypto_instance(inst));
+	if (err)
+		goto out_free_inst;
+
+	inst->alg.base.cra_priority = alg->cra_priority;
+	inst->alg.base.cra_blocksize = alg->cra_blocksize;
+	inst->alg.base.cra_alignmask = alg->cra_alignmask;
+
+	ss = ALIGN(ss, alg->cra_alignmask + 1);
+	inst->alg.digestsize = ds;
+	inst->alg.statesize = ss;
+
+	inst->alg.base.cra_ctxsize = sizeof(struct hmac_ctx) +
+				     ALIGN(ss * 2, crypto_tfm_ctx_alignment());
+
+	inst->alg.base.cra_init = hmac_init_tfm;
+	inst->alg.base.cra_exit = hmac_exit_tfm;
+
+	inst->alg.init = hmac_init;
+	inst->alg.update = hmac_update;
+	inst->alg.final = hmac_final;
+	inst->alg.finup = hmac_finup;
+	inst->alg.export = hmac_export;
+	inst->alg.import = hmac_import;
+	inst->alg.setkey = hmac_setkey;
+
+	err = shash_register_instance(tmpl, inst);
+	if (err) {
+out_free_inst:
+		shash_free_instance(shash_crypto_instance(inst));
+	}
+
+out_put_alg:
+	crypto_mod_put(alg);
+	return err;
+}
+
+static struct crypto_template hmac_tmpl = {
+	.name = "hmac-fmp",
+	.create = hmac_create,
+	.free = shash_free_instance,
+	.module = THIS_MODULE,
+};
+
+static int __init hmac_module_init(void)
+{
+	return crypto_register_template(&hmac_tmpl);
+}
+
+static void __exit hmac_module_exit(void)
+{
+	crypto_unregister_template(&hmac_tmpl);
+}
+
+module_init(hmac_module_init);
+module_exit(hmac_module_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("HMAC hash algorithm");
--- a/drivers/crypto/fmp/last_file.c
+++ b/drivers/crypto/fmp/last_file.c
@ -0,0 +1,30 @@
+/*
+ * Last file for Exynos FMP FIPS integrity check
+ *
+ * Copyright (C) 2015 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#include <linux/init.h>
+
+const int last_fmp_rodata = 1000;
+int last_fmp_data   = 2000;
+
+void last_fmp_text(void) __attribute__((unused));
+void last_fmp_text (void)
+{
+}
+
+void __init last_fmp_init(void) __attribute__((unused));
+void __init last_fmp_init(void)
+{
+}
+
+void __exit last_fmp_exit(void) __attribute__((unused));
+void __exit last_fmp_exit(void)
+{
+}
--- a/drivers/crypto/fmp/sha256_fmp.c
+++ b/drivers/crypto/fmp/sha256_fmp.c
@ -0,0 +1,357 @@
+/*
+ * Cryptographic API.
+ *
+ * SHA-256, as specified in
+ * http://csrc.nist.gov/groups/STM/cavp/documents/shs/sha256-384-512.pdf
+ *
+ * SHA-256 code by Jean-Luc Cooke <jlcooke@certainkey.com>.
+ *
+ * Copyright (c) Jean-Luc Cooke <jlcooke@certainkey.com>
+ * Copyright (c) Andrew McDonald <andrew@mcdonald.org.uk>
+ * Copyright (c) 2002 James Morris <jmorris@intercode.com.au>
+ *
+ * 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.
+ *
+ */
+#include <crypto/internal/hash.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/mm.h>
+#include <linux/types.h>
+#include <crypto/sha.h>
+#include <asm/byteorder.h>
+
+#if defined(CONFIG_FIPS_FMP)
+extern bool in_fmp_fips_err(void);
+#endif
+
+static inline u32 Ch(u32 x, u32 y, u32 z)
+{
+	return z ^ (x & (y ^ z));
+}
+
+static inline u32 Maj(u32 x, u32 y, u32 z)
+{
+	return (x & y) | (z & (x | y));
+}
+
+#define e0(x)       (ror32(x, 2) ^ ror32(x,13) ^ ror32(x,22))
+#define e1(x)       (ror32(x, 6) ^ ror32(x,11) ^ ror32(x,25))
+#define s0(x)       (ror32(x, 7) ^ ror32(x,18) ^ (x >> 3))
+#define s1(x)       (ror32(x,17) ^ ror32(x,19) ^ (x >> 10))
+
+static inline void LOAD_OP(int I, u32 *W, const u8 *input)
+{
+	W[I] = __be32_to_cpu( ((__be32*)(input))[I] );
+}
+
+static inline void BLEND_OP(int I, u32 *W)
+{
+	W[I] = s1(W[I-2]) + W[I-7] + s0(W[I-15]) + W[I-16];
+}
+
+static void sha256_transform(u32 *state, const u8 *input)
+{
+	u32 a, b, c, d, e, f, g, h, t1, t2;
+	u32 W[64];
+	int i;
+
+	/* load the input */
+	for (i = 0; i < 16; i++)
+		LOAD_OP(i, W, input);
+
+	/* now blend */
+	for (i = 16; i < 64; i++)
+		BLEND_OP(i, W);
+
+	/* load the state into our registers */
+	a=state[0];  b=state[1];  c=state[2];  d=state[3];
+	e=state[4];  f=state[5];  g=state[6];  h=state[7];
+
+	/* now iterate */
+	t1 = h + e1(e) + Ch(e,f,g) + 0x428a2f98 + W[ 0];
+	t2 = e0(a) + Maj(a,b,c);    d+=t1;    h=t1+t2;
+	t1 = g + e1(d) + Ch(d,e,f) + 0x71374491 + W[ 1];
+	t2 = e0(h) + Maj(h,a,b);    c+=t1;    g=t1+t2;
+	t1 = f + e1(c) + Ch(c,d,e) + 0xb5c0fbcf + W[ 2];
+	t2 = e0(g) + Maj(g,h,a);    b+=t1;    f=t1+t2;
+	t1 = e + e1(b) + Ch(b,c,d) + 0xe9b5dba5 + W[ 3];
+	t2 = e0(f) + Maj(f,g,h);    a+=t1;    e=t1+t2;
+	t1 = d + e1(a) + Ch(a,b,c) + 0x3956c25b + W[ 4];
+	t2 = e0(e) + Maj(e,f,g);    h+=t1;    d=t1+t2;
+	t1 = c + e1(h) + Ch(h,a,b) + 0x59f111f1 + W[ 5];
+	t2 = e0(d) + Maj(d,e,f);    g+=t1;    c=t1+t2;
+	t1 = b + e1(g) + Ch(g,h,a) + 0x923f82a4 + W[ 6];
+	t2 = e0(c) + Maj(c,d,e);    f+=t1;    b=t1+t2;
+	t1 = a + e1(f) + Ch(f,g,h) + 0xab1c5ed5 + W[ 7];
+	t2 = e0(b) + Maj(b,c,d);    e+=t1;    a=t1+t2;
+
+	t1 = h + e1(e) + Ch(e,f,g) + 0xd807aa98 + W[ 8];
+	t2 = e0(a) + Maj(a,b,c);    d+=t1;    h=t1+t2;
+	t1 = g + e1(d) + Ch(d,e,f) + 0x12835b01 + W[ 9];
+	t2 = e0(h) + Maj(h,a,b);    c+=t1;    g=t1+t2;
+	t1 = f + e1(c) + Ch(c,d,e) + 0x243185be + W[10];
+	t2 = e0(g) + Maj(g,h,a);    b+=t1;    f=t1+t2;
+	t1 = e + e1(b) + Ch(b,c,d) + 0x550c7dc3 + W[11];
+	t2 = e0(f) + Maj(f,g,h);    a+=t1;    e=t1+t2;
+	t1 = d + e1(a) + Ch(a,b,c) + 0x72be5d74 + W[12];
+	t2 = e0(e) + Maj(e,f,g);    h+=t1;    d=t1+t2;
+	t1 = c + e1(h) + Ch(h,a,b) + 0x80deb1fe + W[13];
+	t2 = e0(d) + Maj(d,e,f);    g+=t1;    c=t1+t2;
+	t1 = b + e1(g) + Ch(g,h,a) + 0x9bdc06a7 + W[14];
+	t2 = e0(c) + Maj(c,d,e);    f+=t1;    b=t1+t2;
+	t1 = a + e1(f) + Ch(f,g,h) + 0xc19bf174 + W[15];
+	t2 = e0(b) + Maj(b,c,d);    e+=t1;    a=t1+t2;
+
+	t1 = h + e1(e) + Ch(e,f,g) + 0xe49b69c1 + W[16];
+	t2 = e0(a) + Maj(a,b,c);    d+=t1;    h=t1+t2;
+	t1 = g + e1(d) + Ch(d,e,f) + 0xefbe4786 + W[17];
+	t2 = e0(h) + Maj(h,a,b);    c+=t1;    g=t1+t2;
+	t1 = f + e1(c) + Ch(c,d,e) + 0x0fc19dc6 + W[18];
+	t2 = e0(g) + Maj(g,h,a);    b+=t1;    f=t1+t2;
+	t1 = e + e1(b) + Ch(b,c,d) + 0x240ca1cc + W[19];
+	t2 = e0(f) + Maj(f,g,h);    a+=t1;    e=t1+t2;
+	t1 = d + e1(a) + Ch(a,b,c) + 0x2de92c6f + W[20];
+	t2 = e0(e) + Maj(e,f,g);    h+=t1;    d=t1+t2;
+	t1 = c + e1(h) + Ch(h,a,b) + 0x4a7484aa + W[21];
+	t2 = e0(d) + Maj(d,e,f);    g+=t1;    c=t1+t2;
+	t1 = b + e1(g) + Ch(g,h,a) + 0x5cb0a9dc + W[22];
+	t2 = e0(c) + Maj(c,d,e);    f+=t1;    b=t1+t2;
+	t1 = a + e1(f) + Ch(f,g,h) + 0x76f988da + W[23];
+	t2 = e0(b) + Maj(b,c,d);    e+=t1;    a=t1+t2;
+
+	t1 = h + e1(e) + Ch(e,f,g) + 0x983e5152 + W[24];
+	t2 = e0(a) + Maj(a,b,c);    d+=t1;    h=t1+t2;
+	t1 = g + e1(d) + Ch(d,e,f) + 0xa831c66d + W[25];
+	t2 = e0(h) + Maj(h,a,b);    c+=t1;    g=t1+t2;
+	t1 = f + e1(c) + Ch(c,d,e) + 0xb00327c8 + W[26];
+	t2 = e0(g) + Maj(g,h,a);    b+=t1;    f=t1+t2;
+	t1 = e + e1(b) + Ch(b,c,d) + 0xbf597fc7 + W[27];
+	t2 = e0(f) + Maj(f,g,h);    a+=t1;    e=t1+t2;
+	t1 = d + e1(a) + Ch(a,b,c) + 0xc6e00bf3 + W[28];
+	t2 = e0(e) + Maj(e,f,g);    h+=t1;    d=t1+t2;
+	t1 = c + e1(h) + Ch(h,a,b) + 0xd5a79147 + W[29];
+	t2 = e0(d) + Maj(d,e,f);    g+=t1;    c=t1+t2;
+	t1 = b + e1(g) + Ch(g,h,a) + 0x06ca6351 + W[30];
+	t2 = e0(c) + Maj(c,d,e);    f+=t1;    b=t1+t2;
+	t1 = a + e1(f) + Ch(f,g,h) + 0x14292967 + W[31];
+	t2 = e0(b) + Maj(b,c,d);    e+=t1;    a=t1+t2;
+
+	t1 = h + e1(e) + Ch(e,f,g) + 0x27b70a85 + W[32];
+	t2 = e0(a) + Maj(a,b,c);    d+=t1;    h=t1+t2;
+	t1 = g + e1(d) + Ch(d,e,f) + 0x2e1b2138 + W[33];
+	t2 = e0(h) + Maj(h,a,b);    c+=t1;    g=t1+t2;
+	t1 = f + e1(c) + Ch(c,d,e) + 0x4d2c6dfc + W[34];
+	t2 = e0(g) + Maj(g,h,a);    b+=t1;    f=t1+t2;
+	t1 = e + e1(b) + Ch(b,c,d) + 0x53380d13 + W[35];
+	t2 = e0(f) + Maj(f,g,h);    a+=t1;    e=t1+t2;
+	t1 = d + e1(a) + Ch(a,b,c) + 0x650a7354 + W[36];
+	t2 = e0(e) + Maj(e,f,g);    h+=t1;    d=t1+t2;
+	t1 = c + e1(h) + Ch(h,a,b) + 0x766a0abb + W[37];
+	t2 = e0(d) + Maj(d,e,f);    g+=t1;    c=t1+t2;
+	t1 = b + e1(g) + Ch(g,h,a) + 0x81c2c92e + W[38];
+	t2 = e0(c) + Maj(c,d,e);    f+=t1;    b=t1+t2;
+	t1 = a + e1(f) + Ch(f,g,h) + 0x92722c85 + W[39];
+	t2 = e0(b) + Maj(b,c,d);    e+=t1;    a=t1+t2;
+
+	t1 = h + e1(e) + Ch(e,f,g) + 0xa2bfe8a1 + W[40];
+	t2 = e0(a) + Maj(a,b,c);    d+=t1;    h=t1+t2;
+	t1 = g + e1(d) + Ch(d,e,f) + 0xa81a664b + W[41];
+	t2 = e0(h) + Maj(h,a,b);    c+=t1;    g=t1+t2;
+	t1 = f + e1(c) + Ch(c,d,e) + 0xc24b8b70 + W[42];
+	t2 = e0(g) + Maj(g,h,a);    b+=t1;    f=t1+t2;
+	t1 = e + e1(b) + Ch(b,c,d) + 0xc76c51a3 + W[43];
+	t2 = e0(f) + Maj(f,g,h);    a+=t1;    e=t1+t2;
+	t1 = d + e1(a) + Ch(a,b,c) + 0xd192e819 + W[44];
+	t2 = e0(e) + Maj(e,f,g);    h+=t1;    d=t1+t2;
+	t1 = c + e1(h) + Ch(h,a,b) + 0xd6990624 + W[45];
+	t2 = e0(d) + Maj(d,e,f);    g+=t1;    c=t1+t2;
+	t1 = b + e1(g) + Ch(g,h,a) + 0xf40e3585 + W[46];
+	t2 = e0(c) + Maj(c,d,e);    f+=t1;    b=t1+t2;
+	t1 = a + e1(f) + Ch(f,g,h) + 0x106aa070 + W[47];
+	t2 = e0(b) + Maj(b,c,d);    e+=t1;    a=t1+t2;
+
+	t1 = h + e1(e) + Ch(e,f,g) + 0x19a4c116 + W[48];
+	t2 = e0(a) + Maj(a,b,c);    d+=t1;    h=t1+t2;
+	t1 = g + e1(d) + Ch(d,e,f) + 0x1e376c08 + W[49];
+	t2 = e0(h) + Maj(h,a,b);    c+=t1;    g=t1+t2;
+	t1 = f + e1(c) + Ch(c,d,e) + 0x2748774c + W[50];
+	t2 = e0(g) + Maj(g,h,a);    b+=t1;    f=t1+t2;
+	t1 = e + e1(b) + Ch(b,c,d) + 0x34b0bcb5 + W[51];
+	t2 = e0(f) + Maj(f,g,h);    a+=t1;    e=t1+t2;
+	t1 = d + e1(a) + Ch(a,b,c) + 0x391c0cb3 + W[52];
+	t2 = e0(e) + Maj(e,f,g);    h+=t1;    d=t1+t2;
+	t1 = c + e1(h) + Ch(h,a,b) + 0x4ed8aa4a + W[53];
+	t2 = e0(d) + Maj(d,e,f);    g+=t1;    c=t1+t2;
+	t1 = b + e1(g) + Ch(g,h,a) + 0x5b9cca4f + W[54];
+	t2 = e0(c) + Maj(c,d,e);    f+=t1;    b=t1+t2;
+	t1 = a + e1(f) + Ch(f,g,h) + 0x682e6ff3 + W[55];
+	t2 = e0(b) + Maj(b,c,d);    e+=t1;    a=t1+t2;
+
+	t1 = h + e1(e) + Ch(e,f,g) + 0x748f82ee + W[56];
+	t2 = e0(a) + Maj(a,b,c);    d+=t1;    h=t1+t2;
+	t1 = g + e1(d) + Ch(d,e,f) + 0x78a5636f + W[57];
+	t2 = e0(h) + Maj(h,a,b);    c+=t1;    g=t1+t2;
+	t1 = f + e1(c) + Ch(c,d,e) + 0x84c87814 + W[58];
+	t2 = e0(g) + Maj(g,h,a);    b+=t1;    f=t1+t2;
+	t1 = e + e1(b) + Ch(b,c,d) + 0x8cc70208 + W[59];
+	t2 = e0(f) + Maj(f,g,h);    a+=t1;    e=t1+t2;
+	t1 = d + e1(a) + Ch(a,b,c) + 0x90befffa + W[60];
+	t2 = e0(e) + Maj(e,f,g);    h+=t1;    d=t1+t2;
+	t1 = c + e1(h) + Ch(h,a,b) + 0xa4506ceb + W[61];
+	t2 = e0(d) + Maj(d,e,f);    g+=t1;    c=t1+t2;
+	t1 = b + e1(g) + Ch(g,h,a) + 0xbef9a3f7 + W[62];
+	t2 = e0(c) + Maj(c,d,e);    f+=t1;    b=t1+t2;
+	t1 = a + e1(f) + Ch(f,g,h) + 0xc67178f2 + W[63];
+	t2 = e0(b) + Maj(b,c,d);    e+=t1;    a=t1+t2;
+
+	state[0] += a; state[1] += b; state[2] += c; state[3] += d;
+	state[4] += e; state[5] += f; state[6] += g; state[7] += h;
+
+	/* clear any sensitive info... */
+	a = b = c = d = e = f = g = h = t1 = t2 = 0;
+	memset(W, 0, 64 * sizeof(u32));
+}
+
+
+static int sha256_init(struct shash_desc *desc)
+{
+	struct sha256_state *sctx = shash_desc_ctx(desc);
+
+#if defined(CONFIG_FIPS_FMP)
+	if (unlikely(in_fmp_fips_err())) {
+		printk(KERN_ERR "FMP sha256_init failed due to fips error\n");
+		return -EPERM;
+	}
+#endif
+	sctx->state[0] = SHA256_H0;
+	sctx->state[1] = SHA256_H1;
+	sctx->state[2] = SHA256_H2;
+	sctx->state[3] = SHA256_H3;
+	sctx->state[4] = SHA256_H4;
+	sctx->state[5] = SHA256_H5;
+	sctx->state[6] = SHA256_H6;
+	sctx->state[7] = SHA256_H7;
+	sctx->count = 0;
+
+	return 0;
+}
+
+static int sha256_update(struct shash_desc *desc, const u8 *data,
+	unsigned int len)
+{
+	struct sha256_state *sctx = shash_desc_ctx(desc);
+	unsigned int partial, done;
+	const u8 *src;
+
+	partial = sctx->count & 0x3f;
+	sctx->count += len;
+	done = 0;
+	src = data;
+
+	if ((partial + len) > 63) {
+		if (partial) {
+			done = -partial;
+			memcpy(sctx->buf + partial, data, done + 64);
+			src = sctx->buf;
+		}
+
+		do {
+			sha256_transform(sctx->state, src);
+			done += 64;
+			src = data + done;
+		} while (done + 63 < len);
+
+		partial = 0;
+	}
+	memcpy(sctx->buf + partial, src, len - done);
+
+	return 0;
+}
+
+static int sha256_final(struct shash_desc *desc, u8 *out)
+{
+	struct sha256_state *sctx = shash_desc_ctx(desc);
+	__be32 *dst = (__be32 *)out;
+	__be64 bits;
+	unsigned int index, pad_len;
+	int i;
+	static const u8 padding[64] = { 0x80, };
+
+	/* Save number of bits */
+	bits = cpu_to_be64(sctx->count << 3);
+
+	/* Pad out to 56 mod 64. */
+	index = sctx->count & 0x3f;
+	pad_len = (index < 56) ? (56 - index) : ((64+56) - index);
+	sha256_update(desc, padding, pad_len);
+
+	/* Append length (before padding) */
+	sha256_update(desc, (const u8 *)&bits, sizeof(bits));
+
+	/* Store state in digest */
+	for (i = 0; i < 8; i++)
+		dst[i] = cpu_to_be32(sctx->state[i]);
+
+	/* Zeroize sensitive information. */
+	memset(sctx, 0, sizeof(*sctx));
+
+	printk(KERN_INFO "%s: fmp sha256 S/W final is done.\n", __func__);
+	return 0;
+}
+
+static int sha256_export(struct shash_desc *desc, void *out)
+{
+	struct sha256_state *sctx = shash_desc_ctx(desc);
+
+	memcpy(out, sctx, sizeof(*sctx));
+	return 0;
+}
+
+static int sha256_import(struct shash_desc *desc, const void *in)
+{
+	struct sha256_state *sctx = shash_desc_ctx(desc);
+
+	memcpy(sctx, in, sizeof(*sctx));
+	return 0;
+}
+
+static struct shash_alg sha256_algs[1] = { {
+	.digestsize	=	SHA256_DIGEST_SIZE,
+	.init		=	sha256_init,
+	.update		=	sha256_update,
+	.final		=	sha256_final,
+	.export		=	sha256_export,
+	.import		=	sha256_import,
+	.descsize	=	sizeof(struct sha256_state),
+	.statesize	=	sizeof(struct sha256_state),
+	.base		=	{
+		.cra_name	=	"sha256-fmp",
+		.cra_driver_name=	"sha256-fmp",
+		.cra_flags	=	CRYPTO_ALG_TYPE_SHASH,
+		.cra_blocksize	=	SHA256_BLOCK_SIZE,
+		.cra_module	=	THIS_MODULE,
+	}
+}
+};
+
+static int __init sha256_generic_mod_init(void)
+{
+	return crypto_register_shashes(sha256_algs, ARRAY_SIZE(sha256_algs));
+}
+
+static void __exit sha256_generic_mod_fini(void)
+{
+	crypto_unregister_shashes(sha256_algs, ARRAY_SIZE(sha256_algs));
+}
+
+module_init(sha256_generic_mod_init);
+module_exit(sha256_generic_mod_fini);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("SHA-256 Secure Hash Algorithm");
+
+MODULE_ALIAS("sha256-fmp");