diff --git a/trunk/build_firmware.sh b/trunk/build_firmware.sh index fff4ed37d..955250f9a 100755 --- a/trunk/build_firmware.sh +++ b/trunk/build_firmware.sh @@ -493,6 +493,10 @@ if [ "$CONFIG_FIRMWARE_INCLUDE_ZRAM" = "y" ] ; then func_enable_busybox_param "CONFIG_FEATURE_SWAPONOFF_LABEL" func_enable_busybox_param "CONFIG_FEATURE_SWAPON_PRI" fi +############################## EOIP SUPPORT ########################### +if [ "$CONFIG_FIRMWARE_INCLUDE_EOIP" = "y" ] ; then + func_enable_kernel_param "CONFIG_NET_EOIP" +fi ####################################################################### echo --------------------------MAKE-DEP-------------------------------- make dep diff --git a/trunk/user/Makefile b/trunk/user/Makefile index 1e878c49b..1eb68f5f8 100644 --- a/trunk/user/Makefile +++ b/trunk/user/Makefile @@ -81,6 +81,7 @@ dir_y += busybox dir_y += iptables dir_$(CONFIG_FIRMWARE_INCLUDE_IPSET) += ipset dir_y += ebtables +dir_$(CONFIG_FIRMWARE_INCLUDE_EOIP) += eoip-ctl dir_y += iproute2 dir_y += wireless_tools dir_y += shared diff --git a/trunk/user/eoip-ctl/Makefile b/trunk/user/eoip-ctl/Makefile new file mode 100644 index 000000000..20ef8ae3f --- /dev/null +++ b/trunk/user/eoip-ctl/Makefile @@ -0,0 +1,19 @@ +CFLAGS += -Os -ffunction-sections -fdata-sections -fvisibility=hidden +LDFLAGS += -Wl,--gc-sections + +all: eoip-ctl + +eoip-ctl: eoipcr.o libnetlink.o + $(CC) $(LDFLAGS) -o eoip-ctl eoipcr.o libnetlink.o + +eoipcr.o: eoipcr.c + $(CC) $(CFLAGS) -fno-strict-aliasing -Wall -c eoipcr.c + +libnetlink.o: libnetlink.c libnetlink.h + $(CC) $(CFLAGS) -fno-strict-aliasing -Wall -c libnetlink.c + +clean: + rm -f eoip-ctl eoipcr.o libnetlink.o + +romfs: + $(ROMFSINST) eoip-ctl /usr/bin/eoip-ctl diff --git a/trunk/user/eoip-ctl/eoipcr.c b/trunk/user/eoip-ctl/eoipcr.c new file mode 100644 index 000000000..472702692 --- /dev/null +++ b/trunk/user/eoip-ctl/eoipcr.c @@ -0,0 +1,520 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "libnetlink.h" + +int print_link(const struct sockaddr_nl *who, struct nlmsghdr *n, void *arg) { + struct ndmsg *r = NLMSG_DATA(n); + int len = n->nlmsg_len; + struct rtattr *ifattr[IFLA_MAX+1]; + struct rtattr *ifinfo[IFLA_INFO_MAX+1]; + struct rtattr *ifgreo[IFLA_GRE_MAX+1]; + const char *ifname; + const char *kind; + int link=0; + int tunid=0; + struct in_addr sip,dip; + uint8_t ttl=0,tos=0; + struct ifinfomsg *ifi; + + sip.s_addr=dip.s_addr=htonl(0); + + if (n->nlmsg_type != RTM_NEWLINK && n->nlmsg_type != RTM_DELLINK) { + fprintf(stderr, "Not RTM_xxxLINK: %08x %08x %08x\n", n->nlmsg_len, n->nlmsg_type, n->nlmsg_flags); + return 0; + } + len -= NLMSG_LENGTH(sizeof(*r)); + if (len < 0) { + fprintf(stderr, "BUG: wrong nlmsg len %d\n", len); + return -1; + } + + ifi=(void *)r; + + parse_rtattr(ifattr, IFLA_MAX, IFLA_RTA(r), n->nlmsg_len - NLMSG_LENGTH(sizeof(*r))); + if (ifattr[IFLA_IFNAME]) + ifname=rta_getattr_str(ifattr[IFLA_IFNAME]); + else + ifname=""; + if (ifattr[IFLA_LINKINFO]) + parse_rtattr(ifinfo, IFLA_INFO_MAX, (void *)rta_getattr_str(ifattr[IFLA_LINKINFO]), ifattr[IFLA_LINKINFO]->rta_len); + else + memset(ifinfo,0,sizeof ifinfo); + if (ifinfo[IFLA_INFO_KIND]) + kind=rta_getattr_str(ifinfo[IFLA_INFO_KIND]); + else + kind=""; + if (!strcmp(kind,"eoip") && ifinfo[IFLA_INFO_DATA]) { + char ts[IFNAMSIZ],td[IFNAMSIZ]; + + parse_rtattr(ifgreo, IFLA_GRE_MAX, (void *)rta_getattr_str(ifinfo[IFLA_INFO_DATA]), ifinfo[IFLA_INFO_DATA]->rta_len); + if (ifgreo[IFLA_GRE_LINK]) + link=rta_getattr_u32(ifgreo[IFLA_GRE_LINK]); + if (ifgreo[IFLA_GRE_TOS]) + tos=rta_getattr_u8(ifgreo[IFLA_GRE_TOS]); + if (ifgreo[IFLA_GRE_TTL]) + ttl=rta_getattr_u8(ifgreo[IFLA_GRE_TTL]); + if (ifgreo[IFLA_GRE_LOCAL]) + sip.s_addr=rta_getattr_u32(ifgreo[IFLA_GRE_LOCAL]); + if (ifgreo[IFLA_GRE_REMOTE]) + dip.s_addr=rta_getattr_u32(ifgreo[IFLA_GRE_REMOTE]); + if (ifgreo[IFLA_GRE_IKEY]) + tunid=rta_getattr_u32(ifgreo[IFLA_GRE_IKEY]); + strcpy(ts,inet_ntoa(sip)); + strcpy(td,inet_ntoa(dip)); + printf("%d: %s@%d: link/%s %s remote %s tunnel-id %d ttl %d tos %d\n",ifi->ifi_index,ifname,link,kind,ts,td,tunid,ttl,tos); + } + + return 0; +} + +void list(void) { + struct rtnl_handle rth; + + if (rtnl_open(&rth,0)) { + perror("Cannot open rtnetlink"); + return; + } + if (rtnl_wilddump_request(&rth, AF_UNSPEC, RTM_GETLINK) < 0) { + perror("Cannot send dump request"); + return; + } + + if (rtnl_dump_filter(&rth, print_link, NULL) < 0) { + fprintf(stderr, "Dump terminated\n"); + return; + } + rtnl_close(&rth); +} + +int rtnetlink_request(struct nlmsghdr *msg, int buflen, struct sockaddr_nl *adr) { + int rsk; + int n; + + /* Use a private socket to avoid having to keep state for a sequence number. */ + rsk = socket(PF_NETLINK, SOCK_RAW, NETLINK_ROUTE); + if (rsk < 0) + return -1; + n = sendto(rsk, msg, msg->nlmsg_len, 0, (struct sockaddr *)adr, sizeof(struct sockaddr_nl)); + if (errno) + perror("in send"); + close(rsk); + if (n < 0) + return -1; + return 0; +} + +static int eoip_remove(char *name) { + unsigned long index = 0; + struct { + struct nlmsghdr msg; + struct ifinfomsg ifi; + uint32_t dummy[50]; + } req = { + .msg = { + .nlmsg_len = 0, // fix me later + .nlmsg_type = RTM_DELLINK, + .nlmsg_flags = NLM_F_REQUEST, + }, + .ifi = { + .ifi_family = AF_UNSPEC, + .ifi_index = 0, + } + }; + struct sockaddr_nl adr = { + .nl_family = AF_NETLINK, + }; + + req.msg.nlmsg_len=((char *)&req.dummy-(char *)&req); + + index = if_nametoindex(name); + + if( index == 0 ) { + const int err = errno; + fprintf(stderr, "unable to get interface %s index: %s", name, strerror(err)); + + return 3; + } else { + req.ifi.ifi_index = index; + + if (rtnetlink_request(&req.msg, sizeof req, &adr) < 0) { + perror("error in netlink request"); + return 2; + } + } + + return 0; +} + +static int eoip_add(int excl,char *name,uint16_t tunnelid,uint32_t sip,uint32_t dip,uint32_t link,uint8_t ttl,uint8_t tos) { + struct { + struct nlmsghdr msg; + struct ifinfomsg ifi; + struct rtattr a_name; + char ifname[IFNAMSIZ]; + struct rtattr a_lnfo; + struct rtattr a_kind; + char kind[8]; + struct rtattr a_data; + struct rtattr a_ikey; + uint32_t ikey; + struct rtattr a_sa; + uint32_t sa; + struct rtattr a_da; + uint32_t da; + struct rtattr a_link; + uint32_t link; + struct rtattr a_ttl; + uint8_t ttl; + uint8_t ttlpad[3]; + struct rtattr a_tos; + uint8_t tos; + uint8_t tospad[3]; + uint32_t dummy[50]; + } req = { + .msg = { + .nlmsg_len = 0, // fix me later + .nlmsg_type = RTM_NEWLINK, + .nlmsg_flags = NLM_F_REQUEST|NLM_F_CREATE|(excl ? NLM_F_EXCL : 0), + }, + .ifi = { + .ifi_family = AF_UNSPEC, + .ifi_index = 0, + }, + .a_name = { + .rta_len = IFNAMSIZ + sizeof(struct rtattr), + .rta_type = IFLA_IFNAME, + }, + .ifname="", + .a_lnfo = { + .rta_len = 0, // fix me later + .rta_type = IFLA_LINKINFO, + }, + .a_kind = { + .rta_len = 8 + sizeof(struct rtattr), + .rta_type = IFLA_INFO_KIND, + }, + .kind="eoip", + .a_data = { + .rta_len = 0, // fix me later + .rta_type = IFLA_INFO_DATA, + }, + .a_ikey = { + .rta_len = 4 + sizeof(struct rtattr), + .rta_type = IFLA_GRE_IKEY, + }, + .ikey=4321, + .a_sa = { + .rta_len = 4 + sizeof(struct rtattr), + .rta_type = IFLA_GRE_LOCAL, + }, + .sa=htonl(0), + .a_da = { + .rta_len = 4 + sizeof(struct rtattr), + .rta_type = IFLA_GRE_REMOTE, + }, + .da=htonl(0), + .a_link = { + .rta_len = 4 + sizeof(struct rtattr), + .rta_type = IFLA_GRE_LINK, + }, + .link=0, + .a_ttl = { + .rta_len = 4 + sizeof(struct rtattr), + .rta_type = IFLA_GRE_TTL, + }, + .ttl=0, + .a_tos = { + .rta_len = 4 + sizeof(struct rtattr), + .rta_type = IFLA_GRE_TOS, + }, + .tos=0, + }; + struct sockaddr_nl adr = { + .nl_family = AF_NETLINK, + }; + + req.msg.nlmsg_len=((char *)&req.dummy-(char *)&req); + + req.a_name.rta_len=(char *)&req.a_lnfo-(char *)&req.a_name; + req.a_lnfo.rta_len=(char *)&req.dummy-(char *)&req.a_lnfo; + req.a_kind.rta_len=(char *)&req.a_data-(char *)&req.a_kind; + req.a_data.rta_len=(char *)&req.dummy-(char *)&req.a_data; + req.a_ikey.rta_len=(char *)&req.a_sa-(char *)&req.a_ikey; + req.a_sa.rta_len=(char *)&req.a_da-(char *)&req.a_sa; + req.a_da.rta_len=(char *)&req.a_link-(char *)&req.a_da; + req.a_link.rta_len=(char *)&req.a_ttl-(char *)&req.a_link; + req.a_ttl.rta_len=(char *)&req.a_tos-(char *)&req.a_ttl; + req.a_tos.rta_len=(char *)&req.dummy-(char *)&req.a_tos; + + req.sa = sip; + req.da = dip; + strcpy(req.ifname, name); + req.ikey = tunnelid; + req.link = link; + req.ttl = ttl; + req.tos = tos; + if (0) { + unsigned char *p=(unsigned char *)&req; + int l=(char *)&req.dummy-(char *)&req; + int i; + + printf("req size: %d\n",req.msg.nlmsg_len); + printf("name size: %d\n",req.a_name.rta_len); + printf("lnfo size: %d\n",req.a_lnfo.rta_len); + printf("kind size: %d\n",req.a_kind.rta_len); + printf("data size: %d\n",req.a_data.rta_len); + printf("ikey size: %d\n",req.a_ikey.rta_len); + printf("sadr size: %d\n",req.a_sa.rta_len); + printf("dadr size: %d\n",req.a_da.rta_len); + + printf("packet size: %d, data dump:",l); + + for (i=0;icmd) { + char *ha = l->cmd; + char *ne = pcmd; + int clen = 0; + + while (*ha && *ne && *ha == *ne) + ha++, ne++, clen++; + + if (!*ne) { + if (clen && clen > len) { + cnt = 0; + len = clen; + match = l->cid; + } + if (clen && clen == len) + cnt++; + } + l++; + } + if (cnt == 1) + return match; + return cnt ? E_AMBIGUOUS : E_NONE; +} + +static void usage(char *me) { + printf("usage:\n" + "\t%s add [name ] tunnel-id [local ] remote [ttl ] [tos ] [link ]\n" + "\t%s set name tunnel-id [local ] remote [ttl ] [tos ] [link ]\n" + "\t%s remove name \n" + "\t%s list\n", + me,me,me,me); +} + +int main(int argc,char **argv) { + if (argc >= 1) { + e_cmd c = (argc == 1) ? C_LIST : find_cmd(cmds, argv[1]); + int excl = 1; + + switch (c) { + dafeutl: + default: + usage(argv[0]); + return 0; + case C_LIST: + if (argc > 2) + goto dafeutl; + list(); + return 0; + case C_SET: + excl = 0; + case C_ADD: { + char ifname[IFNAMSIZ + 1] = ""; + uint32_t sip = htonl(0), dip = htonl(0); + uint32_t tid = ~0; + uint8_t ttl=0, tos=0; + uint32_t link = 0; + int i=2; + + while (i < argc) { + e_cmd p = find_cmd(prms, argv[i]); + int noarg=!((i + 1) < argc); + struct in_addr iad; + + switch (p) { + default: + break; + case E_NONE: + printf("unknown option: %s\n", argv[i]); + return 0; + case E_AMBIGUOUS: + printf("option is ambiguous: %s\n", argv[i]); + return 0; + } + + if (noarg) { + printf("option: %s requires an argument\n", argv[i]); + return 0; + } + + switch (p) { + default: + printf("BUG!\n"); + return 0; + case P_NAME: + ifname[(sizeof ifname)-1] = 0; + strncpy(ifname,argv[i + 1], IFNAMSIZ); + break; + case P_TTL: + ttl=atoi(argv[i + 1]); + break; + case P_TOS: + tos=atoi(argv[i + 1]); + break; + case P_LINK: + // convert ifname to ifinidex, also support numeric arg + link=if_nametoindex(argv[i + 1]); + if (!link) + link=atoi(argv[i + 1]); + if (!link) { + printf("invald interface name/index: %s\n", argv[i + 1]); + return 0; + } + break; + case P_TUNNELID: + tid=atoi(argv[i + 1]); + break; + case P_LOCAL: + if (!inet_aton(argv[i + 1], &iad)) { + printf("invald ip address: %s\n", argv[i + 1]); + return 0; + } + sip = iad.s_addr; + break; + case P_REMOTE: + if (!inet_aton(argv[i + 1], &iad)) { + printf("invald ip address: %s\n", argv[i + 1]); + return 0; + } + dip = iad.s_addr; + break; + } + i += 2; + } + if (tid > 0xffff) { + if (tid == ~0) + printf("tunnel-id is mandatory parameter\n"); + else + printf("invalid tunnel-id value: %d\n",tid); + return 0; + } + // tunnel id is in host byte order, addresses are in net byte order + return eoip_add(excl,ifname,tid,sip,dip,link,ttl,tos); + } + case C_REMOVE: { + char ifname[IFNAMSIZ + 1] = ""; + int i=2; + + while (i < argc) { + e_cmd p = find_cmd(prms, argv[i]); + int noarg=!((i + 1) < argc); + + switch (p) { + default: + break; + case E_NONE: + printf("unknown option: %s\n", argv[i]); + return 0; + case E_AMBIGUOUS: + printf("option is ambiguous: %s\n", argv[i]); + return 0; + } + + if (noarg) { + printf("option: %s requires an argument\n", argv[i]); + return 0; + } + + switch (p) { + default: + printf("BUG!\n"); + return 0; + case P_NAME: + ifname[(sizeof ifname)-1] = 0; + strncpy(ifname,argv[i + 1], IFNAMSIZ); + break; + } + i += 2; + } + + return eoip_remove(ifname); + } + } + } + return 0; +} diff --git a/trunk/user/eoip-ctl/libnetlink.c b/trunk/user/eoip-ctl/libnetlink.c new file mode 100644 index 000000000..769203167 --- /dev/null +++ b/trunk/user/eoip-ctl/libnetlink.c @@ -0,0 +1,691 @@ +/* + * libnetlink.c RTnetlink service routines. + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version + * 2 of the License, or (at your option) any later version. + * + * Authors: Alexey Kuznetsov, + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "libnetlink.h" + +int rcvbuf = 1024 * 1024; + +void rtnl_close(struct rtnl_handle *rth) +{ + if (rth->fd >= 0) { + close(rth->fd); + rth->fd = -1; + } +} + +int rtnl_open_byproto(struct rtnl_handle *rth, unsigned subscriptions, + int protocol) +{ + socklen_t addr_len; + int sndbuf = 32768; + + memset(rth, 0, sizeof(*rth)); + + rth->fd = socket(AF_NETLINK, SOCK_RAW, protocol); + if (rth->fd < 0) { + perror("Cannot open netlink socket"); + return -1; + } + + if (setsockopt(rth->fd,SOL_SOCKET,SO_SNDBUF,&sndbuf,sizeof(sndbuf)) < 0) { + perror("SO_SNDBUF"); + return -1; + } + + if (setsockopt(rth->fd,SOL_SOCKET,SO_RCVBUF,&rcvbuf,sizeof(rcvbuf)) < 0) { + perror("SO_RCVBUF"); + return -1; + } + + memset(&rth->local, 0, sizeof(rth->local)); + rth->local.nl_family = AF_NETLINK; + rth->local.nl_groups = subscriptions; + + if (bind(rth->fd, (struct sockaddr*)&rth->local, sizeof(rth->local)) < 0) { + perror("Cannot bind netlink socket"); + return -1; + } + addr_len = sizeof(rth->local); + if (getsockname(rth->fd, (struct sockaddr*)&rth->local, &addr_len) < 0) { + perror("Cannot getsockname"); + return -1; + } + if (addr_len != sizeof(rth->local)) { + fprintf(stderr, "Wrong address length %d\n", addr_len); + return -1; + } + if (rth->local.nl_family != AF_NETLINK) { + fprintf(stderr, "Wrong address family %d\n", rth->local.nl_family); + return -1; + } + rth->seq = time(NULL); + return 0; +} + +int rtnl_open(struct rtnl_handle *rth, unsigned subscriptions) +{ + return rtnl_open_byproto(rth, subscriptions, NETLINK_ROUTE); +} + +int rtnl_wilddump_request(struct rtnl_handle *rth, int family, int type) +{ + struct { + struct nlmsghdr nlh; + struct rtgenmsg g; + __u16 align_rta; /* attribute has to be 32bit aligned */ + struct rtattr ext_req; + __u32 ext_filter_mask; + } req; + + memset(&req, 0, sizeof(req)); + req.nlh.nlmsg_len = sizeof(req); + req.nlh.nlmsg_type = type; + req.nlh.nlmsg_flags = NLM_F_DUMP|NLM_F_REQUEST; + req.nlh.nlmsg_pid = 0; + req.nlh.nlmsg_seq = rth->dump = ++rth->seq; + req.g.rtgen_family = family; + + req.ext_req.rta_type = IFLA_EXT_MASK; + req.ext_req.rta_len = RTA_LENGTH(sizeof(__u32)); + req.ext_filter_mask = RTEXT_FILTER_VF; + + return send(rth->fd, (void*)&req, sizeof(req), 0); +} + +int rtnl_send(struct rtnl_handle *rth, const void *buf, int len) +{ + return send(rth->fd, buf, len, 0); +} + +int rtnl_send_check(struct rtnl_handle *rth, const void *buf, int len) +{ + struct nlmsghdr *h; + int status; + char resp[1024]; + + status = send(rth->fd, buf, len, 0); + if (status < 0) + return status; + + /* Check for immediate errors */ + status = recv(rth->fd, resp, sizeof(resp), MSG_DONTWAIT|MSG_PEEK); + if (status < 0) { + if (errno == EAGAIN) + return 0; + return -1; + } + + for (h = (struct nlmsghdr *)resp; NLMSG_OK(h, status); + h = NLMSG_NEXT(h, status)) { + if (h->nlmsg_type == NLMSG_ERROR) { + struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h); + if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) + fprintf(stderr, "ERROR truncated\n"); + else + errno = -err->error; + return -1; + } + } + + return 0; +} + +int rtnl_dump_request(struct rtnl_handle *rth, int type, void *req, int len) +{ + struct nlmsghdr nlh; + struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK }; + struct iovec iov[2] = { + { .iov_base = &nlh, .iov_len = sizeof(nlh) }, + { .iov_base = req, .iov_len = len } + }; + struct msghdr msg = { + .msg_name = &nladdr, + .msg_namelen = sizeof(nladdr), + .msg_iov = iov, + .msg_iovlen = 2, + }; + + nlh.nlmsg_len = NLMSG_LENGTH(len); + nlh.nlmsg_type = type; + nlh.nlmsg_flags = NLM_F_DUMP|NLM_F_REQUEST; + nlh.nlmsg_pid = 0; + nlh.nlmsg_seq = rth->dump = ++rth->seq; + + return sendmsg(rth->fd, &msg, 0); +} + +int rtnl_dump_filter_l(struct rtnl_handle *rth, + const struct rtnl_dump_filter_arg *arg) +{ + struct sockaddr_nl nladdr; + struct iovec iov; + struct msghdr msg = { + .msg_name = &nladdr, + .msg_namelen = sizeof(nladdr), + .msg_iov = &iov, + .msg_iovlen = 1, + }; + char buf[16384]; + + iov.iov_base = buf; + while (1) { + int status; + const struct rtnl_dump_filter_arg *a; + int found_done = 0; + int msglen = 0; + + iov.iov_len = sizeof(buf); + status = recvmsg(rth->fd, &msg, 0); + + if (status < 0) { + if (errno == EINTR || errno == EAGAIN) + continue; + fprintf(stderr, "netlink receive error %s (%d)\n", + strerror(errno), errno); + return -1; + } + + if (status == 0) { + fprintf(stderr, "EOF on netlink\n"); + return -1; + } + + for (a = arg; a->filter; a++) { + struct nlmsghdr *h = (struct nlmsghdr*)buf; + msglen = status; + + while (NLMSG_OK(h, msglen)) { + int err; + + if (nladdr.nl_pid != 0 || + h->nlmsg_pid != rth->local.nl_pid || + h->nlmsg_seq != rth->dump) + goto skip_it; + + if (h->nlmsg_type == NLMSG_DONE) { + found_done = 1; + break; /* process next filter */ + } + if (h->nlmsg_type == NLMSG_ERROR) { + struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h); + if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) { + fprintf(stderr, + "ERROR truncated\n"); + } else { + errno = -err->error; + perror("RTNETLINK answers"); + } + return -1; + } + err = a->filter(&nladdr, h, a->arg1); + if (err < 0) + return err; + +skip_it: + h = NLMSG_NEXT(h, msglen); + } + } + + if (found_done) + return 0; + + if (msg.msg_flags & MSG_TRUNC) { + fprintf(stderr, "Message truncated\n"); + continue; + } + if (msglen) { + fprintf(stderr, "!!!Remnant of size %d\n", msglen); + exit(1); + } + } +} + +int rtnl_dump_filter(struct rtnl_handle *rth, + rtnl_filter_t filter, + void *arg1) +{ + const struct rtnl_dump_filter_arg a[2] = { + { .filter = filter, .arg1 = arg1, }, + { .filter = NULL, .arg1 = NULL, }, + }; + + return rtnl_dump_filter_l(rth, a); +} + +int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, pid_t peer, + unsigned groups, struct nlmsghdr *answer) +{ + int status; + unsigned seq; + struct nlmsghdr *h; + struct sockaddr_nl nladdr; + struct iovec iov = { + .iov_base = (void*) n, + .iov_len = n->nlmsg_len + }; + struct msghdr msg = { + .msg_name = &nladdr, + .msg_namelen = sizeof(nladdr), + .msg_iov = &iov, + .msg_iovlen = 1, + }; + char buf[16384]; + + memset(&nladdr, 0, sizeof(nladdr)); + nladdr.nl_family = AF_NETLINK; + nladdr.nl_pid = peer; + nladdr.nl_groups = groups; + + n->nlmsg_seq = seq = ++rtnl->seq; + + if (answer == NULL) + n->nlmsg_flags |= NLM_F_ACK; + + status = sendmsg(rtnl->fd, &msg, 0); + + if (status < 0) { + perror("Cannot talk to rtnetlink"); + return -1; + } + + memset(buf,0,sizeof(buf)); + + iov.iov_base = buf; + + while (1) { + iov.iov_len = sizeof(buf); + status = recvmsg(rtnl->fd, &msg, 0); + + if (status < 0) { + if (errno == EINTR || errno == EAGAIN) + continue; + fprintf(stderr, "netlink receive error %s (%d)\n", + strerror(errno), errno); + return -1; + } + if (status == 0) { + fprintf(stderr, "EOF on netlink\n"); + return -1; + } + if (msg.msg_namelen != sizeof(nladdr)) { + fprintf(stderr, "sender address length == %d\n", msg.msg_namelen); + exit(1); + } + for (h = (struct nlmsghdr*)buf; status >= sizeof(*h); ) { + int len = h->nlmsg_len; + int l = len - sizeof(*h); + + if (l < 0 || len>status) { + if (msg.msg_flags & MSG_TRUNC) { + fprintf(stderr, "Truncated message\n"); + return -1; + } + fprintf(stderr, "!!!malformed message: len=%d\n", len); + exit(1); + } + + if (nladdr.nl_pid != peer || + h->nlmsg_pid != rtnl->local.nl_pid || + h->nlmsg_seq != seq) { + /* Don't forget to skip that message. */ + status -= NLMSG_ALIGN(len); + h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len)); + continue; + } + + if (h->nlmsg_type == NLMSG_ERROR) { + struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h); + if (l < sizeof(struct nlmsgerr)) { + fprintf(stderr, "ERROR truncated\n"); + } else { + errno = -err->error; + if (errno == 0) { + if (answer) + memcpy(answer, h, h->nlmsg_len); + return 0; + } + perror("RTNETLINK answers"); + } + return -1; + } + if (answer) { + memcpy(answer, h, h->nlmsg_len); + return 0; + } + + fprintf(stderr, "Unexpected reply!!!\n"); + + status -= NLMSG_ALIGN(len); + h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len)); + } + if (msg.msg_flags & MSG_TRUNC) { + fprintf(stderr, "Message truncated\n"); + continue; + } + if (status) { + fprintf(stderr, "!!!Remnant of size %d\n", status); + exit(1); + } + } +} + +int rtnl_listen(struct rtnl_handle *rtnl, + rtnl_filter_t handler, + void *jarg) +{ + int status; + struct nlmsghdr *h; + struct sockaddr_nl nladdr; + struct iovec iov; + struct msghdr msg = { + .msg_name = &nladdr, + .msg_namelen = sizeof(nladdr), + .msg_iov = &iov, + .msg_iovlen = 1, + }; + char buf[8192]; + + memset(&nladdr, 0, sizeof(nladdr)); + nladdr.nl_family = AF_NETLINK; + nladdr.nl_pid = 0; + nladdr.nl_groups = 0; + + iov.iov_base = buf; + while (1) { + iov.iov_len = sizeof(buf); + status = recvmsg(rtnl->fd, &msg, 0); + + if (status < 0) { + if (errno == EINTR || errno == EAGAIN) + continue; + fprintf(stderr, "netlink receive error %s (%d)\n", + strerror(errno), errno); + if (errno == ENOBUFS) + continue; + return -1; + } + if (status == 0) { + fprintf(stderr, "EOF on netlink\n"); + return -1; + } + if (msg.msg_namelen != sizeof(nladdr)) { + fprintf(stderr, "Sender address length == %d\n", msg.msg_namelen); + exit(1); + } + for (h = (struct nlmsghdr*)buf; status >= sizeof(*h); ) { + int err; + int len = h->nlmsg_len; + int l = len - sizeof(*h); + + if (l<0 || len>status) { + if (msg.msg_flags & MSG_TRUNC) { + fprintf(stderr, "Truncated message\n"); + return -1; + } + fprintf(stderr, "!!!malformed message: len=%d\n", len); + exit(1); + } + + err = handler(&nladdr, h, jarg); + if (err < 0) + return err; + + status -= NLMSG_ALIGN(len); + h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len)); + } + if (msg.msg_flags & MSG_TRUNC) { + fprintf(stderr, "Message truncated\n"); + continue; + } + if (status) { + fprintf(stderr, "!!!Remnant of size %d\n", status); + exit(1); + } + } +} + +int rtnl_from_file(FILE *rtnl, rtnl_filter_t handler, + void *jarg) +{ + int status; + struct sockaddr_nl nladdr; + char buf[8192]; + struct nlmsghdr *h = (void*)buf; + + memset(&nladdr, 0, sizeof(nladdr)); + nladdr.nl_family = AF_NETLINK; + nladdr.nl_pid = 0; + nladdr.nl_groups = 0; + + while (1) { + int err, len; + int l; + + status = fread(&buf, 1, sizeof(*h), rtnl); + + if (status < 0) { + if (errno == EINTR) + continue; + perror("rtnl_from_file: fread"); + return -1; + } + if (status == 0) + return 0; + + len = h->nlmsg_len; + l = len - sizeof(*h); + + if (l<0 || len>sizeof(buf)) { + fprintf(stderr, "!!!malformed message: len=%d @%lu\n", + len, ftell(rtnl)); + return -1; + } + + status = fread(NLMSG_DATA(h), 1, NLMSG_ALIGN(l), rtnl); + + if (status < 0) { + perror("rtnl_from_file: fread"); + return -1; + } + if (status < l) { + fprintf(stderr, "rtnl-from_file: truncated message\n"); + return -1; + } + + err = handler(&nladdr, h, jarg); + if (err < 0) + return err; + } +} + +int addattr(struct nlmsghdr *n, int maxlen, int type) +{ + return addattr_l(n, maxlen, type, NULL, 0); +} + +int addattr8(struct nlmsghdr *n, int maxlen, int type, __u8 data) +{ + return addattr_l(n, maxlen, type, &data, sizeof(__u8)); +} + +int addattr16(struct nlmsghdr *n, int maxlen, int type, __u16 data) +{ + return addattr_l(n, maxlen, type, &data, sizeof(__u16)); +} + +int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data) +{ + return addattr_l(n, maxlen, type, &data, sizeof(__u32)); +} + +int addattr64(struct nlmsghdr *n, int maxlen, int type, __u64 data) +{ + return addattr_l(n, maxlen, type, &data, sizeof(__u64)); +} + +int addattrstrz(struct nlmsghdr *n, int maxlen, int type, const char *str) +{ + return addattr_l(n, maxlen, type, str, strlen(str)+1); +} + +int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data, + int alen) +{ + int len = RTA_LENGTH(alen); + struct rtattr *rta; + + if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) { + fprintf(stderr, "addattr_l ERROR: message exceeded bound of %d\n",maxlen); + return -1; + } + rta = NLMSG_TAIL(n); + rta->rta_type = type; + rta->rta_len = len; + memcpy(RTA_DATA(rta), data, alen); + n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len); + return 0; +} + +int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len) +{ + if (NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len) > maxlen) { + fprintf(stderr, "addraw_l ERROR: message exceeded bound of %d\n",maxlen); + return -1; + } + + memcpy(NLMSG_TAIL(n), data, len); + memset((void *) NLMSG_TAIL(n) + len, 0, NLMSG_ALIGN(len) - len); + n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len); + return 0; +} + +struct rtattr *addattr_nest(struct nlmsghdr *n, int maxlen, int type) +{ + struct rtattr *nest = NLMSG_TAIL(n); + + addattr_l(n, maxlen, type, NULL, 0); + return nest; +} + +int addattr_nest_end(struct nlmsghdr *n, struct rtattr *nest) +{ + nest->rta_len = (void *)NLMSG_TAIL(n) - (void *)nest; + return n->nlmsg_len; +} + +struct rtattr *addattr_nest_compat(struct nlmsghdr *n, int maxlen, int type, + const void *data, int len) +{ + struct rtattr *start = NLMSG_TAIL(n); + + addattr_l(n, maxlen, type, data, len); + addattr_nest(n, maxlen, type); + return start; +} + +int addattr_nest_compat_end(struct nlmsghdr *n, struct rtattr *start) +{ + struct rtattr *nest = (void *)start + NLMSG_ALIGN(start->rta_len); + + start->rta_len = (void *)NLMSG_TAIL(n) - (void *)start; + addattr_nest_end(n, nest); + return n->nlmsg_len; +} + +int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data) +{ + int len = RTA_LENGTH(4); + struct rtattr *subrta; + + if (RTA_ALIGN(rta->rta_len) + len > maxlen) { + fprintf(stderr,"rta_addattr32: Error! max allowed bound %d exceeded\n",maxlen); + return -1; + } + subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len)); + subrta->rta_type = type; + subrta->rta_len = len; + memcpy(RTA_DATA(subrta), &data, 4); + rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len; + return 0; +} + +int rta_addattr_l(struct rtattr *rta, int maxlen, int type, + const void *data, int alen) +{ + struct rtattr *subrta; + int len = RTA_LENGTH(alen); + + if (RTA_ALIGN(rta->rta_len) + RTA_ALIGN(len) > maxlen) { + fprintf(stderr,"rta_addattr_l: Error! max allowed bound %d exceeded\n",maxlen); + return -1; + } + subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len)); + subrta->rta_type = type; + subrta->rta_len = len; + memcpy(RTA_DATA(subrta), data, alen); + rta->rta_len = NLMSG_ALIGN(rta->rta_len) + RTA_ALIGN(len); + return 0; +} + +int parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len) +{ + memset(tb, 0, sizeof(struct rtattr *) * (max + 1)); + while (RTA_OK(rta, len)) { + if ((rta->rta_type <= max) && (!tb[rta->rta_type])) + tb[rta->rta_type] = rta; + rta = RTA_NEXT(rta,len); + } + //if (len) + // fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len, rta->rta_len); + return 0; +} + +int parse_rtattr_byindex(struct rtattr *tb[], int max, struct rtattr *rta, int len) +{ + int i = 0; + + memset(tb, 0, sizeof(struct rtattr *) * max); + while (RTA_OK(rta, len)) { + if (rta->rta_type <= max && i < max) + tb[i++] = rta; + rta = RTA_NEXT(rta,len); + } + if (len) + fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len, rta->rta_len); + return i; +} + +int __parse_rtattr_nested_compat(struct rtattr *tb[], int max, struct rtattr *rta, + int len) +{ + if (RTA_PAYLOAD(rta) < len) + return -1; + if (RTA_PAYLOAD(rta) >= RTA_ALIGN(len) + sizeof(struct rtattr)) { + rta = RTA_DATA(rta) + RTA_ALIGN(len); + return parse_rtattr_nested(tb, max, rta); + } + memset(tb, 0, sizeof(struct rtattr *) * (max + 1)); + return 0; +} diff --git a/trunk/user/eoip-ctl/libnetlink.h b/trunk/user/eoip-ctl/libnetlink.h new file mode 100644 index 000000000..81649afe5 --- /dev/null +++ b/trunk/user/eoip-ctl/libnetlink.h @@ -0,0 +1,138 @@ +#ifndef __LIBNETLINK_H__ +#define __LIBNETLINK_H__ 1 + +#include +#include +#include +#include +#include +#include +#include + +struct rtnl_handle +{ + int fd; + struct sockaddr_nl local; + struct sockaddr_nl peer; + __u32 seq; + __u32 dump; +}; + +extern int rcvbuf; + +extern int rtnl_open(struct rtnl_handle *rth, unsigned subscriptions); +extern int rtnl_open_byproto(struct rtnl_handle *rth, unsigned subscriptions, int protocol); +extern void rtnl_close(struct rtnl_handle *rth); +extern int rtnl_wilddump_request(struct rtnl_handle *rth, int fam, int type); +extern int rtnl_dump_request(struct rtnl_handle *rth, int type, void *req, int len); + +typedef int (*rtnl_filter_t)(const struct sockaddr_nl *, + struct nlmsghdr *n, void *); + +struct rtnl_dump_filter_arg +{ + rtnl_filter_t filter; + void *arg1; +}; + +extern int rtnl_dump_filter_l(struct rtnl_handle *rth, + const struct rtnl_dump_filter_arg *arg); +extern int rtnl_dump_filter(struct rtnl_handle *rth, rtnl_filter_t filter, + void *arg); +extern int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, pid_t peer, + unsigned groups, struct nlmsghdr *answer); +extern int rtnl_send(struct rtnl_handle *rth, const void *buf, int); +extern int rtnl_send_check(struct rtnl_handle *rth, const void *buf, int); + +extern int addattr(struct nlmsghdr *n, int maxlen, int type); +extern int addattr8(struct nlmsghdr *n, int maxlen, int type, __u8 data); +extern int addattr16(struct nlmsghdr *n, int maxlen, int type, __u16 data); +extern int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data); +extern int addattr64(struct nlmsghdr *n, int maxlen, int type, __u64 data); +extern int addattrstrz(struct nlmsghdr *n, int maxlen, int type, const char *data); + +extern int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data, int alen); +extern int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len); +extern struct rtattr *addattr_nest(struct nlmsghdr *n, int maxlen, int type); +extern int addattr_nest_end(struct nlmsghdr *n, struct rtattr *nest); +extern struct rtattr *addattr_nest_compat(struct nlmsghdr *n, int maxlen, int type, const void *data, int len); +extern int addattr_nest_compat_end(struct nlmsghdr *n, struct rtattr *nest); +extern int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data); +extern int rta_addattr_l(struct rtattr *rta, int maxlen, int type, const void *data, int alen); + +extern int parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len); +extern int parse_rtattr_byindex(struct rtattr *tb[], int max, struct rtattr *rta, int len); +extern int __parse_rtattr_nested_compat(struct rtattr *tb[], int max, struct rtattr *rta, int len); + +#define parse_rtattr_nested(tb, max, rta) \ + (parse_rtattr((tb), (max), RTA_DATA(rta), RTA_PAYLOAD(rta))) + +#define parse_rtattr_nested_compat(tb, max, rta, data, len) \ + ({ data = RTA_PAYLOAD(rta) >= len ? RTA_DATA(rta) : NULL; \ + __parse_rtattr_nested_compat(tb, max, rta, len); }) + +static inline __u8 rta_getattr_u8(const struct rtattr *rta) +{ + return *(__u8 *)RTA_DATA(rta); +} +static inline __u16 rta_getattr_u16(const struct rtattr *rta) +{ + return *(__u16 *)RTA_DATA(rta); +} +static inline __u32 rta_getattr_u32(const struct rtattr *rta) +{ + return *(__u32 *)RTA_DATA(rta); +} +static inline __u64 rta_getattr_u64(const struct rtattr *rta) +{ + __u64 tmp; + memcpy(&tmp, RTA_DATA(rta), sizeof(__u64)); + return tmp; +} +static inline const char *rta_getattr_str(const struct rtattr *rta) +{ + return (const char *)RTA_DATA(rta); +} + +extern int rtnl_listen(struct rtnl_handle *, rtnl_filter_t handler, + void *jarg); +extern int rtnl_from_file(FILE *, rtnl_filter_t handler, + void *jarg); + +#define NLMSG_TAIL(nmsg) \ + ((struct rtattr *) (((void *) (nmsg)) + NLMSG_ALIGN((nmsg)->nlmsg_len))) + +#ifndef IFA_RTA +#define IFA_RTA(r) \ + ((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct ifaddrmsg)))) +#endif +#ifndef IFA_PAYLOAD +#define IFA_PAYLOAD(n) NLMSG_PAYLOAD(n,sizeof(struct ifaddrmsg)) +#endif + +#ifndef IFLA_RTA +#define IFLA_RTA(r) \ + ((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct ifinfomsg)))) +#endif +#ifndef IFLA_PAYLOAD +#define IFLA_PAYLOAD(n) NLMSG_PAYLOAD(n,sizeof(struct ifinfomsg)) +#endif + +#ifndef NDA_RTA +#define NDA_RTA(r) \ + ((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct ndmsg)))) +#endif +#ifndef NDA_PAYLOAD +#define NDA_PAYLOAD(n) NLMSG_PAYLOAD(n,sizeof(struct ndmsg)) +#endif + +#ifndef NDTA_RTA +#define NDTA_RTA(r) \ + ((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct ndtmsg)))) +#endif +#ifndef NDTA_PAYLOAD +#define NDTA_PAYLOAD(n) NLMSG_PAYLOAD(n,sizeof(struct ndtmsg)) +#endif + +#endif /* __LIBNETLINK_H__ */ +