PageRenderTime 60ms CodeModel.GetById 29ms RepoModel.GetById 0ms app.codeStats 0ms

/examples/jackd-talker/jackd_talker.c

https://github.com/masche842/Open-AVB
C | 1155 lines | 966 code | 114 blank | 75 comment | 145 complexity | c0b76945376301c51b043333a5060a97 MD5 | raw file
Possible License(s): BSD-3-Clause, LGPL-2.0
  1. /******************************************************************************
  2. Copyright (c) 2012, Intel Corporation
  3. All rights reserved.
  4. Redistribution and use in source and binary forms, with or without
  5. modification, are permitted provided that the following conditions are met:
  6. 1. Redistributions of source code must retain the above copyright notice,
  7. this list of conditions and the following disclaimer.
  8. 2. Redistributions in binary form must reproduce the above copyright
  9. notice, this list of conditions and the following disclaimer in the
  10. documentation and/or other materials provided with the distribution.
  11. 3. Neither the name of the Intel Corporation nor the names of its
  12. contributors may be used to endorse or promote products derived from
  13. this software without specific prior written permission.
  14. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  15. AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  16. IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  17. ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  18. LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  19. CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  20. SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  21. INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  22. CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  23. ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  24. POSSIBILITY OF SUCH DAMAGE.
  25. ******************************************************************************/
  26. #include <unistd.h>
  27. #include <fcntl.h>
  28. #include <stdlib.h>
  29. #include <stdio.h>
  30. #include <string.h>
  31. #include <syslog.h>
  32. #include <signal.h>
  33. #include <errno.h>
  34. #include <sys/ioctl.h>
  35. #include <sys/time.h>
  36. #include <sys/resource.h>
  37. #include <sys/mman.h>
  38. #include <sys/user.h>
  39. #include <pci/pci.h>
  40. #include <sys/socket.h>
  41. #include <linux/if.h>
  42. #include <netpacket/packet.h>
  43. #include <netinet/in.h>
  44. #include <arpa/inet.h>
  45. #include <net/ethernet.h>
  46. #include <sys/un.h>
  47. #include <pthread.h>
  48. #include <poll.h>
  49. #include <sys/mman.h>
  50. #include <sys/stat.h>
  51. #include <fcntl.h>
  52. #include "igb.h"
  53. #include "mrpd.h"
  54. #include "mrp.h"
  55. #include "msrp.h"
  56. #include <math.h>
  57. #include <endian.h>
  58. #include <stdint.h>
  59. #include <jack/weakmacros.h>
  60. #include <jack/jack.h>
  61. #include <jack/ringbuffer.h>
  62. #include <jack/thread.h>
  63. #include "jack.h"
  64. #include "defines.h"
  65. typedef struct {
  66. int64_t ml_phoffset;
  67. int64_t ls_phoffset;
  68. long double ml_freqoffset;
  69. long double ls_freqoffset;
  70. int64_t local_time;
  71. } gPtpTimeData;
  72. #define SHM_SIZE 4*8 + sizeof(pthread_mutex_t) /* 3 - 64 bit and 2 - 32 bits */
  73. #define SHM_NAME "/ptp"
  74. #define MAX_SAMPLE_VALUE ((1U << ((sizeof(int32_t)*8)-1))-1)
  75. #define SRC_CHANNELS (2)
  76. #define SAMPLES_PER_SECOND (48000)
  77. #define FREQUENCY (480)
  78. #define SAMPLES_PER_CYCLE (SAMPLES_PER_SECOND/FREQUENCY)
  79. #define GAIN (.5)
  80. //1722 header
  81. #define ETH_TYPE 0x22F0
  82. #define CD_SUBTYPE 0x02 /* for simple audio format */
  83. #define SV_VER_MR_RS_GV_TV 0x81
  84. #define RS_TU 0x00
  85. #define SAMPLE_FORMAT_NON_INTR_FLOAT 0x02
  86. #define NOMINAL_SAMPLE_RATE 0x09
  87. #define LINEAR_SAMPLE_MSB 0x20
  88. unsigned char GATEWAY_INFO[] =
  89. { SAMPLE_FORMAT_NON_INTR_FLOAT, 0, NOMINAL_SAMPLE_RATE, LINEAR_SAMPLE_MSB };
  90. #define PAYLOAD_SIZE SAMPLE_SIZE*SAMPLES_PER_FRAME*CHANNELS /* 6*4 * 2 channels = 48 bytes */
  91. #define IGB_BIND_NAMESZ 24
  92. #define XMIT_DELAY (200000000) /* us */
  93. #define RENDER_DELAY (XMIT_DELAY+2000000) /* us */
  94. typedef enum { false = 0, true = 1 } bool;
  95. typedef struct __attribute__ ((packed)) {
  96. uint64_t subtype:7;
  97. uint64_t cd_indicator:1;
  98. uint64_t timestamp_valid:1;
  99. uint64_t gateway_valid:1;
  100. uint64_t reserved0:1;
  101. uint64_t reset:1;
  102. uint64_t version:3;
  103. uint64_t sid_valid:1;
  104. uint64_t seq_number:8;
  105. uint64_t timestamp_uncertain:1;
  106. uint64_t reserved1:7;
  107. uint64_t stream_id;
  108. uint64_t timestamp:32;
  109. uint64_t gateway_info:32;
  110. uint64_t length:16;
  111. } seventeen22_header;
  112. /* 61883 CIP with SYT Field */
  113. typedef struct {
  114. uint16_t packet_channel:6;
  115. uint16_t format_tag:2;
  116. uint16_t app_control:4;
  117. uint16_t packet_tcode:4;
  118. uint16_t source_id:6;
  119. uint16_t reserved0:2;
  120. uint16_t data_block_size:8;
  121. uint16_t reserved1:2;
  122. uint16_t source_packet_header:1;
  123. uint16_t quadlet_padding_count:3;
  124. uint16_t fraction_number:2;
  125. uint16_t data_block_continuity:8;
  126. uint16_t format_id:6;
  127. uint16_t eoh:2;
  128. uint16_t format_dependent_field:8;
  129. uint16_t syt;
  130. } six1883_header;
  131. typedef struct {
  132. uint8_t label;
  133. uint8_t value[3];
  134. } six1883_sample;
  135. /* global variables */
  136. int control_socket = -1;
  137. device_t igb_dev;
  138. volatile int halt_tx = 0;
  139. volatile int listeners = 0;
  140. volatile int unleash_jack = 0;
  141. volatile int mrp_okay;
  142. volatile int mrp_error = 0;;
  143. volatile int domain_a_valid = 0;
  144. int domain_class_a_id;
  145. int domain_class_a_priority;
  146. int domain_class_a_vid;
  147. volatile int domain_b_valid = 0;
  148. int domain_class_b_id;
  149. int domain_class_b_priority;
  150. int domain_class_b_vid;
  151. pthread_t packetizer_id;
  152. /* evil global variables
  153. * FIXME: move to thread argument struct */
  154. u_int64_t last_time;
  155. int seqnum;
  156. uint32_t time_stamp;
  157. seventeen22_header *header0;
  158. six1883_header *header1;
  159. struct igb_packet *tmp_packet;
  160. struct igb_packet *free_packets;
  161. #define VERSION_STR "1.0"
  162. static const char *version_str = "simple_talker v" VERSION_STR "\n"
  163. "Copyright (c) 2012, Intel Corporation\n";
  164. #define MRPD_PORT_DEFAULT 7500
  165. static inline uint64_t ST_rdtsc(void)
  166. {
  167. uint64_t ret;
  168. unsigned c, d;
  169. asm volatile ("rdtsc":"=a" (c), "=d"(d));
  170. ret = d;
  171. ret <<= 32;
  172. ret |= c;
  173. return ret;
  174. }
  175. static int shm_fd = -1;
  176. static char *memory_offset_buffer = NULL;
  177. int gptpinit(void)
  178. {
  179. shm_fd = shm_open(SHM_NAME, O_RDWR, 0);
  180. if (shm_fd == -1) {
  181. perror("shm_open()");
  182. return false;
  183. }
  184. memory_offset_buffer =
  185. (char *)mmap(NULL, SHM_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED,
  186. shm_fd, 0);
  187. if (memory_offset_buffer == (char *)-1) {
  188. perror("mmap()");
  189. memory_offset_buffer = NULL;
  190. shm_unlink(SHM_NAME);
  191. return false;
  192. }
  193. return true;
  194. }
  195. void gptpdeinit(void)
  196. {
  197. if (memory_offset_buffer != NULL) {
  198. munmap(memory_offset_buffer, SHM_SIZE);
  199. }
  200. if (shm_fd != -1) {
  201. close(shm_fd);
  202. }
  203. }
  204. int gptpscaling(gPtpTimeData * td)
  205. {
  206. pthread_mutex_lock((pthread_mutex_t *) memory_offset_buffer);
  207. memcpy(td, memory_offset_buffer + sizeof(pthread_mutex_t), sizeof(*td));
  208. pthread_mutex_unlock((pthread_mutex_t *) memory_offset_buffer);
  209. fprintf(stderr, "ml_phoffset = %lld, ls_phoffset = %lld\n",
  210. td->ml_phoffset, td->ls_phoffset);
  211. fprintf(stderr, "ml_freqffset = %Lf, ls_freqoffset = %Lf\n",
  212. td->ml_freqoffset, td->ls_freqoffset);
  213. return true;
  214. }
  215. int mrp_join_listener(uint8_t * streamid);
  216. int send_mrp_msg(char *notify_data, int notify_len)
  217. {
  218. struct sockaddr_in addr;
  219. socklen_t addr_len;
  220. memset(&addr, 0, sizeof(addr));
  221. addr.sin_family = AF_INET;
  222. addr.sin_port = htons(MRPD_PORT_DEFAULT);
  223. inet_aton("127.0.0.1", &addr.sin_addr);
  224. addr_len = sizeof(addr);
  225. if (control_socket != -1)
  226. return (sendto
  227. (control_socket, notify_data, notify_len, 0,
  228. (struct sockaddr *)&addr, addr_len));
  229. else
  230. return (0);
  231. }
  232. int mrp_connect()
  233. {
  234. struct sockaddr_in addr;
  235. int sock_fd = -1;
  236. sock_fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
  237. if (sock_fd < 0)
  238. goto out;
  239. memset(&addr, 0, sizeof(addr));
  240. addr.sin_family = AF_INET;
  241. addr.sin_port = htons(MRPD_PORT_DEFAULT);
  242. inet_aton("127.0.0.1", &addr.sin_addr);
  243. memset(&addr, 0, sizeof(addr));
  244. control_socket = sock_fd;
  245. return (0);
  246. out: if (sock_fd != -1)
  247. close(sock_fd);
  248. sock_fd = -1;
  249. return (-1);
  250. }
  251. int mrp_disconnect()
  252. {
  253. char *msgbuf;
  254. int rc;
  255. msgbuf = malloc(64);
  256. if (NULL == msgbuf)
  257. return -1;
  258. memset(msgbuf, 0, 64);
  259. sprintf(msgbuf, "BYE");
  260. mrp_okay = 0;
  261. rc = send_mrp_msg(msgbuf, 1500);
  262. /* rc = recv_mrp_okay(); */
  263. free(msgbuf);
  264. return rc;
  265. }
  266. int recv_mrp_okay()
  267. {
  268. while ((mrp_okay == 0) && (mrp_error == 0))
  269. usleep(20000);
  270. return 0;
  271. }
  272. int mrp_register_domain(int *class_id, int *priority, u_int16_t * vid)
  273. {
  274. char *msgbuf;
  275. int rc;
  276. msgbuf = malloc(64);
  277. if (NULL == msgbuf)
  278. return -1;
  279. memset(msgbuf, 0, 64);
  280. sprintf(msgbuf, "S+D:C=%d,P=%d,V=%04x", *class_id, *priority, *vid);
  281. mrp_okay = 0;
  282. rc = send_mrp_msg(msgbuf, 1500);
  283. /* rc = recv_mrp_okay(); */
  284. free(msgbuf);
  285. return rc;
  286. }
  287. int mrp_get_domain(int *class_a_id, int *a_priority, u_int16_t * a_vid,
  288. int *class_b_id, int *b_priority, u_int16_t * b_vid)
  289. {
  290. char *msgbuf;
  291. /* we may not get a notification if we are joining late,
  292. * so query for what is already there ...
  293. */
  294. msgbuf = malloc(64);
  295. if (NULL == msgbuf)
  296. return -1;
  297. memset(msgbuf, 0, 64);
  298. sprintf(msgbuf, "S??");
  299. send_mrp_msg(msgbuf, 64);
  300. free(msgbuf);
  301. while (!halt_tx && (domain_a_valid == 0) && (domain_b_valid == 0))
  302. usleep(20000);
  303. *class_a_id = 0;
  304. *a_priority = 0;
  305. *a_vid = 0;
  306. *class_b_id = 0;
  307. *b_priority = 0;
  308. *b_vid = 0;
  309. if (domain_a_valid) {
  310. *class_a_id = domain_class_a_id;
  311. *a_priority = domain_class_a_priority;
  312. *a_vid = domain_class_a_vid;
  313. }
  314. if (domain_b_valid) {
  315. *class_b_id = domain_class_b_id;
  316. *b_priority = domain_class_b_priority;
  317. *b_vid = domain_class_b_vid;
  318. }
  319. return (0);
  320. }
  321. unsigned char monitor_stream_id[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
  322. int mrp_await_listener(unsigned char *streamid)
  323. {
  324. char *msgbuf;
  325. memcpy(monitor_stream_id, streamid, sizeof(monitor_stream_id));
  326. msgbuf = malloc(64);
  327. if (NULL == msgbuf)
  328. return -1;
  329. memset(msgbuf, 0, 64);
  330. sprintf(msgbuf, "S??");
  331. send_mrp_msg(msgbuf, 64);
  332. free(msgbuf);
  333. /* either already there ... or need to wait ... */
  334. while (!halt_tx && (listeners == 0))
  335. usleep(20000);
  336. return (0);
  337. }
  338. int process_mrp_msg(char *buf, int buflen)
  339. {
  340. /*
  341. * 1st character indicates application
  342. * [MVS] - MAC, VLAN or STREAM
  343. */
  344. unsigned int id;
  345. unsigned int priority;
  346. unsigned int vid;
  347. int i, j, k;
  348. unsigned int substate;
  349. unsigned char recovered_streamid[8];
  350. k = 0;
  351. next_line:if (k >= buflen)
  352. return (0);
  353. switch (buf[k]) {
  354. case 'E':
  355. printf("%s from mrpd\n", buf);
  356. fflush(stdout);
  357. mrp_error = 1;
  358. break;
  359. case 'O':
  360. mrp_okay = 1;
  361. break;
  362. case 'M':
  363. case 'V':
  364. printf("%s unhandled from mrpd\n", buf);
  365. fflush(stdout);
  366. /* unhandled for now */
  367. break;
  368. case 'L':
  369. /* parse a listener attribute - see if it matches our monitor_stream_id */
  370. i = k;
  371. while (buf[i] != 'D')
  372. i++;
  373. i += 2; /* skip the ':' */
  374. sscanf(&(buf[i]), "%d", &substate);
  375. while (buf[i] != 'S')
  376. i++;
  377. i += 2; /* skip the ':' */
  378. for (j = 0; j < 8; j++) {
  379. sscanf(&(buf[i + 2 * j]), "%02x", &id);
  380. recovered_streamid[j] = (unsigned char)id;
  381. } printf
  382. ("FOUND STREAM ID=%02x%02x%02x%02x%02x%02x%02x%02x ",
  383. recovered_streamid[0], recovered_streamid[1],
  384. recovered_streamid[2], recovered_streamid[3],
  385. recovered_streamid[4], recovered_streamid[5],
  386. recovered_streamid[6], recovered_streamid[7]);
  387. switch (substate) {
  388. case 0:
  389. printf("with state ignore\n");
  390. break;
  391. case 1:
  392. printf("with state askfailed\n");
  393. break;
  394. case 2:
  395. printf("with state ready\n");
  396. break;
  397. case 3:
  398. printf("with state readyfail\n");
  399. break;
  400. default:
  401. printf("with state UNKNOWN (%d)\n", substate);
  402. break;
  403. }
  404. if (substate > MSRP_LISTENER_ASKFAILED) {
  405. if (memcmp
  406. (recovered_streamid, monitor_stream_id,
  407. sizeof(recovered_streamid)) == 0) {
  408. listeners = 1;
  409. printf("added listener\n");
  410. }
  411. }
  412. fflush(stdout);
  413. /* try to find a newline ... */
  414. while ((i < buflen) && (buf[i] != '\n') && (buf[i] != '\0'))
  415. i++;
  416. if (i == buflen)
  417. return (0);
  418. if (buf[i] == '\0')
  419. return (0);
  420. i++;
  421. k = i;
  422. goto next_line;
  423. break;
  424. case 'D':
  425. i = k + 4;
  426. /* save the domain attribute */
  427. sscanf(&(buf[i]), "%d", &id);
  428. while (buf[i] != 'P')
  429. i++;
  430. i += 2; /* skip the ':' */
  431. sscanf(&(buf[i]), "%d", &priority);
  432. while (buf[i] != 'V')
  433. i++;
  434. i += 2; /* skip the ':' */
  435. sscanf(&(buf[i]), "%x", &vid);
  436. if (id == 6) {
  437. domain_class_a_id = id;
  438. domain_class_a_priority = priority;
  439. domain_class_a_vid = vid;
  440. domain_a_valid = 1;
  441. } else {
  442. domain_class_b_id = id;
  443. domain_class_b_priority = priority;
  444. domain_class_b_vid = vid;
  445. domain_b_valid = 1;
  446. }
  447. while ((i < buflen) && (buf[i] != '\n') && (buf[i] != '\0'))
  448. i++;
  449. if ((i == buflen) || (buf[i] == '\0'))
  450. return (0);
  451. i++;
  452. k = i;
  453. goto next_line;
  454. break;
  455. case 'T':
  456. /* as simple_talker we don't care about other talkers */
  457. i = k;
  458. while ((i < buflen) && (buf[i] != '\n') && (buf[i] != '\0'))
  459. i++;
  460. if (i == buflen)
  461. return (0);
  462. if (buf[i] == '\0')
  463. return (0);
  464. i++;
  465. k = i;
  466. goto next_line;
  467. break;
  468. case 'S':
  469. /* handle the leave/join events */
  470. switch (buf[k + 4]) {
  471. case 'L':
  472. i = k + 5;
  473. while (buf[i] != 'D')
  474. i++;
  475. i += 2; /* skip the ':' */
  476. sscanf(&(buf[i]), "%d", &substate);
  477. while (buf[i] != 'S')
  478. i++;
  479. i += 2; /* skip the ':' */
  480. for (j = 0; j < 8; j++) {
  481. sscanf(&(buf[i + 2 * j]), "%02x", &id);
  482. recovered_streamid[j] = (unsigned char)id;
  483. } printf
  484. ("EVENT on STREAM ID=%02x%02x%02x%02x%02x%02x%02x%02x ",
  485. recovered_streamid[0], recovered_streamid[1],
  486. recovered_streamid[2], recovered_streamid[3],
  487. recovered_streamid[4], recovered_streamid[5],
  488. recovered_streamid[6], recovered_streamid[7]);
  489. switch (substate) {
  490. case 0:
  491. printf("with state ignore\n");
  492. break;
  493. case 1:
  494. printf("with state askfailed\n");
  495. break;
  496. case 2:
  497. printf("with state ready\n");
  498. break;
  499. case 3:
  500. printf("with state readyfail\n");
  501. break;
  502. default:
  503. printf("with state UNKNOWN (%d)\n", substate);
  504. break;
  505. }
  506. switch (buf[k + 1]) {
  507. case 'L':
  508. printf("got a leave indication\n");
  509. if (memcmp
  510. (recovered_streamid, monitor_stream_id,
  511. sizeof(recovered_streamid)) == 0) {
  512. listeners = 0;
  513. printf("listener left\n");
  514. }
  515. break;
  516. case 'J':
  517. case 'N':
  518. printf("got a new/join indication\n");
  519. if (substate > MSRP_LISTENER_ASKFAILED) {
  520. if (memcmp
  521. (recovered_streamid,
  522. monitor_stream_id,
  523. sizeof(recovered_streamid)) == 0)
  524. listeners = 1;
  525. }
  526. break;
  527. }
  528. /* only care about listeners ... */
  529. default:
  530. return (0);
  531. break;
  532. }
  533. break;
  534. case '\0':
  535. break;
  536. }
  537. return (0);
  538. }
  539. void *mrp_monitor_thread(void *arg)
  540. {
  541. char *msgbuf;
  542. struct sockaddr_in client_addr;
  543. struct msghdr msg;
  544. struct iovec iov;
  545. int bytes = 0;
  546. struct pollfd fds;
  547. int rc;
  548. if (NULL == arg)
  549. rc = 0;
  550. else
  551. rc = 1;
  552. msgbuf = (char *)malloc(MAX_MRPD_CMDSZ);
  553. if (NULL == msgbuf)
  554. return NULL;
  555. while (!halt_tx) {
  556. fds.fd = control_socket;
  557. fds.events = POLLIN;
  558. fds.revents = 0;
  559. rc = poll(&fds, 1, 100);
  560. if (rc < 0) {
  561. free(msgbuf);
  562. pthread_exit(NULL);
  563. }
  564. if (rc == 0)
  565. continue;
  566. if ((fds.revents & POLLIN) == 0) {
  567. free(msgbuf);
  568. pthread_exit(NULL);
  569. }
  570. memset(&msg, 0, sizeof(msg));
  571. memset(&client_addr, 0, sizeof(client_addr));
  572. memset(msgbuf, 0, MAX_MRPD_CMDSZ);
  573. iov.iov_len = MAX_MRPD_CMDSZ;
  574. iov.iov_base = msgbuf;
  575. msg.msg_name = &client_addr;
  576. msg.msg_namelen = sizeof(client_addr);
  577. msg.msg_iov = &iov;
  578. msg.msg_iovlen = 1;
  579. bytes = recvmsg(control_socket, &msg, 0);
  580. if (bytes < 0)
  581. continue;
  582. process_mrp_msg(msgbuf, bytes);
  583. }
  584. free(msgbuf);
  585. pthread_exit(NULL);
  586. }
  587. pthread_t monitor_thread;
  588. pthread_attr_t monitor_attr;
  589. int mrp_monitor()
  590. {
  591. pthread_attr_init(&monitor_attr);
  592. pthread_create(&monitor_thread, NULL, mrp_monitor_thread, NULL);
  593. return (0);
  594. }
  595. int mrp_join_listener(uint8_t * streamid)
  596. {
  597. char *msgbuf;
  598. int rc;
  599. msgbuf = malloc(1500);
  600. if (NULL == msgbuf)
  601. return -1;
  602. memset(msgbuf, 0, 1500);
  603. sprintf(msgbuf, "S+L:S=%02X%02X%02X%02X%02X%02X%02X%02X"
  604. ",D=2", streamid[0], streamid[1], streamid[2], streamid[3],
  605. streamid[4], streamid[5], streamid[6], streamid[7]);
  606. mrp_okay = 0;
  607. rc = send_mrp_msg(msgbuf, 1500);
  608. /* rc = recv_mrp_okay(); */
  609. free(msgbuf);
  610. return rc;
  611. }
  612. int
  613. mrp_advertise_stream(uint8_t * streamid,
  614. uint8_t * destaddr,
  615. u_int16_t vlan,
  616. int pktsz, int interval, int priority, int latency)
  617. {
  618. char *msgbuf;
  619. int rc;
  620. msgbuf = malloc(1500);
  621. if (NULL == msgbuf)
  622. return -1;
  623. memset(msgbuf, 0, 1500);
  624. sprintf(msgbuf, "S++:S=%02X%02X%02X%02X%02X%02X%02X%02X"
  625. ",A=%02X%02X%02X%02X%02X%02X"
  626. ",V=%04X"
  627. ",Z=%d"
  628. ",I=%d"
  629. ",P=%d"
  630. ",L=%d", streamid[0], streamid[1], streamid[2],
  631. streamid[3], streamid[4], streamid[5], streamid[6],
  632. streamid[7], destaddr[0], destaddr[1], destaddr[2],
  633. destaddr[3], destaddr[4], destaddr[5], vlan, pktsz,
  634. interval, priority << 5, latency);
  635. mrp_okay = 0;
  636. rc = send_mrp_msg(msgbuf, 1500);
  637. /* rc = recv_mrp_okay(); */
  638. free(msgbuf);
  639. return rc;
  640. }
  641. int
  642. mrp_unadvertise_stream(uint8_t * streamid,
  643. uint8_t * destaddr,
  644. u_int16_t vlan,
  645. int pktsz, int interval, int priority, int latency)
  646. {
  647. char *msgbuf;
  648. int rc;
  649. msgbuf = malloc(1500);
  650. if (NULL == msgbuf)
  651. return -1;
  652. memset(msgbuf, 0, 1500);
  653. sprintf(msgbuf, "S--:S=%02X%02X%02X%02X%02X%02X%02X%02X"
  654. ",A=%02X%02X%02X%02X%02X%02X"
  655. ",V=%04X"
  656. ",Z=%d"
  657. ",I=%d"
  658. ",P=%d"
  659. ",L=%d", streamid[0], streamid[1], streamid[2],
  660. streamid[3], streamid[4], streamid[5], streamid[6],
  661. streamid[7], destaddr[0], destaddr[1], destaddr[2],
  662. destaddr[3], destaddr[4], destaddr[5], vlan, pktsz,
  663. interval, priority << 5, latency);
  664. mrp_okay = 0;
  665. rc = send_mrp_msg(msgbuf, 1500);
  666. /* rc = recv_mrp_okay(); */
  667. free(msgbuf);
  668. return rc;
  669. }
  670. void sigint_handler(int signum)
  671. {
  672. printf("got SIGINT\n");
  673. halt_tx = signum;
  674. unleash_jack = 0;
  675. }
  676. int pci_connect()
  677. {
  678. struct pci_access *pacc;
  679. struct pci_dev *dev;
  680. int err;
  681. char devpath[IGB_BIND_NAMESZ];
  682. memset(&igb_dev, 0, sizeof(device_t));
  683. pacc = pci_alloc();
  684. pci_init(pacc);
  685. pci_scan_bus(pacc);
  686. for (dev = pacc->devices; dev; dev = dev->next) {
  687. pci_fill_info(dev,
  688. PCI_FILL_IDENT | PCI_FILL_BASES | PCI_FILL_CLASS);
  689. igb_dev.pci_vendor_id = dev->vendor_id;
  690. igb_dev.pci_device_id = dev->device_id;
  691. igb_dev.domain = dev->domain;
  692. igb_dev.bus = dev->bus;
  693. igb_dev.dev = dev->dev;
  694. igb_dev.func = dev->func;
  695. snprintf(devpath, IGB_BIND_NAMESZ, "%04x:%02x:%02x.%d",
  696. dev->domain, dev->bus, dev->dev, dev->func);
  697. err = igb_probe(&igb_dev);
  698. if (err) {
  699. continue;
  700. }
  701. printf("attaching to %s\n", devpath);
  702. err = igb_attach(devpath, &igb_dev);
  703. if (err) {
  704. printf("attach failed! (%s)\n", strerror(errno));
  705. continue;
  706. }
  707. goto out;
  708. }
  709. pci_cleanup(pacc);
  710. return ENXIO;
  711. out: pci_cleanup(pacc);
  712. return 0;
  713. }
  714. unsigned char STATION_ADDR[] = { 0, 0, 0, 0, 0, 0 };
  715. unsigned char STREAM_ID[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
  716. /* IEEE 1722 reserved address */
  717. unsigned char DEST_ADDR[] = { 0x91, 0xE0, 0xF0, 0x00, 0x0e, 0x80 };
  718. int get_mac_address(char *interface)
  719. {
  720. struct ifreq if_request;
  721. int lsock;
  722. int rc;
  723. lsock = socket(PF_PACKET, SOCK_RAW, htons(0x800));
  724. if (lsock < 0)
  725. return -1;
  726. memset(&if_request, 0, sizeof(if_request));
  727. strncpy(if_request.ifr_name, interface, sizeof(if_request.ifr_name));
  728. rc = ioctl(lsock, SIOCGIFHWADDR, &if_request);
  729. if (rc < 0) {
  730. close(lsock);
  731. return -1;
  732. }
  733. memcpy(STATION_ADDR, if_request.ifr_hwaddr.sa_data,
  734. sizeof(STATION_ADDR));
  735. close(lsock);
  736. return 0;
  737. }
  738. static void usage(void)
  739. {
  740. fprintf(stderr, "\n"
  741. "usage: simple_talker [-h] -i interface-name"
  742. "\n"
  743. "options:\n"
  744. " -h show this message\n"
  745. " -i specify interface for AVB connection\n"
  746. "\n" "%s" "\n", version_str);
  747. exit(1);
  748. }
  749. #define PACKET_IPG (125000) /* (1) packet every 125 usec */
  750. static void* packetizer_thread(void *arg) {
  751. struct igb_packet *cleaned_packets;
  752. six1883_sample *sample;
  753. unsigned total_samples = 0;
  754. int err;
  755. int i;
  756. const size_t bytes_to_read = CHANNELS * SAMPLES_PER_FRAME *
  757. SAMPLE_SIZE;
  758. jack_default_audio_sample_t* framebuf = malloc (bytes_to_read);
  759. extern jack_ringbuffer_t* ringbuffer;
  760. extern pthread_mutex_t threadLock;
  761. extern pthread_cond_t dataReady;
  762. pthread_setcanceltype (PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
  763. pthread_mutex_lock(&threadLock);
  764. while (listeners && !halt_tx) {
  765. pthread_cond_wait(&dataReady, &threadLock);
  766. while ((jack_ringbuffer_read_space(ringbuffer) >= bytes_to_read)) {
  767. tmp_packet = free_packets;
  768. if (NULL == tmp_packet)
  769. goto cleanup;
  770. header0 =
  771. (seventeen22_header *) (((char *)tmp_packet->vaddr) + 18);
  772. header1 = (six1883_header *) (header0 + 1);
  773. free_packets = tmp_packet->next;
  774. /* unfortuntely unless this thread is at rtprio
  775. * you get pre-empted between fetching the time
  776. * and programming the packet and get a late packet
  777. */
  778. tmp_packet->attime = last_time + PACKET_IPG;
  779. last_time += PACKET_IPG;
  780. jack_ringbuffer_read (ringbuffer, (char*)&framebuf[0], bytes_to_read);
  781. header0->seq_number = seqnum++;
  782. if (seqnum % 4 == 0)
  783. header0->timestamp_valid = 0;
  784. else
  785. header0->timestamp_valid = 1;
  786. time_stamp = htonl(time_stamp);
  787. header0->timestamp = time_stamp;
  788. time_stamp = ntohl(time_stamp);
  789. time_stamp += PACKET_IPG;
  790. header1->data_block_continuity = total_samples;
  791. total_samples += SAMPLES_PER_FRAME*CHANNELS;
  792. sample =
  793. (six1883_sample *) (((char *)tmp_packet->vaddr) +
  794. (18 + sizeof(seventeen22_header) +
  795. sizeof(six1883_header)));
  796. for (i = 0; i < SAMPLES_PER_FRAME * CHANNELS; ++i) {
  797. uint32_t tmp = htonl(MAX_SAMPLE_VALUE * framebuf[i]);
  798. sample[i].label = 0x40;
  799. memcpy(&(sample[i].value), &(tmp),
  800. sizeof(sample[i].value));
  801. }
  802. err = igb_xmit(&igb_dev, 0, tmp_packet);
  803. if (!err) {
  804. continue;
  805. }
  806. if (ENOSPC == err) {
  807. /* put back for now */
  808. tmp_packet->next = free_packets;
  809. free_packets = tmp_packet;
  810. }
  811. cleanup: igb_clean(&igb_dev, &cleaned_packets);
  812. i = 0;
  813. while (cleaned_packets) {
  814. i++;
  815. tmp_packet = cleaned_packets;
  816. cleaned_packets = cleaned_packets->next;
  817. tmp_packet->next = free_packets;
  818. free_packets = tmp_packet;
  819. }
  820. }
  821. }
  822. return NULL;
  823. }
  824. static void run_packetizer(void)
  825. {
  826. jack_acquire_real_time_scheduling(packetizer_id, 70);
  827. unleash_jack = 1;
  828. pthread_join (packetizer_id, NULL);
  829. }
  830. int main(int argc, char *argv[])
  831. {
  832. unsigned i;
  833. int err;
  834. struct igb_dma_alloc a_page;
  835. struct igb_packet a_packet;
  836. int c;
  837. int rc = 0;
  838. char *interface = NULL;
  839. int class_a_id = 0;
  840. int a_priority = 0;
  841. u_int16_t a_vid = 0;
  842. #ifdef DOMAIN_QUERY
  843. int class_b_id = 0;
  844. int b_priority = 0;
  845. u_int16_t b_vid = 0;
  846. #endif
  847. gPtpTimeData td;
  848. uint64_t now_local, now_8021as;
  849. uint64_t update_8021as;
  850. unsigned delta_8021as, delta_local;
  851. long double ml_ratio;
  852. jack_client_t* _jackclient;
  853. for (;;) {
  854. c = getopt(argc, argv, "hi:");
  855. if (c < 0)
  856. break;
  857. switch (c) {
  858. case 'h':
  859. usage();
  860. break;
  861. case 'i':
  862. if (interface) {
  863. printf
  864. ("only one interface per daemon is supported\n");
  865. usage();
  866. }
  867. interface = strdup(optarg);
  868. break;
  869. }
  870. }
  871. if (optind < argc)
  872. usage();
  873. if (NULL == interface) {
  874. usage();
  875. }
  876. rc = mrp_connect();
  877. if (rc) {
  878. printf("socket creation failed\n");
  879. return (errno);
  880. }
  881. err = pci_connect();
  882. if (err) {
  883. printf("connect failed (%s) - are you running as root?\n",
  884. strerror(errno));
  885. return (errno);
  886. }
  887. err = igb_init(&igb_dev);
  888. if (err) {
  889. printf("init failed (%s) - is the driver really loaded?\n",
  890. strerror(errno));
  891. return (errno);
  892. }
  893. err = igb_dma_malloc_page(&igb_dev, &a_page);
  894. if (err) {
  895. printf("malloc failed (%s) - out of memory?\n",
  896. strerror(errno));
  897. return (errno);
  898. }
  899. signal(SIGINT, sigint_handler);
  900. rc = get_mac_address(interface);
  901. if (rc) {
  902. printf("failed to open interface\n");
  903. usage();
  904. }
  905. mrp_monitor();
  906. #ifdef DOMAIN_QUERY
  907. /*
  908. * should use mrp_get_domain() above but this is a simplification
  909. */
  910. #endif
  911. domain_a_valid = 1;
  912. class_a_id = MSRP_SR_CLASS_A;
  913. a_priority = MSRP_SR_CLASS_A_PRIO;
  914. a_vid = 2;
  915. printf("detected domain Class A PRIO=%d VID=%04x...\n", a_priority,
  916. a_vid);
  917. #define PKT_SZ 100
  918. mrp_register_domain(&class_a_id, &a_priority, &a_vid);
  919. igb_set_class_bandwidth(&igb_dev, PACKET_IPG / 125000, 0, PKT_SZ - 22,
  920. 0);
  921. memset(STREAM_ID, 0, sizeof(STREAM_ID));
  922. memcpy(STREAM_ID, STATION_ADDR, sizeof(STATION_ADDR));
  923. a_packet.dmatime = a_packet.attime = a_packet.flags = 0;
  924. a_packet.map.paddr = a_page.dma_paddr;
  925. a_packet.map.mmap_size = a_page.mmap_size;
  926. a_packet.offset = 0;
  927. a_packet.vaddr = a_page.dma_vaddr + a_packet.offset;
  928. a_packet.len = PKT_SZ;
  929. free_packets = NULL;
  930. seqnum = 0;
  931. /* divide the dma page into buffers for packets */
  932. for (i = 1; i < ((a_page.mmap_size) / PKT_SZ); i++) {
  933. tmp_packet = malloc(sizeof(struct igb_packet));
  934. if (NULL == tmp_packet) {
  935. printf("failed to allocate igb_packet memory!\n");
  936. return (errno);
  937. }
  938. *tmp_packet = a_packet;
  939. tmp_packet->offset = (i * PKT_SZ);
  940. tmp_packet->vaddr += tmp_packet->offset;
  941. tmp_packet->next = free_packets;
  942. memset(tmp_packet->vaddr, 0, PKT_SZ); /* MAC header at least */
  943. memcpy(tmp_packet->vaddr, DEST_ADDR, sizeof(DEST_ADDR));
  944. memcpy(tmp_packet->vaddr + 6, STATION_ADDR,
  945. sizeof(STATION_ADDR));
  946. /* Q-tag */
  947. ((char *)tmp_packet->vaddr)[12] = 0x81;
  948. ((char *)tmp_packet->vaddr)[13] = 0x00;
  949. ((char *)tmp_packet->vaddr)[14] =
  950. ((a_priority << 13 | a_vid)) >> 8;
  951. ((char *)tmp_packet->vaddr)[15] =
  952. ((a_priority << 13 | a_vid)) & 0xFF;
  953. ((char *)tmp_packet->vaddr)[16] = 0x22; /* 1722 eth type */
  954. ((char *)tmp_packet->vaddr)[17] = 0xF0;
  955. /* 1722 header update + payload */
  956. header0 =
  957. (seventeen22_header *) (((char *)tmp_packet->vaddr) + 18);
  958. header0->cd_indicator = 0;
  959. header0->subtype = 0;
  960. header0->sid_valid = 1;
  961. header0->version = 0;
  962. header0->reset = 0;
  963. header0->reserved0 = 0;
  964. header0->gateway_valid = 0;
  965. header0->reserved1 = 0;
  966. header0->timestamp_uncertain = 0;
  967. memset(&(header0->stream_id), 0, sizeof(header0->stream_id));
  968. memcpy(&(header0->stream_id), STATION_ADDR,
  969. sizeof(STATION_ADDR));
  970. header0->length = htons(32);
  971. header1 = (six1883_header *) (header0 + 1);
  972. header1->format_tag = 1;
  973. header1->packet_channel = 0x1F;
  974. header1->packet_tcode = 0xA;
  975. header1->app_control = 0x0;
  976. header1->reserved0 = 0;
  977. header1->source_id = 0x3F;
  978. header1->data_block_size = 1;
  979. header1->fraction_number = 0;
  980. header1->quadlet_padding_count = 0;
  981. header1->source_packet_header = 0;
  982. header1->reserved1 = 0;
  983. header1->eoh = 0x2;
  984. header1->format_id = 0x10;
  985. header1->format_dependent_field = 0x02;
  986. header1->syt = 0xFFFF;
  987. tmp_packet->len =
  988. 18 + sizeof(seventeen22_header) + sizeof(six1883_header) +
  989. (SAMPLES_PER_FRAME * CHANNELS * sizeof(six1883_sample));
  990. free_packets = tmp_packet;
  991. }
  992. /*
  993. * subtract 16 bytes for the MAC header/Q-tag - pktsz is limited to the
  994. * data payload of the ethernet frame .
  995. *
  996. * IPG is scaled to the Class (A) observation interval of packets per 125 usec
  997. */
  998. _jackclient = init_jack();
  999. fprintf(stderr, "advertising stream ...\n");
  1000. mrp_advertise_stream(STREAM_ID, DEST_ADDR, a_vid, PKT_SZ - 16,
  1001. PACKET_IPG / 125000, a_priority, 3900);
  1002. fprintf(stderr, "awaiting a listener ...\n");
  1003. mrp_await_listener(STREAM_ID);
  1004. printf("got a listener ...\n");
  1005. halt_tx = 0;
  1006. gptpinit();
  1007. gptpscaling(&td);
  1008. if( igb_get_wallclock( &igb_dev, &now_local, NULL ) != 0 ) {
  1009. fprintf( stderr, "Failed to get wallclock time\n" );
  1010. return -1;
  1011. }
  1012. update_8021as = td.local_time - td.ml_phoffset;
  1013. delta_local = (unsigned)(now_local - td.local_time);
  1014. delta_8021as = (unsigned)(td.ml_freqoffset * delta_local);
  1015. now_8021as = update_8021as + delta_8021as;
  1016. last_time = now_local + XMIT_DELAY;
  1017. time_stamp = now_8021as + RENDER_DELAY;
  1018. rc = nice(-20);
  1019. pthread_create (&packetizer_id, NULL, packetizer_thread, NULL);
  1020. run_packetizer();
  1021. rc = nice(0);
  1022. stop_jack(_jackclient);
  1023. if (halt_tx == 0)
  1024. printf("listener left ...\n");
  1025. halt_tx = 1;
  1026. mrp_unadvertise_stream(STREAM_ID, DEST_ADDR, a_vid, PKT_SZ - 16,
  1027. PACKET_IPG / 125000, a_priority, 3900);
  1028. igb_set_class_bandwidth(&igb_dev, 0, 0, 0, 0); /* disable Qav */
  1029. rc = mrp_disconnect();
  1030. igb_dma_free_page(&igb_dev, &a_page);
  1031. err = igb_detach(&igb_dev);
  1032. pthread_exit(NULL);
  1033. return (0);
  1034. }