void netlink_multicast(void)
{
ssize_t len;
struct nlmsghdr *h;
int flags, newaddr = 0;
/* don't risk blocking reading netlink messages here. */
if ((flags = fcntl(daemon->netlinkfd, F_GETFL)) == -1 ||
fcntl(daemon->netlinkfd, F_SETFL, flags | O_NONBLOCK) == -1)
return;
if ((len = netlink_recv()) != -1)
for (h = (struct nlmsghdr *)iov.iov_base; NLMSG_OK(h, (size_t)len); h = NLMSG_NEXT(h, len))
if (nl_async(h) && option_bool(OPT_CLEVERBIND))
newaddr = 1;
/* restore non-blocking status */
fcntl(daemon->netlinkfd, F_SETFL, flags);
if (newaddr)
{
enumerate_interfaces();
create_bound_listeners(0);
}
}
void *tx_thread(void *arg)
{
int fd=*((int*)arg);
RT_TASK *task;
int ret;
int i;
char dummy_data[10];
if (fd > 0) {
ret = netlink_send(fd,NLCMD_INIT,10,&dummy_data[0]);
printf("tx_thread starting, fd %d\n",fd);
task = rt_task_init_schmod(nam2num("TASK1"), 0, 0, 0, SCHED_FIFO, 0xF);
mlockall(MCL_CURRENT | MCL_FUTURE);
// rt_make_hard_real_time();
while (!oai_exit) {
if (tx_sdu_active == 1)
printf("tx_thread: waiting (MBOX %d)\n",((unsigned int*)DAQ_MBOX)[0]);
while(((volatile int)tx_sdu_active) != 0) {
rt_sleep(nano2count(66666));
}
printf("tx_thread: calling netlink\n");
ret = netlink_recv(fd,rxsdu);
tx_sdu_active = 1;
tx_sdu_length = ret;
/*
if (ret > 0) {
printf("received TX SDU: ");
for (i=0;i<ret;i++) {
printf("%02hhx ",rxsdu[i]);
}
printf("\n");
}
*/
}
} else {
printf("tx_thread: no netlink\n");
}
printf("tx_thread exiting\n");
return(0);
}
static struct nlmsghdr *getNetlinkResponse(int p_socket, int *p_size, int *p_done)
{
size_t l_size = 4096;
void *l_buffer = NULL;
for(;;)
{
int l_read;
free(l_buffer);
l_buffer = malloc(l_size);
if (l_buffer == NULL)
{
return NULL;
}
l_read = netlink_recv(p_socket, l_buffer, l_size);
*p_size = l_read;
if(l_read == -2)
{
free(l_buffer);
return NULL;
}
if(l_read >= 0)
{
pid_t l_pid = getpid();
struct nlmsghdr *l_hdr;
for(l_hdr = (struct nlmsghdr *)l_buffer; NLMSG_OK(l_hdr, (unsigned int)l_read); l_hdr = (struct nlmsghdr *)NLMSG_NEXT(l_hdr, l_read))
{
if((pid_t)l_hdr->nlmsg_pid != l_pid || (int)l_hdr->nlmsg_seq != p_socket)
{
continue;
}
if(l_hdr->nlmsg_type == NLMSG_DONE)
{
*p_done = 1;
break;
}
if(l_hdr->nlmsg_type == NLMSG_ERROR)
{
free(l_buffer);
return NULL;
}
}
return l_buffer;
}
l_size *= 2;
}
}
void netlink_multicast(struct daemon *daemon)
{
ssize_t len;
struct nlmsghdr *h;
if ((len = netlink_recv(daemon)) != -1)
{
for (h = (struct nlmsghdr *)iov.iov_base; NLMSG_OK(h, (size_t)len); h = NLMSG_NEXT(h, len))
if (h->nlmsg_type == NLMSG_ERROR)
nl_err(h);
else
nl_routechange(daemon, h);
}
}
void netlink_multicast(void)
{
ssize_t len;
struct nlmsghdr *h;
int flags;
/* don't risk blocking reading netlink messages here. */
if ((flags = fcntl(daemon->netlinkfd, F_GETFL)) == -1 ||
fcntl(daemon->netlinkfd, F_SETFL, flags | O_NONBLOCK) == -1)
return;
if ((len = netlink_recv()) != -1)
for (h = (struct nlmsghdr *)iov.iov_base; NLMSG_OK(h, (size_t)len); h = NLMSG_NEXT(h, len))
nl_async(h);
/* restore non-blocking status */
fcntl(daemon->netlinkfd, F_SETFL, flags);
}
int main()
{
int ret, sock_fd;
struct sockaddr_nl src_addr;
/* New socket */
sock_fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_HACK_EXECVE);
if(-1 == sock_fd)
{
HACK_DEBUG(9, "error getting socket, %s", strerror(errno));
return -1;
}
/* Bind address */
memset(&src_addr, 0, sizeof(src_addr));
src_addr.nl_family = AF_NETLINK;
src_addr.nl_pid = getpid();
src_addr.nl_groups = 0;
ret = bind(sock_fd, (struct sockaddr*)&src_addr, sizeof(src_addr));
if(0 > ret)
{
HACK_DEBUG(9, "cannot bind socket, %s", strerror(errno));
close(sock_fd);
return -2;
}
int namelen = sizeof(src_addr);
ret = getsockname(sock_fd, (struct sockaddr *)&src_addr, &namelen);
if (0 < ret || namelen != sizeof(src_addr))
{
HACK_DEBUG(9, "cannot get socket name: %s", strerror(errno));
close(sock_fd);
return -3;
}
/* OK, let's go */
netlink_send_ready(sock_fd);
int client_sockfd = init_client_connect();
netlink_recv(sock_fd, &client_sockfd);
if (0 < client_sockfd) close(client_sockfd);
close(sock_fd);
return 0;
}
int main(int argc, char* argv[])
{
int fd;
if (argc == 2)
{
filefd = open(argv[1], O_WRONLY|O_APPEND | O_CREAT, S_IRUSR | S_IWUSR);
if (filefd < 0)
{
printf("open file %s: %s", argv[1], strerror(errno));
return -1;
}
}
else{
filefd = 1;
}
fd = netlink_bind(NETLINK_TCP_RECORD);
if(fd == -1){
printf("error getting socket: %s", strerror(errno));
return -1;
}
int rc;
rc = netlink_send(fd, "hi", 100);
if (rc < 0)
{
printf("get error sendmsg = %s\n",strerror(errno));
return -1;
}
printf("waiting received!\n");
netlink_recv(fd, msg_handler);
close(fd);
return 0;
}
int netlink_send_nlh(
struct proxy_dev *pdev,
struct nlmsghdr *nlh)
{
struct nlmsgerr *msgerr = NULL;
int rvalue = -1;
nlh->nlmsg_pid = pdev->pid;
/* send everything */
if (netlink_send(pdev, nlh) == -1) {
printf("netlink_send_nlh: failed to send message\n");
return -1; /* failure */
}
/* get confirmation of delivery */
if ((nlh = netlink_recv(pdev)) == NULL) {
printf("netlink_send_nlh: failed to receive confirmation\n");
return -1; /* failure */
}
/* check confirmation */
if ( nlh->nlmsg_type == NLMSG_ERROR ) {
msgerr = ((struct nlmsgerr*)NLMSG_DATA(nlh));
if (msgerr->error != 0) {
debug("delivery failure, msgerr->error = %d", msgerr->error);
goto err;
}
} else {
printf("netlink_send: next message was not confirmation!\n");
goto err;
}
rvalue = 0; /* success */
err:
free(nlh);
return rvalue;
}
int iface_enumerate(struct daemon *daemon, void *parm, int (*ipv4_callback)(), int (*ipv6_callback)())
{
struct sockaddr_nl addr;
struct nlmsghdr *h;
ssize_t len;
static unsigned int seq = 0;
int family = AF_INET;
struct {
struct nlmsghdr nlh;
struct rtgenmsg g;
} req;
addr.nl_family = AF_NETLINK;
addr.nl_pad = 0;
addr.nl_groups = 0;
addr.nl_pid = 0; /* address to kernel */
again:
req.nlh.nlmsg_len = sizeof(req);
req.nlh.nlmsg_type = RTM_GETADDR;
req.nlh.nlmsg_flags = NLM_F_ROOT | NLM_F_MATCH | NLM_F_REQUEST | NLM_F_ACK;
req.nlh.nlmsg_pid = 0;
req.nlh.nlmsg_seq = ++seq;
req.g.rtgen_family = family;
/* Don't block in recvfrom if send fails */
while((len = sendto(daemon->netlinkfd, (void *)&req, sizeof(req), 0,
(struct sockaddr *)&addr, sizeof(addr))) == -1 && retry_send());
if (len == -1)
return 0;
while (1)
{
if ((len = netlink_recv(daemon)) == -1)
return 0;
for (h = (struct nlmsghdr *)iov.iov_base; NLMSG_OK(h, (size_t)len); h = NLMSG_NEXT(h, len))
if (h->nlmsg_type == NLMSG_ERROR)
nl_err(h);
else if (h->nlmsg_seq != seq)
nl_routechange(daemon, h); /* May be multicast arriving async */
else if (h->nlmsg_type == NLMSG_DONE)
{
#ifdef HAVE_IPV6
if (family == AF_INET && ipv6_callback)
{
family = AF_INET6;
goto again;
}
#endif
return 1;
}
else if (h->nlmsg_type == RTM_NEWADDR)
{
struct ifaddrmsg *ifa = NLMSG_DATA(h);
struct rtattr *rta = IFA_RTA(ifa);
unsigned int len1 = h->nlmsg_len - NLMSG_LENGTH(sizeof(*ifa));
if (ifa->ifa_family == AF_INET)
{
struct in_addr netmask, addr, broadcast;
netmask.s_addr = htonl(0xffffffff << (32 - ifa->ifa_prefixlen));
addr.s_addr = 0;
broadcast.s_addr = 0;
while (RTA_OK(rta, len1))
{
if (rta->rta_type == IFA_LOCAL)
addr = *((struct in_addr *)(rta+1));
else if (rta->rta_type == IFA_BROADCAST)
broadcast = *((struct in_addr *)(rta+1));
rta = RTA_NEXT(rta, len1);
}
if (addr.s_addr && ipv4_callback)
if (!((*ipv4_callback)(daemon, addr, ifa->ifa_index, netmask, broadcast, parm)))
return 0;
}
#ifdef HAVE_IPV6
else if (ifa->ifa_family == AF_INET6)
{
struct in6_addr *addrp = NULL;
while (RTA_OK(rta, len1))
{
if (rta->rta_type == IFA_ADDRESS)
addrp = ((struct in6_addr *)(rta+1));
rta = RTA_NEXT(rta, len1);
}
if (addrp && ipv6_callback)
if (!((*ipv6_callback)(daemon, addrp, ifa->ifa_index, ifa->ifa_index, parm)))
return 0;
}
#endif
}
}
}
/* family = AF_UNSPEC finds ARP table entries.
family = AF_LOCAL finds MAC addresses. */
int iface_enumerate(int family, void *parm, int (*callback)())
{
struct sockaddr_nl addr;
struct nlmsghdr *h;
ssize_t len;
static unsigned int seq = 0;
int callback_ok = 1;
struct {
struct nlmsghdr nlh;
struct rtgenmsg g;
} req;
addr.nl_family = AF_NETLINK;
addr.nl_pad = 0;
addr.nl_groups = 0;
addr.nl_pid = 0; /* address to kernel */
again:
if (family == AF_UNSPEC)
req.nlh.nlmsg_type = RTM_GETNEIGH;
else if (family == AF_LOCAL)
req.nlh.nlmsg_type = RTM_GETLINK;
else
req.nlh.nlmsg_type = RTM_GETADDR;
req.nlh.nlmsg_len = sizeof(req);
req.nlh.nlmsg_flags = NLM_F_ROOT | NLM_F_MATCH | NLM_F_REQUEST | NLM_F_ACK;
req.nlh.nlmsg_pid = 0;
req.nlh.nlmsg_seq = ++seq;
req.g.rtgen_family = family;
/* Don't block in recvfrom if send fails */
while(retry_send(sendto(daemon->netlinkfd, (void *)&req, sizeof(req), 0,
(struct sockaddr *)&addr, sizeof(addr))));
if (errno != 0)
return 0;
while (1)
{
if ((len = netlink_recv()) == -1)
{
if (errno == ENOBUFS)
{
sleep(1);
goto again;
}
return 0;
}
for (h = (struct nlmsghdr *)iov.iov_base; NLMSG_OK(h, (size_t)len); h = NLMSG_NEXT(h, len))
if (h->nlmsg_pid != netlink_pid || h->nlmsg_type == NLMSG_ERROR)
{
/* May be multicast arriving async */
nl_async(h);
}
else if (h->nlmsg_seq != seq)
{
/* May be part of incomplete response to previous request after
ENOBUFS. Drop it. */
continue;
}
else if (h->nlmsg_type == NLMSG_DONE)
return callback_ok;
else if (h->nlmsg_type == RTM_NEWADDR && family != AF_UNSPEC && family != AF_LOCAL)
{
struct ifaddrmsg *ifa = NLMSG_DATA(h);
struct rtattr *rta = IFA_RTA(ifa);
unsigned int len1 = h->nlmsg_len - NLMSG_LENGTH(sizeof(*ifa));
if (ifa->ifa_family == family)
{
if (ifa->ifa_family == AF_INET)
{
struct in_addr netmask, addr, broadcast;
char *label = NULL;
netmask.s_addr = htonl(~(in_addr_t)0 << (32 - ifa->ifa_prefixlen));
addr.s_addr = 0;
broadcast.s_addr = 0;
while (RTA_OK(rta, len1))
{
if (rta->rta_type == IFA_LOCAL)
addr = *((struct in_addr *)(rta+1));
else if (rta->rta_type == IFA_BROADCAST)
broadcast = *((struct in_addr *)(rta+1));
else if (rta->rta_type == IFA_LABEL)
label = RTA_DATA(rta);
rta = RTA_NEXT(rta, len1);
}
if (addr.s_addr && callback_ok)
if (!((*callback)(addr, ifa->ifa_index, label, netmask, broadcast, parm)))
callback_ok = 0;
}
#ifdef HAVE_IPV6
else if (ifa->ifa_family == AF_INET6)
{
struct in6_addr *addrp = NULL;
u32 valid = 0, preferred = 0;
int flags = 0;
while (RTA_OK(rta, len1))
{
if (rta->rta_type == IFA_ADDRESS)
addrp = ((struct in6_addr *)(rta+1));
else if (rta->rta_type == IFA_CACHEINFO)
{
struct ifa_cacheinfo *ifc = (struct ifa_cacheinfo *)(rta+1);
preferred = ifc->ifa_prefered;
valid = ifc->ifa_valid;
}
rta = RTA_NEXT(rta, len1);
}
if (ifa->ifa_flags & IFA_F_TENTATIVE)
flags |= IFACE_TENTATIVE;
if (ifa->ifa_flags & IFA_F_DEPRECATED)
flags |= IFACE_DEPRECATED;
if (!(ifa->ifa_flags & IFA_F_TEMPORARY))
flags |= IFACE_PERMANENT;
if (addrp && callback_ok)
if (!((*callback)(addrp, (int)(ifa->ifa_prefixlen), (int)(ifa->ifa_scope),
(int)(ifa->ifa_index), flags,
(int) preferred, (int)valid, parm)))
callback_ok = 0;
}
#endif
}
}
else if (h->nlmsg_type == RTM_NEWNEIGH && family == AF_UNSPEC)
{
struct ndmsg *neigh = NLMSG_DATA(h);
struct rtattr *rta = NDA_RTA(neigh);
unsigned int len1 = h->nlmsg_len - NLMSG_LENGTH(sizeof(*neigh));
size_t maclen = 0;
char *inaddr = NULL, *mac = NULL;
while (RTA_OK(rta, len1))
{
if (rta->rta_type == NDA_DST)
inaddr = (char *)(rta+1);
else if (rta->rta_type == NDA_LLADDR)
{
maclen = rta->rta_len - sizeof(struct rtattr);
mac = (char *)(rta+1);
}
rta = RTA_NEXT(rta, len1);
}
if (!(neigh->ndm_state & (NUD_NOARP | NUD_INCOMPLETE | NUD_FAILED)) &&
inaddr && mac && callback_ok)
if (!((*callback)(neigh->ndm_family, inaddr, mac, maclen, parm)))
callback_ok = 0;
}
#ifdef HAVE_DHCP6
else if (h->nlmsg_type == RTM_NEWLINK && family == AF_LOCAL)
{
struct ifinfomsg *link = NLMSG_DATA(h);
struct rtattr *rta = IFLA_RTA(link);
unsigned int len1 = h->nlmsg_len - NLMSG_LENGTH(sizeof(*link));
char *mac = NULL;
size_t maclen = 0;
while (RTA_OK(rta, len1))
{
if (rta->rta_type == IFLA_ADDRESS)
{
maclen = rta->rta_len - sizeof(struct rtattr);
mac = (char *)(rta+1);
}
rta = RTA_NEXT(rta, len1);
}
if (mac && callback_ok && !((link->ifi_flags & (IFF_LOOPBACK | IFF_POINTOPOINT))) &&
!((*callback)((int)link->ifi_index, (unsigned int)link->ifi_type, mac, maclen, parm)))
callback_ok = 0;
}
#endif
}
}
/* family = AF_UNSPEC finds ARP table entries.
family = AF_LOCAL finds MAC addresses. */
int iface_enumerate(int family, void *parm, int (*callback)())
{
struct sockaddr_nl addr;
struct nlmsghdr *h;
ssize_t len;
static unsigned int seq = 0;
int callback_ok = 1, newaddr = 0;
struct {
struct nlmsghdr nlh;
struct rtgenmsg g;
} req;
addr.nl_family = AF_NETLINK;
addr.nl_pad = 0;
addr.nl_groups = 0;
addr.nl_pid = 0; /* address to kernel */
again:
if (family == AF_UNSPEC)
req.nlh.nlmsg_type = RTM_GETNEIGH;
else if (family == AF_LOCAL)
req.nlh.nlmsg_type = RTM_GETLINK;
else
req.nlh.nlmsg_type = RTM_GETADDR;
req.nlh.nlmsg_len = sizeof(req);
req.nlh.nlmsg_flags = NLM_F_ROOT | NLM_F_MATCH | NLM_F_REQUEST | NLM_F_ACK;
req.nlh.nlmsg_pid = 0;
req.nlh.nlmsg_seq = ++seq;
req.g.rtgen_family = family;
/* Don't block in recvfrom if send fails */
while((len = sendto(daemon->netlinkfd, (void *)&req, sizeof(req), 0,
(struct sockaddr *)&addr, sizeof(addr))) == -1 && retry_send());
if (len == -1)
return 0;
while (1)
{
if ((len = netlink_recv()) == -1)
{
if (errno == ENOBUFS)
{
sleep(1);
goto again;
}
return 0;
}
for (h = (struct nlmsghdr *)iov.iov_base; NLMSG_OK(h, (size_t)len); h = NLMSG_NEXT(h, len))
if (h->nlmsg_seq != seq || h->nlmsg_pid != netlink_pid || h->nlmsg_type == NLMSG_ERROR)
{
/* May be multicast arriving async */
if (nl_async(h) && option_bool(OPT_CLEVERBIND))
newaddr = 1;
}
else if (h->nlmsg_type == NLMSG_DONE)
{
/* handle async new interface address arrivals, these have to be done
after we complete as we're not re-entrant */
if (newaddr)
{
enumerate_interfaces();
create_bound_listeners(0);
}
return callback_ok;
}
else if (h->nlmsg_type == RTM_NEWADDR && family != AF_UNSPEC && family != AF_LOCAL)
{
struct ifaddrmsg *ifa = NLMSG_DATA(h);
struct rtattr *rta = IFA_RTA(ifa);
unsigned int len1 = h->nlmsg_len - NLMSG_LENGTH(sizeof(*ifa));
if (ifa->ifa_family == family)
{
if (ifa->ifa_family == AF_INET)
{
struct in_addr netmask, addr, broadcast;
netmask.s_addr = htonl(0xffffffff << (32 - ifa->ifa_prefixlen));
addr.s_addr = 0;
broadcast.s_addr = 0;
while (RTA_OK(rta, len1))
{
if (rta->rta_type == IFA_LOCAL)
addr = *((struct in_addr *)(rta+1));
else if (rta->rta_type == IFA_BROADCAST)
broadcast = *((struct in_addr *)(rta+1));
rta = RTA_NEXT(rta, len1);
}
if (addr.s_addr && callback_ok)
if (!((*callback)(addr, ifa->ifa_index, netmask, broadcast, parm)))
callback_ok = 0;
}
#ifdef HAVE_IPV6
else if (ifa->ifa_family == AF_INET6)
{
struct in6_addr *addrp = NULL;
while (RTA_OK(rta, len1))
{
if (rta->rta_type == IFA_ADDRESS)
addrp = ((struct in6_addr *)(rta+1));
rta = RTA_NEXT(rta, len1);
}
if (addrp && callback_ok)
if (!((*callback)(addrp, (int)(ifa->ifa_prefixlen), (int)(ifa->ifa_scope),
(int)(ifa->ifa_index), (int)(ifa->ifa_flags & IFA_F_TENTATIVE), parm)))
callback_ok = 0;
}
#endif
}
}
else if (h->nlmsg_type == RTM_NEWNEIGH && family == AF_UNSPEC)
{
struct ndmsg *neigh = NLMSG_DATA(h);
struct rtattr *rta = NDA_RTA(neigh);
unsigned int len1 = h->nlmsg_len - NLMSG_LENGTH(sizeof(*neigh));
size_t maclen = 0;
char *inaddr = NULL, *mac = NULL;
while (RTA_OK(rta, len1))
{
if (rta->rta_type == NDA_DST)
inaddr = (char *)(rta+1);
else if (rta->rta_type == NDA_LLADDR)
{
maclen = rta->rta_len - sizeof(struct rtattr);
mac = (char *)(rta+1);
}
rta = RTA_NEXT(rta, len1);
}
if (inaddr && mac && callback_ok)
if (!((*callback)(neigh->ndm_family, inaddr, mac, maclen, parm)))
callback_ok = 0;
}
#ifdef HAVE_DHCP6
else if (h->nlmsg_type == RTM_NEWLINK && family == AF_LOCAL)
{
struct ifinfomsg *link = NLMSG_DATA(h);
struct rtattr *rta = IFLA_RTA(link);
unsigned int len1 = h->nlmsg_len - NLMSG_LENGTH(sizeof(*link));
char *mac = NULL;
size_t maclen = 0;
while (RTA_OK(rta, len1))
{
if (rta->rta_type == IFLA_ADDRESS)
{
maclen = rta->rta_len - sizeof(struct rtattr);
mac = (char *)(rta+1);
}
rta = RTA_NEXT(rta, len1);
}
if (mac && callback_ok && !((link->ifi_flags & (IFF_LOOPBACK | IFF_POINTOPOINT))) &&
!((*callback)((int)link->ifi_index, (unsigned int)link->ifi_type, mac, maclen, parm)))
callback_ok = 0;
}
#endif
}
}
