Fix USDPAA DMA memory compress_frags() logic

The algorithm for joining DMA memory fragments could incorrecly
join fragments such that they were no longer power of 4 sized.
This patch reworks the compress_frags() logic so that can no longer
occur.

Signed-off-by: Roy Pledge <Roy.Pledge@freescale.com>
Change-Id: Ifdc0b357dbb8e83e2b45275e28c3eee5741d629c
Reviewed-on: http://git.am.freescale.net:8181/11685
Tested-by: Review Code-CDREVIEW <CDREVIEW@freescale.com>
Reviewed-by: Jeffrey Ladouceur <Jeffrey.Ladouceur@freescale.com>
Reviewed-by: Geoff Thorpe <Geoff.Thorpe@freescale.com>
Reviewed-by: Jose Rivera <German.Rivera@freescale.com>

1 files changed, 68 insertions, 15 deletions

diff --git a/drivers/staging/fsl_qbman/fsl_usdpaa.c b/drivers/staging/fsl_qbman/fsl_usdpaa.c
index a61ce9e..5d44eac 100644
--- a/drivers/staging/fsl_qbman/fsl_usdpaa.c
+++ b/drivers/staging/fsl_qbman/fsl_usdpaa.c
@@ -51,6 +51,7 @@ struct mem_fragment {
 	unsigned long pfn_len; /* PFN version of 'len' */
 	unsigned int refs; /* zero if unmapped */
 	u64 root_len; /* Size of the orignal fragment */
+	unsigned long root_pfn; /* PFN of the orignal fragment */
 	struct list_head list;
 	/* if mapped, flags+name captured at creation time */
 	u32 flags;
@@ -191,6 +192,14 @@ static u32 largest_page_size(u32 size)
 	return 0;
 }
 
+/* Determine if value is power of 4 */
+static inline bool is_power_of_4(u64 x)
+{
+	if (!is_power_of_2(x))
+		return false;
+	return !!(x & 0x5555555555555555ul);
+}
+
 /* Helper for ioctl_dma_map() when we have a larger fragment than we need. This
  * splits the fragment into 4 and returns the upper-most. (The caller can loop
  * until it has a suitable fragment size.) */
@@ -220,6 +229,7 @@ static struct mem_fragment *split_frag(struct mem_fragment *frag)
 	x[0]->pfn_len = x[1]->pfn_len = x[2]->pfn_len = frag->pfn_len;
 	x[0]->refs = x[1]->refs = x[2]->refs = 0;
 	x[0]->root_len = x[1]->root_len = x[2]->root_len = frag->root_len;
+	x[0]->root_pfn = x[1]->root_pfn = x[2]->root_pfn = frag->root_pfn;
 	list_add_tail(&x[0]->list, &frag->list);
 	list_add_tail(&x[1]->list, &x[0]->list);
 	list_add_tail(&x[2]->list, &x[1]->list);
@@ -231,10 +241,9 @@ static __maybe_unused void dump_frags(void)
 	struct mem_fragment *frag;
 	int i = 0;
 	list_for_each_entry(frag, &mem_list, list) {
-		pr_info("FRAG %d: base 0x%llx pfn_base 0x%lx len 0x%llx "
-			"root_len 0x%llx refs %d\n",
+		pr_info("FRAG %d: base 0x%llx pfn_base 0x%lx len 0x%llx root_len 0x%llx root_pfn 0x%lx refs %d\n",
 			i, frag->base, frag->pfn_base,
-			frag->len, frag->root_len, frag->refs);
+			frag->len, frag->root_len, frag->root_pfn, frag->refs);
 		++i;
 	}
 }
@@ -243,20 +252,63 @@ static __maybe_unused void dump_frags(void)
 static void compress_frags(void)
 {
 	/* Walk the fragment list and combine fragments */
-	struct mem_fragment *frag, *tmpfrag;
-	list_for_each_entry_safe(frag, tmpfrag, &mem_list, list) {
-		struct mem_fragment *next_frag =
-			list_entry(frag->list.next, struct mem_fragment, list);
-		if (frag->refs == 0 &&
-		    frag->len < frag->root_len &&
-		    &next_frag->list != &mem_list) {
-			if (next_frag->refs == 0) {
-				/* Merge with next */
-				next_frag->len += frag->len;
-				next_frag->pfn_len += frag->pfn_len;
-				list_del(&frag->list);
+	struct mem_fragment *frag, *nxtfrag;
+	u64 len = 0;
+
+	int i, numfrags;
+
+
+	frag = list_entry(mem_list.next, struct mem_fragment, list);
+
+	while (&frag->list != &mem_list) {
+		/* Must combine consecutive fragemenst with
+		   same root_pfn such that they are power of 4 */
+		if (frag->refs != 0) {
+			frag = list_entry(frag->list.next,
+					  struct mem_fragment, list);
+			continue; /* Not this window */
+		}
+		len = frag->len;
+		numfrags = 0;
+		nxtfrag =  list_entry(frag->list.next,
+				      struct mem_fragment, list);
+		while (true) {
+			if (&nxtfrag->list == &mem_list) {
+				numfrags = 0;
+				break; /* End of list */
+			}
+			if (nxtfrag->refs) {
+				numfrags = 0;
+				break; /* In use still */
+			}
+			if (nxtfrag->root_pfn != frag->root_pfn) {
+				numfrags = 0;
+				break; /* Crosses root fragment boundary */
+			}
+			len += nxtfrag->len;
+			numfrags++;
+			if (is_power_of_4(len)) {
+				/* These fragments can be combined */
+				break;
 			}
+			nxtfrag =  list_entry(nxtfrag->list.next,
+					      struct mem_fragment, list);
+		}
+		if (numfrags == 0) {
+			frag = list_entry(frag->list.next,
+					  struct mem_fragment, list);
+			continue; /* try the next window */
+		}
+		for (i = 0; i < numfrags; i++) {
+			struct mem_fragment *todel =
+				list_entry(nxtfrag->list.prev,
+					   struct mem_fragment, list);
+			nxtfrag->len += todel->len;
+			nxtfrag->pfn_len += todel->pfn_len;
+			list_del(&todel->list);
 		}
+		/* Re evaluate the list, things may merge now */
+		frag = list_entry(mem_list.next, struct mem_fragment, list);
 	}
 }
 
@@ -1789,6 +1841,7 @@ static int __init usdpaa_init(void)
 		}
 		frag->base = tmp_start;
 		frag->len = frag->root_len = frag_size;
+		frag->root_pfn = tmp_pfn_start;
 		frag->pfn_base = tmp_pfn_start;
 		frag->pfn_len = frag_size / PAGE_SIZE;
 		frag->refs = 0;