[PATCH 08/31] usb: usbssp: Added ring and segment handling functions.

[Pawel Laszczak]

Patch add generic function used for handling rings in driver.

USBSSP controller use three types of ring:
 - command ring - is used for handling commands. In this case driver
			is producer and HW is consumer.
 - event ring - ring for reporting events to driver. In this kind of
			ring HW is producer and driver is consumer.
 - transfer ring - ring used for queuing transfers. In this case
			software is producer and hardware is consumer.

Driver has only single event and transfer ring, but it uses many of
transfer rings. Each endpoint has own transfer rings.
If endpoint use streams than have many transfer rings for such endpoint.

Each ring can contain segments and each segment contains
Transfer Request blocks (TRB). The set of elements create
Transfer Descriptor (TD).

For more information please refer to XHCI specification.

Signed-off-by: Pawel Laszczak <pawell@xxxxxxxxxxx>
---
 drivers/usb/usbssp/gadget-mem.c | 433 +++++++++++++++++++++++++++++++-
 1 file changed, 432 insertions(+), 1 deletion(-)

diff --git a/drivers/usb/usbssp/gadget-mem.c b/drivers/usb/usbssp/gadget-mem.c
index 11bbf18dacba..06a59febbff2 100644
--- a/drivers/usb/usbssp/gadget-mem.c
+++ b/drivers/usb/usbssp/gadget-mem.c
@@ -18,6 +18,347 @@
 #include "gadget.h"
 #include "gadget-trace.h"
 
