gstreamer/subprojects/gst-plugins-bad/ext/avtp/gstavtpcrfbase.c

585 lines
17 KiB
C

/*
* GStreamer AVTP Plugin
* Copyright (C) 2019 Intel Corporation
*
* This library is free software; you can redistribute it and/or modify it
* under the terms of the GNU Library General Public License as published by
* the Free Software Foundation; either version 2 of the License, or (at your
* option) any later version.
*
* This library 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 Library General Public License for more details. You should
* have received a copy of the GNU Library General Public License along with
* this library; if not , write to the Free Software Foundation, Inc., 51
* Franklin St, Fifth Floor, Boston, MA 02110 - 1301, USA.
*/
#include <arpa/inet.h>
#include <avtp.h>
#include <avtp_crf.h>
#include <glib.h>
#include <math.h>
#include <net/ethernet.h>
#include <net/if.h>
#include <stdio.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#include "gstavtpcrfutil.h"
#include "gstavtpcrfbase.h"
GST_DEBUG_CATEGORY_STATIC (avtpcrfbase_debug);
#define GST_CAT_DEFAULT (avtpcrfbase_debug)
#define CRF_TIMESTAMP_SIZE 8
#define MAX_AVTPDU_SIZE 1500
#define MAX_NUM_PERIODS_STORED 10
#define RECV_TIMEOUT 1 // in seconds
#define DEFAULT_STREAMID 0xAABBCCDDEEFF1000
#define DEFAULT_IFNAME "eth0"
#define DEFAULT_ADDRESS "01:AA:AA:AA:AA:AA"
enum
{
PROP_0,
PROP_STREAMID,
PROP_IFNAME,
PROP_ADDRESS,
};
static GstStaticPadTemplate src_template = GST_STATIC_PAD_TEMPLATE ("src",
GST_PAD_SRC,
GST_PAD_ALWAYS,
GST_STATIC_CAPS ("application/x-avtp")
);
static GstStaticPadTemplate sink_template = GST_STATIC_PAD_TEMPLATE ("sink",
GST_PAD_SINK,
GST_PAD_ALWAYS,
GST_STATIC_CAPS ("application/x-avtp")
);
static void gst_avtp_crf_base_finalize (GObject * gobject);
static void
gst_avtp_crf_base_set_property (GObject * object, guint prop_id,
const GValue * value, GParamSpec * pspec);
static void
gst_avtp_crf_base_get_property (GObject * object, guint prop_id,
GValue * value, GParamSpec * pspec);
static GstStateChangeReturn gst_avtp_crf_base_change_state (GstElement *
element, GstStateChange transition);
static void crf_listener_thread_func (GstAvtpCrfBase * avtpcrfbase);
#define gst_avtp_crf_base_parent_class parent_class
G_DEFINE_TYPE (GstAvtpCrfBase, gst_avtp_crf_base, GST_TYPE_BASE_TRANSFORM);
static void
gst_avtp_crf_base_class_init (GstAvtpCrfBaseClass * klass)
{
GstElementClass *element_class = GST_ELEMENT_CLASS (klass);
GObjectClass *object_class = G_OBJECT_CLASS (klass);
object_class->finalize = GST_DEBUG_FUNCPTR (gst_avtp_crf_base_finalize);
object_class->get_property =
GST_DEBUG_FUNCPTR (gst_avtp_crf_base_get_property);
object_class->set_property =
GST_DEBUG_FUNCPTR (gst_avtp_crf_base_set_property);
g_object_class_install_property (object_class, PROP_STREAMID,
g_param_spec_uint64 ("streamid", "Stream ID",
"Stream ID associated with the CRF AVTPDU", 0, G_MAXUINT64,
DEFAULT_STREAMID, G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS |
GST_PARAM_MUTABLE_READY));
g_object_class_install_property (object_class, PROP_IFNAME,
g_param_spec_string ("ifname", "Interface Name",
"Network interface utilized to receive CRF AVTPDUs",
DEFAULT_IFNAME, G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS |
GST_PARAM_MUTABLE_READY));
g_object_class_install_property (object_class, PROP_ADDRESS,
g_param_spec_string ("address", "Destination MAC address",
"Destination MAC address expected on the Ethernet frames",
DEFAULT_ADDRESS, G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS |
GST_PARAM_MUTABLE_READY));
element_class->change_state =
GST_DEBUG_FUNCPTR (gst_avtp_crf_base_change_state);
gst_element_class_add_static_pad_template (element_class, &sink_template);
gst_element_class_add_static_pad_template (element_class, &src_template);
GST_DEBUG_CATEGORY_INIT (avtpcrfbase_debug, "avtpcrfbase", 0, "CRF Base");
gst_type_mark_as_plugin_api (GST_TYPE_AVTP_CRF_BASE, 0);
}
static void
gst_avtp_crf_base_init (GstAvtpCrfBase * avtpcrfbase)
{
avtpcrfbase->streamid = DEFAULT_STREAMID;
avtpcrfbase->ifname = g_strdup (DEFAULT_IFNAME);
avtpcrfbase->address = g_strdup (DEFAULT_ADDRESS);
}
static GstStateChangeReturn
gst_avtp_crf_base_change_state (GstElement * element, GstStateChange transition)
{
GstAvtpCrfBase *avtpcrfbase = GST_AVTP_CRF_BASE (element);
GstAvtpCrfThreadData *thread_data = &avtpcrfbase->thread_data;
GstStateChangeReturn res;
GError *error = NULL;
GST_DEBUG_OBJECT (avtpcrfbase, "transition %d", transition);
switch (transition) {
case GST_STATE_CHANGE_NULL_TO_READY:
thread_data->past_periods =
g_malloc0 (sizeof (guint64) * MAX_NUM_PERIODS_STORED);
thread_data->mr = -1;
thread_data->is_running = TRUE;
thread_data->thread =
g_thread_try_new ("crf-listener",
(GThreadFunc) crf_listener_thread_func, avtpcrfbase, &error);
if (error) {
GST_ERROR_OBJECT (avtpcrfbase, "failed to start thread, %s",
error->message);
g_error_free (error);
g_free (thread_data->past_periods);
return GST_STATE_CHANGE_FAILURE;
}
break;
default:
break;
}
res = GST_ELEMENT_CLASS (parent_class)->change_state (element, transition);
switch (transition) {
case GST_STATE_CHANGE_READY_TO_NULL:
thread_data->is_running = FALSE;
g_thread_join (thread_data->thread);
g_free (thread_data->past_periods);
break;
default:
break;
}
return res;
}
static int
setup_socket (GstAvtpCrfBase * avtpcrfbase)
{
struct sockaddr_ll sk_addr = { 0 };
struct packet_mreq mreq = { 0 };
struct timeval timeout = { 0 };
guint8 addr[ETH_ALEN];
int fd, res, ifindex;
fd = socket (AF_PACKET, SOCK_DGRAM, htons (ETH_P_TSN));
if (fd < 0) {
GST_ERROR_OBJECT (avtpcrfbase, "Failed to open socket: %s",
g_strerror (errno));
return fd;
}
ifindex = if_nametoindex (avtpcrfbase->ifname);
if (!ifindex) {
res = -1;
GST_ERROR_OBJECT (avtpcrfbase, "Failed to get index for interface: %s",
g_strerror (errno));
goto err;
}
sk_addr.sll_family = AF_PACKET;
sk_addr.sll_protocol = htons (ETH_P_TSN);
sk_addr.sll_ifindex = ifindex;
res = bind (fd, (struct sockaddr *) &sk_addr, sizeof (sk_addr));
if (res < 0) {
GST_ERROR_OBJECT (avtpcrfbase, "Failed to bind socket: %s",
g_strerror (errno));
goto err;
}
res = sscanf (avtpcrfbase->address, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
&addr[0], &addr[1], &addr[2], &addr[3], &addr[4], &addr[5]);
if (res != 6) {
res = -1;
GST_ERROR_OBJECT (avtpcrfbase, "Destination MAC address format not valid");
goto err;
}
mreq.mr_ifindex = ifindex;
mreq.mr_type = PACKET_MR_MULTICAST;
mreq.mr_alen = ETH_ALEN;
memcpy (&mreq.mr_address, addr, ETH_ALEN);
res = setsockopt (fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq,
sizeof (struct packet_mreq));
if (res < 0) {
GST_ERROR_OBJECT (avtpcrfbase, "Failed to set multicast address: %s",
g_strerror (errno));
goto err;
}
timeout.tv_sec = RECV_TIMEOUT;
res =
setsockopt (fd, SOL_SOCKET, SO_RCVTIMEO, (void *) &timeout,
sizeof (struct timeval));
if (res < 0) {
GST_ERROR_OBJECT (avtpcrfbase, "Failed to set receive timeout: %s",
g_strerror (errno));
goto err;
}
return fd;
err:
close (fd);
return res;
}
static gboolean
validate_crf_pdu (GstAvtpCrfBase * avtpcrfbase, struct avtp_crf_pdu *crf_pdu,
int packet_size)
{
GstAvtpCrfThreadData *data = &avtpcrfbase->thread_data;
guint64 tstamp_interval, base_freq, pull, type;
guint64 streamid_valid, streamid, data_len;
guint32 subtype;
int res;
if (packet_size < sizeof (struct avtp_crf_pdu))
return FALSE;
res = avtp_pdu_get ((struct avtp_common_pdu *) crf_pdu, AVTP_FIELD_SUBTYPE,
&subtype);
g_assert (res == 0);
if (subtype != AVTP_SUBTYPE_CRF) {
GST_DEBUG_OBJECT (avtpcrfbase, "Not a CRF PDU.subtype: %u", subtype);
return FALSE;
}
res = avtp_crf_pdu_get (crf_pdu, AVTP_CRF_FIELD_SV, &streamid_valid);
g_assert (res == 0);
res = avtp_crf_pdu_get (crf_pdu, AVTP_CRF_FIELD_STREAM_ID, &streamid);
g_assert (res == 0);
res = avtp_crf_pdu_get (crf_pdu, AVTP_CRF_FIELD_CRF_DATA_LEN, &data_len);
g_assert (res == 0);
res = avtp_crf_pdu_get (crf_pdu, AVTP_CRF_FIELD_TIMESTAMP_INTERVAL,
(guint64 *) & tstamp_interval);
g_assert (res == 0);
res = avtp_crf_pdu_get (crf_pdu, AVTP_CRF_FIELD_BASE_FREQ, &base_freq);
g_assert (res == 0);
res = avtp_crf_pdu_get (crf_pdu, AVTP_CRF_FIELD_PULL, &pull);
g_assert (res == 0);
res = avtp_crf_pdu_get (crf_pdu, AVTP_CRF_FIELD_TYPE, &type);
g_assert (res == 0);
if (!streamid_valid || streamid != avtpcrfbase->streamid) {
GST_DEBUG_OBJECT (avtpcrfbase,
"Stream ID doesn't match. Discarding CRF packet");
return FALSE;
}
if (G_UNLIKELY (data_len + sizeof (struct avtp_crf_pdu) > packet_size)) {
GST_DEBUG_OBJECT (avtpcrfbase,
"Packet size smaller than expected. Discarding CRF packet");
return FALSE;
}
if (G_UNLIKELY (!data->timestamp_interval)) {
if (G_UNLIKELY (tstamp_interval == 0)) {
GST_DEBUG_OBJECT (avtpcrfbase,
"timestamp_interval should not be zero. Discarding CRF packet");
return FALSE;
}
data->timestamp_interval = tstamp_interval;
if (G_UNLIKELY (base_freq == 0)) {
GST_DEBUG_OBJECT (avtpcrfbase,
"Base Frequency cannot be zero, Discarding CRF packet");
goto error;
}
data->base_freq = base_freq;
if (G_UNLIKELY (pull > AVTP_CRF_PULL_MULT_BY_1_OVER_8)) {
GST_DEBUG_OBJECT (avtpcrfbase,
"Pull value invalid, Discarding CRF packet");
goto error;
}
data->pull = pull;
if (G_UNLIKELY (type > AVTP_CRF_TYPE_MACHINE_CYCLE)) {
GST_DEBUG_OBJECT (avtpcrfbase,
"CRF timestamp type invalid, Discarding CRF packet");
goto error;
}
data->type = type;
if (G_UNLIKELY (!data_len || data_len % 8 != 0)) {
GST_DEBUG_OBJECT (avtpcrfbase,
"Data Length should be a multiple of 8. Discarding CRF packet.");
goto error;
}
data->num_pkt_tstamps = data_len / CRF_TIMESTAMP_SIZE;
} else {
if (G_UNLIKELY (tstamp_interval != data->timestamp_interval)) {
GST_DEBUG_OBJECT (avtpcrfbase,
"Timestamp interval doesn't match, discarding CRF packet");
return FALSE;
}
if (G_UNLIKELY (base_freq != data->base_freq)) {
GST_DEBUG_OBJECT (avtpcrfbase,
"Base Frequency doesn't match, discarding CRF packet");
return FALSE;
}
if (G_UNLIKELY (pull != data->pull)) {
GST_DEBUG_OBJECT (avtpcrfbase,
"Pull value doesn't match, discarding CRF packet");
return FALSE;
}
if (G_UNLIKELY (data->type != type)) {
GST_DEBUG_OBJECT (avtpcrfbase,
"CRF timestamp type doesn't match, Discarding CRF packet");
return FALSE;
}
if (G_UNLIKELY (data_len / CRF_TIMESTAMP_SIZE != data->num_pkt_tstamps)) {
GST_DEBUG_OBJECT (avtpcrfbase,
"Number of timestamps doesn't match. discarding CRF packet");
return FALSE;
}
}
/* Make sure all the timestamps are monotonically increasing. */
for (int i = 0; i < data->num_pkt_tstamps - 1; i++) {
GstClockTime tstamp, next_tstamp;
tstamp = be64toh (crf_pdu->crf_data[i]);
next_tstamp = be64toh (crf_pdu->crf_data[i + 1]);
if (G_UNLIKELY (tstamp >= next_tstamp)) {
GST_DEBUG_OBJECT (avtpcrfbase,
"Timestamps are not monotonically increasing. discarding CRF packet");
return FALSE;
}
}
return TRUE;
error:
data->timestamp_interval = 0;
return FALSE;
}
static gdouble
get_base_freq_multiplier (GstAvtpCrfBase * avtpcrfbase, guint64 pull)
{
switch (pull) {
case 0:
return 1.0;
case 1:
return 1 / 1.001;
case 2:
return 1.001;
case 3:
return 24.0 / 25;
case 4:
return 25.0 / 24;
case 5:
return 1.0 / 8;
default:
GST_ERROR_OBJECT (avtpcrfbase, "Invalid pull value");
return -1;
}
}
static void
calculate_average_period (GstAvtpCrfBase * avtpcrfbase,
struct avtp_crf_pdu *crf_pdu)
{
GstAvtpCrfThreadData *data = &avtpcrfbase->thread_data;
GstClockTime first_pkt_tstamp, last_pkt_tstamp;
int num_pkt_tstamps, past_periods_iter;
GstClockTime accumulate_period = 0;
num_pkt_tstamps = data->num_pkt_tstamps;
past_periods_iter = data->past_periods_iter;
first_pkt_tstamp = be64toh (crf_pdu->crf_data[0]);
last_pkt_tstamp = be64toh (crf_pdu->crf_data[num_pkt_tstamps - 1]);
/*
* If there is only one CRF Timestamp per CRF AVTPU, at least two packets are
* needed to calculate the period. Also, sequence number needs to be checked
* to ensure consecutive packets are being used to calculate the period.
* Otherwise, we will just use the nominal frequency to estimate period.
*/
if (num_pkt_tstamps == 1) {
guint64 seqnum;
int res;
res = avtp_crf_pdu_get (crf_pdu, AVTP_CRF_FIELD_SEQ_NUM, &seqnum);
g_assert (res == 0);
if (!data->last_received_tstamp ||
((data->last_seqnum + 1) % 255 != seqnum)) {
GstClockTime average_period = data->average_period;
if (!data->last_received_tstamp) {
gdouble base_freq_mult;
base_freq_mult = get_base_freq_multiplier (avtpcrfbase, data->pull);
if (base_freq_mult < 0)
return;
average_period =
gst_util_uint64_scale (1.0, 1000000000,
(data->base_freq * base_freq_mult));
}
data->last_received_tstamp = first_pkt_tstamp;
data->last_seqnum = seqnum;
data->current_ts = first_pkt_tstamp;
data->average_period = average_period;
return;
}
data->past_periods[past_periods_iter] =
first_pkt_tstamp - data->last_received_tstamp;
data->last_received_tstamp = first_pkt_tstamp;
data->last_seqnum = seqnum;
} else {
data->past_periods[past_periods_iter] =
(last_pkt_tstamp - first_pkt_tstamp) /
(data->timestamp_interval * (num_pkt_tstamps - 1));
}
if (data->periods_stored < MAX_NUM_PERIODS_STORED)
data->periods_stored++;
data->past_periods_iter = (past_periods_iter + 1) % data->periods_stored;
for (int i = 0; i < data->periods_stored; i++)
accumulate_period += data->past_periods[i];
data->average_period = accumulate_period / data->periods_stored;
data->current_ts = first_pkt_tstamp;
}
static void
crf_listener_thread_func (GstAvtpCrfBase * avtpcrfbase)
{
GstAvtpCrfThreadData *data = &avtpcrfbase->thread_data;
struct avtp_crf_pdu *crf_pdu = g_alloca (MAX_AVTPDU_SIZE);
guint64 media_clk_reset;
int fd, n, res;
fd = setup_socket (avtpcrfbase);
if (fd < 0) {
GST_ELEMENT_ERROR (avtpcrfbase, RESOURCE, OPEN_READ,
("Cannot open socket for CRF Listener"), (NULL));
return;
}
while (data->is_running) {
n = recv (fd, crf_pdu, MAX_AVTPDU_SIZE, 0);
if (n == -1) {
if (errno == EAGAIN || errno == EINTR)
continue;
GST_ERROR_OBJECT (avtpcrfbase, "Failed to receive packet: %s",
g_strerror (errno));
break;
}
if (!validate_crf_pdu (avtpcrfbase, crf_pdu, n))
continue;
GST_DEBUG_OBJECT (avtpcrfbase, "Packet valid. Adding to buffer\n");
res = avtp_crf_pdu_get (crf_pdu, AVTP_CRF_FIELD_MR, &media_clk_reset);
g_assert (res == 0);
if (media_clk_reset != data->mr) {
memset (data->past_periods, 0, sizeof (gint64) * MAX_NUM_PERIODS_STORED);
data->periods_stored = 0;
data->average_period = 0;
data->current_ts = 0;
data->last_received_tstamp = 0;
data->past_periods_iter = 0;
data->mr = media_clk_reset;
}
calculate_average_period (avtpcrfbase, crf_pdu);
}
close (fd);
}
static void
gst_avtp_crf_base_finalize (GObject * object)
{
GstAvtpCrfBase *avtpcrfbase = GST_AVTP_CRF_BASE (object);
g_free (avtpcrfbase->ifname);
g_free (avtpcrfbase->address);
G_OBJECT_CLASS (parent_class)->finalize (object);
}
static void
gst_avtp_crf_base_set_property (GObject * object, guint prop_id,
const GValue * value, GParamSpec * pspec)
{
GstAvtpCrfBase *avtpcrfbase = GST_AVTP_CRF_BASE (object);
GST_DEBUG_OBJECT (avtpcrfbase, "prop_id %u", prop_id);
switch (prop_id) {
case PROP_STREAMID:
avtpcrfbase->streamid = g_value_get_uint64 (value);
break;
case PROP_IFNAME:
g_free (avtpcrfbase->ifname);
avtpcrfbase->ifname = g_value_dup_string (value);
break;
case PROP_ADDRESS:
g_free (avtpcrfbase->address);
avtpcrfbase->address = g_value_dup_string (value);
break;
default:
G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
break;
}
}
static void
gst_avtp_crf_base_get_property (GObject * object, guint prop_id,
GValue * value, GParamSpec * pspec)
{
GstAvtpCrfBase *avtpcrfbase = GST_AVTP_CRF_BASE (object);
GST_DEBUG_OBJECT (avtpcrfbase, "prop_id %u", prop_id);
switch (prop_id) {
case PROP_STREAMID:
g_value_set_uint64 (value, avtpcrfbase->streamid);
break;
case PROP_IFNAME:
g_value_set_string (value, avtpcrfbase->ifname);
break;
case PROP_ADDRESS:
g_value_set_string (value, avtpcrfbase->address);
break;
default:
G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
break;
}
}