On Wed, 26 Nov 2014 17:53:11 +0100 Boris Brezillon <[email protected]> wrote:
> Add a new function to move bits (not bytes) from a memory region to > another one. > This function is similar to memmove except it acts at bit level. > This function is needed to implement GPMI raw access functions, given the > fact that ECC engine does not pad ECC bits to the next byte boundary. > > Signed-off-by: Boris Brezillon <[email protected]> > --- > drivers/mtd/nand/gpmi-nand/gpmi-lib.c | 151 > +++++++++++++++++++++++++++++++++ > drivers/mtd/nand/gpmi-nand/gpmi-nand.h | 4 + > 2 files changed, 155 insertions(+) > > diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c > b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c > index 87e658c..77eff0e 100644 > --- a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c > +++ b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c > @@ -1353,3 +1353,154 @@ int gpmi_read_page(struct gpmi_nand_data *this, > set_dma_type(this, DMA_FOR_READ_ECC_PAGE); > return start_dma_with_bch_irq(this, desc); > } > + > +/** > + * gpmi_move_bits - move bits from one memory region to another > + * @dst: destination buffer > + * @dst_bit_off: bit offset we're starting to write at > + * @src: source buffer > + * @src_bit_off: bit offset we're starting to read from > + * @nbits: number of bits to copy > + * > + * This functions copies bits from one memory region to another, and is used > by > + * the GPMI driver to copy ECC sections which are not guaranteed to be byte > + * aligned. I forgot to add that src and dst should not overlap, otherwise it won't work... > + * > + */ > +void gpmi_move_bits(u8 *dst, size_t dst_bit_off, > + const u8 *src, size_t src_bit_off, > + size_t nbits) > +{ > + size_t i; > + size_t nbytes; > + u32 src_buffer = 0; > + size_t bits_in_src_buffer = 0; > + > + if (!nbits) > + return; > + > + /* > + * Move src and dst pointers to the closest byte pointer and store bit > + * offsets within a byte. > + */ > + src += src_bit_off / 8; > + src_bit_off %= 8; > + > + dst += dst_bit_off / 8; > + dst_bit_off %= 8; > + > + /* > + * Initialize the src_buffer value with bits available in the first > + * byte of data so that we end up with a byte aligned src pointer. > + */ > + if (src_bit_off) { > + src_buffer = src[0] >> src_bit_off; > + if (nbits >= (8 - src_bit_off)) { > + bits_in_src_buffer += 8 - src_bit_off; > + } else { > + src_buffer &= GENMASK(nbits - 1, 0); > + bits_in_src_buffer += nbits; > + } > + nbits -= bits_in_src_buffer; > + src++; > + } > + > + /* Calculate the number of bytes that can be copied from src to dst. */ > + nbytes = nbits / 8; > + > + /* Try to align dst to a byte boundary. */ > + if (dst_bit_off) { > + if (bits_in_src_buffer < (8 - dst_bit_off) && nbytes) { > + src_buffer |= src[0] << bits_in_src_buffer; > + bits_in_src_buffer += 8; > + src++; > + nbytes--; > + } > + > + if (bits_in_src_buffer >= (8 - dst_bit_off)) { > + dst[0] &= GENMASK(dst_bit_off - 1, 0); > + dst[0] |= src_buffer << dst_bit_off; > + src_buffer >>= (8 - dst_bit_off); > + bits_in_src_buffer -= (8 - dst_bit_off); > + dst_bit_off = 0; > + dst++; > + if (bits_in_src_buffer > 7) { > + bits_in_src_buffer -= 8; > + dst[0] = src_buffer; > + dst++; > + src_buffer >>= 8; > + } > + } > + } > + > + if (!bits_in_src_buffer && !dst_bit_off) { > + /* > + * Both src and dst pointers are byte aligned, thus we can > + * just use the optimized memcpy function. > + */ > + if (nbytes) > + memcpy(dst, src, nbytes); > + } else { > + /* > + * src buffer is not byte aligned, hence we have to copy each > + * src byte to the src_buffer variable before extracting a byte > + * to store in dst. > + */ > + for (i = 0; i < nbytes; i++) { > + src_buffer |= src[i] << bits_in_src_buffer; > + dst[i] = src_buffer; > + src_buffer >>= 8; > + } > + } > + /* Update dst and src pointers */ > + dst += nbytes; > + src += nbytes; > + > + /* > + * nbits is the number of remaining bits. It should not exceed 8 as > + * we've already copied as much bytes as possible. > + */ > + nbits %= 8; > + > + /* > + * If there's no more bits to copy to the destination and src buffer > + * was already byte aligned, then we're done. > + */ > + if (!nbits && !bits_in_src_buffer) > + return; > + > + /* Copy the remaining bits to src_buffer */ > + if (nbits) > + src_buffer |= (*src & GENMASK(nbits - 1, 0)) << > + bits_in_src_buffer; > + bits_in_src_buffer += nbits; > + > + /* > + * In case there were not enough bits to get a byte aligned dst buffer > + * prepare the src_buffer variable to match the dst organization (shift > + * src_buffer by dst_bit_off and retrieve the least significant bits > + * from dst). > + */ > + if (dst_bit_off) > + src_buffer = (src_buffer << dst_bit_off) | > + (*dst & GENMASK(dst_bit_off - 1, 0)); > + bits_in_src_buffer += dst_bit_off; > + > + /* > + * Keep most significant bits from dst if we end up with an unaligned > + * number of bits. > + */ > + nbytes = bits_in_src_buffer / 8; > + if (bits_in_src_buffer % 8) { > + src_buffer |= (dst[nbytes] & > + GENMASK(7, bits_in_src_buffer % 8)) << > + (nbytes * 8); > + nbytes++; > + } > + > + /* Copy the remaining bytes to dst */ > + for (i = 0; i < nbytes; i++) { > + dst[i] = src_buffer; > + src_buffer >>= 8; > + } > +} > diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h > b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h > index 32c6ba4..17d0736 100644 > --- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h > +++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h > @@ -290,6 +290,10 @@ extern int gpmi_send_page(struct gpmi_nand_data *, > extern int gpmi_read_page(struct gpmi_nand_data *, > dma_addr_t payload, dma_addr_t auxiliary); > > +void gpmi_move_bits(u8 *dst, size_t dst_bit_off, > + const u8 *src, size_t src_bit_off, > + size_t nbits); > + > /* BCH : Status Block Completion Codes */ > #define STATUS_GOOD 0x00 > #define STATUS_ERASED 0xff -- Boris Brezillon, Free Electrons Embedded Linux and Kernel engineering http://free-electrons.com -- To unsubscribe from this list: send the line "unsubscribe linux-kernel" in the body of a message to [email protected] More majordomo info at http://vger.kernel.org/majordomo-info.html Please read the FAQ at http://www.tux.org/lkml/
