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;
}
