fix a race in communication between Task and Motion

Fixes #2386

Before this commit, communication of the emcmot_command_t from Task to
Motion had a race condition.  The race could cause Motion to process a
command buffer consisting of some old fields from the previous command
and some new fields from the current command.  The race is demonstrated
by the following sequence of events:

1. Motion starts `emcmotCommandHandler()`.  It gets to command.c line 421
   and reads the (old) `head` and `tail`, sees that they are the same,
   and believes that the command buffer is in a consistent state (i.e.,
   not in the process of being updated).

2. Task starts `usrmotWriteEmcmotCommand()`.  It gets to usrmotintf.cc
   line 96 and starts writing a new command to shared memory.  It writes
   `head` and `commandNum`, but nothing after that.

3. Motion continues, gets to command.c line 425, sees the new
   `commandNum`, and believes that it has a new command.  Motion starts
   processing the command buffer, even though most of the fields haven't
   been updated by Task yet.

4. Task finishes copying the command into the shared memory buffer,
   but it's too late; Motion has processed an inconsistent command.

This commit fixes the race by doing away with the broken `head`/`tail`
synchronization mechanism and replacing it with a mutex in the emcmot
struct, which protects the command struct.

Task locks the mutex before starting to copy the new command to shared
memory.  Motion trylocks the mutex before accessing the command shared
memory.  If the trylock fails, Motion gives up for now and tries again
on the next invocation.

I'm not sure why this bug doesn't cause us more problems - i don't think
i've ever noticed Motion executing a command incorrectly.  The window
for the race is very small, maybe we've just been getting lucky?
Seems fishy...

Also, the head/tail synchronization mechanism probably worked fine back
when it was written, before multi-processor systems were common.

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,9 +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 {
+        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;