linuxptp/pmc_common.c

/**
 * @file pmc_common.c
 * @note Copyright (C) 2012 Richard Cochran <richardcochran@gmail.com>
 * @note Copyright (C) 2013 Miroslav Lichvar <mlichvar@redhat.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */
#include <errno.h>
#include <string.h>
#include <stdlib.h>

#include "print.h"
#include "tlv.h"
#include "transport.h"
#include "util.h"
#include "pmc_common.h"

/*
   Field                  Len  Type
  --------------------------------------------------------
   clockType                2
   physicalLayerProtocol    1  PTPText
   physicalAddressLength    2  UInteger16
   physicalAddress          0
   protocolAddress          4  Enumeration16 + UInteger16
   manufacturerIdentity     3
   reserved                 1
   productDescription       1  PTPText
   revisionData             1  PTPText
   userDescription          1  PTPText
   profileIdentity          6
  --------------------------------------------------------
   TOTAL                   22
*/
#define EMPTY_CLOCK_DESCRIPTION 22
/* Includes one extra byte to make length even. */
#define EMPTY_PTP_TEXT 2

struct pmc {
	UInteger16 sequence_id;
	UInteger8 boundary_hops;
	UInteger8 domain_number;
	UInteger8 transport_specific;
	struct PortIdentity port_identity;
	struct PortIdentity target;

	struct transport *transport;
	struct fdarray fdarray;
	int zero_length_gets;
};

struct pmc *pmc_create(struct config *cfg, enum transport_type transport_type,
		       const char *iface_name, UInteger8 boundary_hops,
		       UInteger8 domain_number, UInteger8 transport_specific,
		       int zero_datalen)
{
	struct interface iface;
	struct pmc *pmc;

	memset(&iface, 0, sizeof(iface));

	pmc = calloc(1, sizeof *pmc);
	if (!pmc)
		return NULL;

	if (transport_type != TRANS_UDS &&
	    generate_clock_identity(&pmc->port_identity.clockIdentity,
				    iface_name)) {
		pr_err("failed to generate a clock identity");
		goto failed;
	}
	pmc->port_identity.portNumber = 1;
	pmc_target_all(pmc);

	pmc->boundary_hops = boundary_hops;
	pmc->domain_number = domain_number;
	pmc->transport_specific = transport_specific;

	pmc->transport = transport_create(cfg, transport_type);
	if (!pmc->transport) {
		pr_err("failed to create transport");
		goto failed;
	}

	strncpy(iface.name, iface_name, MAX_IFNAME_SIZE);
	if (iface.ts_label[0] == '\0') {
		strncpy(iface.ts_label, iface.name, MAX_IFNAME_SIZE);
	}

	if (transport_open(pmc->transport, &iface,
			   &pmc->fdarray, TS_SOFTWARE)) {
		pr_err("failed to open transport");
		goto failed;
	}
	pmc->zero_length_gets = zero_datalen ? 1 : 0;

	return pmc;

failed:
	if (pmc->transport)
		transport_destroy(pmc->transport);
	free(pmc);
	return NULL;
}

void pmc_destroy(struct pmc *pmc)
{
	transport_close(pmc->transport, &pmc->fdarray);
	transport_destroy(pmc->transport);
	free(pmc);
}

static struct ptp_message *pmc_message(struct pmc *pmc, uint8_t action)
{
	struct ptp_message *msg;
	int pdulen;

	msg = msg_allocate();
	if (!msg)
		return NULL;

	pdulen = sizeof(struct management_msg);
	msg->hwts.type = TS_SOFTWARE;

	msg->header.tsmt               = MANAGEMENT | pmc->transport_specific;
	msg->header.ver                = PTP_VERSION;
	msg->header.messageLength      = pdulen;
	msg->header.domainNumber       = pmc->domain_number;
	msg->header.sourcePortIdentity = pmc->port_identity;
	msg->header.sequenceId         = pmc->sequence_id++;
	msg->header.control            = CTL_MANAGEMENT;
	msg->header.logMessageInterval = 0x7f;

	msg->management.targetPortIdentity = pmc->target;
	msg->management.startingBoundaryHops = pmc->boundary_hops;
	msg->management.boundaryHops = pmc->boundary_hops;
	msg->management.flags = action;

	return msg;
}

static int pmc_send(struct pmc *pmc, struct ptp_message *msg)
{
	int err;

	err = msg_pre_send(msg);
	if (err) {
		pr_err("msg_pre_send failed");
		return -1;
	}
	return transport_send(pmc->transport, &pmc->fdarray,
			      TRANS_GENERAL, msg);
}

