diff options
-rw-r--r-- | drivers/nvmem/sunxi_sid.c | 38 |
1 files changed, 18 insertions, 20 deletions
diff --git a/drivers/nvmem/sunxi_sid.c b/drivers/nvmem/sunxi_sid.c index 704c35edf796..15fbfab62595 100644 --- a/drivers/nvmem/sunxi_sid.c +++ b/drivers/nvmem/sunxi_sid.c @@ -115,36 +115,34 @@ static int sun8i_sid_register_readout(const struct sunxi_sid *sid, * to be not reliable at all. * Read by the registers instead. */ -static int sun8i_sid_read_byte_by_reg(const struct sunxi_sid *sid, - const unsigned int offset, - u8 *out) -{ - u32 word; - int ret; - - ret = sun8i_sid_register_readout(sid, offset & ~0x03, &word); - - if (ret) - return ret; - - *out = (word >> ((offset & 0x3) * 8)) & 0xff; - - return 0; -} - static int sun8i_sid_read_by_reg(void *context, unsigned int offset, void *val, size_t bytes) { struct sunxi_sid *sid = context; - u8 *buf = val; + u32 word; int ret; - while (bytes--) { - ret = sun8i_sid_read_byte_by_reg(sid, offset++, buf++); + /* .stride = 4 so offset is guaranteed to be aligned */ + while (bytes >= 4) { + ret = sun8i_sid_register_readout(sid, offset, val); if (ret) return ret; + + val += 4; + offset += 4; + bytes -= 4; } + if (!bytes) + return 0; + + /* Handle any trailing bytes */ + ret = sun8i_sid_register_readout(sid, offset, &word); + if (ret) + return ret; + + memcpy(val, &word, bytes); + return 0; } |