int parse_dhcpmessage (dhcp_t *dhcp, dhcpmessage_t *message)
{
unsigned char *p = message->options;
unsigned char option;
unsigned char length;
unsigned char *end = message->options + sizeof (message->options);
unsigned int len = 0;
int i;
int retval = -1;
route_t *first_route = xmalloc (sizeof (route_t));
route_t *route = first_route;
route_t *last_route = NULL;
route_t *csr = NULL;
char classid[CLASS_ID_MAX_LEN];
char clientid[CLIENT_ID_MAX_LEN];
memset (first_route, 0, sizeof (route_t));
/* The message back never has the class or client id's so we save them */
strcpy (classid, dhcp->classid);
strcpy (clientid, dhcp->clientid);
free_dhcp (dhcp);
memset (dhcp, 0, sizeof (dhcp_t));
dhcp->address.s_addr = message->yiaddr;
strcpy (dhcp->servername, message->servername);
while (p < end)
{
option = *p++;
if (!option)
continue;
length = *p++;
if (p + length >= end)
{
retval = -1;
goto eexit;
}
switch (option)
{
case DHCP_END:
goto eexit;
case DHCP_MESSAGETYPE:
retval = (int) *p;
break;
#define GET_UINT32(_val) \
memcpy (&_val, p, sizeof (uint32_t));
#define GET_UINT32_H(_val) \
GET_UINT32 (_val); \
_val = ntohl (_val);
case DHCP_ADDRESS:
GET_UINT32 (dhcp->address.s_addr);
break;
case DHCP_NETMASK:
GET_UINT32 (dhcp->netmask.s_addr);
break;
case DHCP_BROADCAST:
GET_UINT32 (dhcp->broadcast.s_addr);
break;
case DHCP_SERVERIDENTIFIER:
GET_UINT32 (dhcp->serveraddress.s_addr);
break;
case DHCP_LEASETIME:
GET_UINT32_H (dhcp->leasetime);
break;
case DHCP_RENEWALTIME:
GET_UINT32_H (dhcp->renewaltime);
break;
case DHCP_REBINDTIME:
GET_UINT32_H (dhcp->rebindtime);
break;
case DHCP_MTU:
GET_UINT32_H (dhcp->mtu);
/* Minimum legal mtu is 68 */
if (dhcp->mtu > 0 && dhcp->mtu < 68)
dhcp->mtu = 68;
break;
#undef GET_UINT32_H
#undef GET_UINT32
#define GETSTR(_var) \
if (_var) free (_var); \
_var = xmalloc (length + 1); \
memcpy (_var, p, length); \
memset (_var + length, 0, 1);
case DHCP_HOSTNAME:
GETSTR (dhcp->hostname);
break;
case DHCP_DNSDOMAIN:
GETSTR (dhcp->dnsdomain);
break;
case DHCP_MESSAGE:
GETSTR (dhcp->message);
break;
case DHCP_ROOTPATH:
GETSTR (dhcp->rootpath);
break;
case DHCP_NISDOMAIN:
GETSTR (dhcp->nisdomain);
break;
#undef GETSTR
#define GETADDR(_var) \
if (_var) free (_var); \
_var = xmalloc (sizeof (address_t)); \
dhcp_add_address (_var, p, length);
case DHCP_DNSSERVER:
GETADDR (dhcp->dnsservers);
break;
case DHCP_NTPSERVER:
GETADDR (dhcp->ntpservers);
break;
case DHCP_NISSERVER:
GETADDR (dhcp->nisservers);
break;
#undef GETADDR
case DHCP_DNSSEARCH:
if (dhcp->dnssearch)
free (dhcp->dnssearch);
if ((len = decode_search (p, length, NULL)))
{
dhcp->dnssearch = xmalloc (len);
decode_search (p, length, dhcp->dnssearch);
}
break;
case DHCP_CSR:
csr = decodeCSR (p, length);
break;
case DHCP_STATICROUTE:
for (i = 0; i < length; i += 8)
{
memcpy (&route->destination.s_addr, p + i, 4);
memcpy (&route->gateway.s_addr, p + i + 4, 4);
route->netmask.s_addr = getnetmask (route->destination.s_addr);
last_route = route;
route->next = xmalloc (sizeof (route_t));
route = route->next;
memset (route, 0, sizeof (route_t));
}
break;
case DHCP_ROUTERS:
for (i = 0; i < length; i += 4)
{
memcpy (&route->gateway.s_addr, p + i, 4);
last_route = route;
route->next = xmalloc (sizeof (route_t));
route = route->next;
memset (route, 0, sizeof (route_t));
}
break;
default:
logger (LOG_DEBUG, "no facility to parse DHCP code %u", option);
break;
}
p += length;
}
eexit:
/* Fill in any missing fields */
if (! dhcp->netmask.s_addr)
dhcp->netmask.s_addr = getnetmask (dhcp->address.s_addr);
if (! dhcp->broadcast.s_addr)
dhcp->broadcast.s_addr = dhcp->address.s_addr | ~dhcp->netmask.s_addr;
/* If we have classess static routes then we discard
static routes and routers according to RFC 3442 */
if (csr)
{
dhcp->routes = csr;
free_route (first_route);
}
else
{
dhcp->routes = first_route;
if (last_route)
{
free (last_route->next);
last_route->next = NULL;
}
else
{
free_route (dhcp->routes);
dhcp->routes = NULL;
}
}
/* The message back never has the class or client id's so we restore them */
strcpy (dhcp->classid, classid);
strcpy (dhcp->clientid, clientid);
return retval;
}
int parse_dhcpmessage (dhcp_t *dhcp, const dhcpmessage_t *message)
{
const unsigned char *p = message->options;
const unsigned char *end = p; /* Add size later for gcc-3 issue */
unsigned char option;
unsigned char length;
unsigned int len = 0;
int i;
int retval = -1;
route_t *routers = NULL;
route_t *routersp = NULL;
route_t *static_routes = NULL;
route_t *static_routesp = NULL;
route_t *csr = NULL;
end += sizeof (message->options);
dhcp->address.s_addr = message->yiaddr;
strlcpy (dhcp->servername, message->servername,
sizeof (dhcp->servername));
#define LEN_ERR \
{ \
logger (LOG_ERR, "invalid length %d for option %d", length, option); \
p += length; \
continue; \
}
while (p < end) {
option = *p++;
if (! option)
continue;
length = *p++;
if (p + length >= end) {
logger (LOG_ERR, "dhcp option exceeds message length");
retval = -1;
goto eexit;
}
switch (option) {
case DHCP_END:
goto eexit;
case DHCP_MESSAGETYPE:
retval = (int) *p;
p += length;
continue;
default:
if (length == 0) {
logger (LOG_DEBUG, "option %d has zero length, skipping",
option);
continue;
}
}
#define LENGTH(_length) \
if (length != _length) \
LEN_ERR;
#define MIN_LENGTH(_length) \
if (length < _length) \
LEN_ERR;
#define MULT_LENGTH(_mult) \
if (length % _mult != 0) \
LEN_ERR;
#define GET_UINT8(_val) \
LENGTH (sizeof (uint8_t)); \
memcpy (&_val, p, sizeof (uint8_t));
#define GET_UINT16(_val) \
LENGTH (sizeof (uint16_t)); \
memcpy (&_val, p, sizeof (uint16_t));
#define GET_UINT32(_val) \
LENGTH (sizeof (uint32_t)); \
memcpy (&_val, p, sizeof (uint32_t));
#define GET_UINT16_H(_val) \
GET_UINT16 (_val); \
_val = ntohs (_val);
#define GET_UINT32_H(_val) \
GET_UINT32 (_val); \
_val = ntohl (_val);
switch (option) {
case DHCP_ADDRESS:
GET_UINT32 (dhcp->address.s_addr);
break;
case DHCP_NETMASK:
GET_UINT32 (dhcp->netmask.s_addr);
break;
case DHCP_BROADCAST:
GET_UINT32 (dhcp->broadcast.s_addr);
break;
case DHCP_SERVERIDENTIFIER:
GET_UINT32 (dhcp->serveraddress.s_addr);
break;
case DHCP_LEASETIME:
GET_UINT32_H (dhcp->leasetime);
break;
case DHCP_RENEWALTIME:
GET_UINT32_H (dhcp->renewaltime);
break;
case DHCP_REBINDTIME:
GET_UINT32_H (dhcp->rebindtime);
break;
case DHCP_MTU:
GET_UINT16_H (dhcp->mtu);
/* Minimum legal mtu is 68 accoridng to RFC 2132.
In practise it's 576 (minimum maximum message size) */
if (dhcp->mtu < MTU_MIN) {
logger (LOG_DEBUG, "MTU %d is too low, minimum is %d; ignoring", dhcp->mtu, MTU_MIN);
dhcp->mtu = 0;
}
break;
#undef GET_UINT32_H
#undef GET_UINT32
#undef GET_UINT16_H
#undef GET_UINT16
#undef GET_UINT8
#define GETSTR(_var) \
MIN_LENGTH (sizeof (char)); \
if (_var) free (_var); \
_var = xmalloc (length + 1); \
memcpy (_var, p, length); \
memset (_var + length, 0, 1);
case DHCP_HOSTNAME:
GETSTR (dhcp->hostname);
break;
case DHCP_DNSDOMAIN:
GETSTR (dhcp->dnsdomain);
break;
case DHCP_MESSAGE:
GETSTR (dhcp->message);
break;
case DHCP_ROOTPATH:
GETSTR (dhcp->rootpath);
break;
case DHCP_NISDOMAIN:
GETSTR (dhcp->nisdomain);
break;
#undef GETSTR
#define GETADDR(_var) \
MULT_LENGTH (4); \
if (! dhcp_add_address (&_var, p, length)) \
{ \
retval = -1; \
goto eexit; \
}
case DHCP_DNSSERVER:
GETADDR (dhcp->dnsservers);
break;
case DHCP_NTPSERVER:
GETADDR (dhcp->ntpservers);
break;
case DHCP_NISSERVER:
GETADDR (dhcp->nisservers);
break;
#undef GETADDR
case DHCP_DNSSEARCH:
MIN_LENGTH (1);
free (dhcp->dnssearch);
if ((len = decode_search (p, length, NULL)) > 0) {
dhcp->dnssearch = xmalloc (len);
decode_search (p, length, dhcp->dnssearch);
}
break;
case DHCP_CSR:
MIN_LENGTH (5);
free_route (csr);
csr = decodeCSR (p, length);
break;
case DHCP_STATICROUTE:
MULT_LENGTH (8);
for (i = 0; i < length; i += 8) {
if (static_routesp) {
static_routesp->next = xmalloc (sizeof (route_t));
static_routesp = static_routesp->next;
} else
static_routesp = static_routes = xmalloc (sizeof (route_t));
memset (static_routesp, 0, sizeof (route_t));
memcpy (&static_routesp->destination.s_addr, p + i, 4);
memcpy (&static_routesp->gateway.s_addr, p + i + 4, 4);
static_routesp->netmask.s_addr =
getnetmask (static_routesp->destination.s_addr);
}
break;
case DHCP_ROUTERS:
MULT_LENGTH (4);
for (i = 0; i < length; i += 4)
{
if (routersp) {
routersp->next = xmalloc (sizeof (route_t));
routersp = routersp->next;
} else
routersp = routers = xmalloc (sizeof (route_t));
memset (routersp, 0, sizeof (route_t));
memcpy (&routersp->gateway.s_addr, p + i, 4);
}
break;
#undef LENGTH
#undef MIN_LENGTH
#undef MULT_LENGTH
default:
logger (LOG_DEBUG, "no facility to parse DHCP code %u", option);
break;
}
p += length;
}
eexit:
/* Fill in any missing fields */
if (! dhcp->netmask.s_addr)
dhcp->netmask.s_addr = getnetmask (dhcp->address.s_addr);
if (! dhcp->broadcast.s_addr)
dhcp->broadcast.s_addr = dhcp->address.s_addr | ~dhcp->netmask.s_addr;
/* If we have classess static routes then we discard
static routes and routers according to RFC 3442 */
if (csr) {
dhcp->routes = csr;
free_route (routers);
free_route (static_routes);
} else {
/* Ensure that we apply static routes before routers */
if (static_routes)
{
dhcp->routes = static_routes;
static_routesp->next = routers;
} else
dhcp->routes = routers;
}
return retval;
}
int dhcp_packet_parser(unsigned char *data, int data_len, struct dhcp_packet_info *dhcp_info) {
struct bootp_msg *msg;
unsigned char *p;
unsigned char *end;
int iphdr_len, udphdr_len;
int dhcp_msg_type = 0;
unsigned int lease_time = -1;
int length = 0;
char hostname[DHCP_HOSTNAME_MAX_LEN] = { 0 };
unsigned char ifname[6] = {0};
unsigned char uplink_mac[6] = {0};
//char ifname_type[3] = "eth";
/* IP header */
struct iphdr *piphdr = (struct iphdr *)data;
iphdr_len = piphdr->ihl * 4;
/* UDP header */
udphdr_len = sizeof(struct udphdr);
/* For bootp Application data */
msg = (struct bootp_msg *)(data + iphdr_len + udphdr_len);
p = msg->options + 4; /* offset of magic cookie */
end = msg->options + sizeof(msg->options);
/* ****** option structure *****
* |-opt id-|-length-|-variable value-|
*/
while (p < end) {
switch(*p) {
case dhcpPrivateOption:
length = p[1];
_log(APP_DEBUG, "DHCP message private length: %d",length);
os_memcpy(uplink_mac, &p[3 + (length - 18)], 6);
_log(APP_DEBUG, "nflog packet recv:"MAC_FORMAT, MAC_ADDR(uplink_mac));
os_memcpy(ifname, &p[10 + (length - 18)], 4);
_log(APP_DEBUG, "nflog packet recv sta type: %02x %02x %02x %02x", ifname[0], ifname[1], ifname[2], ifname[3]);
p += p[1];
p += 2;
break;
case endOption:
goto OUT;
case padOption:
p++;
break;
case dhcpIPaddrLeaseTime:
if (p[1]){
#define GET_UINT32(_val) \
memcpy (&_val, &p[2], sizeof (uint32_t));
#define GET_UINT32_H(_val) \
GET_UINT32 (_val); \
_val = ntohl (_val);
GET_UINT32_H(lease_time);
}
p += p[1];
p += 2;
break;
case hostName:
if (p[1])
os_memcpy(hostname, &p[2], MIN(p[1], DHCP_HOSTNAME_MAX_LEN));
p += p[1];
p += 2;
break;
case dhcpMessageType:
if (p[1])
dhcp_msg_type = p[2];
p += p[1];
p += 2;
break;
default:
p += p[1];
p += 2;
break;
}
}
OUT:
dhcp_info->dhcp_msg_type = dhcp_msg_type;
_log(APP_DEBUG, "DHCP message type: %d", dhcp_msg_type);
switch(dhcp_msg_type) {
case DHCP_OFFER:
case DHCP_ACK:
if(dhcp_msg_type == DHCP_ACK)
inet_ntop(AF_INET, &msg->yiaddr, dhcp_info->sta_ip, INET_ADDRSTRLEN);
dhcp_info->lease_time = lease_time;
os_memcpy(dhcp_info->sta_mac, msg->chaddr, sizeof(dhcp_info->sta_mac));
break;
case DHCP_DISCOVER:
case DHCP_REQUEST:
//case DHCP_INFORM:
inet_ntop(AF_INET, &msg->ciaddr, dhcp_info->sta_ip, INET_ADDRSTRLEN);
os_memcpy(dhcp_info->sta_mac, msg->chaddr, sizeof(dhcp_info->sta_mac));
os_memcpy(dhcp_info->sta_hostname, hostname, MIN(sizeof(dhcp_info->sta_hostname), DHCP_HOSTNAME_MAX_LEN));
os_memcpy(dhcp_info->uplink_mac, uplink_mac, sizeof(dhcp_info->uplink_mac));
os_memcpy(dhcp_info->ifname, ifname, sizeof(dhcp_info->ifname));
break;
case DHCP_RELEASE:
os_memcpy(dhcp_info->sta_mac, msg->chaddr, sizeof(dhcp_info->sta_mac));
case DHCP_NAK:
os_memcpy(dhcp_info->sta_mac, msg->chaddr, sizeof(dhcp_info->sta_mac));
break;
default:
break;
}
return 0;
}
