wge100_camera: wge100_camera: wge100lib.c Source 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 "wge100_camera/wge100lib.h"
00036 #include <stdio.h>
00037 #include <stdlib.h>
00038 #include <string.h>
00039 #include <sys/time.h>
00040 #include <errno.h>
00041 #include <unistd.h>
00042 #include <stdbool.h>
00043 #include <net/if.h>
00044 #include <ifaddrs.h>
00045 
00046 #include "wge100_camera/host_netutil.h"
00047 #include "wge100_camera/ipcam_packet.h"
00048 
00050 #define STD_REPLY_TIMEOUT SEC_TO_USEC(0.2) // Should be tens of ms at least or flash writes will fail.
00051 #define STOP_REPLY_TIMEOUT SEC_TO_USEC(0.001)
00052 
00059 int wge100libVersion() {
00060   return WGE100LIB_VERSION;
00061 }
00062 
00063 
00094 int wge100FindByUrl(const char *url, IpCamList *camera, unsigned wait_us, const char **errmsg)
00095 {
00096   static const char *badprefix = "Bad URL prefix, expected name://, serial:// or any://.";
00097   static const char *multiaddress = "Multiple @-prefixed camera addresses found.";
00098   static const char *multiinterface = "Multiple #-prefixed host interfaces found.";
00099   static const char *discoverfailed = "Discover failed. The specified address or interface may be bad.";
00100   static const char *badserial = "serial:// may only be followed by an integer.";
00101   static const char *badany = "Unexpected characters after any://.";
00102   static const char *badip = "@ must be followed by a valid IP address.";
00103   static const char *nomatch = "No cameras matched the URL.";
00104   static const char *multimatch = "More than one camera matched the URL.";
00105   static const char *illegalcase = "Illegal case, this is a bug.";
00106   static const char *nomem = "malloc failed";
00107   const char *name = "name://"; int namelen = strlen(name);
00108   const char *serial = "serial://"; int seriallen = strlen(serial);
00109   const char *any = "any://"; int anylen = strlen(any);
00110   char *idstart;
00111   char *curpos;
00112   char *address = NULL;
00113   char *interface = NULL;
00114   char *copy = malloc(strlen(url) + 1);
00115   int retval = -1;
00116   int camcount = 0;
00117   int mode = 0;
00118   unsigned int serialnum = -1;
00119   IpCamList camList;
00120   IpCamList *curEntry;
00121   
00122   wge100CamListInit(&camList); // Must happen before any potential failures.
00123 
00124   if (errmsg)
00125     *errmsg = illegalcase; // If we return an error
00126 
00127   if (!copy) // malloc is above.
00128   {
00129     *errmsg = nomem;
00130     return -1;
00131   }
00132   strcpy(copy, url);
00133 
00134   // Decypher the prefix.
00135   if (!strncmp(copy, name, namelen))
00136   {
00137     idstart = copy + namelen;
00138     mode = 1;
00139   }
00140   else if (!strncmp(copy, serial, seriallen))
00141   {
00142     idstart = copy + seriallen;
00143     mode = 2;
00144   }
00145   else if (!strncmp(copy, any, anylen))
00146   {
00147     idstart = copy + anylen;
00148     mode = 3;
00149   }
00150   else
00151   {
00152     if (errmsg)
00153       *errmsg = badprefix;
00154     goto bailout;
00155   }
00156 
00157   // Find any # or @ words.
00158   for (curpos = idstart; *curpos; curpos++)
00159   {
00160     if (*curpos == '@')
00161     {
00162       if (address)
00163       {
00164         if (errmsg)
00165           *errmsg = multiaddress;
00166         goto bailout;
00167       }
00168       address = curpos + 1;
00169     }
00170     else if (*curpos == '#')
00171     {
00172       if (interface)
00173       {
00174         if (errmsg)
00175           *errmsg = multiinterface;
00176         goto bailout;
00177       }
00178       interface = curpos + 1;
00179     }
00180     else
00181       continue; // Didn't find anything, don't terminate the string.
00182     *curpos = 0;
00183   }
00184   // Now idstart, address and interface are set.
00185 
00186   if (mode == 3 && *idstart)
00187   {
00188     if (errmsg)
00189       *errmsg = badany;
00190     goto bailout;
00191   }
00192 
00193   if (mode == 2) // serial:// convert the number to integer.
00194   {
00195     char *endpos;
00196     serialnum = strtol(idstart, &endpos, 10);
00197     if (*idstart == 0 || *endpos != 0)
00198     {
00199       if (errmsg)
00200         *errmsg = badserial;
00201       goto bailout;
00202     }
00203   }
00204 
00205   // Build up a list of cameras. Only ones that are on the specified
00206   // interface (if specified), and that can reply from the specified
00207   // address (if specified) will end up in the list.
00208   if (wge100Discover(interface, &camList, address, wait_us) == -1)
00209   {
00210     if (errmsg)
00211       *errmsg = discoverfailed;
00212     goto bailout;
00213   }
00214 
00215   // Now search through the list for a match.
00216   camcount = 0;
00217   list_for_each_entry(curEntry, &(camList.list), list)
00218   {
00219     if ((mode == 1 && strcmp(idstart, curEntry->cam_name) == 0) ||
00220         (mode == 2 && serialnum == curEntry->serial) ||
00221         mode == 3)
00222     {
00223       camcount++;
00224       if (camcount > 1)
00225       {
00226         if (errmsg)
00227           *errmsg = multimatch;
00228         goto bailout;
00229       }
00230       memcpy(camera, curEntry, sizeof(IpCamList));
00231       if (address) // If an address has been specified, we fill it into the camera info.
00232       {
00233         struct in_addr ip;
00234         if (inet_aton(address, &ip))
00235         {
00236           uint8_t *ipbytes = (uint8_t *) &ip.s_addr; // Reconstruct the address, we don't want any funny stuff with the user-provided string.
00237           snprintf(camera->ip_str, sizeof(camera->ip_str), "%i.%i.%i.%i", ipbytes[0], ipbytes[1], ipbytes[2], ipbytes[3]);
00238           camera->ip = ip.s_addr;
00239         }
00240         else
00241         {
00242           if (errmsg)
00243             *errmsg = badip;
00244           goto bailout;
00245         }
00246       }
00247     }
00248   }
00249 
00250   switch (camcount)
00251   {
00252     case 1: // Found exactly one camera, we're good!
00253       retval = 0;
00254       break;
00255 
00256     case 0: // Found no cameras
00257       if (errmsg)
00258         *errmsg = nomatch;
00259       break;
00260 
00261     default:
00262       if (errmsg)
00263         *errmsg = illegalcase;
00264       break;
00265   }
00266 
00267 bailout:
00268   wge100CamListDelAll(&camList);
00269   free(copy);
00270   return retval;
00271 }
00272 
00287 int wge100StatusWait( int s, uint32_t wait_us, uint32_t *type, uint32_t *code ) {
00288   if( wge100WaitForPacket(&s, 1, PKTT_STATUS, sizeof(PacketStatus), &wait_us) != -1 && (wait_us != 0) ) {
00289     PacketStatus sPkt;
00290     if( recvfrom( s, &sPkt, sizeof(PacketStatus), 0, NULL, NULL )  == -1 ) {
00291       perror("wge100StatusWait unable to receive from socket");
00292       *type = PKT_STATUST_ERROR;
00293       *code = PKT_ERROR_SYSERR;
00294       return -1;
00295     }
00296 
00297     *type = ntohl(sPkt.status_type);
00298     *code = ntohl(sPkt.status_code);
00299     return 0;
00300   }
00301 
00302   *type = PKT_STATUST_ERROR;
00303   *code = PKT_ERROR_TIMEOUT;
00304   return 0;
00305 }
00306 
00307 
00308 static void xormem(uint8_t *dst, uint8_t *src, size_t w)
00309 {
00310   while (w--)
00311     *dst++ ^= *src++;
00312 }
00313 
00314 static int wge100DiscoverSend(const char *ifName, const char *ipAddress)
00315 {
00316   // Create and initialize a new Discover packet
00317   PacketDiscover dPkt;
00318   dPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
00319   dPkt.hdr.type = htonl(PKTT_DISCOVER);
00320   strncpy(dPkt.hdr.hrt, "DISCOVER", sizeof(dPkt.hdr.hrt));
00321 
00322   /* Create a new socket bound to a local ephemeral port, and get our local connection
00323    * info so we can request a reply back to the same socket.
00324    */
00325   int s = wge100CmdSocketCreate(ifName, &dPkt.hdr.reply_to);
00326   if(s == -1) {
00327     //perror("Unable to create socket\n");
00328     return -1;
00329   }
00330   
00333 
00334   struct in_addr newIP;
00335   if (ipAddress) // An IP was specified
00336   {
00337     inet_aton(ipAddress, &newIP);
00338     dPkt.ip_addr = newIP.s_addr;
00339   }
00340   else 
00341   {
00342     struct in_addr localip;
00343     struct in_addr netmask;
00344     wge100IpGetLocalAddr(ifName, &localip);
00345     wge100IpGetLocalNetmask(ifName, &netmask);
00346     dPkt.ip_addr = localip.s_addr ^ netmask.s_addr ^ ~0;
00347   }
00348 
00349   if( wge100SendUDPBcast(s, ifName, &dPkt,sizeof(dPkt)) == -1) {
00350     perror("Unable to send broadcast\n");
00351   }
00352 
00353   /*// For the old API
00354   if( wge100SendUDPBcast(s, ifName, &dPkt,sizeof(dPkt)-sizeof(dPkt.ip_addr)) == -1) {
00355     perror("Unable to send broadcast\n");
00356   }*/
00357 
00358   return s;
00359 }
00360 
00372 int wge100Discover(const char *ifName, IpCamList *ipCamList, const char *ipAddress, unsigned wait_us) {
00373   int retval = -1;
00374   int *s = NULL; // Sockets to receive from
00375   int numif = 1;
00376   int nums = 0; // Number of sockets to receive from
00377   int i;
00378   const char **ifNames = NULL;
00379   struct ifaddrs *ifaces = NULL;
00380   struct ifaddrs *curif;
00381   int autoif = 0;
00382 
00383   // Count the number of new cameras found
00384   int newCamCount = 0;
00385 
00386   if (!ifName || !ifName[0]) // The interface has been specified.
00387   {
00388     autoif = 1;
00389     if (getifaddrs(&ifaces))
00390     {
00391       perror("getifaddrs failed");
00392       goto err;
00393     }
00394 
00395     numif = 0;
00396     for (curif = ifaces; curif; curif = curif->ifa_next)
00397       numif++;
00398     //fprintf(stderr, "There are %i interfaces.\n", numif);
00399   }
00400 
00401   ifNames = calloc(numif, sizeof(char *));
00402   if (!ifNames)
00403   {
00404     perror("allocating interfaces memory");
00405     goto err; // Okay because nums == 0 and s and ifNames are allocated or NULL.
00406   }
00407 
00408   if (!autoif)
00409     ifNames[0] = ifName;
00410   else
00411   {
00412      for (numif = 0, curif = ifaces; curif; curif = curif->ifa_next)
00413      {
00414        //fprintf(stderr, "Adding %s to discover list.\n", curif->ifa_name);
00415        if (curif->ifa_addr && curif->ifa_addr->sa_family == AF_INET)
00416          ifNames[numif++] = curif->ifa_name;
00417     }
00418   }
00419 
00420   s = calloc(numif, sizeof(int));
00421   if (!s)
00422   {
00423     perror("allocating socket memory");
00424     goto err; // Okay because nums == 0 and s and ifNames are allocated or NULL.
00425   }
00426 
00427   for (nums = 0; nums < numif; nums++)
00428   {
00429     s[nums] = wge100DiscoverSend(ifNames[nums], ipAddress);
00430     if (s[nums] == -1)
00431     {
00432       //fprintf(stderr, "Removing interface %s.\n", ifNames[nums]);
00433       // Delete this interface as discovery has failed on it.
00434       numif--;
00435       for (i = nums; i < numif; i++)
00436       {
00437         ifNames[i] = ifNames[i+1];
00438       }
00439       nums--;
00440     }
00441   }
00442 
00443   do {
00444     // Wait in the loop for replies. wait_us is updated each time through the loop.
00445     int curs = wge100WaitForPacket(s, nums, PKTT_ANNOUNCE, sizeof(PacketAnnounce) - CAMERA_NAME_LEN - sizeof(IPAddress), &wait_us);
00446     if( curs != -1  && wait_us != 0 ) {
00447       // We've received an Announce packet, so pull it out of the receive queue
00448       PacketAnnounce aPkt;
00449       struct sockaddr_in fromaddr;
00450       fromaddr.sin_family = AF_INET;
00451       socklen_t fromlen = sizeof(fromaddr);
00452       ssize_t packet_len;
00453 
00454       packet_len = recvfrom( s[curs], &aPkt, sizeof(PacketAnnounce), 0, (struct sockaddr *) &fromaddr, &fromlen);
00455       if(packet_len == -1 ) {
00456         perror("wge100Discover unable to receive from socket");
00457         goto err;
00458       }
00459 
00460       if (packet_len != sizeof(PacketAnnounce))
00461       {
00462         //if (packet_len != sizeof(PacketAnnounce) - sizeof(aPkt.camera_name) - sizeof(IPAddress))
00463           continue; // Not a valid packet
00464         /*else // Old announce packet
00465         {
00466           bzero(aPkt.camera_name, sizeof(aPkt.camera_name));
00467           aPkt.camera_ip = fromaddr.sin_addr.s_addr;
00468         } */
00469       }
00470 
00471       // Create a new list entry and initialize it
00472       IpCamList *tmpListItem;
00473       if( (tmpListItem = (IpCamList *)malloc(sizeof(IpCamList))) == NULL ) {
00474         perror("Malloc failed");
00475         goto err;
00476       }
00477       wge100CamListInit( tmpListItem );
00478 
00479       // Initialize the new list item's data fields (byte order corrected)
00480       tmpListItem->hw_version = ntohl(aPkt.hw_version);
00481       tmpListItem->fw_version = ntohl(aPkt.fw_version);
00482       tmpListItem->ip = aPkt.camera_ip;
00483       uint8_t *ipbytes = (uint8_t *) &aPkt.camera_ip;
00484       snprintf(tmpListItem->ip_str, sizeof(tmpListItem->ip_str), "%i.%i.%i.%i", ipbytes[0], ipbytes[1], ipbytes[2], ipbytes[3]);
00485       tmpListItem->serial = ntohl(aPkt.ser_no);
00486       memcpy(&tmpListItem->mac, aPkt.mac, sizeof(aPkt.mac));
00487       memcpy(tmpListItem->cam_name, aPkt.camera_name, sizeof(aPkt.camera_name));
00488       aPkt.camera_name[sizeof(aPkt.camera_name) - 1] = 0;
00489       strncpy(tmpListItem->ifName, ifNames[curs], sizeof(tmpListItem->ifName));
00490       tmpListItem->ifName[sizeof(tmpListItem->ifName) - 1] = 0;
00491       tmpListItem->status = CamStatusDiscovered;
00492       char pcb_rev = 0x0A + (0x0000000F & ntohl(aPkt.hw_version));
00493       int hdl_rev = 0x00000FFF & (ntohl(aPkt.hw_version)>>4);
00494       snprintf(tmpListItem->hwinfo, WGE100_CAMINFO_LEN, "PCB rev %X : HDL rev %3X : FW rev %3X", pcb_rev, hdl_rev, ntohl(aPkt.fw_version));
00495 
00496       // If this camera is already in the list, we don't want to add another copy
00497       if( wge100CamListAdd( ipCamList, tmpListItem ) == CAMLIST_ADD_DUP) {
00498         free(tmpListItem);
00499       } else {
00500         wge100_debug("MAC Address: %02X:%02X:%02X:%02X:%02X:%02X\n", aPkt.mac[0], aPkt.mac[1], aPkt.mac[2], aPkt.mac[3], aPkt.mac[4], aPkt.mac[5]);
00501         wge100_debug("Product #%07u : Unit #%04u\n", ntohl(aPkt.product_id), ntohl(aPkt.ser_no));
00502         wge100_debug("%s\n", tmpListItem->hwinfo);
00503         newCamCount++;
00504       }
00505     }
00506   } while(wait_us > 0);
00507   retval = newCamCount;
00508 
00509 err:
00510   if (ifaces)
00511     freeifaddrs(ifaces);
00512   for (i = 0; i < nums; i++)
00513     close(s[i]);
00514   free(s);
00515   free(ifNames);
00516   return retval;
00517 }
00518 
00519 
00532 int wge100Configure( IpCamList *camInfo, const char *ipAddress, unsigned wait_us) {
00533   // Create and initialize a new Configure packet
00534   PacketConfigure cPkt;
00535 
00536   cPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
00537   cPkt.hdr.type = htonl(PKTT_CONFIGURE);
00538   cPkt.product_id = htonl(CONFIG_PRODUCT_ID);
00539   strncpy(cPkt.hdr.hrt, "CONFIGURE", sizeof(cPkt.hdr.hrt));
00540 
00541   cPkt.ser_no = htonl(camInfo->serial);
00542 
00543   // Create and send the Configure broadcast packet. It is sent broadcast
00544   // because the camera does not yet have an IP address assigned.
00545 
00546   /* Create a new socket bound to a local ephemeral port, and get our local connection
00547    * info so we can request a reply back to the same socket.
00548    */
00549   int s = wge100CmdSocketCreate(camInfo->ifName, &cPkt.hdr.reply_to);
00550   if(s == -1) {
00551     perror("wge100Configure socket creation failed");
00552     return -1;
00553   }
00554   
00555   // Figure out the IP to use, and send the packet.
00556   if (!ipAddress || !*ipAddress)
00557   {
00558     wge100_debug("No ipAddress, using %x\n", camInfo->ip);
00559     cPkt.ip_addr = camInfo->ip;
00560     
00561     if(wge100SendUDP(s, &camInfo->ip, &cPkt, sizeof(cPkt)) == -1) {
00562       wge100_debug("Unable to send packet\n");
00563       close(s);
00564       return -1;
00565     }
00566   }
00567   else
00568   {
00569     struct in_addr newIP;
00570     inet_aton(ipAddress, &newIP);
00571     cPkt.ip_addr = newIP.s_addr;
00572     wge100_debug("Using ipAddress %s -> %x iface %s\n", ipAddress, cPkt.ip_addr, camInfo->ifName);
00573     
00574     wge100_debug("Sending broadcast discover packet.\n");
00575     if(wge100SendUDPBcast(s, camInfo->ifName, &cPkt, sizeof(cPkt)) == -1) {
00576       wge100_debug("Unable to send broadcast\n");
00577       close(s);
00578       return -1;
00579     }
00580   }
00581     
00582   // 'Connect' insures we will only receive datagram replies from the camera's new IP
00583   IPAddress camIP = cPkt.ip_addr;
00584   wge100_debug("Connecting to %x.\n", camIP);
00585   if( wge100SocketConnect(s, &camIP) ) {
00586     wge100_debug("Unable to connect\n");
00587     close(s);
00588     return -1;
00589   }
00590 
00591   // Wait up to wait_us for a valid packet to be received on s
00592   do {
00593     if( wge100WaitForPacket(&s, 1, PKTT_ANNOUNCE, sizeof(PacketAnnounce) - CAMERA_NAME_LEN - sizeof(IPAddress), &wait_us) != -1  && (wait_us != 0) ) {
00594       PacketAnnounce aPkt;
00595 
00596       if( recvfrom( s, &aPkt, sizeof(PacketAnnounce), 0, NULL, NULL )  == -1 ) {
00597         perror("wge100Discover unable to receive from socket");
00598         close(s);
00599         return -1;
00600       }
00601 
00602       // Need to have a valid packet from a camera with the expected serial number
00603       if( ntohl(aPkt.ser_no) == camInfo->serial ) {
00604         camInfo->status = CamStatusConfigured;
00605         memcpy(&camInfo->ip, &cPkt.ip_addr, sizeof(IPAddress));
00606         /*// Add the IP/MAC mapping to the system ARP table
00607         if( wge100ArpAdd(camInfo) ) {
00608           close(s);
00609           return ERR_CONFIG_ARPFAIL;
00610         } */ // ARP handled by the camera except for obsolete firmware versions.
00611         break;
00612       }
00613     }
00614   } while(wait_us > 0);
00615   close(s);
00616 
00617   if(wait_us != 0) {
00618     return 0;
00619   } else {
00620     wge100_debug("Timed out.\n");
00621     return ERR_TIMEOUT;
00622   }
00623 }
00624 
00638 int wge100StartVid( const IpCamList *camInfo, const uint8_t mac[6], const char *ipAddress, uint16_t port ) {
00639   PacketVidStart vPkt;
00640 
00641   // Initialize the packet
00642   vPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
00643   vPkt.hdr.type = htonl(PKTT_VIDSTART);
00644   strncpy(vPkt.hdr.hrt, "Start Video", sizeof(vPkt.hdr.hrt));
00645 
00646 
00647   // Convert the ipAddress string into a binary value in network byte order
00648   inet_aton(ipAddress, (struct in_addr*)&vPkt.receiver.addr);
00649 
00650   // Convert the receiver port into network byte order
00651   vPkt.receiver.port = htons(port);
00652 
00653   // Copy the MAC address into the vPkt
00654   memcpy(&vPkt.receiver.mac, mac, 6);
00655 
00656   /* Create a new socket bound to a local ephemeral port, and get our local connection
00657    * info so we can request a reply back to the same socket.
00658    */
00659   int s = wge100CmdSocketCreate(camInfo->ifName, &vPkt.hdr.reply_to);
00660   if( s == -1 ) {
00661     return -1;
00662   }
00663   
00664   if(wge100SendUDP(s, &camInfo->ip, &vPkt, sizeof(vPkt)) == -1) {
00665     goto err_out;
00666   }
00667 
00668   // 'Connect' insures we will only receive datagram replies from the camera we've specified
00669   if( wge100SocketConnect(s, &camInfo->ip) ) {
00670     goto err_out;
00671   }
00672 
00673   // Wait for a status reply
00674   uint32_t type, code;
00675   if( wge100StatusWait( s, STD_REPLY_TIMEOUT, &type, &code ) == -1) {
00676     goto err_out;
00677   }
00678 
00679   close(s);
00680   if(type == PKT_STATUST_OK) {
00681     return 0;
00682   } else {
00683     wge100_debug("Error: wge100StatusWait returned status %d, code %d\n", type, code);
00684     return 1;
00685   }
00686 
00687 err_out:
00688   close(s);
00689   return -1;
00690 }
00691 
00700 int wge100StopVid( const IpCamList *camInfo ) {
00701   PacketVidStop vPkt;
00702 
00703   // Initialize the packet
00704   vPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
00705   vPkt.hdr.type = htonl(PKTT_VIDSTOP);
00706   strncpy(vPkt.hdr.hrt, "Stop Video", sizeof(vPkt.hdr.hrt));
00707 
00708   /* Create a new socket bound to a local ephemeral port, and get our local connection
00709    * info so we can request a reply back to the same socket.
00710    */
00711   int s = wge100CmdSocketCreate(camInfo->ifName, &vPkt.hdr.reply_to);
00712   if( s == -1 ) {
00713     return -1;
00714   }
00715 
00716   if(  wge100SendUDP(s, &camInfo->ip, &vPkt, sizeof(vPkt)) == -1 ) {
00717     goto err_out;
00718   }
00719 
00720   // 'Connect' insures we will only receive datagram replies from the camera we've specified
00721   if( wge100SocketConnect(s, &camInfo->ip) == -1) {
00722     goto err_out;
00723   }
00724 
00725   uint32_t type, code;
00726   if(wge100StatusWait( s, STOP_REPLY_TIMEOUT, &type, &code ) == -1) {
00727     goto err_out;
00728   }
00729 
00730   close(s);
00731   if(type == PKT_STATUST_OK) {
00732     return 0;
00733   } else {
00734     wge100_debug("Error: wge100StatusWait returned status %d, code %d\n", type, code);
00735     return 1;
00736   }
00737 
00738 err_out:
00739   close(s);
00740   return -1;
00741 }
00742 
00750 int wge100ReconfigureFPGA( IpCamList *camInfo ) {
00751   PacketReconfigureFPGA gPkt;
00752 
00753   // Initialize the packet
00754   gPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
00755   gPkt.hdr.type = htonl(PKTT_RECONFIG_FPGA);
00756   strncpy(gPkt.hdr.hrt, "ReconfigureFPGA", sizeof(gPkt.hdr.hrt));
00757 
00758   /* Create a new socket bound to a local ephemeral port, and get our local connection
00759    * info so we can request a reply back to the same socket.
00760    */
00761   int s = wge100CmdSocketCreate(camInfo->ifName, &gPkt.hdr.reply_to);
00762   if( s == -1 ) {
00763     return -1;
00764   }
00765 
00766   if(  wge100SendUDP(s, &camInfo->ip, &gPkt, sizeof(gPkt)) == -1 ) {
00767     close(s);
00768     return -1;
00769   }
00770 
00771   close(s);
00772 
00773   // Camera is no longer configured after a reset
00774   camInfo->status = CamStatusDiscovered;
00775 
00776   // There is no reply to a reset packet
00777 
00778   return 0;
00779 }
00780 
00788 int wge100Reset( IpCamList *camInfo ) {
00789   PacketReset gPkt;
00790 
00791   // Initialize the packet
00792   gPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
00793   gPkt.hdr.type = htonl(PKTT_RESET);
00794   strncpy(gPkt.hdr.hrt, "Reset", sizeof(gPkt.hdr.hrt));
00795 
00796   /* Create a new socket bound to a local ephemeral port, and get our local connection
00797    * info so we can request a reply back to the same socket.
00798    */
00799   int s = wge100CmdSocketCreate(camInfo->ifName, &gPkt.hdr.reply_to);
00800   if( s == -1 ) {
00801     return -1;
00802   }
00803 
00804   if(  wge100SendUDP(s, &camInfo->ip, &gPkt, sizeof(gPkt)) == -1 ) {
00805     close(s);
00806     return -1;
00807   }
00808 
00809 
00810   close(s);
00811 
00812   // Camera is no longer configured after a reset
00813   camInfo->status = CamStatusDiscovered;
00814 
00815   // There is no reply to a reset packet
00816 
00817   return 0;
00818 }
00819 
00832 int wge100GetTimer( const IpCamList *camInfo, uint64_t *time_us ) {
00833   PacketTimeRequest gPkt;
00834 
00835   // Initialize the packet
00836   gPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
00837   gPkt.hdr.type = htonl(PKTT_TIMEREQ);
00838   strncpy(gPkt.hdr.hrt, "Time Req", sizeof(gPkt.hdr.hrt));
00839 
00840   /* Create a new socket bound to a local ephemeral port, and get our local connection
00841    * info so we can request a reply back to the same socket.
00842    */
00843   int s = wge100CmdSocketCreate(camInfo->ifName, &gPkt.hdr.reply_to);
00844   if( s == -1 ) {
00845     return -1;
00846   }
00847 
00848   if(  wge100SendUDP(s, &camInfo->ip, &gPkt, sizeof(gPkt)) == -1 ) {
00849     close(s);
00850     return -1;
00851   }
00852 
00853   // 'Connect' insures we will only receive datagram replies from the camera we've specified
00854   if( wge100SocketConnect(s, &camInfo->ip) ) {
00855     close(s);
00856     return -1;
00857   }
00858 
00859   uint32_t wait_us = STD_REPLY_TIMEOUT;
00860   do {
00861     if( wge100WaitForPacket(&s, 1, PKTT_TIMEREPLY, sizeof(PacketTimer), &wait_us) != -1 && (wait_us != 0) ) {
00862       PacketTimer tPkt;
00863       if( recvfrom( s, &tPkt, sizeof(PacketTimer), 0, NULL, NULL )  == -1 ) {
00864         perror("GetTime unable to receive from socket");
00865         close(s);
00866         return -1;
00867       }
00868 
00869       *time_us = (uint64_t)ntohl(tPkt.ticks_hi) << 32;
00870       *time_us += ntohl(tPkt.ticks_lo);
00871 
00872       // Need to multiply final result by 1E6 to get us from sec
00873       // Try to do this to limit loss of precision without overflow
00874       // (We will overflow the 64-bit type with this approach
00875       // after the camera has been up for about 11 continuous years)
00876       //FIXME: Review this algorithm for possible improvements.
00877       *time_us *= 1000;
00878       *time_us /= (ntohl(tPkt.ticks_per_sec)/1000);
00879       //debug("Got time of %llu us since last reset\n", *time_us);
00880       close(s);
00881       return 0;
00882     }
00883   } while(wait_us > 0);
00884 
00885   wge100_debug("Timed out waiting for time value\n");
00886   close(s);
00887   return 1;
00888 }
00889 
00906 int wge100ReliableFlashRead( const IpCamList *camInfo, uint32_t address, uint8_t *pageDataOut, int *retries ) {
00907   int retval = -2;
00908 
00909   int counter = 10;
00910 
00911   if (retries == NULL)
00912     retries = &counter;
00913   for (; *retries > 0; (*retries)--)
00914   {
00915     retval = wge100FlashRead( camInfo, address, pageDataOut );
00916 
00917     if (!retval)
00918       return 0;
00919 
00920     wge100_debug("Flash read failed.");
00921   }
00922 
00923   return retval;
00924 }
00925 
00940 int wge100FlashRead( const IpCamList *camInfo, uint32_t address, uint8_t *pageDataOut ) {
00941   PacketFlashRequest rPkt;
00942 
00943   // Initialize the packet
00944   rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
00945   rPkt.hdr.type = htonl(PKTT_FLASHREAD);
00946   if(address > FLASH_MAX_PAGENO) {
00947     return 1;
00948   }
00949 
00950   // The page portion of the address is 12-bits wide, range (bit9..bit21)
00951   rPkt.address = htonl(address<<9);
00952 
00953   strncpy(rPkt.hdr.hrt, "Flash read", sizeof(rPkt.hdr.hrt));
00954 
00955   /* Create a new socket bound to a local ephemeral port, and get our local connection
00956    * info so we can request a reply back to the same socket.
00957    */
00958   int s = wge100CmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
00959   if( s == -1 ) {
00960     return -1;
00961   }
00962 
00963   if(  wge100SendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
00964     close(s);
00965     return -1;
00966   }
00967 
00968 
00969   // 'Connect' insures we will only receive datagram replies from the camera we've specified
00970   if( wge100SocketConnect(s, &camInfo->ip) ) {
00971     close(s);
00972     return -1;
00973   }
00974 
00975   uint32_t wait_us = STD_REPLY_TIMEOUT;
00976   do {
00977     if( wge100WaitForPacket(&s, 1, PKTT_FLASHDATA, sizeof(PacketFlashPayload), &wait_us) != -1 && (wait_us != 0) ) {
00978       PacketFlashPayload fPkt;
00979       if( recvfrom( s, &fPkt, sizeof(PacketFlashPayload), 0, NULL, NULL )  == -1 ) {
00980         perror("GetTime unable to receive from socket");
00981         close(s);
00982         return -1;
00983       }
00984 
00985       // Copy the received data into the space pointed to by pageDataOut
00986       memcpy(pageDataOut, fPkt.data, FLASH_PAGE_SIZE);
00987       close(s);
00988       return 0;
00989     }
00990   } while(wait_us > 0);
00991 
00992   wge100_debug("Timed out waiting for flash value\n");
00993   close(s);
00994   return 1;
00995 }
00996 
01014 int wge100ReliableFlashWrite( const IpCamList *camInfo, uint32_t address, const uint8_t *pageDataIn, int *retries ) {
01015   uint8_t buffer[FLASH_PAGE_SIZE];
01016   int retval = -2;
01017   int counter = 10;
01018   int first_read = 1;
01019 
01020   if (retries == NULL)
01021     retries = &counter;
01022 
01023   (*retries)++; // Don't count the first read as a retry.
01024   goto read_first; // Don't bother writing if nothing has changed.
01025   
01026   for (; *retries > 0; (*retries)--)
01027   {
01028     //printf("Trying write.\n");
01029     retval = wge100FlashWrite( camInfo, address, pageDataIn );
01030     if (retval)
01031     {
01032       wge100_debug("Failed write, retries left: %i.", *retries);
01033       //printf("Failed compare write.\n");
01034       continue;
01035     }
01036     
01037     first_read = 0;
01038 read_first:
01039     retval = wge100ReliableFlashRead( camInfo, address, buffer, retries );
01040     if (retval)
01041     {
01042       //if (!first_read)
01043       //  wge100_debug("Failed compare read.");
01044       //else
01045         //printf("First read failed.\n");
01046       //printf("Failed compare read.\n");
01047       continue;
01048     }
01049 
01050     if (!memcmp(buffer, pageDataIn, FLASH_PAGE_SIZE))
01051       return 0;
01052     //if (!first_read)
01053     //  wge100_debug("Failed compare.");
01054     //else
01055       //printf("First read mismatch.\n");
01056     //printf("Failed compare.\n");
01057     
01058     if (*retries == 0) // In case retries ran out during the read.
01059       break;
01060   }
01061 
01062   return retval;
01063 }
01064 
01077 int wge100FlashWrite( const IpCamList *camInfo, uint32_t address, const uint8_t *pageDataIn ) {
01078   PacketFlashPayload rPkt;
01079 
01080   // Initialize the packet
01081   rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
01082   rPkt.hdr.type = htonl(PKTT_FLASHWRITE);
01083   if(address > FLASH_MAX_PAGENO) {
01084     return 1;
01085   }
01086 
01087   // The page portion of the address is 12-bits wide, range (bit9..bit21)
01088   rPkt.address = htonl(address<<9);
01089   strncpy(rPkt.hdr.hrt, "Flash write", sizeof(rPkt.hdr.hrt));
01090 
01091   memcpy(rPkt.data, pageDataIn, FLASH_PAGE_SIZE);
01092 
01093   /* Create a new socket bound to a local ephemeral port, and get our local connection
01094    * info so we can request a reply back to the same socket.
01095    */
01096   int s = wge100CmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
01097   if( s == -1 ) {
01098     return -1;
01099   }
01100 
01101   if(  wge100SendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
01102     close(s);
01103     return -1;
01104   }
01105 
01106   // 'Connect' insures we will only receive datagram replies from the camera we've specified
01107   if( wge100SocketConnect(s, &camInfo->ip) ) {
01108     close(s);
01109     return -1;
01110   }
01111 
01112   // Wait for response - once we get an OK, we're clear to send the next page.
01113   uint32_t type, code;
01114   wge100StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
01115 
01116   close(s);
01117   if(type == PKT_STATUST_OK) {
01118     return 0;
01119   } else {
01120     wge100_debug("Error: wge100StatusWait returned status %d, code %d\n", type, code);
01121     return 1;
01122   }
01123 }
01124 
01135 int wge100TriggerControl( const IpCamList *camInfo, uint32_t triggerType ) {
01136   PacketTrigControl tPkt;
01137 
01138   // Initialize the packet
01139   tPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
01140   tPkt.hdr.type = htonl(PKTT_TRIGCTRL);
01141   tPkt.trig_state = htonl(triggerType);
01142 
01143   if(triggerType == TRIG_STATE_INTERNAL ) {
01144     strncpy(tPkt.hdr.hrt, "Int. Trigger", sizeof(tPkt.hdr.hrt));
01145   } else {
01146     strncpy(tPkt.hdr.hrt, "Ext. Trigger", sizeof(tPkt.hdr.hrt));
01147   }
01148 
01149   /* Create a new socket bound to a local ephemeral port, and get our local connection
01150    * info so we can request a reply back to the same socket.
01151    */
01152   int s = wge100CmdSocketCreate(camInfo->ifName, &tPkt.hdr.reply_to);
01153   if( s == -1 ) {
01154     return -1;
01155   }
01156 
01157   if(  wge100SendUDP(s, &camInfo->ip, &tPkt, sizeof(tPkt)) == -1 ) {
01158     close(s);
01159     return -1;
01160   }
01161 
01162   // 'Connect' insures we will only receive datagram replies from the camera we've specified
01163   if( wge100SocketConnect(s, &camInfo->ip) ) {
01164     close(s);
01165     return -1;
01166   }
01167 
01168   // Wait for response
01169   uint32_t type, code;
01170   wge100StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
01171 
01172   close(s);
01173   if(type == PKT_STATUST_OK) {
01174     return 0;
01175   } else {
01176     wge100_debug("Error: wge100StatusWait returned status %d, code %d\n", type, code);
01177     return 1;
01178   }
01179 }
01180 
01194 int wge100ConfigureBoard( const IpCamList *camInfo, uint32_t serial, MACAddress *mac ) {
01195   PacketSysConfig sPkt;
01196 
01197   // Initialize the packet
01198   sPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
01199   sPkt.hdr.type = htonl(PKTT_SYSCONFIG);
01200 
01201   strncpy(sPkt.hdr.hrt, "System Config", sizeof(sPkt.hdr.hrt));
01202   memcpy(&sPkt.mac, mac, 6);
01203   sPkt.serial = htonl(serial);
01204 
01205 
01206   /* Create a new socket bound to a local ephemeral port, and get our local connection
01207    * info so we can request a reply back to the same socket.
01208    */
01209   int s = wge100CmdSocketCreate(camInfo->ifName, &sPkt.hdr.reply_to);
01210   if( s == -1 ) {
01211     return -1;
01212   }
01213 
01214   if(  wge100SendUDP(s, &camInfo->ip, &sPkt, sizeof(sPkt)) == -1 ) {
01215     close(s);
01216     return -1;
01217   }
01218 
01219   // 'Connect' insures we will only receive datagram replies from the camera we've specified
01220   if( wge100SocketConnect(s, &camInfo->ip) ) {
01221     close(s);
01222     return -1;
01223   }
01224   // Wait for response
01225   uint32_t type, code;
01226   wge100StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
01227 
01228   close(s);
01229   if(type == PKT_STATUST_OK) {
01230     return 0;
01231   } else {
01232     wge100_debug("Error: wge100StatusWait returned status %d, code %d\n", type, code);
01233     return 1;
01234   }
01235 }
01236 
01252 int wge100ReliableSensorWrite( const IpCamList *camInfo, uint8_t reg, uint16_t data, int *retries ) {
01253   uint16_t readbackdata;
01254   int retval = -2;
01255   int counter = 10;
01256 
01257   if (retries == NULL)
01258     retries = &counter;
01259 
01260   for (; *retries > 0; (*retries)--)
01261   {
01262     retval = wge100SensorWrite( camInfo, reg, data );
01263     if (retval)
01264       continue;
01265 
01266     retval = wge100ReliableSensorRead( camInfo, reg, &readbackdata, retries );
01267     if (retval)
01268       continue;
01269 
01270     if (readbackdata == data)
01271       return 0;
01272     
01273     if (*retries == 0) // In case retries ran out during the read.
01274       retval = -2;
01275   }
01276 
01277   return retval;
01278 }
01279   
01291 int wge100SensorWrite( const IpCamList *camInfo, uint8_t reg, uint16_t data ) {
01292   PacketSensorData sPkt;
01293 
01294   // Initialize the packet
01295   sPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
01296   sPkt.hdr.type = htonl(PKTT_SENSORWR);
01297 
01298   strncpy(sPkt.hdr.hrt, "Write I2C", sizeof(sPkt.hdr.hrt));
01299   sPkt.address = reg;
01300   sPkt.data = htons(data);
01301 
01302   /* Create a new socket bound to a local ephemeral port, and get our local connection
01303    * info so we can request a reply back to the same socket.
01304    */
01305   int s = wge100CmdSocketCreate(camInfo->ifName, &sPkt.hdr.reply_to);
01306   if( s == -1 ) {
01307     return -1;
01308   }
01309 
01310   if(  wge100SendUDP(s, &camInfo->ip, &sPkt, sizeof(sPkt)) == -1 ) {
01311     close(s);
01312     return -1;
01313   }
01314 
01315   // 'Connect' insures we will only receive datagram replies from the camera we've specified
01316   if( wge100SocketConnect(s, &camInfo->ip) ) {
01317     close(s);
01318     return -1;
01319   }
01320 
01321   // Wait for response
01322   uint32_t type, code;
01323   wge100StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
01324 
01325   close(s);
01326   if(type == PKT_STATUST_OK) {
01327     return 0;
01328   } else {
01329     wge100_debug("Error: wge100StatusWait returned status %d, code %d\n", type, code);
01330     return 1;
01331   }
01332 }
01333 
01349 int wge100ReliableSensorRead( const IpCamList *camInfo, uint8_t reg, uint16_t *data, int *retries ) {
01350   int retval = -2;
01351 
01352   int counter = 10;
01353 
01354   if (retries == NULL)
01355     retries = &counter;
01356   for (; *retries > 0; (*retries)--)
01357   {
01358     retval = wge100SensorRead( camInfo, reg, data );
01359 
01360     if (!retval)
01361       return 0;
01362   }
01363 
01364   return retval;
01365 }
01366 
01378 int wge100SensorRead( const IpCamList *camInfo, uint8_t reg, uint16_t *data ) {
01379   PacketSensorRequest rPkt;
01380 
01381   // Initialize the packet
01382   rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
01383   rPkt.hdr.type = htonl(PKTT_SENSORRD);
01384   rPkt.address = reg;
01385   strncpy(rPkt.hdr.hrt, "Read I2C", sizeof(rPkt.hdr.hrt));
01386 
01387   /* Create a new socket bound to a local ephemeral port, and get our local connection
01388    * info so we can request a reply back to the same socket.
01389    */
01390   int s = wge100CmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
01391   if( s == -1 ) {
01392     return -1;
01393   }
01394 
01395   if(  wge100SendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
01396     close(s);
01397     return -1;
01398   }
01399 
01400   // 'Connect' insures we will only receive datagram replies from the camera we've specified
01401   if( wge100SocketConnect(s, &camInfo->ip) ) {
01402     close(s);
01403     return -1;
01404   }
01405 
01406   uint32_t wait_us = STD_REPLY_TIMEOUT;
01407   do {
01408     if( wge100WaitForPacket(&s, 1, PKTT_SENSORDATA, sizeof(PacketSensorData), &wait_us) != -1 && (wait_us != 0) ) {
01409       PacketSensorData sPkt;
01410       if( recvfrom( s, &sPkt, sizeof(PacketSensorData), 0, NULL, NULL )  == -1 ) {
01411         perror("SensorRead unable to receive from socket");
01412         close(s);
01413         return -1;
01414       }
01415 
01416       *data = ntohs(sPkt.data);
01417       close(s);
01418       return 0;
01419     }
01420   } while(wait_us > 0);
01421 
01422   wge100_debug("Timed out waiting for sensor value\n");
01423   close(s);
01424   return 1;
01425 }
01426 
01438 int wge100SensorSelect( const IpCamList *camInfo, uint8_t index, uint32_t reg ) {
01439   PacketSensorSelect sPkt;
01440 
01441   // Initialize the packet
01442   sPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
01443   sPkt.hdr.type = htonl(PKTT_SENSORSEL);
01444 
01445   strncpy(sPkt.hdr.hrt, "Select I2C", sizeof(sPkt.hdr.hrt));
01446   sPkt.index = index;
01447   sPkt.address = htonl(reg);
01448 
01449   /* Create a new socket bound to a local ephemeral port, and get our local connection
01450    * info so we can request a reply back to the same socket.
01451    */
01452   int s = wge100CmdSocketCreate(camInfo->ifName, &sPkt.hdr.reply_to);
01453   if( s == -1 ) {
01454     return -1;
01455   }
01456 
01457   if(  wge100SendUDP(s, &camInfo->ip, &sPkt, sizeof(sPkt)) == -1 ) {
01458     close(s);
01459     return -1;
01460   }
01461 
01462   // 'Connect' insures we will only receive datagram replies from the camera we've specified
01463   if( wge100SocketConnect(s, &camInfo->ip) ) {
01464     close(s);
01465     return -1;
01466   }
01467 
01468   // Wait for response
01469   uint32_t type, code;
01470   wge100StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
01471 
01472   close(s);
01473   if(type == PKT_STATUST_OK) {
01474     return 0;
01475   } else {
01476     wge100_debug("Error: wge100StatusWait returned status %d, code %d\n", type, code);
01477     return 1;
01478   }
01479 }
01480 
01491 int wge100ImagerModeSelect( const IpCamList *camInfo, uint32_t mode ) {
01492   PacketImagerMode mPkt;
01493 
01494   // Initialize the packet
01495   mPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
01496   mPkt.hdr.type = htonl(PKTT_IMGRMODE);
01497 
01498   mPkt.mode = htonl(mode);
01499 
01500   strncpy(mPkt.hdr.hrt, "Set Mode", sizeof(mPkt.hdr.hrt));
01501 
01502   /* Create a new socket bound to a local ephemeral port, and get our local connection
01503    * info so we can request a reply back to the same socket.
01504    */
01505   int s = wge100CmdSocketCreate(camInfo->ifName, &mPkt.hdr.reply_to);
01506   if( s == -1 ) {
01507     return -1;
01508   }
01509 
01510   if(  wge100SendUDP(s, &camInfo->ip, &mPkt, sizeof(mPkt)) == -1 ) {
01511     close(s);
01512     return -1;
01513   }
01514 
01515   // 'Connect' insures we will only receive datagram replies from the camera we've specified
01516   if( wge100SocketConnect(s, &camInfo->ip) ) {
01517     close(s);
01518     return -1;
01519   }
01520 
01521   // Wait for response
01522   uint32_t type, code;
01523   wge100StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
01524 
01525   close(s);
01526   if(type == PKT_STATUST_OK) {
01527     return 0;
01528   } else {
01529     wge100_debug("Error: wge100StatusWait returned status %d, code %d\n", type, code);
01530     return 1;
01531   }
01532 }
01533 
01548 int wge100ImagerSetRes( const IpCamList *camInfo, uint16_t horizontal, uint16_t vertical ) {
01549   PacketImagerSetRes rPkt;
01550 
01551   // Initialize the packet
01552   rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
01553   rPkt.hdr.type = htonl(PKTT_IMGRSETRES);
01554 
01555   rPkt.horizontal = htons(horizontal);
01556   rPkt.vertical = htons(vertical);
01557 
01558   strncpy(rPkt.hdr.hrt, "Set Res", sizeof(rPkt.hdr.hrt));
01559 
01560   /* Create a new socket bound to a local ephemeral port, and get our local connection
01561    * info so we can request a reply back to the same socket.
01562    */
01563   int s = wge100CmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
01564   if( s == -1 ) {
01565     return -1;
01566   }
01567 
01568   if(  wge100SendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
01569     close(s);
01570     return -1;
01571   }
01572 
01573   // 'Connect' insures we will only receive datagram replies from the camera we've specified
01574   if( wge100SocketConnect(s, &camInfo->ip) ) {
01575     close(s);
01576     return -1;
01577   }
01578 
01579   // Wait for response
01580   uint32_t type, code;
01581   wge100StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
01582 
01583   close(s);
01584   if(type == PKT_STATUST_OK) {
01585     return 0;
01586   } else {
01587     wge100_debug("Error: wge100StatusWait returned status %d, code %d\n", type, code);
01588     return 1;
01589   }
01590 }
01591 
01592 #define MAX_HORIZ_RESOLUTION 752
01593 #define LINE_NUMBER_MASK 0x3FF
01594 
01595 int wge100VidReceiveSocket( int s, size_t height, size_t width, FrameHandler frameHandler, void *userData ) {
01596   /*
01597    * The default receive buffer size on a 32-bit Linux machine is only 128kB.
01598    * At a burst data rate of ~ 82.6Mbit/s in the 640x480 @30fps, this buffer will fill in ~12.6ms.
01599    * With the default Linux scheduler settings, this leaves virtually no time for any other processes
01600    * to execute before the buffer overflows. Increasing the buffer reduces the chance of lost
01601    * packets.
01602    *
01603    * The 8MB buffer here is far larger than necessary for most applications and may be reduced with
01604    * some experimentation.
01605    *
01606    * Note that the Linux system command 'sysctl -w net.core.rmem_max=8388608' must be used to enable
01607    * receive buffers larger than the kernel default.
01608    */
01609   size_t bufsize = 16*1024*1024;  // 8MB receive buffer.
01610 
01611   int i;
01612 
01613   int return_value = 0;
01614 
01615   if( setsockopt(s, SOL_SOCKET,SO_RCVBUF, &bufsize, sizeof(bufsize)) == -1) {
01616     perror("Can't set rcvbuf option");
01617     close(s);
01618     return -1;
01619   }
01620 
01621   socklen_t bufsizesize = sizeof(bufsize);
01622   if( getsockopt(s, SOL_SOCKET,SO_RCVBUF, &bufsize, &bufsizesize) == 0) {
01623     wge100_debug("Receive buffer size is: %i (%i)\n", bufsize, bufsizesize);
01624   }
01625   else
01626   {
01627     perror("Can't read receive buffer size");
01628   }
01629 
01630   // We receive data one line at a time; lines will be reassembled into this buffer as they arrive
01631   uint8_t *frame_buf;
01632   frame_buf = malloc(sizeof(uint8_t)*width*height);
01633   if(frame_buf == NULL) {
01634     perror("Can't malloc frame buffer");
01635     close(s);
01636     return -1;
01637   }
01638 
01639   // Allocate enough storage for the header as well as 'width' bytes of video data
01640   PacketVideoLine *vPkt=malloc(sizeof(PacketVideoLine));
01641   if(vPkt == NULL) {
01642     perror("Can't malloc line packet buffer");
01643     close(s);
01644     return -1;
01645   }
01646 
01647   // Flag to indicate that 'frameInfo.frame_number' has not yet been set
01648   bool firstPacket = true;
01649 
01650   // Flag to indicate the current frame is complete (either EOF received or first line of next frame received)
01651   bool frameComplete;
01652 
01653   // Flag to indicate that we have set the 'frameInfo.startTime' timeval for this frame.
01654   bool frameStartTimeSet;
01655 
01656   // Holds return code from frameHandler
01657   int handlerReturn;
01658 
01659   // Counts number of lines received in current frame
01660   uint32_t lineCount;
01661 
01662   // Points to an EOF structure for passing to frameHandler;
01663   PacketEOF *eof = NULL;
01664 
01665   // Information structure to pass to the frame handler.
01666   wge100FrameInfo frameInfo;
01667       
01668   // Keep track of where the last packet came from (to send out pings).
01669   struct sockaddr_in fromaddr;
01670 
01671   // Has the current frame had an XOR line
01672   bool has_xor;
01673 
01674   frameInfo.width = width;
01675   frameInfo.height = height;
01676 
01677   uint8_t xorline[width];
01678 
01679   do {
01680     lineCount = 0;
01681     frameComplete = false;
01682     frameStartTimeSet = false;
01683     frameInfo.lastMissingLine = -1;
01684     frameInfo.missingLines = 0;
01685     frameInfo.shortFrame = false;
01686     frameInfo.frame_number = vPkt->header.frame_number+1;
01687     // Ensure the buffer is cleared out. Makes viewing short packets less confusing; missing lines will be black.
01688     memset(frame_buf, 0, width*height);
01689     has_xor = false;
01690 
01691     // We detect a dropped EOF by unexpectedly receiving a line from the next frame before getting the EOF from this frame.
01692     // After the last frame has been processed, start a fresh frame with the expected line.
01693     if( (eof==NULL) && (firstPacket==false) ) {
01694       if(vPkt->header.line_number < height ) {
01695         memcpy(&(frame_buf[vPkt->header.line_number*width]), vPkt->data, width);
01696         lineCount++;
01697       }
01698       frameInfo.frame_number &= ~0xFFFF;
01699       frameInfo.frame_number |= vPkt->header.frame_number;
01700     }
01701 
01702     do {
01703       // Wait forever for video packets to arrive; could use select() with a timeout here if a timeout is needed
01704       struct timeval readtimeout;
01705       fd_set set;
01706       socklen_t fromaddrlen = sizeof(struct sockaddr_in);
01707       fromaddr.sin_family = AF_INET;
01708 
01709       // Wait for either a packet to be received or for timeout
01710       handlerReturn = 0;
01711       do {
01712         readtimeout.tv_sec = 1;
01713         readtimeout.tv_usec = 0;
01714 
01715         FD_ZERO(&set);
01716         FD_SET(s, &set);
01717 
01718         if( select(s+1, &set, NULL, NULL, &readtimeout) == -1 ) {
01719           perror("wge100VidReceive select failed");
01720           close(s);
01721           return -1;
01722         }
01723 
01724         // Call the frame handler with NULL to see if we should bail out. 
01725         if(! FD_ISSET(s, &set) && errno != EINTR) {
01726           wge100_debug("Select timed out. Calling handler.");
01727           handlerReturn = frameHandler(NULL, userData);
01728           if (handlerReturn)
01729             break;
01730         }
01731       } while (! FD_ISSET(s, &set));
01732 
01733       // We timed out, and the handler returned nonzero.
01734       if (handlerReturn) 
01735         break;
01736 
01737       if( recvfrom( s, vPkt, sizeof(HeaderVideoLine)+width, 0, (struct sockaddr *) &fromaddr, &fromaddrlen )  == -1 ) {
01738         perror("wge100VidReceive unable to receive from socket");
01739         break;
01740       }
01741 
01742       // Convert fields to host byte order for easier processing
01743       vPkt->header.frame_number = ntohl(vPkt->header.frame_number);
01744       vPkt->header.line_number = ntohs(vPkt->header.line_number);
01745       vPkt->header.horiz_resolution = ntohs(vPkt->header.horiz_resolution);
01746       vPkt->header.vert_resolution = ntohs(vPkt->header.vert_resolution);
01747 
01748       //debug("frame: #%i line: %i\n", vPkt->header.frame_number, 0x3FF & vPkt->header.line_number);
01749 
01750       // Check to make sure the frame number information is consistent within the packet
01751       uint16_t temp = (vPkt->header.line_number>>10) & 0x003F;
01752       if (((vPkt->header.line_number & IMAGER_MAGICLINE_MASK) == 0) && (temp != (vPkt->header.frame_number & 0x003F))) {
01753         wge100_debug("Mismatched line/frame numbers: %02X / %02X\n", temp, (vPkt->header.frame_number & 0x003F));
01754       }
01755 
01756       // Validate that the frame is the size we expected
01757       if( (vPkt->header.horiz_resolution != width) || (vPkt->header.vert_resolution != height) ) {
01758         wge100_debug("Invalid frame size received: %u x %u, expected %u x %u\n", vPkt->header.horiz_resolution, vPkt->header.vert_resolution, width, height);
01759         close(s);
01760         return 1;
01761       }
01762 
01763       // First time through we need to initialize the number of the first frame we received
01764       if(firstPacket == true) {
01765         firstPacket = false;
01766         frameInfo.frame_number = vPkt->header.frame_number;
01767       }
01768 
01769       // Store the start time for the frame.
01770       if (!frameStartTimeSet)
01771       {
01772         gettimeofday(&frameInfo.startTime, NULL);
01773         frameStartTimeSet = true;
01774       }
01775 
01776       // Check for frames that ended with an error
01777       if( (vPkt->header.line_number == IMAGER_LINENO_ERR) || 
01778           (vPkt->header.line_number == IMAGER_LINENO_OVF) ) {
01779         wge100_debug("Video error: %04X\n", vPkt->header.line_number);
01780 
01781         // In the case of these special 'error' EOF packets, there has been a serious internal camera failure
01782         // so we will abort rather than try to process the last frame.
01783         return_value = vPkt->header.line_number;
01784         break;
01785       } else if (vPkt->header.line_number == IMAGER_LINENO_ABORT) {
01786         wge100_debug("Video aborted\n");
01787         break;  //don't process last frame
01788       } else if((vPkt->header.frame_number - frameInfo.frame_number) & 0xFFFF) { // The camera only sends 16 bits of frame number
01789         // If we have received a line from the next frame, we must have missed the EOF somehow
01790         wge100_debug("Frame #%u missing EOF, got %i lines\n", frameInfo.frame_number, lineCount);
01791         frameComplete = true;
01792         // EOF was missing
01793         eof = NULL;
01794       } else if( vPkt->header.line_number == IMAGER_LINENO_XOR ) {
01795         memcpy(xorline, vPkt->data, width);
01796         has_xor = true;
01797       } else if( vPkt->header.line_number == IMAGER_LINENO_EOF ) {
01798         // This is a 'normal' (non-error) end of frame packet (line number 0x3FF)
01799         frameComplete = true;
01800 
01801         // Handle EOF data fields
01802         eof = (PacketEOF *)vPkt;
01803 
01804         // Correct to network byte order for frameHandler
01805         eof->ticks_hi = ntohl(eof->ticks_hi);
01806         eof->ticks_lo = ntohl(eof->ticks_lo);
01807         eof->ticks_per_sec = ntohl(eof->ticks_per_sec);
01808 
01809         // Correct to network byte order for frameHandler
01810         eof->i2c_valid = ntohl(eof->i2c_valid);
01811         for(i=0; i<I2C_REGS_PER_FRAME; i++) {
01812           eof->i2c[i] = ntohs(eof->i2c[i]);
01813         }
01814 
01815         if(lineCount != height) {
01816           if ((1 == (height - lineCount)) && has_xor) {
01817             wge100_debug("Restoring line %i\n", frameInfo.lastMissingLine);
01818 
01819             // reconstruct the missing line by XORing the frame's XOR line
01820             // with every *other* line in the frame.
01821             uint8_t *repair = &frame_buf[frameInfo.lastMissingLine * width];
01822             memcpy(repair, xorline, width);
01823             unsigned int y;
01824             for (y = 0; y < height; y++) {
01825               if (y != frameInfo.lastMissingLine) {
01826                 xormem(repair, &frame_buf[y * width], width);
01827               }
01828             }
01829           } else {
01830             // Flag packet as being short for the frameHandler
01831             eof->header.line_number = IMAGER_LINENO_SHORT;
01832             frameInfo.shortFrame = true;
01833             frameInfo.missingLines = height - lineCount;
01834           }
01835         }
01836         // Move to the next frame
01837 
01838       } else {
01839         // Remove extraneous frame information from the line number field
01840         vPkt->header.line_number &= LINE_NUMBER_MASK;
01841                                  
01842         if(lineCount > height)
01843         {
01844           wge100_debug("Too many lines received for frame!\n")
01845           break;
01846         }
01847 
01848         if( vPkt->header.line_number >= vPkt->header.vert_resolution ) {
01849           wge100_debug("Invalid line number received: %u (max %u)\n", vPkt->header.line_number, vPkt->header.vert_resolution);
01850           break;
01851         }
01852         if (lineCount + frameInfo.missingLines < vPkt->header.line_number)
01853         {
01854           int missedLines = vPkt->header.line_number - lineCount - frameInfo.missingLines; 
01855           frameInfo.lastMissingLine = vPkt->header.line_number - 1;
01856           wge100_debug("Frame #%i missed %i line(s) starting at %i src port %i\n", vPkt->header.frame_number, 
01857               missedLines, lineCount + frameInfo.missingLines, ntohs(fromaddr.sin_port));
01858           frameInfo.missingLines += missedLines;
01859         }
01860         memcpy(&(frame_buf[vPkt->header.line_number*width]), vPkt->data, width);
01861         lineCount++;
01862       }
01863     } while(frameComplete == false);
01864 
01865     if( frameComplete == true ) {
01866       frameInfo.frameData = frame_buf;
01867       frameInfo.eofInfo = eof;
01868       frameInfo.frame_number = frameInfo.frame_number; 
01869       handlerReturn = frameHandler(&frameInfo, userData);
01870   
01871       // Send a packet to the camera (this ensures that the switches along
01872       // the way won't time out). We could get away with doing this a lot
01873       // less often, but it is a relatively small cost.
01874       uint8_t dummy = 0xff;
01875       if( sendto( s, &dummy, sizeof(dummy), 0, (struct sockaddr *) &fromaddr, sizeof(fromaddr) )  == -1 ) {
01876         handlerReturn = -1; // Should not happen, bail out...
01877       }
01878     } else {
01879       // We wind up here if a serious error has resulted in us 'break'ing out of the loop.
01880       // We won't call frameHandler, we'll just exit directly.
01881       handlerReturn = -1;
01882     }
01883   } while( handlerReturn == 0 );
01884 
01885   close(s);
01886   return return_value;
01887 }
01888 
01889 int wge100VidReceive( const char *ifName, uint16_t port, size_t height, size_t width, FrameHandler frameHandler, void *userData ) {
01890   struct in_addr host_addr;
01891   wge100IpGetLocalAddr( ifName, &host_addr );
01892 
01893   if( frameHandler == NULL ) {
01894     wge100_debug("Invalid frame handler, aborting.\n");
01895     return 1;
01896   }
01897 
01898   wge100_debug("wge100VidReceive ready to receive on %s (%s:%u)\n", ifName, inet_ntoa(host_addr), port);
01899 
01900   int s = wge100SocketCreate( &host_addr, port );
01901   if( s == -1 ) {
01902     return -1;
01903   }
01904 
01905   return wge100VidReceiveSocket( s, height, width, frameHandler, userData);
01906 }
01907 
01908 int wge100VidReceiveAuto( IpCamList *camera, size_t height, size_t width, FrameHandler frameHandler, void *userData ) {
01909   struct sockaddr localMac;
01910   struct in_addr localIp;
01911   struct sockaddr localPort;
01912   socklen_t localPortLen;
01913   int s;
01914   int retval;
01915   int port;
01916 
01917   if ( wge100IpGetLocalAddr(camera->ifName, &localIp) != 0) {
01918     fprintf(stderr, "Unable to get local IP address for interface %s", camera->ifName);
01919     return -1;
01920   }
01921     
01922   if ( wge100EthGetLocalMac(camera->ifName, &localMac) != 0 ) {
01923     fprintf(stderr, "Unable to get local MAC address for interface %s", camera->ifName);
01924     return -1;
01925   }
01926       
01927   if( frameHandler == NULL ) {
01928     wge100_debug("Invalid frame handler, aborting.\n");
01929     return 1;
01930   }
01931 
01932   s = wge100SocketCreate( &localIp, 0 );
01933   if( s == -1 ) {
01934     return -1;
01935   }
01936 
01937   localPortLen = sizeof(localPort);
01938   if (getsockname(s, &localPort, &localPortLen) == -1)
01939   {
01940     fprintf(stderr, "Unable to get local port for socket.");
01941     close(s);
01942     return -1;
01943   }
01944   
01945   port = ntohs(((struct sockaddr_in *)&localPort)->sin_port);
01946 //  fprintf(stderr, "Streaming to port %i.\n", port);
01947 
01948   wge100_debug("wge100VidReceiveAuto ready to receive on %s (%s:%u)\n", camera->ifName, inet_ntoa(localIp), port);
01949 
01950   if ( wge100StartVid( camera, (uint8_t *)&(localMac.sa_data[0]), inet_ntoa(localIp), port) != 0 ) 
01951   {
01952     wge100_debug("Could not start camera streaming.");
01953     close (s);
01954     return -1;
01955   }
01956 
01957   retval = wge100VidReceiveSocket( s, height, width, frameHandler, userData);
01958       
01959   close(s);
01960   wge100StopVid(camera);
01961   return retval;
01962 }
01963