original files of m241

components/ip_net2-6.9/ipnet2/src/ipnet_pkt_queue_mbc.c

@@ -0,0 +1,607 @@
+/*
+ ******************************************************************************
+ *                     INTERPEAK SOURCE FILE
+ *
+ *   Document no: @(#) $Name: VXWORKS_ITER40_2019102207 $ $RCSfile: ipnet_pkt_queue_mbc.c,v $ $Revision: 1.15 $
+ *   $Source: /home/interpeak/CVSRoot/ipnet2/src/ipnet_pkt_queue_mbc.c,v $
+ *   $Author: kenneth $
+ *   $State: Exp $ $Locker:  $
+ *
+ *   INTERPEAK_COPYRIGHT_STRING
+ *   Design and implementation by Kenneth Jonsson <kenneth@interpeak.se>
+ ******************************************************************************
+ */
+
+
+/*
+ ****************************************************************************
+ * 1                    DESCRIPTION
+ ****************************************************************************
+ */
+/* Multiband Container queue.
+   Traffic is ordered in 2 or more band, where the priority decreses as the
+   band index increases.
+ */
+
+/*
+ ****************************************************************************
+ * 2                    CONFIGURATION
+ ****************************************************************************
+ */
+
+#ifndef IPCOM_KERNEL
+#define IPCOM_KERNEL
+#endif
+#include "ipnet_config.h"
+
+
+/*
+ ****************************************************************************
+ * 3                    INCLUDE FILES
+ ****************************************************************************
+ */
+
+#ifdef IPNET_USE_IFO_MBC
+#ifndef IPNET_USE_IFO_FIFO
+#error "MultiBand Container requires the FIFO queue"
+#endif
+
+#define IPCOM_USE_CLIB_PROTO
+#include <ipcom_clib.h>
+#include <ipcom_syslog.h>
+
+#include "ipnet_h.h"
+#include "ipnet_netif.h"
+#include "ipnet_pkt_queue.h"
+
+/*
+ ****************************************************************************
+ * 4                    DEFINES
+ ****************************************************************************
+ */
+
+/*
+ ****************************************************************************
+ * 5                    TYPES
+ ****************************************************************************
+ */
+
+typedef struct Ipnet_pkt_queue_mbc_struct
+{
+    Ipnet_pkt_queue   hdr;
+    Ip_u32            number_of_bands;
+    Ip_u32            default_band;
+    Ipnet_pkt_queue **bands;
+    Ipnet_classifier *classifier;
+}
+Ipnet_pkt_queue_mbc;
+
+
+/*
+ ****************************************************************************
+ * 6                    EXTERN PROTOTYPES
+ ****************************************************************************
+ */
+
+/*
+ ****************************************************************************
+ * 7                    LOCAL PROTOTYPES
+ ****************************************************************************
+ */
+
+/*
+ ****************************************************************************
+ * 8                    DATA
+ ****************************************************************************
+ */
+
+/*
+ ****************************************************************************
+ * 9                    STATIC FUNCTIONS
+ ****************************************************************************
+ */
+
+
+/*
+ *===========================================================================
+ *                  ipnet_pkt_queue_mbc_insert_default_queue
+ *===========================================================================
+ * Description: Inserts a FIFO queue in the MBC.
+ * Parameters:  pq - Pointer to where the new FIFO should be stored.
+ *              netif - The interface the queue belongs to.
+ * Returns:     0  = success
+ *              <0 = error code
+ *
+ */
+IP_STATIC int
+ipnet_pkt_queue_mbc_insert_default_queue(Ipnet_pkt_queue_mbc *q, int band)
+{
+    struct Ipnet_ifqueue fifo;
+
+    ipcom_strcpy(fifo.ifq_name, q->hdr.netif->ipcom.name);
+    ipcom_strcpy(fifo.ifq_type, "fifo");
+    if (q->bands[band] == IP_NULL)
+        fifo.ifq_id = IP_IFQ_ID_NONE;
+    else
+        fifo.ifq_id = q->bands[band]->id;
+    fifo.ifq_parent_id            = q->hdr.id;
+    fifo.ifq_data.fifo.fifo_limit = IPNET_PKT_QUEUE_DEFAULT_FIFO_SIZE;
+    return ipnet_pkt_queue_new(&fifo, q->hdr.netif, &q->bands[band]);
+}
+
+
+/*
+ *===========================================================================
+ *                      ipnet_pkt_queue_mbc_enqueue
+ *===========================================================================
+ * Description: Inserts a packet into the queue.
+ * Parameters:  q - A MBC based queue.
+ *              pkt - The packet to store.
+ * Returns:     0  = success
+ *              <0 = error code
+ *
+ */
+IP_STATIC int
+ipnet_pkt_queue_mbc_enqueue(Ipnet_pkt_queue_mbc *q, Ipcom_pkt *pkt)
+{
+    int              band;
+    Ipnet_pkt_queue *child_q;
+
+    band = ipnet_classifier_pkt_to_tag(q->classifier, pkt);
+    if (band < 0)
+        child_q = q->bands[q->default_band];
+    else
+        child_q = q->bands[band];
+
+    return child_q->enqueue(child_q, pkt);
+}
+
+
+/*
+ *===========================================================================
+ *                      ipnet_pkt_queue_mbc_dequeue
+ *===========================================================================
+ * Description: Removed a packet from the queue.
+ * Parameters:  q - A MBC based queue.
+ * Returns:     A packet from the queue or IP_NULL if all bands of the queue
+ *              are empty.
+ *
+ */
+IP_STATIC Ipcom_pkt *
+ipnet_pkt_queue_mbc_dequeue(Ipnet_pkt_queue_mbc *q)
+{
+    Ipcom_pkt *pkt;
+    Ip_u32     band;
+
+    for (band = 0; band < q->number_of_bands; band++)
+    {
+        if (q->bands[band] == IP_NULL)
+            continue;
+
+        pkt = q->bands[band]->dequeue(q->bands[band]);
+        if (pkt)
+            return pkt;
+    }
+    return IP_NULL;
+}
+
+
+/*
+ *===========================================================================
+ *                      ipnet_pkt_queue_mbc_requeue
+ *===========================================================================
+ * Description: Puts back a packet so it will be the first to dequeue next
+ *              time.
+ * Parameters:  q - A MBC packet queue.
+ *              pkt - The packet to requeue.
+ * Returns:
+ *
+ */
+IP_STATIC void
+ipnet_pkt_queue_mbc_requeue(Ipnet_pkt_queue_mbc *q, Ipcom_pkt *pkt)
+{
+    int              band;
+    Ipnet_pkt_queue *child_q;
+
+    band = ipnet_classifier_pkt_to_tag(q->classifier, pkt);
+    if (band < 0)
+        child_q = q->bands[q->default_band];
+    else
+        child_q = q->bands[band];
+
+    child_q->requeue(child_q, pkt);
+}
+
+
+/*
+ *===========================================================================
+ *                      ipnet_pkt_queue_mbc_count
+ *===========================================================================
+ * Description: Returns the number of packets that is currently queued.
+ * Parameters:  q - A MBC based queue.
+ * Returns:
+ *
+ */
+IP_STATIC int
+ipnet_pkt_queue_mbc_count(Ipnet_pkt_queue_mbc *q)
+{
+    Ip_u32     band;
+    int        count = 0;
+
+    for (band = 0; band < q->number_of_bands; band++)
+        count += q->bands[band]->count(q->bands[band]);
+    return count;
+}
+
+
+/*
+ *===========================================================================
+ *                      ipnet_pkt_queue_mbc_reset
+ *===========================================================================
+ * Description: Resets the queue, all packets is discarded.
+ * Parameters:  q - A MBC based queue.
+ * Returns:
+ *
+ */
+IP_STATIC void
+ipnet_pkt_queue_mbc_reset(Ipnet_pkt_queue_mbc *q)
+{
+    Ip_u32     band;
+
+    for (band = 0; band < q->number_of_bands; band++)
+        q->bands[band]->reset(q->bands[band]);
+}
+
+
+/*
+ *===========================================================================
+ *                      ipnet_pkt_queue_mbc_dump
+ *===========================================================================
+ * Description: Dumps the configuration to the supplied buffer.
+ * Parameters:  q - A packet queue.
+ *              m - The buffer where the data should be dumped.
+ * Returns:
+ *
+ */
+IP_STATIC void
+ipnet_pkt_queue_mbc_dump(Ipnet_pkt_queue_mbc *q, struct Ipnet_ifqueue_mbc *m)
+{
+    Ip_u32     band;
+
+    m->mbc_bands        = q->number_of_bands;
+    m->mbc_default_band = q->default_band;
+
+    for (band = 0; band < q->number_of_bands; band++)
+        *(int *)&m->mbc_container.child_ids[band] = q->bands[band]->id;
+
+    *(int *)&m->mbc_container.child_count = (int) q->number_of_bands;
+}
+
+
+/*
+ *===========================================================================
+ *                      ipnet_pkt_queue_mbc_configure
+ *===========================================================================
+ * Description: Configures a queue with new parameters.
+ * Parameters:  q - A packet queue.
+ *              m - The queue parameters.
+ * Returns:     0  = success
+ *              <0 = error code
+ */
+IP_STATIC int
+ipnet_pkt_queue_mbc_configure(Ipnet_pkt_queue_mbc *q, struct Ipnet_ifqueue_mbc *m)
+{
+    Ip_u32 band;
+
+    if (q->number_of_bands)
+    {
+        IPCOM_LOG0(ERR, "MBC configure: Can only be configured once");
+        return -IP_ERRNO_EINVAL;
+    }
+
+    if (m->mbc_bands < 2 || m->mbc_bands > IPNET_IFQ_CONTAINER_MAX_COUNT)
+    {
+        IPCOM_LOG1(ERR,
+                   "MBC configure: Number of priority bands must be between 2 and %d",
+                   IPNET_IFQ_CONTAINER_MAX_COUNT);
+        return -IP_ERRNO_EINVAL;
+    }
+
+    if (m->mbc_bands <= m->mbc_default_band)
+    {
+        IPCOM_LOG1(ERR, "MBC configure: invalid default band, must be [0..%d]",
+                   m->mbc_bands - 1);
+        return -IP_ERRNO_EINVAL;
+    }
+
+    q->bands = ipcom_calloc(m->mbc_bands, sizeof(Ipnet_pkt_queue *));
+    if (q->bands == IP_NULL)
+    {
+        IPCOM_LOG0(CRIT, "MBC configure: out of memory");
+        return -IP_ERRNO_ENOMEM;
+    }
+
+    q->number_of_bands = m->mbc_bands;
+    q->default_band    = m->mbc_default_band;
+
+    for (band = 0; band < q->number_of_bands; band++)
+    {
+        int                    ret;
+
+        ret = ipnet_pkt_queue_mbc_insert_default_queue(q, band);
+        if (ret < 0)
+        {
+            IPCOM_LOG1(ERR, "MBC configure: Failed to create default queue: %s",
+                       ipcom_strerror(-ret));
+            return ret;
+        }
+    }
+
+    return 0;
+}
+
+
+/*
+ *===========================================================================
+ *                      ipnet_pkt_queue_mbc_init
+ *===========================================================================
+ * Description: Configures a queue with new parameters.
+ * Parameters:  q - A packet queue.
+ * Returns:
+ *
+ */
+IP_STATIC int
+ipnet_pkt_queue_mbc_init(Ipnet_pkt_queue_mbc *q)
+{
+    ipcom_memset((Ip_u8 *) q + sizeof(Ipnet_pkt_queue),
+                 0,
+                 sizeof(Ipnet_pkt_queue_mbc) - sizeof(Ipnet_pkt_queue));
+
+    q->classifier = ipnet_classifier_new();
+    if (q->classifier == IP_NULL)
+    {
+        IPCOM_LOG0(ERR, "MBC init: Failed to create classifier: out of memory");
+        return -IP_ERRNO_ENOMEM;
+    }
+
+    return 0;
+}
+
+
+/*
+ *===========================================================================
+ *                      ipnet_pkt_queue_mbc_destroy
+ *===========================================================================
+ * Description: Frees all contained queues.
+ * Parameters:  q - A MBC based queue.
+ * Returns:
+ *
+ */
+IP_STATIC void
+ipnet_pkt_queue_mbc_destroy(Ipnet_pkt_queue_mbc *q)
+{
+    Ip_u32     band;
+
+    for (band = 0; band < q->number_of_bands; band++)
+        ipnet_pkt_queue_delete(q->bands[band]);
+    ipcom_free(q->bands);
+
+    if (q->classifier)
+        ipnet_classifier_delete(q->classifier);
+}
+
+
+/*
+ *===========================================================================
+ *                  ipnet_pkt_queue_mbc_get
+ *===========================================================================
+ * Description: Returns the queue that matches the specified ID.
+ * Parameters:  q - A MBC packet queue.
+ *              id - The ID of the contained queue.
+ * Returns:     A pointer to the queue or IP_NULL if it was not found.
+ *
+ */
+IP_STATIC struct Ipnet_pkt_queue_struct *
+ipnet_pkt_queue_mbc_get(Ipnet_pkt_queue_mbc *q, int id)
+{
+    Ip_u32           band;
+    Ipnet_pkt_queue *c;
+
+    if (q->hdr.id == id)
+        return &q->hdr;
+
+    for (band = 0; band < q->number_of_bands; band++)
+    {
+        c = q->bands[band];
+        if (c)
+        {
+            if (c->id == id)
+                return c;
+            if (c->c_ops && IP_NULL != (c = c->c_ops->q_get(c, id)))
+                return c;
+        }
+    }
+    IPCOM_LOG3(DEBUG2, "MBC: Did not find %d queue inside %s:%d",
+               id, q->hdr.type, q->hdr.id);
+    return IP_NULL;
+}
+
+
+/*
+ *===========================================================================
+ *                  ipnet_pkt_queue_mbc_remove
+ *===========================================================================
+ * Description: Removes an queue from a container queue and put pack a
+ *              default queue type.
+ * Parameters:  c - The MBC parent queue for 'q'.
+ *              q - The queue to remove.
+ * Returns:     0 = success, <0 = error code (-IP_ERRNO_xxx)
+ *
+ */
+IP_STATIC int
+ipnet_pkt_queue_mbc_remove(Ipnet_pkt_queue_mbc *c, Ipnet_pkt_queue *q)
+{
+    Ip_u32           band;
+
+    ip_assert(c->hdr.id == q->parent_id);
+    for (band = 0; band < c->number_of_bands; band++)
+    {
+        if (q == c->bands[band])
+        {
+            int ret = ipnet_pkt_queue_mbc_insert_default_queue(c, band);
+            if (ret < 0)
+                IPCOM_LOG3(WARNING, "MBC: Failed to replace %s:%d with a fifo in remove(): %s",
+                           q->type, q->id, ipcom_strerror(-ret));
+            return ret;
+        }
+    }
+
+    return -IP_ERRNO_ESRCH;
+}
+
+
+/*
+ *===========================================================================
+ *                  ipnet_pkt_queue_mbc_insert
+ *===========================================================================
+ * Description: Inserts an queue to a container queue.
+ * Parameters:  c - The parent queue for 'q', must be container capable
+ *              q - The queue to insert.
+ * Returns:     0 = success, <0 = error code (-IP_ERRNO_xxx)
+ *
+ */
+IP_STATIC int
+ipnet_pkt_queue_mbc_insert(Ipnet_pkt_queue_mbc *c, Ipnet_pkt_queue *q)
+{
+    Ip_u32           band;
+
+    for (band = 0; band < c->number_of_bands; band++)
+    {
+        if (c->bands[band] == IP_NULL)
+        {
+            /* Is adding the initial queues */
+            c->bands[band] = q;
+            return 0;
+        }
+
+        if (q->id == c->bands[band]->id)
+        {
+            /* Replace this queue */
+            ipnet_pkt_queue_delete(c->bands[band]);
+            c->bands[band] = q;
+            return 0;
+        }
+    }
+    IPCOM_LOG1(ERR, "MBC: No band has queue id %d", q->id);
+    return -IP_ERRNO_ESRCH;
+}
+
+
+/*
+ *===========================================================================
+ *                  ipnet_pkt_queue_htbc_f_insert
+ *===========================================================================
+ * Description: Inserts a new filter into a MBC queue.
+ * Parameters:  c - A HTBC queue.
+ *              filter_id - The ID the filter will have.
+ *              rule - The rule to match this filter.
+ *              child_queue_id - The queue ID of the child filter that packets
+ *              that matches the rule will be queued at.
+ * Returns:     0 = success, <0 = error code (-IP_ERRNO_xxx)
+ *
+ */
+IP_STATIC int
+ipnet_pkt_queue_mbc_f_insert(Ipnet_pkt_queue_mbc *c,
+                             int filter_id,
+                             struct Ipnet_classifier_rule *rule,
+                             int child_queue_id)
+{
+    int    ret;
+    Ip_u32 band;
+
+    for (band = 0; band < c->number_of_bands; band++)
+        if (c->bands[band]->id == child_queue_id)
+            break;
+
+    if (0 > (ret = ipnet_classifier_add_mapping(c->classifier, rule, filter_id)))
+        return ret;
+    return ipnet_classifier_set_tag(c->classifier, filter_id, band);
+}
+
+
+/*
+ *===========================================================================
+ *                  ipnet_pkt_queue_mbc_f_remove
+ *===========================================================================
+ * Description: Removes a filter from a MBC queue.
+ * Parameters:  mbc - A MBC queue.
+ *              queue_id - The
+ * Returns:     0 = success, <0 = error code (-IP_ERRNO_xxx)
+ *
+ */
+IP_STATIC int
+ipnet_pkt_queue_mbc_f_remove(Ipnet_pkt_queue_mbc *c, int filter_id)
+{
+    return ipnet_classifier_del_mapping(c->classifier, filter_id);
+}
+
+
+/*
+ ****************************************************************************
+ * 10                   GLOBAL FUNCTIONS
+ ****************************************************************************
+ */
+
+/*
+ ****************************************************************************
+ * 11                   PUBLIC FUNCTIONS
+ ****************************************************************************
+ */
+
+/*
+ *===========================================================================
+ *                      ipnet_pkt_queue_htbc_template
+ *===========================================================================
+ * Description: Returns a template for Hierarchical Token Bucket Filter queue.
+ * Parameters:
+ * Returns:
+ *
+ */
+IP_GLOBAL Ipnet_pkt_queue *
+ipnet_pkt_queue_mbc_template()
+{
+    static Ipnet_pkt_queue_container c_ops = {
+        (Ipnet_pkt_queue_container_get_queue)    ipnet_pkt_queue_mbc_get,
+        (Ipnet_pkt_queue_container_insert_queue) ipnet_pkt_queue_mbc_insert,
+        (Ipnet_pkt_queue_container_remove_queue) ipnet_pkt_queue_mbc_remove,
+        (Ipnet_pkt_queue_container_insert_filter) ipnet_pkt_queue_mbc_f_insert,
+        (Ipnet_pkt_queue_container_remove_filter) ipnet_pkt_queue_mbc_f_remove,
+    };
+
+    static Ipnet_pkt_queue mbc_template = {
+        "mbc",
+        sizeof(Ipnet_pkt_queue_mbc),
+        &c_ops,
+        (Ipnet_pkt_queue_enqueue)   ipnet_pkt_queue_mbc_enqueue,
+        (Ipnet_pkt_queue_dequeue)   ipnet_pkt_queue_mbc_dequeue,
+        (Ipnet_pkt_queue_requeue)   ipnet_pkt_queue_mbc_requeue,
+        (Ipnet_pkt_queue_count)     ipnet_pkt_queue_mbc_count,
+        (Ipnet_pkt_queue_reset)     ipnet_pkt_queue_mbc_reset,
+        (Ipnet_pkt_queue_dump)      ipnet_pkt_queue_mbc_dump,
+        (Ipnet_pkt_queue_configure) ipnet_pkt_queue_mbc_configure,
+        (Ipnet_pkt_queue_init)      ipnet_pkt_queue_mbc_init,
+        (Ipnet_pkt_queue_destroy)   ipnet_pkt_queue_mbc_destroy,
+    };
+    return &mbc_template;
+}
+
+#else
+int ipnet_pkt_q_container_mbc_empty_file;
+#endif /* IPNET_USE_IFO_MBC */
+
+/*
+ ****************************************************************************
+ *                      END OF FILE
+ ****************************************************************************
+ */
+