qed: Fix qed_chain_set_prod() for PBL chains with non power of 2 page count

In PBL chains with non power of 2 page count, the producer is not at the
beginning of the chain when index is 0 after a wrap. Therefore, after the
producer index wrap around, page index should be calculated more carefully.

Signed-off-by: Denis Bolotin <dbolotin@marvell.com>
Signed-off-by: Ariel Elior <aelior@marvell.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/include/linux/qed/qed_chain.h b/include/linux/qed/qed_chain.h
index 59ddf9af909e456fcc6c3ed3b3574b6d4da06477..2dd0a9ed5b361472fbb1794b3b77f96fc69780ce 100644 (file)
--- a/include/linux/qed/qed_chain.h
+++ b/include/linux/qed/qed_chain.h
@@ -663,6 +663,37 @@ out:
 static inline void qed_chain_set_prod(struct qed_chain *p_chain,
                                      u32 prod_idx, void *p_prod_elem)
 {
+       if (p_chain->mode == QED_CHAIN_MODE_PBL) {
+               u32 cur_prod, page_mask, page_cnt, page_diff;
+
+               cur_prod = is_chain_u16(p_chain) ? p_chain->u.chain16.prod_idx :
+                          p_chain->u.chain32.prod_idx;
+
+               /* Assume that number of elements in a page is power of 2 */
+               page_mask = ~p_chain->elem_per_page_mask;
+
+               /* Use "cur_prod - 1" and "prod_idx - 1" since producer index
+                * reaches the first element of next page before the page index
+                * is incremented. See qed_chain_produce().
+                * Index wrap around is not a problem because the difference
+                * between current and given producer indices is always
+                * positive and lower than the chain's capacity.
+                */
+               page_diff = (((cur_prod - 1) & page_mask) -
+                            ((prod_idx - 1) & page_mask)) /
+                           p_chain->elem_per_page;
+
+               page_cnt = qed_chain_get_page_cnt(p_chain);
+               if (is_chain_u16(p_chain))
+                       p_chain->pbl.c.u16.prod_page_idx =
+                               (p_chain->pbl.c.u16.prod_page_idx -
+                                page_diff + page_cnt) % page_cnt;
+               else
+                       p_chain->pbl.c.u32.prod_page_idx =
+                               (p_chain->pbl.c.u32.prod_page_idx -
+                                page_diff + page_cnt) % page_cnt;
+       }
+
        if (is_chain_u16(p_chain))
                p_chain->u.chain16.prod_idx = (u16) prod_idx;
        else