Add support for zsdadev queue enqueue.

Signed-off-by: Hanxiao Li <li.hanxiao@zte.com.cn>
---
 drivers/common/zsda/zsda_qp.c | 105 ++++++++++++++++++++++++++++++++++
 drivers/common/zsda/zsda_qp.h |   2 +
 2 files changed, 107 insertions(+)

diff --git a/drivers/common/zsda/zsda_qp.c b/drivers/common/zsda/zsda_qp.c
index f1fe6c5817..87932597ed 100644
--- a/drivers/common/zsda/zsda_qp.c
+++ b/drivers/common/zsda/zsda_qp.c
@@ -682,6 +682,111 @@ zsda_queue_pair_release(struct zsda_qp **qp_addr)
 	return 0;
 }
 
+static int
+zsda_find_next_free_cookie(const struct zsda_queue *queue, void **op_cookie,
+		      uint16_t *idx)
+{
+	uint16_t old_tail = queue->tail;
+	uint16_t tail = queue->tail;
+	struct zsda_op_cookie *cookie;
+
+	do {
+		cookie = op_cookie[tail];
+		if (!cookie->used) {
+			*idx = tail & (queue->queue_size - 1);
+			return 0;
+		}
+		tail = zsda_modulo_16(tail++, queue->modulo_mask);
+	} while (old_tail != tail);
+
+	return -EINVAL;
+}
+
+static int
+zsda_enqueue(void *op, struct zsda_qp *qp)
+{
+	uint16_t new_tail;
+	enum zsda_service_type type;
+	void **op_cookie;
+	int ret = 0;
+	struct zsda_queue *queue;
+
+	for (type = 0; type < ZSDA_SERVICE_INVALID; type++) {
+		if (qp->srv[type].used) {
+			if (!qp->srv[type].match(op))
+				continue;
+			queue = &qp->srv[type].tx_q;
+			op_cookie = qp->srv[type].op_cookies;
+
+			if (zsda_find_next_free_cookie(queue, op_cookie,
+						  &new_tail)) {
+				ret = -EBUSY;
+				break;
+			}
+			ret = qp->srv[type].tx_cb(op, queue, op_cookie,
+						  new_tail);
+			if (ret) {
+				qp->srv[type].stats.enqueue_err_count++;
+				ZSDA_LOG(ERR, "Failed! config wqe");
+				break;
+			}
+			qp->srv[type].stats.enqueued_count++;
+
+			queue->tail = zsda_modulo_16(new_tail + 1,
+						     queue->queue_size - 1);
+
+			if (new_tail > queue->tail)
+				queue->valid =
+					zsda_modulo_8(queue->valid + 1,
+					(uint8_t)(queue->cycle_size - 1));
+
+			queue->pushed_wqe++;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static void
+zsda_tx_write_tail(struct zsda_queue *queue)
+{
+	if (queue->pushed_wqe)
+		WRITE_CSR_WQ_TAIL(queue->io_addr, queue->hw_queue_number,
+				  queue->tail);
+
+	queue->pushed_wqe = 0;
+}
+
+uint16_t
+zsda_enqueue_op_burst(struct zsda_qp *qp, void **ops, uint16_t nb_ops)
+{
+	int ret = 0;
+	enum zsda_service_type type;
+	uint16_t i;
+	uint16_t nb_send = 0;
+	void *op;
+
+	if (nb_ops > ZSDA_MAX_DESC) {
+		ZSDA_LOG(ERR, "Enqueue number bigger than %d", ZSDA_MAX_DESC);
+		return 0;
+	}
+
+	for (i = 0; i < nb_ops; i++) {
+		op = ops[i];
+		ret = zsda_enqueue(op, qp);
+		if (ret < 0)
+			break;
+		nb_send++;
+	}
+
+	for (type = 0; type < ZSDA_SERVICE_INVALID; type++)
+		if (qp->srv[type].used)
+			zsda_tx_write_tail(&qp->srv[type].tx_q);
+
+	return nb_send;
+}
+
 int
 zsda_common_setup_qp(uint32_t zsda_dev_id, struct zsda_qp **qp_addr,
 		const uint16_t queue_pair_id, const struct zsda_qp_config *conf)
diff --git a/drivers/common/zsda/zsda_qp.h b/drivers/common/zsda/zsda_qp.h
index a9f0d38ba5..f9efff0e5a 100644
--- a/drivers/common/zsda/zsda_qp.h
+++ b/drivers/common/zsda/zsda_qp.h
@@ -135,6 +135,8 @@ int zsda_get_queue_cfg(struct zsda_pci_device *zsda_pci_dev);
 
 int zsda_queue_pair_release(struct zsda_qp **qp_addr);
 
+uint16_t zsda_enqueue_op_burst(struct zsda_qp *qp, void **ops, const uint16_t nb_ops);
+
 int zsda_common_setup_qp(uint32_t dev_id, struct zsda_qp **qp_addr,
 		    const uint16_t queue_pair_id,
 		    const struct zsda_qp_config *conf);
-- 
2.27.0