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