[GitHub/MotorolaMobilityLLC/kernel-slsi.git] / drivers / crypto / fmp / fmp_test.c

/*
 * Exynos FMP cipher driver
 *
 * Copyright (C) 2016 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/of.h>
#include <linux/of_platform.h>
#include <linux/crypto.h>
#include <linux/buffer_head.h>
#include <linux/genhd.h>
#include <linux/delay.h>
#include <linux/fs.h>
#include <linux/blk_types.h>
#include <crypto/fmp.h>

#include "fmp_test.h"

#define MAX_SCAN_PART	(50)
#define MAX_RETRY_COUNT (0x100000)

static dev_t find_devt_for_test(struct exynos_fmp *fmp,
				struct fmp_test_data *data)
{
	int i, idx = 0;
	uint32_t count = 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;
	struct device *dev = fmp->dev;
	fmode_t fmode = FMODE_WRITE | FMODE_READ;

	memset(size_list, 0, sizeof(size_list));
	memset(devt_list, 0, sizeof(devt_list));
	do {
		for (i = 1; i < MAX_SCAN_PART; i++) {
			devt_scan = blk_lookup_devt(data->block_type, 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;
				blkdev_put(bdev, fmode);
			}
		}
		if (!idx) {
			mdelay(100);
			count++;
			continue;
		}
		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];
			}
		}
		bdev = blkdev_get_by_dev(devt, fmode, NULL);
		dev_dbg(dev, "Found partno %d for FMP test\n",
			bdev->bd_part->partno);
		blkdev_put(bdev, fmode);
		return devt;
	} while (count < MAX_RETRY_COUNT);
	dev_err(dev, "Block device isn't initialized yet for FMP test\n");
	return (dev_t) 0;
}

static int get_fmp_host_type(struct device *dev,
				    struct fmp_test_data *data)
{
	int ret;
	struct device_node *node = dev->of_node;
	const char *type;

	ret =
	    of_property_read_string_index(node, "exynos,block-type", 0, &type);
	if (ret) {
		pr_err("%s: Could not get block type\n", __func__);
		return ret;
	}
	strlcpy(data->block_type, type, FMP_BLOCK_TYPE_NAME_LEN);
	return 0;
}

static int get_fmp_test_block_offset(struct device *dev,
				      struct fmp_test_data *data)
{
	int ret = 0;
	struct device_node *node = dev->of_node;
	uint32_t offset;

	ret = of_property_read_u32(node, "exynos,fips-block_offset", &offset);
	if (ret) {
		pr_err("%s: Could not get fips test block offset\n", __func__);
		ret = -EINVAL;
		goto err;
	}
	data->test_block_offset = offset;
err:
	return ret;
}

/* test block device init for fmp test */
struct fmp_test_data *fmp_test_init(struct exynos_fmp *fmp)
{
	int ret = 0;
	struct fmp_test_data *data;
	struct device *dev;
	struct inode *inode;
	struct super_block *sb;
	unsigned long blocksize;
	unsigned char blocksize_bits;
	fmode_t fmode = FMODE_WRITE | FMODE_READ;

	if (!fmp) {
		pr_err("%s: Invalid exynos fmp struct\n", __func__);
		return NULL;
	}
	data = kmalloc(sizeof(struct fmp_test_data), GFP_KERNEL);
	if (!data)
		return NULL;

	dev = fmp->dev;
	ret = get_fmp_host_type(dev, data);
	if (ret) {
		dev_err(dev, "%s: Fail to get host type. ret(%d)", __func__,
			ret);
		goto err;
	}
	data->devt = find_devt_for_test(fmp, data);
	if (!data->devt) {
		dev_err(dev, "%s: Fail to find devt for self test\n", __func__);
		goto err;
	}
	data->bdev = blkdev_get_by_dev(data->devt, fmode, NULL);
	if (IS_ERR(data->bdev)) {
		dev_err(dev, "%s: Fail to open block device\n", __func__);
		goto err;
	}
	ret = get_fmp_test_block_offset(dev, data);
	if (ret) {
		dev_err(dev, "%s: Fail to get fips offset. ret(%d)\n",
			__func__, ret);
		goto err;
	}
	inode = data->bdev->bd_inode;
	sb = inode->i_sb;
	blocksize = sb->s_blocksize;
	blocksize_bits = sb->s_blocksize_bits;
	data->sector =
	    (i_size_read(inode) -
	     (blocksize * data->test_block_offset)) >> blocksize_bits;

