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