src/fragroute/iputil.c
#include "defines.h"
#include "config.h"
#include "common.h"
#ifdef HAVE_DNET_H
#include <dnet.h>
#endif
#ifdef HAVE_DUMBNET_H
#include <dumbnet.h>
#endif
#include <errno.h>
#include <stdio.h>
#include <string.h>
static ssize_t inet_add_option_6(void *buf, size_t len, int proto, const void *optbuf, size_t optlen);
ssize_t
inet_add_option(uint16_t eth_type, void *buf, size_t len, int proto, const void *optbuf, size_t optlen)
{
if (eth_type == ETH_TYPE_IP) {
return ip_add_option(buf, len, proto, optbuf, optlen);
} else if (eth_type == ETH_TYPE_IPV6) {
return inet_add_option_6(buf, len, proto, optbuf, optlen);
} else {
errno = EINVAL;
return -1;
}
}
/*
* IPv6 version of libdnet's ip_add_option():
*/
ssize_t
inet_add_option_6(void *buf, size_t len, int proto, const void *optbuf, size_t optlen)
{
struct ip6_hdr *ip6;
struct tcp_hdr *tcp = NULL;
u_char *p;
int hl, datalen, padlen;
if (proto != IP_PROTO_TCP) {
errno = EINVAL;
return (-1);
}
ip6 = (struct ip6_hdr *)buf;
p = (u_char *)buf + IP6_HDR_LEN;
tcp = (struct tcp_hdr *)p;
hl = tcp->th_off << 2;
p = (u_char *)tcp + hl;
datalen = ntohs(ip6->ip6_plen) + IP6_HDR_LEN - (int)(p - (u_char *)buf);
/* Compute padding to next word boundary. */
if ((padlen = 4 - (int)(optlen % 4)) == 4)
padlen = 0;
/* XXX - IP_HDR_LEN_MAX == TCP_HDR_LEN_MAX */
if (hl + optlen + padlen > IP_HDR_LEN_MAX || ntohs(ip6->ip6_plen) + IP6_HDR_LEN + optlen + padlen > len) {
errno = EINVAL;
return (-1);
}
/* Shift any existing data. */
if (datalen) {
memmove(p + optlen + padlen, p, datalen);
}
/* XXX - IP_OPT_NOP == TCP_OPT_NOP */
if (padlen) {
memset(p, IP_OPT_NOP, padlen);
p += padlen;
}
memmove(p, optbuf, optlen);
p += optlen;
optlen += padlen;
tcp->th_off = (p - (u_char *)tcp) >> 2;
ip6->ip6_plen = htons(ntohs(ip6->ip6_plen) + optlen);
return (ssize_t)optlen;
}
void
inet_checksum(uint16_t eth_type, void *buf, size_t len)
{
if (eth_type == ETH_TYPE_IP) {
return ip_checksum(buf, len);
} else if (eth_type == ETH_TYPE_IPV6) {
return ip6_checksum(buf, len);
}
}
int
raw_ip_opt_parse(int argc, char *argv[], uint8_t *opt_type, uint8_t *opt_len, uint8_t *buff, int buff_len)
{
int i, j;
if (sscanf(argv[0], "%hhx", opt_type) != 1) {
warn("invalid opt_type");
return -1;
}
if (sscanf(argv[1], "%hhx", opt_len) != 1) {
warn("invalid opt_len");
return -1;
}
j = 0;
for (i = 2; i < argc && j < buff_len; ++i, ++j) {
if (sscanf(argv[i], "%hhx", &buff[j]) != 1) {
warn("invalid opt_data");
return -1;
}
}
if (*opt_len != j + 2) {
warnx("invalid opt->len (%d) doesn't match data length (%d)", *opt_len, j);
return -1;
}
return 0;
}
int
raw_ip6_opt_parse(int argc, char *argv[], uint8_t *proto, int *len, uint8_t *buff, int buff_len)
{
int i, j;
if (sscanf(argv[0], "%hhx", proto) != 1) {
warn("invalid protocol");
return -1;
}
j = 0;
for (i = 1; i < argc && j < buff_len; ++i, ++j) {
if (sscanf(argv[i], "%hhx", &buff[j]) != 1) {
warn("invalid opt_data");
return -1;
}
}
*len = j;
if ((j + 2) % 8 != 0) {
warnx("(opt_len (%d) + 2) %% 8 != 0", j);
return -1;
}
return 0;
}