phc2sys: check clockIdentity
Make sure that we handle only one PTP clock (node). This is for an extra safety. Signed-off-by: Jiri Benc <jbenc@redhat.com>master
parent
3dd4d51c8e
commit
66c9f9baaf
44
phc2sys.c
44
phc2sys.c
|
@ -107,6 +107,8 @@ struct node {
|
||||||
int pmc_ds_requested;
|
int pmc_ds_requested;
|
||||||
uint64_t pmc_last_update;
|
uint64_t pmc_last_update;
|
||||||
int state_changed;
|
int state_changed;
|
||||||
|
int clock_identity_set;
|
||||||
|
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;
|
||||||
struct clock *master;
|
struct clock *master;
|
||||||
|
@ -612,6 +614,15 @@ static int do_loop(struct node *node, int subscriptions)
|
||||||
return 0; /* unreachable */
|
return 0; /* unreachable */
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static int check_clock_identity(struct node *node, struct ptp_message *msg)
|
||||||
|
{
|
||||||
|
if (!node->clock_identity_set)
|
||||||
|
return 1;
|
||||||
|
return !memcmp(&node->clock_identity,
|
||||||
|
&msg->header.sourcePortIdentity.clockIdentity,
|
||||||
|
sizeof(struct ClockIdentity));
|
||||||
|
}
|
||||||
|
|
||||||
static int is_msg_mgt(struct ptp_message *msg)
|
static int is_msg_mgt(struct ptp_message *msg)
|
||||||
{
|
{
|
||||||
struct TLV *tlv;
|
struct TLV *tlv;
|
||||||
|
@ -789,6 +800,12 @@ static int run_pmc(struct node *node, int timeout, int ds_id,
|
||||||
if (!*msg)
|
if (!*msg)
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
|
if (!check_clock_identity(node, *msg)) {
|
||||||
|
msg_put(*msg);
|
||||||
|
*msg = NULL;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
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) {
|
||||||
node->pmc_ds_requested = 0;
|
node->pmc_ds_requested = 0;
|
||||||
|
@ -927,6 +944,24 @@ out:
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static int run_pmc_clock_identity(struct node *node, int timeout)
|
||||||
|
{
|
||||||
|
struct ptp_message *msg;
|
||||||
|
struct defaultDS *dds;
|
||||||
|
int res;
|
||||||
|
|
||||||
|
res = run_pmc(node, timeout, DEFAULT_DATA_SET, &msg);
|
||||||
|
if (res <= 0)
|
||||||
|
return res;
|
||||||
|
|
||||||
|
dds = (struct defaultDS *)get_mgt_data(msg);
|
||||||
|
memcpy(&node->clock_identity, &dds->clockIdentity,
|
||||||
|
sizeof(struct ClockIdentity));
|
||||||
|
node->clock_identity_set = 1;
|
||||||
|
msg_put(msg);
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
static void close_pmc(struct node *node)
|
static void close_pmc(struct node *node)
|
||||||
{
|
{
|
||||||
pmc_destroy(node->pmc);
|
pmc_destroy(node->pmc);
|
||||||
|
@ -943,7 +978,7 @@ static int auto_init_ports(struct node *node, int add_rt)
|
||||||
char iface[IFNAMSIZ];
|
char iface[IFNAMSIZ];
|
||||||
|
|
||||||
while (1) {
|
while (1) {
|
||||||
res = run_pmc_get_number_ports(node, 1000);
|
res = run_pmc_clock_identity(node, 1000);
|
||||||
if (res < 0)
|
if (res < 0)
|
||||||
return -1;
|
return -1;
|
||||||
if (res > 0)
|
if (res > 0)
|
||||||
|
@ -951,7 +986,12 @@ static int auto_init_ports(struct node *node, int add_rt)
|
||||||
/* res == 0, timeout */
|
/* res == 0, timeout */
|
||||||
pr_notice("Waiting for ptp4l...");
|
pr_notice("Waiting for ptp4l...");
|
||||||
}
|
}
|
||||||
number_ports = res;
|
|
||||||
|
number_ports = run_pmc_get_number_ports(node, 1000);
|
||||||
|
if (number_ports <= 0) {
|
||||||
|
pr_err("failed to get number of ports");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
res = run_pmc_subscribe(node, 1000);
|
res = run_pmc_subscribe(node, 1000);
|
||||||
if (res <= 0) {
|
if (res <= 0) {
|
||||||
|
|
Loading…
Reference in New Issue