phc2sys: extract PMC functionality into a smaller struct pmc_node
This creates a smaller structure within phc2sys_private, which embeds all properties related to the PMC. This structure is called "pmc_node", which is somewhat reminiscent of the old name of phc2sys_private (struct node). But the advantage is that struct pmc_node can be reused by other modules. The phc2sys code that is executed upon a subscription update, recv_subscribed, is now refactored into a function pointer callback. It is imaginable that other programs might to do other things in it. Note that putting this function pointer in struct pmc_node is, long term, maybe not the best of choices. It is only needed from the run_pmc_events() code path, and could be therefore passed as a more local callback to that function only. However, for that, further refactoring is needed inside the common run_pmc() function, so that is being left for another time. Signed-off-by: Vladimir Oltean <olteanv@gmail.com>master
parent
2f8470980d
commit
317e419d1f
228
phc2sys.c
228
phc2sys.c
|
@ -39,6 +39,7 @@
|
||||||
|
|
||||||
#include "clockadj.h"
|
#include "clockadj.h"
|
||||||
#include "clockcheck.h"
|
#include "clockcheck.h"
|
||||||
|
#include "contain.h"
|
||||||
#include "ds.h"
|
#include "ds.h"
|
||||||
#include "fsm.h"
|
#include "fsm.h"
|
||||||
#include "missing.h"
|
#include "missing.h"
|
||||||
|
@ -99,23 +100,34 @@ struct port {
|
||||||
struct clock *clock;
|
struct clock *clock;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
struct pmc_node;
|
||||||
|
|
||||||
|
typedef int pmc_node_recv_subscribed_t(struct pmc_node *node,
|
||||||
|
struct ptp_message *msg,
|
||||||
|
int excluded);
|
||||||
|
|
||||||
|
struct pmc_node {
|
||||||
|
struct pmc *pmc;
|
||||||
|
int pmc_ds_requested;
|
||||||
|
uint64_t pmc_last_update;
|
||||||
|
int sync_offset;
|
||||||
|
int leap;
|
||||||
|
int utc_offset_traceable;
|
||||||
|
int clock_identity_set;
|
||||||
|
struct ClockIdentity clock_identity;
|
||||||
|
pmc_node_recv_subscribed_t *recv_subscribed;
|
||||||
|
};
|
||||||
|
|
||||||
struct phc2sys_private {
|
struct phc2sys_private {
|
||||||
unsigned int stats_max_count;
|
unsigned int stats_max_count;
|
||||||
int sanity_freq_limit;
|
int sanity_freq_limit;
|
||||||
enum servo_type servo_type;
|
enum servo_type servo_type;
|
||||||
int phc_readings;
|
int phc_readings;
|
||||||
double phc_interval;
|
double phc_interval;
|
||||||
int sync_offset;
|
|
||||||
int forced_sync_offset;
|
int forced_sync_offset;
|
||||||
int utc_offset_traceable;
|
|
||||||
int leap;
|
|
||||||
int kernel_leap;
|
int kernel_leap;
|
||||||
struct pmc *pmc;
|
|
||||||
int pmc_ds_requested;
|
|
||||||
uint64_t pmc_last_update;
|
|
||||||
int state_changed;
|
int state_changed;
|
||||||
int clock_identity_set;
|
struct pmc_node node;
|
||||||
struct ClockIdentity clock_identity;
|
|
||||||
LIST_HEAD(port_head, port) ports;
|
LIST_HEAD(port_head, port) ports;
|
||||||
LIST_HEAD(clock_head, clock) clocks;
|
LIST_HEAD(clock_head, clock) clocks;
|
||||||
LIST_HEAD(dst_clock_head, clock) dst_clocks;
|
LIST_HEAD(dst_clock_head, clock) dst_clocks;
|
||||||
|
@ -124,16 +136,16 @@ struct phc2sys_private {
|
||||||
|
|
||||||
static struct config *phc2sys_config;
|
static struct config *phc2sys_config;
|
||||||
|
|
||||||
static int update_pmc(struct phc2sys_private *priv, int subscribe);
|
static int update_pmc_node(struct pmc_node *node, int subscribe);
|
||||||
static int clock_handle_leap(struct phc2sys_private *priv,
|
static int clock_handle_leap(struct phc2sys_private *priv,
|
||||||
struct clock *clock,
|
struct clock *clock,
|
||||||
int64_t offset, uint64_t ts);
|
int64_t offset, uint64_t ts);
|
||||||
static int run_pmc_get_utc_offset(struct phc2sys_private *priv,
|
static int run_pmc_get_utc_offset(struct pmc_node *node,
|
||||||
int timeout);
|
int timeout);
|
||||||
static void run_pmc_events(struct phc2sys_private *priv);
|
static void run_pmc_events(struct pmc_node *node);
|
||||||
|
|
||||||
static int normalize_state(int state);
|
static int normalize_state(int state);
|
||||||
static int run_pmc_port_properties(struct phc2sys_private *priv,
|
static int run_pmc_port_properties(struct pmc_node *node,
|
||||||
int timeout, unsigned int port,
|
int timeout, unsigned int port,
|
||||||
int *state, int *tstamping, char *iface);
|
int *state, int *tstamping, char *iface);
|
||||||
|
|
||||||
|
@ -324,7 +336,7 @@ static void clock_reinit(struct phc2sys_private *priv, struct clock *clock,
|
||||||
|
|
||||||
LIST_FOREACH(p, &priv->ports, list) {
|
LIST_FOREACH(p, &priv->ports, list) {
|
||||||
if (p->clock == clock) {
|
if (p->clock == clock) {
|
||||||
ret = run_pmc_port_properties(priv, 1000, p->number,
|
ret = run_pmc_port_properties(&priv->node, 1000, p->number,
|
||||||
&state, ×tamping,
|
&state, ×tamping,
|
||||||
iface);
|
iface);
|
||||||
if (ret > 0)
|
if (ret > 0)
|
||||||
|
@ -659,7 +671,7 @@ static int do_pps_loop(struct phc2sys_private *priv, struct clock *clock,
|
||||||
|
|
||||||
if (src == CLOCK_INVALID) {
|
if (src == CLOCK_INVALID) {
|
||||||
/* The sync offset can't be applied with PPS alone. */
|
/* The sync offset can't be applied with PPS alone. */
|
||||||
priv->sync_offset = 0;
|
priv->node.sync_offset = 0;
|
||||||
} else {
|
} else {
|
||||||
enable_pps_output(priv->master->clkid);
|
enable_pps_output(priv->master->clkid);
|
||||||
}
|
}
|
||||||
|
@ -690,7 +702,7 @@ static int do_pps_loop(struct phc2sys_private *priv, struct clock *clock,
|
||||||
pps_offset = pps_ts - phc_ts;
|
pps_offset = pps_ts - phc_ts;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (update_pmc(priv, 0) < 0)
|
if (update_pmc_node(&priv->node, 0) < 0)
|
||||||
continue;
|
continue;
|
||||||
update_clock(priv, clock, pps_offset, pps_ts, -1);
|
update_clock(priv, clock, pps_offset, pps_ts, -1);
|
||||||
}
|
}
|
||||||
|
@ -727,15 +739,15 @@ static int do_loop(struct phc2sys_private *priv, int subscriptions)
|
||||||
|
|
||||||
while (is_running()) {
|
while (is_running()) {
|
||||||
clock_nanosleep(CLOCK_MONOTONIC, 0, &interval, NULL);
|
clock_nanosleep(CLOCK_MONOTONIC, 0, &interval, NULL);
|
||||||
if (update_pmc(priv, subscriptions) < 0)
|
if (update_pmc_node(&priv->node, subscriptions) < 0)
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
if (subscriptions) {
|
if (subscriptions) {
|
||||||
run_pmc_events(priv);
|
run_pmc_events(&priv->node);
|
||||||
if (priv->state_changed) {
|
if (priv->state_changed) {
|
||||||
/* force getting offset, as it may have
|
/* force getting offset, as it may have
|
||||||
* changed after the port state change */
|
* changed after the port state change */
|
||||||
if (run_pmc_get_utc_offset(priv, 1000) <= 0) {
|
if (run_pmc_get_utc_offset(&priv->node, 1000) <= 0) {
|
||||||
pr_err("failed to get UTC offset");
|
pr_err("failed to get UTC offset");
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
@ -792,13 +804,12 @@ static int do_loop(struct phc2sys_private *priv, int subscriptions)
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int check_clock_identity(struct phc2sys_private *priv,
|
static int check_clock_identity(struct pmc_node *node, struct ptp_message *msg)
|
||||||
struct ptp_message *msg)
|
|
||||||
{
|
{
|
||||||
if (!priv->clock_identity_set)
|
if (!node->clock_identity_set)
|
||||||
return 1;
|
return 1;
|
||||||
return cid_eq(&priv->clock_identity,
|
return cid_eq(&node->clock_identity,
|
||||||
&msg->header.sourcePortIdentity.clockIdentity);
|
&msg->header.sourcePortIdentity.clockIdentity);
|
||||||
}
|
}
|
||||||
|
|
||||||
static int is_msg_mgt(struct ptp_message *msg)
|
static int is_msg_mgt(struct ptp_message *msg)
|
||||||
|
@ -872,9 +883,13 @@ static int clock_compute_state(struct phc2sys_private *priv,
|
||||||
return state;
|
return state;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int recv_subscribed(struct phc2sys_private *priv,
|
#define node_to_phc2sys(node) \
|
||||||
struct ptp_message *msg, int excluded)
|
container_of(node, struct phc2sys_private, node)
|
||||||
|
|
||||||
|
static int phc2sys_recv_subscribed(struct pmc_node *node,
|
||||||
|
struct ptp_message *msg, int excluded)
|
||||||
{
|
{
|
||||||
|
struct phc2sys_private *priv = node_to_phc2sys(node);
|
||||||
int mgt_id, state;
|
int mgt_id, state;
|
||||||
struct portDS *pds;
|
struct portDS *pds;
|
||||||
struct port *port;
|
struct port *port;
|
||||||
|
@ -909,29 +924,28 @@ static int recv_subscribed(struct phc2sys_private *priv,
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void send_subscription(struct phc2sys_private *priv)
|
static void send_subscription(struct pmc_node *node)
|
||||||
{
|
{
|
||||||
struct subscribe_events_np sen;
|
struct subscribe_events_np sen;
|
||||||
|
|
||||||
memset(&sen, 0, sizeof(sen));
|
memset(&sen, 0, sizeof(sen));
|
||||||
sen.duration = PMC_SUBSCRIBE_DURATION;
|
sen.duration = PMC_SUBSCRIBE_DURATION;
|
||||||
sen.bitmask[0] = 1 << NOTIFY_PORT_STATE;
|
sen.bitmask[0] = 1 << NOTIFY_PORT_STATE;
|
||||||
pmc_send_set_action(priv->pmc, TLV_SUBSCRIBE_EVENTS_NP, &sen, sizeof(sen));
|
pmc_send_set_action(node->pmc, TLV_SUBSCRIBE_EVENTS_NP, &sen, sizeof(sen));
|
||||||
}
|
}
|
||||||
|
|
||||||
static int init_pmc(struct config *cfg, struct phc2sys_private *priv)
|
static int init_pmc_node(struct config *cfg, struct pmc_node *node,
|
||||||
|
const char *uds,
|
||||||
|
pmc_node_recv_subscribed_t *recv_subscribed)
|
||||||
{
|
{
|
||||||
char uds_local[MAX_IFNAME_SIZE + 1];
|
node->pmc = pmc_create(cfg, TRANS_UDS, uds, 0,
|
||||||
|
|
||||||
snprintf(uds_local, sizeof(uds_local), "/var/run/phc2sys.%d",
|
|
||||||
getpid());
|
|
||||||
priv->pmc = pmc_create(cfg, TRANS_UDS, uds_local, 0,
|
|
||||||
config_get_int(cfg, NULL, "domainNumber"),
|
config_get_int(cfg, NULL, "domainNumber"),
|
||||||
config_get_int(cfg, NULL, "transportSpecific") << 4, 1);
|
config_get_int(cfg, NULL, "transportSpecific") << 4, 1);
|
||||||
if (!priv->pmc) {
|
if (!node->pmc) {
|
||||||
pr_err("failed to create pmc");
|
pr_err("failed to create pmc");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
node->recv_subscribed = recv_subscribed;
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -942,7 +956,7 @@ static int init_pmc(struct config *cfg, struct phc2sys_private *priv)
|
||||||
* -1: error reported by the other side
|
* -1: error reported by the other side
|
||||||
* -2: local error, fatal
|
* -2: local error, fatal
|
||||||
*/
|
*/
|
||||||
static int run_pmc(struct phc2sys_private *priv, int timeout, int ds_id,
|
static int run_pmc(struct pmc_node *node, int timeout, int ds_id,
|
||||||
struct ptp_message **msg)
|
struct ptp_message **msg)
|
||||||
{
|
{
|
||||||
#define N_FD 1
|
#define N_FD 1
|
||||||
|
@ -950,9 +964,9 @@ static int run_pmc(struct phc2sys_private *priv, int timeout, int ds_id,
|
||||||
int cnt, res;
|
int cnt, res;
|
||||||
|
|
||||||
while (1) {
|
while (1) {
|
||||||
pollfd[0].fd = pmc_get_transport_fd(priv->pmc);
|
pollfd[0].fd = pmc_get_transport_fd(node->pmc);
|
||||||
pollfd[0].events = POLLIN|POLLPRI;
|
pollfd[0].events = POLLIN|POLLPRI;
|
||||||
if (!priv->pmc_ds_requested && ds_id >= 0)
|
if (!node->pmc_ds_requested && ds_id >= 0)
|
||||||
pollfd[0].events |= POLLOUT;
|
pollfd[0].events |= POLLOUT;
|
||||||
|
|
||||||
cnt = poll(pollfd, N_FD, timeout);
|
cnt = poll(pollfd, N_FD, timeout);
|
||||||
|
@ -962,7 +976,7 @@ static int run_pmc(struct phc2sys_private *priv, int timeout, int ds_id,
|
||||||
}
|
}
|
||||||
if (!cnt) {
|
if (!cnt) {
|
||||||
/* Request the data set again in the next run. */
|
/* Request the data set again in the next run. */
|
||||||
priv->pmc_ds_requested = 0;
|
node->pmc_ds_requested = 0;
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -971,24 +985,24 @@ static int run_pmc(struct phc2sys_private *priv, int timeout, int ds_id,
|
||||||
!(pollfd[0].revents & (POLLIN|POLLPRI))) {
|
!(pollfd[0].revents & (POLLIN|POLLPRI))) {
|
||||||
switch (ds_id) {
|
switch (ds_id) {
|
||||||
case TLV_SUBSCRIBE_EVENTS_NP:
|
case TLV_SUBSCRIBE_EVENTS_NP:
|
||||||
send_subscription(priv);
|
send_subscription(node);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
pmc_send_get_action(priv->pmc, ds_id);
|
pmc_send_get_action(node->pmc, ds_id);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
priv->pmc_ds_requested = 1;
|
node->pmc_ds_requested = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!(pollfd[0].revents & (POLLIN|POLLPRI)))
|
if (!(pollfd[0].revents & (POLLIN|POLLPRI)))
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
*msg = pmc_recv(priv->pmc);
|
*msg = pmc_recv(node->pmc);
|
||||||
|
|
||||||
if (!*msg)
|
if (!*msg)
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
if (!check_clock_identity(priv, *msg)) {
|
if (!check_clock_identity(node, *msg)) {
|
||||||
msg_put(*msg);
|
msg_put(*msg);
|
||||||
*msg = NULL;
|
*msg = NULL;
|
||||||
continue;
|
continue;
|
||||||
|
@ -996,29 +1010,29 @@ static int run_pmc(struct phc2sys_private *priv, int timeout, int ds_id,
|
||||||
|
|
||||||
res = is_msg_mgt(*msg);
|
res = is_msg_mgt(*msg);
|
||||||
if (res < 0 && get_mgt_err_id(*msg) == ds_id) {
|
if (res < 0 && get_mgt_err_id(*msg) == ds_id) {
|
||||||
priv->pmc_ds_requested = 0;
|
node->pmc_ds_requested = 0;
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
if (res <= 0 || recv_subscribed(priv, *msg, ds_id) ||
|
if (res <= 0 || node->recv_subscribed(node, *msg, ds_id) ||
|
||||||
get_mgt_id(*msg) != ds_id) {
|
get_mgt_id(*msg) != ds_id) {
|
||||||
msg_put(*msg);
|
msg_put(*msg);
|
||||||
*msg = NULL;
|
*msg = NULL;
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
priv->pmc_ds_requested = 0;
|
node->pmc_ds_requested = 0;
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static int run_pmc_wait_sync(struct phc2sys_private *priv, int timeout)
|
static int run_pmc_wait_sync(struct pmc_node *node, int timeout)
|
||||||
{
|
{
|
||||||
struct ptp_message *msg;
|
struct ptp_message *msg;
|
||||||
int res;
|
|
||||||
void *data;
|
|
||||||
Enumeration8 portState;
|
Enumeration8 portState;
|
||||||
|
void *data;
|
||||||
|
int res;
|
||||||
|
|
||||||
while (1) {
|
while (1) {
|
||||||
res = run_pmc(priv, timeout, TLV_PORT_DATA_SET, &msg);
|
res = run_pmc(node, timeout, TLV_PORT_DATA_SET, &msg);
|
||||||
if (res <= 0)
|
if (res <= 0)
|
||||||
return res;
|
return res;
|
||||||
|
|
||||||
|
@ -1032,47 +1046,47 @@ static int run_pmc_wait_sync(struct phc2sys_private *priv, int timeout)
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
/* try to get more data sets (for other ports) */
|
/* try to get more data sets (for other ports) */
|
||||||
priv->pmc_ds_requested = 1;
|
node->pmc_ds_requested = 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static int run_pmc_get_utc_offset(struct phc2sys_private *priv, int timeout)
|
static int run_pmc_get_utc_offset(struct pmc_node *node, int timeout)
|
||||||
{
|
{
|
||||||
struct ptp_message *msg;
|
struct ptp_message *msg;
|
||||||
int res;
|
int res;
|
||||||
struct timePropertiesDS *tds;
|
struct timePropertiesDS *tds;
|
||||||
|
|
||||||
res = run_pmc(priv, timeout, TLV_TIME_PROPERTIES_DATA_SET, &msg);
|
res = run_pmc(node, timeout, TLV_TIME_PROPERTIES_DATA_SET, &msg);
|
||||||
if (res <= 0)
|
if (res <= 0)
|
||||||
return res;
|
return res;
|
||||||
|
|
||||||
tds = (struct timePropertiesDS *)get_mgt_data(msg);
|
tds = (struct timePropertiesDS *)get_mgt_data(msg);
|
||||||
if (tds->flags & PTP_TIMESCALE) {
|
if (tds->flags & PTP_TIMESCALE) {
|
||||||
priv->sync_offset = tds->currentUtcOffset;
|
node->sync_offset = tds->currentUtcOffset;
|
||||||
if (tds->flags & LEAP_61)
|
if (tds->flags & LEAP_61)
|
||||||
priv->leap = 1;
|
node->leap = 1;
|
||||||
else if (tds->flags & LEAP_59)
|
else if (tds->flags & LEAP_59)
|
||||||
priv->leap = -1;
|
node->leap = -1;
|
||||||
else
|
else
|
||||||
priv->leap = 0;
|
node->leap = 0;
|
||||||
priv->utc_offset_traceable = tds->flags & UTC_OFF_VALID &&
|
node->utc_offset_traceable = tds->flags & UTC_OFF_VALID &&
|
||||||
tds->flags & TIME_TRACEABLE;
|
tds->flags & TIME_TRACEABLE;
|
||||||
} else {
|
} else {
|
||||||
priv->sync_offset = 0;
|
node->sync_offset = 0;
|
||||||
priv->leap = 0;
|
node->leap = 0;
|
||||||
priv->utc_offset_traceable = 0;
|
node->utc_offset_traceable = 0;
|
||||||
}
|
}
|
||||||
msg_put(msg);
|
msg_put(msg);
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int run_pmc_get_number_ports(struct phc2sys_private *priv, int timeout)
|
static int run_pmc_get_number_ports(struct pmc_node *node, int timeout)
|
||||||
{
|
{
|
||||||
struct ptp_message *msg;
|
struct ptp_message *msg;
|
||||||
int res;
|
int res;
|
||||||
struct defaultDS *dds;
|
struct defaultDS *dds;
|
||||||
|
|
||||||
res = run_pmc(priv, timeout, TLV_DEFAULT_DATA_SET, &msg);
|
res = run_pmc(node, timeout, TLV_DEFAULT_DATA_SET, &msg);
|
||||||
if (res <= 0)
|
if (res <= 0)
|
||||||
return res;
|
return res;
|
||||||
|
|
||||||
|
@ -1082,36 +1096,36 @@ static int run_pmc_get_number_ports(struct phc2sys_private *priv, int timeout)
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int run_pmc_subscribe(struct phc2sys_private *priv, int timeout)
|
static int run_pmc_subscribe(struct pmc_node *node, int timeout)
|
||||||
{
|
{
|
||||||
struct ptp_message *msg;
|
struct ptp_message *msg;
|
||||||
int res;
|
int res;
|
||||||
|
|
||||||
res = run_pmc(priv, timeout, TLV_SUBSCRIBE_EVENTS_NP, &msg);
|
res = run_pmc(node, timeout, TLV_SUBSCRIBE_EVENTS_NP, &msg);
|
||||||
if (res <= 0)
|
if (res <= 0)
|
||||||
return res;
|
return res;
|
||||||
msg_put(msg);
|
msg_put(msg);
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void run_pmc_events(struct phc2sys_private *priv)
|
static void run_pmc_events(struct pmc_node *node)
|
||||||
{
|
{
|
||||||
struct ptp_message *msg;
|
struct ptp_message *msg;
|
||||||
|
|
||||||
run_pmc(priv, 0, -1, &msg);
|
run_pmc(node, 0, -1, &msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
static int run_pmc_port_properties(struct phc2sys_private *priv, int timeout,
|
static int run_pmc_port_properties(struct pmc_node *node, int timeout,
|
||||||
unsigned int port,
|
unsigned int port, int *state,
|
||||||
int *state, int *tstamping, char *iface)
|
int *tstamping, char *iface)
|
||||||
{
|
{
|
||||||
struct ptp_message *msg;
|
struct ptp_message *msg;
|
||||||
int res, len;
|
int res, len;
|
||||||
struct port_properties_np *ppn;
|
struct port_properties_np *ppn;
|
||||||
|
|
||||||
pmc_target_port(priv->pmc, port);
|
pmc_target_port(node->pmc, port);
|
||||||
while (1) {
|
while (1) {
|
||||||
res = run_pmc(priv, timeout, TLV_PORT_PROPERTIES_NP, &msg);
|
res = run_pmc(node, timeout, TLV_PORT_PROPERTIES_NP, &msg);
|
||||||
if (res <= 0)
|
if (res <= 0)
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
|
@ -1134,32 +1148,35 @@ static int run_pmc_port_properties(struct phc2sys_private *priv, int timeout,
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
out:
|
out:
|
||||||
pmc_target_all(priv->pmc);
|
pmc_target_all(node->pmc);
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int run_pmc_clock_identity(struct phc2sys_private *priv, int timeout)
|
static int run_pmc_clock_identity(struct pmc_node *node, int timeout)
|
||||||
{
|
{
|
||||||
struct ptp_message *msg;
|
struct ptp_message *msg;
|
||||||
struct defaultDS *dds;
|
struct defaultDS *dds;
|
||||||
int res;
|
int res;
|
||||||
|
|
||||||
res = run_pmc(priv, timeout, TLV_DEFAULT_DATA_SET, &msg);
|
res = run_pmc(node, timeout, TLV_DEFAULT_DATA_SET, &msg);
|
||||||
if (res <= 0)
|
if (res <= 0)
|
||||||
return res;
|
return res;
|
||||||
|
|
||||||
dds = (struct defaultDS *)get_mgt_data(msg);
|
dds = (struct defaultDS *)get_mgt_data(msg);
|
||||||
memcpy(&priv->clock_identity, &dds->clockIdentity,
|
memcpy(&node->clock_identity, &dds->clockIdentity,
|
||||||
sizeof(struct ClockIdentity));
|
sizeof(struct ClockIdentity));
|
||||||
priv->clock_identity_set = 1;
|
node->clock_identity_set = 1;
|
||||||
msg_put(msg);
|
msg_put(msg);
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void close_pmc(struct phc2sys_private *priv)
|
static void close_pmc_node(struct pmc_node *node)
|
||||||
{
|
{
|
||||||
pmc_destroy(priv->pmc);
|
if (!node->pmc)
|
||||||
priv->pmc = NULL;
|
return;
|
||||||
|
|
||||||
|
pmc_destroy(node->pmc);
|
||||||
|
node->pmc = NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int auto_init_ports(struct phc2sys_private *priv, int add_rt)
|
static int auto_init_ports(struct phc2sys_private *priv, int add_rt)
|
||||||
|
@ -1174,7 +1191,7 @@ static int auto_init_ports(struct phc2sys_private *priv, int add_rt)
|
||||||
while (1) {
|
while (1) {
|
||||||
if (!is_running())
|
if (!is_running())
|
||||||
return -1;
|
return -1;
|
||||||
res = run_pmc_clock_identity(priv, 1000);
|
res = run_pmc_clock_identity(&priv->node, 1000);
|
||||||
if (res < 0)
|
if (res < 0)
|
||||||
return -1;
|
return -1;
|
||||||
if (res > 0)
|
if (res > 0)
|
||||||
|
@ -1183,20 +1200,20 @@ static int auto_init_ports(struct phc2sys_private *priv, int add_rt)
|
||||||
pr_notice("Waiting for ptp4l...");
|
pr_notice("Waiting for ptp4l...");
|
||||||
}
|
}
|
||||||
|
|
||||||
number_ports = run_pmc_get_number_ports(priv, 1000);
|
number_ports = run_pmc_get_number_ports(&priv->node, 1000);
|
||||||
if (number_ports <= 0) {
|
if (number_ports <= 0) {
|
||||||
pr_err("failed to get number of ports");
|
pr_err("failed to get number of ports");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
res = run_pmc_subscribe(priv, 1000);
|
res = run_pmc_subscribe(&priv->node, 1000);
|
||||||
if (res <= 0) {
|
if (res <= 0) {
|
||||||
pr_err("failed to subscribe");
|
pr_err("failed to subscribe");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (i = 1; i <= number_ports; i++) {
|
for (i = 1; i <= number_ports; i++) {
|
||||||
res = run_pmc_port_properties(priv, 1000, i, &state,
|
res = run_pmc_port_properties(&priv->node, 1000, i, &state,
|
||||||
×tamping, iface);
|
×tamping, iface);
|
||||||
if (res == -1) {
|
if (res == -1) {
|
||||||
/* port does not exist, ignore the port */
|
/* port does not exist, ignore the port */
|
||||||
|
@ -1233,7 +1250,7 @@ static int auto_init_ports(struct phc2sys_private *priv, int add_rt)
|
||||||
}
|
}
|
||||||
|
|
||||||
/* get initial offset */
|
/* get initial offset */
|
||||||
if (run_pmc_get_utc_offset(priv, 1000) <= 0) {
|
if (run_pmc_get_utc_offset(&priv->node, 1000) <= 0) {
|
||||||
pr_err("failed to get UTC offset");
|
pr_err("failed to get UTC offset");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
@ -1241,7 +1258,7 @@ static int auto_init_ports(struct phc2sys_private *priv, int add_rt)
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Returns: -1 in case of error, 0 otherwise */
|
/* Returns: -1 in case of error, 0 otherwise */
|
||||||
static int update_pmc(struct phc2sys_private *priv, int subscribe)
|
static int update_pmc_node(struct pmc_node *node, int subscribe)
|
||||||
{
|
{
|
||||||
struct timespec tp;
|
struct timespec tp;
|
||||||
uint64_t ts;
|
uint64_t ts;
|
||||||
|
@ -1252,13 +1269,13 @@ static int update_pmc(struct phc2sys_private *priv, int subscribe)
|
||||||
}
|
}
|
||||||
ts = tp.tv_sec * NS_PER_SEC + tp.tv_nsec;
|
ts = tp.tv_sec * NS_PER_SEC + tp.tv_nsec;
|
||||||
|
|
||||||
if (priv->pmc &&
|
if (node->pmc &&
|
||||||
!(ts > priv->pmc_last_update &&
|
!(ts > node->pmc_last_update &&
|
||||||
ts - priv->pmc_last_update < PMC_UPDATE_INTERVAL)) {
|
ts - node->pmc_last_update < PMC_UPDATE_INTERVAL)) {
|
||||||
if (subscribe)
|
if (subscribe)
|
||||||
run_pmc_subscribe(priv, 0);
|
run_pmc_subscribe(node, 0);
|
||||||
if (run_pmc_get_utc_offset(priv, 0) > 0)
|
if (run_pmc_get_utc_offset(node, 0) > 0)
|
||||||
priv->pmc_last_update = ts;
|
node->pmc_last_update = ts;
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -1268,9 +1285,9 @@ static int update_pmc(struct phc2sys_private *priv, int subscribe)
|
||||||
static int clock_handle_leap(struct phc2sys_private *priv, struct clock *clock,
|
static int clock_handle_leap(struct phc2sys_private *priv, struct clock *clock,
|
||||||
int64_t offset, uint64_t ts)
|
int64_t offset, uint64_t ts)
|
||||||
{
|
{
|
||||||
int clock_leap, node_leap = priv->leap;
|
int clock_leap, node_leap = priv->node.leap;
|
||||||
|
|
||||||
clock->sync_offset = priv->sync_offset;
|
clock->sync_offset = priv->node.sync_offset;
|
||||||
|
|
||||||
if ((node_leap || clock->leap_set) &&
|
if ((node_leap || clock->leap_set) &&
|
||||||
clock->is_utc != priv->master->is_utc) {
|
clock->is_utc != priv->master->is_utc) {
|
||||||
|
@ -1311,7 +1328,7 @@ static int clock_handle_leap(struct phc2sys_private *priv, struct clock *clock,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (priv->utc_offset_traceable &&
|
if (priv->node.utc_offset_traceable &&
|
||||||
clock->utc_offset_set != clock->sync_offset) {
|
clock->utc_offset_set != clock->sync_offset) {
|
||||||
if (clock->clkid == CLOCK_REALTIME)
|
if (clock->clkid == CLOCK_REALTIME)
|
||||||
sysclk_set_tai_offset(clock->sync_offset);
|
sysclk_set_tai_offset(clock->sync_offset);
|
||||||
|
@ -1365,6 +1382,7 @@ static void usage(char *progname)
|
||||||
int main(int argc, char *argv[])
|
int main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
char *config = NULL, *dst_name = NULL, *progname, *src_name = NULL;
|
char *config = NULL, *dst_name = NULL, *progname, *src_name = NULL;
|
||||||
|
char uds_local[MAX_IFNAME_SIZE + 1];
|
||||||
struct clock *src, *dst;
|
struct clock *src, *dst;
|
||||||
struct config *cfg;
|
struct config *cfg;
|
||||||
struct option *opts;
|
struct option *opts;
|
||||||
|
@ -1473,7 +1491,7 @@ int main(int argc, char *argv[])
|
||||||
goto end;
|
goto end;
|
||||||
break;
|
break;
|
||||||
case 'O':
|
case 'O':
|
||||||
if (get_arg_val_i(c, optarg, &priv.sync_offset,
|
if (get_arg_val_i(c, optarg, &priv.node.sync_offset,
|
||||||
INT_MIN, INT_MAX))
|
INT_MIN, INT_MAX))
|
||||||
goto end;
|
goto end;
|
||||||
priv.forced_sync_offset = -1;
|
priv.forced_sync_offset = -1;
|
||||||
|
@ -1593,8 +1611,12 @@ int main(int argc, char *argv[])
|
||||||
priv.kernel_leap = config_get_int(cfg, NULL, "kernel_leap");
|
priv.kernel_leap = config_get_int(cfg, NULL, "kernel_leap");
|
||||||
priv.sanity_freq_limit = config_get_int(cfg, NULL, "sanity_freq_limit");
|
priv.sanity_freq_limit = config_get_int(cfg, NULL, "sanity_freq_limit");
|
||||||
|
|
||||||
|
snprintf(uds_local, sizeof(uds_local), "/var/run/phc2sys.%d",
|
||||||
|
getpid());
|
||||||
|
|
||||||
if (autocfg) {
|
if (autocfg) {
|
||||||
if (init_pmc(cfg, &priv))
|
if (init_pmc_node(cfg, &priv.node, uds_local,
|
||||||
|
phc2sys_recv_subscribed))
|
||||||
goto end;
|
goto end;
|
||||||
if (auto_init_ports(&priv, rt) < 0)
|
if (auto_init_ports(&priv, rt) < 0)
|
||||||
goto end;
|
goto end;
|
||||||
|
@ -1631,11 +1653,12 @@ int main(int argc, char *argv[])
|
||||||
r = -1;
|
r = -1;
|
||||||
|
|
||||||
if (wait_sync) {
|
if (wait_sync) {
|
||||||
if (init_pmc(cfg, &priv))
|
if (init_pmc_node(cfg, &priv.node, uds_local,
|
||||||
|
phc2sys_recv_subscribed))
|
||||||
goto end;
|
goto end;
|
||||||
|
|
||||||
while (is_running()) {
|
while (is_running()) {
|
||||||
r = run_pmc_wait_sync(&priv, 1000);
|
r = run_pmc_wait_sync(&priv.node, 1000);
|
||||||
if (r < 0)
|
if (r < 0)
|
||||||
goto end;
|
goto end;
|
||||||
if (r > 0)
|
if (r > 0)
|
||||||
|
@ -1645,7 +1668,7 @@ int main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!priv.forced_sync_offset) {
|
if (!priv.forced_sync_offset) {
|
||||||
r = run_pmc_get_utc_offset(&priv, 1000);
|
r = run_pmc_get_utc_offset(&priv.node, 1000);
|
||||||
if (r <= 0) {
|
if (r <= 0) {
|
||||||
pr_err("failed to get UTC offset");
|
pr_err("failed to get UTC offset");
|
||||||
goto end;
|
goto end;
|
||||||
|
@ -1655,7 +1678,7 @@ int main(int argc, char *argv[])
|
||||||
if (priv.forced_sync_offset ||
|
if (priv.forced_sync_offset ||
|
||||||
(src->clkid != CLOCK_REALTIME && dst->clkid != CLOCK_REALTIME) ||
|
(src->clkid != CLOCK_REALTIME && dst->clkid != CLOCK_REALTIME) ||
|
||||||
src->clkid == CLOCK_INVALID)
|
src->clkid == CLOCK_INVALID)
|
||||||
close_pmc(&priv);
|
close_pmc_node(&priv.node);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (pps_fd >= 0) {
|
if (pps_fd >= 0) {
|
||||||
|
@ -1668,8 +1691,7 @@ int main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
end:
|
end:
|
||||||
if (priv.pmc)
|
close_pmc_node(&priv.node);
|
||||||
close_pmc(&priv);
|
|
||||||
clock_cleanup(&priv);
|
clock_cleanup(&priv);
|
||||||
port_cleanup(&priv);
|
port_cleanup(&priv);
|
||||||
config_destroy(cfg);
|
config_destroy(cfg);
|
||||||
|
|
Loading…
Reference in New Issue