Signed-off-by: Martin Pecka <pecka...@fel.cvut.cz>
---
clock.c | 7 +++++++
pmc.c | 6 ++++++
pmc_common.c | 1 +
tlv.h | 5 +++++
util.c | 16 ++++++++++++++++
util.h | 8 ++++++++
6 files changed, 43 insertions(+)
diff --git a/clock.c b/clock.c
index bbfd1a8..cd46b06 100644
--- a/clock.c
+++ b/clock.c
@@ -346,6 +346,7 @@ static int clock_management_fill_response(struct clock *c,
struct port *p,
struct grandmaster_settings_np *gsn;
struct management_tlv_datum *mtd;
struct subscribe_events_np *sen;
+ struct servo_state_np *servo;
struct management_tlv *tlv;
struct time_status_np *tsn;
struct tlv_extra *extra;
@@ -462,6 +463,11 @@ static int clock_management_fill_response(struct clock *c,
struct port *p,
mtd->val = c->local_sync_uncertain;
datalen = sizeof(*mtd);
break;
+ case MID_SERVO_STATE_NP:
+ servo = (struct servo_state_np *) tlv->data;
+ servo->state = c->servo_state;
+ datalen = sizeof(*servo);
+ break;
default:
/* The caller should *not* respond to this message. */
tlv_extra_recycle(extra);
@@ -1524,6 +1530,7 @@ int clock_manage(struct clock *c, struct port *p, struct
ptp_message *msg)
case MID_GRANDMASTER_SETTINGS_NP:
case MID_SUBSCRIBE_EVENTS_NP:
case MID_SYNCHRONIZATION_UNCERTAIN_NP:
+ case MID_SERVO_STATE_NP:
clock_management_send_error(p, msg, MID_NOT_SUPPORTED);
break;
default:
diff --git a/pmc.c b/pmc.c
index 7916153..a3dcf44 100644
--- a/pmc.c
+++ b/pmc.c
@@ -165,6 +165,7 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
struct unicast_master_entry *ume;
struct subscribe_events_np *sen;
struct port_properties_np *ppn;
+ struct servo_state_np *servo;
struct timePropertiesDS *tp;
struct management_tlv *mgt;
struct time_status_np *tsn;
@@ -425,6 +426,11 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
fprintf(fp, "SYNCHRONIZATION_UNCERTAIN_NP "
IFMT "uncertain %hhu", mtd->val);
break;
+ case MID_SERVO_STATE_NP:
+ servo = (struct servo_state_np *) mgt->data;
+ fprintf(fp, "SERVO_STATE_NP "
+ IFMT "state %s", servo_state2str(servo->state));
+ break;
case MID_PORT_DATA_SET:
p = (struct portDS *) mgt->data;
if (p->portState > PS_SLAVE) {
diff --git a/pmc_common.c b/pmc_common.c
index 017a4f7..a2fee8b 100644
--- a/pmc_common.c
+++ b/pmc_common.c
@@ -111,6 +111,7 @@ struct management_id idtab[] = {
{ "GRANDMASTER_SETTINGS_NP", MID_GRANDMASTER_SETTINGS_NP, do_set_action
},
{ "SUBSCRIBE_EVENTS_NP", MID_SUBSCRIBE_EVENTS_NP, do_set_action },
{ "SYNCHRONIZATION_UNCERTAIN_NP", MID_SYNCHRONIZATION_UNCERTAIN_NP,
do_set_action },
+ { "SERVO_STATE_NP", MID_SERVO_STATE_NP, do_get_action },
/* Port management ID values */
{ "NULL_MANAGEMENT", MID_NULL_MANAGEMENT, null_management },
{ "CLOCK_DESCRIPTION", MID_CLOCK_DESCRIPTION, do_get_action },
diff --git a/tlv.h b/tlv.h
index 915b9fc..fe7b1f8 100644
--- a/tlv.h
+++ b/tlv.h
@@ -100,6 +100,7 @@ enum management_action {
#define MID_GRANDMASTER_SETTINGS_NP 0xC001
#define MID_SUBSCRIBE_EVENTS_NP 0xC003
#define MID_SYNCHRONIZATION_UNCERTAIN_NP 0xC006
+#define MID_SERVO_STATE_NP 0xC009
/* Port management ID values */
#define MID_NULL_MANAGEMENT 0x0000
@@ -331,6 +332,10 @@ struct port_ds_np {
Integer32 asCapable;
} PACKED;
+struct servo_state_np {
+ uint8_t state;
+} PACKED;
+
#define EVENT_BITMASK_CNT 64
diff --git a/util.c b/util.c
index a59b559..f045543 100644
--- a/util.c
+++ b/util.c
@@ -207,6 +207,22 @@ const char *ustate2str(enum unicast_state ustate)
return "???";
}
+const char *servo_state2str(enum servo_state sstate)
+{
+ switch (sstate) {
+ case SERVO_UNLOCKED:
+ return "UNLOCKED";
+ case SERVO_JUMP:
+ return "JUMP";
+ case SERVO_LOCKED:
+ return "LOCKED";
+ case SERVO_LOCKED_STABLE:
+ return "LOCKED_STABLE";
+ }
+
+ return "???";
+}
+
void posix_clock_close(clockid_t clock)
{
if (clock == CLOCK_REALTIME) {
diff --git a/util.h b/util.h
index 558a675..6e7abe4 100644
--- a/util.h
+++ b/util.h
@@ -26,6 +26,7 @@
#include "address.h"
#include "ddt.h"
#include "ether.h"
+#include "servo.h"
#include "transport.h"
#include "unicast_fsm.h"
@@ -113,6 +114,13 @@ char *portaddr2str(struct PortAddress *addr);
const char *ustate2str(enum unicast_state ustate);
+/**
+ * Convert servo state to a human readable string.
+ * @param sstate The state to show.
+ * @return The string representing the state.
+ */
+const char *servo_state2str(enum servo_state sstate);
+
/**
* Closes a dynamic posix clock.
* @param clock A clock ID obtained via posix_clock_close().
--
2.17.1
_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