+/*
+ * Allocates a generic ring segment from the ring pool, sets the dma address,
+ * initializes the segment to zero, and sets the private next pointer to NULL.
+ *
+ * "All components of all Command and Transfer TRBs shall be initialized to '0'"
+ */
+static struct usbssp_segment *usbssp_segment_alloc(struct usbssp_udc *usbssp_data,
+						   unsigned int cycle_state,
+						   unsigned int max_packet,
+						   gfp_t flags)
+{
+	struct usbssp_segment *seg;
+	dma_addr_t dma;
+	int i;
+
+	seg = kzalloc(sizeof(*seg), flags);
+	if (!seg)
+		return NULL;
+
+	seg->trbs = dma_pool_zalloc(usbssp_data->segment_pool, flags, &dma);
+	if (!seg->trbs) {
+		kfree(seg);
+		return NULL;
+	}
+
+	if (max_packet) {
+		seg->bounce_buf = kzalloc(max_packet, flags | GFP_DMA);
+		if (!seg->bounce_buf) {
+			dma_pool_free(usbssp_data->segment_pool,
+				seg->trbs, dma);
+			kfree(seg);
+			return NULL;
+		}
+	}
+
+	/* If the cycle state is 0, set the cycle bit to 1 for all the TRBs */
+	if (cycle_state == 0) {
+		for (i = 0; i < TRBS_PER_SEGMENT; i++)
+			seg->trbs[i].link.control |= cpu_to_le32(TRB_CYCLE);
+	}
+	seg->dma = dma;
+	seg->next = NULL;
+
+	return seg;
+}
+
+static void usbssp_segment_free(struct usbssp_udc *usbssp_data,
+				struct usbssp_segment *seg)
+{
+	if (seg->trbs) {
+		dma_pool_free(usbssp_data->segment_pool, seg->trbs, seg->dma);
+		seg->trbs = NULL;
+	}
+	kfree(seg->bounce_buf);
+	kfree(seg);
+}
+
+static void usbssp_free_segments_for_ring(struct usbssp_udc *usbssp_data,
+					  struct usbssp_segment *first)
+{
+	struct usbssp_segment *seg;
+
+	seg = first->next;
+	while (seg != first) {
+		struct usbssp_segment *next = seg->next;
+
+		usbssp_segment_free(usbssp_data, seg);
+		seg = next;
+	}
+	usbssp_segment_free(usbssp_data, first);
+}
+
+/*
+ * Make the prev segment point to the next segment.
+ *
+ * Change the last TRB in the prev segment to be a Link TRB which points to the
+ * DMA address of the next segment. The caller needs to set any Link TRB
+ * related flags, such as End TRB, Toggle Cycle, and no snoop.
+ */
+static void usbssp_link_segments(struct usbssp_udc *usbssp_data,
+				 struct usbssp_segment *prev,
+				 struct usbssp_segment *next,
+				 enum usbssp_ring_type type)
+{
+	u32 val;
+
+	if (!prev || !next)
+		return;
+	prev->next = next;
+	if (type != TYPE_EVENT) {
+		prev->trbs[TRBS_PER_SEGMENT-1].link.segment_ptr =
+			cpu_to_le64(next->dma);
+
+		/*
+		 * Set the last TRB in the segment to have a TRB type ID
+		 * of Link TRB
+		 */
+		val = le32_to_cpu(prev->trbs[TRBS_PER_SEGMENT-1].link.control);
+		val &= ~TRB_TYPE_BITMASK;
+		val |= TRB_TYPE(TRB_LINK);
+		prev->trbs[TRBS_PER_SEGMENT-1].link.control = cpu_to_le32(val);
+	}
+}
+
+/*
+ * Link the ring to the new segments.
+ * Set Toggle Cycle for the new ring if needed.
+ */
+static void usbssp_link_rings(struct usbssp_udc *usbssp_data,
+			      struct usbssp_ring *ring,
+			      struct usbssp_segment *first,
+			      struct usbssp_segment *last,
+			      unsigned int num_segs)
+{
+	struct usbssp_segment *next;
+
+	if (!ring || !first || !last)
+		return;
+
+	next = ring->enq_seg->next;
+	usbssp_link_segments(usbssp_data, ring->enq_seg, first, ring->type);
+	usbssp_link_segments(usbssp_data, last, next, ring->type);
+	ring->num_segs += num_segs;
+	ring->num_trbs_free += (TRBS_PER_SEGMENT - 1) * num_segs;
+
+	if (ring->type != TYPE_EVENT && ring->enq_seg == ring->last_seg) {
+		ring->last_seg->trbs[TRBS_PER_SEGMENT-1].link.control
+			&= ~cpu_to_le32(LINK_TOGGLE);
+		last->trbs[TRBS_PER_SEGMENT-1].link.control
+			|= cpu_to_le32(LINK_TOGGLE);
+		ring->last_seg = last;
+	}
+}
+
+/*
+ * We need a radix tree for mapping physical addresses of TRBs to which stream
+ * ID they belong to. We need to do this because the device controller won't
+ * tell us which stream ring the TRB came from. We could store the stream ID
+ * in an event data TRB, but that doesn't help us for the cancellation case,
+ * since the endpoint may stop before it reaches that event data TRB.
+ *
+ * The radix tree maps the upper portion of the TRB DMA address to a ring
+ * segment that has the same upper portion of DMA addresses. For example,
+ * say I have segments of size 1KB, that are always 1KB aligned. A segment may
+ * start at 0x10c91000 and end at 0x10c913f0. If I use the upper 10 bits, the
+ * key to the stream ID is 0x43244. I can use the DMA address of the TRB to
+ * pass the radix tree a key to get the right stream ID:
+ *
+ *	0x10c90fff >> 10 = 0x43243
+ *	0x10c912c0 >> 10 = 0x43244
+ *	0x10c91400 >> 10 = 0x43245
+ *
+ * Obviously, only those TRBs with DMA addresses that are within the segment
+ * will make the radix tree return the stream ID for that ring.
+ *
+ * Caveats for the radix tree:
+ *
+ * The radix tree uses an unsigned long as a key pair. On 32-bit systems, an
+ * unsigned long will be 32-bits; on a 64-bit system an unsigned long will be
+ * 64-bits. Since we only request 32-bit DMA addresses, we can use that as the
+ * key on 32-bit or 64-bit systems (it would also be fine if we asked for 64-bit
+ * PCI DMA addresses on a 64-bit system). There might be a problem on 32-bit
+ * extended systems (where the DMA address can be bigger than 32-bits),
+ * if we allow the PCI dma mask to be bigger than 32-bits. So don't do that.
+ */
+static int usbssp_insert_segment_mapping(struct radix_tree_root *trb_address_map,
+					 struct usbssp_ring *ring,
+					 struct usbssp_segment *seg,
+					 gfp_t mem_flags)
+{
+	unsigned long key;
+	int ret;
+
+	key = (unsigned long)(seg->dma >> TRB_SEGMENT_SHIFT);
+
+	/* Skip any segments that were already added. */
+	if (radix_tree_lookup(trb_address_map, key))
+		return 0;
+
+	ret = radix_tree_maybe_preload(mem_flags);
+	if (ret)
+		return ret;
+
+	ret = radix_tree_insert(trb_address_map, key, ring);
+	radix_tree_preload_end();
+	return ret;
+}
+
+static void usbssp_remove_segment_mapping(struct radix_tree_root *trb_address_map,
+					  struct usbssp_segment *seg)
+{
+	unsigned long key;
+
+	key = (unsigned long)(seg->dma >> TRB_SEGMENT_SHIFT);
+	if (radix_tree_lookup(trb_address_map, key))
+		radix_tree_delete(trb_address_map, key);
+}
+
+static int usbssp_update_stream_segment_mapping(
+					struct radix_tree_root *trb_address_map,
+					struct usbssp_ring *ring,
+					struct usbssp_segment *first_seg,
+					struct usbssp_segment *last_seg,
+					gfp_t mem_flags)
+{
+	struct usbssp_segment *seg;
+	struct usbssp_segment *failed_seg;
+	int ret;
+
+	if (WARN_ON_ONCE(trb_address_map == NULL))
+		return 0;
+
+	seg = first_seg;
+	do {
+		ret = usbssp_insert_segment_mapping(trb_address_map,
+				ring, seg, mem_flags);
+		if (ret)
+			goto remove_streams;
+		if (seg == last_seg)
+			return 0;
+		seg = seg->next;
+	} while (seg != first_seg);
+
+	return 0;
+
+remove_streams:
+	failed_seg = seg;
+	seg = first_seg;
+	do {
+		usbssp_remove_segment_mapping(trb_address_map, seg);
+		if (seg == failed_seg)
+			return ret;
+		seg = seg->next;
+	} while (seg != first_seg);
+
+	return ret;
+}
+
+static void usbssp_remove_stream_mapping(struct usbssp_ring *ring)
+{
+	struct usbssp_segment *seg;
+
+	if (WARN_ON_ONCE(ring->trb_address_map == NULL))
+		return;
+
+	seg = ring->first_seg;
+	do {
+		usbssp_remove_segment_mapping(ring->trb_address_map, seg);
+		seg = seg->next;
+	} while (seg != ring->first_seg);
+}
+
+void usbssp_ring_free(struct usbssp_udc *usbssp_data, struct usbssp_ring *ring)
+{
+	if (!ring)
+		return;
+
+	trace_usbssp_ring_free(ring);
+
+	if (ring->first_seg) {
+		if (ring->type == TYPE_STREAM)
+			usbssp_remove_stream_mapping(ring);
+		usbssp_free_segments_for_ring(usbssp_data, ring->first_seg);
+	}
+
+	kfree(ring);
+}
+
+static void usbssp_initialize_ring_info(struct usbssp_ring *ring,
+					unsigned int cycle_state)
+{
+	/* The ring is empty, so the enqueue pointer == dequeue pointer */
+	ring->enqueue = ring->first_seg->trbs;
+	ring->enq_seg = ring->first_seg;
+	ring->dequeue = ring->enqueue;
+	ring->deq_seg = ring->first_seg;
+
+	/*
+	 * The ring is initialized to 0. The producer must write 1 to the cycle
+	 * bit to handover ownership of the TRB, so PCS = 1. The consumer must
+	 * compare CCS to the cycle bit to check ownership, so CCS = 1.
+	 *
+	 * New rings are initialized with cycle state equal to 1; if we are
+	 * handling ring expansion, set the cycle state equal to the old ring.
+	 */
+	ring->cycle_state = cycle_state;
+
+	/*
+	 * Each segment has a link TRB, and leave an extra TRB for SW
+	 * accounting purpose
+	 */
+	ring->num_trbs_free = ring->num_segs * (TRBS_PER_SEGMENT - 1) - 1;
+}
+
+/* Allocate segments and link them for a ring */
+static int usbssp_alloc_segments_for_ring(struct usbssp_udc *usbssp_data,
+					  struct usbssp_segment **first,
+					  struct usbssp_segment **last,
+					  unsigned int num_segs,
+					  unsigned int cycle_state,
+					  enum usbssp_ring_type type,
+					  unsigned int max_packet,
+					  gfp_t flags)
+{
+	struct usbssp_segment *prev;
+
+	/*allocation first segment */
+	prev = usbssp_segment_alloc(usbssp_data, cycle_state,
+			max_packet, flags);
+	if (!prev)
+		return -ENOMEM;
+	num_segs--;
+
+	*first = prev;
+	/*allocation all other segments*/
+	while (num_segs > 0) {
+		struct usbssp_segment	*next;
+
+		next = usbssp_segment_alloc(usbssp_data, cycle_state,
+				max_packet, flags);
+		if (!next) {
+			prev = *first;
+			/*Free all reserved segment*/
+			while (prev) {
+				next = prev->next;
+				usbssp_segment_free(usbssp_data, prev);
+				prev = next;
+			}
+			return -ENOMEM;
+		}
+		usbssp_link_segments(usbssp_data, prev, next, type);
+
+		prev = next;
+		num_segs--;
+	}
+	usbssp_link_segments(usbssp_data, prev, *first, type);
+	*last = prev;
+
+	return 0;
+}
+
 /**
  * Create a new ring with zero or more segments.
  *
@@ -32,10 +373,100 @@ static struct usbssp_ring *usbssp_ring_alloc(struct usbssp_udc *usbssp_data,
 					     unsigned int max_packet,
 					     gfp_t flags)
 {
-	/*TODO: implements function*/
+	struct usbssp_ring *ring;
+	int ret;
+
+	ring = kzalloc(sizeof *(ring), flags);
+	if (!ring)
+		return NULL;
+
+	ring->num_segs = num_segs;
+	ring->bounce_buf_len = max_packet;
+	INIT_LIST_HEAD(&ring->td_list);
+	ring->type = type;
+	if (num_segs == 0)
+		return ring;
+
+	ret = usbssp_alloc_segments_for_ring(usbssp_data, &ring->first_seg,
+			&ring->last_seg, num_segs, cycle_state, type,
+			max_packet, flags);
+	if (ret)
+		goto fail;
+
+	/* Only event ring does not use link TRB */
+	if (type != TYPE_EVENT) {
+		ring->last_seg->trbs[TRBS_PER_SEGMENT - 1].link.control |=
+			cpu_to_le32(LINK_TOGGLE);
+	}
+	usbssp_initialize_ring_info(ring, cycle_state);
+	trace_usbssp_ring_alloc(ring);
+	return ring;
+fail:
+	kfree(ring);
 	return NULL;
 }
 
