qospktio.c

Vista 核心Rally技术之-LLTD 实现源代码

    g_qprb_hdr  = NULL;    /* pointer to qos probe-header in rxbuf */
    g_qinit_hdr = NULL;    /* pointer to qos init-sink--header in rxbuf */
    g_LinkSpeed = 1000000;
    g_TimeStampFreq = 1000000;	/* usec granularity */
    g_pktio_timestamp = 0;

    /* repeats every 30 seconds - a session is killed after 4 ticks' inactivity */
    gettimeofday(&now, NULL);
    now.tv_sec += 30;
    g_qos_inactivity_timer = event_add(&now, qos_inactivity_timeout, /*state:*/NULL);

#ifdef START_WITH_COUNTER_LEASE
    /* start collection: repeats every second - until the lease runs out */
    gettimeofday(&now, NULL);
    now.tv_sec += 1;
    g_qos_CTA_timer = event_add(&now, interface_counter_recorder, /*state:*/NULL);
    g_samples_remaining = 300;
#endif
}


/**************************  Q O S   M S G   H A N D L E R S   **************************/

static
void
qos_initsink(void)
{
    qos_session_t*	pThisSsn;
    size_t		nbytes;
    int			i;
    uint16_t	errcode;

    if (g_rcvd_pkt_len < sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) +  sizeof(qos_initsink_header_t))
    {
        warn("qos_initsink: frame with truncated InitializeSink header (len=" FMT_SIZET " src="
              ETHERADDR_FMT " dst=" ETHERADDR_FMT "); ignoring\n",
              g_rcvd_pkt_len, ETHERADDR_PRINT(&g_ethernet_hdr->eh_src), ETHERADDR_PRINT(&g_ethernet_hdr->eh_dst));
        return;
    }

    /* Check interrupt moderation request */
    if (g_qinit_hdr->init_intmod_ctrl != 0xFF)
    {
        /* Compose msg to return */
        fmt_base(
            g_txbuf,
            &g_hwaddr,
            &g_base_hdr->tbh_realsrc,
            ToS_QoSDiagnostics,
            (topo_opcode_t)Qopcode_Error,
            g_sequencenum,
            FALSE /*no broadcast*/
            );
        nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);

        /* add Error-Code */
        errcode = htons(Qoserror_ModerationNotAvailable);
        memcpy(&g_txbuf[nbytes], &errcode, sizeof(uint16_t));
        nbytes += sizeof(uint16_t);

        tx_write(g_txbuf, nbytes);

        IF_TRACED(TRC_PACKET)
            dbgprintf("qos_initsink: unsupported interrupt moderation request (intmod=0x%02x)\n", g_qinit_hdr->init_intmod_ctrl);
        END_TRACE

        return;
    }

    /* Check for existing session with this controller and use it, if found; */
    /* If not found (the usual case), get an unused one... */
    if ((pThisSsn = qos_find_session()) == NULL)    
    {
        /* Check for available session slot, reject with ErrBusy if none available */
        pThisSsn = g_QosSessions;
        for (i=0;i<MAX_QOS_SESSIONS;i++)
        {
            if (!pThisSsn->qssn_is_valid)  break;
            pThisSsn++;
        }
        if (i>=MAX_QOS_SESSIONS)
        {
            /* Compose Busy msg to return. */
            fmt_base(g_txbuf, &g_hwaddr, &g_base_hdr->tbh_realsrc, ToS_QoSDiagnostics,
                     Qopcode_Error, g_sequencenum, FALSE /*no broadcast*/);
            nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);

            /* add Error-Code */

            errcode = htons(Qoserror_Busy);
            memcpy(&g_txbuf[nbytes], &errcode, sizeof(uint16_t));
            nbytes += sizeof(uint16_t);

            tx_write(g_txbuf, nbytes);

            IF_TRACED(TRC_PACKET)
                dbgprintf("qos_initsink: tx_error_Busy, seq=%d -> " ETHERADDR_FMT "\n",
                          g_sequencenum, ETHERADDR_PRINT(&g_base_hdr->tbh_realsrc));
            END_TRACE
            return;
        }
    }

    /* Record session data, */
    pThisSsn->qssn_is_valid = TRUE;
    memcpy( &pThisSsn->qssn_ctrlr_real, &g_base_hdr->tbh_realsrc, sizeof(etheraddr_t) );
    pThisSsn->qssn_ticks_til_discard = 4;

    /* and compose & send a Ready msg. */
    fmt_base(g_txbuf, &g_hwaddr, &pThisSsn->qssn_ctrlr_real, ToS_QoSDiagnostics,
             Qopcode_Ready, g_sequencenum, FALSE /*no broadcast*/);
    nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);

    /* add Sink Link Speed */
    g_reorder_buffer = htonl(g_LinkSpeed);
    memcpy(&g_txbuf[nbytes], &g_reorder_buffer, sizeof(uint32_t));
    nbytes += sizeof(uint32_t);

    /* add Performance Counter Frequency */

    cpy_hton64(&g_txbuf[nbytes], &g_TimeStampFreq);
    nbytes += sizeof(uint64_t);

    tx_write(g_txbuf, nbytes);

    IF_TRACED(TRC_PACKET)
	dbgprintf("qos_initsink: tx_ready, seq=%d -> " ETHERADDR_FMT "\n",
		g_sequencenum, ETHERADDR_PRINT(&pThisSsn->qssn_ctrlr_real));
    END_TRACE
}


