[PATCH v2 0/2] mtd: nand: gpmi: add proper raw access support

@@ -1353,3 +1353,89 @@ 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);
 }
+
+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_byte = 0;
+
+	src += src_bit_off / 8;
+	src_bit_off %= 8;
+
+	dst += dst_bit_off / 8;
+	dst_bit_off %= 8;
+
+	if (src_bit_off) {
+		src_byte = src[0] >> src_bit_off;
+		nbits -= 8 - src_bit_off;
+		src++;
+	}
+
+	nbytes = nbits / 8;
+
+	if (src_bit_off <= dst_bit_off) {
+		dst[0] &= GENMASK(dst_bit_off - 1, 0);
+		dst[0] |= src_byte << dst_bit_off;
+		src_bit_off += (8 - dst_bit_off);
+		src_byte >>= (8 - dst_bit_off);
+		dst_bit_off = 0;
+		dst++;
+	} else if (nbytes) {
+		src_byte |= src[0] << (8 - src_bit_off);
+		dst[0] &= GENMASK(dst_bit_off - 1, 0);
+		dst[0] |= src_byte << dst_bit_off;
+		src_bit_off += dst_bit_off;
+		src_byte >>= (8 - dst_bit_off);
+		dst_bit_off = 0;
+		dst++;
+		nbytes--;
+		src++;
+		if (src_bit_off > 7) {
+			src_bit_off -= 8;
+			dst[0] = src_byte;
+			dst++;
+			src_byte >>= 8;
+		}
+	}
+
+	if (!src_bit_off && !dst_bit_off) {
+		if (nbytes)
+			memcpy(dst, src, nbytes);
+	} else {
+		for (i = 0; i < nbytes; i++) {
+			src_byte |= src[i] << (8 - src_bit_off);
+			dst[i] = src_byte;
+			src_byte >>= 8;
+		}
+	}
+
+	dst += nbytes;
+	src += nbytes;
+	nbits %= 8;
+
+	if (!nbits && !src_bit_off)
+		return;
+
+	if (nbits)
+		src_byte |= (*src & GENMASK(nbits - 1, 0)) <<
+			    ((8 - src_bit_off) % 8);
+	nbits += (8 - src_bit_off) % 8;
+
+	if (dst_bit_off)
+		src_byte = (src_byte << dst_bit_off) |
+			   (*dst & GENMASK(dst_bit_off - 1, 0));
+	nbits += dst_bit_off;
+
+	if (nbits % 8)
+		src_byte |= (dst[nbits / 8] & GENMASK(7, nbits % 8)) <<
+			    (nbits / 8);
+
+	nbytes = DIV_ROUND_UP(nbits, 8);
+	for (i = 0; i < nbytes; i++) {
+		dst[i] = src_byte;
+		src_byte >>= 8;
+	}
+}

@@ -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

@@ -831,7 +831,13 @@ static int gpmi_alloc_dma_buffer(struct gpmi_nand_data *this)
 	 * power of two and is much larger than four, which guarantees the
 	 * auxiliary buffer will appear on a 32-bit boundary.
 	 */
-	this->page_buffer_size = geo->payload_size + geo->auxiliary_size;
+	if (geo->payload_size + geo->auxiliary_size >
+	    mtd->writesize + mtd->oobsize)
+		this->page_buffer_size =
+				geo->payload_size + geo->auxiliary_size;
+	else
+		this->page_buffer_size = mtd->writesize + mtd->oobsize;
+
 	this->page_buffer_virt = dma_alloc_coherent(dev, this->page_buffer_size,
 					&this->page_buffer_phys, GFP_DMA);
 	if (!this->page_buffer_virt)

@@ -1347,6 +1353,114 @@ gpmi_ecc_write_oob(struct mtd_info *mtd, struct nand_chip *chip, int page)
 	return status & NAND_STATUS_FAIL ? -EIO : 0;
 }
 