+void usbssp_free_endpoint_ring(struct usbssp_udc *usbssp_data,
+			       struct usbssp_device *dev_priv,
+			       unsigned int ep_index)
+{
+	usbssp_ring_free(usbssp_data, dev_priv->eps[ep_index].ring);
+	dev_priv->eps[ep_index].ring = NULL;
+}
+
+/*
+ * Expand an existing ring.
+ * Allocate a new ring which has same segment numbers and link the two rings.
+ */
+int usbssp_ring_expansion(struct usbssp_udc *usbssp_data,
+			  struct usbssp_ring *ring,
+			  unsigned int num_trbs, gfp_t flags)
+{
+	struct usbssp_segment *first;
+	struct usbssp_segment *last;
+	unsigned int num_segs;
+	unsigned int num_segs_needed;
+	int ret;
+
+	num_segs_needed = (num_trbs + (TRBS_PER_SEGMENT - 1) - 1) /
+			(TRBS_PER_SEGMENT - 1);
+
+	/* Allocate number of segments we needed, or double the ring size */
+	num_segs = ring->num_segs > num_segs_needed ?
+			ring->num_segs : num_segs_needed;
+
+	ret = usbssp_alloc_segments_for_ring(usbssp_data, &first, &last,
+			num_segs, ring->cycle_state, ring->type,
+			ring->bounce_buf_len, flags);
+	if (ret)
+		return -ENOMEM;
+
+	if (ring->type == TYPE_STREAM)
+		ret = usbssp_update_stream_segment_mapping(
+				ring->trb_address_map, ring, first,
+				last, flags);
+	if (ret) {
+		struct usbssp_segment *next;
+
+		do {
+			next = first->next;
+			usbssp_segment_free(usbssp_data, first);
+			if (first == last)
+				break;
+			first = next;
+		} while (true);
+		return ret;
+	}
+
+	usbssp_link_rings(usbssp_data, ring, first, last, num_segs);
+	trace_usbssp_ring_expansion(ring);
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_ring_expansion,
+			"ring expansion succeed, now has %d segments",
+			ring->num_segs);
+
+	return 0;
+}
+
 struct usbssp_container_ctx *usbssp_alloc_container_ctx(
 					struct usbssp_udc *usbssp_data,
 					int type, gfp_t flags)
-- 
2.17.1