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
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
|
// L2TPNS: control
#include <string.h>
#include <netinet/ip6.h>
#include "dhcp6.h"
#include "l2tpns.h"
#include "control.h"
int pack_control(uint8_t *data, int len, uint8_t type, int argc, char *argv[])
{
struct nsctl_packet pkt;
struct nsctl_args arg;
char *p = pkt.argv;
int sz = (p - (char *) &pkt);
if (len > sizeof(pkt))
len = sizeof(pkt);
if (argc > 0xff)
argc = 0xff; // paranoia
pkt.magic = ntohs(NSCTL_MAGIC);
pkt.type = type;
pkt.argc = argc;
while (argc-- > 0)
{
char *a = *argv++;
int s = strlen(a);
if (s > sizeof(arg.value))
s = sizeof(arg.value); // silently truncate
arg.len = s;
s += sizeof(arg.len);
if (sz + s > len)
return -1; // overflow
if (arg.len)
memcpy(arg.value, a, arg.len);
memcpy(p, &arg, s);
sz += s;
p += s;
}
/*
* terminate: this is both a sanity check and additionally
* ensures that there's a spare byte in the packet to null
* terminate the last argument when unpacking (see unpack_control)
*/
if (sz + sizeof(arg.len) > len)
return -1; // overflow
arg.len = 0xff;
memcpy(p, &arg.len, sizeof(arg.len));
sz += sizeof(arg.len);
memcpy(data, &pkt, sz);
return sz;
}
int unpack_control(struct nsctl *control, uint8_t *data, int len)
{
struct nsctl_packet pkt;
char *p = pkt.argv;
int sz = (p - (char *) &pkt);
int i;
if (len < sz)
return NSCTL_ERR_SHORT;
if (len > sizeof(pkt))
return NSCTL_ERR_LONG;
memcpy(&pkt, data, len);
if (ntohs(pkt.magic) != NSCTL_MAGIC)
return NSCTL_ERR_MAGIC;
switch (pkt.type)
{
case NSCTL_REQ_LOAD:
case NSCTL_REQ_UNLOAD:
case NSCTL_REQ_HELP:
case NSCTL_REQ_CONTROL:
case NSCTL_RES_OK:
case NSCTL_RES_ERR:
control->type = pkt.type;
break;
default:
return NSCTL_ERR_TYPE;
}
control->argc = pkt.argc;
for (i = 0; i <= control->argc; i++)
{
unsigned s;
if (len < sz + 1)
return NSCTL_ERR_SHORT;
s = (uint8_t) *p;
*p++ = 0; // null terminate previous arg
sz++;
if (i < control->argc)
{
if (len < sz + s)
return NSCTL_ERR_SHORT;
control->argv[i] = p;
p += s;
sz += s;
}
else
{
/* check for terminator */
if (s != 0xff)
return NSCTL_ERR_SHORT;
}
}
if (sz != len)
return NSCTL_ERR_LONG; // trailing cr*p
return control->type;
}
void dump_control(struct nsctl *control, FILE *stream)
{
char *type = "*unknown*";
if (!stream)
stream = stdout;
switch (control->type)
{
case NSCTL_REQ_LOAD: type = "NSCTL_REQ_LOAD"; break;
case NSCTL_REQ_UNLOAD: type = "NSCTL_REQ_UNLOAD"; break;
case NSCTL_REQ_HELP: type = "NSCTL_REQ_HELP"; break;
case NSCTL_REQ_CONTROL: type = "NSCTL_REQ_CONTROL"; break;
case NSCTL_RES_OK: type = "NSCTL_RES_OK"; break;
case NSCTL_RES_ERR: type = "NSCTL_RES_ERR"; break;
}
fprintf(stream, "Control packet:\n");
fprintf(stream, " Type: %d (%s)\n", (int) control->type, type);
fprintf(stream, " Args: %d", (int) control->argc);
if (control->argc)
{
int i;
fprintf(stream, " (\"");
for (i = 0; i < control->argc; i++)
fprintf(stream, "%s%s", i ? "\", \"" : "", control->argv[i]);
fprintf(stream, "\")");
}
fprintf(stream, "\n\n");
}
|
