klipper/klippy/chelper/trdispatch.c

227 lines
6.9 KiB
C

// Trigger sync "trsync" message dispatch
//
// Copyright (C) 2021 Kevin O'Connor <kevin@koconnor.net>
//
// This file may be distributed under the terms of the GNU GPLv3 license.
#include <pthread.h> // pthread_mutex_lock
#include <stddef.h> // offsetof
#include <stdlib.h> // malloc
#include <string.h> // memset
#include "compiler.h" // ARRAY_SIZE
#include "list.h" // list_add_tail
#include "pollreactor.h" // PR_NEVER
#include "pyhelper.h" // report_errno
#include "serialqueue.h" // serialqueue_add_fastreader
struct trdispatch {
struct list_head tdm_list;
pthread_mutex_t lock; // protects variables below
uint32_t is_active, can_trigger, dispatch_reason;
};
struct trdispatch_mcu {
struct fastreader fr;
struct trdispatch *td;
struct list_node node;
struct serialqueue *sq;
struct command_queue *cq;
uint32_t trsync_oid, set_timeout_msgtag, trigger_msgtag;
// Remaining fields protected by trdispatch lock
uint64_t last_status_clock, expire_clock;
uint64_t expire_ticks, min_extend_ticks;
struct clock_estimate ce;
};
// Send: trsync_trigger oid=%c reason=%c
static void
send_trsync_trigger(struct trdispatch_mcu *tdm)
{
uint32_t msg[3] = {
tdm->trigger_msgtag, tdm->trsync_oid, tdm->td->dispatch_reason
};
struct queue_message *qm = message_alloc_and_encode(msg, ARRAY_SIZE(msg));
serialqueue_send_one(tdm->sq, tdm->cq, qm);
}
// Send: trsync_set_timeout oid=%c clock=%u
static void
send_trsync_set_timeout(struct trdispatch_mcu *tdm)
{
uint32_t msg[3] = {
tdm->set_timeout_msgtag, tdm->trsync_oid, tdm->expire_clock
};
struct queue_message *qm = message_alloc_and_encode(msg, ARRAY_SIZE(msg));
qm->req_clock = tdm->expire_clock;
serialqueue_send_one(tdm->sq, tdm->cq, qm);
}
// Handle a trsync_state message (callback from serialqueue fastreader)
static void
handle_trsync_state(struct fastreader *fr, uint8_t *data, int len)
{
struct trdispatch_mcu *tdm = container_of(fr, struct trdispatch_mcu, fr);
// Parse: trsync_state oid=%c can_trigger=%c trigger_reason=%c clock=%u
uint32_t fields[5];
int ret = msgblock_decode(fields, ARRAY_SIZE(fields), data, len);
if (ret || fields[1] != tdm->trsync_oid)
return;
uint32_t can_trigger=fields[2], clock=fields[4];
// Process message
struct trdispatch *td = tdm->td;
pthread_mutex_lock(&td->lock);
if (!td->can_trigger)
goto done;
if (!can_trigger) {
// mcu reports trigger or timeout - propagate to all mcus
td->can_trigger = 0;
struct trdispatch_mcu *m;
list_for_each_entry(m, &td->tdm_list, node) {
send_trsync_trigger(m);
}
goto done;
}
// mcu is still working okay - update last_status_clock
serialqueue_get_clock_est(tdm->sq, &tdm->ce);
tdm->last_status_clock = clock_from_clock32(&tdm->ce, clock);
// Determine minimum acknowledged time among all mcus
double min_time = PR_NEVER, next_min_time = PR_NEVER;
struct trdispatch_mcu *m, *min_tdm = NULL;
list_for_each_entry(m, &td->tdm_list, node) {
double status_time = clock_to_time(&m->ce, m->last_status_clock);
if (status_time < next_min_time) {
next_min_time = status_time;
if (status_time < min_time) {
next_min_time = min_time;
min_time = status_time;
min_tdm = m;
}
}
}
if (next_min_time == PR_NEVER)
next_min_time = min_time;
// Send trsync_set_timeout messages to other mcus (if needed)
list_for_each_entry(m, &td->tdm_list, node) {
double status_time = m == min_tdm ? next_min_time : min_time;
uint64_t expire=clock_from_time(&m->ce, status_time) + m->expire_ticks;
if ((int64_t)(expire - m->expire_clock) >= m->min_extend_ticks) {
m->expire_clock = expire;
send_trsync_set_timeout(m);
}
}
done:
pthread_mutex_unlock(&td->lock);
}
// Begin synchronization
void __visible
trdispatch_start(struct trdispatch *td, uint32_t dispatch_reason)
{
pthread_mutex_lock(&td->lock);
if (td->is_active || list_empty(&td->tdm_list)) {
pthread_mutex_unlock(&td->lock);
return;
}
td->dispatch_reason = dispatch_reason;
td->is_active = td->can_trigger = 1;
pthread_mutex_unlock(&td->lock);
// Register handle_trsync_state message parser for each mcu
struct trdispatch_mcu *tdm;
list_for_each_entry(tdm, &td->tdm_list, node) {
serialqueue_add_fastreader(tdm->sq, &tdm->fr);
}
}
// Cleanup after a test completes
void __visible
trdispatch_stop(struct trdispatch *td)
{
pthread_mutex_lock(&td->lock);
if (!td->is_active) {
pthread_mutex_unlock(&td->lock);
return;
}
td->is_active = 0;
pthread_mutex_unlock(&td->lock);
// Unregister handle_trsync_state message parsers
struct trdispatch_mcu *tdm;
list_for_each_entry(tdm, &td->tdm_list, node) {
serialqueue_rm_fastreader(tdm->sq, &tdm->fr);
}
}
// Create a new 'struct trdispatch' object
struct trdispatch * __visible
trdispatch_alloc(void)
{
struct trdispatch *td = malloc(sizeof(*td));
memset(td, 0, sizeof(*td));
list_init(&td->tdm_list);
int ret = pthread_mutex_init(&td->lock, NULL);
if (ret) {
report_errno("trdispatch_alloc pthread_mutex_init", ret);
return NULL;
}
return td;
}
// Create a new 'struct trdispatch_mcu' object
struct trdispatch_mcu * __visible
trdispatch_mcu_alloc(struct trdispatch *td, struct serialqueue *sq
, struct command_queue *cq, uint32_t trsync_oid
, uint32_t set_timeout_msgtag, uint32_t trigger_msgtag
, uint32_t state_msgtag)
{
struct trdispatch_mcu *tdm = malloc(sizeof(*tdm));
memset(tdm, 0, sizeof(*tdm));
tdm->sq = sq;
tdm->cq = cq;
tdm->trsync_oid = trsync_oid;
tdm->set_timeout_msgtag = set_timeout_msgtag;
tdm->trigger_msgtag = trigger_msgtag;
// Setup fastreader to match trsync_state messages
uint32_t state_prefix[] = {state_msgtag, trsync_oid};
struct queue_message *dummy = message_alloc_and_encode(
state_prefix, ARRAY_SIZE(state_prefix));
memcpy(tdm->fr.prefix, dummy->msg, dummy->len);
tdm->fr.prefix_len = dummy->len;
free(dummy);
tdm->fr.func = handle_trsync_state;
tdm->td = td;
list_add_tail(&tdm->node, &td->tdm_list);
return tdm;
}
// Setup for a trigger test
void __visible
trdispatch_mcu_setup(struct trdispatch_mcu *tdm
, uint64_t last_status_clock, uint64_t expire_clock
, uint64_t expire_ticks, uint64_t min_extend_ticks)
{
struct trdispatch *td = tdm->td;
pthread_mutex_lock(&td->lock);
tdm->last_status_clock = last_status_clock;
tdm->expire_clock = expire_clock;
tdm->expire_ticks = expire_ticks;
tdm->min_extend_ticks = min_extend_ticks;
serialqueue_get_clock_est(tdm->sq, &tdm->ce);
pthread_mutex_unlock(&td->lock);
}