Ignore:
File:
1 edited

Legend:

Unmodified
Added
Removed
  • lib/protocols_l2.c

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