From bb12203bdf7bf1eea4a20870bb33b9077af50bda Mon Sep 17 00:00:00 2001 From: Peng Fan Date: Fri, 10 May 2019 16:24:56 +0800 Subject: [PATCH] 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 finished reading, copy the needed data from buffer to caller and free buffer. Signed-off-by: Peng Fan Signed-off-by: Fugang Duan --- drivers/nvmem/imx-ocotp.c | 30 +++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/drivers/nvmem/imx-ocotp.c b/drivers/nvmem/imx-ocotp.c index dff2f3c357f5..d824aed25fe7 100644 --- a/drivers/nvmem/imx-ocotp.c +++ b/drivers/nvmem/imx-ocotp.c @@ -4,6 +4,8 @@ * * Copyright (c) 2015 Pengutronix, Philipp Zabel * + * Copyright 2019 NXP + * * Based on the barebox ocotp driver, * Copyright (c) 2010 Baruch Siach , * Orex Computed Radiography @@ -121,22 +123,29 @@ 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; mutex_lock(&ocotp_mutex); + p = kzalloc(num_bytes, GFP_KERNEL); + if (!p) + return -ENOMEM; + 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; } @@ -147,7 +156,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 @@ -156,14 +165,22 @@ 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->base); + + buf += 4; } ret = 0; + index = offset % 4; + memcpy(val, &p[index], bytes); + read_end: clk_disable_unprepare(priv->clk); mutex_unlock(&ocotp_mutex); + + kfree(p); + return ret; } @@ -415,7 +432,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, }; @@ -528,7 +545,6 @@ static int imx_ocotp_probe(struct platform_device *pdev) priv->config = &imx_ocotp_nvmem_config; nvmem = devm_nvmem_register(dev, &imx_ocotp_nvmem_config); - return PTR_ERR_OR_ZERO(nvmem); }