diff options
| -rw-r--r-- | drivers/nvmem/core.c | 15 |
1 files changed, 10 insertions, 5 deletions
diff --git a/drivers/nvmem/core.c b/drivers/nvmem/core.c index 635886e4835c..dba3f4d0a63d 100644 --- a/drivers/nvmem/core.c +++ b/drivers/nvmem/core.c @@ -954,7 +954,7 @@ static inline void nvmem_shift_read_buffer_in_place(struct nvmem_cell *cell, void *buf) { u8 *p, *b; - int i, bit_offset = cell->bit_offset; + int i, extra, bit_offset = cell->bit_offset; p = b = buf; if (bit_offset) { @@ -969,11 +969,16 @@ static inline void nvmem_shift_read_buffer_in_place(struct nvmem_cell *cell, p = b; *b++ >>= bit_offset; } - - /* result fits in less bytes */ - if (cell->bytes != DIV_ROUND_UP(cell->nbits, BITS_PER_BYTE)) - *p-- = 0; + } else { + /* point to the msb */ + p += cell->bytes - 1; } + + /* result fits in less bytes */ + extra = cell->bytes - DIV_ROUND_UP(cell->nbits, BITS_PER_BYTE); + while (--extra >= 0) + *p-- = 0; + /* clear msb bits if any leftover in the last byte */ *p &= GENMASK((cell->nbits%BITS_PER_BYTE) - 1, 0); } |