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