	return data;
err:
	kzfree(data);
	return NULL;
}

int fmp_cipher_run(struct exynos_fmp *fmp, struct fmp_test_data *fdata,
		uint8_t *data, uint32_t len, bool bypass, uint32_t write,
		void *priv, struct fmp_crypto_info *ci)
{
	int ret = 0;
	struct device *dev;
	static struct buffer_head *bh;
	u32 org_algo_mode;
	int op_flags;

	if (!fmp || !fdata || !ci) {
		pr_err("%s: Invalid fmp struct: %p, %p, %p\n",
			__func__, fmp, fdata, ci);
		return -EINVAL;
	}
	dev = fmp->dev;

	bh = __getblk(fdata->bdev, fdata->sector, FMP_BLK_SIZE);
	if (!bh) {
		dev_err(dev, "%s: Fail to get block from bdev\n", __func__);
		return -ENODEV;
	}

	/* set algo_mode for test */
	org_algo_mode = ci->algo_mode;
	if (bypass)
		ci->algo_mode = EXYNOS_FMP_BYPASS_MODE;
	ci->algo_mode |= EXYNOS_FMP_ALGO_MODE_TEST;

	get_bh(bh);
	/* priv is diskc for crypto test. */
	if (!priv) {
		/* ci is fmp_test_data->ci */
		fmp->test_data = fdata;
		ci->ctx = fmp;
		ci->use_diskc = 0;
		ci->enc_mode = EXYNOS_FMP_FILE_ENC;
		bh->b_private = ci;
	} else {
		/* ci is crypto_tfm_ctx(tfm) */
		bh->b_private = priv;
	}
	op_flags = REQ_CRYPT;

	if (write == WRITE_MODE) {
		memcpy(bh->b_data, data, len);
		set_buffer_dirty(bh);
		ret = __sync_dirty_buffer(bh, op_flags | REQ_SYNC);
		if (ret) {
			dev_err(dev, "%s: IO error syncing for write mode\n",
				__func__);
			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(REQ_OP_READ, REQ_SYNC | REQ_PRIO | op_flags, bh);
		wait_on_buffer(bh);
		if (unlikely(!buffer_uptodate(bh))) {
			ret = -EIO;
			goto out;
		}
		memcpy(data, bh->b_data, len);
	}
out:
	if (ci)
		ci->algo_mode = org_algo_mode;
	put_bh(bh);
	return ret;
}

int fmp_test_crypt(struct exynos_fmp *fmp, struct fmp_test_data *fdata,
		uint8_t *src, uint8_t *dst, uint32_t len, uint32_t enc,
		void *priv, struct fmp_crypto_info *ci)
{
	int ret = 0;

	if (!fdata) {
		pr_err("%s: Invalid exynos fmp struct\n", __func__);
		return -1;
	}

	if (enc == ENCRYPT) {
		ret = fmp_cipher_run(fmp, fdata, src, len, 0,
				WRITE_MODE, priv, ci);
		if (ret) {
			pr_err("Fail to run fmp cipher ret(%d)\n",
				ret);
			goto err;
		}
		ret = fmp_cipher_run(fmp, fdata, dst, len, 1,
				READ_MODE, priv, ci);
		if (ret) {
			pr_err("Fail to run fmp cipher ret(%d)\n",
				ret);
			goto err;
		}
	} else if (enc == DECRYPT) {
		ret = fmp_cipher_run(fmp, fdata, src, len, 1,
				WRITE_MODE, priv, ci);
		if (ret) {
			pr_err("Fail to run fmp cipher ret(%d)\n",
				ret);
			goto err;
		}
		ret = fmp_cipher_run(fmp, fdata, dst, len, 0,
				READ_MODE, priv, ci);
		if (ret) {
			pr_err("Fail to run fmp cipher ret(%d)\n",
				ret);
			goto err;
		}
	} else {
		pr_err("%s: Invalid enc %d mode\n", __func__, enc);
		goto err;
	}

	return 0;
err:
	return -EINVAL;
}

/* test block device release for fmp test */
void fmp_test_exit(struct fmp_test_data *fdata)
{
	fmode_t fmode = FMODE_WRITE | FMODE_READ;

	if (!fdata) {
		pr_err("%s: Invalid exynos fmp struct\n", __func__);
		return;
	}
	if (fdata->bdev)
		blkdev_put(fdata->bdev, fmode);
	kzfree(fdata);
}
Commit	Line	Data
e9be92a8 BK	1	/*
	2	* Exynos FMP cipher driver
	3	*
	4	* Copyright (C) 2016 Samsung Electronics Co., Ltd.
	5	*
	6	* This program is free software; you can redistribute it and/or modify
	7	* it under the terms of the GNU General Public License as published by
	8	* the Free Software Foundation; either version 2 of the License, or
	9	* (at your option) any later version.
	10	*/
	11
	12	#include <linux/of.h>
	13	#include <linux/of_platform.h>
	14	#include <linux/crypto.h>
	15	#include <linux/buffer_head.h>
	16	#include <linux/genhd.h>
	17	#include <linux/delay.h>
	18	#include <linux/fs.h>
	19	#include <linux/blk_types.h>
	20	#include <crypto/fmp.h>
	21
	22	#include "fmp_test.h"
	23
	24	#define MAX_SCAN_PART (50)
	25	#define MAX_RETRY_COUNT (0x100000)
	26
	27	static dev_t find_devt_for_test(struct exynos_fmp *fmp,
	28	struct fmp_test_data *data)
	29	{
	30	int i, idx = 0;
	31	uint32_t count = 0;
	32	uint64_t size;
	33	uint64_t size_list[MAX_SCAN_PART];
	34	dev_t devt_list[MAX_SCAN_PART];
	35	dev_t devt_scan, devt;
	36	struct block_device *bdev;
	37	struct device *dev = fmp->dev;
	38	fmode_t fmode = FMODE_WRITE \| FMODE_READ;
	39
	40	memset(size_list, 0, sizeof(size_list));
	41	memset(devt_list, 0, sizeof(devt_list));
e9be92a8 BK	42	do {
	43	for (i = 1; i < MAX_SCAN_PART; i++) {
	44	devt_scan = blk_lookup_devt(data->block_type, i);
	45	bdev = blkdev_get_by_dev(devt_scan, fmode, NULL);
	46	if (IS_ERR(bdev))
	47	continue;
	48	else {
	49	size_list[idx] =
	50	(uint64_t) i_size_read(bdev->bd_inode);
	51	devt_list[idx++] = devt_scan;
	52	blkdev_put(bdev, fmode);
	53	}
	54	}
	55	if (!idx) {
	56	mdelay(100);
	57	count++;
	58	continue;
	59	}
	60	for (i = 0; i < idx; i++) {
	61	if (i == 0) {
	62	size = size_list[i];
	63	devt = devt_list[i];
	64	} else {
	65	if (size < size_list[i])
	66	devt = devt_list[i];
	67	}
	68	}
	69	bdev = blkdev_get_by_dev(devt, fmode, NULL);
	70	dev_dbg(dev, "Found partno %d for FMP test\n",
	71	bdev->bd_part->partno);
	72	blkdev_put(bdev, fmode);
	73	return devt;
	74	} while (count < MAX_RETRY_COUNT);
	75	dev_err(dev, "Block device isn't initialized yet for FMP test\n");
	76	return (dev_t) 0;
	77	}
	78
	79	static int get_fmp_host_type(struct device *dev,
	80	struct fmp_test_data *data)
	81	{
	82	int ret;
	83	struct device_node *node = dev->of_node;
	84	const char *type;
	85
	86	ret =
	87	of_property_read_string_index(node, "exynos,block-type", 0, &type);
	88	if (ret) {
	89	pr_err("%s: Could not get block type\n", __func__);
	90	return ret;
	91	}
	92	strlcpy(data->block_type, type, FMP_BLOCK_TYPE_NAME_LEN);
	93	return 0;
	94	}
	95
	96	static int get_fmp_test_block_offset(struct device *dev,
	97	struct fmp_test_data *data)
	98	{
	99	int ret = 0;
	100	struct device_node *node = dev->of_node;
	101	uint32_t offset;
	102
	103	ret = of_property_read_u32(node, "exynos,fips-block_offset", &offset);
	104	if (ret) {
	105	pr_err("%s: Could not get fips test block offset\n", __func__);
106	ret = -EINVAL;
107	goto err;
108	}
109	data->test_block_offset = offset;
110	err:
111	return ret;
112	}
113
114	/* test block device init for fmp test */
115	struct fmp_test_data fmp_test_init(struct exynos_fmp fmp)
116	{
117	int ret = 0;
118	struct fmp_test_data *data;
2b13c4c4	119	struct device *dev;
e9be92a8 BK	120	struct inode *inode;
	121	struct super_block *sb;
	122	unsigned long blocksize;
	123	unsigned char blocksize_bits;
	124	fmode_t fmode = FMODE_WRITE \| FMODE_READ;
	125
	126	if (!fmp) {
2b13c4c4	127	pr_err("%s: Invalid exynos fmp struct\n", __func__);
e9be92a8 BK	128	return NULL;
	129	}
	130	data = kmalloc(sizeof(struct fmp_test_data), GFP_KERNEL);
	131	if (!data)
	132	return NULL;
	133
2b13c4c4	134	dev = fmp->dev;
e9be92a8 BK	135	ret = get_fmp_host_type(dev, data);
	136	if (ret) {
	137	dev_err(dev, "%s: Fail to get host type. ret(%d)", __func__,
	138	ret);
	139	goto err;
	140	}
	141	data->devt = find_devt_for_test(fmp, data);
	142	if (!data->devt) {
	143	dev_err(dev, "%s: Fail to find devt for self test\n", __func__);
	144	goto err;
	145	}
	146	data->bdev = blkdev_get_by_dev(data->devt, fmode, NULL);
	147	if (IS_ERR(data->bdev)) {
	148	dev_err(dev, "%s: Fail to open block device\n", __func__);
	149	goto err;
	150	}
	151	ret = get_fmp_test_block_offset(dev, data);
	152	if (ret) {
	153	dev_err(dev, "%s: Fail to get fips offset. ret(%d)\n",
	154	__func__, ret);
	155	goto err;
	156	}
	157	inode = data->bdev->bd_inode;
	158	sb = inode->i_sb;
	159	blocksize = sb->s_blocksize;
	160	blocksize_bits = sb->s_blocksize_bits;
	161	data->sector =
	162	(i_size_read(inode) -
	163	(blocksize * data->test_block_offset)) >> blocksize_bits;
	164
	165	return data;
	166	err:
	167	kzfree(data);
	168	return NULL;
	169	}
	170
	171	int fmp_cipher_run(struct exynos_fmp fmp, struct fmp_test_data fdata,
	172	uint8_t *data, uint32_t len, bool bypass, uint32_t write,
	173	void priv, struct fmp_crypto_info ci)
	174	{
	175	int ret = 0;
6417c908	176	struct device *dev;
e9be92a8 BK	177	static struct buffer_head *bh;
	178	u32 org_algo_mode;
	179	int op_flags;
	180
6417c908 BK	181	if (!fmp \|\| !fdata \|\| !ci) {
	182	pr_err("%s: Invalid fmp struct: %p, %p, %p\n",
	183	__func__, fmp, fdata, ci);
e9be92a8	184	return -EINVAL;
6417c908 BK	185	}
6417c908 BK	186	dev = fmp->dev;
e9be92a8 BK	187
	188	bh = __getblk(fdata->bdev, fdata->sector, FMP_BLK_SIZE);
	189	if (!bh) {
	190	dev_err(dev, "%s: Fail to get block from bdev\n", __func__);
	191	return -ENODEV;
	192	}
	193
	194	/* set algo_mode for test */
	195	org_algo_mode = ci->algo_mode;
	196	if (bypass)
	197	ci->algo_mode = EXYNOS_FMP_BYPASS_MODE;
	198	ci->algo_mode \|= EXYNOS_FMP_ALGO_MODE_TEST;
	199
	200	get_bh(bh);
	201	/* priv is diskc for crypto test. */
	202	if (!priv) {
	203	/* ci is fmp_test_data->ci */
	204	fmp->test_data = fdata;
	205	ci->ctx = fmp;
	206	ci->use_diskc = 0;
	207	ci->enc_mode = EXYNOS_FMP_FILE_ENC;
	208	bh->b_private = ci;
	209	} else {
	210	/* ci is crypto_tfm_ctx(tfm) */
	211	bh->b_private = priv;
	212	}
e4bdfe04	213	op_flags = REQ_CRYPT;
e9be92a8 BK	214
	215	if (write == WRITE_MODE) {
	216	memcpy(bh->b_data, data, len);
	217	set_buffer_dirty(bh);
	218	ret = __sync_dirty_buffer(bh, op_flags \| REQ_SYNC);
	219	if (ret) {
	220	dev_err(dev, "%s: IO error syncing for write mode\n",
	221	__func__);
	222	ret = -EIO;
	223	goto out;
	224	}
	225	memset(bh->b_data, 0, FMP_BLK_SIZE);
	226	} else {
	227	lock_buffer(bh);
	228	bh->b_end_io = end_buffer_read_sync;
	229	submit_bh(REQ_OP_READ, REQ_SYNC \| REQ_PRIO \| op_flags, bh);
	230	wait_on_buffer(bh);
	231	if (unlikely(!buffer_uptodate(bh))) {
	232	ret = -EIO;
	233	goto out;
	234	}
	235	memcpy(data, bh->b_data, len);
	236	}
	237	out:
	238	if (ci)
	239	ci->algo_mode = org_algo_mode;
	240	put_bh(bh);
	241	return ret;
	242	}
	243
	244	int fmp_test_crypt(struct exynos_fmp fmp, struct fmp_test_data fdata,
	245	uint8_t src, uint8_t dst, uint32_t len, uint32_t enc,
	246	void priv, struct fmp_crypto_info ci)
	247	{
	248	int ret = 0;
	249
	250	if (!fdata) {
	251	pr_err("%s: Invalid exynos fmp struct\n", __func__);
	252	return -1;
	253	}
	254
	255	if (enc == ENCRYPT) {
	256	ret = fmp_cipher_run(fmp, fdata, src, len, 0,
	257	WRITE_MODE, priv, ci);
	258	if (ret) {
	259	pr_err("Fail to run fmp cipher ret(%d)\n",
	260	ret);
	261	goto err;
	262	}
	263	ret = fmp_cipher_run(fmp, fdata, dst, len, 1,
	264	READ_MODE, priv, ci);
	265	if (ret) {
	266	pr_err("Fail to run fmp cipher ret(%d)\n",
	267	ret);
	268	goto err;
	269	}
	270	} else if (enc == DECRYPT) {
	271	ret = fmp_cipher_run(fmp, fdata, src, len, 1,
	272	WRITE_MODE, priv, ci);
	273	if (ret) {
	274	pr_err("Fail to run fmp cipher ret(%d)\n",
	275	ret);
	276	goto err;
	277	}
278	ret = fmp_cipher_run(fmp, fdata, dst, len, 0,
279	READ_MODE, priv, ci);
280	if (ret) {
281	pr_err("Fail to run fmp cipher ret(%d)\n",
282	ret);
283	goto err;
284	}
285	} else {
286	pr_err("%s: Invalid enc %d mode\n", __func__, enc);
287	goto err;
288	}
289
290	return 0;
291	err:
292	return -EINVAL;
293	}
294
295	/* test block device release for fmp test */
296	void fmp_test_exit(struct fmp_test_data *fdata)
297	{
298	fmode_t fmode = FMODE_WRITE \| FMODE_READ;
299
6417c908 BK	300	if (!fdata) {
6417c908 BK	301	pr_err("%s: Invalid exynos fmp struct\n", __func__);
e9be92a8	302	return;
6417c908	303	}
e9be92a8 BK	304	if (fdata->bdev)
	305	blkdev_put(fdata->bdev, fmode);
	306	kzfree(fdata);
	307	}