Merge pull request #2479 from LinuxCNC/fix-2386-task-motion-race

Fix #2386 task motion race

Author: SebKuzminsky

src/emc/motion-logger/motion-logger.c

@@ -312,12 +312,11 @@ int main(int argc, char* argv[]) {
     init_comm_buffers();
 
     while (1) {
-        if (c->commandNum != c->tail) {
-            // "split read"
-            continue;
-        }
+        rtapi_mutex_get(&emcmotStruct->command_mutex);
+
         if (c->commandNum == emcmotStatus->commandNumEcho) {
             // nothing new
+            rtapi_mutex_give(&emcmotStruct->command_mutex);
             maybe_reopen_logfile();
             usleep(10 * 1000);
             continue;
@@ -706,6 +705,8 @@ int main(int argc, char* argv[]) {
         emcmotStatus->commandNumEcho = c->commandNum;
         emcmotStatus->commandStatus = EMCMOT_COMMAND_OK;
         emcmotStatus->tail = emcmotStatus->head;
+
+        rtapi_mutex_give(&emcmotStruct->command_mutex);
     }
 
     return 0;

src/emc/motion/command.c

@@ -401,11 +401,14 @@ STATIC int is_feed_type(int motion_type)
     }
 }
 
+
 /*
   emcmotCommandHandler() is called each main cycle to read the
   shared memory buffer
+
+  This function runs with the emcmotCommand struct locked.
   */
-void emcmotCommandHandler(void *arg, long servo_period)
+void emcmotCommandHandler_locked(void *arg, long servo_period)
 {
     int joint_num, axis_num, spindle_num;
     int n,s0,s1; 
@@ -417,11 +420,6 @@ void emcmotCommandHandler(void *arg, long servo_period)
     int abort = 0;
     char* emsg = "";
 
-    /* check for split read */
-    if (emcmotCommand->head != emcmotCommand->tail) {
-	emcmotDebug->split++;
-	return;			/* not really an error */
-    }
     if (emcmotCommand->commandNum != emcmotStatus->commandNumEcho) {
 	/* increment head count-- we'll be modifying emcmotStatus */
 	emcmotStatus->head++;
@@ -2021,3 +2019,15 @@ void emcmotCommandHandler(void *arg, long servo_period)
 
     return;
 }
+
+
+void emcmotCommandHandler(void *arg, long servo_period) {
+    if (rtapi_mutex_try(&emcmotStruct->command_mutex) != 0) {
+        // Failed to take the mutex, because it is held by Task.
+        // This means Task is in the process of updating the command.
+        // Give up for now, and try again on the next invocation.
+        return;
+    }
+    emcmotCommandHandler_locked(arg, servo_period);
+    rtapi_mutex_give(&emcmotStruct->command_mutex);
+}

src/emc/motion/motion.c

@@ -692,10 +692,8 @@ static int init_comm_buffers(void)
     emcmotErrorInit(emcmotError);
 
     /* init command struct */
-    emcmotCommand->head = 0;
     emcmotCommand->command = 0;
     emcmotCommand->commandNum = 0;
-    emcmotCommand->tail = 0;
 
     /* init status struct */
     emcmotStatus->head = 0;

src/emc/motion/motion.h

@@ -214,7 +214,6 @@ extern "C" {
    memory, and all commands from higher level code come thru it.
 */
     typedef struct emcmot_command_t {
-	unsigned char head;	/* flag count for mutex detect */
 	cmd_code_t command;	/* command code (enum) */
 	int commandNum;		/* increment this for new command */
 	double motor_offset;    /* offset from joint to motor position */
@@ -265,7 +264,6 @@ extern "C" {
 	char    direction;      /* CANON_DIRECTION flag for spindle orient */
 	double  timeout;        /* of wait for spindle orient to complete */
 	unsigned char wait_for_spindle_at_speed; // EMCMOT_SPINDLE_ON now carries this, for next feed move
-	unsigned char tail;	/* flag count for mutex detect */
         int arcBlendOptDepth;
         int arcBlendEnable;
         int arcBlendFallbackEnable;

src/emc/motion/motion_struct.h

@@ -12,10 +12,14 @@
 #ifndef MOTION_STRUCT_H
 #define MOTION_STRUCT_H
 
+#include <rtapi_mutex.h>
+
+
 /* big comm structure, for upper memory */
     typedef struct emcmot_struct_t {
-	struct emcmot_command_t command;	/* struct used to pass commands/data
-					   to the RT module from usr space */
+        rtapi_mutex_t command_mutex;  // Used to protect access to `command`.
+        struct emcmot_command_t command;   /* struct used to pass commands/data from Task to Motion */
+
 	struct emcmot_status_t status;	/* Struct used to store RT status */
 	struct emcmot_config_t config;	/* Struct used to store RT config */
 	struct emcmot_internal_t internal;	/*! \todo FIXME - doesn't need to be in

src/emc/motion/usrmotintf.cc

@@ -76,24 +76,26 @@ int usrmotWriteEmcmotCommand(emcmot_command_t * c)
 {
     emcmot_status_t s;
     static int commandNum = 0;
-    static unsigned char headCount = 0;
     double end;
 
     if (!MOTION_ID_VALID(c->id)) {
         rcs_print("USRMOT: ERROR: invalid motion id: %d\n",c->id);
 	return EMCMOT_COMM_INVALID_MOTION_ID;
     }
-    c->head = ++headCount;
-    c->tail = c->head;
+
     c->commandNum = ++commandNum;
 
     /* check for mapped mem still around */
     if (0 == emcmotCommand) {
         rcs_print("USRMOT: ERROR: can't connect to shared memory\n");
 	return EMCMOT_COMM_ERROR_CONNECT;
     }
+
     /* copy entire command structure to shared memory */
+    rtapi_mutex_get(&emcmotStruct->command_mutex);
     *emcmotCommand = *c;
+    rtapi_mutex_give(&emcmotStruct->command_mutex);
+
     /* poll for receipt of command */
     /* set timeout for comm failure, now + timeout */
     end = etime() + EMCMOT_COMM_TIMEOUT;