host_netutil.c
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, 2010 Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <stdio.h>
00036 #include <stdlib.h>
00037 #include <string.h>
00038 
00039 #include <sys/ioctl.h>
00040 #include <netinet/in.h>
00041 #include <sys/time.h>
00042 #include <net/if.h>
00043 #include <unistd.h>
00044 #include <net/if_arp.h>
00045 
00046 #include "wge100_camera/list.h"
00047 #include "wge100_camera/host_netutil.h"
00048 
00049 
00061 int wge100ArpAdd(IpCamList *camInfo) {
00062   // Create new permanent mapping in the system ARP table
00063   struct arpreq arp;
00064   int s;
00065 
00066   // Create a dummy socket
00067   if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
00068     perror("wge100ArpAdd can't create socket");
00069     return -1;
00070   }
00071 
00072   wge100_debug("Registering ARP info for S/N %d \n", camInfo->serial);
00073 
00074   // Populate the arpreq structure for this mapping
00075   ((struct sockaddr_in*)&arp.arp_pa)->sin_family = AF_INET;
00076   memcpy(&((struct sockaddr_in*)&arp.arp_pa)->sin_addr, &camInfo->ip, sizeof(struct in_addr));
00077 
00078   // This is a 'permanent' mapping; it will not time out (but will be cleared by a reboot)
00079   arp.arp_flags = ATF_PERM;
00080 
00081   arp.arp_ha.sa_family = ARPHRD_ETHER;
00082   memcpy(&arp.arp_ha.sa_data, camInfo->mac, 6);
00083 
00084   strncpy(arp.arp_dev, camInfo->ifName, sizeof(arp.arp_dev));
00085 
00086   if( ioctl(s, SIOCSARP, &arp) == -1 ) {
00087     //perror("Warning, was unable to create ARP entry (are you root?)");
00088     close(s);
00089     return -1;
00090   } else {
00091     wge100_debug("Camera %u successfully configured\n", camInfo->serial);
00092   }
00093   return 0;
00094 }
00095 
00096 
00107 int wge100ArpDel(IpCamList *camInfo) {
00108   // Create new permanent mapping in the system ARP table
00109   struct arpreq arp;
00110   int s;
00111 
00112   // Create a dummy socket
00113   if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
00114     perror("wge100ArpDel can't create socket");
00115     return -1;
00116   }
00117 
00118   wge100_debug("Removing ARP info for S/N %d \n", camInfo->serial);
00119 
00120   // Populate the arpreq structure for this mapping
00121   ((struct sockaddr_in*)&arp.arp_pa)->sin_family = AF_INET;
00122   memcpy(&((struct sockaddr_in*)&arp.arp_pa)->sin_addr, &camInfo->ip, sizeof(struct in_addr));
00123 
00124   // No flags required for removal
00125   arp.arp_flags = 0;
00126 
00127   arp.arp_ha.sa_family = ARPHRD_ETHER;
00128   memcpy(&arp.arp_ha.sa_data, camInfo->mac, 6);
00129 
00130   strncpy(arp.arp_dev, camInfo->ifName, sizeof(arp.arp_dev));
00131 
00132   // Make the request to the kernel
00133   if( ioctl(s, SIOCDARP, &arp) == -1 ) {
00134     perror("Warning, was unable to remove ARP entry");
00135     close(s);
00136     return -1;
00137   } else {
00138     wge100_debug("Camera %u successfully removed from ARP table\n", camInfo->serial);
00139   }
00140 
00141   return 0;
00142 }
00143 
00144 
00154 int wge100EthGetLocalMac(const char *ifName, struct sockaddr *macAddr) {
00155   struct ifreq ifr;
00156   int s;
00157 
00158   // Create a dummy socket
00159   if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
00160     perror("wge100EthGetLocalMac can't create socket");
00161     return -1;
00162   }
00163 
00164   // Initialize the ifreq structure with the interface name
00165   strncpy(ifr.ifr_name,ifName,sizeof(ifr.ifr_name)-1);
00166   ifr.ifr_name[sizeof(ifr.ifr_name)-1]='\0';
00167 
00168   // Use socket ioctl to retrieve the HW (MAC) address of this interface
00169   if( ioctl(s, SIOCGIFHWADDR, &ifr) == -1 ) {
00170     fprintf(stderr, "On interface '%s': ", ifName);
00171     perror("wge100EthGetLocalMac ioctl failed");
00172     close(s);
00173     return -1;
00174   }
00175 
00176   // Transfer address from ifreq struct to output pointer
00177   memcpy(macAddr, &ifr.ifr_addr, sizeof(struct sockaddr));
00178 
00179   close(s);
00180   return 0;
00181 }
00182 
00183 
00193 int wge100IpGetLocalBcast(const char *ifName, struct in_addr *bcast) {
00194   struct ifreq ifr;
00195   int s;
00196 
00197   // Create a dummy socket
00198   if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
00199     perror("wge100IpGetLocalBcast can't create socket");
00200     return -1;
00201   }
00202 
00203   // Initialize the ifreq structure
00204   strncpy(ifr.ifr_name,ifName,sizeof(ifr.ifr_name)-1);
00205   ifr.ifr_name[sizeof(ifr.ifr_name)-1]='\0';
00206 
00207   // Use socket ioctl to get broadcast address for this interface
00208   if( ioctl(s,SIOCGIFBRDADDR , &ifr) == -1 ) {
00209     //perror("wge100IpGetLocalBcast ioctl failed");
00210     close(s);
00211     return -1;
00212   }
00213 
00214   // Requires some fancy casting because the IP portion of a sockaddr_in is stored
00215   // after the port (which we don't need) in the struct
00216   memcpy(&(bcast->s_addr), &((struct sockaddr_in *)(&ifr.ifr_broadaddr))->sin_addr, sizeof(struct in_addr));
00217 
00218   close(s);
00219   return 0;
00220 }
00221 
00231 int wge100IpGetLocalAddr(const char *ifName, struct in_addr *addr) {
00232   struct ifreq ifr;
00233   int s;
00234 
00235   // Create a dummy socket
00236   if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
00237     perror("wge100IpGetLocalAddr can't create socket");
00238     return -1;
00239   }
00240 
00241   // Initialize the ifreq structure
00242   strncpy(ifr.ifr_name,ifName,sizeof(ifr.ifr_name)-1);
00243   ifr.ifr_name[sizeof(ifr.ifr_name)-1]='\0';
00244 
00245   // Use socket ioctl to get the IP address for this interface
00246   if( ioctl(s,SIOCGIFADDR , &ifr) == -1 ) {
00247     //perror("wge100IpGetLocalAddr ioctl failed");
00248     close(s);
00249     return -1;
00250   }
00251 
00252   // Requires some fancy casting because the IP portion of a sockaddr_in after the port (which we don't need) in the struct
00253   memcpy(&(addr->s_addr), &((struct sockaddr_in *)(&ifr.ifr_broadaddr))->sin_addr, sizeof(struct in_addr));
00254   close(s);
00255 
00256   return 0;
00257 }
00258 
00259 
00269 int wge100IpGetLocalNetmask(const char *ifName, struct in_addr *addr) {
00270   struct ifreq ifr;
00271   int s;
00272 
00273   // Create a dummy socket
00274   if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
00275     perror("wge100IpGetLocalAddr can't create socket");
00276     return -1;
00277   }
00278 
00279   // Initialize the ifreq structure
00280   strncpy(ifr.ifr_name,ifName,sizeof(ifr.ifr_name)-1);
00281   ifr.ifr_name[sizeof(ifr.ifr_name)-1]='\0';
00282 
00283   // Use socket ioctl to get the netmask for this interface
00284   if( ioctl(s,SIOCGIFNETMASK , &ifr) == -1 ) {
00285     //perror("wge100IpGetLocalNetmask ioctl failed");
00286     close(s);
00287     return -1;
00288   }
00289 
00290   // Requires some fancy casting because the IP portion of a sockaddr_in after the port (which we don't need) in the struct
00291   memcpy(&(addr->s_addr), &((struct sockaddr_in *)(&ifr.ifr_broadaddr))->sin_addr, sizeof(struct in_addr));
00292   close(s);
00293 
00294   return 0;
00295 }
00296 
00297 
00306 int wge100SocketCreate(const struct in_addr *addr, uint16_t port) {
00307 
00308   // Create a UDP socket for communicating with the network
00309   int s;
00310   if ( (s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1 ) {
00311     perror("wge100SocketCreate can't create socket");
00312     return -1;
00313   }
00314 
00315   // Define the address we'll be listening on
00316   struct sockaddr_in si_host;
00317   memset( (uint8_t*)&si_host, 0, sizeof(si_host) );
00318   si_host.sin_family = AF_INET;                // This is an INET address
00319   memcpy(&si_host.sin_addr, addr, sizeof(struct in_addr));  // Bind only to this address
00320   si_host.sin_port = htons(port);                // If port==0,bind chooses an ephemeral source port
00321 
00322 
00323   // Bind the socket to the requested port
00324   if( bind(s, (struct sockaddr *)&si_host, sizeof(si_host)) == -1 ) {
00325     //perror("wge100SocketCreate unable to bind");
00326     close(s);
00327     return -1;
00328   }
00329 
00330   // Setting the broadcast flag allows us to transmit to the broadcast address
00331   // from this socket if we want to. Some platforms do not allow unprivileged
00332   // users to set this flag, but Linux does.
00333   int flags = 1;
00334   if( setsockopt(s, SOL_SOCKET,SO_BROADCAST, &flags, sizeof(flags)) == -1) {
00335     perror("wge100SocketCreate unable to set broadcast socket option");
00336     close(s);
00337     return -1;
00338   }
00339 
00340   return s;
00341 }
00342 
00343 
00353 int wge100SocketConnect(int s, const IPAddress *ip) {
00354 
00355   struct sockaddr_in camIP;
00356 
00358   // work until a few days ago (late july/early june 09). Now the
00359   // regression tests won't work without the hack, but real cameras work
00360   // just fine. Perhaps the simulated camera was being discovered on
00361   // another interface.
00362   if (*ip == 0x0100007F)
00363     return 0; 
00364 
00365   camIP.sin_family = AF_INET;
00366   camIP.sin_port = 0;      // Unused by connect
00367   camIP.sin_addr.s_addr=*ip;
00368 
00369   if( connect(s, (struct sockaddr *)&camIP, sizeof(camIP)) == -1 ) {
00370     perror("Could not connect datagram socket");
00371     close(s);
00372     return -1;
00373   }
00374 
00375   return 0;
00376 }
00377 
00378 
00387 int wge100CmdSocketCreate(const char *ifName, NetHost *localHost) {
00388   // Identify the IP address of the requested interface
00389   struct in_addr host_addr;
00390   wge100IpGetLocalAddr(ifName, &host_addr);
00391 
00392   // Create & bind a new listening socket, s
00393   // We specify a port of 0 to have bind choose an available host port
00394   int s;
00395   if( (s=wge100SocketCreate(&host_addr, 0)) == -1 ) {
00396     //perror("Unable to create socket");
00397     return -1;
00398   }
00399 
00400   if(localHost != NULL) {
00401     struct sockaddr_in socketAddr;
00402     socklen_t socketAddrSize = sizeof(socketAddr);
00403     if( getsockname(s, (struct sockaddr *)&socketAddr, &socketAddrSize) == -1) {
00404       perror("wge100SocketToNetHost Could not get socket name");
00405       close(s);
00406       return -1;
00407     }
00408 
00409     struct sockaddr macAddr;
00410     if( wge100EthGetLocalMac(ifName, &macAddr) == -1) {
00411       close(s);
00412       return -1;
00413     }
00414 
00415     memcpy(localHost->mac, macAddr.sa_data, sizeof(localHost->mac));
00416     localHost->addr = socketAddr.sin_addr.s_addr;
00417     localHost->port = socketAddr.sin_port;
00418   }
00419 
00420   return s;
00421 }
00422 
00435 int wge100SendUDP(int s, const IPAddress *ip, const void *data, size_t dataSize) {
00436   // Create and initialize a sockaddr_in structure to hold remote port & IP
00437   struct sockaddr_in si_cam;
00438   memset( (uint8_t *)&si_cam, 0, sizeof(si_cam) );
00439   si_cam.sin_family = AF_INET;
00440   si_cam.sin_port = htons(WG_CAMCMD_PORT);
00441   si_cam.sin_addr.s_addr = *ip;
00442 
00443   // Send the datagram
00444   if( sendto(s, data, dataSize, 0, (struct sockaddr*)&si_cam, sizeof(si_cam)) == -1 ) {
00445     perror("wge100SendUDP unable to send packet");
00446     close(s);
00447     return -1;
00448   }
00449   return 0;
00450 }
00451 
00464 int wge100SendUDPBcast(int s, const char *ifName, const void *data, size_t dataSize) {
00465   // Identify the broadcast address on the specified interface
00466   struct in_addr bcastIP;
00467   wge100IpGetLocalBcast(ifName, &bcastIP);
00468 
00469   // Use wge100SendUDP to send the broadcast packet
00470   return wge100SendUDP(s, &bcastIP.s_addr, data, dataSize);
00471 }
00472 
00473 
00493 int wge100WaitForPacket( int *s, int nums, uint32_t type, size_t pktLen, uint32_t *wait_us ) {
00494   int i;
00495   // Convert wait_us argument into a struct timeval
00496   struct timeval timeout;
00497   timeout.tv_sec = *wait_us / 1000000UL;
00498   timeout.tv_usec = *wait_us % 1000000UL;
00499 
00500   // We have been asked to wait wait_us microseconds for a response; compute the time
00501   // at which the timeout will expire and store it into "timeout"
00502   struct timeval timestarted;
00503   struct timeval timenow;
00504   gettimeofday(&timestarted, NULL);
00505   gettimeofday(&timenow, NULL);
00506   timeradd( &timeout, &timestarted, &timeout );
00507 
00508   struct timeval looptimeout;
00509   fd_set set;
00510   while( timercmp( &timeout, &timenow, >= ) ) {
00511     int maxs = 0;
00512     // Since we could receive multiple packets, we need to decrease the timeout
00513     // to select() as we go. (Multiple packets should be an unlikely event, but
00514     // UDP provides no guarantees)
00515     timersub(&timeout, &timestarted, &looptimeout);
00516 
00517     FD_ZERO(&set);
00518     for (i = 0; i < nums; i++)
00519     {
00520       FD_SET(s[i], &set);
00521       if (s[i] > maxs)
00522         maxs = s[i];
00523     }
00524 
00525     // Wait for either a packet to be received or for timeout
00526     if( select(maxs+1, &set, NULL, NULL, &looptimeout) == -1 ) {
00527       perror("wge100WaitForPacket select failed");
00528       return -1;
00529     }
00530 
00531     for (i = 0; i < nums; i++) {
00532       // If we received a packet
00533       if( FD_ISSET(s[i], &set) ) {
00534         PacketGeneric gPkt;
00535         int r;
00536         // Inspect the packet in the buffer without removing it
00537         if( (r=recvfrom( s[i], &gPkt, sizeof(PacketGeneric), MSG_PEEK|MSG_TRUNC, NULL, NULL ))  == -1 ) {
00538           perror("wge100WaitForPacket unable to receive from socket");
00539           return -1;
00540         }
00541   
00542         // All valid WG command packets have magic_no == WG_MAGIC NO
00543         // We also know the minimum packet size we're looking for
00544         // So we can drop short or invalid packets at this stage
00545         if( ((unsigned int) r < pktLen) ||
00546               gPkt.magic_no != htonl(WG_MAGIC_NO) ||
00547               gPkt.type != htonl(type) ) {
00548           wge100_debug("Dropping packet with magic #%08X, type 0x%02X (looking for 0x%02X), length %d (looking for %d)\n", ntohl(gPkt.magic_no), ntohl(gPkt.type), type, r, pktLen);
00549           // Pull it out of the buffer (we used MSG_PEEK before, so it's still in there)
00550           if( recvfrom( s[i], &gPkt, sizeof(PacketGeneric), 0, NULL, NULL ) == -1 ) {
00551             perror("wge100WaitForPacket unable to receive from socket");
00552             return -1;
00553           }
00554         } else {  // The packet appears to be valid and correct
00555           // Compute the amount of time left on the timeout in case the calling function
00556           // decides this is not the packet we're looking for
00557           struct timeval timeleft;
00558           gettimeofday(&timenow, NULL);
00559           timersub(&timeout, &timenow, &timeleft);
00560   
00561           if (timeleft.tv_sec < 0) 
00562             // Otherwise we risk returning a very large number.
00563             *wait_us = 0;
00564           else
00565             *wait_us = timeleft.tv_usec+timeleft.tv_sec*1000000UL;              
00566           return i;
00567         }
00568   
00569       }
00570     }
00571     gettimeofday(&timenow, NULL);
00572   } 
00573   // If we reach here, we've timed out
00574   *wait_us = 0;
00575   return 0;
00576 }


wge100_camera
Author(s): Blaise Gassend, Patrick Mihelich, Eric MacIntosh, David Palchak
autogenerated on Fri Jan 3 2014 12:16:00