staging/mops_dot1Q.c

   1 /*
   2  * Mausezahn - A fast versatile traffic generator
   3  * Copyright (C) 2008-2010 Herbert Haas
   4  *
   5  * This program is free software; you can redistribute it and/or modify it under
   6  * the terms of the GNU General Public License version 2 as published by the
   7  * Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  11  * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
  12  * details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, see http://www.gnu.org/licenses/gpl-2.0.html
  16  *
  17 */
  18
  19 #include "mz.h"
  20 #include "mops.h"
  21
  22
  23 // Remove 802.1Q tags from packet mp
  24 //
  25 // k indicates which tag to be removed (1..n)
  26 // k=0 means: remove all tags!
  27 //
  28 // RETURN VALUE: 1 upon failure, 0 upon success
  29 int mops_dot1Q_remove (struct mops *mp, int k)
  30 {
  31         int a,b,n;
  32
  33         if (k==0) {
  34                 mp->dot1Q_s=0;
  35                 mp->use_dot1Q=0;
  36                 return 0;
  37         }
  38
  39         n = mp->dot1Q_s/4; // n = total number of tags
  40         if (k>n) return 1;
  41
  42         if (k==1) { // only delete the single tag
  43                 mp->dot1Q_s=0;
  44                 mp->use_dot1Q=0;
  45                 return 0;
  46         }
  47
  48         // we have more than one tag:
  49         //
  50         if (k==n) { // remove last tag (of several)
  51                 mp->dot1Q_s -=4;
  52                 return 0;
  53         }
  54
  55         // remove some non-ending tag: 0, 1, 2, 3
  56         a = (k-1)*4; // target
  57         b = k*4;     // source (what should be copied)
  58         memcpy(&mp->dot1Q[a], &mp->dot1Q[b], (n-k)*4);
  59         mp->dot1Q_s -=4;
  60
  61         return 0;
  62 }
  63
  64
  65 // Unset CFI in tag k where k=1..n
  66 int mops_dot1Q_nocfi (struct mops *mp, int k)
  67 {
  68         int n;
  69
  70         n = mp->dot1Q_s/4; // n = total number of tags
  71         if (k>n) return 1;
  72
  73         mp->dot1Q[((k-1)*4)+2] &=0xef; // unset CFI (0xef = 1110 1111)
  74         return 0;
  75 }
  76
  77
  78 // Set CFI in tag k where k=1..n
  79 int mops_dot1Q_cfi (struct mops *mp, int k)
  80 {
  81         int n;
  82
  83         n = mp->dot1Q_s/4; // n = total number of tags
  84         if (k>n) return 1;
  85
  86         mp->dot1Q[((k-1)*4)+2] |=0x10; // set CFI (0x10 = 0001 0000)
  87         return 0;
  88 }
  89
  90
  91 // Assign 802.1Q tag with
  92 //   v ... VLAN
  93 //   c ... CoS
  94 //   i ... tag position (starting from zero!)
  95 //
  96 //   m ... modification: 1 = dot1Q_s is not changed
  97 //
  98 // NOTE:
  99 //   When called from for-loop to add all tags the total size dot1Q_s
 100 //   is updated continuously, therefore use m=1.
 101 //
 102 //   But when changing a particular tag within an existing 802.1Q stack
 103 //   the total number of tags does not change, therefore use m=0.
 104 //
 105 // RETURN VALUE: 0 upon success, 1 upon failure
 106 //
 107 int mops_dot1Q (struct mops *mp, int i, int m, u_int16_t v, u_int16_t c)
 108 {
 109         u_int8_t *ptr, c8;
 110
 111         if (i>=MAX_MOPS_DOT1Q_TAGS) return 1;  // max number of tags, see definitions in mops.h
 112         if ((v>4095)||(c>7)) return 1;         // greater values do not make sense
 113
 114         // Format: 0x8100 CoS-CFI-VLAN
 115         // where c=CoS, v=VLAN
 116         c8 = (u_int8_t) c;
 117         mp->dot1Q[4*i+0]= 0x81;
 118         mp->dot1Q[4*i+1]= 0x00;
 119         ptr = (u_int8_t*) &v;
 120         mp->dot1Q[4*i+3]=*ptr;
 121         mp->dot1Q[4*i+2]=*(ptr+1);
 122         mp->dot1Q[4*i+2]^= (c8 << 5);
 123
 124         if (m) {
 125                 mp->dot1Q_s=4*(1+i);  // NOTE: dot1Q_s = current tag position + 1
 126                 if (mp->dot1Q_s) mp->use_dot1Q = 1;
 127         }
 128
 129         return 0;
 130 }
 131