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