+static int gpmi_ecc_read_page_raw(struct mtd_info *mtd,
+				  struct nand_chip *chip, uint8_t *buf,
+				  int oob_required, int page)
+{
+	struct gpmi_nand_data *this = chip->priv;
+	struct bch_geometry *nfc_geo = &this->bch_geometry;
+	int eccsize = nfc_geo->ecc_chunk_size;
+	int eccbits = nfc_geo->ecc_strength * nfc_geo->gf_len;
+	u8 *tmp_buf = this->page_buffer_virt;
+	size_t src_bit_off;
+	size_t oob_bit_off;
+	size_t oob_byte_off;
+	uint8_t *oob = chip->oob_poi;
+	int step;
+
+	chip->read_buf(mtd, tmp_buf,
+		       mtd->writesize + mtd->oobsize);
+
+	if (this->swap_block_mark) {
+		u8 swap = tmp_buf[0];
+
+		tmp_buf[0] = tmp_buf[mtd->writesize];
+		tmp_buf[mtd->writesize] = swap;
+	}
+
+	memcpy(oob, tmp_buf, nfc_geo->metadata_size);
+	oob_bit_off = nfc_geo->metadata_size * 8;
+	src_bit_off = oob_bit_off;
+
+	for (step = 0; step < nfc_geo->ecc_chunk_count; step++) {
+		gpmi_move_bits(buf, step * eccsize * 8,
+			       tmp_buf, src_bit_off,
+			       eccsize * 8);
+		src_bit_off += eccsize * 8;
+		gpmi_move_bits(oob, oob_bit_off,
+			       tmp_buf, src_bit_off,
+			       eccbits);
+		src_bit_off += eccbits;
+		oob_bit_off += eccbits;
+	}
+
+	if (oob_bit_off % 8)
+		oob[oob_bit_off / 8] &= GENMASK(oob_bit_off - 1, 0);
+
+	oob_byte_off = DIV_ROUND_UP(oob_bit_off, 8);
+
+	if (oob_byte_off  < mtd->oobsize)
+		memcpy(oob + oob_byte_off,
+		       tmp_buf + mtd->writesize + oob_byte_off,
+		       mtd->oobsize - oob_byte_off);
+
+	return 0;
+}
+
+static int gpmi_ecc_write_page_raw(struct mtd_info *mtd,
+				   struct nand_chip *chip,
+				   const uint8_t *buf,
+				   int oob_required)
+{
+	struct gpmi_nand_data *this = chip->priv;
+	struct bch_geometry *nfc_geo = &this->bch_geometry;
+	int eccsize = nfc_geo->ecc_chunk_size;
+	int eccbits = nfc_geo->ecc_strength * nfc_geo->gf_len;
+	u8 *tmp_buf = this->page_buffer_virt;
+	uint8_t *oob = chip->oob_poi;
+	size_t dst_bit_off;
+	size_t oob_bit_off;
+	size_t oob_byte_off;
+	int step;
+
+	memcpy(tmp_buf, oob, nfc_geo->metadata_size);
+	oob_bit_off = nfc_geo->metadata_size * 8;
+	dst_bit_off = oob_bit_off;
+
+	for (step = 0; step < nfc_geo->ecc_chunk_count; step++) {
+		gpmi_move_bits(tmp_buf, dst_bit_off,
+			       buf, step * eccsize * 8, eccsize * 8);
+		dst_bit_off += eccsize * 8;
+
+		/* Pad last ECC block to align following data on a byte */
+		if (step == nfc_geo->ecc_chunk_count - 1 &&
+		    (oob_bit_off + eccbits) % 8)
+			eccbits += 8 - ((oob_bit_off + eccbits) % 8);
+
+		gpmi_move_bits(tmp_buf, dst_bit_off,
+			       oob, oob_bit_off, eccbits);
+		dst_bit_off += eccbits;
+		oob_bit_off += eccbits;
+	}
+
+	oob_byte_off = oob_bit_off / 8;
+
+	if (oob_required && oob_byte_off < mtd->oobsize)
+		memcpy(tmp_buf + mtd->writesize + oob_byte_off,
+		       oob + oob_byte_off, mtd->oobsize - oob_byte_off);
+
+	if (this->swap_block_mark) {
+		u8 swap = tmp_buf[0];
+
+		tmp_buf[0] = tmp_buf[mtd->writesize];
+		tmp_buf[mtd->writesize] = swap;
+	}
+
+	chip->write_buf(mtd, tmp_buf, mtd->writesize + mtd->oobsize);
+
+	return 0;
+}
+
 static int gpmi_block_markbad(struct mtd_info *mtd, loff_t ofs)
 {
 	struct nand_chip *chip = mtd->priv;

@@ -1664,6 +1778,8 @@ static int gpmi_init_last(struct gpmi_nand_data *this)
 	ecc->write_page	= gpmi_ecc_write_page;
 	ecc->read_oob	= gpmi_ecc_read_oob;
 	ecc->write_oob	= gpmi_ecc_write_oob;
+	ecc->read_page_raw = gpmi_ecc_read_page_raw;
+	ecc->write_page_raw = gpmi_ecc_write_page_raw;
 	ecc->mode	= NAND_ECC_HW;
 	ecc->size	= bch_geo->ecc_chunk_size;
 	ecc->strength	= bch_geo->ecc_strength;