1 files changed, 54 insertions, 0 deletions

diff --git a/drivers/crypto/caam/caampkc.c b/drivers/crypto/caam/caampkc.c
index 7a897209f181..979072b25eaa 100644
--- a/drivers/crypto/caam/caampkc.c
+++ b/drivers/crypto/caam/caampkc.c
@@ -166,18 +166,71 @@ static void rsa_priv_f3_done(struct device *dev, u32 *desc, u32 err,
 	akcipher_request_complete(req, err);
 }
 
+static int caam_rsa_count_leading_zeros(struct scatterlist *sgl,
+					unsigned int nbytes,
+					unsigned int flags)
+{
+	struct sg_mapping_iter miter;
+	int lzeros, ents;
+	unsigned int len;
+	unsigned int tbytes = nbytes;
+	const u8 *buff;
+
+	ents = sg_nents_for_len(sgl, nbytes);
+	if (ents < 0)
+		return ents;
+
+	sg_miter_start(&miter, sgl, ents, SG_MITER_FROM_SG | flags);
+
+	lzeros = 0;
+	len = 0;
+	while (nbytes > 0) {
+		while (len && !*buff) {
+			lzeros++;
+			len--;
+			buff++;
+		}
+
+		if (len && *buff)
+			break;
+
+		sg_miter_next(&miter);
+		buff = miter.addr;
+		len = miter.length;
+
+		nbytes -= lzeros;
+		lzeros = 0;
+	}
+
+	miter.consumed = lzeros;
+	sg_miter_stop(&miter);
+	nbytes -= lzeros;
+
+	return tbytes - nbytes;
+}
+
 static struct rsa_edesc *rsa_edesc_alloc(struct akcipher_request *req,
 					 size_t desclen)
 {
 	struct crypto_akcipher *tfm = crypto_akcipher_reqtfm(req);
 	struct caam_rsa_ctx *ctx = akcipher_tfm_ctx(tfm);
 	struct device *dev = ctx->dev;
+	struct caam_rsa_req_ctx *req_ctx = akcipher_request_ctx(req);
 	struct rsa_edesc *edesc;
 	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
 		       GFP_KERNEL : GFP_ATOMIC;
+	int sg_flags = (flags == GFP_ATOMIC) ? SG_MITER_ATOMIC : 0;
 	int sgc;
 	int sec4_sg_index, sec4_sg_len = 0, sec4_sg_bytes;
 	int src_nents, dst_nents;
+	int lzeros;
+
+	lzeros = caam_rsa_count_leading_zeros(req->src, req->src_len, sg_flags);
+	if (lzeros < 0)
+		return ERR_PTR(lzeros);
+
+	req->src_len -= lzeros;
+	req->src = scatterwalk_ffwd(req_ctx->src, req->src, lzeros);
 
 	src_nents = sg_nents_for_len(req->src, req->src_len);
 	dst_nents = sg_nents_for_len(req->dst, req->dst_len);
@@ -953,6 +1006,7 @@ static struct akcipher_alg caam_rsa = {
 	.max_size = caam_rsa_max_size,
 	.init = caam_rsa_init_tfm,
 	.exit = caam_rsa_exit_tfm,
+	.reqsize = sizeof(struct caam_rsa_req_ctx),
 	.base = {
 		.cra_name = "rsa",
 		.cra_driver_name = "rsa-caam",