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 "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);
00123
00124 if (errmsg)
00125 *errmsg = illegalcase;
00126
00127 if (!copy)
00128 {
00129 *errmsg = nomem;
00130 return -1;
00131 }
00132 strcpy(copy, url);
00133
00134
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
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;
00182 *curpos = 0;
00183 }
00184
00185
00186 if (mode == 3 && *idstart)
00187 {
00188 if (errmsg)
00189 *errmsg = badany;
00190 goto bailout;
00191 }
00192
00193 if (mode == 2)
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
00206
00207
00208 if (wge100Discover(interface, &camList, address, wait_us) == -1)
00209 {
00210 if (errmsg)
00211 *errmsg = discoverfailed;
00212 goto bailout;
00213 }
00214
00215
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)
00232 {
00233 struct in_addr ip;
00234 if (inet_aton(address, &ip))
00235 {
00236 uint8_t *ipbytes = (uint8_t *) &ip.s_addr;
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:
00253 retval = 0;
00254 break;
00255
00256 case 0:
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
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
00323
00324
00325 int s = wge100CmdSocketCreate(ifName, &dPkt.hdr.reply_to);
00326 if(s == -1) {
00327
00328 return -1;
00329 }
00330
00333
00334 struct in_addr newIP;
00335 if (ipAddress)
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
00354
00355
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;
00375 int numif = 1;
00376 int nums = 0;
00377 int i;
00378 const char **ifNames = NULL;
00379 struct ifaddrs *ifaces = NULL;
00380 struct ifaddrs *curif;
00381 int autoif = 0;
00382
00383
00384 int newCamCount = 0;
00385
00386 if (!ifName || !ifName[0])
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
00399 }
00400
00401 ifNames = calloc(numif, sizeof(char *));
00402 if (!ifNames)
00403 {
00404 perror("allocating interfaces memory");
00405 goto err;
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
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;
00425 }
00426
00427 for (nums = 0; nums < numif; nums++)
00428 {
00429 s[nums] = wge100DiscoverSend(ifNames[nums], ipAddress);
00430 if (s[nums] == -1)
00431 {
00432
00433
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
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
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
00463 continue;
00464
00465
00466
00467
00468
00469 }
00470
00471
00472 IpCamList *tmpListItem;
00473 if( (tmpListItem = (IpCamList *)malloc(sizeof(IpCamList))) == NULL ) {
00474 perror("Malloc failed");
00475 goto err;
00476 }
00477 wge100CamListInit( tmpListItem );
00478
00479
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
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
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
00544
00545
00546
00547
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
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
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
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
00603 if( ntohl(aPkt.ser_no) == camInfo->serial ) {
00604 camInfo->status = CamStatusConfigured;
00605 memcpy(&camInfo->ip, &cPkt.ip_addr, sizeof(IPAddress));
00606
00607
00608
00609
00610
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
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
00648 inet_aton(ipAddress, (struct in_addr*)&vPkt.receiver.addr);
00649
00650
00651 vPkt.receiver.port = htons(port);
00652
00653
00654 memcpy(&vPkt.receiver.mac, mac, 6);
00655
00656
00657
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
00669 if( wge100SocketConnect(s, &camInfo->ip) ) {
00670 goto err_out;
00671 }
00672
00673
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
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
00709
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
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
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
00759
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
00774 camInfo->status = CamStatusDiscovered;
00775
00776
00777
00778 return 0;
00779 }
00780
00788 int wge100Reset( IpCamList *camInfo ) {
00789 PacketReset gPkt;
00790
00791
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
00797
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
00813 camInfo->status = CamStatusDiscovered;
00814
00815
00816
00817 return 0;
00818 }
00819
00832 int wge100GetTimer( const IpCamList *camInfo, uint64_t *time_us ) {
00833 PacketTimeRequest gPkt;
00834
00835
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
00841
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
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
00873
00874
00875
00876
00877 *time_us *= 1000;
00878 *time_us /= (ntohl(tPkt.ticks_per_sec)/1000);
00879
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
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
00951 rPkt.address = htonl(address<<9);
00952
00953 strncpy(rPkt.hdr.hrt, "Flash read", sizeof(rPkt.hdr.hrt));
00954
00955
00956
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
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
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)++;
01024 goto read_first;
01025
01026 for (; *retries > 0; (*retries)--)
01027 {
01028
01029 retval = wge100FlashWrite( camInfo, address, pageDataIn );
01030 if (retval)
01031 {
01032 wge100_debug("Failed write, retries left: %i.", *retries);
01033
01034 continue;
01035 }
01036
01037 first_read = 0;
01038 read_first:
01039 retval = wge100ReliableFlashRead( camInfo, address, buffer, retries );
01040 if (retval)
01041 {
01042
01043
01044
01045
01046
01047 continue;
01048 }
01049
01050 if (!memcmp(buffer, pageDataIn, FLASH_PAGE_SIZE))
01051 return 0;
01052
01053
01054
01055
01056
01057
01058 if (*retries == 0)
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
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
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
01094
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
01107 if( wge100SocketConnect(s, &camInfo->ip) ) {
01108 close(s);
01109 return -1;
01110 }
01111
01112
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
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
01150
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
01163 if( wge100SocketConnect(s, &camInfo->ip) ) {
01164 close(s);
01165 return -1;
01166 }
01167
01168
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
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
01207
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
01220 if( wge100SocketConnect(s, &camInfo->ip) ) {
01221 close(s);
01222 return -1;
01223 }
01224
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)
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
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
01303
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
01316 if( wge100SocketConnect(s, &camInfo->ip) ) {
01317 close(s);
01318 return -1;
01319 }
01320
01321
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
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
01388
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
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
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
01450
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
01463 if( wge100SocketConnect(s, &camInfo->ip) ) {
01464 close(s);
01465 return -1;
01466 }
01467
01468
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
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
01503
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
01516 if( wge100SocketConnect(s, &camInfo->ip) ) {
01517 close(s);
01518 return -1;
01519 }
01520
01521
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
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
01561
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
01574 if( wge100SocketConnect(s, &camInfo->ip) ) {
01575 close(s);
01576 return -1;
01577 }
01578
01579
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
01598
01599
01600
01601
01602
01603
01604
01605
01606
01607
01608
01609 size_t bufsize = 16*1024*1024;
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
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
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
01648 bool firstPacket = true;
01649
01650
01651 bool frameComplete;
01652
01653
01654 bool frameStartTimeSet;
01655
01656
01657 int handlerReturn;
01658
01659
01660 uint32_t lineCount;
01661
01662
01663 PacketEOF *eof = NULL;
01664
01665
01666 wge100FrameInfo frameInfo;
01667
01668
01669 struct sockaddr_in fromaddr;
01670
01671
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
01688 memset(frame_buf, 0, width*height);
01689 has_xor = false;
01690
01691
01692
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
01704 struct timeval readtimeout;
01705 fd_set set;
01706 socklen_t fromaddrlen = sizeof(struct sockaddr_in);
01707 fromaddr.sin_family = AF_INET;
01708
01709
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
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
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
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
01749
01750
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
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
01764 if(firstPacket == true) {
01765 firstPacket = false;
01766 frameInfo.frame_number = vPkt->header.frame_number;
01767 }
01768
01769
01770 if (!frameStartTimeSet)
01771 {
01772 gettimeofday(&frameInfo.startTime, NULL);
01773 frameStartTimeSet = true;
01774 }
01775
01776
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
01782
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;
01788 } else if((vPkt->header.frame_number - frameInfo.frame_number) & 0xFFFF) {
01789
01790 wge100_debug("Frame #%u missing EOF, got %i lines\n", frameInfo.frame_number, lineCount);
01791 frameComplete = true;
01792
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
01799 frameComplete = true;
01800
01801
01802 eof = (PacketEOF *)vPkt;
01803
01804
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
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
01820
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
01831 eof->header.line_number = IMAGER_LINENO_SHORT;
01832 frameInfo.shortFrame = true;
01833 frameInfo.missingLines = height - lineCount;
01834 }
01835 }
01836
01837
01838 } else {
01839
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
01872
01873
01874 uint8_t dummy = 0xff;
01875 if( sendto( s, &dummy, sizeof(dummy), 0, (struct sockaddr *) &fromaddr, sizeof(fromaddr) ) == -1 ) {
01876 handlerReturn = -1;
01877 }
01878 } else {
01879
01880
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
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