diff --git a/clock.c b/clock.c index e4e4f30..87a2275 100644 --- a/clock.c +++ b/clock.c @@ -159,6 +159,7 @@ static int clock_management_get_response(struct clock *c, struct port *p, struct ptp_message *rsp; struct time_status_np *tsn; struct PortIdentity pid = port_identity(p); + struct PTPText *text; rsp = port_management_reply(pid, p, req); if (!rsp) { @@ -169,6 +170,13 @@ static int clock_management_get_response(struct clock *c, struct port *p, tlv->id = id; switch (id) { + case USER_DESCRIPTION: + text = (struct PTPText *) tlv->data; + text->length = c->desc.userDescription.length; + memcpy(text->text, c->desc.userDescription.text, text->length); + datalen = 1 + text->length; + respond = 1; + break; case DEFAULT_DATA_SET: memcpy(tlv->data, &c->dds, sizeof(c->dds)); datalen = sizeof(c->dds); @@ -209,6 +217,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; @@ -1082,3 +1094,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 9899687..1770706 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,10 @@ static int port_management_get_response(struct port *target, struct ptp_message *rsp; struct portDS *pds; struct PortIdentity pid = port_identity(target); + struct clock_description *desc; + struct mgmt_clock_description *cd; + uint8_t *buf; + uint16_t u16; rsp = port_management_reply(pid, ingress, req); if (!rsp) { @@ -461,6 +468,72 @@ static int port_management_get_response(struct port *target, datalen = 0; respond = 1; break; + case CLOCK_DESCRIPTION: + cd = &rsp->last_tlv.cd; + buf = tlv->data; + cd->clockType = (UInteger16 *) buf; + buf += sizeof(*cd->clockType); + if (clock_num_ports(target->clock) > 1) { + *cd->clockType = CLOCK_TYPE_BOUNDARY; + } else { + *cd->clockType = CLOCK_TYPE_ORDINARY; + } + + cd->physicalLayerProtocol = (struct PTPText *) buf; + 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; + } + buf += sizeof(struct PTPText) + cd->physicalLayerProtocol->length; + + cd->physicalAddress = (struct PhysicalAddress *) buf; + u16 = transport_physical_addr(target->trp, + cd->physicalAddress->address); + memcpy(&cd->physicalAddress->length, &u16, 2); + buf += sizeof(struct PhysicalAddress) + u16; + + cd->protocolAddress = (struct PortAddress *) buf; + u16 = transport_type(target->trp); + memcpy(&cd->protocolAddress->networkProtocol, &u16, 2); + u16 = transport_protocol_addr(target->trp, + cd->protocolAddress->address); + memcpy(&cd->protocolAddress->addressLength, &u16, 2); + buf += sizeof(struct PortAddress) + u16; + + desc = clock_description(target->clock); + cd->manufacturerIdentity = buf; + memcpy(cd->manufacturerIdentity, + desc->manufacturerIdentity, OUI_LEN); + buf += OUI_LEN; + *(buf++) = 0; /* reserved */ + + cd->productDescription = (struct PTPText *) buf; + ptp_text_copy(cd->productDescription, &desc->productDescription); + buf += sizeof(struct PTPText) + cd->productDescription->length; + + cd->revisionData = (struct PTPText *) buf; + ptp_text_copy(cd->revisionData, &desc->revisionData); + buf += sizeof(struct PTPText) + cd->revisionData->length; + + cd->userDescription = (struct PTPText *) buf; + ptp_text_copy(cd->userDescription, &desc->userDescription); + buf += sizeof(struct PTPText) + cd->userDescription->length; + + if (target->delayMechanism == DM_P2P) { + memcpy(buf, profile_id_p2p, PROFILE_ID_LEN); + } else { + memcpy(buf, profile_id_drr, PROFILE_ID_LEN); + } + buf += PROFILE_ID_LEN; + datalen = buf - tlv->data; + respond = 1; + break; case PORT_DATA_SET: pds = (struct portDS *) tlv->data; pds->portIdentity = target->portIdentity; @@ -486,6 +559,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 5471d4b..9b9e420 100644 --- a/tlv.c +++ b/tlv.c @@ -42,6 +42,14 @@ static void scaled_ns_h2n(ScaledNs *sns) sns->fractional_nanoseconds = htons(sns->fractional_nanoseconds); } +static uint16_t flip16(uint16_t *p) { + uint16_t v; + memcpy(&v, p, sizeof(v)); + v = htons(v); + memcpy(p, &v, sizeof(v)); + return v; +} + static int mgt_post_recv(struct management_tlv *m, uint16_t data_len, struct tlv_extra *extra) { @@ -51,8 +59,55 @@ static int mgt_post_recv(struct management_tlv *m, uint16_t data_len, struct timePropertiesDS *tp; struct portDS *p; struct time_status_np *tsn; + struct mgmt_clock_description *cd; int extra_len = 0; + uint8_t *buf; + uint16_t u16; switch (m->id) { + case CLOCK_DESCRIPTION: + cd = &extra->cd; + buf = m->data; + + cd->clockType = (UInteger16 *) buf; + flip16(cd->clockType); + buf += sizeof(*cd->clockType); + + cd->physicalLayerProtocol = (struct PTPText *) buf; + buf += sizeof(struct PTPText); + buf += cd->physicalLayerProtocol->length; + + cd->physicalAddress = (struct PhysicalAddress *) buf; + u16 = flip16(&cd->physicalAddress->length); + if (u16 > TRANSPORT_ADDR_LEN) + goto bad_length; + buf += sizeof(struct PhysicalAddress) + u16; + + cd->protocolAddress = (struct PortAddress *) buf; + flip16(&cd->protocolAddress->networkProtocol); + u16 = flip16(&cd->protocolAddress->addressLength); + if (u16 > TRANSPORT_ADDR_LEN) + goto bad_length; + buf += sizeof(struct PortAddress) + u16; + + cd->manufacturerIdentity = buf; + buf += OUI_LEN + 1; + + cd->productDescription = (struct PTPText *) buf; + buf += sizeof(struct PTPText) + cd->productDescription->length; + cd->revisionData = (struct PTPText *) buf; + buf += sizeof(struct PTPText) + cd->revisionData->length; + cd->userDescription = (struct PTPText *) buf; + buf += sizeof(struct PTPText) + cd->userDescription->length; + + cd->profileIdentity = buf; + buf += PROFILE_ID_LEN; + extra_len = buf - m->data; + break; + case USER_DESCRIPTION: + extra->cd.userDescription = (struct PTPText *) m->data; + extra_len = sizeof(struct PTPText); + extra_len += extra->cd.userDescription->length; + break; case DEFAULT_DATA_SET: if (data_len != sizeof(struct defaultDS)) goto bad_length; @@ -127,7 +182,17 @@ 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; switch (m->id) { + case CLOCK_DESCRIPTION: + if (extra) { + cd = &extra->cd; + flip16(cd->clockType); + flip16(&cd->physicalAddress->length); + flip16(&cd->protocolAddress->networkProtocol); + flip16(&cd->protocolAddress->addressLength); + } + break; case DEFAULT_DATA_SET: dds = (struct defaultDS *) m->data; dds->numberPorts = htons(dds->numberPorts);