kernel/linux/net/bridge/netfilter/ebt_wmm_mark.c

   1 /*
   2  *  ebt_wmm_mark
   3  *
   4  */
   5 #include <linux/netfilter_bridge/ebtables.h>
   6 #include <linux/netfilter_bridge/ebt_wmm_mark_t.h>
   7 #include <linux/if_vlan.h>
   8 #include <linux/module.h>
   9 #include <linux/ip.h>
  10 #include <linux/skbuff.h>
  11
  12 static int ebt_target_wmm_mark(struct sk_buff **pskb, unsigned int hooknr,
  13    const struct net_device *in, const struct net_device *out,
  14    const void *data, unsigned int datalen)
  15 {
  16         struct ebt_wmm_mark_t_info *info = (struct ebt_wmm_mark_t_info *)data;
  17
  18         struct iphdr *iph;
  19         struct vlan_hdr *frame;
  20         unsigned char prio = 0;
  21         unsigned short TCI;
  22
  23         if(info->markset != WMM_MARK_VALUE_NONE) {
  24                 /* use marset regardless of supported classification method */
  25                 prio = (unsigned char)info->markset;
  26
  27         } else if (info->mark & WMM_MARK_8021D) {
  28                 if ((*pskb)->protocol == __constant_htons(ETH_P_8021Q)) {
  29                         frame = (struct vlan_hdr *)((*pskb)->nh.raw);
  30                         TCI = ntohs(frame->h_vlan_TCI);
  31                         prio = (unsigned char)((TCI >> 13) & 0x7);
  32                 } else
  33                         return EBT_CONTINUE;
  34
  35         } else if (info->mark & WMM_MARK_DSCP) {
  36
  37                 /* if VLAN frame, we need to point to correct network header */
  38                 if ((*pskb)->protocol == __constant_htons(ETH_P_8021Q))
  39                         iph = (struct iphdr *)((*pskb)->nh.raw + VLAN_HLEN);
  40                 /* ip */
  41                 else if ((*pskb)->protocol == __constant_htons(ETH_P_IP))
  42                         iph = (*pskb)->nh.iph;
  43                 else
  44                 /* pass for others */
  45                         return EBT_CONTINUE;
  46
  47                 prio = iph->tos>>WMM_DSCP_MASK_SHIFT ;
  48         }
  49
  50         if(prio) {
  51                 (*pskb)->nfmark &= ~(PRIO_LOC_NFMASK << info->markpos);
  52                 (*pskb)->nfmark |= (prio << info->markpos);
  53                 (*pskb)->nfcache |= NFC_ALTERED;
  54                 //printk("mark 0x%x\n",( prio << info->markpos));
  55         }
  56
  57         return info->target;
  58 }
  59
  60 static int ebt_target_wmm_mark_check(const char *tablename, unsigned int hookmask,
  61    const struct ebt_entry *e, void *data, unsigned int datalen)
  62 {
  63         struct ebt_wmm_mark_t_info *info = (struct ebt_wmm_mark_t_info *)data;
  64
  65         if (datalen != EBT_ALIGN(sizeof(struct ebt_wmm_mark_t_info)))
  66                 return -EINVAL;
  67
  68         //printk("e->ethproto=0x%x, e->invflags=0x%x\n",e->ethproto, e->invflags);
  69
  70         if ((e->ethproto != __constant_htons(ETH_P_IP) && e->ethproto != __constant_htons(ETH_P_8021Q)) ||
  71            e->invflags & EBT_IPROTO)
  72                 return -EINVAL;
  73
  74         if (BASE_CHAIN && info->target == EBT_RETURN)
  75                 return -EINVAL;
  76
  77         CLEAR_BASE_CHAIN_BIT;
  78         if (INVALID_TARGET)
  79                 return -EINVAL;
  80         return 0;
  81
  82 }
  83
  84 static struct ebt_target mark_target =
  85 {
  86         .name           = EBT_WMM_MARK_TARGET,
  87         .target         = ebt_target_wmm_mark,
  88         .check          = ebt_target_wmm_mark_check,
  89         .me             = THIS_MODULE,
  90 };
  91
  92 static int __init init(void)
  93 {
  94         return ebt_register_target(&mark_target);
  95 }
  96
  97 static void __exit fini(void)
  98 {
  99         ebt_unregister_target(&mark_target);
 100 }
 101
 102 module_init(init);
 103 module_exit(fini);
 104 MODULE_LICENSE("GPL");