qse/lib/dhcp/dhcp4msg.c

292 lines
7.4 KiB
C

#include <qse/dhcp/dhcpmsg.h>
#include <qse/cmn/hton.h>
#include "../cmn/mem-prv.h"
#include <qse/pack1.h>
struct magic_cookie_t
{
qse_uint32_t value;
};
typedef struct magic_cookie_t magic_cookie_t;
#include <qse/unpack.h>
int qse_dhcp4_initialize_pktbuf (qse_dhcp4_pktbuf_t* pkt, void* buf, qse_size_t capa)
{
if (capa < QSE_SIZEOF(*pkt->hdr)) return -1;
pkt->hdr = (qse_dhcp4_pkt_hdr_t*)buf;
pkt->len = QSE_SIZEOF(*pkt->hdr);
pkt->capa = capa;
QSE_MEMSET (pkt->hdr, 0, QSE_SIZEOF(*pkt->hdr));
return 0;
}
int qse_dhcp4_add_option (qse_dhcp4_pktbuf_t* pkt, int code, void* optr, qse_uint8_t olen)
{
qse_dhcp4_opt_hdr_t* opthdr;
magic_cookie_t* cookie;
int optlen;
/* TODO: support to override sname and file */
if (pkt->len < QSE_SIZEOF(*pkt->hdr) || pkt->capa < pkt->len)
{
/* the pktbuf_t structure got messy */
return -1;
}
if (pkt->len == QSE_SIZEOF(*pkt->hdr))
{
/* the first option is being added */
if (pkt->capa - pkt->len < QSE_SIZEOF(*cookie)) return -1;
cookie = (magic_cookie_t*)((qse_uint8_t*)pkt->hdr + pkt->len);
cookie->value = QSE_CONST_HTON32(QSE_DHCP4_MAGIC_COOKIE);
pkt->len += QSE_SIZEOF(*cookie);
}
else if (pkt->len < QSE_SIZEOF(*pkt->hdr) + QSE_SIZEOF(*cookie))
{
/* no space for cookie */
return -1;
}
else
{
cookie = (magic_cookie_t*)(pkt->hdr + 1);
if (cookie->value != QSE_CONST_HTON32(QSE_DHCP4_MAGIC_COOKIE)) return -1;
}
/* do i need to disallow adding a new option if END is found? */
if (code == QSE_DHCP4_OPT_PADDING || code == QSE_DHCP4_OPT_END)
{
optlen = 1; /* no length field in the header and no option palyload */
if (pkt->capa - pkt->len < optlen) return -1;
opthdr = (qse_dhcp4_opt_hdr_t*)((qse_uint8_t*)pkt->hdr + pkt->len);
}
else
{
optlen = QSE_SIZEOF(*opthdr) + olen;
if (pkt->capa - pkt->len < optlen) return -1;
opthdr = (qse_dhcp4_opt_hdr_t*)((qse_uint8_t*)pkt->hdr + pkt->len);
opthdr->len = olen;
if (olen > 0) QSE_MEMCPY (opthdr + 1, optr, olen);
}
opthdr->code = code;
pkt->len += optlen;
return 0;
}
int qse_dhcp4_delete_option (qse_dhcp4_pktbuf_t* pkt, int code)
{
qse_dhcp4_opt_hdr_t* ohdr;
qse_size_t olen;
qse_uint8_t* ovend;
ohdr = qse_dhcp4_find_option((qse_dhcp4_pktinf_t*)pkt, code);
if (!ohdr) return -1;
olen = (code == QSE_DHCP4_OPT_PADDING || code == QSE_DHCP4_OPT_END)? 1: (ohdr->len) + QSE_SIZEOF(*ohdr);
if ((ohdr >= pkt->hdr->file && ohdr < (ovend = (qse_uint8_t*)pkt->hdr->file + QSE_SIZEOF(pkt->hdr->file))) ||
(ohdr >= pkt->hdr->sname && ohdr < (ovend = (qse_uint8_t*)pkt->hdr->sname + QSE_SIZEOF(pkt->hdr->sname))))
{
/* the option resides in the overload area */
QSE_MEMMOVE (ohdr, (qse_uint8_t*)ohdr + olen, ovend - ((qse_uint8_t*)ohdr + olen));
QSE_MEMSET (ovend - olen, 0, olen);
/* packet length remains unchanged */
}
else
{
QSE_MEMMOVE (ohdr, (qse_uint8_t*)ohdr + olen, ((qse_uint8_t*)pkt->hdr + pkt->len) - ((qse_uint8_t*)ohdr + olen));
pkt->len -= olen;
}
return 0;
}
void qse_dhcp4_compact_options (qse_dhcp4_pktbuf_t* pkt)
{
/* TODO: move some optiosn to sname or file fields if they are not in use. */
}
static qse_uint8_t* get_option_start (const qse_dhcp4_pkt_hdr_t* pkt, qse_size_t len, qse_size_t* olen)
{
magic_cookie_t* cookie;
qse_size_t optlen;
/* check if a packet is large enough to hold the known header */
if (len < QSE_SIZEOF(qse_dhcp4_pkt_hdr_t)) return QSE_NULL;
/* get the length of option fields */
optlen = len - QSE_SIZEOF(qse_dhcp4_pkt_hdr_t);
/* check if a packet is large enough to have a magic cookie */
if (optlen < QSE_SIZEOF(*cookie)) return QSE_NULL;
/* get the pointer to the beginning of options */
cookie = (magic_cookie_t*)(pkt + 1);
/* check if the packet contains the right magic cookie */
if (cookie->value != QSE_CONST_HTON32(QSE_DHCP4_MAGIC_COOKIE)) return QSE_NULL;
*olen = optlen - QSE_SIZEOF(*cookie);
return (qse_uint8_t*)(cookie + 1);
}
int qse_dhcp4_walk_options (const qse_dhcp4_pktinf_t* pkt, qse_dhcp4_opt_walker_t walker)
{
const qse_uint8_t* optptr[3];
qse_size_t optlen[3];
int i;
optptr[0] = get_option_start(pkt->hdr, pkt->len, &optlen[0]);
if (optptr[0] == QSE_NULL) return -1;
optptr[1] = (const qse_uint8_t*)pkt->hdr->file;
optptr[2] = (const qse_uint8_t*)pkt->hdr->sname;
optlen[1] = 0;
optlen[2] = 0;
for (i = 0; i < 3; i++)
{
const qse_uint8_t* opt = optptr[i];
const qse_uint8_t* end = opt + optlen[i];
while (opt < end)
{
/* option code */
qse_dhcp4_opt_hdr_t* opthdr;
if (opt + QSE_SIZEOF(*opthdr) >= end) return -1;
opthdr = (qse_dhcp4_opt_hdr_t*)opt;
opt += QSE_SIZEOF(*opthdr);
/* no len field exists for PADDING and END */
if (opthdr->code == QSE_DHCP4_OPT_PADDING) continue;
if (opthdr->code == QSE_DHCP4_OPT_END) break;
if (opt + opthdr->len >= end) return -1; /* the length field is wrong */
if (opthdr->code == QSE_DHCP4_OPT_OVERLOAD)
{
if (opthdr->len != 1) return -1;
if (*opt & QSE_DHCP4_OPT_OVERLOAD_FILE) optlen[1] = QSE_SIZEOF(pkt->hdr->file);
if (*opt & QSE_DHCP4_OPT_OVERLOAD_SNAME) optlen[2] = QSE_SIZEOF(pkt->hdr->sname);
}
else
{
int n;
if ((n = walker(opthdr)) <= -1) return -1;
if (n == 0) break; /* stop */
}
opt += opthdr->len;
}
}
return 0;
}
qse_dhcp4_opt_hdr_t* qse_dhcp4_find_option (const qse_dhcp4_pktinf_t* pkt, int code)
{
const qse_uint8_t* optptr[3];
qse_size_t optlen[3];
int i;
optptr[0] = get_option_start(pkt->hdr, pkt->len, &optlen[0]);
if (!optptr[0]) return QSE_NULL;
optptr[1] = (const qse_uint8_t*)pkt->hdr->file;
optptr[2] = (const qse_uint8_t*)pkt->hdr->sname;
optlen[1] = 0;
optlen[2] = 0;
for (i = 0; i < 3; i++)
{
const qse_uint8_t* opt = optptr[i];
const qse_uint8_t* end = opt + optlen[i];
while (opt < end)
{
/* option code */
qse_dhcp4_opt_hdr_t* opthdr;
/* at least 1 byte is available. the check is because of PADDING or END */
if (*opt == QSE_DHCP4_OPT_PADDING)
{
opt++;
continue;
}
if (*opt == QSE_DHCP4_OPT_END)
{
if (code == QSE_DHCP4_OPT_END)
{
/* the caller must handle END specially becuase it is only 1 byte long
* for no length part in the header */
return (qse_dhcp4_opt_hdr_t*)opt;
}
break;
}
if (opt + QSE_SIZEOF(*opthdr) > end) break;
opthdr = (qse_dhcp4_opt_hdr_t*)opt;
opt += QSE_SIZEOF(*opthdr);
/* option length */
if (opthdr->code == code)
{
if (opt + opthdr->len > end) break;
return opthdr;
}
/*
* If option overload is used, the SName and/or File fields are read and
* interpreted in the same way as the Options field, after all options in
* the Option field are parsed. If the message actually does need to carry
* a server name or boot file, these are included as separate options
* (number 66 and number 67, respectively), which are variable-length and
* can therefore be made exactly the length needed.
*/
if (opthdr->code == QSE_DHCP4_OPT_OVERLOAD)
{
if (opthdr->len != 1) break;
if (*opt & QSE_DHCP4_OPT_OVERLOAD_FILE) optlen[1] = QSE_SIZEOF(pkt->hdr->file);
if (*opt & QSE_DHCP4_OPT_OVERLOAD_SNAME) optlen[2] = QSE_SIZEOF(pkt->hdr->sname);
}
opt += opthdr->len;
}
}
return QSE_NULL;
}
qse_uint8_t* qse_dhcp4_get_relay_suboption (const qse_uint8_t* ptr, qse_uint8_t len, int code, qse_uint8_t* olen)
{
const qse_uint8_t* end = ptr + len;
while (ptr < end)
{
qse_uint8_t oc, ol;
oc = *ptr++;
if (ptr >= end) break;
ol = *ptr++;
if (oc == code)
{
*olen = ol;
return (qse_uint8_t*)ptr;
}
ptr += ol;
}
return QSE_NULL;
}