Expose the interpreter subroutine call stack in task status

configs/common/client.nml

@@ -13,7 +13,7 @@
 
 # Top-level buffers to EMC
 B emcCommand            SHMEM   192.168.0.4       8192    0       0       1       16 1001 TCP=5005 xdr queue confirm_write serial
-B emcStatus             SHMEM   192.168.0.4       10240   0       0       2       16 1002 TCP=5005 xdr
+B emcStatus             SHMEM   192.168.0.4       20480   0       0       2       16 1002 TCP=5005 xdr
 B emcError              SHMEM   192.168.0.4       8192    0       0       3       16 1003 TCP=5005 xdr queue
 
 # Processes

configs/common/server.nml

@@ -10,7 +10,7 @@
 
 # Top-level buffers to EMC
 B emcCommand            SHMEM   localhost       8192    0       0       1       16 1001 TCP=5005 xdr queue confirm_write serial
-B emcStatus             SHMEM   localhost       10240   0       0       2       16 1002 TCP=5005 xdr
+B emcStatus             SHMEM   localhost       20480   0       0       2       16 1002 TCP=5005 xdr
 B emcError              SHMEM   localhost       8192    0       0       3       16 1003 TCP=5005 xdr queue
 
 # Processes

src/emc/nml_intf/emc.cc

@@ -1381,6 +1381,11 @@ void EMC_TASK_STAT::update(CMS * cms)
     cms->update((int *) &execState, 1);
     cms->update((int *) &interpState, 1);
     cms->update(callLevel);
+    for (int i = 0; i < EMC_MAX_CALL_STACK; i++) {
+        cms->update(callStack[i].filename, sizeof(callStack[i].filename));
+        cms->update(callStack[i].subname, sizeof(callStack[i].subname));
+        cms->update(callStack[i].line);
+    }
     cms->update(motionLine);
     cms->update(currentLine);
     cms->update(readLine);

src/emc/nml_intf/emc_nml.hh

@@ -1441,6 +1441,21 @@ class EMC_TASK_STAT_MSG:public RCS_STAT_MSG {
     uint64_t taskbeat;  // milltask's main loop heartbeat counter
 };
 
