[PATCH 10/31] usb: usbssp: added usbssp_trb_in_td function.

[Pawel Laszczak]

Patch adds usbssp_trb_in_td function. This function checks if given
TRB object belongs to TD.

Patch also add procedure for testing this function and some testing
cases.

Signed-off-by: Pawel Laszczak <pawell@xxxxxxxxxxx>
---
 drivers/usb/usbssp/gadget-mem.c  | 168 +++++++++++++++++++++++++++++++
 drivers/usb/usbssp/gadget-ring.c |  76 ++++++++++++++
 drivers/usb/usbssp/gadget.h      |   5 +
 3 files changed, 249 insertions(+)

diff --git a/drivers/usb/usbssp/gadget-mem.c b/drivers/usb/usbssp/gadget-mem.c
index ecb6e1bbd212..fef83b6b6cf0 100644
--- a/drivers/usb/usbssp/gadget-mem.c
+++ b/drivers/usb/usbssp/gadget-mem.c
@@ -765,6 +765,170 @@ void usbssp_mem_cleanup(struct usbssp_udc *usbssp_data)
 	usbssp_data->page_shift = 0;
 }
 
+static int usbssp_test_trb_in_td(struct usbssp_udc *usbssp_data,
+				 struct usbssp_segment *input_seg,
+				 union usbssp_trb *start_trb,
+				 union usbssp_trb *end_trb,
+				 dma_addr_t input_dma,
+				 struct usbssp_segment *result_seg,
+				 char *test_name, int test_number)
+{
+	unsigned long long start_dma;
+	unsigned long long end_dma;
+	struct usbssp_segment *seg;
+
+	start_dma = usbssp_trb_virt_to_dma(input_seg, start_trb);
+	end_dma = usbssp_trb_virt_to_dma(input_seg, end_trb);
+
+	seg = usbssp_trb_in_td(usbssp_data, input_seg, start_trb,
+			end_trb, input_dma, false);
+
+	if (seg != result_seg) {
+		dev_warn(usbssp_data->dev, "WARN: %s TRB math test %d failed!\n",
+				test_name, test_number);
+		dev_warn(usbssp_data->dev, "Tested TRB math w/ seg %p and "
+				"input DMA 0x%llx\n",
+				input_seg,
+				(unsigned long long) input_dma);
+		dev_warn(usbssp_data->dev, "starting TRB %p (0x%llx DMA), "
+				"ending TRB %p (0x%llx DMA)\n",
+				start_trb, start_dma,
+				end_trb, end_dma);
+		dev_warn(usbssp_data->dev, "Expected seg %p, got seg %p\n",
+				result_seg, seg);
+
+		usbssp_trb_in_td(usbssp_data, input_seg, start_trb,
+			end_trb, input_dma, true);
+		return -1;
+	}
+	return 0;
+}
+
+/* TRB math checks for usbssp_trb_in_td(), using the command and event rings. */
+static int usbssp_check_trb_in_td_math(struct usbssp_udc *usbssp_data)
+{
+	struct {
+		dma_addr_t input_dma;
+		struct usbssp_segment *result_seg;
+	} simple_test_vector[] = {
+		/* A zeroed DMA field should fail */
+		{ 0, NULL },
+		/* One TRB before the ring start should fail */
+		{ usbssp_data->event_ring->first_seg->dma - 16, NULL },
+		/* One byte before the ring start should fail */
+		{ usbssp_data->event_ring->first_seg->dma - 1, NULL },
+		/* Starting TRB should succeed */
+		{ usbssp_data->event_ring->first_seg->dma,
+				usbssp_data->event_ring->first_seg },
+		/* Ending TRB should succeed */
+		{ usbssp_data->event_ring->first_seg->dma +
+				(TRBS_PER_SEGMENT - 1)*16,
+			usbssp_data->event_ring->first_seg },
+		/* One byte after the ring end should fail */
+		{ usbssp_data->event_ring->first_seg->dma +
+				(TRBS_PER_SEGMENT - 1)*16 + 1, NULL },
+		/* One TRB after the ring end should fail */
+		{ usbssp_data->event_ring->first_seg->dma +
+				(TRBS_PER_SEGMENT)*16, NULL },
+		/* An address of all ones should fail */
+		{ (dma_addr_t) (~0), NULL },
+	};
+	struct {
+		struct usbssp_segment *input_seg;
+		union usbssp_trb *start_trb;
+		union usbssp_trb *end_trb;
+		dma_addr_t input_dma;
+		struct usbssp_segment *result_seg;
+	} complex_test_vector[] = {
+		/* Test feeding a valid DMA address from a different ring */
+		{	.input_seg = usbssp_data->event_ring->first_seg,
+			.start_trb = usbssp_data->event_ring->first_seg->trbs,
+			.end_trb = &usbssp_data->event_ring->first_seg->trbs[TRBS_PER_SEGMENT - 1],
+			.input_dma = usbssp_data->cmd_ring->first_seg->dma,
+			.result_seg = NULL,
+		},
+		/* Test feeding a valid end TRB from a different ring */
+		{	.input_seg = usbssp_data->event_ring->first_seg,
+			.start_trb = usbssp_data->event_ring->first_seg->trbs,
+			.end_trb = &usbssp_data->cmd_ring->first_seg->trbs[TRBS_PER_SEGMENT - 1],
+			.input_dma = usbssp_data->cmd_ring->first_seg->dma,
+			.result_seg = NULL,
+		},
+		/* Test feeding a valid start and end TRB from a different ring */
+		{	.input_seg = usbssp_data->event_ring->first_seg,
+			.start_trb = usbssp_data->cmd_ring->first_seg->trbs,
+			.end_trb = &usbssp_data->cmd_ring->first_seg->trbs[TRBS_PER_SEGMENT - 1],
+			.input_dma = usbssp_data->cmd_ring->first_seg->dma,
+			.result_seg = NULL,
+		},
+		/* TRB in this ring, but after this TD */
+		{	.input_seg = usbssp_data->event_ring->first_seg,
+			.start_trb = &usbssp_data->event_ring->first_seg->trbs[0],
+			.end_trb = &usbssp_data->event_ring->first_seg->trbs[3],
+			.input_dma = usbssp_data->event_ring->first_seg->dma + 4*16,
+			.result_seg = NULL,
+		},
+		/* TRB in this ring, but before this TD */
+		{	.input_seg = usbssp_data->event_ring->first_seg,
+			.start_trb = &usbssp_data->event_ring->first_seg->trbs[3],
+			.end_trb = &usbssp_data->event_ring->first_seg->trbs[6],
+			.input_dma = usbssp_data->event_ring->first_seg->dma + 2*16,
+			.result_seg = NULL,
+		},
+		/* TRB in this ring, but after this wrapped TD */
+		{	.input_seg = usbssp_data->event_ring->first_seg,
+			.start_trb = &usbssp_data->event_ring->first_seg->trbs[TRBS_PER_SEGMENT - 3],
+			.end_trb = &usbssp_data->event_ring->first_seg->trbs[1],
+			.input_dma = usbssp_data->event_ring->first_seg->dma + 2*16,
+			.result_seg = NULL,
+		},
+		/* TRB in this ring, but before this wrapped TD */
+		{	.input_seg = usbssp_data->event_ring->first_seg,
+			.start_trb = &usbssp_data->event_ring->first_seg->trbs[TRBS_PER_SEGMENT - 3],
+			.end_trb = &usbssp_data->event_ring->first_seg->trbs[1],
+			.input_dma = usbssp_data->event_ring->first_seg->dma + (TRBS_PER_SEGMENT - 4)*16,
+			.result_seg = NULL,
+		},
+		/* TRB not in this ring, and we have a wrapped TD */
+		{	.input_seg = usbssp_data->event_ring->first_seg,
+			.start_trb = &usbssp_data->event_ring->first_seg->trbs[TRBS_PER_SEGMENT - 3],
+			.end_trb = &usbssp_data->event_ring->first_seg->trbs[1],
+			.input_dma = usbssp_data->cmd_ring->first_seg->dma + 2*16,
+			.result_seg = NULL,
+		},
+	};
+
+	unsigned int num_tests;
+	int i, ret;
+
+	num_tests = ARRAY_SIZE(simple_test_vector);
+	for (i = 0; i < num_tests; i++) {
+		ret = usbssp_test_trb_in_td(usbssp_data,
+				usbssp_data->event_ring->first_seg,
+				usbssp_data->event_ring->first_seg->trbs,
+				&usbssp_data->event_ring->first_seg->trbs[TRBS_PER_SEGMENT - 1],
+				simple_test_vector[i].input_dma,
+				simple_test_vector[i].result_seg,
+				"Simple", i);
+		if (ret < 0)
+			return ret;
+	}
+
+	num_tests = ARRAY_SIZE(complex_test_vector);
+	for (i = 0; i < num_tests; i++) {
+		ret = usbssp_test_trb_in_td(usbssp_data,
+				complex_test_vector[i].input_seg,
+				complex_test_vector[i].start_trb,
+				complex_test_vector[i].end_trb,
+				complex_test_vector[i].input_dma,
+				complex_test_vector[i].result_seg,
+				"Complex", i);
+		if (ret < 0)
+			return ret;
+	}
+	dev_dbg(usbssp_data->dev, "TRB math tests passed.\n");
+	return 0;
+}
 
 static void usbssp_set_event_deq(struct usbssp_udc *usbssp_data)
 {
@@ -1187,6 +1351,10 @@ int usbssp_mem_init(struct usbssp_udc *usbssp_data, gfp_t flags)
 	if (!usbssp_data->event_ring)
 		goto fail;
 
+	/*invoke check procedure for usbssp_trb_in_td function*/
+	if (usbssp_check_trb_in_td_math(usbssp_data) < 0)
+		goto fail;
+
 	ret = usbssp_alloc_erst(usbssp_data, usbssp_data->event_ring,
 			&usbssp_data->erst, flags);
 	if (ret)
diff --git a/drivers/usb/usbssp/gadget-ring.c b/drivers/usb/usbssp/gadget-ring.c
index 7c4b6b7b7b0a..3075909c2e31 100644
--- a/drivers/usb/usbssp/gadget-ring.c
+++ b/drivers/usb/usbssp/gadget-ring.c
@@ -73,3 +73,79 @@ void usbssp_cleanup_command_queue(struct usbssp_udc *usbssp_data)
 	list_for_each_entry_safe(cur_cmd, tmp_cmd, &usbssp_data->cmd_list, cmd_list)
 		usbssp_complete_del_and_free_cmd(cur_cmd, COMP_COMMAND_ABORTED);
 }
