#include #include #include #include #include #include #include #include #include #include #include #include #include #include "libnetlinku.h" #include "commuapinl.h" #ifndef SOL_NETLINK #define SOL_NETLINK 270 #endif #ifndef NETLINK_EXT_ACK #define NETLINK_EXT_ACK 11 #endif #if 0 struct nlumsg_list { struct list_head list; struct nlmsghdr *n; }; #endif #define NETLINK_COMM_FAMILY_NUM (NETLINK_COMMMAX_MAX_FAMILY - NETLINK_COMMMAX_MIN_FAMILY -1) #define NETLINK_COMM_FAMILY_INDEX(family) (family - NETLINK_COMMMAX_MIN_FAMILY -1) struct upmnl_handle * gcommnl[NETLINK_COMM_FAMILY_NUM][COMMLIBNLGRP_MAX] = {0};/*包含单播和组播一起,组0是单播,1开始是组播*/ int rcvbuf = 1024 * 1024; struct upmnl_handle * commnl_get_handle(int protocol,unsigned int group) { struct upmnl_handle * handle = NULL; handle = gcommnl[NETLINK_COMM_FAMILY_INDEX(protocol)][group]; //printf("get handle proto %d,group%d,handle = %p.\r\n",NETLINK_COMM_FAMILY_INDEX(protocol),group,handle); return handle; } void commnl_del_handle(int protocol,unsigned int group) { gcommnl[NETLINK_COMM_FAMILY_INDEX(protocol)][group] = NULL; return; } int commnl_add_handle(struct upmnl_handle * handle,int protocol,unsigned int group) { if(commnl_get_handle(protocol,group) != NULL) { printf("handel already exist,proto:%d,group%d.\r\n",protocol,group); return -1; } //printf("add handle %p proto %d,group%d\r\n",handle,NETLINK_COMM_FAMILY_INDEX(protocol),group); gcommnl[NETLINK_COMM_FAMILY_INDEX(protocol)][group]= handle; return 0; } void printf_mem(unsigned char * p,int len) { int num = 0; for(num = 0; numnlmsg_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; if (alen) memcpy(RTA_DATA(rta), data, alen); n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len); return 0; } int commnl_addattr(struct nlmsghdr *n, int maxlen, int type) { return commnl_addattr_l(n, maxlen, type, NULL, 0); } int commnl_addattr8(struct nlmsghdr *n, int maxlen, int type, __u8 data) { return commnl_addattr_l(n, maxlen, type, &data, sizeof(__u8)); } int commnl_addattr16(struct nlmsghdr *n, int maxlen, int type, __u16 data) { return commnl_addattr_l(n, maxlen, type, &data, sizeof(__u16)); } int commnl_addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data) { return commnl_addattr_l(n, maxlen, type, &data, sizeof(__u32)); } int commnl_addattr64(struct nlmsghdr *n, int maxlen, int type, __u64 data) { return commnl_addattr_l(n, maxlen, type, &data, sizeof(__u64)); } int commnl_addattrstrz(struct nlmsghdr *n, int maxlen, int type, const char *str) { return commnl_addattr_l(n, maxlen, type, str, strlen(str)+1); } int commnl_open_byproto( unsigned int subscriptions,int protocol) { socklen_t addr_len; int sndbuf = 32768; int one = 1; struct upmnl_handle * upmh = NULL; struct timeval timeout={1,10000};//10ms printf("commnl_open_byproto begin:protocol=%d,subscriptions=%d\r\n",protocol,subscriptions); upmh = calloc(1,sizeof(struct upmnl_handle) ); if(upmh == NULL) { printf("commnl_new_handle calloc mem error protocol:%d,group:%d.\r\n",protocol,subscriptions); return -1; } upmh->local.nl_family = AF_NETLINK; upmh->local.nl_groups = subscriptions; upmh->local.nl_pid = getpid(); upmh->proto = protocol; upmh->fd = socket(AF_NETLINK, SOCK_RAW, protocol); if (upmh->fd < 0) { perror("Cannot open netlink socket"); goto ERR; } if (setsockopt(upmh->fd, SOL_SOCKET, SO_SNDBUF, &sndbuf, sizeof(sndbuf)) < 0) { perror("SO_SNDBUF"); goto ERR; } if (setsockopt(upmh->fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf, sizeof(rcvbuf)) < 0) { perror("SO_RCVBUF"); goto ERR; } if (setsockopt(upmh->fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) < 0) { perror("SO_RCVTIMEO"); goto ERR; } /* Older kernels may no support extended ACK reporting */ if (setsockopt(upmh->fd, SOL_NETLINK, NETLINK_EXT_ACK, &one, sizeof(one))){ perror("NETLINK_EXT_ACK"); goto ERR; } memset(&upmh->local, 0, sizeof(upmh->local)); upmh->local.nl_family = AF_NETLINK; upmh->local.nl_pid = getpid(); upmh->local.nl_groups = subscriptions; if (bind(upmh->fd, (struct sockaddr *)&(upmh->local), sizeof(upmh->local)) < 0) { perror("Cannot bind netlink socket"); //printf("Cannot bind netlink socket\r\n"); goto ERR; } addr_len = sizeof(upmh->local); if (getsockname(upmh->fd, (struct sockaddr *)&upmh->local, &addr_len) < 0) { perror("Cannot getsockname"); goto ERR; } if (addr_len != sizeof(upmh->local)) { fprintf(stderr, "Wrong address length %d\n", addr_len); goto ERR; } if (upmh->local.nl_family != AF_NETLINK) { fprintf(stderr, "Wrong address family %d\n", upmh->local.nl_family); goto ERR; } upmh->seq = time(NULL); commnl_add_handle(upmh, protocol, subscriptions); printf("commnl_open_byproto succes.\r\n"); return 0; ERR: if (upmh->fd >= 0) { close(upmh->fd); upmh->fd = -1; } if(NULL != upmh){ free(upmh); } commnl_del_handle(protocol, subscriptions); return -1; } void commnl_close( unsigned int subscriptions,int protocol) { struct upmnl_handle *upmh = commnl_get_handle(protocol,subscriptions); if (upmh->fd >= 0) { close(upmh->fd); upmh->fd = -1; } commnl_del_handle(upmh->proto, upmh->local.nl_groups); free(upmh); return; } int commcfgnl_open() { return commnl_open_byproto(0, NETLINK_COMMCFG); } void commcfgnl_close() { commnl_close(0,NETLINK_COMMCFG); return; } int pdelivnl_open() { return commnl_open_byproto(PDNLGRP_ALLRAW, NETLINK_PDELIVERY); } void pdelivnl_close() { commnl_close(PDNLGRP_ALLRAW,NETLINK_PDELIVERY); return; } int commnl_send(struct upmnl_handle *nl, struct nlmsghdr *n) { int ret = -1; //if(n->nlmsg_flags |NLM_F_ACK) ret = send(nl->fd, n, n->nlmsg_len, 0); } int commcfg_send(struct nlmsghdr *n) { struct upmnl_handle *nl = NULL; nl = commnl_get_handle(NETLINK_COMMCFG,0); if(nl == NULL) { printf("err:pdeliv_talk faid,pdeliv netlink has not open.\r\n"); return -1; } return(commnl_send(nl,n)); } int pdelivnl_send(int group,struct nlmsghdr *n) { struct upmnl_handle *nl = NULL; nl = commnl_get_handle(NETLINK_PDELIVERY,group); if(nl == NULL) { printf("err:pdeliv_talk faid,pdeliv netlink has not open.\r\n"); return -1; } return(commnl_send(nl,n)); } static int __commnl_recvmsg(int fd, struct msghdr *msg, int flags) { int len = 0; unsigned int num = COMMNL_TALKRCV_TIMEOUT;/*timeout*/ do { len = recvmsg(fd, msg, flags); #if 1 num --; if(num ==0) break; #endif printf("__rtnl_recvmsg return num %d.\r\n",num); } while (len < 0 && (errno == EINTR || errno == EAGAIN));/*超时*/ if (len < 0) { fprintf(stderr, "netlink receive error %s (%d)\n", strerror(errno), errno); return -errno; } if (len == 0) { fprintf(stderr, "EOF on netlink\n"); return -ENODATA; } return len; } static int commnl_recvmsg(int fd, struct msghdr *msg, char **answer) { struct iovec *iov = msg->msg_iov; char *buf; int len; struct nlmsghdr *recv_hdr; iov->iov_base = NULL; iov->iov_len = 0; len = __commnl_recvmsg(fd, msg, MSG_PEEK | MSG_TRUNC); if (len < 0) return len; if (len < 32768) len = 32768; buf = malloc(len); if (!buf) { fprintf(stderr, "malloc error: not enough buffer\n"); return -ENOMEM; } iov->iov_base = buf; iov->iov_len = len; len = __commnl_recvmsg(fd, msg, 0); if (len < 0) { free(buf); return len; } if (answer) *answer = buf; else free(buf); recv_hdr = (struct nlmsghdr *)(*answer); return len; } static int __commnl_talk_iov(struct upmnl_handle *rtnl, struct iovec *iov, size_t iovlen, struct nlmsghdr **answer, bool show_rtnl_err, nl_ext_ack_fn_t errfn) { struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK }; struct iovec riov; struct msghdr msg = { .msg_name = &nladdr, .msg_namelen = sizeof(nladdr), .msg_iov = iov, .msg_iovlen = iovlen, }; unsigned int seq = 0; struct nlmsghdr *h; int i, status; char *buf; for (i = 0; i < iovlen; i++) { h = iov[i].iov_base; h->nlmsg_seq = seq = ++rtnl->seq; if (answer == NULL) h->nlmsg_flags |= NLM_F_ACK; } status = sendmsg(rtnl->fd, &msg, 0); if (status < 0) { perror("Cannot talk to rtnetlink"); return -1; } else{ printf("__commnl_talk_iov send msg sucess.\r\n"); } /* change msg to use the response iov */ msg.msg_iov = &riov; msg.msg_iovlen = 1; i = 0; while (1) { next: printf("talk recvmsg-ing...\r\n"); status = commnl_recvmsg(rtnl->fd, &msg, &buf); ++i; if (status < 0) { printf("__commnl_talk_iov: commnl_recvmsg return error=%d...\r\n",status); return status; } 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"); free(buf); return -1; } fprintf(stderr, "!!!malformed message: len=%d\n", len); exit(1); } /*单播消息,pid必须正确*/ if (nladdr.nl_pid != 0 || h->nlmsg_pid != rtnl->local.nl_pid || h->nlmsg_seq > seq || h->nlmsg_seq < seq - iovlen) { /* Don't forget to skip that message. */ status -= NLMSG_ALIGN(len); h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len)); printf("__commnl_talk_iov recv a msg,but not to me:h->nlmsg_pid=%d,local_pid=%d,h->nlmsg_seq=%d,seq=%d.\r\n", h->nlmsg_pid,rtnl->local.nl_pid,h->nlmsg_seq,seq); continue; } if (h->nlmsg_type == NLMSG_ERROR) { printf("nltalk recv a ack msg.\r\n"); struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(h); int error = err->error; if (l < sizeof(struct nlmsgerr)) { fprintf(stderr, "ERROR truncated\n"); free(buf); return -1; } if (!error) { /* check messages from kernel */ //nl_dump_ext_ack(h, errfn); } else { errno = -error; //if (rtnl->proto != NETLINK_SOCK_DIAG && show_rtnl_err) //rtnl_talk_error(h, err, errfn); } if (answer) *answer = (struct nlmsghdr *)buf; else free(buf); if (i < iovlen) goto next; return error ? -i : 0; } if (answer) { *answer = (struct nlmsghdr *)buf; return 0; } fprintf(stderr, "Unexpected reply!!!\n"); status -= NLMSG_ALIGN(len); h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len)); printf("goto next nlmsghdr"); } free(buf); if (msg.msg_flags & MSG_TRUNC) { fprintf(stderr, "Message truncated\n"); continue; } if (status) { fprintf(stderr, "!!!Remnant of size %d\n", status); exit(1); } } } static int __commnl_talk(struct upmnl_handle *nl, struct nlmsghdr *n, struct nlmsghdr **answer, bool show_rtnl_err, nl_ext_ack_fn_t errfn) { struct iovec iov = { .iov_base = n, .iov_len = n->nlmsg_len }; return __commnl_talk_iov(nl, &iov, 1, answer, show_rtnl_err, errfn); } int commnl_talk(struct upmnl_handle *nl, struct nlmsghdr *n, struct nlmsghdr **answer) { return __commnl_talk(nl, n, answer, true, NULL); } int pdeliv_talk(int group,struct nlmsghdr *n, struct nlmsghdr **answer) { struct upmnl_handle *nl = NULL; nl = commnl_get_handle(NETLINK_PDELIVERY,group); if(nl == NULL) { printf("pdeliv_talk faid,pdeliv netlink has not open.\r\n"); return -1; } return(commnl_talk(nl,n,answer)); } int commcfg_talk(struct nlmsghdr *n, struct nlmsghdr **answer) { struct upmnl_handle *nl = NULL; nl = commnl_get_handle(NETLINK_COMMCFG,0); if(nl == NULL) { printf("commcfg_talk faid,commcfg netlink has not open.\r\n"); return -1; } return(commnl_talk(nl,n,answer)); } int pdelivnl_listen(int group, pdelivnl_listen_filter_t handler, void *jarg) { struct upmnl_handle *nl = NULL; printf("pdelivnl_listen:.\r\n"); nl = commnl_get_handle(NETLINK_PDELIVERY,group); if(nl == NULL) { printf("pdelivnl_listen faid,pdeliv netlink has not open.\r\n"); return -1; } return(conmnl_listen(nl,handler,jarg)); } int conmnl_listen(struct upmnl_handle *nl, pdelivnl_listen_filter_t handler, void *jarg) { int status; struct nlmsghdr *h; struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK }; struct iovec iov; struct msghdr msg = { .msg_name = &nladdr, .msg_namelen = sizeof(nladdr), .msg_iov = &iov, .msg_iovlen = 1, }; char buf[16384]; char cmsgbuf[BUFSIZ]; printf("conmnl_listen begin.\r\n"); #if 0 if (nl->flags & RTNL_HANDLE_F_LISTEN_ALL_NSID) { msg.msg_control = &cmsgbuf; msg.msg_controllen = sizeof(cmsgbuf); } #endif iov.iov_base = buf; while (1) { struct pdelivnl_ctrl_data ctrl; struct cmsghdr *cmsg; printf("pdelivnl_listen recvmsg....\r\n"); iov.iov_len = sizeof(buf); //status = recvmsg(nl->fd, &msg, MSG_WAITALL); status = recvmsg(nl->fd, &msg, MSG_WAITFORONE); 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); } #if 0 if (nl->flags & RTNL_HANDLE_F_LISTEN_ALL_NSID) { memset(&ctrl, 0, sizeof(ctrl)); ctrl.nsid = -1; for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) if (cmsg->cmsg_level == SOL_NETLINK && cmsg->cmsg_type == NETLINK_LISTEN_ALL_NSID && cmsg->cmsg_len == CMSG_LEN(sizeof(int))) { int *data = (int *)CMSG_DATA(cmsg); ctrl.nsid = *data; } } #endif #if 1 printf("pdelivnl_listen prase msg:\r\n"); 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(&ctrl, h, jarg); if (err < 0) return err; status -= NLMSG_ALIGN(len); h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len)); } #endif //if (msg.msg_flags & MSG_TRUNC) { //fprintf(stderr, "Message truncated\n"); //continue; //} if (status) { fprintf(stderr, "!!!Remnant of size %d\n", status); exit(1); } } } #if 0 int upnl_dump_type(struct upmnl_handle *upmh, unsigned int type) { struct { struct nlmsghdr nlh; //struct rtgenmsg g; } req; struct sockaddr_nl nladdr; memset(&nladdr, 0, sizeof(nladdr)); memset(&req, 0, sizeof(req)); nladdr.nl_family = AF_NETLINK; req.nlh.nlmsg_len = sizeof(req); req.nlh.nlmsg_type = type; req.nlh.nlmsg_flags = NLM_F_ROOT|NLM_F_MATCH|NLM_F_REQUEST; req.nlh.nlmsg_pid = 0; //req.nlh.nlmsg_seq = upmh->rtnl_dump = ++(upmh->rtnl_seq); req.nlh.nlmsg_seq = 1; //req.g.rtgen_family = AF_INET; return sendto(upmh->fd, &req, sizeof(req), 0, (struct sockaddr*)&nladdr, sizeof(nladdr)); } #endif int pdeliv_main(pdelivnl_listen_filter_t process_pkt) { int ret = -1; printf("pdeliv_main begin:\r\n"); /*创建通道*/ ret = pdelivnl_open(); if(ret < 0) { printf("pdelivnl_open fail,exit.\r\n"); return -1; } /*接收报文*/ pdelivnl_listen(PDNLGRP_ALLRAW,process_pkt,NULL); /*关闭netlink通道*/ pdelivnl_close(); printf("pdeliv_main exit!\r\n"); return 0; } void nl_debugfs(struct nlmsghdr *n) { struct netlink_debugfs *debugfs_total =NULL; struct netlink_debugfs *debugfs_msgtype =NULL; int msg_len = 0; debugfs_total = (struct netlink_debugfs *)NLMSG_DATA(n);/*msg data*/ msg_len = (n)->nlmsg_len - NLMSG_HDRLEN; /*msg_len*/ debugfs_msgtype = debugfs_total+1; printf("netlink total msg stats:\r\n"); printf(" rev_total = %d\n",debugfs_total->rev_total); printf(" rev_cb_sucess = %d\n",debugfs_total->rev_cb_sucess); printf(" rev_cb_fail = %d\n",debugfs_total->rev_cb_fail); printf(" rev_drop_total = %d\n",debugfs_total->rev_drop_total); printf(" rev_drop_nodoit = %d\n",debugfs_total->rev_drop_nodoit); printf(" send_succ = %d\n",debugfs_total->send_succ); printf(" send_fail = %d\n",debugfs_total->send_fail); printf("netlink msg stats:\r\n"); printf(" rev_total = %d\n",debugfs_msgtype->rev_total); printf(" rev_cb_sucess = %d\n",debugfs_msgtype->rev_cb_sucess); printf(" rev_cb_fail = %d\n",debugfs_msgtype->rev_cb_fail); printf(" rev_drop_total = %d\n",debugfs_msgtype->rev_drop_total); printf(" rev_drop_nodoit = %d\n",debugfs_msgtype->rev_drop_nodoit); printf(" send_succ = %d\n",debugfs_msgtype->send_succ); printf(" send_fail = %d\n",debugfs_msgtype->send_fail); return; } void nl_debugfs_extack(struct nlmsghdr *n) { //struct netlink_debugfs *debugfs_statis =NULL; char * rcv_cookies = NULL; int msg_len = 0; struct nlmsgerr *errmsg = NULL; struct rtattr * cookies = NULL; int cookies_len = 0; char * ptr; printf("nl_debugfs_extack said:\r\nmsgtype=%d,msglen=%d\r\n",n->nlmsg_type,n->nlmsg_len); errmsg = (struct nlmsgerr *)NLMSG_DATA(n);/*msg data*/ msg_len = (n)->nlmsg_len - NLMSG_HDRLEN; /*msg_len*/ printf("ack result=%d,ack msgtype:%d\r\n",errmsg->error,errmsg->msg.nlmsg_type); if(errmsg->error == 0)/*ack result is SUCESS*/ { ptr = (char * )errmsg + sizeof(struct nlmsgerr); cookies = (struct rtattr *)ptr; //if(NLMSGERR_ATTR_COOKIE == cookies->rta_type) { cookies_len = cookies->rta_len; rcv_cookies = RTA_DATA(cookies); printf("nl_debugfs_extack rcv cookies:%s.\r\n",rcv_cookies); } } else{ printf("nl_debugfs kernel said fail,error =%d.\r\n",errmsg->error); } return; }