lldp_min.c

Go to the documentation of this file.

#include <stdio.h>
#include <string.h>
#include <lldp.h>
#include <tlvhelper.h>
#define DIMOF(a)        (sizeof(a)/sizeof(a[0]))

#define         NETTLV_INITPKTOFFSET    14
#define         NETTLV_HDRSZ    2




guint8
get_lldptlv_type(const void* tlv_vp,  
                 const void* pktend)  
{
        return (guint8)((tlv_get_guint8(tlv_vp, pktend)>>1) & 0x7F);
}

gsize
get_lldptlv_len(const void* tlv_vp,     
                const void* pktend)     
{
        const unsigned char * tlvp = tlv_vp;
        guint8 byte0 = tlv_get_guint8(tlvp, pktend);
        guint8 byte1 = tlv_get_guint8(tlvp+1, pktend);

        return ((((gsize)(byte0&0x1)) << 8) & (gsize)0x1FF)
               |        ((gsize)byte1) ;
}

const void *
get_lldptlv_body(const void * tlv_vp,   
                 const void* pktend)    
{
        (void)pktend;
        return ((const guint8*)tlv_vp + NETTLV_HDRSZ);
}

gboolean
is_valid_lldp_packet(const void* tlv_vp,        //<[in] pointer to beginning pf LLDP packet
                     const void* pktend)        //<[in] pointer to first byte past the end of the packet
{
        const unsigned  reqtypes [] = {LLDP_TLV_CHID,LLDP_TLV_PID, LLDP_TLV_TTL};
        unsigned        j = 0;
#ifdef PEDANTIC_LLDP_NERD
        int             lasttype = -1;
#endif
        if (NULL == tlv_vp || ((const guint8*)tlv_vp+NETTLV_HDRSZ)  > (const guint8*)pktend) {
                g_warning("LLDP Invalid because packet is too short\n");
                return FALSE;
        }
        for (tlv_vp = get_lldptlv_first(tlv_vp, pktend)
        ;       NULL != tlv_vp
        ;       tlv_vp = get_lldptlv_next(tlv_vp, pktend)) {
                unsigned        ttype;
                unsigned        length;
                const guint8*   next;

                if (tlv_vp >= pktend) {
                        return FALSE;
                }
                ttype = get_lldptlv_type(tlv_vp, pktend);
#ifdef PEDANTIC_LLDP_NERD
                lasttype = ttype;
#endif
                length = get_lldptlv_len(tlv_vp, pktend);
                next = (const guint8*)tlv_vp + (length+NETTLV_HDRSZ);
                if (next > (const guint8*)pktend) {
                        g_warning("LLDP Invalid because TLV entry extends past end\n");
                        return FALSE;
                }
                if (ttype == LLDP_TLV_END) {
                        if (get_lldptlv_body(tlv_vp, pktend) == pktend) {
                                return length == 0;
                        }else{
                                g_warning("LLDP Invalid because END item isn't at end of packet\n");
                                return FALSE;
                        }
                }
                if (j < DIMOF(reqtypes) && ttype != reqtypes[j]) {
                        g_warning("LLDP Invalid because required TLV type [%d] isn't present in right position (%d)\n"
                        ,       reqtypes[j], j);
                        return FALSE;
                }
                j += 1;
        }
#ifdef PEDANTIC_LLDP_NERD
        if (lasttype != LLDP_TLV_END) {
                g_warning("LLDP Invalid because final type wasn't LLDP_TLV_END (it was %d)\n"
                ,       lasttype);
                return FALSE;
        }
#endif
        return TRUE;
}

const void *
get_lldptlv_first(const void* packet,   
                  const void* pktend)   
{
        const guint8*   inittlv = (const guint8*)packet + NETTLV_INITPKTOFFSET;
        if (packet == NULL
        ||      (inittlv + NETTLV_HDRSZ) > (const guint8*)pktend
        ||      (inittlv + NETTLV_HDRSZ + get_lldptlv_len(inittlv, pktend)) > (const guint8*)pktend) {
                return NULL;
        }
        return inittlv;
}

const void *
get_lldptlv_next(const void* tlv_vp,            
                  const void* pktend)   
{
        const guint8 *  nexttlv;
        const void*     nextend;
        if (tlv_vp == NULL
        ||  ((const guint8*)tlv_vp+NETTLV_HDRSZ) > (const guint8*)pktend
        ||  get_lldptlv_type(tlv_vp, pktend) == LLDP_TLV_END) {
                return NULL;
        }
        nexttlv = (const guint8*)tlv_vp  + NETTLV_HDRSZ + get_lldptlv_len(tlv_vp, pktend);
        /* Watch out for malformed packets! (BLACKHAT, PARANOIA) */
        nextend = nexttlv + NETTLV_HDRSZ + get_lldptlv_len(nexttlv, pktend);
        /* fprintf(stderr, "LOOK: cur:%p, next: %p, nextend: %p, vpend: %p\n"
        ,       tlv_vp, nexttlv, nextend, pktend); */
        return nextend > pktend ? NULL : nexttlv;
}

const void *
find_next_lldptlv_type(const void* tlv_vp,      
                       unsigned tlvtype,        
                       const void* pktend)      
{
        while (NULL != tlv_vp && ((const guint8*)tlv_vp+NETTLV_HDRSZ) <= (const guint8*)pktend) {
                if (get_lldptlv_type(tlv_vp, pktend) == tlvtype) {
                        return tlv_vp;
                }
                tlv_vp = get_lldptlv_next(tlv_vp, pktend);
        }
        return NULL;
}

const void *
get_lldp_chassis_id(gconstpointer       tlv_vp,         
                    gssize*             idlength,       
                    gconstpointer       pktend)         
{
        const void *    tlventry;
        tlv_vp = get_lldptlv_first(tlv_vp, pktend);
        tlventry = find_next_lldptlv_type(tlv_vp, LLDP_TLV_CHID, pktend);
        if (tlventry == NULL) {
                return NULL;
        }
        *idlength = get_lldptlv_len(tlv_vp, pktend);
        return get_lldptlv_body(tlv_vp, pktend);
}

const void *
get_lldp_port_id(gconstpointer tlv_vp,  
                 gssize* idlength,      
                 gconstpointer pktend)  
{
        const void *    tlventry;
        tlv_vp = get_lldptlv_first(tlv_vp, pktend);
        tlventry = find_next_lldptlv_type(tlv_vp, LLDP_TLV_PID, pktend);
        if (tlventry == NULL) {
                return NULL;
        }
        *idlength = get_lldptlv_len(tlv_vp, pktend);
        return get_lldptlv_body(tlv_vp, pktend);
}