mmc: cull sg list to match mmc request size

mmc layer may introduce additional (compared to block layer) limits on
request size. Culling of the sg list to match adjusted request size
simplifies the handling of such cases in the low level driver, allowing
it to skip block count checks while processing sg entries.

(fixes for wbsd and sdhci by Pierre Ossman)
Signed-off-by: Alex Dubov <oakad@yahoo.com>
Signed-off-by: Pierre Ossman <drzeus@drzeus.cx>

drivers/mmc/mmc_block.c

@@ -223,7 +223,7 @@ static int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
 	struct mmc_blk_data *md = mq->data;
 	struct mmc_card *card = md->queue.card;
 	struct mmc_blk_request brq;
-	int ret = 1;
+	int ret = 1, sg_pos, data_size;
 
 	if (mmc_card_claim_host(card))
 		goto flush_queue;
@@ -283,6 +283,20 @@ static int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
 		brq.data.sg = mq->sg;
 		brq.data.sg_len = blk_rq_map_sg(req->q, req, brq.data.sg);
 
+		if (brq.data.blocks !=
+		    (req->nr_sectors >> (md->block_bits - 9))) {
+			data_size = brq.data.blocks * brq.data.blksz;
+			for (sg_pos = 0; sg_pos < brq.data.sg_len; sg_pos++) {
+				data_size -= mq->sg[sg_pos].length;
+				if (data_size <= 0) {
+					mq->sg[sg_pos].length += data_size;
+					sg_pos++;
+					break;
+				}
+			}
+			brq.data.sg_len = sg_pos;
+		}
+
 		mmc_wait_for_req(card->host, &brq.mrq);
 		if (brq.cmd.error) {
 			printk(KERN_ERR "%s: error %d sending read/write command\n",

drivers/mmc/sdhci.c

 /*
  *  linux/drivers/mmc/sdhci.c - Secure Digital Host Controller Interface driver
  *
- *  Copyright (C) 2005-2006 Pierre Ossman, All Rights Reserved.
+ *  Copyright (C) 2005-2007 Pierre Ossman, All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -247,14 +247,13 @@ static void sdhci_read_block_pio(struct sdhci_host *host)
 			chunk_remain = min(blksize, 4);
 		}
 
-		size = min(host->size, host->remain);
-		size = min(size, chunk_remain);
+		size = min(host->remain, chunk_remain);
 
 		chunk_remain -= size;
 		blksize -= size;
 		host->offset += size;
 		host->remain -= size;
-		host->size -= size;
+
 		while (size) {
 			*buffer = data & 0xFF;
 			buffer++;
@@ -289,14 +288,13 @@ static void sdhci_write_block_pio(struct sdhci_host *host)
 	buffer = sdhci_sg_to_buffer(host) + host->offset;
 
 	while (blksize) {
-		size = min(host->size, host->remain);
-		size = min(size, chunk_remain);
+		size = min(host->remain, chunk_remain);
 
 		chunk_remain -= size;
 		blksize -= size;
 		host->offset += size;
 		host->remain -= size;
-		host->size -= size;
+
 		while (size) {
 			data >>= 8;
 			data |= (u32)*buffer << 24;
@@ -325,7 +323,7 @@ static void sdhci_transfer_pio(struct sdhci_host *host)
 
 	BUG_ON(!host->data);
 
-	if (host->size == 0)
+	if (host->num_sg == 0)
 		return;
 
 	if (host->data->flags & MMC_DATA_READ)
@@ -339,10 +337,8 @@ static void sdhci_transfer_pio(struct sdhci_host *host)
 		else
 			sdhci_write_block_pio(host);
 
-		if (host->size == 0)
+		if (host->num_sg == 0)
 			break;
-
-		BUG_ON(host->num_sg == 0);
 	}
 
 	DBG("PIO transfer complete.\n");
@@ -408,8 +404,6 @@ static void sdhci_prepare_data(struct sdhci_host *host, struct mmc_data *data)
 
 		writel(sg_dma_address(data->sg), host->ioaddr + SDHCI_DMA_ADDRESS);
 	} else {
-		host->size = data->blksz * data->blocks;
-
 		host->cur_sg = data->sg;
 		host->num_sg = data->sg_len;
 
@@ -473,10 +467,6 @@ static void sdhci_finish_data(struct sdhci_host *host)
 			"though there were blocks left.\n",
 			mmc_hostname(host->mmc));
 		data->error = MMC_ERR_FAILED;
-	} else if (host->size != 0) {
-		printk(KERN_ERR "%s: %d bytes were left untransferred.\n",
-			mmc_hostname(host->mmc), host->size);
-		data->error = MMC_ERR_FAILED;
 	}
 
 	DBG("Ending data transfer (%d bytes)\n", data->bytes_xfered);

drivers/mmc/sdhci.h

 /*
  *  linux/drivers/mmc/sdhci.h - Secure Digital Host Controller Interface driver
  *
- *  Copyright (C) 2005 Pierre Ossman, All Rights Reserved.
+ *  Copyright (C) 2005-2007 Pierre Ossman, All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -187,8 +187,6 @@ struct sdhci_host {
 	int			offset;		/* Offset into current sg */
 	int			remain;		/* Bytes left in current */
 
-	int			size;		/* Remaining bytes in transfer */
-
 	char			slot_descr[20];	/* Name for reservations */
 
 	int			irq;		/* Device IRQ */

drivers/mmc/wbsd.c

 /*
  *  linux/drivers/mmc/wbsd.c - Winbond W83L51xD SD/MMC driver
  *
- *  Copyright (C) 2004-2006 Pierre Ossman, All Rights Reserved.
+ *  Copyright (C) 2004-2007 Pierre Ossman, All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -278,90 +278,36 @@ static inline char *wbsd_sg_to_buffer(struct wbsd_host *host)
 
 static inline void wbsd_sg_to_dma(struct wbsd_host *host, struct mmc_data *data)
 {
-	unsigned int len, i, size;
+	unsigned int len, i;
 	struct scatterlist *sg;
 	char *dmabuf = host->dma_buffer;
 	char *sgbuf;
 
-	size = host->size;
-
 	sg = data->sg;
 	len = data->sg_len;
 
-	/*
-	 * Just loop through all entries. Size might not
-	 * be the entire list though so make sure that
-	 * we do not transfer too much.
-	 */
 	for (i = 0; i < len; i++) {
 		sgbuf = page_address(sg[i].page) + sg[i].offset;
-		if (size < sg[i].length)
-			memcpy(dmabuf, sgbuf, size);
-		else
 		memcpy(dmabuf, sgbuf, sg[i].length);
 		dmabuf += sg[i].length;
-
-		if (size < sg[i].length)
-			size = 0;
-		else
-			size -= sg[i].length;
-
-		if (size == 0)
-			break;
 	}
-
-	/*
-	 * Check that we didn't get a request to transfer
-	 * more data than can fit into the SG list.
-	 */
-
-	BUG_ON(size != 0);
-
-	host->size -= size;
 }
 
 static inline void wbsd_dma_to_sg(struct wbsd_host *host, struct mmc_data *data)
 {
-	unsigned int len, i, size;
+	unsigned int len, i;
 	struct scatterlist *sg;
 	char *dmabuf = host->dma_buffer;
 	char *sgbuf;
 
-	size = host->size;
-
 	sg = data->sg;
 	len = data->sg_len;
 
-	/*
-	 * Just loop through all entries. Size might not
-	 * be the entire list though so make sure that
-	 * we do not transfer too much.
-	 */
 	for (i = 0; i < len; i++) {
 		sgbuf = page_address(sg[i].page) + sg[i].offset;
-		if (size < sg[i].length)
-			memcpy(sgbuf, dmabuf, size);
-		else
 		memcpy(sgbuf, dmabuf, sg[i].length);
 		dmabuf += sg[i].length;
-
-		if (size < sg[i].length)
-			size = 0;
-		else
-			size -= sg[i].length;
-
-		if (size == 0)
-			break;
 	}
-
-	/*
-	 * Check that we didn't get a request to transfer
-	 * more data than can fit into the SG list.
-	 */
-
-	BUG_ON(size != 0);
-
-	host->size -= size;
 }
 
 /*
@@ -484,7 +430,7 @@ static void wbsd_empty_fifo(struct wbsd_host *host)
 	/*
 	 * Handle excessive data.
 	 */
-	if (data->bytes_xfered == host->size)
+	if (host->num_sg == 0)
 		return;
 
 	buffer = wbsd_sg_to_buffer(host) + host->offset;
@@ -513,12 +459,6 @@ static void wbsd_empty_fifo(struct wbsd_host *host)
 
 			data->bytes_xfered++;
 
-			/*
-			 * Transfer done?
-			 */
-			if (data->bytes_xfered == host->size)
-				return;
-
 			/*
 			 * End of scatter list entry?
 			 */
@@ -526,19 +466,8 @@ static void wbsd_empty_fifo(struct wbsd_host *host)
 				/*
 				 * Get next entry. Check if last.
 				 */
-				if (!wbsd_next_sg(host)) {
-					/*
-					 * We should never reach this point.
-					 * It means that we're trying to
-					 * transfer more blocks than can fit
-					 * into the scatter list.
-					 */
-					BUG_ON(1);
-
-					host->size = data->bytes_xfered;
-
+				if (!wbsd_next_sg(host))
 					return;
-				}
 
 				buffer = wbsd_sg_to_buffer(host);
 			}