static int pmc_tlv_datalen(struct pmc *pmc, int id)
{
	int len = 0;

	if (pmc->zero_length_gets)
		return len;

	switch (id) {
	case TLV_USER_DESCRIPTION:
		len += EMPTY_PTP_TEXT;
		break;
	case TLV_DEFAULT_DATA_SET:
		len += sizeof(struct defaultDS);
		break;
	case TLV_CURRENT_DATA_SET:
		len += sizeof(struct currentDS);
		break;
	case TLV_PARENT_DATA_SET:
		len += sizeof(struct parentDS);
		break;
	case TLV_TIME_PROPERTIES_DATA_SET:
		len += sizeof(struct timePropertiesDS);
		break;
	case TLV_PRIORITY1:
	case TLV_PRIORITY2:
	case TLV_DOMAIN:
	case TLV_SLAVE_ONLY:
	case TLV_CLOCK_ACCURACY:
	case TLV_TRACEABILITY_PROPERTIES:
	case TLV_TIMESCALE_PROPERTIES:
		len += sizeof(struct management_tlv_datum);
		break;
	case TLV_TIME_STATUS_NP:
		len += sizeof(struct time_status_np);
		break;
	case TLV_GRANDMASTER_SETTINGS_NP:
		len += sizeof(struct grandmaster_settings_np);
		break;
	case TLV_NULL_MANAGEMENT:
		break;
	case TLV_CLOCK_DESCRIPTION:
		len += EMPTY_CLOCK_DESCRIPTION;
		break;
	case TLV_PORT_DATA_SET:
		len += sizeof(struct portDS);
		break;
	case TLV_PORT_DATA_SET_NP:
		len += sizeof(struct port_ds_np);
		break;
	case TLV_LOG_ANNOUNCE_INTERVAL:
	case TLV_ANNOUNCE_RECEIPT_TIMEOUT:
	case TLV_LOG_SYNC_INTERVAL:
	case TLV_VERSION_NUMBER:
	case TLV_DELAY_MECHANISM:
	case TLV_LOG_MIN_PDELAY_REQ_INTERVAL:
		len += sizeof(struct management_tlv_datum);
		break;
	}
	return len;
}

int pmc_get_transport_fd(struct pmc *pmc)
{
	return pmc->fdarray.fd[FD_GENERAL];
}

int pmc_send_get_action(struct pmc *pmc, int id)
{
	int datalen, pdulen;
	struct ptp_message *msg;
	struct management_tlv *mgt;
	struct tlv_extra *extra;

	msg = pmc_message(pmc, GET);
	if (!msg) {
		return -1;
	}
	mgt = (struct management_tlv *) msg->management.suffix;
	mgt->type = TLV_MANAGEMENT;
	datalen = pmc_tlv_datalen(pmc, id);
	mgt->length = 2 + datalen;
	mgt->id = id;
	pdulen = msg->header.messageLength + sizeof(*mgt) + datalen;
	msg->header.messageLength = pdulen;

	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) {
		/*
		 * Make sure the tlv_extra pointers dereferenced in
		 * mgt_pre_send() do point to something.
		 */
		struct mgmt_clock_description *cd = &extra->cd;
		uint8_t *buf = mgt->data;
		cd->clockType = (UInteger16 *) buf;
		buf += sizeof(*cd->clockType);
		cd->physicalLayerProtocol = (struct PTPText *) buf;
		buf += sizeof(struct PTPText) + cd->physicalLayerProtocol->length;
		cd->physicalAddress = (struct PhysicalAddress *) buf;
		buf += sizeof(struct PhysicalAddress) + 0;
		cd->protocolAddress = (struct PortAddress *) buf;
	}

	pmc_send(pmc, msg);
	msg_put(msg);

	return 0;
}

int pmc_send_set_action(struct pmc *pmc, int id, void *data, int datasize)
{
	struct management_tlv *mgt;
	struct ptp_message *msg;
	struct tlv_extra *extra;

	msg = pmc_message(pmc, SET);
	if (!msg) {
		return -1;
	}
	extra = msg_tlv_append(msg, sizeof(*mgt) + datasize);
	if (!extra) {
		return -ENOMEM;
	}
	mgt = (struct management_tlv *) extra->tlv;
	mgt->type = TLV_MANAGEMENT;
	mgt->length = 2 + datasize;
	mgt->id = id;
	memcpy(mgt->data, data, datasize);
	pmc_send(pmc, msg);
	msg_put(msg);

	return 0;
}

struct ptp_message *pmc_recv(struct pmc *pmc)
{
	struct ptp_message *msg;
	int cnt, err;

	msg = msg_allocate();
	if (!msg) {
		pr_err("low memory");
		return NULL;
	}
	msg->hwts.type = TS_SOFTWARE;
	cnt = transport_recv(pmc->transport, pmc_get_transport_fd(pmc), msg);
	if (cnt <= 0) {
		pr_err("recv message failed");
		goto failed;
	}
	err = msg_post_recv(msg, cnt);
	if (err) {
		switch (err) {
		case -EBADMSG:
			pr_err("bad message");
			break;
		case -EPROTO:
			pr_debug("ignoring message");
			break;
		}
		goto failed;
	}
	if (msg_sots_missing(msg)) {
		pr_err("received %s without timestamp",
		       msg_type_string(msg_type(msg)));
		goto failed;
	}

	return msg;
failed:
	msg_put(msg);
	return NULL;
}

int pmc_target(struct pmc *pmc, struct PortIdentity *pid)
{
	pmc->target = *pid;
	return 0;
}

void pmc_target_port(struct pmc *pmc, UInteger16 portNumber)
{
	pmc->target.portNumber = portNumber;
}

void pmc_target_all(struct pmc *pmc)
{
	memset(&pmc->target, 0xff, sizeof(pmc->target));
}