int hal_init_timing(char *filename) { timeout_t lock_tmo; static struct { char *cfgname; int modevalue; } *m, modes[] = { {"TIME_GM", HAL_TIMING_MODE_GRAND_MASTER}, {"TIME_FM", HAL_TIMING_MODE_FREE_MASTER}, {"TIME_BC", HAL_TIMING_MODE_BC}, {NULL, HAL_TIMING_MODE_BC /* default */}, }; if (rts_connect(NULL) < 0) { pr_error( "Failed to establish communication with the RT subsystem.\n"); return -1; } for (m = modes; m->cfgname; m++) if (libwr_cfg_get(m->cfgname)) break; timing_mode = m->modevalue; if (!m->cfgname) pr_error("%s: no config variable set, defaults used\n", __func__); /* initialize the RT Subsys */ switch (timing_mode) { case HAL_TIMING_MODE_GRAND_MASTER: rts_set_mode(RTS_MODE_GM_EXTERNAL); tmo_init(&lock_tmo, LOCK_TIMEOUT_EXT, 0); break; case HAL_TIMING_MODE_FREE_MASTER: case HAL_TIMING_MODE_BC: default: /* never hit, but having it here prevents a warning */ rts_set_mode(RTS_MODE_GM_FREERUNNING); tmo_init(&lock_tmo, LOCK_TIMEOUT_INT, 0); break; } while (1) { struct rts_pll_state pstate; if (tmo_expired(&lock_tmo)) { pr_error("Can't lock the PLL. " "If running in the GrandMaster mode, " "are you sure the 1-PPS and 10 MHz " "reference clock signals are properly connected?," " retrying...\n"); if (timing_mode == HAL_TIMING_MODE_GRAND_MASTER) { /*ups... something went wrong, try again */ rts_set_mode(RTS_MODE_GM_EXTERNAL); tmo_init(&lock_tmo, LOCK_TIMEOUT_EXT, 0); } else { pr_error("Got timeout\n"); return -1; } } if (rts_get_state(&pstate) < 0) { /* Don't give up when rts_get_state fails, it may be * due to race with ppsi at boot. No problems seen * because of waiting here. */ pr_error("rts_get_state failed try again\n"); continue; } if (pstate.flags & RTS_DMTD_LOCKED) { if (timing_mode == HAL_TIMING_MODE_GRAND_MASTER) pr_info("GrandMaster locked to external " "reference\n"); break; } usleep(100000); } /* * We had "timing.use_nmea", but it was hardwired to /dev/ttyS2 * which is not wired out any more, so this is removed after v4.1 */ return 0; }
wr_socket_t *ptpd_netif_create_socket(int sock_type, int flags, wr_sockaddr_t *bind_addr) { struct my_socket *s; struct sockaddr_ll sll; struct ifreq f; hexp_port_state_t pstate; int fd; // fprintf(stderr,"CreateSocket!\n"); if(sock_type != PTPD_SOCK_RAW_ETHERNET) return NULL; if(halexp_get_port_state(&pstate, bind_addr->if_name) < 0) return NULL; fd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)); if(fd < 0) { perror("socket()"); return NULL; } fcntl(fd, F_SETFL, O_NONBLOCK); // Put the controller in promiscious mode, so it receives everything strcpy(f.ifr_name, bind_addr->if_name); if(ioctl(fd, SIOCGIFFLAGS,&f) < 0) { perror("ioctl()"); return NULL; } f.ifr_flags |= IFF_PROMISC; if(ioctl(fd, SIOCSIFFLAGS,&f) < 0) { perror("ioctl()"); return NULL; } // Find the inteface index strcpy(f.ifr_name, bind_addr->if_name); ioctl(fd, SIOCGIFINDEX, &f); sll.sll_ifindex = f.ifr_ifindex; sll.sll_family = AF_PACKET; sll.sll_protocol = htons(bind_addr->ethertype); sll.sll_halen = 6; memcpy(sll.sll_addr, bind_addr->mac, 6); if(bind(fd, (struct sockaddr *)&sll, sizeof(struct sockaddr_ll)) < 0) { close(fd); perror("bind()"); return NULL; } // timestamping stuff: int so_timestamping_flags = SOF_TIMESTAMPING_TX_HARDWARE | SOF_TIMESTAMPING_RX_HARDWARE | SOF_TIMESTAMPING_RAW_HARDWARE; struct ifreq ifr; struct hwtstamp_config hwconfig; strncpy(ifr.ifr_name, bind_addr->if_name, sizeof(ifr.ifr_name)); hwconfig.tx_type = HWTSTAMP_TX_ON; hwconfig.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; ifr.ifr_data = &hwconfig; if (ioctl(fd, SIOCSHWTSTAMP, &ifr) < 0) { perror("SIOCSHWTSTAMP"); return NULL; } if(setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping_flags, sizeof(int)) < 0) { perror("setsockopt(SO_TIMESTAMPING)"); return NULL; } s=calloc(sizeof(struct my_socket), 1); s->if_index = f.ifr_ifindex; // get interface MAC address if (ioctl(fd, SIOCGIFHWADDR, &f) < 0) { perror("ioctl()"); return NULL; } memcpy(s->local_mac, f.ifr_hwaddr.sa_data, 6); memcpy(&s->bind_addr, bind_addr, sizeof(wr_sockaddr_t)); s->fd = fd; // store the linearization parameters s->clock_period = pstate.clock_period; s->phase_transition = pstate.t2_phase_transition; s->dmtd_phase = pstate.phase_val; tmo_init(&s->dmtd_update_tmo, DMTD_UPDATE_INTERVAL); return (wr_socket_t*)s; }