src/proto_mpls_unicast.c

   1 /*
   2  * netsniff-ng - the packet sniffing beast
   3  * Copyright 2012 Markus Amend <markus@netsniff-ng.org>, Deutsche Flugsicherung GmbH
   4  * Subject to the GPL, version 2.
   5  *
   6  * http://tools.ietf.org/html/rfc3032
   7  */
   8
   9 #include <stdio.h>
  10 #include <stdint.h>
  11 #include <netinet/in.h>    /* for ntohs() */
  12 #include <errno.h>
  13
  14 #include "proto.h"
  15 #include "protos.h"
  16 #include "dissector_eth.h"
  17 #include "built_in.h"
  18 #include "pkt_buff.h"
  19
  20 struct mpls_uchdr {
  21         uint32_t mpls_uc_hdr;
  22 } __packed;
  23
  24 static int mpls_uc_next_proto(struct pkt_buff *pkt)
  25 {
  26         uint8_t proto;
  27         uint16_t key = 0;
  28
  29         if (pkt_len(pkt))
  30                 proto = *(pkt->data);
  31         else
  32                 return -EIO;
  33
  34         /* FIXME: Right now only test for IP Version field */
  35         switch (proto >> 4) {
  36         case 4:
  37                 key = 0x0800; /* IPv4*/
  38                 break;
  39         case 6:
  40                 key = 0x86DD; /* IPv6*/
  41                 break;
  42         default:
  43                 /* Nothing detected ... */
  44                 return -ENOENT;
  45         }
  46
  47         return key;
  48 }
  49
  50 static void mpls_uc_full(struct pkt_buff *pkt)
  51 {
  52         int next;
  53         uint32_t mpls_uc_data;
  54         struct mpls_uchdr *mpls_uc;
  55         uint8_t s = 0;
  56
  57         do {
  58                 mpls_uc = (struct mpls_uchdr *) pkt_pull(pkt, sizeof(*mpls_uc));
  59                 if (mpls_uc == NULL)
  60                         return;
  61
  62                 mpls_uc_data = ntohl(mpls_uc->mpls_uc_hdr);
  63                 s = (mpls_uc_data >> 8) & 0x1;
  64
  65                 tprintf(" [ MPLS ");
  66                 tprintf("Label (%u), ", mpls_uc_data >> 12);
  67                 tprintf("Exp (%u), ", (mpls_uc_data >> 9) & 0x7);
  68                 tprintf("S (%u), ", s);
  69                 tprintf("TTL (%u)", (mpls_uc_data & 0xFF));
  70                 tprintf(" ]\n");
  71         } while (!s);
  72
  73         next = mpls_uc_next_proto(pkt);
  74         if (next < 0)
  75                 return;
  76
  77         pkt_set_proto(pkt, &eth_lay2, (uint16_t) next);
  78 }
  79
  80 static void mpls_uc_less(struct pkt_buff *pkt)
  81 {
  82         int next;
  83         uint32_t mpls_uc_data;
  84         struct mpls_uchdr *mpls_uc;
  85         uint8_t s = 0;
  86
  87         do {
  88                 mpls_uc = (struct mpls_uchdr *) pkt_pull(pkt, sizeof(*mpls_uc));
  89                 if (mpls_uc == NULL)
  90                         return;
  91
  92                 mpls_uc_data = ntohl(mpls_uc->mpls_uc_hdr);
  93                 s = (mpls_uc_data >> 8) & 0x1;
  94
  95                 tprintf(" MPLS/%u", mpls_uc_data >> 12);
  96         } while (!s);
  97
  98         next = mpls_uc_next_proto(pkt);
  99         if (next < 0)
 100                 return;
 101
 102         pkt_set_proto(pkt, &eth_lay2, (uint16_t) next);
 103 }
 104
 105 struct protocol mpls_uc_ops = {
 106         .key = 0x8847,
 107         .print_full = mpls_uc_full,
 108         .print_less = mpls_uc_less,
 109 };
 110
 111 EXPORT_SYMBOL(mpls_uc_ops);