Changeset 692bf9c for lib/protocols_l2.c


Ignore:
Timestamp:
02/05/19 09:41:34 (2 years ago)
Author:
Jacob Van Walraven <jcv9@…>
Branches:
develop
Children:
db919d5
Parents:
1668208
Message:

Add support to retrieve outermost vlan tag, outermost mpls label, and all layer2 headers

File:
1 edited

Legend:

Unmodified
Added
Removed
  • lib/protocols_l2.c

    rd439067 r692bf9c  
    9090        return (void*)((char *)ethernet + sizeof(*vlanhdr));
    9191
     92}
     93
     94int trace_destroy_layer2_headers(libtrace_layer2_headers_t *headers) {
     95        if (headers == NULL) {
     96                fprintf(stderr, "NULL libtrace_layer2_headers_t passed into "
     97                        "trace_destroy_layer2_headers()\n");
     98                return -1;
     99        }
     100
     101        if (headers->header != NULL) {
     102                free(headers->header);
     103        }
     104        free(headers);
     105        return 1;
     106}
     107libtrace_layer2_headers_t *trace_get_layer2_headers(libtrace_packet_t *packet) {
     108
     109        char *ptr;
     110        libtrace_linktype_t linktype;
     111        uint32_t remaining;
     112        uint16_t ethertype;
     113        libtrace_layer2_headers_t *r;
     114
     115        if (packet == NULL) {
     116                fprintf(stderr, "NULL packet passed into trace_get_layer2_headers()\n");
     117                return NULL;
     118        }
     119        if (packet->trace == NULL) {
     120                fprintf(stderr, "Packet contains a NULL trace in trace_get_layer2_headers()\n");
     121                return NULL;
     122        }
     123
     124        /* jump to layer 2 */
     125        ptr = trace_get_layer2(packet, &linktype, &remaining);
     126        /* packet does not contain layer2 */
     127        if (ptr == NULL) {
     128                return NULL;
     129        }
     130
     131        /* allocate memory for the result */
     132        r = calloc(1, sizeof(libtrace_layer2_headers_t));
     133        if (r == NULL) {
     134                trace_set_err(packet->trace, TRACE_ERR_OUT_OF_MEMORY,
     135                        "Unable to allocate memory in trace_get_layer2_headers()\n");
     136                return NULL;
     137        }
     138        r->header = calloc(1, sizeof(libtrace_layer2_header_t));
     139        if (r->header == NULL) {
     140                trace_set_err(packet->trace, TRACE_ERR_OUT_OF_MEMORY,
     141                        "Unable to allocate memory in trace_get_layer2_headers()\n");
     142                free(r);
     143                return NULL;
     144        }
     145
     146        /* get the first layer2 header */
     147        ptr = trace_get_payload_from_layer2(ptr, linktype, &ethertype, &remaining);
     148
     149        while (remaining != 0 && ptr != NULL) {
     150
     151                if (ethertype == TRACE_ETHERTYPE_LOOPBACK ||
     152                        ethertype == TRACE_ETHERTYPE_IP ||
     153                        ethertype == TRACE_ETHERTYPE_ARP ||
     154                        ethertype == TRACE_ETHERTYPE_RARP ||
     155                        ethertype == TRACE_ETHERTYPE_8021Q ||
     156                        ethertype == TRACE_ETHERTYPE_IPV6 ||
     157                        ethertype == TRACE_ETHERTYPE_8021QS ||
     158                        ethertype == TRACE_ETHERTYPE_MPLS ||
     159                        ethertype == TRACE_ETHERTYPE_MPLS_MC ||
     160                        ethertype == TRACE_ETHERTYPE_PPP_DISC ||
     161                        ethertype == TRACE_ETHERTYPE_PPP_SES) {
     162
     163                        /* Set the bitmask */
     164                        switch (ethertype) {
     165                                case (TRACE_ETHERTYPE_LOOPBACK):
     166                                        r->bitmask |= TRACE_BITMASK_LOOPBACK;
     167                                        break;
     168                                case (TRACE_ETHERTYPE_IP):
     169                                        r->bitmask |= TRACE_BITMASK_IP;
     170                                        break;
     171                                case (TRACE_ETHERTYPE_ARP):
     172                                        r->bitmask |= TRACE_BITMASK_ARP;
     173                                        break;
     174                                case (TRACE_ETHERTYPE_RARP):
     175                                        r->bitmask |= TRACE_BITMASK_RARP;
     176                                        break;
     177                                case (TRACE_ETHERTYPE_8021Q):
     178                                        r->bitmask |= TRACE_BITMASK_8021Q;
     179                                        break;
     180                                case (TRACE_ETHERTYPE_IPV6):
     181                                        r->bitmask |= TRACE_BITMASK_IPV6;
     182                                        break;
     183                                case (TRACE_ETHERTYPE_8021QS):
     184                                        r->bitmask |= TRACE_BITMASK_8021QS;
     185                                        break;
     186                                case (TRACE_ETHERTYPE_MPLS):
     187                                        r->bitmask |= TRACE_BITMASK_MPLS;
     188                                        break;
     189                                case (TRACE_ETHERTYPE_MPLS_MC):
     190                                        r->bitmask |= TRACE_BITMASK_MPLS_MC;
     191                                        break;
     192                                case (TRACE_ETHERTYPE_PPP_DISC):
     193                                        r->bitmask |= TRACE_BITMASK_PPP_DISC;
     194                                        break;
     195                                case (TRACE_ETHERTYPE_PPP_SES):
     196                                        r->bitmask |= TRACE_BITMASK_PPP_SES;
     197                                        break;
     198                        }
     199
     200                        r->header = realloc(r->header,
     201                                sizeof(libtrace_layer2_header_t)*(r->num+1));
     202                        r->header[r->num].ethertype = ethertype;
     203                        r->header[r->num++].data = ptr;
     204                }
     205
     206                /* if the last ethertype was IP stop */
     207                if (ethertype == TRACE_ETHERTYPE_IP || ethertype == TRACE_ETHERTYPE_IPV6) {
     208                        break;
     209                }
     210
     211                /* get the next header */
     212                ptr = trace_get_payload_from_layer2(ptr, linktype, &ethertype, &remaining);
     213        }
     214
     215        /* If no results were found free memory now and just return NULL */
     216        if (r->num) {
     217                free(r->header);
     218                free(r);
     219                return NULL;
     220        }
     221
     222        return r;
     223}
     224
     225uint16_t trace_get_outermost_vlan(libtrace_packet_t *packet, uint8_t **vlanptr,
     226        uint32_t *remaining) {
     227
     228        uint8_t *ptr;
     229        libtrace_linktype_t linktype;
     230        uint32_t rem;
     231        uint16_t vlanid = VLAN_NOT_FOUND;
     232        uint16_t ethertype = 0;
     233
     234        if (!packet) {
     235                fprintf(stderr, "NULL packet passed into trace_get_outermost_vlan()\n");
     236                *vlanptr = NULL;
     237                *remaining = rem;
     238                return vlanid;
     239        }
     240
     241        ptr = trace_get_layer2(packet, &linktype, &rem);
     242        /* No layer 2 */
     243        if (ptr == NULL) {
     244                *vlanptr = NULL;
     245                        *remaining = rem;
     246                return vlanid;
     247        }
     248
     249        while (ethertype != TRACE_ETHERTYPE_8021Q && ethertype != TRACE_ETHERTYPE_8021QS) {
     250
     251                if (rem == 0 || ptr == NULL || ethertype == TRACE_ETHERTYPE_IP ||
     252                        ethertype == TRACE_ETHERTYPE_IPV6) {
     253
     254                        *vlanptr = NULL;
     255                        *remaining = rem;
     256                        return vlanid;
     257                }
     258
     259                /* get the next layer 2 header */
     260                ptr = trace_get_payload_from_layer2(ptr, linktype, &ethertype, &rem);
     261        }
     262
     263        /* found a vlan header */
     264        uint32_t val = ntohl(*(uint32_t *)ptr);
     265        /* the id portion is only 12 bits */
     266        vlanid = (((val >> 16) << 4) >> 4);
     267
     268        *remaining = rem;
     269        *vlanptr = ptr;
     270        return vlanid;
     271}
     272
     273uint32_t trace_get_outermost_mpls(libtrace_packet_t *packet, uint8_t **mplsptr,
     274        uint32_t *remaining) {
     275
     276        uint8_t *ptr;
     277        uint32_t mplslabel = MPLS_NOT_FOUND;
     278        libtrace_linktype_t linktype;
     279        uint32_t rem;
     280        uint16_t ethertype = 0;
     281
     282        if (!packet) {
     283                fprintf(stderr, "NULL packet passed into trace_get_outermost_mpls()\n");
     284                *remaining = 0;
     285                *mplsptr = NULL;
     286                return mplslabel;
     287        }
     288
     289        ptr = trace_get_layer2(packet, &linktype, &rem);
     290        /* No layer2 */
     291        if (ptr == NULL) {
     292                *remaining = rem;
     293                *mplsptr = NULL;
     294                return mplslabel;
     295        }
     296
     297        /* loop over the packet until we find a mpls label */
     298        while (ethertype != TRACE_ETHERTYPE_MPLS) {
     299                if (rem == 0 || ptr == NULL) {
     300
     301                        *remaining = rem;
     302                        *mplsptr = NULL;
     303                        return mplslabel;
     304                }
     305
     306                /* get next layer2 header */
     307                ptr = trace_get_payload_from_layer2(ptr, linktype, &ethertype, &rem);
     308        }
     309
     310        uint32_t val = ntohl(*(uint32_t *)ptr);
     311        mplslabel = val >> 12;
     312
     313        *remaining = rem;
     314        *mplsptr = ptr;
     315        return mplslabel;
    92316}
    93317
Note: See TracChangeset for help on using the changeset viewer.