@@ -550,7 +479,7 @@ static void wbsd_empty_fifo(struct wbsd_host *host)
 	 * hardware problem. The chip doesn't trigger
 	 * FIFO threshold interrupts properly.
 	 */
-	if ((host->size - data->bytes_xfered) < 16)
+	if ((data->blocks * data->blksz - data->bytes_xfered) < 16)
 		tasklet_schedule(&host->fifo_tasklet);
 }
 
@@ -564,7 +493,7 @@ static void wbsd_fill_fifo(struct wbsd_host *host)
 	 * Check that we aren't being called after the
 	 * entire buffer has been transfered.
 	 */
-	if (data->bytes_xfered == host->size)
+	if (host->num_sg == 0)
 		return;
 
 	buffer = wbsd_sg_to_buffer(host) + host->offset;
@@ -593,12 +522,6 @@ static void wbsd_fill_fifo(struct wbsd_host *host)
 
 			data->bytes_xfered++;
 
-			/*
-			 * Transfer done?
-			 */
-			if (data->bytes_xfered == host->size)
-				return;
-
 			/*
 			 * End of scatter list entry?
 			 */
@@ -606,19 +529,8 @@ static void wbsd_fill_fifo(struct wbsd_host *host)
 				/*
 				 * Get next entry. Check if last.
 				 */
