diff --git a/ifchd/linux.c b/ifchd/linux.c index d2fb72a..08370ab 100644 --- a/ifchd/linux.c +++ b/ifchd/linux.c @@ -40,6 +40,8 @@ #include #include #include +#include +#include #include #include @@ -151,6 +153,192 @@ static int set_if_flag(struct ifchd_client *cl, short flag) return ret; } +#define NLMSG_TAIL(nmsg) \ + ((struct rtattr *) (((uint8_t*) (nmsg)) + \ + NLMSG_ALIGN((nmsg)->nlmsg_len))) + +static int add_rtattr(struct nlmsghdr *n, size_t max_length, int type, + const void *data, size_t data_length) +{ + size_t length; + struct rtattr *rta; + + length = RTA_LENGTH(data_length); + + if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(length) > max_length) + return -E2BIG; + + rta = NLMSG_TAIL(n); + rta->rta_type = type; + rta->rta_len = length; + memcpy(RTA_DATA(rta), data, data_length); + n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(length); + + return 0; +} + +static int get_ifindex(const char *name) +{ + struct ifreq ifr; + int sk, err; + + if (!name) + return -1; + + sk = socket(PF_INET, SOCK_DGRAM, 0); + if (sk < 0) + return -1; + + memset(&ifr, 0, sizeof ifr); + strnkcpy(ifr.ifr_name, name, sizeof ifr.ifr_name); + + err = ioctl(sk, SIOCGIFINDEX, &ifr); + close(sk); + if (err < 0) + return -1; + + return ifr.ifr_ifindex; +} + +// 32-bit position values are relatively prime to 37, so the residue mod37 +// gives a unique mapping for each value. Gives correct result for v=0. +static int trailz(uint32_t v) +{ + static const int bpm37[] = { + 32, 0, 1, 26, 2, 23, 27, 0, 3, 16, 24, 30, 28, 11, 0, 13, 4, 7, 17, + 0, 25, 22, 31, 15, 29, 10, 12, 6, 0, 21, 14, 9, 5, 20, 8, 19, 18 + }; + return bpm37[(-v & v) % 37]; +} + +// sn must be in network order +static inline int subnet4_to_prefixlen(uint32_t sn) +{ + return 32 - trailz(sn); +} + +// str_bcast is optional. +void perform_ip_subnet_bcast(struct ifchd_client *cl, + const char *str_ipaddr, + const char *str_subnet, + const char *str_bcast) +{ + uint8_t request[NLMSG_ALIGN(sizeof(struct nlmsghdr)) + + NLMSG_ALIGN(sizeof(struct ifaddrmsg)) + + RTA_LENGTH(sizeof(struct in6_addr))]; + struct in_addr ipaddr, subnet, bcast; + struct sockaddr_nl nl_addr; + struct nlmsghdr *header; + struct ifaddrmsg *ifaddrmsg; + int nls, ifidx, r; + uint8_t prefixlen; + + if (!str_ipaddr) { + log_line("%s: (%s) interface ip address is NULL", + cl->ifnam, __func__); + return; + } + if (!str_subnet) { + log_line("%s: (%s) interface subnet address is NULL", + cl->ifnam, __func__); + return; + } + + if (!is_permitted(cl->ifnam)) + return; + ifidx = get_ifindex(cl->ifnam); + if (ifidx < 0) { + log_line("%s: (%s) can't get interface index", + cl->ifnam, __func__); + return; + } + + if (inet_pton(AF_INET, str_ipaddr, &ipaddr) <= 0) { + log_line("%s: (%s) bad interface ip address: '%s'", + cl->ifnam, __func__, str_ipaddr); + return; + } + + if (inet_pton(AF_INET, str_subnet, &subnet) <= 0) { + log_line("%s: (%s) bad interface subnet address: '%s'", + cl->ifnam, __func__, str_subnet); + return; + } + prefixlen = subnet4_to_prefixlen(subnet.s_addr); + + if (str_bcast) { + if (inet_pton(AF_INET, str_bcast, &bcast) <= 0) { + log_line("%s: (%s) bad interface broadcast address: '%s'", + cl->ifnam, __func__, str_bcast); + return; + } + } else { + // Generate the standard broadcast address if unspecified. + bcast.s_addr = ipaddr.s_addr | htonl(0xfffffffflu >> prefixlen); + } + + memset(&request, 0, sizeof request); + header = (struct nlmsghdr *)request; + header->nlmsg_len = NLMSG_LENGTH(sizeof(struct ifaddrmsg)); + header->nlmsg_type = RTM_NEWADDR; + header->nlmsg_flags = NLM_F_REPLACE | NLM_F_ACK | NLM_F_REQUEST; + header->nlmsg_seq = 1; + + ifaddrmsg = NLMSG_DATA(header); + ifaddrmsg->ifa_family = AF_INET; + ifaddrmsg->ifa_prefixlen = prefixlen; + ifaddrmsg->ifa_flags = IFA_F_PERMANENT; + ifaddrmsg->ifa_scope = RT_SCOPE_UNIVERSE; + ifaddrmsg->ifa_index = ifidx; + + if (add_rtattr(header, sizeof request, IFA_LOCAL, + &ipaddr, sizeof ipaddr) < 0) { + log_line("%s: (%s) couldn't add IFA_LOCAL to nlmsg", + cl->ifnam, __func__); + return; + } + if (add_rtattr(header, sizeof request, IFA_BROADCAST, + &bcast, sizeof bcast) < 0) { + log_line("%s: (%s) couldn't add IFA_BROADCAST to nlmsg", + cl->ifnam, __func__); + return; + } + + nls = socket(AF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE); + if (nls < 0) { + log_line("%s: (%s) netlink socket open failed: %s", + cl->ifnam, __func__, strerror(errno)); + return; + } + + memset(&nl_addr, 0, sizeof nl_addr); + nl_addr.nl_family = AF_NETLINK; + +retry_sendto: + r = sendto(nls, request, header->nlmsg_len, 0, + (struct sockaddr *)&nl_addr, sizeof nl_addr); + if (r < 0) { + if (errno == EINTR) + goto retry_sendto; + else { + log_line("%s: (%s) netlink sendto socket failed: %s", + cl->ifnam, __func__, strerror(errno)); + close(nls); + return; + } + } + close(nls); + log_line("Interface IP set to: '%s'", str_ipaddr); + log_line("Interface subnet set to: '%s'", str_subnet); + if (str_bcast) + log_line("Broadcast address set to: '%s'", str_bcast); + + // XXX: Would be nice to do this via netlink, too. + if (set_if_flag(cl, (IFF_UP | IFF_RUNNING))) + return; +} + + /* Sets IP address on an interface and brings it up. */ void perform_ip(struct ifchd_client *cl, const char *str, size_t len) {