/*
 * Copyright (c) 2019 Vestas Wind Systems A/S
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <CANopen.h>

/**
 * @brief CANopen sync thread.
 *
 * The CANopen real-time sync thread processes SYNC RPDOs and TPDOs
 * through the CANopenNode stack with an interval of 1 millisecond.
 *
 * @param p1 Unused
 * @param p2 Unused
 * @param p3 Unused
 */
static void canopen_sync_thread(void *p1, void *p2, void *p3)
{
	u32_t start; /* cycles */
	u32_t stop;  /* cycles */
	u32_t delta; /* cycles */
	u32_t elapsed = 0; /* microseconds */
	bool sync;

	ARG_UNUSED(p1);
	ARG_UNUSED(p2);
	ARG_UNUSED(p3);

	while (true) {
		start = k_cycle_get_32();
		if (CO && CO->CANmodule[0] && CO->CANmodule[0]->CANnormal) {
			CO_LOCK_OD();
			sync = CO_process_SYNC(CO, elapsed);
			CO_process_RPDO(CO, sync);
			CO_process_TPDO(CO, sync, elapsed);
			CO_UNLOCK_OD();
		}

		k_sleep(K_MSEC(1));
		stop = k_cycle_get_32();
		delta = stop - start;
		elapsed = (u32_t)k_cyc_to_ns_floor64(delta) / NSEC_PER_USEC;
	}
}

K_THREAD_DEFINE(canopen_sync, CONFIG_CANOPEN_SYNC_THREAD_STACK_SIZE,
		canopen_sync_thread, NULL, NULL, NULL,
		CONFIG_CANOPEN_SYNC_THREAD_PRIORITY, 0, K_MSEC(1));