+
+/*
+ * This TD is defined by the TRBs starting at start_trb in start_seg and ending
+ * at end_trb, which may be in another segment. If the suspect DMA address is a
+ * TRB in this TD, this function returns that TRB's segment. Otherwise it
+ * returns 0.
+ */
+struct usbssp_segment *usbssp_trb_in_td(struct usbssp_udc *usbssp_data,
+					struct usbssp_segment *start_seg,
+					union usbssp_trb *start_trb,
+					union usbssp_trb *end_trb,
+					dma_addr_t suspect_dma,
+					bool debug)
+{
+	dma_addr_t start_dma;
+	dma_addr_t end_seg_dma;
+	dma_addr_t end_trb_dma;
+	struct usbssp_segment *cur_seg;
+
+	start_dma = usbssp_trb_virt_to_dma(start_seg, start_trb);
+	cur_seg = start_seg;
+
+	do {
+		if (start_dma == 0)
+			return NULL;
+		/* We may get an event for a Link TRB in the middle of a TD */
+		end_seg_dma = usbssp_trb_virt_to_dma(cur_seg,
+				&cur_seg->trbs[TRBS_PER_SEGMENT - 1]);
+		/* If the end TRB isn't in this segment, this is set to 0 */
+		end_trb_dma = usbssp_trb_virt_to_dma(cur_seg, end_trb);
+
+		if (debug)
+			dev_warn(usbssp_data->dev,
+				"Looking for event-dma %016llx trb-start"
+				"%016llx trb-end %016llx seg-start %016llx"
+				" seg-end %016llx\n",
+				(unsigned long long)suspect_dma,
+				(unsigned long long)start_dma,
+				(unsigned long long)end_trb_dma,
+				(unsigned long long)cur_seg->dma,
+				(unsigned long long)end_seg_dma);
+
+		if (end_trb_dma > 0) {
+			/*
+			 * The end TRB is in this segment, so suspect should
+			 * be here
+			 */
+			if (start_dma <= end_trb_dma) {
+				if (suspect_dma >= start_dma &&
+				    suspect_dma <= end_trb_dma)
+					return cur_seg;
+			} else {
+				/*
+				 * Case for one segment with a
+				 * TD wrapped around to the top
+				 */
+				if ((suspect_dma >= start_dma &&
+						suspect_dma <= end_seg_dma) ||
+						(suspect_dma >= cur_seg->dma &&
+						 suspect_dma <= end_trb_dma))
+					return cur_seg;
+			}
+			return NULL;
+		} else {
+			/* Might still be somewhere in this segment */
+			if (suspect_dma >= start_dma &&
+			    suspect_dma <= end_seg_dma)
+				return cur_seg;
+		}
+
+		cur_seg = cur_seg->next;
+		start_dma = usbssp_trb_virt_to_dma(cur_seg, &cur_seg->trbs[0]);
+	} while (cur_seg != start_seg);
+
+	return NULL;
+}
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 9dba86a0274a..927c34579899 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1711,6 +1711,11 @@ irqreturn_t usbssp_irq(int irq, void *priv);
 /* USBSSP ring, segment, TRB, and TD functions */
 dma_addr_t usbssp_trb_virt_to_dma(struct usbssp_segment *seg,
 				union usbssp_trb *trb);
+struct usbssp_segment *usbssp_trb_in_td(struct usbssp_udc *usbssp_data,
+				struct usbssp_segment *start_seg,
+				union usbssp_trb *start_trb,
+				union usbssp_trb *end_trb,
+				dma_addr_t suspect_dma, bool debug);
 void usbssp_handle_command_timeout(struct work_struct *work);
 
 void usbssp_cleanup_command_queue(struct usbssp_udc *usbssp_data);
-- 
2.17.1