-				if (!wbsd_next_sg(host)) {
-					/*
-					 * We should never reach this point.
-					 * It means that we're trying to
-					 * transfer more blocks than can fit
-					 * into the scatter list.
-					 */
-					BUG_ON(1);
-
-					host->size = data->bytes_xfered;
-
+				if (!wbsd_next_sg(host))
 					return;
-				}
 
 				buffer = wbsd_sg_to_buffer(host);
 			}
@@ -638,6 +550,7 @@ static void wbsd_prepare_data(struct wbsd_host *host, struct mmc_data *data)
 	u16 blksize;
 	u8 setup;
 	unsigned long dmaflags;
+	unsigned int size;
 
 	DBGF("blksz %04x blks %04x flags %08x\n",
 		data->blksz, data->blocks, data->flags);
@@ -647,7 +560,7 @@ static void wbsd_prepare_data(struct wbsd_host *host, struct mmc_data *data)
 	/*
 	 * Calculate size.
 	 */
-	host->size = data->blocks * data->blksz;
+	size = data->blocks * data->blksz;
 
 	/*
 	 * Check timeout values for overflow.
@@ -705,8 +618,8 @@ static void wbsd_prepare_data(struct wbsd_host *host, struct mmc_data *data)
 		/*
 		 * The buffer for DMA is only 64 kB.
 		 */
-		BUG_ON(host->size > 0x10000);
-		if (host->size > 0x10000) {
+		BUG_ON(size > 0x10000);
+		if (size > 0x10000) {
 			data->error = MMC_ERR_INVALID;
 			return;
 		}
@@ -729,7 +642,7 @@ static void wbsd_prepare_data(struct wbsd_host *host, struct mmc_data *data)
 		else
 			set_dma_mode(host->dma, DMA_MODE_WRITE & ~0x40);
 		set_dma_addr(host->dma, host->dma_addr);
-		set_dma_count(host->dma, host->size);
+		set_dma_count(host->dma, size);
 
 		enable_dma(host->dma);
 		release_dma_lock(dmaflags);
@@ -812,6 +725,10 @@ static void wbsd_finish_data(struct wbsd_host *host, struct mmc_data *data)
 		count = get_dma_residue(host->dma);
 		release_dma_lock(dmaflags);
 
+		data->bytes_xfered = host->mrq->data->blocks *
+			host->mrq->data->blksz - count;
+		data->bytes_xfered -= data->bytes_xfered % data->blksz;
+
 		/*
 		 * Any leftover data?
 		 */
@@ -820,6 +737,7 @@ static void wbsd_finish_data(struct wbsd_host *host, struct mmc_data *data)
 				"%d bytes left.\n",
 				mmc_hostname(host->mmc), count);
 
+			if (data->error == MMC_ERR_NONE)
 				data->error = MMC_ERR_FAILED;
 		} else {
 			/*
@@ -828,8 +746,11 @@ static void wbsd_finish_data(struct wbsd_host *host, struct mmc_data *data)
 			 */
 			if (data->flags & MMC_DATA_READ)
 				wbsd_dma_to_sg(host, data);
+		}
 
-			data->bytes_xfered = host->size;
+		if (data->error != MMC_ERR_NONE) {
+			if (data->bytes_xfered)
+				data->bytes_xfered -= data->blksz;
 		}
 	}
 
@@ -1167,7 +1088,7 @@ static void wbsd_tasklet_fifo(unsigned long param)
 	/*
 	 * Done?
 	 */
-	if (host->size == data->bytes_xfered) {
+	if (host->num_sg == 0) {
 		wbsd_write_index(host, WBSD_IDX_FIFOEN, 0);
 		tasklet_schedule(&host->finish_tasklet);
 	}

drivers/mmc/wbsd.h

 /*
  *  linux/drivers/mmc/wbsd.h - Winbond W83L51xD SD/MMC driver
  *
- *  Copyright (C) 2004-2005 Pierre Ossman, All Rights Reserved.
+ *  Copyright (C) 2004-2007 Pierre Ossman, All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -158,8 +158,6 @@ struct wbsd_host
 	unsigned int		offset;		/* Offset into current entry */
 	unsigned int		remain;		/* Data left in curren entry */
 
-	int			size;		/* Total size of transfer */
-
 	char*			dma_buffer;	/* ISA DMA buffer */
 	dma_addr_t		dma_addr;	/* Physical address for same */