static
void
qos_reset(void)
{
    qos_session_t*	pThisSsn = qos_find_session();
    size_t		nbytes;

    /* Find associated session, and reject with silence if none found. */
    if (pThisSsn == NULL)
    {
        warn("packetio_recv_handler: no session active for " ETHERADDR_FMT "; ignoring...\n",
             ETHERADDR_PRINT(&g_base_hdr->tbh_realsrc));
        return;
    }

    /* Otherwise, clear the associated session */
    pThisSsn->qssn_is_valid = FALSE;
    pThisSsn->qssn_ticks_til_discard = 0;
    pThisSsn->qssn_num_active_buckets = 0;

    /* and compose & send an ACK msg */

    fmt_base(g_txbuf, &g_hwaddr, &pThisSsn->qssn_ctrlr_real, ToS_QoSDiagnostics,
             Qopcode_ACK, g_sequencenum, FALSE /*no broadcast*/);
    nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);

    tx_write(g_txbuf, nbytes);

    IF_TRACED(TRC_PACKET)
	dbgprintf("qos_reset: tx_ack, seq=%d -> " ETHERADDR_FMT "\n",
		g_sequencenum, ETHERADDR_PRINT(&pThisSsn->qssn_ctrlr_real));
    END_TRACE
}



static
void
qos_probe(void)
{
    qos_session_t*	pThisSsn;

    /* Pick up the rx-timestamp from the driver, put in global timestamp */
    get_timestamp(&g_pktio_timestamp);

    /* Find associated session, and reject with silence if none found. */
    pThisSsn = qos_find_session();

    if (pThisSsn == NULL)
    {
        IF_TRACED(TRC_QOS)
            dbgprintf("qos_probe: no matching session found; ignoring.\n");
        END_TRACE
        return;
    }

    /* Valid session - mark it as "still active" */
    pThisSsn->qssn_ticks_til_discard = 4;

    /* If ProbeGap, just stamp it and reflect it, unless it has an 802.1p field */
    if (g_qprb_hdr->probe_testtype == 1)
    {
        if ((g_qprb_hdr->probe_pqval & 0x80) == 0)
        {
            /* change the test-type to 2 when reflecting */
            g_qprb_hdr->probe_testtype = 2;

            /* setup ethernet and Base-hdr source and dest addresses to return to sender */
            memcpy(&g_ethernet_hdr->eh_dst, &g_base_hdr->tbh_realsrc, sizeof(etheraddr_t));
            memcpy(&g_ethernet_hdr->eh_src, &g_base_hdr->tbh_realdst, sizeof(etheraddr_t));
            memcpy(&g_base_hdr->tbh_realdst, &g_base_hdr->tbh_realsrc, sizeof(etheraddr_t));
            memcpy(&g_base_hdr->tbh_realsrc, &g_ethernet_hdr->eh_src, sizeof(etheraddr_t));

            /* Add the rcv timestamp from the global save area */
            cpy_hton64(&g_qprb_hdr->probe_rxstamp, &g_pktio_timestamp);

            /* Add the rtx timestamp just before sending */
            get_timestamp(&g_pktio_timestamp);
            cpy_hton64(&g_qprb_hdr->probe_rtxstamp, &g_pktio_timestamp);

            /* and return the packet - do not save in re_txbuf! */
            tx_write(g_rxbuf, g_rcvd_pkt_len);

            IF_TRACED(TRC_PACKET)
                dbgprintf("qos_probegap: reflecting, no 802.1p, seq=%d -> " ETHERADDR_FMT "\n",
                          g_sequencenum, ETHERADDR_PRINT(&pThisSsn->qssn_ctrlr_real));
            END_TRACE
        } else {
            /* there is a valid 802.1p field, so the reflected packet must be tagged. */
            qos_ether_header_t     *ethr_hdr;   /* pointer to qos ethernet-header in txbuf */
            qos_base_header_t      *base_hdr;   /* pointer to qos base-header in txbuf */
            qos_probe_header_t     *qprb_hdr;   /* pointer to qos probe-header in txbuf */
            
            ethr_hdr = (qos_ether_header_t*) g_txbuf;
            base_hdr = (qos_base_header_t*) (ethr_hdr+1);
            qprb_hdr = (qos_probe_header_t*)(base_hdr+1);

            /* setup ethernet and base-hdr source and dest addresses to return to sender */
            memcpy(&ethr_hdr->qeh_dst, &g_base_hdr->tbh_realsrc, sizeof(etheraddr_t));
            memcpy(&ethr_hdr->qeh_src, &g_base_hdr->tbh_realdst, sizeof(etheraddr_t));
            memcpy(&base_hdr->qbh_realdst, &g_base_hdr->tbh_realsrc, sizeof(etheraddr_t));
            memcpy(&base_hdr->qbh_realsrc, &g_base_hdr->tbh_realdst, sizeof(etheraddr_t));
            
            /* Set up the 802.1q tag, then insert the .1p value in the highest 7 bits (vlan==0) */
            ethr_hdr->qeh_qtag = 0x0081;
            ethr_hdr->qeh_ptag = htons((g_qprb_hdr->probe_pqval << 9));
            ethr_hdr->qeh_ethertype = g_ethernet_hdr->eh_ethertype;
            
            /* fill out rest of base header */
            base_hdr->qbh_version = g_base_hdr->tbh_version;
            base_hdr->qbh_tos     = g_base_hdr->tbh_tos;
            base_hdr->qbh_resrvd  = g_base_hdr->tbh_resrvd;
            base_hdr->qbh_opcode  = g_base_hdr->tbh_opcode;
            base_hdr->qbh_seqnum  = g_base_hdr->tbh_seqnum;

            /* Fill out the probe-hdr */
            qprb_hdr->probe_txstamp = g_qprb_hdr->probe_txstamp;
            qprb_hdr->probe_testtype = 2;
            qprb_hdr->probe_pktID = g_qprb_hdr->probe_pktID;
            qprb_hdr->probe_pqval = g_qprb_hdr->probe_pqval;

            /* Add the rcv timestamp from the global save area */
            cpy_hton64(&qprb_hdr->probe_rxstamp, &g_pktio_timestamp);

            /* Copy the payload */
            memcpy(&qprb_hdr->probe_payload, &g_qprb_hdr->probe_payload,
                   g_rcvd_pkt_len - (((uint8_t*)&g_qprb_hdr->probe_payload) - g_txbuf));

            /* Add the rtx timestamp just before sending */
            get_timestamp(&g_pktio_timestamp);
            cpy_hton64(&qprb_hdr->probe_rtxstamp, &g_pktio_timestamp);

            /* and return the packet (4 bytes longer due to tags) - do not save in re_txbuf! */
            tx_write(g_txbuf, g_rcvd_pkt_len+4);

            IF_TRACED(TRC_PACKET)
                dbgprintf("qos_probegap: reflecting, with 802.1p priority of: %d, seq=%d -> " ETHERADDR_FMT "\n",
                          (g_qprb_hdr->probe_pqval & 0x7f), g_sequencenum,
                           ETHERADDR_PRINT(&pThisSsn->qssn_ctrlr_real));
            END_TRACE
        }
    } else if (g_qprb_hdr->probe_testtype == 0) { /* timed probe */
        qosEventDescr_t* evt;
        qosEventBucket_t* bucket = qos_find_bucket(pThisSsn);

        do
        {
            if (bucket)
            {
                // Make sure we don't try to store more than fits in the bucket
                if (bucket->evt_numEvts >= MAX_QOS_EVENTS_PER_BUCKET)
                {
                    break;
                }
            }
            else
            {
                /* There is no existing bucket to dump the event into, so find a new one */
                if (pThisSsn->qssn_num_active_buckets >= MAX_QOS_BUCKETS)
                {
                    /* Reuse the oldest bucket */
                    bucket = &pThisSsn->qssn_evt_buckets[pThisSsn->qssn_first_bucket];
                    pThisSsn->qssn_first_bucket = (pThisSsn->qssn_first_bucket + 1) & (MAX_QOS_BUCKETS - 1);
                }
                else
                {
                    /* Use the next available bucket */
                    bucket = &pThisSsn->qssn_evt_buckets[(pThisSsn->qssn_first_bucket + pThisSsn->qssn_num_active_buckets) & (MAX_QOS_BUCKETS - 1)];
                    pThisSsn->qssn_num_active_buckets++;