00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
00063 struct arpreq arp;
00064 int s;
00065
00066
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
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
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
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
00109 struct arpreq arp;
00110 int s;
00111
00112
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
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
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
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
00159 if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
00160 perror("wge100EthGetLocalMac can't create socket");
00161 return -1;
00162 }
00163
00164
00165 strncpy(ifr.ifr_name,ifName,sizeof(ifr.ifr_name)-1);
00166 ifr.ifr_name[sizeof(ifr.ifr_name)-1]='\0';
00167
00168
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
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
00198 if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
00199 perror("wge100IpGetLocalBcast can't create socket");
00200 return -1;
00201 }
00202
00203
00204 strncpy(ifr.ifr_name,ifName,sizeof(ifr.ifr_name)-1);
00205 ifr.ifr_name[sizeof(ifr.ifr_name)-1]='\0';
00206
00207
00208 if( ioctl(s,SIOCGIFBRDADDR , &ifr) == -1 ) {
00209
00210 close(s);
00211 return -1;
00212 }
00213
00214
00215
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
00236 if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
00237 perror("wge100IpGetLocalAddr can't create socket");
00238 return -1;
00239 }
00240
00241
00242 strncpy(ifr.ifr_name,ifName,sizeof(ifr.ifr_name)-1);
00243 ifr.ifr_name[sizeof(ifr.ifr_name)-1]='\0';
00244
00245
00246 if( ioctl(s,SIOCGIFADDR , &ifr) == -1 ) {
00247
00248 close(s);
00249 return -1;
00250 }
00251
00252
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
00274 if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
00275 perror("wge100IpGetLocalAddr can't create socket");
00276 return -1;
00277 }
00278
00279
00280 strncpy(ifr.ifr_name,ifName,sizeof(ifr.ifr_name)-1);
00281 ifr.ifr_name[sizeof(ifr.ifr_name)-1]='\0';
00282
00283
00284 if( ioctl(s,SIOCGIFNETMASK , &ifr) == -1 ) {
00285
00286 close(s);
00287 return -1;
00288 }
00289
00290
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
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
00316 struct sockaddr_in si_host;
00317 memset( (uint8_t*)&si_host, 0, sizeof(si_host) );
00318 si_host.sin_family = AF_INET;
00319 memcpy(&si_host.sin_addr, addr, sizeof(struct in_addr));
00320 si_host.sin_port = htons(port);
00321
00322
00323
00324 if( bind(s, (struct sockaddr *)&si_host, sizeof(si_host)) == -1 ) {
00325
00326 close(s);
00327 return -1;
00328 }
00329
00330
00331
00332
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
00359
00360
00361
00362 if (*ip == 0x0100007F)
00363 return 0;
00364
00365 camIP.sin_family = AF_INET;
00366 camIP.sin_port = 0;
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
00389 struct in_addr host_addr;
00390 wge100IpGetLocalAddr(ifName, &host_addr);
00391
00392
00393
00394 int s;
00395 if( (s=wge100SocketCreate(&host_addr, 0)) == -1 ) {
00396
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
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
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
00466 struct in_addr bcastIP;
00467 wge100IpGetLocalBcast(ifName, &bcastIP);
00468
00469
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
00496 struct timeval timeout;
00497 timeout.tv_sec = *wait_us / 1000000UL;
00498 timeout.tv_usec = *wait_us % 1000000UL;
00499
00500
00501
00502 struct timeval timestarted;
00503 struct timeval timenow;
00504 gettimeofday(×tarted, NULL);
00505 gettimeofday(&timenow, NULL);
00506 timeradd( &timeout, ×tarted, &timeout );
00507
00508 struct timeval looptimeout;
00509 fd_set set;
00510 while( timercmp( &timeout, &timenow, >= ) ) {
00511 int maxs = 0;
00512
00513
00514
00515 timersub(&timeout, ×tarted, &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
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
00533 if( FD_ISSET(s[i], &set) ) {
00534 PacketGeneric gPkt;
00535 int r;
00536
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
00543
00544
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
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 {
00555
00556
00557 struct timeval timeleft;
00558 gettimeofday(&timenow, NULL);
00559 timersub(&timeout, &timenow, &timeleft);
00560
00561 if (timeleft.tv_sec < 0)
00562
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
00574 *wait_us = 0;
00575 return 0;
00576 }