From ed7d45e8f564c73f773928a27031c7e214499a6c Mon Sep 17 00:00:00 2001
From: Richard Wackerbarth <rkw@dataplex.net>
Date: Sun, 5 Jul 2015 19:42:13 -0500
Subject: [PATCH] Memory watcher

---
 Marlin/Configuration.h      |   5 +
 Marlin/M99_Free_Mem_Chk.cpp | 277 ++++++++++++++++++++++++++++++++++++
 Marlin/Marlin_main.cpp      |  10 ++
 3 files changed, 292 insertions(+)
 create mode 100644 Marlin/M99_Free_Mem_Chk.cpp

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 9ba9c6ea982..f7baeaa8690 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -612,6 +612,11 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
   #define EEPROM_CHITCHAT // Please keep turned on if you can.
 #endif
 
+//
+// M99 Free Memory Watcher
+//
+#define M99_FREE_MEMORY_WATCHER // uncomment to add the M99 Free Memory Watcher for debug purpose
+
 // @section temperature
 
 // Preheat Constants
diff --git a/Marlin/M99_Free_Mem_Chk.cpp b/Marlin/M99_Free_Mem_Chk.cpp
new file mode 100644
index 00000000000..529ad7ba4d9
--- /dev/null
+++ b/Marlin/M99_Free_Mem_Chk.cpp
@@ -0,0 +1,277 @@
+#define M99_FREE_MEMORY_DUMPER			// Comment out to remove Dump sub-command
+#define M99_FREE_MEMORY_CORRUPTOR		// Comment out to remove Corrupt sub-command
+
+
+// M99 Free Memory Watcher
+//
+// This code watches the free memory block between the bottom of the heap and the top of the stack.
+// This memory block is initialized and watched via the M99 command.
+//
+// M99 I	Initializes the free memory block and prints vitals statistics about the area
+// M99 F	Identifies how much of the free memory block remains free and unused.  It also
+// 		detects and reports any corruption within the free memory block that may have
+// 		happened due to errant firmware.
+// M99 D	Does a hex display of the free memory block along with a flag for any errant
+// 		data that does not match the expected value.
+// M99 C x	Corrupts x locations within the free memory block.   This is useful to check the
+// 		correctness of the M99 F and M99 D commands.
+//
+// Initial version by Roxy-3DPrintBoard
+//
+//
+
+
+#include "Marlin.h"
+
+#ifdef M99_FREE_MEMORY_WATCHER
+extern void *__brkval;
+extern size_t  __heap_start, __heap_end, __flp;
+
+
+//
+// Declare all the functions we need from Marlin_Main.cpp to do the work!
+//
+
+float code_value();
+long code_value_long();
+bool code_seen(char );
+void serial_echopair_P(const char *, float );
+void serial_echopair_P(const char *, double );
+void serial_echopair_P(const char *, unsigned long );
+void serial_echopair_P(const char *, int );
+void serial_echopair_P(const char *, long );
+
+
+
+
+//
+// Utility functions used by M99 to get its work done.
+//
+
+unsigned char *top_of_stack();
+void prt_hex_nibble( unsigned int );
+void prt_hex_byte(unsigned int );
+void prt_hex_word(unsigned int );
+int how_many_E5s_are_here( unsigned char *);
+
+
+
+
+void m99_code()
+{
+static int m99_not_initialized=1;
+unsigned char *sp, *ptr;
+int i, j, n;
+
+//
+// M99 D dumps the free memory block from __brkval to the stack pointer.
+// malloc() eats memory from the start of the block and the stack grows
+// up from the bottom of the block.    Solid 0xE5's indicate nothing has
+// used that memory yet.   There should not be anything but 0xE5's within
+// the block of 0xE5's.  If there is, that would indicate memory corruption
+// probably caused by bad pointers.  Any unexpected values will be flagged in
+// the right hand column to help spotting them.
+//
+
+#ifdef M99_FREE_MEMORY_DUMPER			// Comment out to remove Dump sub-command
+	if ( code_seen('D') ) {
+ 		ptr = (unsigned char *) __brkval;
+
+//
+// We want to start and end the dump on a nice 16 byte boundry even though
+// the values we are using are not 16 byte aligned.
+//
+  		SERIAL_ECHOPGM("\n__brkval : ");
+		prt_hex_word( (unsigned int) ptr );
+  		ptr = (unsigned char *) ((unsigned long) ptr & 0xfff0);
+
+		sp = top_of_stack();
+  		SERIAL_ECHOPGM("\nStack Pointer : ");
+		prt_hex_word( (unsigned int) sp );
+  		SERIAL_ECHOPGM("\n");
+
+		sp = (unsigned char *) ((unsigned long) sp | 0x000f);
+		n = sp - ptr;
+//
+// This is the main loop of the Dump command.
+//
+		while ( ptr < sp ) {
+			prt_hex_word( (unsigned int) ptr);	// Print the address
+  			SERIAL_ECHOPGM(":");
+			for(i=0; i<16; i++) {			// and 16 data bytes
+				prt_hex_byte( *(ptr+i));
+  				SERIAL_ECHOPGM(" ");
+				delay(2);
+			}
+
+  			SERIAL_ECHO("|");   			// now show where non 0xE5's are
+			for(i=0; i<16; i++) {
+				delay(2);
+				if ( *(ptr+i)==0xe5)
+  					SERIAL_ECHOPGM(" ");
+				else
+  					SERIAL_ECHOPGM("?");
+			}
+  			SERIAL_ECHO("\n");
+
+			ptr += 16;
+			delay(2);
+		}
+  		SERIAL_ECHOLNPGM("Done.\n");
+		return;
+	}
+#endif
+
+//
+// M99 F   requests the code to return the number of free bytes in the memory pool along with
+// other vital statistics that define the memory pool.
+//
+	if ( code_seen('F') ) {
+	int max_addr = (int) __brkval;
+	int max_cnt = 0;
+	int block_cnt = 0;
+  		ptr = (unsigned char *) __brkval;
+		sp = top_of_stack();
+		n = sp - ptr;
+
+// Scan through the range looking for the biggest block of 0xE5's we can find
+
+		for(i=0; i<n; i++) {
+			if ( *(ptr+i) == (unsigned char) 0xe5) {
+				j = how_many_E5s_are_here( (unsigned char *) ptr+i );
+				if ( j>8) {
+ 					SERIAL_ECHOPAIR("Found ", j );
+ 					SERIAL_ECHOPGM(" bytes free at 0x");
+					prt_hex_word( (int) ptr+i );
+ 					SERIAL_ECHOPGM("\n");
+					i += j;
+				        block_cnt++;
+				}
+				if ( j>max_cnt) {			// We don't do anything with this information yet
+					max_cnt  = j;			// but we do know where the biggest free memory block is.
+					max_addr = (int) ptr+i;
+				}
+			}
+		}
+		if (block_cnt>1)
+  			SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.\n");
+
+  		SERIAL_ECHO("\nDone.\n");
+		return;
+	}
+//
+// M99 C x  Corrupts x locations in the free memory pool and reports the locations of the corruption.
+// This is useful to check the correctness of the M99 D and the M99 F commands.
+//
+#ifdef M99_FREE_MEMORY_CORRUPTOR
+	if ( code_seen('C') ) {
+		int x;			// x gets the # of locations to corrupt within the memory pool
+		x = code_value();
+  		SERIAL_ECHOLNPGM("Corrupting free memory block.\n");
+  		ptr = (unsigned char *) __brkval;
+  		SERIAL_ECHOPAIR("\n__brkval : ",(long) ptr );
+  		ptr += 8;
+
+		sp = top_of_stack();
+  		SERIAL_ECHOPAIR("\nStack Pointer : ",(long) sp );
+  		SERIAL_ECHOLNPGM("\n");
+
+		n = sp - ptr - 64;  	// -64 just to keep us from finding interrupt activity that
+	       				// has altered the stack.
+		j = n / (x+1);
+		for(i=1; i<=x; i++) {
+			*(ptr+(i*j)) = i;
+  			SERIAL_ECHO("\nCorrupting address: 0x");
+		      	prt_hex_word( (unsigned int)  (ptr+(i*j)) );
+		}
+  		SERIAL_ECHOLNPGM("\n");
+		return;
+	}
+#endif
+
+//
+// M99 I    Initializes the free memory pool so it can be watched and prints vital
+// statistics that define the free memory pool.
+//
+	if (m99_not_initialized || code_seen('I') ) {				// If no sub-command is specified, the first time
+  		SERIAL_ECHOLNPGM("Initializing free memory block.\n");   	// this happens, it will Initialize.
+  		ptr = (unsigned char *) __brkval;				// Repeated M99 with no sub-command will not destroy the
+  		SERIAL_ECHOPAIR("\n__brkval : ",(long) ptr );			// state of the initialized free memory pool.
+  		ptr += 8;
+
+		sp = top_of_stack();
+  		SERIAL_ECHOPAIR("\nStack Pointer : ",(long) sp );
+  		SERIAL_ECHOLNPGM("\n");
+
+		n = sp - ptr - 64;  	// -64 just to keep us from finding interrupt activity that
+	       				// has altered the stack.
+
+  		SERIAL_ECHO( n );
+  		SERIAL_ECHOLNPGM(" bytes of memory initialized.\n");
+
+		for(i=0; i<n; i++)
+			*(ptr+i) = (unsigned char) 0xe5;
+
+		for(i=0; i<n; i++) {
+			if ( *(ptr+i) != (unsigned char) 0xe5 ) {
+  				SERIAL_ECHOPAIR("? address : ", (unsigned long) ptr+i );
+  				SERIAL_ECHOPAIR("=", *(ptr+i) );
+  				SERIAL_ECHOLNPGM("\n");
+			}
+		}
+		m99_not_initialized = 0;
+  		SERIAL_ECHOLNPGM("Done.\n");
+		return;
+	}
+	return;
+}
+
+// top_of_stack() returns the location of a variable on its stack frame.  The value returned is above
+// the stack once the function returns to the caller.
+
+unsigned char *top_of_stack() {
+unsigned char x;
+	return &x;
+}
+
+//
+// 3 support routines to print hex numbers.  We can print a nibble, byte and word
+//
+
+void prt_hex_nibble( unsigned int n )
+{
+	if ( n <= 9 )
+		SERIAL_ECHO(n);
+	else
+		SERIAL_ECHO( (char) ('A'+n-10) );
+	delay(2);
+}
+
+void prt_hex_byte(unsigned int b)
+{
+	prt_hex_nibble( ( b & 0xf0 ) >> 4 );
+	prt_hex_nibble(  b & 0x0f );
+}
+
+void prt_hex_word(unsigned int w)
+{
+	prt_hex_byte( ( w & 0xff00 ) >> 8 );
+	prt_hex_byte(  w & 0x0ff );
+}
+
+// how_many_E5s_are_here() is a utility function to easily find out how many 0xE5's are
+// at the specified location.  Having this logic as a function simplifies the search code.
+//
+int how_many_E5s_are_here( unsigned char *p)
+{
+int n;
+
+	for(n=0; n<32000; n++) {
+		if ( *(p+n) != (unsigned char) 0xe5)
+			return n-1;
+	}
+	return -1;
+}
+
+#endif
+
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 09e559f1634..5e7ea46ab3b 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -225,6 +225,10 @@
  *
  */
 
+#ifdef M99_FREE_MEMORY_WATCHER
+  void m99_code();
+#endif
+
 #ifdef SDSUPPORT
   CardReader card;
 #endif
@@ -5372,6 +5376,12 @@ void process_next_command() {
           break;
       #endif // ENABLE_AUTO_BED_LEVELING && Z_PROBE_REPEATABILITY_TEST
 
+      #ifdef M99_FREE_MEMORY_WATCHER
+        case 99:
+          m99_code();
+          break;
+      #endif
+
       case 104: // M104
         gcode_M104();
         break;