From dba1718a2a37aa498d3af837a0d6305bd4aad153 Mon Sep 17 00:00:00 2001
From: Nikos Mavrogiannopoulos <nmav@gnutls.org>
Date: Wed, 23 Nov 2011 23:35:42 +0100
Subject: [PATCH 147/291] cryptodev: Allow

---
 .../staging/crypto/cryptodev/crypto/cryptodev.h    |  50 +++-
 drivers/staging/crypto/cryptodev/cryptodev_auth.c  | 240 +++++++++++++++---
 .../staging/crypto/cryptodev/cryptodev_cipher.c    |   3 +-
 drivers/staging/crypto/cryptodev/cryptodev_int.h   |   5 +-
 drivers/staging/crypto/cryptodev/cryptodev_main.c  |   6 +-
 .../crypto/cryptodev/examples/cipher-aead-srtp.c   | 273 +++++++++++++++++++++
 .../crypto/cryptodev/examples/cipher-aead.c        |   4 +-
 7 files changed, 539 insertions(+), 42 deletions(-)
 create mode 100644 drivers/staging/crypto/cryptodev/examples/cipher-aead-srtp.c

diff --git a/drivers/staging/crypto/cryptodev/crypto/cryptodev.h b/drivers/staging/crypto/cryptodev/crypto/cryptodev.h
index 0610282d16d8..f56a043ba93d 100644
--- a/drivers/staging/crypto/cryptodev/crypto/cryptodev.h
+++ b/drivers/staging/crypto/cryptodev/crypto/cryptodev.h
@@ -121,20 +121,56 @@ struct crypt_op {
 struct crypt_auth_op {
 	__u32	ses;		/* session identifier */
 	__u16	op;		/* COP_ENCRYPT or COP_DECRYPT */
-	__u16	flags;		/* see COP_FLAG_* */
+	__u16	flags;		/* see COP_FLAG_AEAD_* */
 	__u32	len;		/* length of source data */
-	__u32	auth_len;		/* length of auth data */
-	__u32	tag_len;		/* the length of the tag */
+	__u32	auth_len;	/* length of auth data */
+	__u32	tag_len;	/* the length of the tag. Use zero for digest size. */
 	__u8	__user *auth_src;	/* authenticated-only data */
+
+	/* The current implementation is more efficient if data are
+	 * encrypted in-place (src==dst). */
 	__u8	__user *src;	/* data to be encrypted and authenticated */
 	__u8	__user *dst;	/* pointer to output data. Must have
-	                         * space for tag. This should be at least 
-	                         * src_len + tag_size + block_size for padding */
+	                         * space for tag. For TLS this should be at least 
+	                         * len + tag_size + block_size for padding */
+
+	__u8    __user *tag;    /* where the tag will be copied to. TLS mode
+                                 * doesn't use that as tag is copied to dst.
+                                 * SRTP mode copies tag there. */
 
 	/* initialization vector for encryption operations */
 	__u8	__user *iv;
 };
 
+/* In TLS mode the following are required:
+ *  flags   : COP_FLAG_AEAD_TLS_TYPE
+ *  iv      : the initialization vector
+ *  auth_len: the length of the data to be authenticated only
+ *  len     : length of data to be encrypted
+ *  auth_src: the data to be authenticated
+ *  src     : the data to be encrypted
+ *  dst     : space to hold encrypted data (preferably in-place). It must have
+ *            at least a size of len + tag_size + blocksize.
+ *  tag_size: the size of the desired authentication tag or zero to use
+ *            the default mac output.
+ */
+
+/* In SRTP mode the following are required:
+ *  flags   : COP_FLAG_AEAD_SRTP_TYPE
+ *  iv      : the initialization vector
+ *  auth_len: the length of the data to be authenticated. This must
+ *            include the SRTP header + SRTP payload (data to be encrypted) + rest
+ *            
+ *  len     : length of data to be encrypted
+ *  auth_src: pointer the data to be authenticated. Should point at the same buffer as src.
+ *  src     : pointer to the data to be encrypted.
+ *  dst     : This is mandatory to be the same as src (in-place only).
+ *  tag_size: the size of the desired authentication tag or zero to use
+ *            the default mac output.
+ *  tag     : Pointer to an address where the authentication tag will be copied.
+ */
+
+
 /* struct crypt_op flags */
 
 #define COP_FLAG_NONE		(0 << 0) /* totally no flag */
@@ -143,7 +179,9 @@ struct crypt_auth_op {
 #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 
-                                          * protocol TLS rules */
+                                          * TLS protocol rules */
+#define COP_FLAG_AEAD_SRTP_TYPE  (1 << 5) /* authenticate and encrypt using the 
+                                           * SRTP protocol rules */
 
 
 /* Stuff for bignum arithmetic and public key
diff --git a/drivers/staging/crypto/cryptodev/cryptodev_auth.c b/drivers/staging/crypto/cryptodev/cryptodev_auth.c
index 546f295fdf56..fdbfff675efc 100644
--- a/drivers/staging/crypto/cryptodev/cryptodev_auth.c
+++ b/drivers/staging/crypto/cryptodev/cryptodev_auth.c
@@ -1,8 +1,7 @@
 /*
  * Driver for /dev/crypto device (aka CryptoDev)
  *
- * Copyright (c) 2004 Michal Ludvig <mludvig@logix.net.nz>, SuSE Labs
- * Copyright (c) 2009,2010 Nikos Mavrogiannopoulos <nmav@gnutls.org>
+ * Copyright (c) 2011 Nikos Mavrogiannopoulos <nmav@gnutls.org>
  *
  * This file is part of linux cryptodev.
  *
@@ -23,12 +22,7 @@
  */
 
 /*
- * Device /dev/crypto provides an interface for
- * accessing kernel CryptoAPI algorithms (ciphers,
- * hashes) from userspace programs.
- *
- * /dev/crypto interface was originally introduced in
- * OpenBSD and this module attempts to keep the API.
+ * This file handles the AEAD part of /dev/crypto.
  *
  */
 
@@ -49,13 +43,8 @@
 #include "version.h"
 
 
-/* Idea zero-copy src data 
- * Read source data and encrypt to temporary buffer
- * then copy temporary buffer to dst.
- */
-
 /* make cop->src and cop->dst available in scatterlists */
-static int get_userbuf3(struct csession *ses, struct kernel_crypt_auth_op *kcaop,
+static int get_userbuf_aead(struct csession *ses, struct kernel_crypt_auth_op *kcaop,
 			struct scatterlist **auth_sg, struct scatterlist **dst_sg, 
 			int *tot_pages)
 {
@@ -77,7 +66,7 @@ static int get_userbuf3(struct csession *ses, struct kernel_crypt_auth_op *kcaop
 	}
 
 	if (kcaop->dst_len == 0) {
-		dprintk(1, KERN_WARNING, "Destination cannot be zero\n");
+		dprintk(1, KERN_WARNING, "Destination length cannot be zero\n");
 		return -EINVAL;
 	}
 
@@ -119,6 +108,64 @@ static int get_userbuf3(struct csession *ses, struct kernel_crypt_auth_op *kcaop
 	return 0;
 }
 
+static int get_userbuf_srtp(struct csession *ses, struct kernel_crypt_auth_op *kcaop,
+			struct scatterlist **auth_sg, struct scatterlist **dst_sg, 
+			int *tot_pages)
+{
+	int pagecount, diff;
+	int auth_pagecount = 0;
+	struct crypt_auth_op *caop = &kcaop->caop;
+	int rc;
+
+	if (caop->dst == NULL && caop->auth_src == NULL)
+		return -EINVAL;
+
+	if (ses->alignmask) {
+		if (!IS_ALIGNED((unsigned long)caop->dst, ses->alignmask))
+			dprintk(2, KERN_WARNING, "%s: careful - source address %lx is not %d byte aligned\n",
+				__func__, (unsigned long)caop->dst, ses->alignmask + 1);
+		if (!IS_ALIGNED((unsigned long)caop->auth_src, ses->alignmask))
+			dprintk(2, KERN_WARNING, "%s: careful - source address %lx is not %d byte aligned\n",
+				__func__, (unsigned long)caop->auth_src, ses->alignmask + 1);
+	}
+
+	if (unlikely(kcaop->dst_len == 0 || caop->auth_len == 0)) {
+		dprintk(1, KERN_WARNING, "Destination length cannot be zero\n");
+		return -EINVAL;
+	}
+
+	/* Note that in SRTP auth data overlap with data to be encrypted (dst)
+         */
+
+	auth_pagecount = PAGECOUNT(caop->auth_src, caop->auth_len);
+	diff = (int)(caop->src - caop->auth_src);
+	if (diff > 256 || diff < 0) {
+		dprintk(1, KERN_WARNING, "auth_src must overlap with src (diff: %d).\n", diff);
+		return -EINVAL;
+	}
+
+	(*tot_pages) = pagecount = auth_pagecount;
+
+	rc = adjust_sg_array(ses, pagecount);
+	if (rc)
+		return rc;
+
+	rc = __get_userbuf(caop->auth_src, caop->auth_len, 1, auth_pagecount,
+			   ses->pages, ses->sg, kcaop->task, kcaop->mm);
+	if (unlikely(rc)) {
+		dprintk(1, KERN_ERR,
+			"failed to get user pages for data input\n");
+		return -EINVAL;
+	}
+	(*auth_sg) = ses->sg;
+
+	memcpy(&ses->sg2, ses->sg, sizeof(ses->sg[0]));
+	ses->sg2.offset += diff;
+	(*dst_sg) = &ses->sg2;
+
+	return 0;
+}
+
 
 static int fill_kcaop_from_caop(struct kernel_crypt_auth_op *kcaop, struct fcrypt *fcr)
 {
@@ -134,17 +181,36 @@ static int fill_kcaop_from_caop(struct kernel_crypt_auth_op *kcaop, struct fcryp
 	}
 
 	if (caop->src != caop->dst) {
-		dprintk(1, KERN_ERR,
-			"Only inplace encryption and decryption is supported\n");
-		rc = -EINVAL;
-		goto out_unlock;
+		dprintk(2, KERN_ERR,
+			"Non-inplace encryption and decryption is not efficient\n");
+		if (caop->len > sizeof(ses_ptr->buffer)) {
+			dprintk(1, KERN_ERR,
+				"The provided buffer is too large\n");
+			rc = -EINVAL;
+			goto out_unlock;
+		}
+
+		if (unlikely(copy_from_user(ses_ptr->buffer, caop->src, caop->len))) {
+			rc = -EFAULT;
+			goto out_unlock;
+		}
+
+		if (unlikely(copy_to_user(caop->dst, ses_ptr->buffer, caop->len))) {
+			rc = -EFAULT;
+			goto out_unlock;
+		}
+
 	}
 
 	if (caop->tag_len == 0)
 		caop->tag_len = ses_ptr->hdata.digestsize;
 
 	kcaop->ivlen = caop->iv ? ses_ptr->cdata.ivsize : 0;
-	kcaop->dst_len = caop->len + ses_ptr->cdata.blocksize /* pad */ + caop->tag_len;
+
+	if (caop->flags & COP_FLAG_AEAD_TLS_TYPE)
+		kcaop->dst_len = caop->len + ses_ptr->cdata.blocksize /* pad */ + caop->tag_len;
+	else
+		kcaop->dst_len = caop->len;
 
 	kcaop->task = current;
 	kcaop->mm = current->mm;
@@ -268,8 +334,7 @@ tls_auth_n_crypt(struct csession *ses_ptr, struct kernel_crypt_auth_op *kcaop,
 	uint8_t vhash[AALG_MAX_RESULT_LEN];
 	uint8_t hash_output[AALG_MAX_RESULT_LEN];
 
-	/* Always hash before encryption and after decryption. Maybe
-	 * we should introduce a flag to switch... TBD later on.
+	/* TLS authenticates the plaintext except for the padding.
 	 */
 	if (caop->op == COP_ENCRYPT) {
 		if (ses_ptr->hdata.init != 0) {
@@ -381,6 +446,112 @@ out_err:
 	return ret;
 }
 
+static int
+srtp_auth_n_crypt(struct csession *ses_ptr, struct kernel_crypt_auth_op *kcaop,
+		  struct scatterlist *auth_sg, uint32_t auth_len,
+		  struct scatterlist *dst_sg, uint32_t len)
+{
+	int ret, fail = 0;
+	struct crypt_auth_op *caop = &kcaop->caop;
+	uint8_t vhash[AALG_MAX_RESULT_LEN];
+	uint8_t hash_output[AALG_MAX_RESULT_LEN];
+
+	/* SRTP authenticates the encrypted data.
+	 */
+	if (caop->op == COP_ENCRYPT) {
+		if (ses_ptr->cdata.init != 0) {
+			if (ses_ptr->cdata.stream == 0) {
+				dprintk(0, KERN_ERR, "Only stream modes are allowed in SRTP mode\n");
+				ret = -EINVAL;
+				goto out_err;
+			}
+
+			ret = cryptodev_cipher_encrypt(&ses_ptr->cdata,
+							dst_sg, dst_sg, len);
+			if (unlikely(ret)) {
+				dprintk(0, KERN_ERR, "cryptodev_cipher_encrypt: %d\n", ret);
+				goto out_err;
+			}
+		}
+
+		if (ses_ptr->hdata.init != 0) {
+			if (auth_len > 0) {
+				ret = cryptodev_hash_update(&ses_ptr->hdata,
+								auth_sg, auth_len);
+				if (unlikely(ret)) {
+					dprintk(0, KERN_ERR, "cryptodev_hash_update: %d\n", ret);
+					goto out_err;
+				}
+			}
+
+			ret = cryptodev_hash_final(&ses_ptr->hdata, hash_output);
+			if (unlikely(ret)) {
+				dprintk(0, KERN_ERR, "cryptodev_hash_final: %d\n", ret);
+				goto out_err;
+			}
+
+			if (unlikely(copy_to_user(caop->tag, hash_output, caop->tag_len))) {
+				ret = -EFAULT;
+				goto out_err;
+			}
+		}
+
+	} else {
+		if (ses_ptr->hdata.init != 0) {
+			if (unlikely(caop->tag_len > sizeof(vhash) || caop->tag_len > len)) {
+				dprintk(1, KERN_ERR, "Illegal tag len size\n");
+				ret = -EINVAL;
+				goto out_err;
+			}
+
+			if (unlikely(copy_from_user(vhash, caop->tag, caop->tag_len))) {
+				ret = -EFAULT;
+				goto out_err;
+			}
+
+			ret = cryptodev_hash_update(&ses_ptr->hdata,
+							auth_sg, auth_len);
+			if (unlikely(ret)) {
+				dprintk(0, KERN_ERR, "cryptodev_hash_update: %d\n", ret);
+				goto out_err;
+			}
+
+			ret = cryptodev_hash_final(&ses_ptr->hdata, hash_output);
+			if (unlikely(ret)) {
+				dprintk(0, KERN_ERR, "cryptodev_hash_final: %d\n", ret);
+				goto out_err;
+			}
+
+			if (memcmp(vhash, hash_output, caop->tag_len) != 0 || fail != 0) {
+				dprintk(1, KERN_ERR, "MAC verification failed\n");
+				ret = -ECANCELED;
+				goto out_err;
+			}
+		}
+
+		if (ses_ptr->cdata.init != 0) {
+			if (ses_ptr->cdata.stream == 0) {
+				dprintk(0, KERN_ERR, "Only stream modes are allowed in SRTP mode\n");
+				ret = -EINVAL;
+				goto out_err;
+			}
+
+			ret = cryptodev_cipher_decrypt(&ses_ptr->cdata,
+							dst_sg, dst_sg, len);
+
+			if (unlikely(ret)) {
+				dprintk(0, KERN_ERR, "cryptodev_cipher_decrypt: %d\n", ret);
+				goto out_err;
+			}
+		}
+
+	}
+	kcaop->dst_len = len;
+	return 0;
+out_err:
+	return ret;
+}
+
 /* This is the main crypto function - zero-copy edition */
 static int
 __crypto_auth_run_zc(struct csession *ses_ptr, struct kernel_crypt_auth_op *kcaop)
@@ -389,18 +560,27 @@ __crypto_auth_run_zc(struct csession *ses_ptr, struct kernel_crypt_auth_op *kcao
 	struct crypt_auth_op *caop = &kcaop->caop;
 	int ret = 0, pagecount;
 
-	ret = get_userbuf3(ses_ptr, kcaop, &auth_sg, &dst_sg, &pagecount);
-	if (unlikely(ret)) {
-		dprintk(1, KERN_ERR, "Error getting user pages.\n");
-		return ret;
-	}
+	if (caop->flags & COP_FLAG_AEAD_TLS_TYPE) {
+		ret = get_userbuf_aead(ses_ptr, kcaop, &auth_sg, &dst_sg, &pagecount);
+		if (unlikely(ret)) {
+			dprintk(1, KERN_ERR, "Error getting user pages.\n");
+			return ret;
+		}
 
-	if (caop->flags & COP_FLAG_AEAD_TLS_TYPE)
 		ret = tls_auth_n_crypt(ses_ptr, kcaop, auth_sg, caop->auth_len, 
 			   dst_sg, caop->len);
-	else {
+	} else if (caop->flags & COP_FLAG_AEAD_SRTP_TYPE) {
+		ret = get_userbuf_srtp(ses_ptr, kcaop, &auth_sg, &dst_sg, &pagecount);
+		if (unlikely(ret)) {
+			dprintk(1, KERN_ERR, "Error getting user pages.\n");
+			return ret;
+		}
+
+		ret = srtp_auth_n_crypt(ses_ptr, kcaop, auth_sg, caop->auth_len, 
+			   dst_sg, caop->len);
+	} else {
 		dprintk(1, KERN_ERR, "Unsupported flag for authenc\n");
-		ret = -EINVAL;
+		return -EINVAL;
 	}
 
 	release_user_pages(ses_ptr->pages, pagecount);
diff --git a/drivers/staging/crypto/cryptodev/cryptodev_cipher.c b/drivers/staging/crypto/cryptodev/cryptodev_cipher.c
index d71be78239c8..ba6d7c0afa19 100644
--- a/drivers/staging/crypto/cryptodev/cryptodev_cipher.c
+++ b/drivers/staging/crypto/cryptodev/cryptodev_cipher.c
@@ -53,7 +53,7 @@ static void cryptodev_complete(struct crypto_async_request *req, int err)
 }
 
 int cryptodev_cipher_init(struct cipher_data *out, const char *alg_name,
-				uint8_t *keyp, size_t keylen)
+				uint8_t *keyp, size_t keylen, int stream)
 {
 	struct ablkcipher_alg *alg;
 	int ret;
@@ -92,6 +92,7 @@ int cryptodev_cipher_init(struct cipher_data *out, const char *alg_name,
 		goto error;
 	}
 
+	out->stream = stream;
 	out->blocksize = crypto_ablkcipher_blocksize(out->async.s);
 	out->ivsize = crypto_ablkcipher_ivsize(out->async.s);
 	out->alignmask = crypto_ablkcipher_alignmask(out->async.s);
diff --git a/drivers/staging/crypto/cryptodev/cryptodev_int.h b/drivers/staging/crypto/cryptodev/cryptodev_int.h
index f24b3896f833..27974c060e52 100644
--- a/drivers/staging/crypto/cryptodev/cryptodev_int.h
+++ b/drivers/staging/crypto/cryptodev/cryptodev_int.h
@@ -42,6 +42,7 @@ void release_user_pages(struct page **pg, int pagecount);
 struct cipher_data {
 	int init; /* 0 uninitialized */
 	int blocksize;
+	int stream;
 	int ivsize;
 	int alignmask;
 	struct {
@@ -58,7 +59,7 @@ struct fcrypt {
 };
 
 int cryptodev_cipher_init(struct cipher_data *out, const char *alg_name,
-						uint8_t *key, size_t keylen);
+			  uint8_t *key, size_t keylen, int stream);
 void cryptodev_cipher_deinit(struct cipher_data *cdata);
 ssize_t cryptodev_cipher_decrypt(struct cipher_data *cdata,
 			const struct scatterlist *sg1,
@@ -189,6 +190,8 @@ struct csession {
 	int array_size;
 	struct page **pages;
 	struct scatterlist *sg;
+	struct scatterlist sg2; /* temporal sg */
+	uint8_t buffer[16*1024]; /* temporal buffer */
 };
 
 struct csession *crypto_get_session_by_sid(struct fcrypt *fcr, uint32_t sid);
diff --git a/drivers/staging/crypto/cryptodev/cryptodev_main.c b/drivers/staging/crypto/cryptodev/cryptodev_main.c
index cbb9ef4f4413..0c97f0f79c15 100644
--- a/drivers/staging/crypto/cryptodev/cryptodev_main.c
+++ b/drivers/staging/crypto/cryptodev/cryptodev_main.c
@@ -109,7 +109,7 @@ crypto_create_session(struct fcrypt *fcr, struct session_op *sop)
 	int ret = 0;
 	const char *alg_name = NULL;
 	const char *hash_name = NULL;
-	int hmac_mode = 1;
+	int hmac_mode = 1, stream = 0;
 
 	/* Does the request make sense? */
 	if (unlikely(!sop->cipher && !sop->mac)) {
@@ -140,9 +140,11 @@ crypto_create_session(struct fcrypt *fcr, struct session_op *sop)
 		break;
 	case CRYPTO_AES_CTR:
 		alg_name = "ctr(aes)";
+		stream = 1;
 		break;
 	case CRYPTO_NULL:
 		alg_name = "ecb(cipher_null)";
+		stream = 1;
 		break;
 	default:
 		dprintk(1, KERN_DEBUG, "%s: bad cipher: %d\n", __func__,
@@ -227,7 +229,7 @@ crypto_create_session(struct fcrypt *fcr, struct session_op *sop)
 		}
 
 		ret = cryptodev_cipher_init(&ses_new->cdata, alg_name, keyp,
-								sop->keylen);
+						sop->keylen, stream);
 		if (ret < 0) {
 			dprintk(1, KERN_DEBUG,
 				"%s: Failed to load cipher for %s\n",
diff --git a/drivers/staging/crypto/cryptodev/examples/cipher-aead-srtp.c b/drivers/staging/crypto/cryptodev/examples/cipher-aead-srtp.c
new file mode 100644
index 000000000000..8ef142a2ac81
--- /dev/null
+++ b/drivers/staging/crypto/cryptodev/examples/cipher-aead-srtp.c
@@ -0,0 +1,273 @@
+/*
+ * Demo on how to use /dev/crypto device for ciphering.
+ *
+ * Placed under public domain.
+ *
+ */
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+#include <sys/ioctl.h>
+#include <crypto/cryptodev.h>
+
+#define	DATA_SIZE	(8*1024+11)
+#define HEADER_SIZE 256
+#define PLAINTEXT_SIZE 1021
+#define FOOTER_SIZE 15
+#define	BLOCK_SIZE	16
+#define	KEY_SIZE	16
+
+#define MAC_SIZE 20 /* SHA1 */
+
+static int
+get_sha1_hmac(int cfd, void* key, int key_size, void* data, int data_size, void* mac)
+{
+	struct session_op sess;
+	struct crypt_op cryp;
+	int i;
+
+	memset(&sess, 0, sizeof(sess));
+	memset(&cryp, 0, sizeof(cryp));
+
+	sess.cipher = 0;
+	sess.mac = CRYPTO_SHA1_HMAC;
+	sess.mackeylen = key_size;
+	sess.mackey = key;
+	if (ioctl(cfd, CIOCGSESSION, &sess)) {
+		perror("ioctl(CIOCGSESSION)");
+		return 1;
+	}
+
+	/* Encrypt data.in to data.encrypted */
+	cryp.ses = sess.ses;
+	cryp.len = data_size;
+	cryp.src = data;
+	cryp.dst = NULL;
+	cryp.iv = NULL;
+	cryp.mac = mac;
+	cryp.op = COP_ENCRYPT;
+	if (ioctl(cfd, CIOCCRYPT, &cryp)) {
+		perror("ioctl(CIOCCRYPT)");
+		return 1;
+	}
+
+	/* Finish crypto session */
+	if (ioctl(cfd, CIOCFSESSION, &sess.ses)) {
+		perror("ioctl(CIOCFSESSION)");
+		return 1;
+	}
+
+	return 0;
+}
+
+static void print_buf(char* desc, unsigned char* buf, int size)
+{
+int i;
+	fputs(desc, stdout);
+	for (i=0;i<size;i++) {
+		printf("%.2x", (uint8_t)buf[i]);
+	}
+	fputs("\n", stdout);
+}
+
+static int
+test_crypto(int cfd)
+{
+	char plaintext_raw[DATA_SIZE + 63], *plaintext;
+	char ciphertext_raw[DATA_SIZE + 63], *ciphertext;
+	char iv[BLOCK_SIZE];
+	char key[KEY_SIZE];
+	unsigned char sha1mac[20];
+	unsigned char tag[20];
+	unsigned char mackey[] = "\x0b\x0b\x0b\x0b\x0b\x0b\x0b\x0b\x0b\x0b\x0b\x0b\x0b\x0b\x0b\x0b";
+	int mackey_len = 16;
+	int pad, i;
+
+	struct session_op sess;
+	struct crypt_op co;
+	struct crypt_auth_op cao;
+#ifdef CIOCGSESSINFO
+	struct session_info_op siop;
+#endif
+
+	memset(&sess, 0, sizeof(sess));
+	memset(&cao, 0, sizeof(cao));
+	memset(&co, 0, sizeof(co));
+
+	memset(key,0x33,  sizeof(key));
+	memset(iv, 0x03,  sizeof(iv));
+
+	/* Get crypto session for AES128 */
+	sess.cipher = CRYPTO_AES_CTR;
+	sess.keylen = KEY_SIZE;
+	sess.key = key;
+
+	sess.mac = CRYPTO_SHA1_HMAC;
+	sess.mackeylen = mackey_len;
+	sess.mackey = mackey;
+
+	if (ioctl(cfd, CIOCGSESSION, &sess)) {
+		perror("ioctl(CIOCGSESSION)");
+		return 1;
+	}
+
+#ifdef CIOCGSESSINFO
+	siop.ses = sess.ses;
+	if (ioctl(cfd, CIOCGSESSINFO, &siop)) {
+		perror("ioctl(CIOCGSESSINFO)");
+		return 1;
+	}
+	printf("requested cipher CRYPTO_AES_CBC/HMAC-SHA1, got %s with driver %s\n",
+			siop.cipher_info.cra_name, siop.cipher_info.cra_driver_name);
+
+	plaintext = (char *)(((unsigned long)plaintext_raw + siop.alignmask) & ~siop.alignmask);
+	ciphertext = (char *)(((unsigned long)ciphertext_raw + siop.alignmask) & ~siop.alignmask);
+#else
+	plaintext = plaintext_raw;
+	ciphertext = ciphertext_raw;
+#endif
+	memset(plaintext, 0x15, HEADER_SIZE); /* header */
+	memset(&plaintext[HEADER_SIZE], 0x17, PLAINTEXT_SIZE); /* payload */
+	memset(&plaintext[HEADER_SIZE+PLAINTEXT_SIZE], 0x22, FOOTER_SIZE);
+
+	memcpy(ciphertext, plaintext, DATA_SIZE);
+
+	/* Encrypt data.in to data.encrypted */
+	cao.ses = sess.ses;
+	cao.len = PLAINTEXT_SIZE;
+	cao.auth_len = HEADER_SIZE+PLAINTEXT_SIZE+FOOTER_SIZE;
+	cao.auth_src = ciphertext;
+	cao.src = ciphertext+HEADER_SIZE;
+	cao.dst = cao.src;
+	cao.iv = iv;
+	cao.op = COP_ENCRYPT;
+	cao.flags = COP_FLAG_AEAD_SRTP_TYPE;
+	cao.tag = tag;
+	cao.tag_len = 20;
+
+	if (ioctl(cfd, CIOCAUTHCRYPT, &cao)) {
+		perror("ioctl(CIOCAUTHCRYPT)");
+		return 1;
+	}
+
+
+	if (ioctl(cfd, CIOCFSESSION, &sess.ses)) {
+		perror("ioctl(CIOCFSESSION)");
+		return 1;
+	}
+
+	/* Get crypto session for AES128 */
+	memset(&sess, 0, sizeof(sess));
+	sess.cipher = CRYPTO_AES_CTR;
+	sess.keylen = KEY_SIZE;
+	sess.key = key;
+
+	if (ioctl(cfd, CIOCGSESSION, &sess)) {
+		perror("ioctl(CIOCGSESSION)");
+		return 1;
+	}
+
+	if (get_sha1_hmac(cfd, mackey, mackey_len, ciphertext, HEADER_SIZE + PLAINTEXT_SIZE + FOOTER_SIZE, sha1mac) != 0) {
+		fprintf(stderr, "SHA1 MAC failed\n");
+		return 1;
+	}
+
+	if (memcmp(tag, sha1mac, 20) != 0) {
+		fprintf(stderr, "AEAD SHA1 MAC does not match plain MAC\n");
+		print_buf("SHA1: ", sha1mac, 20);
+		print_buf("SHA1-SRTP: ", tag, 20);
+		return 1;
+	}
+
+	/* Decrypt data.encrypted to data.decrypted */
+	co.ses = sess.ses;
+	co.len = PLAINTEXT_SIZE;
+	co.src = ciphertext+HEADER_SIZE;
+	co.dst = ciphertext+HEADER_SIZE;
+	co.iv = iv;
+	co.op = COP_DECRYPT;
+	if (ioctl(cfd, CIOCCRYPT, &co)) {
+		perror("ioctl(CIOCCRYPT)");
+		return 1;
+	}
+
+	/* Verify the result */
+	if (memcmp(plaintext+HEADER_SIZE, ciphertext+HEADER_SIZE, PLAINTEXT_SIZE) != 0) {
+		int i;
+		fprintf(stderr,
+			"FAIL: Decrypted data are different from the input data.\n");
+		printf("plaintext:");
+		for (i = 0; i < DATA_SIZE; i++) {
+			if ((i % 30) == 0)
+				printf("\n");
+			printf("%02x ", plaintext[i]);
+		}
+		printf("ciphertext:");
+		for (i = 0; i < DATA_SIZE; i++) {
+			if ((i % 30) == 0)
+				printf("\n");
+			printf("%02x ", ciphertext[i]);
+		}
+		printf("\n");
+		return 1;
+	}
+
+	printf("Test passed\n");
+
+
+	/* Finish crypto session */
+	if (ioctl(cfd, CIOCFSESSION, &sess.ses)) {
+		perror("ioctl(CIOCFSESSION)");
+		return 1;
+	}
+
+	return 0;
+}
+
+int
+main()
+{
+	int fd = -1, cfd = -1;
+
+	/* Open the crypto device */
+	fd = open("/dev/crypto", O_RDWR, 0);
+	if (fd < 0) {
+		perror("open(/dev/crypto)");
+		return 1;
+	}
+
+	/* Clone file descriptor */
+	if (ioctl(fd, CRIOGET, &cfd)) {
+		perror("ioctl(CRIOGET)");
+		return 1;
+	}
+
+	/* Set close-on-exec (not really neede here) */
+	if (fcntl(cfd, F_SETFD, 1) == -1) {
+		perror("fcntl(F_SETFD)");
+		return 1;
+	}
+
+	/* Run the test itself */
+
+	if (test_crypto(cfd))
+		return 1;
+
+	/* Close cloned descriptor */
+	if (close(cfd)) {
+		perror("close(cfd)");
+		return 1;
+	}
+
+	/* Close the original descriptor */
+	if (close(fd)) {
+		perror("close(fd)");
+		return 1;
+	}
+
+	return 0;
+}
+
diff --git a/drivers/staging/crypto/cryptodev/examples/cipher-aead.c b/drivers/staging/crypto/cryptodev/examples/cipher-aead.c
index 8b69cb1d25ab..a6c45567668e 100644
--- a/drivers/staging/crypto/cryptodev/examples/cipher-aead.c
+++ b/drivers/staging/crypto/cryptodev/examples/cipher-aead.c
@@ -130,12 +130,12 @@ test_crypto(int cfd)
 		return 1;
 	}
 
-	memcpy(ciphertext, plaintext, DATA_SIZE);
+	//memcpy(ciphertext, plaintext, DATA_SIZE);
 
 	/* Encrypt data.in to data.encrypted */
 	cao.ses = sess.ses;
 	cao.len = DATA_SIZE;
-	cao.src = ciphertext;
+	cao.src = plaintext;
 	cao.dst = ciphertext;
 	cao.iv = iv;
 	cao.op = COP_ENCRYPT;
-- 
2.1.0

