28111ba254
This commit adds implementation of the PTP thread that will poll sockets descriptors and PTP Port's timeres for timeouts, generate events and handle them and trigger STATE_DECISION_EVENT handling when needed. Signed-off-by: Adam Wojasinski <awojasinski@baylibre.com>
117 lines
2.6 KiB
C
117 lines
2.6 KiB
C
/*
|
|
* Copyright (c) 2024 BayLibre SAS
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <zephyr/logging/log.h>
|
|
LOG_MODULE_REGISTER(ptp, CONFIG_PTP_LOG_LEVEL);
|
|
|
|
#include <errno.h>
|
|
#include <stdbool.h>
|
|
|
|
#include <zephyr/kernel.h>
|
|
#include <zephyr/net/net_if.h>
|
|
#include <zephyr/net/ptp.h>
|
|
|
|
#include "clock.h"
|
|
#include "port.h"
|
|
#include "transport.h"
|
|
|
|
K_KERNEL_STACK_DEFINE(ptp_stack, CONFIG_PTP_STACK_SIZE);
|
|
|
|
static struct k_thread ptp_thread_data;
|
|
|
|
static void ptp_thread(void *p1, void *p2, void *p3)
|
|
{
|
|
static const size_t timer_offset[] = {
|
|
offsetof(struct ptp_port, timers.announce),
|
|
offsetof(struct ptp_port, timers.delay),
|
|
offsetof(struct ptp_port, timers.sync),
|
|
offsetof(struct ptp_port, timers.qualification)
|
|
};
|
|
static const int timeout_bit[] = {
|
|
PTP_PORT_TIMER_ANNOUNCE_TO,
|
|
PTP_PORT_TIMER_DELAY_TO,
|
|
PTP_PORT_TIMER_SYNC_TO,
|
|
PTP_PORT_TIMER_QUALIFICATION_TO,
|
|
};
|
|
|
|
struct k_timer *timer;
|
|
struct ptp_port *port;
|
|
struct zsock_pollfd *fd;
|
|
enum ptp_port_event event;
|
|
|
|
ARG_UNUSED(p1);
|
|
ARG_UNUSED(p2);
|
|
ARG_UNUSED(p3);
|
|
|
|
while (1) {
|
|
fd = ptp_clock_poll_sockets();
|
|
|
|
SYS_SLIST_FOR_EACH_CONTAINER(ptp_clock_ports_list(), port, node) {
|
|
|
|
for (int i = 0; i < ARRAY_SIZE(timer_offset); i++) {
|
|
timer = (struct k_timer *)((uint8_t *)port +
|
|
timer_offset[i]);
|
|
|
|
if (!atomic_test_bit(&port->timeouts, timeout_bit[i])) {
|
|
continue;
|
|
}
|
|
|
|
event = ptp_port_timer_event_gen(port, timer);
|
|
|
|
if (event == PTP_EVT_STATE_DECISION ||
|
|
event == PTP_EVT_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES) {
|
|
ptp_clock_state_decision_req();
|
|
}
|
|
|
|
ptp_port_event_handle(port, event, false);
|
|
}
|
|
|
|
for (int i = 0; i < PTP_SOCKET_CNT; i++, fd++) {
|
|
if (!(fd->revents & (ZSOCK_POLLIN | ZSOCK_POLLPRI))) {
|
|
continue;
|
|
}
|
|
|
|
event = ptp_port_event_gen(port, i);
|
|
|
|
if (event == PTP_EVT_STATE_DECISION ||
|
|
event == PTP_EVT_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES) {
|
|
ptp_clock_state_decision_req();
|
|
}
|
|
|
|
ptp_port_event_handle(port, event, false);
|
|
}
|
|
}
|
|
|
|
ptp_clock_handle_state_decision_evt();
|
|
}
|
|
}
|
|
|
|
static int ptp_init(void)
|
|
{
|
|
k_tid_t tid;
|
|
const struct ptp_clock *domain = ptp_clock_init();
|
|
struct ptp_port *port;
|
|
|
|
if (!domain) {
|
|
return -ENODEV;
|
|
}
|
|
|
|
net_if_foreach(ptp_port_init, NULL);
|
|
|
|
SYS_SLIST_FOR_EACH_CONTAINER(ptp_clock_ports_list(), port, node) {
|
|
ptp_port_event_handle(port, PTP_EVT_INITIALIZE, false);
|
|
}
|
|
|
|
tid = k_thread_create(&ptp_thread_data, ptp_stack, K_KERNEL_STACK_SIZEOF(ptp_stack),
|
|
ptp_thread, NULL, NULL, NULL,
|
|
K_PRIO_COOP(1), 0, K_NO_WAIT);
|
|
k_thread_name_set(&ptp_thread_data, "PTP");
|
|
|
|
return 0;
|
|
}
|
|
|
|
SYS_INIT(ptp_init, APPLICATION, CONFIG_PTP_INIT_PRIO);
|