nvmem: imx-ocotp: add support for the unaliged word count

When offset is not 4 bytes aligned, directly shift righty by 2 bits
will cause reading out wrong data. Since imx ocotp only supports
4 bytes reading once, we need handle offset is not 4 bytes aligned
and enlarge the bytes to 4 bytes aligned. After reading finished,
copy the needed data from buffer to caller and free buffer.

Signed-off-by: Peng Fan <peng.fan@nxp.com>
Signed-off-by: Srinivas Kandagatla <srinivas.kandagatla@linaro.org>
Link: https://lore.kernel.org/r/20201127102837.19366-6-srinivas.kandagatla@linaro.org
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/nvmem/imx-ocotp.c b/drivers/nvmem/imx-ocotp.c
index 7a1ebd6fd08b2af4c9ddefbe8da919fb27f1cac1..08f41328cc71199fd1220343ce64e62f02ffe0e0 100644 (file)
--- a/drivers/nvmem/imx-ocotp.c
+++ b/drivers/nvmem/imx-ocotp.c
@@ -4,6 +4,8 @@
  *
  * Copyright (c) 2015 Pengutronix, Philipp Zabel <p.zabel@pengutronix.de>
  *
+ * Copyright 2019 NXP
+ *
  * Based on the barebox ocotp driver,
  * Copyright (c) 2010 Baruch Siach <baruch@tkos.co.il>,
  *     Orex Computed Radiography
@@ -158,22 +160,30 @@ static int imx_ocotp_read(void *context, unsigned int offset,
 {
        struct ocotp_priv *priv = context;
        unsigned int count;
-       u32 *buf = val;
+       u8 *buf, *p;
        int i, ret;
-       u32 index;
+       u32 index, num_bytes;
 
        index = offset >> 2;
-       count = bytes >> 2;
+       num_bytes = round_up((offset % 4) + bytes, 4);
+       count = num_bytes >> 2;
 
        if (count > (priv->params->nregs - index))
                count = priv->params->nregs - index;
 
+       p = kzalloc(num_bytes, GFP_KERNEL);
+       if (!p)
+               return -ENOMEM;
+
        mutex_lock(&ocotp_mutex);
 
+       buf = p;
+
        ret = clk_prepare_enable(priv->clk);
        if (ret < 0) {
                mutex_unlock(&ocotp_mutex);
                dev_err(priv->dev, "failed to prepare/enable ocotp clk\n");
+               kfree(p);
                return ret;
        }
 
@@ -184,7 +194,7 @@ static int imx_ocotp_read(void *context, unsigned int offset,
        }
 
        for (i = index; i < (index + count); i++) {
-               *buf++ = readl(priv->base + IMX_OCOTP_OFFSET_B0W0 +
+               *(u32 *)buf = readl(priv->base + IMX_OCOTP_OFFSET_B0W0 +
                               i * IMX_OCOTP_OFFSET_PER_WORD);
 
                /* 47.3.1.2
@@ -193,13 +203,21 @@ static int imx_ocotp_read(void *context, unsigned int offset,
                 * software before any new write, read or reload access can be
                 * issued
                 */
-               if (*(buf - 1) == IMX_OCOTP_READ_LOCKED_VAL)
+               if (*((u32 *)buf) == IMX_OCOTP_READ_LOCKED_VAL)
                        imx_ocotp_clr_err_if_set(priv);
+
+               buf += 4;
        }
 
+       index = offset % 4;
+       memcpy(val, &p[index], bytes);
+
 read_end:
        clk_disable_unprepare(priv->clk);
        mutex_unlock(&ocotp_mutex);
+
+       kfree(p);
+
        return ret;
 }
 
@@ -447,7 +465,7 @@ static struct nvmem_config imx_ocotp_nvmem_config = {
        .name = "imx-ocotp",
        .read_only = false,
        .word_size = 4,
-       .stride = 4,
+       .stride = 1,
        .reg_read = imx_ocotp_read,
        .reg_write = imx_ocotp_write,
 };