vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Com/linux/vp_com_config_itf.c

   1 #include <VP_Os/vp_os_malloc.h>
   2 #include <VP_Os/vp_os_print.h>
   3
   4 #include <VP_Com/linux/vp_com_config_itf.h>
   5
   6 #include <string.h>
   7 #include <sys/socket.h>
   8 #include <sys/ioctl.h>
   9 #include <net/if.h>
  10 #include <netinet/in.h>
  11 #include <fcntl.h>
  12 #include <arpa/inet.h>
  13 #include <unistd.h>
  14
  15 /********************************************************************
  16  * Static function declarations
  17  *******************************************************************/
  18 static int vp_com_set_interface_flags( int skfd, const char *ifname, int isUp );
  19
  20 /********************************************************************
  21  * Static functions
  22  *******************************************************************/
  23 // taken from ifconfig
  24 static int vp_com_set_interface_flags( int skfd, const char *ifname, int isUp)
  25 {
  26         struct ifreq ifr;
  27
  28         vp_os_memset( &ifr, 0, sizeof(struct ifreq) );
  29         strncpy( ifr.ifr_name, ifname, IFNAMSIZ );
  30
  31         if( ioctl( skfd, SIOCGIFFLAGS, &ifr ) < 0 )
  32                 return -1;
  33
  34         strncpy( ifr.ifr_name, ifname, IFNAMSIZ );
  35
  36         if( isUp )
  37                 ifr.ifr_flags |= (IFF_UP | IFF_RUNNING);
  38         else
  39                 ifr.ifr_flags &= ~IFF_UP;
  40
  41         if( ioctl(skfd, SIOCSIFFLAGS, &ifr ) < 0 )
  42                 return -1;
  43
  44         return 0;
  45 }
  46
  47 static int vp_com_set_ip( int skfd, const char* ifname, int cmd, int ip )
  48 {
  49   struct ifreq ifr;
  50   struct sockaddr_in* addr;
  51   struct sockaddr_in local_sin;
  52
  53   vp_os_memset( &ifr, 0, sizeof(struct ifreq) );
  54   strncpy( ifr.ifr_name, ifname, IFNAMSIZ );
  55
  56   // Check if an ip is already set
  57   if( ioctl( skfd, SIOCGIFADDR, &ifr ) != -1 )
  58   {
  59     addr = (struct sockaddr_in*) &ifr.ifr_addr;
  60
  61     // Is this the same ip?
  62     if( addr->sin_family == AF_INET && ip == addr->sin_addr.s_addr )
  63       return 0;
  64 /*
  65     // No so we try to delete it
  66     if( ioctl( skfd, SIOCDIFADDR, &ifr ) < 0 )
  67     {
  68       DEBUG_PRINT_SDK("Unable to delete old ip address - You should remove your interface %s\n", ifname);
  69       return -1;
  70     }
  71 */
  72   }
  73
  74   vp_os_memset( &ifr, 0, sizeof(struct ifreq) );
  75   vp_os_memset( &local_sin, 0, sizeof(local_sin) );
  76   strncpy( ifr.ifr_name, ifname, IFNAMSIZ );
  77
  78   addr = (struct sockaddr_in*) &ifr.ifr_addr;
  79
  80   local_sin.sin_family      = AF_INET;
  81   local_sin.sin_addr.s_addr = ip;
  82   vp_os_memcpy(addr,&local_sin,sizeof(local_sin));
  83
  84   strncpy( ifr.ifr_name, ifname, IFNAMSIZ );
  85
  86   if( ioctl( skfd, cmd, &ifr ) < 0 )
  87     return -1;
  88
  89   return 0;
  90 }
  91
  92
  93 int vp_com_config_itf( const char* interface, const char * ip, const char* broadcast, const char* netmask )
  94 {
  95   int ret = -1;
  96
  97   int sck = socket( PF_INET, SOCK_DGRAM, 0 );
  98
  99   if( sck < 0 )
 100     return -1;
 101
 102   if( vp_com_set_interface_flags( sck, interface, 0 ) < 0 )
 103     goto vp_com_config_itf_end;
 104
 105   if( vp_com_set_ip( sck, interface, SIOCSIFADDR, inet_addr(ip) ) < 0 )
 106     goto vp_com_config_itf_end;
 107
 108   if( vp_com_set_ip( sck, interface, SIOCSIFBRDADDR, inet_addr(broadcast) ) < 0 )
 109     goto vp_com_config_itf_end;
 110
 111   if( vp_com_set_ip( sck, interface, SIOCSIFNETMASK, inet_addr(netmask) ) < 0 )
 112     goto vp_com_config_itf_end;
 113
 114   if( vp_com_set_interface_flags( sck, interface, 1 ) < 0 )
 115     goto vp_com_config_itf_end;
 116
 117   ret = 0;
 118
 119 vp_com_config_itf_end:
 120
 121   close(sck);
 122
 123   return ret;
 124 }