+// Keep >= INTERP_SUB_ROUTINE_LEVELS (interp_internal.hh); asserted in emctask.cc,
+// which is able to include both headers.
+#define EMC_MAX_CALL_STACK 10
+
+// One frame of the subroutine call stack: "at line <line> of <filename> we
+// called subroutine <subname>".  Sizes are kept tight because EMC_TASK_STAT is
+// copied through the emcStatus NML buffer on every cycle - see the static_assert
+// on sizeof(EMC_STAT) in emcops.cc.
+struct EmcCallFrame {
+    char filename[LINELEN]; // file containing the call site; same PATH_MAX ->
+                            // LINELEN truncation as EMC_TASK_STAT::file
+    char subname[64];       // O-word subroutine name that was called
+    int  line;              // line number of the call site
+};
+
 class EMC_TASK_STAT:public EMC_TASK_STAT_MSG {
   public:
     EMC_TASK_STAT();
@@ -1455,7 +1470,12 @@ class EMC_TASK_STAT:public EMC_TASK_STAT_MSG {
 
     EMC_TASK_EXEC execState;	// EMC_DONE,WAITING_FOR_MOTION, etc.
     EMC_TASK_INTERP interpState;	// EMC_IDLE,READING,PAUSED,WAITING
-    int callLevel;              // current subroutine level - 0 if not in a subroutine, > 0 otherwise
+    // Subroutine depth of the move motion is executing -- 0 if not in a
+    // subroutine.  Like motionLine, this lags the interpreter, which reads ahead.
+    int callLevel;
+    // The call stack of that same move, callLevel frames deep.  callStack[0] is
+    // the call made from the main program; entries >= callLevel are zeroed.
+    EmcCallFrame callStack[EMC_MAX_CALL_STACK];
     int motionLine;		// line motion is executing-- may lag
     int currentLine;		// line currently executing
     int readLine;		// line interpreter has read to

src/emc/nml_intf/emcops.cc

@@ -18,6 +18,15 @@
 #include "emc.hh"
 #include "emc_nml.hh"
 
+// EMC_STAT is written to the emcStatus NML buffer every cycle.  If it outgrows
+// that buffer, NML::write() rejects the message and status silently stops
+// updating -- LinuxCNC then looks hung to every GUI, with nothing failing at
+// build time.  The shipped buffers are 20480 bytes (configs/common/*.nml), less
+// CMS header overhead, so trip the build well before that.
+static_assert(sizeof(EMC_STAT) < 20000,
+              "EMC_STAT outgrew the emcStatus NML buffer; "
+              "see the B emcStatus lines in configs/common/*.nml");
+
 EMC_AXIS_STAT::EMC_AXIS_STAT()
   : EMC_AXIS_STAT_MSG(EMC_AXIS_STAT_TYPE, sizeof(EMC_AXIS_STAT)),
     minPositionLimit(0.0),
@@ -122,6 +131,7 @@ EMC_TASK_STAT::EMC_TASK_STAT()
     execState(EMC_TASK_EXEC::DONE),
     interpState(EMC_TASK_INTERP::IDLE),
     callLevel(0),
+    callStack{},
     motionLine(0),
     currentLine(0),
     readLine(0),

src/emc/nml_intf/state_tag.h

@@ -85,6 +85,11 @@ typedef enum {
     GM_FIELD_M_MODES_4,
     GM_FIELD_ORIGIN,
     GM_FIELD_TOOLCHANGE,
+    /* Id of the interpreter call-stack node for the block this tag came from.
+       Lets task recover the full subroutine call stack of the move motion is
+       executing, long after the interpreter has read past it.  0 means the main
+       program (empty stack).  See Interp::resolve_call_stack_frame(). */
+    GM_FIELD_CALL_STACK_ID,
     GM_FIELD_MAX_FIELDS
 } StateField;
 

src/emc/rs274ngc/interp_base.hh

@@ -49,6 +49,20 @@ public:
     virtual int reset() = 0;
     virtual int line() = 0;
     virtual int call_level() = 0;
+
+    // Recover the subroutine call stack that was active when a block was
+    // interpreted, identified by the call-stack node id its StateTag carries.
+    // This lets task report the call stack of the move motion is *executing*,
+    // which lags the interpreter by the whole read-ahead queue.
+    // resolve_call_stack_depth() returns the depth (0 == main program), or 0 if
+    // the id is no longer known.  resolve_call_stack_frame() fills in one frame
+    // and returns 0 on success, -1 if the id or level cannot be resolved.
+    // Interpreters that do not track subroutine calls keep these defaults.
+    virtual int resolve_call_stack_depth(int /*node_id*/) { return 0; }
+    virtual int resolve_call_stack_frame(int /*node_id*/, int /*level*/,
+                                         const char ** /*filename*/,
+                                         const char ** /*subname*/,
+                                         int * /*line*/) { return -1; }
     virtual char *command(char *buf, size_t buflen) = 0;
     virtual char *file(char *buf, size_t buflen) = 0;
     virtual int on_abort(int reason, const char *message) = 0;

src/emc/rs274ngc/interp_internal.cc

@@ -479,6 +479,95 @@ int Interp::set_probe_data(setup_pointer settings)       //!< pointer to machine
 
 int Interp::call_level(void) { return _setup.call_level; }
 
+// Record a subroutine call made at filename:sequence_number and return the id of
+// the new call-stack node.  Nodes are kept in a ring and each stores its own id,
+// so a node whose slot has since been reused is detected on lookup instead of
+// being reported as some unrelated call.
+int Interp::push_call_stack_node(setup_pointer settings, const char *filename,
+                                 const char *subName, int sequence_number)
+{
+    if (settings->call_stack_nodes.empty())
+        settings->call_stack_nodes.resize(INTERP_CALL_STACK_NODES);
+
+    // id 0 is the reserved root (main program), so it is never handed out
+    if (settings->call_stack_next_id < 1 ||
+        settings->call_stack_next_id == INT_MAX)
+        settings->call_stack_next_id = 1;
+    int id = settings->call_stack_next_id++;
+
+    call_stack_node &node =
+        settings->call_stack_nodes[id % INTERP_CALL_STACK_NODES];
+    node.id = id;
+    node.parent = settings->call_stack_id;
+    node.filename = filename;
+    node.subName = subName;
+    node.sequence_number = sequence_number;
+    return id;
+}
+
+// Look up one node by id, or NULL if the ring slot has been reused since.
+call_stack_node *Interp::find_call_stack_node(int node_id)
+{
+    if (node_id <= 0)   // 0 is the root: the main program has no frames
+        return NULL;
+    if (_setup.call_stack_nodes.empty())
+        return NULL;
+    call_stack_node &node =
+        _setup.call_stack_nodes[node_id % INTERP_CALL_STACK_NODES];
+    if (node.id != node_id)
+        return NULL;
+    return &node;
+}
+
+// Walk a node chain to its root, filling frames[] outermost-first.
+// Returns the depth, or 0 if the chain cannot be resolved in full: a partially
+// resolved stack would be reported against a truncated depth, which is exactly
+// the kind of plausible-but-wrong output this mechanism exists to avoid.
+int Interp::walk_call_stack(int node_id, call_stack_node **frames, int max_frames)
+{
+    call_stack_node *chain[INTERP_SUB_ROUTINE_LEVELS];
+    int depth = 0;
+
+    for (int id = node_id; id > 0; ) {
+        if (depth >= INTERP_SUB_ROUTINE_LEVELS)
+            return 0;   // longer than the interpreter can nest: not a valid chain
+        call_stack_node *node = find_call_stack_node(id);
+        if (node == NULL)
+            return 0;   // slot reused, so this id is older than the ring
+        chain[depth++] = node;
+        id = node->parent;
+    }
+
+    // chain[] came out innermost-first; callers want outermost-first
+    for (int i = 0; i < depth && i < max_frames; i++)
+        frames[i] = chain[depth - 1 - i];
+    return depth;
+}
+
+int Interp::resolve_call_stack_depth(int node_id)
+{
+    call_stack_node *frames[INTERP_SUB_ROUTINE_LEVELS];
+    return walk_call_stack(node_id, frames, INTERP_SUB_ROUTINE_LEVELS);
+}
+
+// Frame[i] records "at line L of file F we called subroutine S", with i == 0
+// being the call made from the main program.
+int Interp::resolve_call_stack_frame(int node_id, int level, const char **filename,
+                                     const char **subname, int *line)
+{
+    call_stack_node *frames[INTERP_SUB_ROUTINE_LEVELS];
+    int depth = walk_call_stack(node_id, frames, INTERP_SUB_ROUTINE_LEVELS);
+
+    if (level < 0 || level >= depth)
+        return -1;
+
+    const call_stack_node *node = frames[level];
+    if (filename) *filename = node->filename ? node->filename : "";
+    if (subname)  *subname  = node->subName  ? node->subName  : "";
+    if (line)     *line     = node->sequence_number;
+    return 0;
+}
+
 std::string toString(GCodes g)
 {
     char buf[15]={};

src/emc/rs274ngc/interp_internal.hh

@@ -20,6 +20,7 @@
 #include <stdio.h>
 #include <set>
 #include <map>
+#include <vector>
 #include <bitset>
 #include "nml_intf/canon.hh"
 #include <emcpos.h>
@@ -622,6 +623,26 @@ struct context_struct {
 #define CONTEXT_RESTORE_ON_RETURN 2 // automatically execute M71 on sub return
 #define REMAP_FRAME   4 // a remap call frame
 
+// Number of call-stack nodes kept for after-the-fact stack resolution.
+// The interpreter reads ahead of motion, so a node
+// must stay resolvable from the moment it is stamped into a StateTag until the
+// corresponding move has finished executing.  16384 nodes covers far more
+// subroutine calls than can be in flight through interp_list plus the motion
+// queue, and costs ~512kB of ordinary (non-realtime) memory.
+#define INTERP_CALL_STACK_NODES 16384
+
+// One entry of the interpreter call stack, recorded as a link to its caller so
+// that a complete stack can be recovered later from a single integer id.
+// Both strings are interned by strstore() and therefore valid for the lifetime
+// of the process, so nodes store pointers rather than copies.
+struct call_stack_node {
+    int id;                   // monotonic id of this node; 0 is the reserved root
+    int parent;               // id of the calling node; 0 at the outermost level
+    const char *filename;     // file containing the call site
+    const char *subName;      // name of the subroutine that was entered
+    int sequence_number;      // line number of the call site
+};
+
 struct offset_struct {
   int type;
   const char *filename;  // the name of the file
@@ -787,6 +808,9 @@ struct setup
   int value_returned;                // the last NGC procedure did/did not return a value
   int call_level;                    // current subroutine level
   context sub_context[INTERP_SUB_ROUTINE_LEVELS];
+  int call_stack_id;                 // id of the current call-stack node, 0 == main
+  int call_stack_next_id;            // next node id to hand out
+  std::vector<call_stack_node> call_stack_nodes;  // ring of INTERP_CALL_STACK_NODES
   int call_state;                  //  enum call_states - indicate Py handler reexecution
   offset_map_type offset_map;      // store label x name, file, line
 

src/emc/rs274ngc/interp_o_word.cc

@@ -679,10 +679,12 @@ int Interp::enter_context(setup_pointer settings, block_pointer block)
 	     settings->call_level, settings->call_level+1, 
 	     call_typenames[block->call_type]);
 
-    settings->call_level++;
-    if (settings->call_level >= INTERP_SUB_ROUTINE_LEVELS) {
+    // check before incrementing: leaving call_level past the end of
+    // sub_context[] would make unwind_call() index out of bounds
+    if (settings->call_level + 1 >= INTERP_SUB_ROUTINE_LEVELS) {
 	ERS(NCE_TOO_MANY_SUBROUTINE_LEVELS);
     }
+    settings->call_level++;
     context_pointer frame = &settings->sub_context[settings->call_level];
     frame->clear();
     // mark frame for finishing remap
@@ -693,6 +695,13 @@ int Interp::enter_context(setup_pointer settings, block_pointer block)
     frame->pystuff.impl->py_return_type = -1;
     // distinguish call frames: oword,m99,python,remap
     frame->call_type = block->call_type;
+    // Record this call so that the stack can be rebuilt later from the node id
+    // stamped into each block's StateTag.  The call site is the caller's current
+    // position, the same values execute_call() stores in the previous frame.
+    settings->call_stack_id = push_call_stack_node(settings,
+						   strstore(settings->filename),
+						   block->o_name,
+						   settings->sequence_number);
     return INTERP_OK;
 }
 
@@ -712,6 +721,13 @@ int Interp::leave_context(setup_pointer settings, bool restore)
     leaving_frame->subName = NULL;
     settings->call_level--;  // drop back
 
+    // pop the call-stack node pushed by enter_context().  The node itself stays
+    // in the ring: moves already queued still refer to it by id.
+    {
+	call_stack_node *node = find_call_stack_node(settings->call_stack_id);
+	settings->call_stack_id = node ? node->parent : 0;
+    }
+
     if (restore && ((leaving_frame->context_status &
 		     (CONTEXT_RESTORE_ON_RETURN|CONTEXT_VALID)) ==
 		    (CONTEXT_RESTORE_ON_RETURN|CONTEXT_VALID))) {

src/emc/rs274ngc/interp_setup.cc

@@ -157,6 +157,9 @@ setup::setup() :
     value_returned(0),
     call_level(0),
     sub_context{},
+    call_stack_id(0),
+    call_stack_next_id(1),
+    call_stack_nodes(),
     call_state(0),
     adaptive_feed(0),
     feed_hold(0),

src/emc/rs274ngc/interp_write.cc

@@ -220,6 +220,10 @@ int Interp::write_state_tag(block_pointer block,
 {
 
     state.fields[GM_FIELD_LINE_NUMBER] = settings->sequence_number;
+    // Carried through segment merging and TP blending, so task can report the
+    // call stack of the move actually being executed rather than the one the
+    // interpreter has since read ahead to.
+    state.fields[GM_FIELD_CALL_STACK_ID] = settings->call_stack_id;
     //FIXME refactor these into setup methods, and maybe put this
     //whole method in setup struct
     bool in_remap = (settings->remap_level > 0);

src/emc/rs274ngc/interpmodule.cc

@@ -628,6 +628,10 @@ static inline int get_call_level (Interp &interp)  {
     return interp._setup.call_level;
 }
 static inline void set_call_level(Interp &interp, int value)  {
+    // sub_context[] is indexed by call_level, so an out-of-range value here
+    // becomes an out-of-bounds access in leave_context()/unwind_call()
+    if (value < 0 || value >= INTERP_SUB_ROUTINE_LEVELS)
+	throw std::out_of_range("call_level out of range");
     interp._setup.call_level = value;
 }
 static inline int get_current_pocket (Interp &interp)  {

src/emc/rs274ngc/rs274ngc_interp.hh

@@ -126,6 +126,9 @@ public:
 
  int line() override { return sequence_number(); }
  int call_level() override;
+ int resolve_call_stack_depth(int node_id) override;
+ int resolve_call_stack_frame(int node_id, int level, const char **filename,
+                              const char **subname, int *line) override;
 
  char *command(char *buf, size_t len) override { line_text(buf, len); return buf; }
 
@@ -574,6 +577,12 @@ int read_dollar(char *line, int *counter, block_pointer block,
  // leave current subroutine context
  int leave_context(setup_pointer settings, bool restore = true);
 
+ // call-stack node bookkeeping, backing resolve_call_stack_*()
+ int push_call_stack_node(setup_pointer settings, const char *filename,
+                          const char *subName, int sequence_number);
+ call_stack_node *find_call_stack_node(int node_id);
+ int walk_call_stack(int node_id, call_stack_node **frames, int max_frames);
+
     //int call_fsm(setup_pointer settings, int event);
     //int execute_pycall(setup_pointer settings, const char *name, int call_phase);
  int execute_call(setup_pointer settings, context_pointer current_frame, int call_type);

src/emc/rs274ngc/rs274ngc_pre.cc

@@ -1219,6 +1219,7 @@ int Interp::init()
 
   // initialization stuff for subroutines and control structures
   _setup.call_level = 0;
+  _setup.call_stack_id = 0;
   _setup.defining_sub = 0;
   _setup.skipping_o = NULL;
   _setup.offset_map.clear();
@@ -1737,6 +1738,10 @@ int Interp::unwind_call(int status, const char *file, int line, const char *func
 	_setup.sub_name = NULL;
     }
     _setup.remap_level = 0; // reset remapping stack
+    // back at the main program; nodes already in the ring stay resolvable for
+    // moves still queued or executing
+    _setup.call_stack_id = 0;
+
     _setup.defining_sub = 0;
     _setup.skipping_o = NULL;
     _setup.skipping_to_sub = NULL;