user/wsc_upnp/wsc_netlink.c

   1 /*
   2         function handler for linux netlink socket!
   3  */
   4 #include <unistd.h>
   5 #include <fcntl.h>
   6 #include <pthread.h>
   7 #include <asm/types.h>
   8 #include <sys/socket.h>
   9 #include <linux/netlink.h>
  10 #include <linux/rtnetlink.h>
  11 #include "wsc_common.h"
  12 #include "wsc_netlink.h"
  13 #include "wsc_msg.h"
  14
  15 int we_version_compiled = WIRELESS_EXT;
  16 static int netlink_sock = -1;
  17
  18 static void wscEventHandler(char *data, int len)
  19 {
  20         struct iw_event iwe_buf, *iwe = &iwe_buf;
  21         char *pos, *end, *custom;
  22 //      char *buf;
  23
  24         pos = data;
  25         end = data + len;
  26
  27         while (pos + IW_EV_LCP_LEN <= end)
  28         {
  29                 /* Event data may be unaligned, so make a local, aligned copy
  30                  * before processing. */
  31                 memcpy(&iwe_buf, pos, IW_EV_LCP_LEN);
  32                 //DBGPRINTF(RT_DBG_INFO, "Wireless event: cmd=0x%x len=%d, we_version_compiled=%d!\n", iwe->cmd, iwe->len, we_version_compiled);
  33                 if (iwe->len <= IW_EV_LCP_LEN)
  34                         return;
  35
  36                 custom = pos + IW_EV_POINT_LEN;
  37                 if(iwe->cmd == IWEVCUSTOM)
  38                 {
  39                         if (we_version_compiled > 18)
  40                         {
  41                         /* WE-19 removed the pointer from struct iw_point */
  42                         char *dpos = (char *) &iwe_buf.u.data.length;
  43                         int dlen = dpos - (char *) &iwe_buf;
  44                         memcpy(dpos, pos + IW_EV_LCP_LEN, sizeof(struct iw_event) - dlen);
  45                 }
  46                 else
  47                 {
  48                         memcpy(&iwe_buf, pos, sizeof(struct iw_event));
  49                         }
  50
  51                         if (custom + iwe->u.data.length > end){
  52                                 DBGPRINTF(RT_DBG_INFO, "custom(0x%x) + iwe->u.data.length(0x%x) > end(0x%x)!\n", custom, iwe->u.data.length, end);
  53                                 return;
  54                         }
  55 #if 0
  56                         buf = malloc(iwe->u.data.length + 1);
  57                         if (buf == NULL)
  58                                 return;
  59
  60                         memcpy(buf, custom, iwe->u.data.length);
  61                         buf[iwe->u.data.length] = '\0';
  62 #endif
  63                         //DBGPRINTF(RT_DBG_INFO, "iwe->u.data.flags=0x%x!\n", iwe->u.data.flags);
  64                         switch(iwe->u.data.flags)
  65                         {
  66                                 case RT_ASSOC_EVENT_FLAG:
  67                                 case RT_DISASSOC_EVENT_FLAG:
  68                                 case RT_REQIE_EVENT_FLAG:
  69                                 case RT_RESPIE_EVENT_FLAG:
  70                                 case RT_ASSOCINFO_EVENT_FLAG:
  71                                 case RT_PMKIDCAND_FLAG:
  72                                         break;
  73                                 default:
  74                                         if(strncmp(custom, "RAWSCMSG", 8) == 0)
  75                                         {
  76                                                 DBGPRINTF(RT_DBG_INFO, "Recevive a RAWSCMSG segment\n");
  77                                                 WscRTMPMsgHandler(custom, iwe->u.data.length);
  78                                         }
  79                                         break;
  80                         }
  81 //                      free(buf);
  82                 }
  83                 pos += iwe->len;
  84         }
  85
  86 }
  87
  88
  89 static void wscNLEventRTMNewlinkHandle(struct nlmsghdr *nlmsgHdr, int len)
  90 {
  91         struct ifinfomsg *ifi;
  92         int attrlen, nlmsg_len, rta_len;
  93         struct rtattr * attr;
  94
  95     DBGPRINTF(RT_DBG_INFO, "%s", __FUNCTION__);
  96
  97         if (len < sizeof(struct ifinfomsg))
  98                 return;
  99
 100         ifi = NLMSG_DATA(nlmsgHdr);
 101     //wsc_hexdump("ifi: ", (char *)ifi, sizeof(struct ifinfomsg));
 102
 103         nlmsg_len = NLMSG_ALIGN(sizeof(struct ifinfomsg));
 104
 105         attrlen = nlmsgHdr->nlmsg_len - nlmsg_len;
 106 //      DBGPRINTF("attrlen=%d\n",attrlen);
 107         if (attrlen < 0)
 108                 return;
 109
 110         attr = (struct rtattr *) (((char *) ifi) + nlmsg_len);
 111     //wsc_hexdump("rtattr: ", (char *)attr,sizeof(struct rtattr));
 112         rta_len = RTA_ALIGN(sizeof(struct rtattr));
 113         //wsc_hexdump("rtattr: ", (char *)attr, rta_len);
 114         while (RTA_OK(attr, attrlen))
 115         {
 116                 if (attr->rta_type == IFLA_WIRELESS)
 117                 {
 118                         wscEventHandler(((char *) attr) + rta_len, attr->rta_len - rta_len);
 119                 }
 120                 attr = RTA_NEXT(attr, attrlen);
 121                 //wsc_hexdump("rta_type: ", (char *)attr,sizeof(struct rtattr));
 122         }
 123 }
 124
 125 void wscNLSockRecv(int sock)
 126 {
 127         char buf[8192];
 128         int left;
 129         struct sockaddr_nl from;
 130         socklen_t fromlen;
 131         struct nlmsghdr *nlmsgHdr;
 132
 133         fromlen = sizeof(from);
 134         left = recvfrom(sock, buf, sizeof(buf), MSG_DONTWAIT, (struct sockaddr *)&from, &fromlen);
 135
 136         if (left < 0)
 137         {
 138                 if (errno != EINTR && errno != EAGAIN)
 139                         perror("recvfrom(netlink)");
 140                 return;
 141         }
 142
 143         nlmsgHdr = (struct nlmsghdr *)buf;
 144         //wsc_hexdump("nlmsgHdr: ", (char *)nlmsgHdr, nlmsgHdr->nlmsg_len);
 145
 146         while (left >= sizeof(*nlmsgHdr))
 147         {
 148                 int len, plen;
 149
 150                 len = nlmsgHdr->nlmsg_len;
 151                 plen = len - sizeof(*nlmsgHdr);
 152                 if (len > left || plen < 0)
 153                 {
 154                         DBGPRINTF(RT_DBG_INFO, "Malformed netlink message: len=%d left=%d plen=%d", len, left, plen);
 155                         break;
 156                 }
 157
 158                 switch (nlmsgHdr->nlmsg_type)
 159                 {
 160                         case RTM_NEWLINK:
 161                                 wscNLEventRTMNewlinkHandle(nlmsgHdr, plen);
 162                                 break;
 163                 }
 164
 165                 len = NLMSG_ALIGN(len);
 166                 left -= len;
 167                 nlmsgHdr = (struct nlmsghdr *) ((char *) nlmsgHdr + len);
 168         }
 169
 170         if (left > 0)
 171         {
 172                 DBGPRINTF(RT_DBG_INFO, "%d extra bytes in the end of netlink message", left);
 173         }
 174
 175 }
 176
 177 /******************************************************************************
 178  * wscDevNLHandle
 179  *
 180  * Description:
 181  *       Function that receives netlink socket msg from the kernel
 182  *       during the lifetime of the device, and send to appropriate
 183  *       msg queue and trigger the callback handler.
 184  *
 185  * Parameters:
 186  *    None
 187  *
 188  * Return:
 189  *    None
 190  *****************************************************************************/
 191 void *wscDevNLHandle(void *args)
 192 {
 193         fd_set rfds;
 194         int retVal;
 195         int flags;
 196
 197         DBGPRINTF(RT_DBG_INFO, "Pthread(%s)Now waiting for the netlink socket incoming message!\n", __FUNCTION__);
 198
 199         flags = fcntl(netlink_sock, F_GETFL, 0);
 200         if (flags == -1)
 201         goto done;
 202     fcntl(netlink_sock, F_SETFL, flags | O_NONBLOCK);
 203
 204         while(!stopThread)
 205         {
 206                 FD_ZERO(&rfds);
 207                 FD_SET(netlink_sock, &rfds);
 208
 209         retVal = select(netlink_sock + 1, &rfds, NULL, NULL, NULL);
 210
 211                 /* Don¡¦t rely on the value of Wsc now! */
 212                 if (retVal == -1)
 213                         perror("select()");
 214         else if (retVal) {
 215                         DBGPRINTF(RT_DBG_INFO, "(%s):netlink socket data is available now.\n", __FUNCTION__);
 216                         if(FD_ISSET(netlink_sock, &rfds)){
 217                                 wscNLSockRecv(netlink_sock);
 218                         }
 219                 }
 220         }
 221
 222 done:
 223         if(netlink_sock >= 0)
 224                 close(netlink_sock);
 225
 226
 227         pthread_exit(NULL);
 228 }
 229
 230 /******************************************************************************
 231  * wscK2UModuleInit
 232  *
 233  * Description:
 234  *       The Kernel space 2 user space msg subsystem entry point.
 235  *        In Linux system, we use netlink socket to receive the specific type msg
 236  *        send from wireless driver.
 237  *               This function mainly create a posix thread and recvive msg then dispatch
 238  *        to coressponding handler.
 239  *
 240  * Parameters:
 241  *    None
 242  *
 243  * Return:
 244  *    success: 1
 245  *    fail   : 0
 246  *****************************************************************************/
 247 int wscK2UModuleInit(void)
 248 {
 249         int sock, retVal;
 250     struct sockaddr_nl local;
 251         pthread_t wscNLHandle_thread;
 252
 253         //Create netlink socket
 254         sock = socket(PF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
 255         if (sock < 0)
 256         {
 257                 perror("socket(PF_NETLINK,SOCK_RAW,NETLINK_ROUTE)");
 258                 return WSC_SYS_ERROR;
 259         }
 260
 261         memset(&local, 0, sizeof(local));
 262         local.nl_family = AF_NETLINK;
 263         local.nl_groups = RTMGRP_LINK;
 264
 265         if (bind(sock, (struct sockaddr *)&local, sizeof(local)) < 0)
 266         {
 267                 perror("bind(netlink)");
 268                 close(sock);
 269                 return WSC_SYS_ERROR;
 270         }
 271
 272     /*
 273         start a netlink socket receiver handle thread
 274     */
 275     DBGPRINTF(RT_DBG_INFO, "sock=%d!(0x%p)\n", sock, &sock);
 276         netlink_sock = sock;
 277         retVal = pthread_create(&wscNLHandle_thread, NULL, wscDevNLHandle, NULL);
 278         if(retVal == 0)
 279                 pthread_detach(wscNLHandle_thread);
 280
 281         return WSC_SYS_SUCCESS;
 282 }
 283