You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

nerdjack.c 16 KiB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697
  1. /*
  2. * Labjack Tools
  3. * Copyright (c) 2003-2007 Jim Paris <jim@jtan.com>
  4. *
  5. * This is free software; you can redistribute it and/or modify it and
  6. * it is provided under the terms of version 2 of the GNU General Public
  7. * License as published by the Free Software Foundation; see COPYING.
  8. */
  9. #include <errno.h>
  10. #include <stdint.h>
  11. #include <stdlib.h>
  12. #include <unistd.h>
  13. #include <stdio.h>
  14. #include <string.h>
  15. #include <math.h>
  16. #include "netutil.h"
  17. #include "compat.h"
  18. #include "debug.h"
  19. #include "nerdjack.h"
  20. #include "util.h"
  21. #include "netutil.h"
  22. #include "ethstream.h"
  23. #define NERD_HEADER_SIZE 8
  24. #define MAX_SOCKETS 32
  25. typedef struct __attribute__ ((__packed__)) {
  26. unsigned char headerone;
  27. unsigned char headertwo;
  28. unsigned short packetNumber;
  29. unsigned short adcused;
  30. unsigned short packetsready;
  31. signed short data[NERDJACK_NUM_SAMPLES];
  32. } dataPacket;
  33. struct discovered_socket {
  34. int sock;
  35. uint32_t local_ip;
  36. uint32_t subnet_mask;
  37. };
  38. struct discover_t {
  39. struct discovered_socket socks[MAX_SOCKETS];
  40. unsigned int sock_count;
  41. };
  42. /* Choose the best ScanConfig and ScanInterval parameters for the
  43. desired scanrate. Returns -1 if no valid config found */
  44. int
  45. nerdjack_choose_scan(double desired_rate, double *actual_rate,
  46. unsigned long *period)
  47. {
  48. //The ffffe is because of a silicon bug. The last bit is unusable in all
  49. //devices so far. It is worked around on the chip, but giving it exactly
  50. //0xfffff would cause the workaround code to roll over.
  51. *period = floor((double)NERDJACK_CLOCK_RATE / desired_rate);
  52. if (*period > 0x0ffffe) {
  53. info("Cannot sample that slowly\n");
  54. *actual_rate = (double)NERDJACK_CLOCK_RATE / (double)0x0ffffe;
  55. *period = 0x0ffffe;
  56. return -1;
  57. }
  58. //Period holds the period register for the NerdJack, so it needs to be right
  59. *actual_rate = (double)NERDJACK_CLOCK_RATE / (double)*period;
  60. if (*actual_rate != desired_rate) {
  61. return -1;
  62. }
  63. return 0;
  64. }
  65. /**
  66. * Create a discovered socket and add it to the socket list structure.
  67. * All sockets in the structure should be created, bound, and ready for broadcasting
  68. */
  69. static int discovered_sock_create(struct discover_t *ds, uint32_t local_ip,
  70. uint32_t subnet_mask)
  71. {
  72. if (ds->sock_count >= MAX_SOCKETS) {
  73. return 0;
  74. }
  75. /* Create socket. */
  76. int sock = (int)socket(AF_INET, SOCK_DGRAM, 0);
  77. if (sock == -1) {
  78. return 0;
  79. }
  80. /* Allow broadcast. */
  81. int sock_opt = 1;
  82. setsockopt(sock, SOL_SOCKET, SO_BROADCAST, (char *)&sock_opt,
  83. sizeof(sock_opt));
  84. /* Set nonblocking */
  85. if (soblock(sock, 0) < 0) {
  86. verb("can't set nonblocking\n");
  87. close(sock);
  88. return 0;
  89. }
  90. /* Bind socket. */
  91. struct sockaddr_in sock_addr;
  92. memset(&sock_addr, 0, sizeof(sock_addr));
  93. sock_addr.sin_family = AF_INET;
  94. sock_addr.sin_addr.s_addr = htonl(local_ip);
  95. sock_addr.sin_port = htons(0);
  96. if (bind(sock, (struct sockaddr *)&sock_addr, sizeof(sock_addr)) != 0) {
  97. close(sock);
  98. return 0;
  99. }
  100. /* Write sock entry. */
  101. struct discovered_socket *dss = &ds->socks[ds->sock_count++];
  102. dss->sock = sock;
  103. dss->local_ip = local_ip;
  104. dss->subnet_mask = subnet_mask;
  105. return 1;
  106. }
  107. /**
  108. * Enumerate all interfaces we can find and open sockets on each
  109. */
  110. #if defined(USE_IPHLPAPI)
  111. static void enumerate_interfaces(struct discover_t *ds)
  112. {
  113. PIP_ADAPTER_INFO pAdapterInfo =
  114. (IP_ADAPTER_INFO *) malloc(sizeof(IP_ADAPTER_INFO));
  115. ULONG ulOutBufLen = sizeof(IP_ADAPTER_INFO);
  116. DWORD Ret = GetAdaptersInfo(pAdapterInfo, &ulOutBufLen);
  117. if (Ret != NO_ERROR) {
  118. free(pAdapterInfo);
  119. if (Ret != ERROR_BUFFER_OVERFLOW) {
  120. return;
  121. }
  122. pAdapterInfo = (IP_ADAPTER_INFO *) malloc(ulOutBufLen);
  123. Ret = GetAdaptersInfo(pAdapterInfo, &ulOutBufLen);
  124. if (Ret != NO_ERROR) {
  125. free(pAdapterInfo);
  126. return;
  127. }
  128. }
  129. PIP_ADAPTER_INFO pAdapter = pAdapterInfo;
  130. while (pAdapter) {
  131. IP_ADDR_STRING *pIPAddr = &pAdapter->IpAddressList;
  132. while (pIPAddr) {
  133. uint32_t local_ip =
  134. ntohl(inet_addr(pIPAddr->IpAddress.String));
  135. uint32_t mask =
  136. ntohl(inet_addr(pIPAddr->IpMask.String));
  137. if (local_ip == 0) {
  138. pIPAddr = pIPAddr->Next;
  139. continue;
  140. }
  141. discovered_sock_create(ds, local_ip, mask);
  142. pIPAddr = pIPAddr->Next;
  143. }
  144. pAdapter = pAdapter->Next;
  145. }
  146. free(pAdapterInfo);
  147. }
  148. #else
  149. static void enumerate_interfaces(struct discover_t *ds)
  150. {
  151. int fd = socket(AF_INET, SOCK_DGRAM, 0);
  152. if (fd == -1) {
  153. return;
  154. }
  155. struct ifconf ifc;
  156. uint8_t buf[8192];
  157. ifc.ifc_len = sizeof(buf);
  158. ifc.ifc_buf = (char *)buf;
  159. memset(buf, 0, sizeof(buf));
  160. if (ioctl(fd, SIOCGIFCONF, &ifc) != 0) {
  161. close(fd);
  162. return;
  163. }
  164. uint8_t *ptr = (uint8_t *) ifc.ifc_req;
  165. uint8_t *end = (uint8_t *) & ifc.ifc_buf[ifc.ifc_len];
  166. while (ptr <= end) {
  167. struct ifreq *ifr = (struct ifreq *)ptr;
  168. ptr += _SIZEOF_ADDR_IFREQ(*ifr);
  169. if (ioctl(fd, SIOCGIFADDR, ifr) != 0) {
  170. continue;
  171. }
  172. struct sockaddr_in *addr_in =
  173. (struct sockaddr_in *)&(ifr->ifr_addr);
  174. uint32_t local_ip = ntohl(addr_in->sin_addr.s_addr);
  175. if (local_ip == 0) {
  176. continue;
  177. }
  178. if (ioctl(fd, SIOCGIFNETMASK, ifr) != 0) {
  179. continue;
  180. }
  181. struct sockaddr_in *mask_in =
  182. (struct sockaddr_in *)&(ifr->ifr_addr);
  183. uint32_t mask = ntohl(mask_in->sin_addr.s_addr);
  184. discovered_sock_create(ds, local_ip, mask);
  185. }
  186. }
  187. #endif
  188. /**
  189. * Close all sockets previously enumerated and free the struct
  190. */
  191. static void destroy_socks(struct discover_t *ds)
  192. {
  193. unsigned int i;
  194. for (i = 0; i < ds->sock_count; i++) {
  195. struct discovered_socket *dss = &ds->socks[i];
  196. close(dss->sock);
  197. }
  198. free(ds);
  199. }
  200. /* Perform autodetection. Returns 0 on success, -1 on error
  201. * Sets ipAddress to the detected address
  202. */
  203. int nerdjack_detect(char *ipAddress)
  204. {
  205. int32_t receivesock;
  206. struct sockaddr_in sa, receiveaddr, sFromAddr;
  207. int buffer_length;
  208. char buffer[200];
  209. char incomingData[10];
  210. socklen_t lFromLen;
  211. sprintf(buffer, "TEST");
  212. buffer_length = strlen(buffer) + 1;
  213. net_init();
  214. receivesock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
  215. /* Set nonblocking */
  216. if (soblock(receivesock, 0) < 0) {
  217. verb("can't set nonblocking\n");
  218. return -1;
  219. }
  220. if (-1 == receivesock) { /* if socket failed to initialize, exit */
  221. verb("Error Creating Socket\n");
  222. return -1;
  223. }
  224. //Setup family for both sockets
  225. sa.sin_family = PF_INET;
  226. receiveaddr.sin_family = PF_INET;
  227. //Setup ports to send on DATA and receive on RECEIVE
  228. receiveaddr.sin_port = htons(NERDJACK_UDP_RECEIVE_PORT);
  229. sa.sin_port = htons(NERDJACK_DATA_PORT);
  230. //Receive from any IP address
  231. receiveaddr.sin_addr.s_addr = INADDR_ANY;
  232. bind(receivesock, (struct sockaddr *)&receiveaddr,
  233. sizeof(struct sockaddr_in));
  234. struct discover_t *ds =
  235. (struct discover_t *)calloc(1, sizeof(struct discover_t));
  236. if (!ds) {
  237. return -1;
  238. }
  239. /* Create a routable broadcast socket. */
  240. if (!discovered_sock_create(ds, 0, 0)) {
  241. free(ds);
  242. return -1;
  243. }
  244. /* Detect & create local sockets. */
  245. enumerate_interfaces(ds);
  246. /*
  247. * Send subnet broadcast using each local ip socket.
  248. * This will work with multiple separate 169.254.x.x interfaces.
  249. */
  250. unsigned int i;
  251. for (i = 0; i < ds->sock_count; i++) {
  252. struct discovered_socket *dss = &ds->socks[i];
  253. uint32_t target_ip = dss->local_ip | ~dss->subnet_mask;
  254. sa.sin_addr.s_addr = htonl(target_ip);
  255. sendto(dss->sock, buffer, buffer_length, 0,
  256. (struct sockaddr *)&sa, sizeof(struct sockaddr_in));
  257. }
  258. destroy_socks(ds);
  259. lFromLen = sizeof(sFromAddr);
  260. if (0 >
  261. recvfrom_timeout(receivesock, incomingData, sizeof(incomingData), 0,
  262. (struct sockaddr *)&sFromAddr, &lFromLen,
  263. &(struct timeval) {
  264. .tv_sec = TIMEOUT})) {
  265. close(receivesock);
  266. return -1;
  267. }
  268. ipAddress = malloc(INET_ADDRSTRLEN);
  269. //It isn't ipv6 friendly, but inet_ntop isn't on Windows...
  270. strcpy(ipAddress, inet_ntoa(sFromAddr.sin_addr));
  271. close(receivesock);
  272. return 0;
  273. }
  274. /*
  275. * Get the NerdJack version string and print it
  276. */
  277. int nerd_get_version(const char *address)
  278. {
  279. int ret, fd_command;
  280. char buf[200];
  281. fd_command = nerd_open(address, NERDJACK_COMMAND_PORT);
  282. if (fd_command < 0) {
  283. info("Connect failed: %s:%d\n", address, NERDJACK_COMMAND_PORT);
  284. return -2;
  285. }
  286. /* Send request */
  287. ret = send_all_timeout(fd_command, "VERS", 4, 0, &(struct timeval) {
  288. .tv_sec = TIMEOUT});
  289. if (ret < 0) {
  290. verb("short send %d\n", (int)ret);
  291. return -1;
  292. }
  293. ret = recv_all_timeout(fd_command, buf, 200, 0, &(struct timeval) {
  294. .tv_sec = TIMEOUT});
  295. nerd_close_conn(fd_command);
  296. if (ret < 0) {
  297. verb("Error receiving command\n");
  298. return -1;
  299. }
  300. //Slice off the "OK" from the string
  301. buf[strlen(buf) - 2] = '\0';
  302. printf("%s\n", buf);
  303. return 0;
  304. }
  305. /* Send the given command to address. The command should be something
  306. * of the specified length. This expects the NerdJack to reply with OK
  307. * or NO
  308. */
  309. int nerd_send_command(const char *address, void *command, int length)
  310. {
  311. int ret, fd_command;
  312. char buf[3];
  313. fd_command = nerd_open(address, NERDJACK_COMMAND_PORT);
  314. if (fd_command < 0) {
  315. info("Connect failed: %s:%d\n", address, NERDJACK_COMMAND_PORT);
  316. return -2;
  317. }
  318. /* Send request */
  319. ret = send_all_timeout(fd_command, command, length, 0, &(struct timeval) {
  320. .tv_sec = TIMEOUT});
  321. if (ret < 0 || ret != length) {
  322. verb("short send %d\n", (int)ret);
  323. return -1;
  324. }
  325. ret = recv_all_timeout(fd_command, buf, 3, 0, &(struct timeval) {
  326. .tv_sec = TIMEOUT});
  327. nerd_close_conn(fd_command);
  328. if (ret < 0 || ret != 3) {
  329. verb("Error receiving OK for command\n");
  330. return -1;
  331. }
  332. if (0 != strcmp("OK", buf)) {
  333. verb("Did not receive OK. Received %s\n", buf);
  334. return -4;
  335. }
  336. return 0;
  337. }
  338. int
  339. nerd_data_stream(int data_fd, int numChannels, int *channel_list,
  340. int precision, int convert, int lines, int showmem,
  341. unsigned short *currentcount, unsigned int period,
  342. int wasreset)
  343. {
  344. //Variables that should persist across retries
  345. static dataPacket buf;
  346. static int linesleft = 0;
  347. static int linesdumped = 0;
  348. //Variables essential to packet processing
  349. signed short datapoint = 0;
  350. int i;
  351. int numChannelsSampled = channel_list[0] + 1;
  352. //The number sampled will be the highest channel requested plus 1
  353. //(i.e. channel 0 requested means 1 sampled)
  354. for (i = 0; i < numChannels; i++) {
  355. if (channel_list[i] + 1 > numChannelsSampled)
  356. numChannelsSampled = channel_list[i] + 1;
  357. }
  358. double voltline[numChannels];
  359. unsigned short dataline[numChannels];
  360. unsigned short packetsready = 0;
  361. unsigned short adcused = 0;
  362. unsigned short tempshort = 0;
  363. int charsread = 0;
  364. int numgroupsProcessed = 0;
  365. double volts;
  366. //The timeout should be the expected time plus 60 seconds
  367. //This permits slower speeds to work properly
  368. unsigned int expectedtimeout =
  369. (period * NERDJACK_NUM_SAMPLES / NERDJACK_CLOCK_RATE) + 60;
  370. //Check to see if we're trying to resume
  371. //Don't blow away linesleft in that case
  372. if (lines != 0 && linesleft == 0) {
  373. linesleft = lines;
  374. }
  375. //If there was a reset, we still need to dump a line because of faulty PDCA start
  376. if (wasreset) {
  377. linesdumped = 0;
  378. }
  379. //If this is the first time called, warn the user if we're too fast
  380. if (linesdumped == 0) {
  381. if (period < (numChannelsSampled * 200 + 600)) {
  382. info("You are sampling close to the limit of NerdJack\n");
  383. info("Sample fewer channels or sample slower\n");
  384. }
  385. }
  386. //Now destination structure array is set as well as numDuplicates.
  387. int totalGroups = NERDJACK_NUM_SAMPLES / numChannelsSampled;
  388. //Loop forever to grab data
  389. while ((charsread =
  390. recv_all_timeout(data_fd, &buf, NERDJACK_PACKET_SIZE, 0,
  391. &(struct timeval) {
  392. .tv_sec = expectedtimeout}))) {
  393. if (charsread != NERDJACK_PACKET_SIZE) {
  394. //There was a problem getting data. Probably a closed
  395. //connection.
  396. info("Packet timed out or was too short\n");
  397. return -2;
  398. }
  399. //First check the header info
  400. if (buf.headerone != 0xF0 || buf.headertwo != 0xAA) {
  401. info("No Header info\n");
  402. return -1;
  403. }
  404. //Check counter info to make sure not out of order
  405. tempshort = ntohs(buf.packetNumber);
  406. if (tempshort != *currentcount) {
  407. info("Count wrong. Expected %hd but got %hd\n",
  408. *currentcount, tempshort);
  409. return -1;
  410. }
  411. //Increment number of packets received
  412. *currentcount = *currentcount + 1;
  413. adcused = ntohs(buf.adcused);
  414. packetsready = ntohs(buf.packetsready);
  415. numgroupsProcessed = 0;
  416. if (showmem) {
  417. printf("%hd %hd\n", adcused, packetsready);
  418. continue;
  419. }
  420. //While there is still more data in the packet, process it
  421. while (numgroupsProcessed < totalGroups) {
  422. //Poison the data structure
  423. switch (convert) {
  424. case CONVERT_VOLTS:
  425. memset(voltline, 0,
  426. numChannels * sizeof(double));
  427. break;
  428. default:
  429. case CONVERT_HEX:
  430. case CONVERT_DEC:
  431. memset(dataline, 0,
  432. numChannels * sizeof(unsigned char));
  433. }
  434. //Read in each group
  435. for (i = 0; i < numChannels; i++) {
  436. //Get the datapoint associated with the desired channel
  437. datapoint =
  438. ntohs(buf.data[channel_list[i] +
  439. numgroupsProcessed *
  440. numChannelsSampled]);
  441. //Place it into the line
  442. switch (convert) {
  443. case CONVERT_VOLTS:
  444. if (channel_list[i] <= 5) {
  445. volts =
  446. (double)(datapoint /
  447. 32767.0) *
  448. ((precision & 0x01) ? 5.0 :
  449. 10.0);
  450. } else {
  451. volts =
  452. (double)(datapoint /
  453. 32767.0) *
  454. ((precision & 0x02) ? 5.0 :
  455. 10.0);
  456. }
  457. voltline[i] = volts;
  458. break;
  459. default:
  460. case CONVERT_HEX:
  461. case CONVERT_DEC:
  462. dataline[i] =
  463. (unsigned short)(datapoint -
  464. INT16_MIN);
  465. break;
  466. }
  467. }
  468. //We want to dump the first line because it's usually spurious
  469. if (linesdumped != 0) {
  470. //Now print the group
  471. switch (convert) {
  472. case CONVERT_VOLTS:
  473. for (i = 0; i < numChannels; i++) {
  474. if (printf("%lf ", voltline[i])
  475. < 0)
  476. goto bad;
  477. }
  478. break;
  479. case CONVERT_HEX:
  480. for (i = 0; i < numChannels; i++) {
  481. if (printf("%04hX", dataline[i])
  482. < 0)
  483. goto bad;
  484. }
  485. break;
  486. default:
  487. case CONVERT_DEC:
  488. for (i = 0; i < numChannels; i++) {
  489. if (printf("%hu ", dataline[i])
  490. < 0)
  491. goto bad;
  492. }
  493. break;
  494. }
  495. if (printf("\n") < 0)
  496. goto bad;
  497. //If we're counting lines, decrement them
  498. if (lines != 0) {
  499. linesleft--;
  500. if (linesleft == 0) {
  501. return 0;
  502. }
  503. }
  504. } else {
  505. linesdumped = linesdumped + 1;
  506. }
  507. //We've processed this group, so advance the counter
  508. numgroupsProcessed++;
  509. }
  510. }
  511. return 0;
  512. bad:
  513. info("Output error (disk full?)\n");
  514. return -3;
  515. }
  516. /* Open a connection to the NerdJack */
  517. int nerd_open(const char *address, int port)
  518. {
  519. struct hostent *he;
  520. net_init();
  521. int32_t i32SocketFD = socket(PF_INET, SOCK_STREAM, 0);
  522. if (-1 == i32SocketFD) {
  523. verb("cannot create socket");
  524. return -1;
  525. }
  526. /* Set nonblocking */
  527. if (soblock(i32SocketFD, 0) < 0) {
  528. verb("can't set nonblocking\n");
  529. return -1;
  530. }
  531. struct sockaddr_in stSockAddr;
  532. memset(&stSockAddr, 0, sizeof(stSockAddr));
  533. stSockAddr.sin_family = AF_INET;
  534. stSockAddr.sin_port = htons(port);
  535. he = gethostbyname(address);
  536. if (he == NULL) {
  537. verb("gethostbyname(\"%s\") failed\n", address);
  538. return -1;
  539. }
  540. stSockAddr.sin_addr = *((struct in_addr *)he->h_addr);
  541. debug("Resolved %s -> %s\n", address, inet_ntoa(stSockAddr.sin_addr));
  542. /* Connect */
  543. if (connect_timeout
  544. (i32SocketFD, (struct sockaddr *)&stSockAddr, sizeof(stSockAddr),
  545. &(struct timeval) {
  546. .tv_sec = 3}) < 0) {
  547. verb("connection to %s:%d failed: %s\n",
  548. inet_ntoa(stSockAddr.sin_addr), port,
  549. compat_strerror(errno));
  550. return -1;
  551. }
  552. return i32SocketFD;
  553. }
  554. //Generate an appropriate sample initiation command
  555. int
  556. nerd_generate_command(getPacket * command, int *channel_list,
  557. int channel_count, int precision, unsigned long period)
  558. {
  559. short channelbit = 0;
  560. int i;
  561. int highestchannel = 0;
  562. for (i = 0; i < channel_count; i++) {
  563. if (channel_list[i] > highestchannel) {
  564. highestchannel = channel_list[i];
  565. }
  566. //channelbit = channelbit | (0x1 << channel_list[i]);
  567. }
  568. for (i = 0; i <= highestchannel; i++) {
  569. channelbit = channelbit | (0x01 << i);
  570. }
  571. command->word[0] = 'G';
  572. command->word[1] = 'E';
  573. command->word[2] = 'T';
  574. command->word[3] = 'D';
  575. command->channelbit = htons(channelbit);
  576. command->precision = precision;
  577. command->period = htonl(period);
  578. command->prescaler = 0;
  579. return 0;
  580. }
  581. int nerd_close_conn(int data_fd)
  582. {
  583. shutdown(data_fd, 2);
  584. close(data_fd);
  585. return 0;
  586. }