MLPPP not working in cluster mode.
[l2tpns.git] / control.c
1 // L2TPNS: control
2
3 #include <string.h>
4 #include "l2tpns.h"
5 #include "control.h"
6
7 int pack_control(uint8_t *data, int len, uint8_t type, int argc, char *argv[])
8 {
9 struct nsctl_packet pkt;
10 struct nsctl_args arg;
11 char *p = pkt.argv;
12 int sz = (p - (char *) &pkt);
13
14 if (len > sizeof(pkt))
15 len = sizeof(pkt);
16
17 if (argc > 0xff)
18 argc = 0xff; // paranoia
19
20 pkt.magic = ntohs(NSCTL_MAGIC);
21 pkt.type = type;
22 pkt.argc = argc;
23
24 while (argc-- > 0)
25 {
26 char *a = *argv++;
27 int s = strlen(a);
28
29 if (s > sizeof(arg.value))
30 s = sizeof(arg.value); // silently truncate
31
32 arg.len = s;
33 s += sizeof(arg.len);
34
35 if (sz + s > len)
36 return -1; // overflow
37
38 if (arg.len)
39 memcpy(arg.value, a, arg.len);
40
41 memcpy(p, &arg, s);
42 sz += s;
43 p += s;
44 }
45
46 /*
47 * terminate: this is both a sanity check and additionally
48 * ensures that there's a spare byte in the packet to null
49 * terminate the last argument when unpacking (see unpack_control)
50 */
51 if (sz + sizeof(arg.len) > len)
52 return -1; // overflow
53
54 arg.len = 0xff;
55 memcpy(p, &arg.len, sizeof(arg.len));
56
57 sz += sizeof(arg.len);
58 memcpy(data, &pkt, sz);
59
60 return sz;
61 }
62
63 int unpack_control(struct nsctl *control, uint8_t *data, int len)
64 {
65 struct nsctl_packet pkt;
66 char *p = pkt.argv;
67 int sz = (p - (char *) &pkt);
68 int i;
69
70 if (len < sz)
71 return NSCTL_ERR_SHORT;
72
73 if (len > sizeof(pkt))
74 return NSCTL_ERR_LONG;
75
76 memcpy(&pkt, data, len);
77 if (ntohs(pkt.magic) != NSCTL_MAGIC)
78 return NSCTL_ERR_MAGIC;
79
80 switch (pkt.type)
81 {
82 case NSCTL_REQ_LOAD:
83 case NSCTL_REQ_UNLOAD:
84 case NSCTL_REQ_HELP:
85 case NSCTL_REQ_CONTROL:
86 case NSCTL_RES_OK:
87 case NSCTL_RES_ERR:
88 control->type = pkt.type;
89 break;
90
91 default:
92 return NSCTL_ERR_TYPE;
93 }
94
95 control->argc = pkt.argc;
96 for (i = 0; i <= control->argc; i++)
97 {
98 unsigned s;
99
100 if (len < sz + 1)
101 return NSCTL_ERR_SHORT;
102
103 s = (uint8_t) *p;
104 *p++ = 0; // null terminate previous arg
105 sz++;
106
107 if (i < control->argc)
108 {
109 if (len < sz + s)
110 return NSCTL_ERR_SHORT;
111
112 control->argv[i] = p;
113 p += s;
114 sz += s;
115 }
116 else
117 {
118 /* check for terminator */
119 if (s != 0xff)
120 return NSCTL_ERR_SHORT;
121 }
122 }
123
124 if (sz != len)
125 return NSCTL_ERR_LONG; // trailing cr*p
126
127 return control->type;
128 }
129
130 void dump_control(struct nsctl *control, FILE *stream)
131 {
132 char *type = "*unknown*";
133
134 if (!stream)
135 stream = stdout;
136
137 switch (control->type)
138 {
139 case NSCTL_REQ_LOAD: type = "NSCTL_REQ_LOAD"; break;
140 case NSCTL_REQ_UNLOAD: type = "NSCTL_REQ_UNLOAD"; break;
141 case NSCTL_REQ_HELP: type = "NSCTL_REQ_HELP"; break;
142 case NSCTL_REQ_CONTROL: type = "NSCTL_REQ_CONTROL"; break;
143 case NSCTL_RES_OK: type = "NSCTL_RES_OK"; break;
144 case NSCTL_RES_ERR: type = "NSCTL_RES_ERR"; break;
145 }
146
147 fprintf(stream, "Control packet:\n");
148 fprintf(stream, " Type: %d (%s)\n", (int) control->type, type);
149 fprintf(stream, " Args: %d", (int) control->argc);
150 if (control->argc)
151 {
152 int i;
153 fprintf(stream, " (\"");
154 for (i = 0; i < control->argc; i++)
155 fprintf(stream, "%s%s", i ? "\", \"" : "", control->argv[i]);
156
157 fprintf(stream, "\")");
158 }
159
160 fprintf(stream, "\n\n");
161 }