On 5/2/2022 12:05 PM, Arkadiusz Kubalewski wrote:
synce_clock interface allows creation, polling and destruction of
SyncE clock, designed to step and drive SyncE logic on all configured
SyncE devices.
SyncE clock is created with a SyncE-type configuration, where
SyncE-type configuration is prepared by parsing SyncE config file.
Poll of a SyncE clock, will step each configured SyncE device.
Destruction of SyncE clock releases all resources obtained by SyncE
clock and devices.
Co-developed-by: Piotr Kwapulinski <[email protected]>
Signed-off-by: Piotr Kwapulinski <[email protected]>
Co-developed-by: Michal Michalik <[email protected]>
Signed-off-by: Michal Michalik <[email protected]>
Signed-off-by: Arkadiusz Kubalewski <[email protected]>
---
synce_clock.c | 296 ++++++++++++++++++++++++++++++++++++++++++++++++++
synce_clock.h | 52 +++++++++
2 files changed, 348 insertions(+)
create mode 100644 synce_clock.c
create mode 100644 synce_clock.h
diff --git a/synce_clock.c b/synce_clock.c
new file mode 100644
index 000000000000..8007fa390bbd
--- /dev/null
+++ b/synce_clock.c
@@ -0,0 +1,296 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/**
+ * @file synce_clock.c
+ * @brief Implements a SYNCE clock interface.
+ * @note Copyright (C) 2022 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2, as published
+ * by the Free Software Foundation.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ */
+#include <stdlib.h>
+#include <errno.h>
+#include <unistd.h>
+#include <sys/queue.h>
+
+#include "synce_clock.h"
+#include "interface.h"
+#include "synce_dev.h"
+#include "print.h"
+#include "config.h"
+#include "missing.h"
+
+#define SYNCE_CLOCK_DELAY_USEC 20000
+#define SYNCE_CLOCK_INIT_DELAY_USEC 200000
+#define SYNCE_CLOCK_INIT_N_TRIES 10
+
+struct interface {
+ STAILQ_ENTRY(interface) list;
+};
+
+struct synce_dev {
+ LIST_ENTRY(synce_dev) list;
+};
+
+enum synce_clock_state {
+ SYNCE_CLK_UNKNOWN = 0,
+ SYNCE_CLK_INITED,
+ SYNCE_CLK_DEV_RDY,
+ SYNCE_CLK_DEV_INITED,
+ SYNCE_CLK_RUNNING,
+ SYNCE_CLK_FAILED,
+};
+
+struct synce_clock {
+ int num_devices;
+ int state;
+ LIST_HEAD(devices_head, synce_dev) devices;
+};
+
+static struct synce_clock *create_clock(void)
Please rename to create_synce_clock()
+{
+ static struct synce_clock clk;
+
+ if (clk.state != SYNCE_CLK_UNKNOWN) {
+ synce_clock_destroy(&clk);
+ pr_info("old synce_clock destroyed");
+ }
+ clk.state = SYNCE_CLK_INITED;
+
+ return &clk;
+}
+
+static void add_device(struct synce_clock *clk, struct synce_dev *dev)
+{
+ struct synce_dev *dev_iter, *last_dev = NULL;
+
+ LIST_FOREACH(dev_iter, &clk->devices, list) {
+ last_dev = dev_iter;
+ }
+
+ if (last_dev) {
+ LIST_INSERT_AFTER(last_dev, dev, list);
+ } else {
+ LIST_INSERT_HEAD(&clk->devices, dev, list);
+ }
+}
+
+static int create_synce_devices(struct synce_clock *clk, struct config *cfg)
+{
+ struct interface *iface;
+ struct synce_dev *dev;
+ const char *dev_name;
+ int count = 0;
+
+ if (clk->state != SYNCE_CLK_INITED) {
+ goto err;
+ }
+
+ LIST_INIT(&clk->devices);
+ STAILQ_FOREACH(iface, &cfg->interfaces, list) {
+ /* only parent devices shall be addresed */
+ if (!interface_se_has_parent_dev(iface)) {
+ dev_name = interface_name(iface);
+ dev = synce_dev_create(dev_name);
+ if (!dev) {
+ pr_err("failed to create device %s", dev_name);
+ continue;
+ }
+
+ pr_debug("device init %s addr %p", dev_name, dev);
+ add_device(clk, dev);
+ count++;
+ }
+ }
+
+ if (!count) {
+ pr_err("no devices created");
+ goto err;
+ }
+
+ pr_info("created num_devices: %d", count);
+ clk->num_devices = count;
+ clk->state = SYNCE_CLK_DEV_RDY;
+
+ return 0;
+err:
+ clk->state = SYNCE_CLK_FAILED;
+ return -EINVAL;
+}
+
+static int init_synce_devices(struct synce_clock *clk, struct config *cfg)
+{
+ struct synce_dev *dev, *tmp;
+ int count = 0;
+
+ if (clk->state != SYNCE_CLK_DEV_RDY) {
+ goto err;
+ }
+
+ LIST_FOREACH_SAFE(dev, &clk->devices, list, tmp) {
+ /* Each parent device will init its ports */
+ if (synce_dev_init(dev, cfg)) {
+ pr_err("failed to init device %s",
+ synce_dev_name(dev));
+ synce_dev_destroy(dev);
+ LIST_REMOVE(dev, list);
+ free(dev);
+ continue;
+ } else {
+ pr_debug("device inited %s", synce_dev_name(dev));
+ count++;
+ }
+ }
+
+ if (count == 0) {
+ pr_err("no Sync-E devices initialized");
+ goto err;
+ } else if (count != clk->num_devices) {
+ pr_warning("initialized only %d from %d Sync-E devices",
+ count, clk->num_devices);
+ clk->num_devices = count;
+ }
+ clk->state = SYNCE_CLK_DEV_INITED;
+
+ return 0;
+err:
+ clk->state = SYNCE_CLK_FAILED;
+ return -EINVAL;
+}
+
+static void remove_failed_devices(struct synce_clock *clk)
+{
+ struct synce_dev *dev, *tmp;
+ int failed_cnt = 0;
+
+ LIST_FOREACH_SAFE(dev, &clk->devices, list, tmp) {
+ if (!synce_dev_is_running(dev)) {
+ synce_dev_destroy(dev);
+ LIST_REMOVE(dev, list);
+ free(dev);
+ failed_cnt++;
+ }
+ }
+ clk->num_devices -= failed_cnt;
+ pr_warning("Found dead devices: %d", failed_cnt);
+ pr_info("devices still running: %d", clk->num_devices);
+}
+
+static int verify_clock_state(struct synce_clock *clk)
+{
+ int i, running, timeout = SYNCE_CLOCK_INIT_N_TRIES;
+ struct synce_dev *dev;
+
+ if (clk->state < SYNCE_CLK_DEV_INITED) {
+ return -ENODEV;
+ }
+
+ /* let threads get running */
+ for (i = 0; i < timeout; ++i) {
+ running = 0;
+ LIST_FOREACH(dev, &clk->devices, list) {
+ if (synce_dev_is_running(dev))
+ running++;
+ }
+
+ if (running == clk->num_devices) {
+ clk->state = SYNCE_CLK_RUNNING;
+ break;
+ }
+ usleep(SYNCE_CLOCK_INIT_DELAY_USEC);
+ }
+
+ pr_debug("running num_devices %d configured %d",
+ running, clk->num_devices);
+
+ /* If at least one dev is running we leave clock running
+ * while removing failed devices.
+ * Previous traces shall indicate which ones have failed.
+ */
+ if (!running) {
+ pr_err("no device is running");
+ return -ENODEV;
+ } else if (running != clk->num_devices) {
+ remove_failed_devices(clk);
+ }
+
+ return 0;
+}
+
+struct synce_clock *synce_clock_create(struct config *cfg)
+{
+ struct synce_clock *clk;
+ int err;
+
+ if (!cfg) {
+ pr_err("%s cfg is NULL", __func__);
+ return NULL;
+ }
+
+ clk = create_clock();
+ if (!clk) {
+ return NULL;
+ }
+ err = create_synce_devices(clk, cfg);
+ if (err) {
+ goto destroy;
+ }
+ err = init_synce_devices(clk, cfg);
+ if (err) {
+ goto destroy;
+ }
+ err = verify_clock_state(clk);
+ if (err) {
+ goto destroy;
+ }
+
+ return clk;
+
+destroy:
+ synce_clock_destroy(clk);
+
+ return NULL;
+}
+
+void synce_clock_destroy(struct synce_clock *clk)
+{
+ struct synce_dev *dev, *tmp;
+
+ pr_debug("%s", __func__);
+
+ LIST_FOREACH_SAFE(dev, &clk->devices, list, tmp) {
+ synce_dev_destroy(dev);
+ LIST_REMOVE(dev, list);
+ free(dev);
+ }
+ clk->num_devices = 0;
+ clk->state = SYNCE_CLK_UNKNOWN;
+
+ return;
+}
+
+int synce_clock_poll(struct synce_clock *clk)
+{
+ struct synce_dev *dev;
+ int ret = -ENODEV;
+
+ if (clk->state == SYNCE_CLK_RUNNING) {
+ LIST_FOREACH(dev, &clk->devices, list) {
+ ret = synce_dev_step(dev);
+ if (ret) {
+ pr_err("dev_step fail");
+ }
+ }
+ }
+ usleep(SYNCE_CLOCK_DELAY_USEC);
+
+ return ret;
+}
diff --git a/synce_clock.h b/synce_clock.h
new file mode 100644
index 000000000000..e5559a4cc9cc
--- /dev/null
+++ b/synce_clock.h
@@ -0,0 +1,52 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/**
+ * @file synce_clock.h
+ * @brief Implements a Sync-E clock behavior.
+ * @note Copyright (C) 2022 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2, as published
+ * by the Free Software Foundation.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef HAVE_SYNCE_CLOCK_H
+#define HAVE_SYNCE_CLOCK_H
+
+#include "config.h"
+
+/* Opaque type */
+struct synce_clock;
+
+/**
+ * Create a Sync-E clock instance. There can only be one synce_clock in a
+ * system so subsequent calls will destroy the previous clock instance.
+ *
+ * @param cfg Pointer to the SYNCE-type configuration database
+ * @return Pointer to the single global Sync-E clock instance
+ */
+struct synce_clock *synce_clock_create(struct config *cfg);
+
+/**
+ * Destroy resources associated with the synce clock.
+ *
+ * @param clk Pointer to synce_clock instance
+ */
+void synce_clock_destroy(struct synce_clock *clk);
+
+/**
+ * Poll for synce events and dispatch them.
+ *
+ * @param clk A pointer to a synce_clock instance obtained with
+ * synce_clock_create().
+ * @return Zero on success, non-zero otherwise
+ */
+int synce_clock_poll(struct synce_clock *clk);
+
+#endif
_______________________________________________
Linuxptp-devel mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
