phc2sys: break out pmc code into pmc_common.c

The code through which phc2sys sends various PTP management messages to
ptp4l via pmc can be reused.

This patch is a trivial movement of that code to a separate translation
module, outside of phc2sys. This makes it available to other programs
that want to subscribe to port state change events too, such as ts2phc.

Signed-off-by: Vladimir Oltean <olteanv@gmail.com>
Reviewed-by: Jacob Keller <jacob.e.keller@intel.com>
master
Vladimir Oltean 2020-08-30 01:37:51 +03:00
parent cc88812d12
commit f44b3d8b6c
3 changed files with 372 additions and 358 deletions

358
phc2sys.c
View File

@ -63,12 +63,6 @@
#define NS_PER_SEC 1000000000LL
#define PHC_PPS_OFFSET_LIMIT 10000000
#define PMC_UPDATE_INTERVAL (60 * NS_PER_SEC)
#define PMC_SUBSCRIBE_DURATION 180 /* 3 minutes */
/* Note that PMC_SUBSCRIBE_DURATION has to be longer than
* PMC_UPDATE_INTERVAL otherwise subscription will time out before it is
* renewed.
*/
struct clock {
LIST_ENTRY(clock) list;
@ -100,24 +94,6 @@ struct port {
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 {
unsigned int stats_max_count;
int sanity_freq_limit;
@ -136,17 +112,11 @@ struct phc2sys_private {
static struct config *phc2sys_config;
int update_pmc_node(struct pmc_node *node, int subscribe);
static int clock_handle_leap(struct phc2sys_private *priv,
struct clock *clock,
int64_t offset, uint64_t ts);
int run_pmc_get_utc_offset(struct pmc_node *node, int timeout);
void run_pmc_events(struct pmc_node *node);
static int normalize_state(int state);
int run_pmc_port_properties(struct pmc_node *node, int timeout,
unsigned int port, int *state,
int *tstamping, char *iface);
static struct servo *servo_add(struct phc2sys_private *priv,
struct clock *clock)
@ -803,56 +773,6 @@ static int do_loop(struct phc2sys_private *priv, int subscriptions)
return 0;
}
static int check_clock_identity(struct pmc_node *node, struct ptp_message *msg)
{
if (!node->clock_identity_set)
return 1;
return cid_eq(&node->clock_identity,
&msg->header.sourcePortIdentity.clockIdentity);
}
static int is_msg_mgt(struct ptp_message *msg)
{
struct TLV *tlv;
if (msg_type(msg) != MANAGEMENT)
return 0;
if (management_action(msg) != RESPONSE)
return 0;
if (msg_tlv_count(msg) != 1)
return 0;
tlv = (struct TLV *) msg->management.suffix;
if (tlv->type == TLV_MANAGEMENT)
return 1;
if (tlv->type == TLV_MANAGEMENT_ERROR_STATUS)
return -1;
return 0;
}
int get_mgt_id(struct ptp_message *msg)
{
struct management_tlv *mgt;
mgt = (struct management_tlv *) msg->management.suffix;
return mgt->id;
}
void *get_mgt_data(struct ptp_message *msg)
{
struct management_tlv *mgt;
mgt = (struct management_tlv *) msg->management.suffix;
return mgt->data;
}
static int get_mgt_err_id(struct ptp_message *msg)
{
struct management_error_status *mgt;
mgt = (struct management_error_status *)msg->management.suffix;
return mgt->id;
}
static int normalize_state(int state)
{
if (state != PS_MASTER && state != PS_SLAVE &&
@ -923,260 +843,6 @@ static int phc2sys_recv_subscribed(struct pmc_node *node,
return 0;
}
static void send_subscription(struct pmc_node *node)
{
struct subscribe_events_np sen;
memset(&sen, 0, sizeof(sen));
sen.duration = PMC_SUBSCRIBE_DURATION;
sen.bitmask[0] = 1 << NOTIFY_PORT_STATE;
pmc_send_set_action(node->pmc, TLV_SUBSCRIBE_EVENTS_NP, &sen, sizeof(sen));
}
int init_pmc_node(struct config *cfg, struct pmc_node *node, const char *uds,
pmc_node_recv_subscribed_t *recv_subscribed)
{
node->pmc = pmc_create(cfg, TRANS_UDS, uds, 0,
config_get_int(cfg, NULL, "domainNumber"),
config_get_int(cfg, NULL, "transportSpecific") << 4, 1);
if (!node->pmc) {
pr_err("failed to create pmc");
return -1;
}
node->recv_subscribed = recv_subscribed;
return 0;
}
/* Return values:
* 1: success
* 0: timeout
* -1: error reported by the other side
* -2: local error, fatal
*/
static int run_pmc(struct pmc_node *node, int timeout, int ds_id,
struct ptp_message **msg)
{
#define N_FD 1
struct pollfd pollfd[N_FD];
int cnt, res;
while (1) {
pollfd[0].fd = pmc_get_transport_fd(node->pmc);
pollfd[0].events = POLLIN|POLLPRI;
if (!node->pmc_ds_requested && ds_id >= 0)
pollfd[0].events |= POLLOUT;
cnt = poll(pollfd, N_FD, timeout);
if (cnt < 0) {
pr_err("poll failed");
return -2;
}
if (!cnt) {
/* Request the data set again in the next run. */
node->pmc_ds_requested = 0;
return 0;
}
/* Send a new request if there are no pending messages. */
if ((pollfd[0].revents & POLLOUT) &&
!(pollfd[0].revents & (POLLIN|POLLPRI))) {
switch (ds_id) {
case TLV_SUBSCRIBE_EVENTS_NP:
send_subscription(node);
break;
default:
pmc_send_get_action(node->pmc, ds_id);
break;
}
node->pmc_ds_requested = 1;
}
if (!(pollfd[0].revents & (POLLIN|POLLPRI)))
continue;
*msg = pmc_recv(node->pmc);
if (!*msg)
continue;
if (!check_clock_identity(node, *msg)) {
msg_put(*msg);
*msg = NULL;
continue;
}
res = is_msg_mgt(*msg);
if (res < 0 && get_mgt_err_id(*msg) == ds_id) {
node->pmc_ds_requested = 0;
return -1;
}
if (res <= 0 || node->recv_subscribed(node, *msg, ds_id) ||
get_mgt_id(*msg) != ds_id) {
msg_put(*msg);
*msg = NULL;
continue;
}
node->pmc_ds_requested = 0;
return 1;
}
}
static int run_pmc_wait_sync(struct pmc_node *node, int timeout)
{
struct ptp_message *msg;
Enumeration8 portState;
void *data;
int res;
while (1) {
res = run_pmc(node, timeout, TLV_PORT_DATA_SET, &msg);
if (res <= 0)
return res;
data = get_mgt_data(msg);
portState = ((struct portDS *)data)->portState;
msg_put(msg);
switch (portState) {
case PS_MASTER:
case PS_SLAVE:
return 1;
}
/* try to get more data sets (for other ports) */
node->pmc_ds_requested = 1;
}
}
int run_pmc_get_utc_offset(struct pmc_node *node, int timeout)
{
struct ptp_message *msg;
int res;
struct timePropertiesDS *tds;
res = run_pmc(node, timeout, TLV_TIME_PROPERTIES_DATA_SET, &msg);
if (res <= 0)
return res;
tds = (struct timePropertiesDS *)get_mgt_data(msg);
if (tds->flags & PTP_TIMESCALE) {
node->sync_offset = tds->currentUtcOffset;
if (tds->flags & LEAP_61)
node->leap = 1;
else if (tds->flags & LEAP_59)
node->leap = -1;
else
node->leap = 0;
node->utc_offset_traceable = tds->flags & UTC_OFF_VALID &&
tds->flags & TIME_TRACEABLE;
} else {
node->sync_offset = 0;
node->leap = 0;
node->utc_offset_traceable = 0;
}
msg_put(msg);
return 1;
}
int run_pmc_get_number_ports(struct pmc_node *node, int timeout)
{
struct ptp_message *msg;
int res;
struct defaultDS *dds;
res = run_pmc(node, timeout, TLV_DEFAULT_DATA_SET, &msg);
if (res <= 0)
return res;
dds = (struct defaultDS *)get_mgt_data(msg);
res = dds->numberPorts;
msg_put(msg);
return res;
}
int run_pmc_subscribe(struct pmc_node *node, int timeout)
{
struct ptp_message *msg;
int res;
res = run_pmc(node, timeout, TLV_SUBSCRIBE_EVENTS_NP, &msg);
if (res <= 0)
return res;
msg_put(msg);
return 1;
}
void run_pmc_events(struct pmc_node *node)
{
struct ptp_message *msg;
run_pmc(node, 0, -1, &msg);
}
int run_pmc_port_properties(struct pmc_node *node, int timeout,
unsigned int port, int *state,
int *tstamping, char *iface)
{
struct ptp_message *msg;
int res, len;
struct port_properties_np *ppn;
pmc_target_port(node->pmc, port);
while (1) {
res = run_pmc(node, timeout, TLV_PORT_PROPERTIES_NP, &msg);
if (res <= 0)
goto out;
ppn = get_mgt_data(msg);
if (ppn->portIdentity.portNumber != port) {
msg_put(msg);
continue;
}
*state = ppn->port_state;
*tstamping = ppn->timestamping;
len = ppn->interface.length;
if (len > IFNAMSIZ - 1)
len = IFNAMSIZ - 1;
memcpy(iface, ppn->interface.text, len);
iface[len] = '\0';
msg_put(msg);
res = 1;
break;
}
out:
pmc_target_all(node->pmc);
return res;
}
static int run_pmc_clock_identity(struct pmc_node *node, int timeout)
{
struct ptp_message *msg;
struct defaultDS *dds;
int res;
res = run_pmc(node, timeout, TLV_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;
}
void close_pmc_node(struct pmc_node *node)
{
if (!node->pmc)
return;
pmc_destroy(node->pmc);
node->pmc = NULL;
}
static int auto_init_ports(struct phc2sys_private *priv, int add_rt)
{
struct port *port;
@ -1255,30 +921,6 @@ static int auto_init_ports(struct phc2sys_private *priv, int add_rt)
return 0;
}
/* Returns: -1 in case of error, 0 otherwise */
int update_pmc_node(struct pmc_node *node, int subscribe)
{
struct timespec tp;
uint64_t ts;
if (clock_gettime(CLOCK_MONOTONIC, &tp)) {
pr_err("failed to read clock: %m");
return -1;
}
ts = tp.tv_sec * NS_PER_SEC + tp.tv_nsec;
if (node->pmc &&
!(ts > node->pmc_last_update &&
ts - node->pmc_last_update < PMC_UPDATE_INTERVAL)) {
if (subscribe)
run_pmc_subscribe(node, 0);
if (run_pmc_get_utc_offset(node, 0) > 0)
node->pmc_last_update = ts;
}
return 0;
}
/* Returns: non-zero to skip clock update */
static int clock_handle_leap(struct phc2sys_private *priv, struct clock *clock,
int64_t offset, uint64_t ts)

View File

@ -18,6 +18,8 @@
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#include <errno.h>
#include <net/if.h>
#include <poll.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
@ -56,6 +58,13 @@
/* Includes one extra byte to make length even. */
#define EMPTY_PTP_TEXT 2
#define PMC_UPDATE_INTERVAL (60 * NS_PER_SEC)
#define PMC_SUBSCRIBE_DURATION 180 /* 3 minutes */
/* Note that PMC_SUBSCRIBE_DURATION has to be longer than
* PMC_UPDATE_INTERVAL otherwise subscription will time out before it is
* renewed.
*/
static void do_get_action(struct pmc *pmc, int action, int index, char *str);
static void do_set_action(struct pmc *pmc, int action, int index, char *str);
static void not_supported(struct pmc *pmc, int action, int index, char *str);
@ -711,3 +720,331 @@ int pmc_do_command(struct pmc *pmc, char *str)
return 0;
}
static void send_subscription(struct pmc_node *node)
{
struct subscribe_events_np sen;
memset(&sen, 0, sizeof(sen));
sen.duration = PMC_SUBSCRIBE_DURATION;
sen.bitmask[0] = 1 << NOTIFY_PORT_STATE;
pmc_send_set_action(node->pmc, TLV_SUBSCRIBE_EVENTS_NP, &sen, sizeof(sen));
}
static int check_clock_identity(struct pmc_node *node, struct ptp_message *msg)
{
if (!node->clock_identity_set)
return 1;
return cid_eq(&node->clock_identity,
&msg->header.sourcePortIdentity.clockIdentity);
}
static int is_msg_mgt(struct ptp_message *msg)
{
struct TLV *tlv;
if (msg_type(msg) != MANAGEMENT)
return 0;
if (management_action(msg) != RESPONSE)
return 0;
if (msg_tlv_count(msg) != 1)
return 0;
tlv = (struct TLV *) msg->management.suffix;
if (tlv->type == TLV_MANAGEMENT)
return 1;
if (tlv->type == TLV_MANAGEMENT_ERROR_STATUS)
return -1;
return 0;
}
int get_mgt_id(struct ptp_message *msg)
{
struct management_tlv *mgt;
mgt = (struct management_tlv *) msg->management.suffix;
return mgt->id;
}
void *get_mgt_data(struct ptp_message *msg)
{
struct management_tlv *mgt;
mgt = (struct management_tlv *) msg->management.suffix;
return mgt->data;
}
static int get_mgt_err_id(struct ptp_message *msg)
{
struct management_error_status *mgt;
mgt = (struct management_error_status *)msg->management.suffix;
return mgt->id;
}
/* Return values:
* 1: success
* 0: timeout
* -1: error reported by the other side
* -2: local error, fatal
*/
static int run_pmc(struct pmc_node *node, int timeout, int ds_id,
struct ptp_message **msg)
{
#define N_FD 1
struct pollfd pollfd[N_FD];
int cnt, res;
while (1) {
pollfd[0].fd = pmc_get_transport_fd(node->pmc);
pollfd[0].events = POLLIN|POLLPRI;
if (!node->pmc_ds_requested && ds_id >= 0)
pollfd[0].events |= POLLOUT;
cnt = poll(pollfd, N_FD, timeout);
if (cnt < 0) {
pr_err("poll failed");
return -2;
}
if (!cnt) {
/* Request the data set again in the next run. */
node->pmc_ds_requested = 0;
return 0;
}
/* Send a new request if there are no pending messages. */
if ((pollfd[0].revents & POLLOUT) &&
!(pollfd[0].revents & (POLLIN|POLLPRI))) {
switch (ds_id) {
case TLV_SUBSCRIBE_EVENTS_NP:
send_subscription(node);
break;
default:
pmc_send_get_action(node->pmc, ds_id);
break;
}
node->pmc_ds_requested = 1;
}
if (!(pollfd[0].revents & (POLLIN|POLLPRI)))
continue;
*msg = pmc_recv(node->pmc);
if (!*msg)
continue;
if (!check_clock_identity(node, *msg)) {
msg_put(*msg);
*msg = NULL;
continue;
}
res = is_msg_mgt(*msg);
if (res < 0 && get_mgt_err_id(*msg) == ds_id) {
node->pmc_ds_requested = 0;
return -1;
}
if (res <= 0 || node->recv_subscribed(node, *msg, ds_id) ||
get_mgt_id(*msg) != ds_id) {
msg_put(*msg);
*msg = NULL;
continue;
}
node->pmc_ds_requested = 0;
return 1;
}
}
int run_pmc_wait_sync(struct pmc_node *node, int timeout)
{
struct ptp_message *msg;
Enumeration8 portState;
void *data;
int res;
while (1) {
res = run_pmc(node, timeout, TLV_PORT_DATA_SET, &msg);
if (res <= 0)
return res;
data = get_mgt_data(msg);
portState = ((struct portDS *)data)->portState;
msg_put(msg);
switch (portState) {
case PS_MASTER:
case PS_SLAVE:
return 1;
}
/* try to get more data sets (for other ports) */
node->pmc_ds_requested = 1;
}
}
int run_pmc_get_utc_offset(struct pmc_node *node, int timeout)
{
struct ptp_message *msg;
int res;
struct timePropertiesDS *tds;
res = run_pmc(node, timeout, TLV_TIME_PROPERTIES_DATA_SET, &msg);
if (res <= 0)
return res;
tds = (struct timePropertiesDS *)get_mgt_data(msg);
if (tds->flags & PTP_TIMESCALE) {
node->sync_offset = tds->currentUtcOffset;
if (tds->flags & LEAP_61)
node->leap = 1;
else if (tds->flags & LEAP_59)
node->leap = -1;
else
node->leap = 0;
node->utc_offset_traceable = tds->flags & UTC_OFF_VALID &&
tds->flags & TIME_TRACEABLE;
} else {
node->sync_offset = 0;
node->leap = 0;
node->utc_offset_traceable = 0;
}
msg_put(msg);
return 1;
}
int run_pmc_get_number_ports(struct pmc_node *node, int timeout)
{
struct ptp_message *msg;
int res;
struct defaultDS *dds;
res = run_pmc(node, timeout, TLV_DEFAULT_DATA_SET, &msg);
if (res <= 0)
return res;
dds = (struct defaultDS *)get_mgt_data(msg);
res = dds->numberPorts;
msg_put(msg);
return res;
}
int run_pmc_subscribe(struct pmc_node *node, int timeout)
{
struct ptp_message *msg;
int res;
res = run_pmc(node, timeout, TLV_SUBSCRIBE_EVENTS_NP, &msg);
if (res <= 0)
return res;
msg_put(msg);
return 1;
}
void run_pmc_events(struct pmc_node *node)
{
struct ptp_message *msg;
run_pmc(node, 0, -1, &msg);
}
int run_pmc_port_properties(struct pmc_node *node, int timeout,
unsigned int port, int *state,
int *tstamping, char *iface)
{
struct ptp_message *msg;
int res, len;
struct port_properties_np *ppn;
pmc_target_port(node->pmc, port);
while (1) {
res = run_pmc(node, timeout, TLV_PORT_PROPERTIES_NP, &msg);
if (res <= 0)
goto out;
ppn = get_mgt_data(msg);
if (ppn->portIdentity.portNumber != port) {
msg_put(msg);
continue;
}
*state = ppn->port_state;
*tstamping = ppn->timestamping;
len = ppn->interface.length;
if (len > IFNAMSIZ - 1)
len = IFNAMSIZ - 1;
memcpy(iface, ppn->interface.text, len);
iface[len] = '\0';
msg_put(msg);
res = 1;
break;
}
out:
pmc_target_all(node->pmc);
return res;
}
int run_pmc_clock_identity(struct pmc_node *node, int timeout)
{
struct ptp_message *msg;
struct defaultDS *dds;
int res;
res = run_pmc(node, timeout, TLV_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;
}
/* Returns: -1 in case of error, 0 otherwise */
int update_pmc_node(struct pmc_node *node, int subscribe)
{
struct timespec tp;
uint64_t ts;
if (clock_gettime(CLOCK_MONOTONIC, &tp)) {
pr_err("failed to read clock: %m");
return -1;
}
ts = tp.tv_sec * NS_PER_SEC + tp.tv_nsec;
if (node->pmc &&
!(ts > node->pmc_last_update &&
ts - node->pmc_last_update < PMC_UPDATE_INTERVAL)) {
if (subscribe)
run_pmc_subscribe(node, 0);
if (run_pmc_get_utc_offset(node, 0) > 0)
node->pmc_last_update = ts;
}
return 0;
}
int init_pmc_node(struct config *cfg, struct pmc_node *node, const char *uds,
pmc_node_recv_subscribed_t *recv_subscribed)
{
node->pmc = pmc_create(cfg, TRANS_UDS, uds, 0,
config_get_int(cfg, NULL, "domainNumber"),
config_get_int(cfg, NULL, "transportSpecific") << 4, 1);
if (!node->pmc) {
pr_err("failed to create pmc");
return -1;
}
node->recv_subscribed = recv_subscribed;
return 0;
}
void close_pmc_node(struct pmc_node *node)
{
if (!node->pmc)
return;
pmc_destroy(node->pmc);
node->pmc = NULL;
}

View File

@ -22,6 +22,7 @@
#define HAVE_PMC_COMMON_H
#include "config.h"
#include "fsm.h"
#include "msg.h"
#include "transport.h"
@ -49,4 +50,38 @@ void pmc_target_all(struct pmc *pmc);
const char *pmc_action_string(int action);
int pmc_do_command(struct pmc *pmc, char *str);
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;
};
int init_pmc_node(struct config *cfg, struct pmc_node *node, const char *uds,
pmc_node_recv_subscribed_t *recv_subscribed);
void close_pmc_node(struct pmc_node *node);
int update_pmc_node(struct pmc_node *node, int subscribe);
int run_pmc_subscribe(struct pmc_node *node, int timeout);
int run_pmc_clock_identity(struct pmc_node *node, int timeout);
int run_pmc_wait_sync(struct pmc_node *node, int timeout);
int run_pmc_get_number_ports(struct pmc_node *node, int timeout);
void run_pmc_events(struct pmc_node *node);
int run_pmc_port_properties(struct pmc_node *node, int timeout,
unsigned int port, int *state,
int *tstamping, char *iface);
int run_pmc_get_utc_offset(struct pmc_node *node, int timeout);
int get_mgt_id(struct ptp_message *msg);
void *get_mgt_data(struct ptp_message *msg);
#endif