161 lines
3.2 KiB
C
161 lines
3.2 KiB
C
// L2TPNS: control
|
|
|
|
#include <string.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");
|
|
}
|