Signed-off-by: Geoff Salmon <***@se-instruments.com>
---
clock.c | 14 +++++++
clock.h | 7 ++++
port.c | 60 ++++++++++++++++++++++++++++
tlv.c | 137 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
4 files changed, 218 insertions(+)
diff --git a/clock.c b/clock.c
index f5836fd..26f927b 100644
--- a/clock.c
+++ b/clock.c
@@ -169,6 +169,11 @@ static int clock_management_get_response(struct clock *c, struct port *p,
tlv->id = id;
switch (id) {
+ case USER_DESCRIPTION:
+ ptp_text_copy(&rsp->last_tlv.cd.userDescription, &c->desc.userDescription);
+ datalen = 1 + rsp->last_tlv.cd.userDescription.length;
+ respond = 1;
+ break;
case DEFAULT_DATA_SET:
memcpy(tlv->data, &c->dds, sizeof(c->dds));
datalen = sizeof(c->dds);
@@ -209,6 +214,10 @@ static int clock_management_get_response(struct clock *c, struct port *p,
break;
}
if (respond) {
+ if (datalen % 2) {
+ tlv->data[datalen] = 0;
+ datalen++;
+ }
tlv->length = sizeof(tlv->id) + datalen;
pdulen = rsp->header.messageLength + sizeof(*tlv) + datalen;
rsp->header.messageLength = pdulen;
@@ -1074,3 +1083,8 @@ struct clock_description *clock_description(struct clock *c)
{
return &c->desc;
}
+
+int clock_num_ports(struct clock *c)
+{
+ return c->nports;
+}
diff --git a/clock.h b/clock.h
index 581fa14..7b4bfb0 100644
--- a/clock.h
+++ b/clock.h
@@ -222,4 +222,11 @@ struct timePropertiesDS *clock_time_properties(struct clock *c);
*/
struct clock_description *clock_description(struct clock *c);
+/**
+ * Obtain the number of ports a clock has, excluding the UDS port.
+ * @param c The clock instance.
+ * @return The number of ports.
+ */
+int clock_num_ports(struct clock *c);
+
#endif
diff --git a/port.c b/port.c
index 1438808..ed8892e 100644
--- a/port.c
+++ b/port.c
@@ -438,6 +438,9 @@ static void port_management_send_error(struct port *p, struct port *ingress,
pr_err("port %hu: management error failed", portnum(p));
}
+static const Octet profile_id_drr[] = {0x00, 0x1B, 0x19, 0x00, 0x01, 0x00};
+static const Octet profile_id_p2p[] = {0x00, 0x1B, 0x19, 0x00, 0x02, 0x00};
+
static int port_management_get_response(struct port *target,
struct port *ingress, int id,
struct ptp_message *req)
@@ -447,6 +450,9 @@ static int port_management_get_response(struct port *target,
struct ptp_message *rsp;
struct portDS *pds;
struct PortIdentity pid = port_identity(target);
+ uint8_t protocol_addr[TRANSPORT_ADDR_LEN], physical_addr[TRANSPORT_ADDR_LEN];
+ struct clock_description *desc;
+ struct mgmt_clock_description *cd;
rsp = port_management_reply(pid, ingress, req);
if (!rsp) {
@@ -461,6 +467,56 @@ static int port_management_get_response(struct port *target,
datalen = 0;
respond = 1;
break;
+ case CLOCK_DESCRIPTION:
+ cd = &rsp->last_tlv.cd;
+ if (clock_num_ports(target->clock) > 1) {
+ cd->clockType = CLOCK_TYPE_BOUNDARY;
+ } else {
+ cd->clockType = CLOCK_TYPE_ORDINARY;
+ }
+
+ switch(transport_type(target->trp)) {
+ case TRANS_UDP_IPV4:
+ case TRANS_UDP_IPV6:
+ case TRANS_IEEE_802_3:
+ ptp_text_set(&cd->physicalLayerProtocol, "IEEE 802.3");
+ break;
+ default:
+ ptp_text_set(&cd->physicalLayerProtocol, NULL);
+ break;
+ }
+ cd->physicalAddress.length =
+ transport_physical_addr(target->trp, physical_addr);
+ cd->physicalAddress.address = physical_addr;
+
+ cd->protocolAddress.networkProtocol = transport_type(target->trp);
+ cd->protocolAddress.addressLength =
+ transport_protocol_addr(target->trp, protocol_addr);
+ cd->protocolAddress.address = protocol_addr;
+
+ desc = clock_description(target->clock);
+ memcpy(cd->manufacturerIdentity, desc->manufacturerIdentity, OUI_LEN);
+ ptp_text_copy(&cd->productDescription, &desc->productDescription);
+ ptp_text_copy(&cd->revisionData, &desc->revisionData);
+ ptp_text_copy(&cd->userDescription, &desc->userDescription);
+
+ if (target->delayMechanism == DM_P2P) {
+ memcpy(cd->profileIdentity, profile_id_p2p, sizeof(cd->profileIdentity));
+ } else {
+ memcpy(cd->profileIdentity, profile_id_drr, sizeof(cd->profileIdentity));
+ }
+
+ datalen = 2 + 1 + cd->physicalLayerProtocol.length
+ + 2 + cd->physicalAddress.length
+ + 4 + cd->protocolAddress.addressLength
+ + sizeof(cd->manufacturerIdentity)
+ + 1
+ + 1 + cd->productDescription.length
+ + 1 + cd->revisionData.length
+ + 1 + cd->userDescription.length
+ + sizeof(cd->profileIdentity);
+ respond = 1;
+ break;
case PORT_DATA_SET:
pds = (struct portDS *) tlv->data;
pds->portIdentity = target->portIdentity;
@@ -486,6 +542,10 @@ static int port_management_get_response(struct port *target,
break;
}
if (respond) {
+ if (datalen % 2) {
+ tlv->data[datalen] = 0;
+ datalen++;
+ }
tlv->length = sizeof(tlv->id) + datalen;
pdulen = rsp->header.messageLength + sizeof(*tlv) + datalen;
rsp->header.messageLength = pdulen;
diff --git a/tlv.c b/tlv.c
index 7d2c3a7..fafa50b 100644
--- a/tlv.c
+++ b/tlv.c
@@ -42,6 +42,91 @@ static void scaled_ns_h2n(ScaledNs *sns)
sns->fractional_nanoseconds = htons(sns->fractional_nanoseconds);
}
+static int pack_uint16(const UInteger16 *from, uint8_t *to)
+{
+ *(UInteger16 *)to = htons(*from);
+ return sizeof(UInteger16);
+}
+
+static int unpack_uint16(const uint8_t *from, UInteger16 *to)
+{
+ *to = htons(*(UInteger16 *) from);
+ return sizeof(UInteger16);
+}
+
+static int unpack_octet_ptr(const uint8_t *buf, Octet **bytes, int len)
+{
+ if (len)
+ *bytes = (Octet *)buf;
+ else
+ *bytes = 0;
+ return len;
+}
+
+static int pack_octet_ptr(Octet * const *bytes, uint8_t *buf, int len)
+{
+ /*
+ Use memmove instead of memcpy to support calling
+ msg_post_recv and then msg_pre_send on the same message,
+ which could cause parts of the message's TLVs (PTPTexts for
+ example) to be copied from and to the same message buffer.
+ The only place this happens currently, is when management
+ messages are forwarded.
+ */
+ memmove(buf, *bytes, len);
+ return len;
+}
+
+static int pack_port_address(const struct PortAddress *data, uint8_t *buf)
+{
+ int offset = 0;
+ offset += pack_uint16(&data->networkProtocol, buf + offset);
+ offset += pack_uint16(&data->addressLength, buf + offset);
+ offset += pack_octet_ptr(&data->address, buf + offset, data->addressLength);
+ return offset;
+}
+
+static int unpack_port_address(const uint8_t *buf, struct PortAddress *data)
+{
+ int offset = 0;
+ offset += unpack_uint16(buf + offset, &data->networkProtocol);
+ offset += unpack_uint16(buf + offset, &data->addressLength);
+ offset += unpack_octet_ptr(buf + offset, &data->address, data->addressLength);
+ return offset;
+}
+
+static int pack_ptp_text(const struct PTPText *data, uint8_t *buf)
+{
+ int offset = 0;
+ buf[offset++] = data->length;
+ offset += pack_octet_ptr(&data->text, buf + offset, data->length);
+ return offset;
+}
+
+static int unpack_ptp_text(const uint8_t *buf, struct PTPText *data)
+{
+ int offset = 0;
+ data->length = buf[offset++];
+ offset += unpack_octet_ptr(buf + offset, &data->text, data->length);
+ return offset;
+}
+
+static int pack_physical_address(const struct PhysicalAddress *data, uint8_t *buf)
+{
+ int offset = 0;
+ offset += pack_uint16(&data->length, buf + offset);
+ offset += pack_octet_ptr(&data->address, buf + offset, data->length);
+ return offset;
+}
+
+static int unpack_physical_address(const uint8_t *buf, struct PhysicalAddress *data)
+{
+ int offset = 0;
+ offset += unpack_uint16(buf + offset, &data->length);
+ offset += unpack_octet_ptr(buf + offset, &data->address, data->length);
+ return offset;
+}
+
static int mgt_post_recv(struct management_tlv *m, uint16_t data_len, struct tlv_extra *extra)
{
struct defaultDS *dds;
@@ -50,8 +135,36 @@ static int mgt_post_recv(struct management_tlv *m, uint16_t data_len, struct tlv
struct timePropertiesDS *tp;
struct portDS *p;
struct time_status_np *tsn;
+ struct mgmt_clock_description *cd;
int extra_len = 0;
+ uint8_t *buf;
switch (m->id) {
+ case CLOCK_DESCRIPTION:
+ cd = &extra->cd;
+ buf = m->data;
+ buf += unpack_uint16(buf, &cd->clockType);
+ buf += unpack_ptp_text(buf, &cd->physicalLayerProtocol);
+ unpack_uint16(buf, &cd->physicalAddress.length);
+ if (cd->physicalAddress.length > TRANSPORT_ADDR_LEN)
+ goto bad_length;
+ buf += unpack_physical_address(buf, &cd->physicalAddress);
+ unpack_uint16(buf + 2, &cd->protocolAddress.addressLength);
+ if (cd->protocolAddress.addressLength > TRANSPORT_ADDR_LEN)
+ goto bad_length;
+ buf += unpack_port_address(buf, &cd->protocolAddress);
+ memcpy(cd->manufacturerIdentity, buf, sizeof(cd->manufacturerIdentity));
+ buf += sizeof(cd->manufacturerIdentity);
+ buf += 1; /* reserved */
+ buf += unpack_ptp_text(buf, &cd->productDescription);
+ buf += unpack_ptp_text(buf, &cd->revisionData);
+ buf += unpack_ptp_text(buf, &cd->userDescription);
+ memcpy(cd->profileIdentity, buf, sizeof(cd->profileIdentity));
+ buf += sizeof(cd->profileIdentity);
+ extra_len = (buf - m->data);
+ break;
+ case USER_DESCRIPTION:
+ extra_len = unpack_ptp_text(m->data, &extra->cd.userDescription);
+ break;
case DEFAULT_DATA_SET:
if (data_len != sizeof(struct defaultDS))
goto bad_length;
@@ -126,7 +239,31 @@ static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra)
struct timePropertiesDS *tp;
struct portDS *p;
struct time_status_np *tsn;
+ struct mgmt_clock_description *cd;
+ uint8_t *buf;
switch (m->id) {
+ case CLOCK_DESCRIPTION:
+ if (extra) {
+ cd = &extra->cd;
+ buf = m->data;
+ buf += pack_uint16(&cd->clockType, buf);
+ buf += pack_ptp_text(&cd->physicalLayerProtocol, buf);
+ buf += pack_physical_address(&cd->physicalAddress, buf);
+ buf += pack_port_address(&cd->protocolAddress, buf);
+ memcpy(buf, cd->manufacturerIdentity, sizeof(cd->manufacturerIdentity));
+ buf += sizeof(cd->manufacturerIdentity);
+ *(buf++) = 0;
+ buf += pack_ptp_text(&cd->productDescription, buf);
+ buf += pack_ptp_text(&cd->revisionData, buf);
+ buf += pack_ptp_text(&cd->userDescription, buf);
+ memcpy(buf, cd->profileIdentity, sizeof(cd->profileIdentity));
+ buf += sizeof(cd->profileIdentity);
+ }
+ break;
+ case USER_DESCRIPTION:
+ if (extra)
+ pack_ptp_text(&extra->cd.userDescription, m->data);
+ break;
case DEFAULT_DATA_SET:
dds = (struct defaultDS *) m->data;
dds->numberPorts = htons(dds->numberPorts);
--
1.7.9.5