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 ptp_message *rsp;
|
||||||
struct time_status_np *tsn;
|
struct time_status_np *tsn;
|
||||||
struct PortIdentity pid = port_identity(p);
|
struct PortIdentity pid = port_identity(p);
|
||||||
|
struct PTPText *text;
|
||||||
|
|
||||||
rsp = port_management_reply(pid, p, req);
|
rsp = port_management_reply(pid, p, req);
|
||||||
if (!rsp) {
|
if (!rsp) {
|
||||||
|
@ -169,6 +170,13 @@ static int clock_management_get_response(struct clock *c, struct port *p,
|
||||||
tlv->id = id;
|
tlv->id = id;
|
||||||
|
|
||||||
switch (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:
|
case DEFAULT_DATA_SET:
|
||||||
memcpy(tlv->data, &c->dds, sizeof(c->dds));
|
memcpy(tlv->data, &c->dds, sizeof(c->dds));
|
||||||
datalen = sizeof(c->dds);
|
datalen = sizeof(c->dds);
|
||||||
|
@ -209,6 +217,10 @@ static int clock_management_get_response(struct clock *c, struct port *p,
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if (respond) {
|
if (respond) {
|
||||||
|
if (datalen % 2) {
|
||||||
|
tlv->data[datalen] = 0;
|
||||||
|
datalen++;
|
||||||
|
}
|
||||||
tlv->length = sizeof(tlv->id) + datalen;
|
tlv->length = sizeof(tlv->id) + datalen;
|
||||||
pdulen = rsp->header.messageLength + sizeof(*tlv) + datalen;
|
pdulen = rsp->header.messageLength + sizeof(*tlv) + datalen;
|
||||||
rsp->header.messageLength = pdulen;
|
rsp->header.messageLength = pdulen;
|
||||||
|
@ -1082,3 +1094,8 @@ struct clock_description *clock_description(struct clock *c)
|
||||||
{
|
{
|
||||||
return &c->desc;
|
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);
|
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
|
#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));
|
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,
|
static int port_management_get_response(struct port *target,
|
||||||
struct port *ingress, int id,
|
struct port *ingress, int id,
|
||||||
struct ptp_message *req)
|
struct ptp_message *req)
|
||||||
|
@ -447,6 +450,10 @@ static int port_management_get_response(struct port *target,
|
||||||
struct ptp_message *rsp;
|
struct ptp_message *rsp;
|
||||||
struct portDS *pds;
|
struct portDS *pds;
|
||||||
struct PortIdentity pid = port_identity(target);
|
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);
|
rsp = port_management_reply(pid, ingress, req);
|
||||||
if (!rsp) {
|
if (!rsp) {
|
||||||
|
@ -461,6 +468,72 @@ static int port_management_get_response(struct port *target,
|
||||||
datalen = 0;
|
datalen = 0;
|
||||||
respond = 1;
|
respond = 1;
|
||||||
break;
|
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:
|
case PORT_DATA_SET:
|
||||||
pds = (struct portDS *) tlv->data;
|
pds = (struct portDS *) tlv->data;
|
||||||
pds->portIdentity = target->portIdentity;
|
pds->portIdentity = target->portIdentity;
|
||||||
|
@ -486,6 +559,10 @@ static int port_management_get_response(struct port *target,
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if (respond) {
|
if (respond) {
|
||||||
|
if (datalen % 2) {
|
||||||
|
tlv->data[datalen] = 0;
|
||||||
|
datalen++;
|
||||||
|
}
|
||||||
tlv->length = sizeof(tlv->id) + datalen;
|
tlv->length = sizeof(tlv->id) + datalen;
|
||||||
pdulen = rsp->header.messageLength + sizeof(*tlv) + datalen;
|
pdulen = rsp->header.messageLength + sizeof(*tlv) + datalen;
|
||||||
rsp->header.messageLength = pdulen;
|
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);
|
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,
|
static int mgt_post_recv(struct management_tlv *m, uint16_t data_len,
|
||||||
struct tlv_extra *extra)
|
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 timePropertiesDS *tp;
|
||||||
struct portDS *p;
|
struct portDS *p;
|
||||||
struct time_status_np *tsn;
|
struct time_status_np *tsn;
|
||||||
|
struct mgmt_clock_description *cd;
|
||||||
int extra_len = 0;
|
int extra_len = 0;
|
||||||
|
uint8_t *buf;
|
||||||
|
uint16_t u16;
|
||||||
switch (m->id) {
|
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:
|
case DEFAULT_DATA_SET:
|
||||||
if (data_len != sizeof(struct defaultDS))
|
if (data_len != sizeof(struct defaultDS))
|
||||||
goto bad_length;
|
goto bad_length;
|
||||||
|
@ -127,7 +182,17 @@ static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra)
|
||||||
struct timePropertiesDS *tp;
|
struct timePropertiesDS *tp;
|
||||||
struct portDS *p;
|
struct portDS *p;
|
||||||
struct time_status_np *tsn;
|
struct time_status_np *tsn;
|
||||||
|
struct mgmt_clock_description *cd;
|
||||||
switch (m->id) {
|
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:
|
case DEFAULT_DATA_SET:
|
||||||
dds = (struct defaultDS *) m->data;
|
dds = (struct defaultDS *) m->data;
|
||||||
dds->numberPorts = htons(dds->numberPorts);
|
dds->numberPorts = htons(dds->numberPorts);
|
||||||
|
|
Loading…
Reference in New Issue