staging: xillybus: Reorganizing xilly_setupchannels()

Duplicate code in this function was moved into a new function,
xilly_get_dma_buffers().

There is no change in functionality.
Signed-off-by: Eli Billauer <eli.billauer@gmail.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

drivers/staging/xillybus/xillybus_core.c

@@ -319,28 +319,130 @@ EXPORT_SYMBOL(xillybus_isr);
 
 static void xillybus_autoflush(struct work_struct *work);
 
+struct xilly_alloc_state {
+	void *salami;
+	int left_of_salami;
+	int nbuffer;
+	enum dma_data_direction direction;
+	u32 regdirection;
+};
+
+static int xilly_get_dma_buffers(struct xilly_endpoint *ep,
+				 struct xilly_alloc_state *s,
+				 struct xilly_buffer **buffers,
+				 int bufnum, int bytebufsize)
+{
+	int i, rc;
+	dma_addr_t dma_addr;
+	struct device *dev = ep->dev;
+	struct xilly_buffer *this_buffer = NULL; /* Init to silence warning */
+
+	if (buffers) { /* Not the message buffer */
+		this_buffer = devm_kzalloc(
+			dev, bufnum * sizeof(struct xilly_buffer),
+			GFP_KERNEL);
+
+		if (!this_buffer)
+			return -ENOMEM;
+	}
+
+	for (i = 0; i < bufnum; i++) {
+		/*
+		 * Buffers are expected in descending size order, so there
+		 * is either enough space for this buffer or none at all.
+		 */
+
+		if ((s->left_of_salami < bytebufsize) &&
+		    (s->left_of_salami > 0)) {
+			dev_err(ep->dev,
+				"Corrupt buffer allocation in IDT. Aborting.\n");
+			return -ENODEV;
+		}
+
+		if (s->left_of_salami == 0) {
+			int allocorder, allocsize;
+
+			allocsize = PAGE_SIZE;
+			allocorder = 0;
+			while (bytebufsize > allocsize) {
+				allocsize *= 2;
+				allocorder++;
+			}
+
+			s->salami = (void *) devm_get_free_pages(
+				dev,
+				GFP_KERNEL | __GFP_DMA32 | __GFP_ZERO,
+				allocorder);
+
+			if (!s->salami)
+				return -ENOMEM;
+			s->left_of_salami = allocsize;
+		}
+
+		rc = ep->ephw->map_single(ep, s->salami,
+					  bytebufsize, s->direction,
+					  &dma_addr);
+
+		if (rc)
+			return rc;
+
+		iowrite32((u32) (dma_addr & 0xffffffff),
+			  &ep->registers[fpga_dma_bufaddr_lowaddr_reg]);
+		iowrite32(((u32) ((((u64) dma_addr) >> 32) & 0xffffffff)),
+			  &ep->registers[fpga_dma_bufaddr_highaddr_reg]);
+		mmiowb();
+
+		if (buffers) { /* Not the message buffer */
+			this_buffer->addr = s->salami;
+			this_buffer->dma_addr = dma_addr;
+			buffers[i] = this_buffer++;
+
+			iowrite32(s->regdirection | s->nbuffer++,
+				  &ep->registers[fpga_dma_bufno_reg]);
+		} else {
+			ep->msgbuf_addr = s->salami;
+			ep->msgbuf_dma_addr = dma_addr;
+			ep->msg_buf_size = bytebufsize;
+
+			iowrite32(s->regdirection,
+				  &ep->registers[fpga_dma_bufno_reg]);
+		}
+
+		s->left_of_salami -= bytebufsize;
+		s->salami += bytebufsize;
+	}
+	return 0; /* Success */
+}
+
 static int xilly_setupchannels(struct xilly_endpoint *ep,
 			       unsigned char *chandesc,
 			       int entries
 	)
 {
 	struct device *dev = ep->dev;
-	int i, entry, wr_nbuffer, rd_nbuffer;
+	int i, entry, rc;
 	struct xilly_channel *channel;
 	int channelnum, bufnum, bufsize, format, is_writebuf;
 	int bytebufsize;
 	int synchronous, allowpartial, exclusive_open, seekable;
 	int supports_nonempty;
-	void *wr_salami = NULL;
-	void *rd_salami = NULL;
-	int left_of_wr_salami = 0;
-	int left_of_rd_salami = 0;
-	dma_addr_t dma_addr;
 	int msg_buf_done = 0;
-	const gfp_t gfp_mask = GFP_KERNEL | __GFP_DMA32 | __GFP_ZERO;
 
-	struct xilly_buffer *this_buffer = NULL; /* Init to silence warning */
-	int rc = 0;
+	struct xilly_alloc_state rd_alloc = {
+		.salami = NULL,
+		.left_of_salami = 0,
+		.nbuffer = 1,
+		.direction = DMA_TO_DEVICE,
+		.regdirection = 0,
+	};
+
+	struct xilly_alloc_state wr_alloc = {
+		.salami = NULL,
+		.left_of_salami = 0,
+		.nbuffer = 1,
+		.direction = DMA_FROM_DEVICE,
+		.regdirection = 0x80000000,
+	};
 
 	channel = devm_kzalloc(dev, ep->num_channels *
 			       sizeof(struct xilly_channel), GFP_KERNEL);
@@ -395,17 +497,9 @@ static int xilly_setupchannels(struct xilly_endpoint *ep,
 		ep->channels[i] = channel++;
 	}
 
-	/*
-	 * The DMA buffer address update is atomic on the FPGA, so even if
-	 * it was in the middle of sending messages to some buffer, changing
-	 * the address is safe, since the data will go to either of the
-	 * buffers. Not that this situation should occur at all anyhow.
-	 */
-
-	wr_nbuffer = 1;
-	rd_nbuffer = 1; /* Buffer zero isn't used at all */
-
 	for (entry = 0; entry < entries; entry++, chandesc += 4) {
+		struct xilly_buffer **buffers = NULL;
+
 		is_writebuf = chandesc[0] & 0x01;
 		channelnum = (chandesc[0] >> 1) | ((chandesc[1] & 0x0f) << 7);
 		format = (chandesc[1] >> 4) & 0x03;
@@ -426,40 +520,35 @@ static int xilly_setupchannels(struct xilly_endpoint *ep,
 
 		channel = ep->channels[channelnum]; /* NULL for msg channel */
 
-		bytebufsize = bufsize << 2; /* Overwritten just below */
-
-		if (!is_writebuf) {
-			channel->num_rd_buffers = bufnum;
+		if (!is_writebuf || channelnum > 0) {
 			channel->log2_element_size = ((format > 2) ?
 						      2 : format);
+
 			bytebufsize = channel->rd_buf_size = bufsize *
 				(1 << channel->log2_element_size);
-			channel->rd_allow_partial = allowpartial;
-			channel->rd_synchronous = synchronous;
-			channel->rd_exclusive_open = exclusive_open;
-			channel->seekable = seekable;
 
-			channel->rd_buffers = devm_kzalloc(dev,
+			buffers = devm_kzalloc(dev,
 				bufnum * sizeof(struct xilly_buffer *),
 				GFP_KERNEL);
 
-			if (!channel->rd_buffers)
+			if (!buffers)
 				goto memfail;
+		} else
+			bytebufsize = bufsize << 2;
 
-			this_buffer = devm_kzalloc(dev,
-				bufnum * sizeof(struct xilly_buffer),
-				GFP_KERNEL);
+		if (!is_writebuf) {
+			channel->num_rd_buffers = bufnum;
+			channel->rd_allow_partial = allowpartial;
+			channel->rd_synchronous = synchronous;
+			channel->rd_exclusive_open = exclusive_open;
+			channel->seekable = seekable;
 
-			if (!this_buffer)
-				goto memfail;
+			channel->rd_buffers = buffers;
+			rc = xilly_get_dma_buffers(ep, &rd_alloc, buffers,
+						   bufnum, bytebufsize);
 		}
-
 		else if (channelnum > 0) {
 			channel->num_wr_buffers = bufnum;
-			channel->log2_element_size = ((format > 2) ?
-						      2 : format);
-			bytebufsize = channel->wr_buf_size = bufsize *
-				(1 << channel->log2_element_size);
 
 			channel->seekable = seekable;
 			channel->wr_supports_nonempty = supports_nonempty;
@@ -468,171 +557,17 @@ static int xilly_setupchannels(struct xilly_endpoint *ep,
 			channel->wr_synchronous = synchronous;
 			channel->wr_exclusive_open = exclusive_open;
 
-			channel->wr_buffers = devm_kzalloc(dev,
-				bufnum * sizeof(struct xilly_buffer *),
-				GFP_KERNEL);
-
-			if (!channel->wr_buffers)
-				goto memfail;
-
-			this_buffer = devm_kzalloc(dev,
-				bufnum * sizeof(struct xilly_buffer),
-				GFP_KERNEL);
-
-			if (!this_buffer)
-				goto memfail;
+			channel->wr_buffers = buffers;
+			rc = xilly_get_dma_buffers(ep, &wr_alloc, buffers,
+						   bufnum, bytebufsize);
+		} else {
+			rc = xilly_get_dma_buffers(ep, &wr_alloc, NULL,
+						   bufnum, bytebufsize);
+			msg_buf_done++;
 		}
 
-		/*
-		 * Although daunting, we cut the chunks for read buffers
-		 * from a different salami than the write buffers',
-		 * possibly improving performance.
-		 */
-
-		if (is_writebuf)
-			for (i = 0; i < bufnum; i++) {
-				/*
-				 * Buffers are expected in descending
-				 * byte-size order, so there is either
-				 * enough for this buffer or none at all.
-				 */
-				if ((left_of_wr_salami < bytebufsize) &&
-				    (left_of_wr_salami > 0)) {
-					dev_err(ep->dev,
-						"Corrupt buffer allocation in IDT. Aborting.\n");
-					return -ENODEV;
-				}
-
-				if (left_of_wr_salami == 0) {
-					int allocorder, allocsize;
-
-					allocsize = PAGE_SIZE;
-					allocorder = 0;
-					while (bytebufsize > allocsize) {
-						allocsize *= 2;
-						allocorder++;
-					}
-
-					wr_salami = (void *)
-						devm_get_free_pages(
-							dev, gfp_mask,
-							allocorder);
-
-					if (!wr_salami)
-						goto memfail;
-					left_of_wr_salami = allocsize;
-				}
-
-				rc = ep->ephw->map_single(ep, wr_salami,
-							  bytebufsize,
-							  DMA_FROM_DEVICE,
-							  &dma_addr);
-
-				if (rc)
-					goto dmafail;
-
-				iowrite32(
-					(u32) (dma_addr & 0xffffffff),
-					&ep->registers[
-						fpga_dma_bufaddr_lowaddr_reg]
-					);
-				iowrite32(
-					((u32) ((((u64) dma_addr) >> 32)
-						& 0xffffffff)),
-					&ep->registers[
-						fpga_dma_bufaddr_highaddr_reg]
-					);
-				mmiowb();
-
-				if (channelnum > 0) {
-					this_buffer->addr = wr_salami;
-					this_buffer->dma_addr = dma_addr;
-					channel->wr_buffers[i] = this_buffer++;
-
-					iowrite32(
-						0x80000000 | wr_nbuffer++,
-						&ep->registers[
-							fpga_dma_bufno_reg]);
-				} else {
-					ep->msgbuf_addr = wr_salami;
-					ep->msgbuf_dma_addr = dma_addr;
-					ep->msg_buf_size = bytebufsize;
-					msg_buf_done++;
-
-					iowrite32(
-						0x80000000, &ep->registers[
-							fpga_dma_bufno_reg]);
-				}
-
-				left_of_wr_salami -= bytebufsize;
-				wr_salami += bytebufsize;
-			}
-		else /* Read buffers */
-			for (i = 0; i < bufnum; i++) {
-				/*
-				 * Buffers are expected in descending
-				 * byte-size order, so there is either
-				 * enough for this buffer or none at all.
-				 */
-				if ((left_of_rd_salami < bytebufsize) &&
-				    (left_of_rd_salami > 0)) {
-					dev_err(ep->dev,
-						"Corrupt buffer allocation in IDT. Aborting.\n");
-					return -ENODEV;
-				}
-
-				if (left_of_rd_salami == 0) {
-					int allocorder, allocsize;
-
-					allocsize = PAGE_SIZE;
-					allocorder = 0;
-					while (bytebufsize > allocsize) {
-						allocsize *= 2;
-						allocorder++;
-					}
-
-					rd_salami = (void *)
-						devm_get_free_pages(
-							dev, gfp_mask,
-							allocorder);
-
-					if (!rd_salami)
-						goto memfail;
-					left_of_rd_salami = allocsize;
-				}
-
-
-				rc = ep->ephw->map_single(ep, rd_salami,
-							  bytebufsize,
-							  DMA_TO_DEVICE,
-							  &dma_addr);
-
-				if (rc)
-					goto dmafail;
-
-				iowrite32(
-					(u32) (dma_addr & 0xffffffff),
-					&ep->registers[
-						fpga_dma_bufaddr_lowaddr_reg]
-					);
-				iowrite32(
-					((u32) ((((u64) dma_addr) >> 32)
-						& 0xffffffff)),
-					&ep->registers[
-						fpga_dma_bufaddr_highaddr_reg]
-					);
-				mmiowb();
-
-				this_buffer->addr = rd_salami;
-				this_buffer->dma_addr = dma_addr;
-				channel->rd_buffers[i] = this_buffer++;
-
-				iowrite32(rd_nbuffer++,
-					  &ep->registers[fpga_dma_bufno_reg]);
-
-				left_of_rd_salami -= bytebufsize;
-				rd_salami += bytebufsize;
-			}
+		if (rc)
+			goto memfail;
 	}
 
 	if (!msg_buf_done) {
@@ -640,16 +575,12 @@ static int xilly_setupchannels(struct xilly_endpoint *ep,
 			"Corrupt IDT: No message buffer. Aborting.\n");
 		return -ENODEV;
 	}
-
 	return 0;
 
 memfail:
 	dev_err(ep->dev,
-		"Failed to allocate write buffer memory. Aborting.\n");
+		"Failed to assign DMA buffer memory. Aborting.\n");
 	return -ENOMEM;
-dmafail:
-	dev_err(ep->dev, "Failed to map DMA memory!. Aborting.\n");
-	return rc;
 }
 
 static void xilly_scan_idt(struct xilly_endpoint *endpoint,