Add support for subscribing to events (run_pmc_subscribe) and receiving and
handling of received events (run_pmc_events).
Add initial support for port status changes.
Signed-off-by: Jiri Benc <jb...@redhat.com>
---
phc2sys.c | 113 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++--
1 files changed, 110 insertions(+), 3 deletions(-)
diff --git a/phc2sys.c b/phc2sys.c
index ece4560e0c67..369fb7c177a5 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -41,6 +41,7 @@
#include "ds.h"
#include "fsm.h"
#include "missing.h"
+#include "notification.h"
#include "phc.h"
#include "pi.h"
#include "pmc_common.h"
@@ -65,6 +66,8 @@ struct clock {
clockid_t clkid;
int sysoff_supported;
int is_utc;
+ int state;
+ int new_state;
struct servo *servo;
enum servo_state servo_state;
char *device;
@@ -78,6 +81,7 @@ struct clock {
struct port {
LIST_ENTRY(port) list;
unsigned int number;
+ int state;
struct clock *clock;
};
@@ -95,6 +99,7 @@ struct node {
struct pmc *pmc;
int pmc_ds_requested;
uint64_t pmc_last_update;
+ int state_changed;
LIST_HEAD(port_head, port) ports;
LIST_HEAD(clock_head, clock) clocks;
struct clock *master;
@@ -511,7 +516,8 @@ static int is_msg_mgt(struct ptp_message *msg)
if (msg_type(msg) != MANAGEMENT)
return 0;
- if (management_action(msg) != RESPONSE)
+ if (management_action(msg) != RESPONSE &&
+ management_action(msg) != ACKNOWLEDGE)
return 0;
if (msg->tlv_count != 1)
return 0;
@@ -533,6 +539,79 @@ static void *get_mgt_data(struct ptp_message *msg)
return mgt->data;
}
+static int normalize_state(int state)
+{
+ if (state != PS_MASTER && state != PS_SLAVE &&
+ state != PS_PRE_MASTER && state != PS_UNCALIBRATED) {
+ /* treat any other state as "not a master nor a slave" */
+ state = PS_DISABLED;
+ }
+ return state;
+}
+
+static int clock_compute_state(struct node *node, struct clock *clock)
+{
+ struct port *p;
+ int state = PS_DISABLED;
+
+ LIST_FOREACH(p, &node->ports, list) {
+ if (p->clock != clock)
+ continue;
+ /* PS_SLAVE takes the highest precedence, PS_UNCALIBRATED
+ * after that, PS_MASTER is third, PS_PRE_MASTER fourth and
+ * all of that overrides PS_DISABLED, which corresponds
+ * nicely with the numerical values */
+ if (p->state > state)
+ state = p->state;
+ }
+ return state;
+}
+
+static int recv_subscribed(struct node *node, struct ptp_message *msg,
+ int excluded)
+{
+ int mgt_id, state;
+ struct portDS *pds;
+ struct port *port;
+ struct clock *clock;
+
+ mgt_id = get_mgt_id(msg);
+ if (mgt_id == excluded)
+ return 0;
+ switch (mgt_id) {
+ case PORT_DATA_SET:
+ pds = get_mgt_data(msg);
+ port = port_get(node, pds->portIdentity.portNumber);
+ if (!port) {
+ pr_info("received data for unknown port %s",
+ pid2str(&pds->portIdentity));
+ return 1;
+ }
+ state = normalize_state(pds->portState);
+ if (port->state != state) {
+ pr_info("port %s changed state",
+ pid2str(&pds->portIdentity));
+ port->state = state;
+ clock = port->clock;
+ state = clock_compute_state(node, clock);
+ if (clock->state != state) {
+ clock->new_state = state;
+ node->state_changed = 1;
+ }
+ }
+ return 1;
+ }
+ return 0;
+}
+
+static void send_subscription(struct node *node)
+{
+ struct subscribe_events_np sen;
+
+ sen.bitmask = 1 << NOTIFY_PORT_STATE;
+ pmc_send_command_action(node->pmc, SUBSCRIBE_EVENTS_NP, &sen,
sizeof(sen));
+}
+
static int init_pmc(struct node *node, int domain_number)
{
node->pmc = pmc_create(TRANS_UDS, "/var/run/phc2sys", 0,
@@ -555,7 +634,7 @@ static int run_pmc(struct node *node, int timeout, int
ds_id,
while (1) {
pollfd[0].fd = pmc_get_transport_fd(node->pmc);
pollfd[0].events = POLLIN|POLLPRI;
- if (!node->pmc_ds_requested)
+ if (!node->pmc_ds_requested && ds_id >= 0)
pollfd[0].events |= POLLOUT;
cnt = poll(pollfd, N_FD, timeout);
@@ -572,7 +651,14 @@ static int run_pmc(struct node *node, int timeout, int
ds_id,
/* Send a new request if there are no pending messages. */
if ((pollfd[0].revents & POLLOUT) &&
!(pollfd[0].revents & (POLLIN|POLLPRI))) {
- pmc_send_get_action(node->pmc, ds_id);
+ switch (ds_id) {
+ case SUBSCRIBE_EVENTS_NP:
+ send_subscription(node);
+ break;
+ default:
+ pmc_send_get_action(node->pmc, ds_id);
+ break;
+ }
node->pmc_ds_requested = 1;
}
@@ -585,6 +671,7 @@ static int run_pmc(struct node *node, int timeout, int
ds_id,
continue;
if (!is_msg_mgt(*msg) ||
+ recv_subscribed(node, *msg, ds_id) ||
get_mgt_id(*msg) != ds_id) {
msg_put(*msg);
*msg = NULL;
@@ -645,6 +732,26 @@ static int run_pmc_get_utc_offset(struct node *node, int
timeout)
return 1;
}
+static int run_pmc_subscribe(struct node *node, int timeout)
+{
+ struct ptp_message *msg;
+ int res;
+
+ res = run_pmc(node, timeout, SUBSCRIBE_EVENTS_NP, &msg);
+ if (res <= 0)
+ return res;
+ res = management_action(msg) == ACKNOWLEDGE;
+ msg_put(msg);
+ return res;
+}
+
+static void run_pmc_events(struct node *node)
+{
+ struct ptp_message *msg;
+
+ run_pmc(node, 0, -1, &msg);
+}
+
static void close_pmc(struct node *node)
{
pmc_destroy(node->pmc);
--
1.7.6.5
------------------------------------------------------------------------------
Learn Graph Databases - Download FREE O'Reilly Book
"Graph Databases" is the definitive new guide to graph databases and their
applications. Written by three acclaimed leaders in the field,
this first edition is now available. Download your free book today!
http://p.sf.net/sfu/13534_NeoTech
_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
