diff --git a/include/rtapi_seqlock.h b/include/rtapi_seqlock.h new file mode 100644 index 00000000000..8947f771f8e --- /dev/null +++ b/include/rtapi_seqlock.h @@ -0,0 +1,58 @@ +// Copyright 2026 Luca Toniolo +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// 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, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. +#ifndef __LINUXCNC_RTAPI_SEQLOCK_H +#define __LINUXCNC_RTAPI_SEQLOCK_H + +/* Split read protection for a struct in shared memory that one writer + changes and readers copy. The struct holds two plain unsigned int + counters, head and tail, so it stays trivially copyable in C++. + + The writer calls rtapi_seqlock_write_begin() before it changes the + struct and rtapi_seqlock_write_end() when done. A reader copies with + rtapi_seqlock_read() and keeps the copy only when it returns nonzero: + tail read before the copy matched head read after it, so no write + started or was under way while the copy was taken. The fences keep + the counters and the data in that order on weakly ordered CPUs. */ + +#include "rtapi_string.h" /* memcpy() */ + +static inline void rtapi_seqlock_write_begin(unsigned int *head) +{ + __atomic_store_n(head, *head + 1, __ATOMIC_RELAXED); + /* readers see the new head before any change to the struct */ + __atomic_thread_fence(__ATOMIC_RELEASE); +} + +static inline void rtapi_seqlock_write_end(unsigned int *tail, const unsigned int *head) +{ + /* readers see every change to the struct before the new tail */ + __atomic_store_n(tail, *head, __ATOMIC_RELEASE); +} + +static inline int rtapi_seqlock_read(void *dst, const void *src, size_t size, + const unsigned int *head, const unsigned int *tail) +{ + unsigned int t = __atomic_load_n(tail, __ATOMIC_ACQUIRE); + if (__atomic_load_n(head, __ATOMIC_RELAXED) != t) { + return 0; /* a write is under way */ + } + memcpy(dst, src, size); + /* the copy is complete before head is read again */ + __atomic_thread_fence(__ATOMIC_ACQUIRE); + return __atomic_load_n(head, __ATOMIC_RELAXED) == t; +} + +#endif diff --git a/src/emc/motion-logger/motion-logger.c b/src/emc/motion-logger/motion-logger.c index 36b77e54528..1f8edab971b 100644 --- a/src/emc/motion-logger/motion-logger.c +++ b/src/emc/motion-logger/motion-logger.c @@ -59,10 +59,10 @@ int num_spindles = EMCMOT_MAX_SPINDLES; void emcmot_config_change(void) { if (emcmotConfig->head == emcmotConfig->tail) { - emcmotConfig->head++; + rtapi_seqlock_write_begin(&emcmotConfig->head); emcmotConfig->config_num++; emcmotStatus->config_num = emcmotConfig->config_num; - emcmotConfig->tail = emcmotConfig->head; + rtapi_seqlock_write_end(&emcmotConfig->tail, &emcmotConfig->head); } } @@ -329,7 +329,7 @@ int main(int argc, char* argv[]) { // new incoming command! // - emcmotStatus->head++; + rtapi_seqlock_write_begin(&emcmotStatus->head); switch (c->command) { case EMCMOT_ABORT: @@ -711,7 +711,7 @@ int main(int argc, char* argv[]) { emcmotStatus->commandEcho = c->command; emcmotStatus->commandNumEcho = c->commandNum; emcmotStatus->commandStatus = EMCMOT_COMMAND_OK; - emcmotStatus->tail = emcmotStatus->head; + rtapi_seqlock_write_end(&emcmotStatus->tail, &emcmotStatus->head); rtapi_mutex_give(&emcmotStruct->command_mutex); } diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 14356e1c9c1..a3c607f12fc 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -430,8 +430,8 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) if (emcmotCommand->commandNum != emcmotStatus->commandNumEcho) { /* increment head count-- we'll be modifying emcmotStatus */ - emcmotStatus->head++; - emcmotInternal->head++; + rtapi_seqlock_write_begin(&emcmotStatus->head); + rtapi_seqlock_write_begin(&emcmotInternal->head); /* got a new command-- echo command and number... */ emcmotStatus->commandEcho = emcmotCommand->command; @@ -679,6 +679,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) (motion_num_spindles) */ rtapi_print_msg(RTAPI_MSG_DBG, "SET_NUM_SPINDLES"); + emcmot_config_change(); rtapi_print_msg(RTAPI_MSG_DBG, " %d", emcmotCommand->spindle); if ( emcmotCommand->spindle > motion_num_spindles || emcmotCommand->spindle <= 0 @@ -2100,6 +2101,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) break; case EMCMOT_SELECT_KINS_TYPE: + emcmot_config_change(); emcmotConfig->switchkins_type = emcmotCommand->switchkins_type; emcmotConfig->switchkins_seq++; break; @@ -2111,10 +2113,12 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) break; case EMCMOT_SET_MAX_FEED_OVERRIDE: rtapi_print_msg(RTAPI_MSG_DBG, "SET_MAX_FEED_OVERRIDE"); + emcmot_config_change(); emcmotConfig->maxFeedScale = emcmotCommand->maxFeedScale; break; case EMCMOT_SETUP_ARC_BLENDS: rtapi_print_msg(RTAPI_MSG_DBG, "SETUP_ARC_BLENDS"); + emcmot_config_change(); emcmotConfig->arcBlendEnable = emcmotCommand->arcBlendEnable; emcmotConfig->arcBlendFallbackEnable = emcmotCommand->arcBlendFallbackEnable; emcmotConfig->arcBlendOptDepth = emcmotCommand->arcBlendOptDepth; @@ -2124,6 +2128,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) break; case EMCMOT_SET_PROBE_ERR_INHIBIT: rtapi_print_msg(RTAPI_MSG_DBG, "SET_PROBE_ERR_INHIBIT"); + emcmot_config_change(); emcmotConfig->inhibit_probe_jog_error = emcmotCommand->probe_jog_err_inhibit; emcmotConfig->inhibit_probe_home_error = emcmotCommand->probe_home_err_inhibit; break; @@ -2135,9 +2140,9 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) } rtapi_print_msg(RTAPI_MSG_DBG, "\n"); /* synch tail count */ - emcmotStatus->tail = emcmotStatus->head; - emcmotConfig->tail = emcmotConfig->head; - emcmotInternal->tail = emcmotInternal->head; + rtapi_seqlock_write_end(&emcmotStatus->tail, &emcmotStatus->head); + rtapi_seqlock_write_end(&emcmotConfig->tail, &emcmotConfig->head); + rtapi_seqlock_write_end(&emcmotInternal->tail, &emcmotInternal->head); } /* end of: if-new-command */ diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 17fb96b7ddd..fb69e40cce7 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -239,13 +239,15 @@ void emcmotController(void *arg, long period) /* calculate servo period as a double - period is in integer nsec */ servo_period = period * 0.000000001; + /* increment head count to indicate work in progress */ + rtapi_seqlock_write_begin(&emcmotStatus->head); + rtapi_seqlock_write_begin(&emcmotInternal->head); + if((rtapi_uint)period != last_period) { emcmotSetCycleTime(period); last_period = period; } - /* increment head count to indicate work in progress */ - emcmotStatus->head++; /* here begins the core of the controller */ read_homing_in_pins(ALL_JOINTS); @@ -279,7 +281,8 @@ void emcmotController(void *arg, long period) /* here ends the core of the controller */ emcmotStatus->heartbeat++; /* set tail to head, to indicate work complete */ - emcmotStatus->tail = emcmotStatus->head; + rtapi_seqlock_write_end(&emcmotStatus->tail, &emcmotStatus->head); + rtapi_seqlock_write_end(&emcmotInternal->tail, &emcmotInternal->head); /* end of controller function */ } diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index 30ce42a6315..afaf6bbc6d7 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -194,9 +194,9 @@ void switch_to_teleop_mode(void) { void emcmot_config_change(void) { if (emcmotConfig->head == emcmotConfig->tail) { + rtapi_seqlock_write_begin(&emcmotConfig->head); emcmotConfig->config_num++; emcmotStatus->config_num = emcmotConfig->config_num; - emcmotConfig->head++; } } diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 7f3572eda73..a867e655435 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -84,6 +84,7 @@ to another. #include /* must precede rtapi_atomic.h in kernel mode */ #include +#include #ifdef __cplusplus extern "C" { @@ -583,7 +584,7 @@ Suggestion: Split this in to an Error and a Status flag register.. */ typedef struct emcmot_status_t { - unsigned char head; /* flag count for mutex detect */ + unsigned int head; /* split read counter, see rtapi_seqlock.h */ /* these three are updated only when a new command is handled */ cmd_code_t commandEcho; /* echo of input command */ int commandNumEcho; /* echo of input command number */ @@ -673,7 +674,7 @@ Suggestion: Split this in to an Error and a Status flag register.. unsigned int tcqlen; EmcPose tool_offset; int atspeed_next_feed; /* at next feed move, wait for spindle to be at speed */ - unsigned char tail; /* flag count for mutex detect */ + unsigned int tail; /* split read counter, see rtapi_seqlock.h */ int external_offsets_applied; EmcPose eoffset_pose; int numExtraJoints; @@ -715,7 +716,7 @@ Suggestion: Split this in to an Error and a Status flag register.. evaluated - either they move up, or they go away. */ typedef struct emcmot_config_t { - unsigned char head; /* flag count for mutex detect */ + unsigned int head; /* split read counter, see rtapi_seqlock.h */ int config_num; /* Incremented everytime configuration changed, should match status.config_num */ @@ -750,7 +751,7 @@ Suggestion: Split this in to an Error and a Status flag register.. double limitVel; /* scalar upper limit on vel */ int debug; /* copy of DEBUG, from INI file */ - unsigned char tail; /* flag count for mutex detect */ + unsigned int tail; /* split read counter, see rtapi_seqlock.h */ int arcBlendOptDepth; int arcBlendEnable; int arcBlendFallbackEnable; @@ -776,8 +777,8 @@ Suggestion: Split this in to an Error and a Status flag register.. typedef struct emcmot_internal_t { - unsigned char head; /* flag count for mutex detect */ - unsigned char tail; /* flag count for mutex detect */ + unsigned int head; /* split read counter, see rtapi_seqlock.h */ + unsigned int tail; /* split read counter, see rtapi_seqlock.h */ int split; /* number of split command reads */ int enabling; /* starts up disabled */ int coordinating; /* starts up in free mode */ diff --git a/src/emc/motion/usrmotintf.cc b/src/emc/motion/usrmotintf.cc index 944b34489f1..e314d97b7e9 100644 --- a/src/emc/motion/usrmotintf.cc +++ b/src/emc/motion/usrmotintf.cc @@ -135,10 +135,9 @@ int usrmotReadEmcmotStatus(emcmot_status_t * s) do { if(split_read_count > 0) esleep(1us); // Don't busy-loop and give time to process /* copy status struct from shmem to local memory */ - memcpy(s, emcmotStatus, sizeof(emcmot_status_t)); - /* got it, now check head-tail matche */ - if (s->head == s->tail) { - /* head and tail match, done */ + if (rtapi_seqlock_read(s, emcmotStatus, sizeof(emcmot_status_t), + &emcmotStatus->head, &emcmotStatus->tail)) { + /* no write overlapped the copy, done */ return EMCMOT_COMM_OK; } /* inc counter and try again, max three times */ @@ -161,10 +160,9 @@ int usrmotReadEmcmotConfig(emcmot_config_t * s) do { if(split_read_count > 0) esleep(1us); // Don't busy-loop and give time to process /* copy config struct from shmem to local memory */ - memcpy(s, emcmotConfig, sizeof(emcmot_config_t)); - /* got it, now check head-tail matches */ - if (s->head == s->tail) { - /* head and tail match, done */ + if (rtapi_seqlock_read(s, emcmotConfig, sizeof(emcmot_config_t), + &emcmotConfig->head, &emcmotConfig->tail)) { + /* no write overlapped the copy, done */ return EMCMOT_COMM_OK; } /* inc counter and try again, max three times */ @@ -186,10 +184,9 @@ int usrmotReadEmcmotInternal(emcmot_internal_t * s) do { if(split_read_count > 0) esleep(1us); // Don't busy-loop and give time to process /* copy debug struct from shmem to local memory */ - memcpy(s, emcmotInternal, sizeof(emcmot_internal_t)); - /* got it, now check head-tail matches */ - if (s->head == s->tail) { - /* head and tail match, done */ + if (rtapi_seqlock_read(s, emcmotInternal, sizeof(emcmot_internal_t), + &emcmotInternal->head, &emcmotInternal->tail)) { + /* no write overlapped the copy, done */ return EMCMOT_COMM_OK; } /* inc counter and try again, max three times */ diff --git a/tests/rtapi-seqlock/.gitignore b/tests/rtapi-seqlock/.gitignore new file mode 100644 index 00000000000..c8a2ae37ca7 --- /dev/null +++ b/tests/rtapi-seqlock/.gitignore @@ -0,0 +1 @@ +seqlock_check diff --git a/tests/rtapi-seqlock/check.hal b/tests/rtapi-seqlock/check.hal new file mode 100644 index 00000000000..fef84060af5 --- /dev/null +++ b/tests/rtapi-seqlock/check.hal @@ -0,0 +1 @@ +loadusr -w ./seqlock_check diff --git a/tests/rtapi-seqlock/checkresult b/tests/rtapi-seqlock/checkresult new file mode 100755 index 00000000000..dd7377eb3cf --- /dev/null +++ b/tests/rtapi-seqlock/checkresult @@ -0,0 +1,2 @@ +#!/bin/sh +grep -q ', torn kept 0$' "$1" diff --git a/tests/rtapi-seqlock/seqlock_check.comp b/tests/rtapi-seqlock/seqlock_check.comp new file mode 100644 index 00000000000..4c1a391b122 --- /dev/null +++ b/tests/rtapi-seqlock/seqlock_check.comp @@ -0,0 +1,107 @@ +component seqlock_check "Check rtapi_seqlock.h: a reader must never keep a torn copy"; +description """ +A writer thread rewrites a struct the size of the motion status over +and over, every word set to the same count, while the reader copies it +with rtapi_seqlock_read(). A kept copy whose words differ was torn. +The writer pauses for a varying time between writes, so copies start +before, during and after writes. Exits nonzero when a torn copy is +kept, or when too few copies were kept for the run to mean anything. +"""; +license "GPL"; +author "Luca Toniolo"; +pin out uint torn_kept "torn copies kept"; +option userspace yes; +option extra_link_args "-pthread"; +;; +#include +#include +#include +#include +#include + +#define WORDS 1500 +#define AFTER 32 +#define SECONDS 3 + +/* laid out like the motion structs, some data after tail too */ +typedef struct { + unsigned int head; + int data[WORDS]; + unsigned int tail; + int after[AFTER]; +} shared_t; + +static shared_t shared; +static int stop; + +static void *writer(void *arg) +{ + int k = 0; + (void)arg; + while (!__atomic_load_n(&stop, __ATOMIC_RELAXED)) { + int i; + k++; + rtapi_seqlock_write_begin(&shared.head); + for (i = 0; i < WORDS; i++) { + shared.data[i] = k; + } + for (i = 0; i < AFTER; i++) { + shared.after[i] = k; + } + rtapi_seqlock_write_end(&shared.tail, &shared.head); + /* idle for about 0 to 3 writes' worth */ + for (i = (k % 7) * 3000; i > 0; i--) { + __asm__ __volatile__("" ::: "memory"); + } + } + return NULL; +} + +static int torn(const shared_t *c) +{ + int i; + for (i = 0; i < WORDS; i++) { + if (c->data[i] != c->data[0]) { + return 1; + } + } + for (i = 0; i < AFTER; i++) { + if (c->after[i] != c->data[0]) { + return 1; + } + } + return 0; +} + +void user_mainloop(void) +{ + static shared_t copy; + pthread_t thread; + unsigned long reads = 0, kept = 0, bad = 0; + time_t end = time(NULL) + SECONDS; + + if (pthread_create(&thread, NULL, writer, NULL)) { + perror("seqlock_check: pthread_create"); + hal_exit(comp_id); + exit(1); + } + while (time(NULL) < end) { + int n; + for (n = 0; n < 1000; n++) { + reads++; + if (rtapi_seqlock_read(©, &shared, sizeof(copy), &shared.head, &shared.tail)) { + kept++; + bad += torn(©); + } + } + } + __atomic_store_n(&stop, 1, __ATOMIC_RELAXED); + pthread_join(thread, NULL); + + FOR_ALL_INSTS() { + torn_kept_set(bad); + } + printf("reads %lu, kept %lu, torn kept %lu\n", reads, kept, bad); + hal_exit(comp_id); + exit(bad || kept < 1000 ? 1 : 0); +} diff --git a/tests/rtapi-seqlock/test.sh b/tests/rtapi-seqlock/test.sh new file mode 100755 index 00000000000..a7b0a3bef2b --- /dev/null +++ b/tests/rtapi-seqlock/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash +set -e +halcompile --compile seqlock_check.comp >/dev/null +halrun -f check.hal