support GET CLOCK_DESCRIPTION and USER_DESCRIPTION mgmt messages
Signed-off-by: Geoff Salmon <gsalmon@se-instruments.com>master
parent
e919971905
commit
36a5921e88
17
clock.c
17
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;
|
||||
}
|
||||
|
|
7
clock.h
7
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
|
||||
|
|
77
port.c
77
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;
|
||||
|
|
65
tlv.c
65
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);
|
||||
|
|
Loading…
Reference in New Issue