msg: Populate the TLV list on receive.
This patch changes the receive message parsing code to place each TLV into the list. A method is introduced that allows attaching TLVs to the end of the list. In addition, msg.last_tlv is converted into a pointer to the last item in the list. Because of this change, the transmit code that uses this field now allocates a TLV before using it. Signed-off-by: Richard Cochran <richardcochran@gmail.com>master
parent
22b894b687
commit
7fe69e7ba0
10
clock.c
10
clock.c
|
@ -340,9 +340,17 @@ static int clock_management_fill_response(struct clock *c, struct port *p,
|
||||||
struct subscribe_events_np *sen;
|
struct subscribe_events_np *sen;
|
||||||
struct management_tlv *tlv;
|
struct management_tlv *tlv;
|
||||||
struct time_status_np *tsn;
|
struct time_status_np *tsn;
|
||||||
|
struct tlv_extra *extra;
|
||||||
struct PTPText *text;
|
struct PTPText *text;
|
||||||
int datalen = 0;
|
int datalen = 0;
|
||||||
|
|
||||||
|
extra = tlv_extra_alloc();
|
||||||
|
if (!extra) {
|
||||||
|
pr_err("failed to allocate TLV descriptor");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
extra->tlv = (struct TLV *) rsp->management.suffix;
|
||||||
|
|
||||||
tlv = (struct management_tlv *) rsp->management.suffix;
|
tlv = (struct management_tlv *) rsp->management.suffix;
|
||||||
tlv->type = TLV_MANAGEMENT;
|
tlv->type = TLV_MANAGEMENT;
|
||||||
tlv->id = id;
|
tlv->id = id;
|
||||||
|
@ -448,7 +456,7 @@ static int clock_management_fill_response(struct clock *c, struct port *p,
|
||||||
}
|
}
|
||||||
tlv->length = sizeof(tlv->id) + datalen;
|
tlv->length = sizeof(tlv->id) + datalen;
|
||||||
rsp->header.messageLength += sizeof(*tlv) + datalen;
|
rsp->header.messageLength += sizeof(*tlv) + datalen;
|
||||||
rsp->tlv_count = 1;
|
msg_tlv_attach(rsp, extra);
|
||||||
|
|
||||||
/* The caller can respond to this message. */
|
/* The caller can respond to this message. */
|
||||||
return 1;
|
return 1;
|
||||||
|
|
53
msg.c
53
msg.c
|
@ -120,33 +120,43 @@ static void port_id_pre_send(struct PortIdentity *pid)
|
||||||
pid->portNumber = htons(pid->portNumber);
|
pid->portNumber = htons(pid->portNumber);
|
||||||
}
|
}
|
||||||
|
|
||||||
static int suffix_post_recv(uint8_t *ptr, int len, struct tlv_extra *last)
|
static int suffix_post_recv(struct ptp_message *msg, uint8_t *ptr, int len)
|
||||||
{
|
{
|
||||||
int cnt, err;
|
struct tlv_extra *extra;
|
||||||
struct TLV *tlv;
|
int err;
|
||||||
|
|
||||||
if (!ptr)
|
if (!ptr)
|
||||||
return 0;
|
return 0;
|
||||||
|
|
||||||
for (cnt = 0; len > sizeof(struct TLV); cnt++) {
|
while (len > sizeof(struct TLV)) {
|
||||||
tlv = (struct TLV *) ptr;
|
extra = tlv_extra_alloc();
|
||||||
tlv->type = ntohs(tlv->type);
|
if (!extra) {
|
||||||
tlv->length = ntohs(tlv->length);
|
pr_err("failed to allocate TLV descriptor");
|
||||||
if (tlv->length % 2) {
|
return -ENOMEM;
|
||||||
|
}
|
||||||
|
extra->tlv = (struct TLV *) ptr;
|
||||||
|
extra->tlv->type = ntohs(extra->tlv->type);
|
||||||
|
extra->tlv->length = ntohs(extra->tlv->length);
|
||||||
|
if (extra->tlv->length % 2) {
|
||||||
|
tlv_extra_recycle(extra);
|
||||||
return -EBADMSG;
|
return -EBADMSG;
|
||||||
}
|
}
|
||||||
len -= sizeof(struct TLV);
|
len -= sizeof(struct TLV);
|
||||||
ptr += sizeof(struct TLV);
|
ptr += sizeof(struct TLV);
|
||||||
if (tlv->length > len) {
|
if (extra->tlv->length > len) {
|
||||||
|
tlv_extra_recycle(extra);
|
||||||
return -EBADMSG;
|
return -EBADMSG;
|
||||||
}
|
}
|
||||||
len -= tlv->length;
|
len -= extra->tlv->length;
|
||||||
ptr += tlv->length;
|
ptr += extra->tlv->length;
|
||||||
err = tlv_post_recv(tlv, len > sizeof(struct TLV) ? NULL : last);
|
err = tlv_post_recv(extra);
|
||||||
if (err)
|
if (err) {
|
||||||
|
tlv_extra_recycle(extra);
|
||||||
return err;
|
return err;
|
||||||
|
}
|
||||||
|
msg_tlv_attach(msg, extra);
|
||||||
}
|
}
|
||||||
return cnt;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void suffix_pre_send(uint8_t *ptr, int cnt, struct tlv_extra *last)
|
static void suffix_pre_send(uint8_t *ptr, int cnt, struct tlv_extra *last)
|
||||||
|
@ -324,9 +334,9 @@ int msg_post_recv(struct ptp_message *m, int cnt)
|
||||||
if (msg_sots_missing(m))
|
if (msg_sots_missing(m))
|
||||||
return -ETIME;
|
return -ETIME;
|
||||||
|
|
||||||
m->tlv_count = suffix_post_recv(suffix, cnt - pdulen, &m->last_tlv);
|
err = suffix_post_recv(m, suffix, cnt - pdulen);
|
||||||
if (m->tlv_count < 0)
|
if (err)
|
||||||
return m->tlv_count;
|
return err;
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -381,10 +391,17 @@ int msg_pre_send(struct ptp_message *m)
|
||||||
default:
|
default:
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
suffix_pre_send(suffix, m->tlv_count, &m->last_tlv);
|
suffix_pre_send(suffix, m->tlv_count, m->last_tlv);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void msg_tlv_attach(struct ptp_message *msg, struct tlv_extra *extra)
|
||||||
|
{
|
||||||
|
TAILQ_INSERT_TAIL(&msg->tlv_list, extra, list);
|
||||||
|
msg->tlv_count++;
|
||||||
|
msg->last_tlv = extra;
|
||||||
|
}
|
||||||
|
|
||||||
const char *msg_type_string(int type)
|
const char *msg_type_string(int type)
|
||||||
{
|
{
|
||||||
switch (type) {
|
switch (type) {
|
||||||
|
|
10
msg.h
10
msg.h
|
@ -233,7 +233,7 @@ struct ptp_message {
|
||||||
* the layout of the TLV makes it difficult to access the data
|
* the layout of the TLV makes it difficult to access the data
|
||||||
* directly from the message's buffer.
|
* directly from the message's buffer.
|
||||||
*/
|
*/
|
||||||
struct tlv_extra last_tlv;
|
struct tlv_extra *last_tlv;
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -258,6 +258,14 @@ static inline Boolean field_is_set(struct ptp_message *m, int index, Octet bit)
|
||||||
return m->header.flagField[index] & bit ? TRUE : FALSE;
|
return m->header.flagField[index] & bit ? TRUE : FALSE;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Place a TLV descriptor into a message's list of TLVs.
|
||||||
|
*
|
||||||
|
* @param msg A message obtained using msg_allocate().
|
||||||
|
* @param extra The TLV to be added to the list.
|
||||||
|
*/
|
||||||
|
void msg_tlv_attach(struct ptp_message *msg, struct tlv_extra *extra);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Obtain the transportSpecific field from a message.
|
* Obtain the transportSpecific field from a message.
|
||||||
* @param m Message to test.
|
* @param m Message to test.
|
||||||
|
|
7
pmc.c
7
pmc.c
|
@ -194,8 +194,10 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
|
||||||
struct time_status_np *tsn;
|
struct time_status_np *tsn;
|
||||||
struct grandmaster_settings_np *gsn;
|
struct grandmaster_settings_np *gsn;
|
||||||
struct mgmt_clock_description *cd;
|
struct mgmt_clock_description *cd;
|
||||||
|
struct tlv_extra *extra;
|
||||||
struct portDS *p;
|
struct portDS *p;
|
||||||
struct port_ds_np *pnp;
|
struct port_ds_np *pnp;
|
||||||
|
|
||||||
if (msg_type(msg) != MANAGEMENT) {
|
if (msg_type(msg) != MANAGEMENT) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -209,6 +211,7 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
|
||||||
if (msg->tlv_count != 1) {
|
if (msg->tlv_count != 1) {
|
||||||
goto out;
|
goto out;
|
||||||
}
|
}
|
||||||
|
extra = TAILQ_FIRST(&msg->tlv_list);
|
||||||
tlv = (struct TLV *) msg->management.suffix;
|
tlv = (struct TLV *) msg->management.suffix;
|
||||||
if (tlv->type == TLV_MANAGEMENT) {
|
if (tlv->type == TLV_MANAGEMENT) {
|
||||||
fprintf(fp, "MANAGEMENT ");
|
fprintf(fp, "MANAGEMENT ");
|
||||||
|
@ -226,7 +229,7 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
|
||||||
}
|
}
|
||||||
switch (mgt->id) {
|
switch (mgt->id) {
|
||||||
case TLV_CLOCK_DESCRIPTION:
|
case TLV_CLOCK_DESCRIPTION:
|
||||||
cd = &msg->last_tlv.cd;
|
cd = &extra->cd;
|
||||||
fprintf(fp, "CLOCK_DESCRIPTION "
|
fprintf(fp, "CLOCK_DESCRIPTION "
|
||||||
IFMT "clockType 0x%hx"
|
IFMT "clockType 0x%hx"
|
||||||
IFMT "physicalLayerProtocol %s"
|
IFMT "physicalLayerProtocol %s"
|
||||||
|
@ -252,7 +255,7 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
|
||||||
case TLV_USER_DESCRIPTION:
|
case TLV_USER_DESCRIPTION:
|
||||||
fprintf(fp, "USER_DESCRIPTION "
|
fprintf(fp, "USER_DESCRIPTION "
|
||||||
IFMT "userDescription %s",
|
IFMT "userDescription %s",
|
||||||
text2str(msg->last_tlv.cd.userDescription));
|
text2str(extra->cd.userDescription));
|
||||||
break;
|
break;
|
||||||
case TLV_DEFAULT_DATA_SET:
|
case TLV_DEFAULT_DATA_SET:
|
||||||
dds = (struct defaultDS *) mgt->data;
|
dds = (struct defaultDS *) mgt->data;
|
||||||
|
|
13
pmc_common.c
13
pmc_common.c
|
@ -228,6 +228,8 @@ int pmc_send_get_action(struct pmc *pmc, int id)
|
||||||
int datalen, pdulen;
|
int datalen, pdulen;
|
||||||
struct ptp_message *msg;
|
struct ptp_message *msg;
|
||||||
struct management_tlv *mgt;
|
struct management_tlv *mgt;
|
||||||
|
struct tlv_extra *extra;
|
||||||
|
|
||||||
msg = pmc_message(pmc, GET);
|
msg = pmc_message(pmc, GET);
|
||||||
if (!msg) {
|
if (!msg) {
|
||||||
return -1;
|
return -1;
|
||||||
|
@ -239,14 +241,21 @@ int pmc_send_get_action(struct pmc *pmc, int id)
|
||||||
mgt->id = id;
|
mgt->id = id;
|
||||||
pdulen = msg->header.messageLength + sizeof(*mgt) + datalen;
|
pdulen = msg->header.messageLength + sizeof(*mgt) + datalen;
|
||||||
msg->header.messageLength = pdulen;
|
msg->header.messageLength = pdulen;
|
||||||
msg->tlv_count = 1;
|
|
||||||
|
extra = tlv_extra_alloc();
|
||||||
|
if (!extra) {
|
||||||
|
pr_err("failed to allocate TLV descriptor");
|
||||||
|
return -ENOMEM;
|
||||||
|
}
|
||||||
|
extra->tlv = (struct TLV *) msg->management.suffix;
|
||||||
|
msg_tlv_attach(msg, extra);
|
||||||
|
|
||||||
if (id == TLV_CLOCK_DESCRIPTION && !pmc->zero_length_gets) {
|
if (id == TLV_CLOCK_DESCRIPTION && !pmc->zero_length_gets) {
|
||||||
/*
|
/*
|
||||||
* Make sure the tlv_extra pointers dereferenced in
|
* Make sure the tlv_extra pointers dereferenced in
|
||||||
* mgt_pre_send() do point to something.
|
* mgt_pre_send() do point to something.
|
||||||
*/
|
*/
|
||||||
struct mgmt_clock_description *cd = &msg->last_tlv.cd;
|
struct mgmt_clock_description *cd = &extra->cd;
|
||||||
uint8_t *buf = mgt->data;
|
uint8_t *buf = mgt->data;
|
||||||
cd->clockType = (UInteger16 *) buf;
|
cd->clockType = (UInteger16 *) buf;
|
||||||
buf += sizeof(*cd->clockType);
|
buf += sizeof(*cd->clockType);
|
||||||
|
|
12
port.c
12
port.c
|
@ -711,11 +711,19 @@ static int port_management_fill_response(struct port *target,
|
||||||
struct port_properties_np *ppn;
|
struct port_properties_np *ppn;
|
||||||
struct management_tlv *tlv;
|
struct management_tlv *tlv;
|
||||||
struct port_ds_np *pdsnp;
|
struct port_ds_np *pdsnp;
|
||||||
|
struct tlv_extra *extra;
|
||||||
struct portDS *pds;
|
struct portDS *pds;
|
||||||
uint16_t u16;
|
uint16_t u16;
|
||||||
uint8_t *buf;
|
uint8_t *buf;
|
||||||
int datalen;
|
int datalen;
|
||||||
|
|
||||||
|
extra = tlv_extra_alloc();
|
||||||
|
if (!extra) {
|
||||||
|
pr_err("failed to allocate TLV descriptor");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
extra->tlv = (struct TLV *) rsp->management.suffix;
|
||||||
|
|
||||||
tlv = (struct management_tlv *) rsp->management.suffix;
|
tlv = (struct management_tlv *) rsp->management.suffix;
|
||||||
tlv->type = TLV_MANAGEMENT;
|
tlv->type = TLV_MANAGEMENT;
|
||||||
tlv->id = id;
|
tlv->id = id;
|
||||||
|
@ -725,7 +733,7 @@ static int port_management_fill_response(struct port *target,
|
||||||
datalen = 0;
|
datalen = 0;
|
||||||
break;
|
break;
|
||||||
case TLV_CLOCK_DESCRIPTION:
|
case TLV_CLOCK_DESCRIPTION:
|
||||||
cd = &rsp->last_tlv.cd;
|
cd = &extra->cd;
|
||||||
buf = tlv->data;
|
buf = tlv->data;
|
||||||
cd->clockType = (UInteger16 *) buf;
|
cd->clockType = (UInteger16 *) buf;
|
||||||
buf += sizeof(*cd->clockType);
|
buf += sizeof(*cd->clockType);
|
||||||
|
@ -867,7 +875,7 @@ static int port_management_fill_response(struct port *target,
|
||||||
}
|
}
|
||||||
tlv->length = sizeof(tlv->id) + datalen;
|
tlv->length = sizeof(tlv->id) + datalen;
|
||||||
rsp->header.messageLength += sizeof(*tlv) + datalen;
|
rsp->header.messageLength += sizeof(*tlv) + datalen;
|
||||||
rsp->tlv_count = 1;
|
msg_tlv_attach(rsp, extra);
|
||||||
|
|
||||||
/* The caller can respond to this message. */
|
/* The caller can respond to this message. */
|
||||||
return 1;
|
return 1;
|
||||||
|
|
6
tlv.c
6
tlv.c
|
@ -444,15 +444,13 @@ void tlv_extra_recycle(struct tlv_extra *extra)
|
||||||
TAILQ_INSERT_HEAD(&tlv_pool, extra, list);
|
TAILQ_INSERT_HEAD(&tlv_pool, extra, list);
|
||||||
}
|
}
|
||||||
|
|
||||||
int tlv_post_recv(struct TLV *tlv, struct tlv_extra *extra)
|
int tlv_post_recv(struct tlv_extra *extra)
|
||||||
{
|
{
|
||||||
int result = 0;
|
int result = 0;
|
||||||
struct management_tlv *mgt;
|
struct management_tlv *mgt;
|
||||||
struct management_error_status *mes;
|
struct management_error_status *mes;
|
||||||
|
struct TLV *tlv = extra->tlv;
|
||||||
struct path_trace_tlv *ptt;
|
struct path_trace_tlv *ptt;
|
||||||
struct tlv_extra dummy_extra;
|
|
||||||
if (!extra)
|
|
||||||
extra = &dummy_extra;
|
|
||||||
|
|
||||||
switch (tlv->type) {
|
switch (tlv->type) {
|
||||||
case TLV_MANAGEMENT:
|
case TLV_MANAGEMENT:
|
||||||
|
|
6
tlv.h
6
tlv.h
|
@ -256,12 +256,10 @@ void tlv_extra_recycle(struct tlv_extra *extra);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Converts recognized value sub-fields into host byte order.
|
* Converts recognized value sub-fields into host byte order.
|
||||||
* @param tlv Pointer to a Type Length Value field.
|
* @param extra TLV descriptor pointing to the protocol data.
|
||||||
* @param extra Additional struct where data from tlv will be saved,
|
|
||||||
* can be NULL.
|
|
||||||
* @return Zero if successful, otherwise non-zero
|
* @return Zero if successful, otherwise non-zero
|
||||||
*/
|
*/
|
||||||
int tlv_post_recv(struct TLV *tlv, struct tlv_extra *extra);
|
int tlv_post_recv(struct tlv_extra *extra);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Converts recognized value sub-fields into network byte order.
|
* Converts recognized value sub-fields into network byte order.
|
||||||
|
|
Loading…
Reference in New Issue