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