diff --git a/.gitmodules b/.gitmodules
index e6f7678..e69de29 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,30 +0,0 @@
-[submodule "src/grbl"]
- path = src/grbl
- url = https://github.com/grblHAL/core
-[submodule "src/odometer"]
- path = src/odometer
- url = https://github.com/grblHAL/Plugin_odometer
-[submodule "src/laser"]
- path = src/laser
- url = https://github.com/grblHAL/Plugins_laser
-[submodule "src/encoder"]
- path = src/encoder
- url = https://github.com/grblHAL/Plugin_encoder
-[submodule "src/keypad"]
- path = src/keypad
- url = https://github.com/grblHAL/Plugin_I2C_keypad
-[submodule "src/networking"]
- path = src/networking
- url = https://github.com/grblHAL/Plugin_networking
-[submodule "src/sdcard"]
- path = src/sdcard
- url = https://github.com/grblHAL/Plugin_SD_card
-[submodule "src/spindle"]
- path = src/spindle
- url = https://github.com/grblHAL/Plugins_spindle
-[submodule "src/plasma"]
- path = src/plasma
- url = https://github.com/grblHAL/Plugin_plasma
-[submodule "src/eeprom"]
- path = src/eeprom
- url = https://github.com/grblHAL/Plugin_EEPROM
diff --git a/examples/grblHAL_Teensy4_Upload/grblHAL_Teensy4_Upload.ino b/grblHAL_Teensy4/examples/grblHAL_Teensy4_Upload/grblHAL_Teensy4_Upload.ino
similarity index 100%
rename from examples/grblHAL_Teensy4_Upload/grblHAL_Teensy4_Upload.ino
rename to grblHAL_Teensy4/examples/grblHAL_Teensy4_Upload/grblHAL_Teensy4_Upload.ino
diff --git a/grblHAL_Teensy4/library.json b/grblHAL_Teensy4/library.json
new file mode 100644
index 0000000..3ddec62
--- /dev/null
+++ b/grblHAL_Teensy4/library.json
@@ -0,0 +1,17 @@
+{
+ "name": "grblHAL for Teensy 4.x",
+ "frameworks": "Arduino",
+ "platforms": "Twensy 4.0 - 4.1",
+ "keywords": "grbl, CNC",
+ "description": "grblHAL for Teensy 4.x",
+ "version": "1.1.7",
+ "authors":
+ { "name": "Terje Io",
+ "maintainer": true
+ },
+ "repository":
+ { "type": "git",
+ "url": "https://github.com/terjeio/grblHAL"
+ },
+ "examples": "examples/*/*.ino"
+}
diff --git a/grblHAL_Teensy4/library.properties b/grblHAL_Teensy4/library.properties
new file mode 100644
index 0000000..3be2198
--- /dev/null
+++ b/grblHAL_Teensy4/library.properties
@@ -0,0 +1,9 @@
+name=grblHAL for Teensy 4.x
+version=1.1.7
+author=Terje Io
+maintainer=Terje Io
+sentence=grblHAL for Teensy 4.x
+paragraph=
+category=Other
+url=https://github.com/terjeio/grblHAL
+architectures=*
diff --git a/grblHAL_Teensy4/src/.cproject b/grblHAL_Teensy4/src/.cproject
new file mode 100644
index 0000000..9cbf287
--- /dev/null
+++ b/grblHAL_Teensy4/src/.cproject
@@ -0,0 +1,115 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ make
+
+ iMXRT1062
+ true
+ true
+ true
+
+
+
+
diff --git a/grblHAL_Teensy4/src/.project b/grblHAL_Teensy4/src/.project
new file mode 100644
index 0000000..dc514aa
--- /dev/null
+++ b/grblHAL_Teensy4/src/.project
@@ -0,0 +1,45 @@
+
+
+ Driver Teensy4
+
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.genmakebuilder
+ clean,full,incremental,
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder
+ full,incremental,
+
+
+
+
+
+ org.eclipse.cdt.core.cnature
+ org.eclipse.cdt.core.ccnature
+ org.eclipse.cdt.managedbuilder.core.managedBuildNature
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigNature
+
+
+
+ ARDUINOPATH
+ file:/C:/Program%20Files%20(x86)/Arduino
+
+
+ COMPILER_LOC
+ file:/C:/Program%20Files%20(x86)/Arduino/hardware/tools/arm/arm-none-eabi
+
+
+ ETHERNET_LIB
+ file:/C:/users/terjeio/Documents/Arduino/libraries/teensy41_ethernet-master/src
+
+
+ TEENSY4_CORE
+ file:/C:/Program%20Files%20(x86)/Arduino/hardware/teensy/avr/cores/teensy4
+
+
+
diff --git a/grblHAL_Teensy4/src/.settings/language.settings.xml b/grblHAL_Teensy4/src/.settings/language.settings.xml
new file mode 100644
index 0000000..7f5978c
--- /dev/null
+++ b/grblHAL_Teensy4/src/.settings/language.settings.xml
@@ -0,0 +1,15 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/grblHAL_Teensy4/src/.settings/org.eclipse.cdt.codan.core.prefs b/grblHAL_Teensy4/src/.settings/org.eclipse.cdt.codan.core.prefs
new file mode 100644
index 0000000..59c0b37
--- /dev/null
+++ b/grblHAL_Teensy4/src/.settings/org.eclipse.cdt.codan.core.prefs
@@ -0,0 +1,93 @@
+eclipse.preferences.version=1
+org.eclipse.cdt.codan.checkers.errnoreturn=Warning
+org.eclipse.cdt.codan.checkers.errnoreturn.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"No return\\")",implicit\=>false}
+org.eclipse.cdt.codan.checkers.errreturnvalue=Error
+org.eclipse.cdt.codan.checkers.errreturnvalue.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Unused return value\\")"}
+org.eclipse.cdt.codan.checkers.nocommentinside=-Error
+org.eclipse.cdt.codan.checkers.nocommentinside.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Nesting comments\\")"}
+org.eclipse.cdt.codan.checkers.nolinecomment=-Error
+org.eclipse.cdt.codan.checkers.nolinecomment.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Line comments\\")"}
+org.eclipse.cdt.codan.checkers.noreturn=Error
+org.eclipse.cdt.codan.checkers.noreturn.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"No return value\\")",implicit\=>false}
+org.eclipse.cdt.codan.internal.checkers.AbstractClassCreation=Error
+org.eclipse.cdt.codan.internal.checkers.AbstractClassCreation.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Abstract class cannot be instantiated\\")"}
+org.eclipse.cdt.codan.internal.checkers.AmbiguousProblem=Error
+org.eclipse.cdt.codan.internal.checkers.AmbiguousProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Ambiguous problem\\")"}
+org.eclipse.cdt.codan.internal.checkers.AssignmentInConditionProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.AssignmentInConditionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Assignment in condition\\")"}
+org.eclipse.cdt.codan.internal.checkers.AssignmentToItselfProblem=Error
+org.eclipse.cdt.codan.internal.checkers.AssignmentToItselfProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Assignment to itself\\")"}
+org.eclipse.cdt.codan.internal.checkers.CStyleCastProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.CStyleCastProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"C-Style cast instead of C++ cast\\")"}
+org.eclipse.cdt.codan.internal.checkers.CaseBreakProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.CaseBreakProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"No break at end of case\\")",no_break_comment\=>"no break",last_case_param\=>false,empty_case_param\=>false,enable_fallthrough_quickfix_param\=>false}
+org.eclipse.cdt.codan.internal.checkers.CatchByReference=Warning
+org.eclipse.cdt.codan.internal.checkers.CatchByReference.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Catching by reference is recommended\\")",unknown\=>false,exceptions\=>()}
+org.eclipse.cdt.codan.internal.checkers.CircularReferenceProblem=Error
+org.eclipse.cdt.codan.internal.checkers.CircularReferenceProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Circular inheritance\\")"}
+org.eclipse.cdt.codan.internal.checkers.ClassMembersInitialization=Warning
+org.eclipse.cdt.codan.internal.checkers.ClassMembersInitialization.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Class members should be properly initialized\\")",skip\=>true}
+org.eclipse.cdt.codan.internal.checkers.CopyrightProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.CopyrightProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Lack of copyright information\\")",regex\=>".*Copyright.*"}
+org.eclipse.cdt.codan.internal.checkers.DecltypeAutoProblem=Error
+org.eclipse.cdt.codan.internal.checkers.DecltypeAutoProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid 'decltype(auto)' specifier\\")"}
+org.eclipse.cdt.codan.internal.checkers.FieldResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.FieldResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Field cannot be resolved\\")"}
+org.eclipse.cdt.codan.internal.checkers.FunctionResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.FunctionResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Function cannot be resolved\\")"}
+org.eclipse.cdt.codan.internal.checkers.GotoStatementProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.GotoStatementProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Goto statement used\\")"}
+org.eclipse.cdt.codan.internal.checkers.InvalidArguments=Error
+org.eclipse.cdt.codan.internal.checkers.InvalidArguments.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid arguments\\")"}
+org.eclipse.cdt.codan.internal.checkers.InvalidTemplateArgumentsProblem=Error
+org.eclipse.cdt.codan.internal.checkers.InvalidTemplateArgumentsProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid template argument\\")"}
+org.eclipse.cdt.codan.internal.checkers.LabelStatementNotFoundProblem=Error
+org.eclipse.cdt.codan.internal.checkers.LabelStatementNotFoundProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Label statement not found\\")"}
+org.eclipse.cdt.codan.internal.checkers.MemberDeclarationNotFoundProblem=Error
+org.eclipse.cdt.codan.internal.checkers.MemberDeclarationNotFoundProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Member declaration not found\\")"}
+org.eclipse.cdt.codan.internal.checkers.MethodResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.MethodResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Method cannot be resolved\\")"}
+org.eclipse.cdt.codan.internal.checkers.MissCaseProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.MissCaseProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Missing cases in switch\\")"}
+org.eclipse.cdt.codan.internal.checkers.MissDefaultProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.MissDefaultProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Missing default in switch\\")",defaultWithAllEnums\=>false}
+org.eclipse.cdt.codan.internal.checkers.MissReferenceProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.MissReferenceProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Missing reference return value in assignment operator\\")"}
+org.eclipse.cdt.codan.internal.checkers.MissSelfCheckProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.MissSelfCheckProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Missing self check in assignment operator\\")"}
+org.eclipse.cdt.codan.internal.checkers.NamingConventionFunctionChecker=-Info
+org.eclipse.cdt.codan.internal.checkers.NamingConventionFunctionChecker.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Name convention for function\\")",pattern\=>"^[a-z]",macro\=>true,exceptions\=>()}
+org.eclipse.cdt.codan.internal.checkers.NonVirtualDestructorProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.NonVirtualDestructorProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Class has a virtual method and non-virtual destructor\\")"}
+org.eclipse.cdt.codan.internal.checkers.OverloadProblem=Error
+org.eclipse.cdt.codan.internal.checkers.OverloadProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid overload\\")"}
+org.eclipse.cdt.codan.internal.checkers.RedeclarationProblem=Error
+org.eclipse.cdt.codan.internal.checkers.RedeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid redeclaration\\")"}
+org.eclipse.cdt.codan.internal.checkers.RedefinitionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.RedefinitionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid redefinition\\")"}
+org.eclipse.cdt.codan.internal.checkers.ReturnStyleProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.ReturnStyleProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Return with parenthesis\\")"}
+org.eclipse.cdt.codan.internal.checkers.ScanfFormatStringSecurityProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.ScanfFormatStringSecurityProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Format String Vulnerability\\")"}
+org.eclipse.cdt.codan.internal.checkers.StatementHasNoEffectProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.StatementHasNoEffectProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Statement has no effect\\")",macro\=>true,exceptions\=>()}
+org.eclipse.cdt.codan.internal.checkers.SuggestedParenthesisProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.SuggestedParenthesisProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Suggested parenthesis around expression\\")",paramNot\=>false}
+org.eclipse.cdt.codan.internal.checkers.SuspiciousSemicolonProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.SuspiciousSemicolonProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Suspicious semicolon\\")",else\=>false,afterelse\=>false}
+org.eclipse.cdt.codan.internal.checkers.TypeResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.TypeResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Type cannot be resolved\\")"}
+org.eclipse.cdt.codan.internal.checkers.UnusedFunctionDeclarationProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.UnusedFunctionDeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Unused function declaration\\")",macro\=>true}
+org.eclipse.cdt.codan.internal.checkers.UnusedStaticFunctionProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.UnusedStaticFunctionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Unused static function\\")",macro\=>true}
+org.eclipse.cdt.codan.internal.checkers.UnusedVariableDeclarationProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.UnusedVariableDeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Unused variable declaration in file scope\\")",macro\=>true,exceptions\=>("@(\#)","$Id")}
+org.eclipse.cdt.codan.internal.checkers.UsingInHeaderProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.UsingInHeaderProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Using directive in header\\")"}
+org.eclipse.cdt.codan.internal.checkers.VariableResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.VariableResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Symbol is not resolved\\")"}
+org.eclipse.cdt.codan.internal.checkers.VirtualMethodCallProblem=-Error
+org.eclipse.cdt.codan.internal.checkers.VirtualMethodCallProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Virtual method call in constructor/destructor\\")"}
+org.eclipse.cdt.qt.core.qtproblem=Warning
+org.eclipse.cdt.qt.core.qtproblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_ON_FILE_OPEN\=>true,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>null}
diff --git a/grblHAL_Teensy4/src/T40X101_map.h b/grblHAL_Teensy4/src/T40X101_map.h
new file mode 100644
index 0000000..60c1634
--- /dev/null
+++ b/grblHAL_Teensy4/src/T40X101_map.h
@@ -0,0 +1,118 @@
+/*
+ T40X101_map.h - driver code for IMXRT1062 processor (on Teensy 4.0 board)
+
+ Part of grblHAL
+
+ Board by Phil Barrett: https://github.com/phil-barrett/grblHAL-teensy-4.x
+
+ Copyright (c) 2020 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+#define BOARD_NAME "T40X101"
+
+#if N_AXIS > 4
+#error Max number of axes is 4 for T40X101
+#endif
+
+#if QEI_ENABLE
+#error No pins available for encoder input!
+#endif
+
+// Default pin assignments allow only one axis to be ganged or auto squared.
+// A axis pin numbers are used for the ganged/auto squared axis.
+// If a second axis is to be ganged/auto squared pin assignments needs to be changed!
+// Set to 1 to enable, 0 to disable.
+#define X_GANGED 0
+#define X_AUTO_SQUARE 0
+#define Y_GANGED 0
+#define Y_AUTO_SQUARE 0
+#define Z_GANGED 0
+#define Z_AUTO_SQUARE 0
+//
+
+#define X_STEP_PIN (2u)
+#define X_DIRECTION_PIN (3u)
+#define X_LIMIT_PIN (20u)
+
+#if X_GANGED || X_AUTO_SQUARE
+#define X2_STEP_PIN (8u)
+#define X2_DIRECTION_PIN (9u)
+#if X_AUTO_SQUARE
+ #define X2_LIMIT_PIN (23u)
+#endif
+#endif
+
+#define Y_STEP_PIN (4u)
+#define Y_DIRECTION_PIN (5u)
+#define Y_LIMIT_PIN (21u)
+
+#if Y_GANGED || Y_AUTO_SQUARE
+#define Y2_STEP_PIN (8u)
+#define Y2_DIRECTION_PIN (9u)
+#if Y_AUTO_SQUARE
+ #define Y2_LIMIT_PIN (23u)
+#endif
+#endif
+
+#define Z_STEP_PIN (6u)
+#define Z_DIRECTION_PIN (7u)
+#define Z_LIMIT_PIN (22u)
+
+#if Z_GANGED || Z_AUTO_SQUARE
+#define Z2_STEP_PIN (8u)
+#define Z2_DIRECTION_PIN (9u)
+#define Z2_LIMIT_PIN (23u)
+#endif
+
+#if N_AXIS > 3
+#define A_STEP_PIN (8u)
+#define A_DIRECTION_PIN (9u)
+#define A_LIMIT_PIN (23u)
+#endif
+
+#if N_AXIS > 4
+#define B_STEP_PIN (26u)
+#define B_DIRECTION_PIN (27u)
+#define B_ENABLE_PIN (37u)
+#define B_LIMIT_PIN (28u)
+#endif
+
+// Define stepper driver enable/disable output pin(s).
+#define STEPPERS_ENABLE_PIN (10u)
+
+// Define spindle enable and spindle direction output pins.
+#define SPINDLE_ENABLE_PIN (12u)
+#define SPINDLE_DIRECTION_PIN (11u)
+#define SPINDLEPWMPIN (13u) // NOTE: only pin 12 or pin 13 can be assigned!
+
+// Define flood and mist coolant enable output pins.
+#define COOLANT_FLOOD_PIN (19u)
+#define COOLANT_MIST_PIN (18u)
+
+// Define user-control CONTROLs (cycle start, reset, feed hold, door) input pins.
+#define RESET_PIN (14u)
+#define FEED_HOLD_PIN (16u)
+#define CYCLE_START_PIN (17u)
+#define SAFETY_DOOR_PIN (29u)
+
+// Define probe switch input pin.
+#define PROBE_PIN (15U)
+
+#if EEPROM_ENABLE || KEYPAD_ENABLE
+#define I2C_PORT 4
+#define I2C_SCL4 (24u) // Not used, for info only
+#define I2C_SDA4 (25u) // Not used, for info only
+#endif
diff --git a/grblHAL_Teensy4/src/T41U5XBB.c b/grblHAL_Teensy4/src/T41U5XBB.c
new file mode 100644
index 0000000..b85d35f
--- /dev/null
+++ b/grblHAL_Teensy4/src/T41U5XBB.c
@@ -0,0 +1,194 @@
+/*
+ T41U5XBB.c - driver code for IMXRT1062 processor (on Teensy 4.1 board)
+
+ Part of grblHAL
+
+ Board by Phil Barrett: https://github.com/phil-barrett/grblHAL-teensy-4.x
+
+ Copyright (c) 2020-2021 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+#include "driver.h"
+
+#ifdef BOARD_T41U5XBB
+
+//#include "Arduino.h"
+#include
+#include
+
+#include "grbl/protocol.h"
+
+static gpio_t stx[AUX_N_IN];
+static gpio_t aux_out[AUX_N_OUT];
+
+static void aux_settings_load (void);
+static status_code_t aux_set_invert_out (setting_id_t id, uint_fast16_t int_value);
+static uint32_t aux_get_invert_out (setting_id_t setting);
+
+static input_signal_t aux_in[] = {
+ { .id = Input_Aux0, .port = &stx[0], .pin = AUXINPUT0_PIN, .group = 0 }
+#if AUX_N_IN == 4
+ , { .id = Input_Aux1, .port = &stx[1], .pin = AUXINPUT1_PIN, .group = 0 },
+ { .id = Input_Aux2, .port = &stx[2], .pin = AUXINPUT2_PIN, .group = 0 },
+ { .id = Input_Aux3, .port = &stx[3], .pin = AUXINPUT3_PIN, .group = 0 }
+#endif
+};
+
+static const setting_group_detail_t aux_groups[] = {
+ { Group_Root, Group_AuxPorts, "Aux ports"}
+};
+
+static const setting_detail_t aux_settings[] = {
+#if AUX_N_IN == 4
+ { Settings_IoPort_InvertIn, Group_AuxPorts, "Invert I/O Port inputs", NULL, Format_Bitfield, "Port 0,Port 1,Port 2,Port 3", NULL, NULL, Setting_NonCore, &settings.ioport.invert_in.mask },
+#else
+ { Settings_IoPort_InvertIn, Group_AuxPorts, "Invert I/O Port inputs", NULL, Format_Bitfield, "Port 0", NULL, NULL, Setting_NonCore, &settings.ioport.invert_in.mask },
+#endif
+ { Settings_IoPort_InvertOut, Group_AuxPorts, "Invert I/O Port outputs", NULL, Format_Bitfield, "Port 0,Port 1,Port 2", NULL, NULL, Setting_NonCoreFn, aux_set_invert_out, aux_get_invert_out },
+};
+
+static setting_details_t details = {
+ .groups = aux_groups,
+ .n_groups = sizeof(aux_groups) / sizeof(setting_group_detail_t),
+ .settings = aux_settings,
+ .n_settings = sizeof(aux_settings) / sizeof(setting_detail_t),
+ .load = aux_settings_load,
+ .save = settings_write_global
+};
+
+static setting_details_t *on_get_settings (void)
+{
+ return &details;
+}
+
+static void aux_settings_load (void)
+{
+ uint_fast8_t idx = AUX_N_OUT;
+
+ do {
+ idx--;
+ DIGITAL_OUT(aux_out[idx], (settings.ioport.invert_out.mask >> idx) & 0x01);
+ } while(idx);
+}
+
+static status_code_t aux_set_invert_out (setting_id_t id, uint_fast16_t value)
+{
+ ioport_bus_t invert;
+ invert.mask = (uint8_t)value & AUX_OUT_MASK;
+
+ if(invert.mask != settings.ioport.invert_out.mask) {
+ uint_fast8_t idx = AUX_N_OUT;
+ do {
+ idx--;
+ if(((settings.ioport.invert_out.mask >> idx) & 0x01) != ((invert.mask >> idx) & 0x01))
+ DIGITAL_OUT(aux_out[idx], !DIGITAL_IN(aux_out[idx]));
+ } while(idx);
+
+ settings.ioport.invert_out.mask = invert.mask;
+ }
+
+ return Status_OK;
+}
+
+static uint32_t aux_get_invert_out (setting_id_t setting)
+{
+ return settings.ioport.invert_out.mask;
+}
+
+static void digital_out (uint8_t port, bool on)
+{
+ if(port < AUX_N_OUT)
+ DIGITAL_OUT(aux_out[port], ((settings.ioport.invert_out.mask >> port) & 0x01) ? !on : on);
+}
+
+inline static __attribute__((always_inline)) int32_t get_input (gpio_t *gpio, bool invert, wait_mode_t wait_mode, float timeout)
+{
+ if(wait_mode == WaitMode_Immediate)
+ return !!(gpio->reg->DR & gpio->bit) ^ invert;
+
+ uint_fast16_t delay = (uint_fast16_t)ceilf((1000.0f / 50.0f) * timeout) + 1;
+
+ bool wait_for = wait_mode != WaitMode_Low;
+
+ do {
+ if((!!(gpio->reg->DR & gpio->bit) ^ invert) == wait_for)
+ return !!(gpio->reg->DR & gpio->bit);
+
+ if(delay) {
+ protocol_execute_realtime();
+ hal.delay_ms(50, NULL);
+ } else
+ break;
+ } while(--delay && !sys.abort);
+
+ return -1;
+}
+
+static int32_t wait_on_input (bool digital, uint8_t port, wait_mode_t wait_mode, float timeout)
+{
+ int32_t value = -1;
+
+ if(digital) {
+ if(port < AUX_N_IN)
+ value = get_input(aux_in[port].port, (settings.ioport.invert_in.mask << port) & 0x01, wait_mode, timeout);
+ }
+// else if(port == 0)
+// value = analogRead(41);
+/*
+ hal.stream.write("[MSG:AUX");
+ hal.stream.write(uitoa(port));
+ hal.stream.write("=");
+ hal.stream.write(value == -1 ? "fail" : uitoa(value));
+ hal.stream.write("]" ASCII_EOL);
+*/
+ return value;
+}
+
+void board_init (void)
+{
+ hal.port.digital_out = digital_out;
+ hal.port.wait_on_input = wait_on_input;
+// hal.port.num_analog_in = 1;
+ hal.port.num_digital_in = AUX_N_IN;
+ hal.port.num_digital_out = AUX_N_OUT;
+
+ details.on_get_settings = grbl.on_get_settings;
+ grbl.on_get_settings = on_get_settings;
+
+ bool pullup;
+ uint32_t i = AUX_N_IN;
+ input_signal_t *signal;
+ do {
+ pullup = true; //??
+ signal = &aux_in[--i];
+ signal->irq_mode = IRQ_Mode_None;
+
+ pinMode(signal->pin, pullup ? INPUT_PULLUP : INPUT_PULLDOWN);
+ signal->gpio.reg = (gpio_reg_t *)digital_pin_to_info_PGM[signal->pin].reg;
+ signal->gpio.bit = digital_pin_to_info_PGM[signal->pin].mask;
+
+ if(signal->port != NULL)
+ memcpy(signal->port, &signal->gpio, sizeof(gpio_t));
+ } while(i);
+
+ pinModeOutput(&aux_out[0], AUXOUTPUT0_PIN);
+ pinModeOutput(&aux_out[1], AUXOUTPUT1_PIN);
+ pinModeOutput(&aux_out[2], AUXOUTPUT2_PIN);
+
+// analog_init();
+}
+
+#endif
diff --git a/grblHAL_Teensy4/src/T41U5XBB_map.h b/grblHAL_Teensy4/src/T41U5XBB_map.h
new file mode 100644
index 0000000..d17b420
--- /dev/null
+++ b/grblHAL_Teensy4/src/T41U5XBB_map.h
@@ -0,0 +1,150 @@
+/*
+ T41U5XBB_map.h - driver code for IMXRT1062 processor (on Teensy 4.1 board)
+
+ Part of grblHAL
+
+ Board by Phil Barrett: https://github.com/phil-barrett/grblHAL-teensy-4.x
+
+ Copyright (c) 2020 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+#define BOARD_NAME "T41U5XBB"
+#define HAS_BOARD_INIT
+
+void board_init (void);
+
+#if N_AXIS > 5
+#error Max number of axes is 5 for T41U5XBB
+#endif
+
+// Default pin assignments allow only one axis to be ganged or auto squared.
+// B axis pin numbers are used for the ganged/auto squared axis.
+// If a second axis is to be ganged/auto squared pin assignments needs to be changed!
+// Set to 1 to enable, 0 to disable.
+#define X_GANGED 0
+#define X_AUTO_SQUARE 0
+#define Y_GANGED 0
+#define Y_AUTO_SQUARE 0
+#define Z_GANGED 0
+#define Z_AUTO_SQUARE 0
+//
+
+#define X_STEP_PIN (2u)
+#define X_DIRECTION_PIN (3u)
+#define X_ENABLE_PIN (10u)
+#define X_LIMIT_PIN (20u)
+
+#if X_GANGED || X_AUTO_SQUARE
+#define X2_STEP_PIN (26u)
+#define X2_DIRECTION_PIN (27u)
+#define X2_ENABLE_PIN (37u)
+#if X_AUTO_SQUARE
+ #define X2_LIMIT_PIN (28u)
+#endif
+#endif
+
+#define Y_STEP_PIN (4u)
+#define Y_DIRECTION_PIN (5u)
+#define Y_ENABLE_PIN (40u)
+#define Y_LIMIT_PIN (21u)
+
+#if Y_GANGED || Y_AUTO_SQUARE
+#define Y2_STEP_PIN (26u)
+#define Y2_DIRECTION_PIN (27u)
+#define Y2_ENABLE_PIN (37u)
+#if Y_AUTO_SQUARE
+ #define Y2_LIMIT_PIN (28u)
+#endif
+#endif
+
+#define Z_STEP_PIN (6u)
+#define Z_DIRECTION_PIN (7u)
+#define Z_ENABLE_PIN (39u)
+#define Z_LIMIT_PIN (22u)
+
+#if Z_GANGED || Z_AUTO_SQUARE
+#define Z2_STEP_PIN (26u)
+#define Z2_DIRECTION_PIN (27u)
+#define Z2_ENABLE_PIN (37u)
+#define Z2_LIMIT_PIN (28u)
+#endif
+
+#if N_AXIS > 3
+#define A_STEP_PIN (8u)
+#define A_DIRECTION_PIN (9u)
+#define A_ENABLE_PIN (38u)
+#define A_LIMIT_PIN (23u)
+#endif
+
+#if N_AXIS > 4
+#define B_STEP_PIN (26u)
+#define B_DIRECTION_PIN (27u)
+#define B_ENABLE_PIN (37u)
+#define B_LIMIT_PIN (28u)
+#endif
+
+// Define spindle enable and spindle direction output pins.
+#define SPINDLE_ENABLE_PIN (12u)
+#define SPINDLE_DIRECTION_PIN (11u)
+#define SPINDLEPWMPIN (13u) // NOTE: only pin 12 or pin 13 can be assigned!
+
+// Define flood and mist coolant enable output pins.
+#define COOLANT_FLOOD_PIN (19u)
+#define COOLANT_MIST_PIN (18u)
+
+// Define user-control CONTROLs (cycle start, reset, feed hold, door) input pins.
+#define RESET_PIN (14u)
+#define FEED_HOLD_PIN (16u)
+#define CYCLE_START_PIN (17u)
+#define SAFETY_DOOR_PIN (29u)
+
+// Define probe switch input pin.
+#define PROBE_PIN (15U)
+
+// Define auxillary input pins
+#define AUXINPUT0_PIN (36u) // ST0
+#if !QEI_ENABLE
+#define AUXINPUT1_PIN (30u) // ST1
+#define AUXINPUT2_PIN (34u) // ST2
+#define AUXINPUT3_PIN (35u) // ST3
+#define AUX_N_IN 4
+#define AUX_IN_MASK 0b1111
+#else
+#define AUX_N_IN 1
+#define AUX_IN_MASK 0b1
+#endif
+
+#define AUXOUTPUT0_PIN (31u) // AUX0
+#define AUXOUTPUT1_PIN (32u) // AUX1
+#define AUXOUTPUT2_PIN (33u) // AUX2
+#define AUX_N_OUT 3
+#define AUX_OUT_MASK 0b111
+
+#if KEYPAD_ENABLE
+#define KEYPAD_STROBE_PIN (41u) // I2C ST
+#endif
+
+#if EEPROM_ENABLE || KEYPAD_ENABLE
+#define I2C_PORT 4
+#define I2C_SCL4 (24u) // Not used, for info only
+#define I2C_SDA4 (25u) // Not used, for info only
+#endif
+
+#if QEI_ENABLE
+#define QEI_A_PIN (30u) // ST1
+#define QEI_B_PIN (34u) // ST2
+#define QEI_SELECT_PIN (35u) // ST3
+#endif
diff --git a/grblHAL_Teensy4/src/cnc_boosterpack_map.h b/grblHAL_Teensy4/src/cnc_boosterpack_map.h
new file mode 100644
index 0000000..6fe12eb
--- /dev/null
+++ b/grblHAL_Teensy4/src/cnc_boosterpack_map.h
@@ -0,0 +1,96 @@
+/*
+ cnc_boosterpack_map.h - driver code for IMXRT1062 processor (on Teensy 4.0 board)
+
+ Part of grblHAL
+
+ Copyright (c) 2020 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+#define BOARD_NAME "CNC BoosterPack"
+
+#if N_AXIS > 3
+#error Max number of axes is 3 for CNC BoosterPack
+#endif
+
+#ifdef EEPROM_ENABLE
+#undef EEPROM_ENABLE
+#endif
+#define EEPROM_ENABLE 1 // CNC BoosterPack has on-board EEPROM
+
+// Define step pulse output pins.
+#define X_STEP_PIN (32u)
+#define Y_STEP_PIN (30u)
+#define Z_STEP_PIN (26u)
+
+// Define step direction output pins.
+#define X_DIRECTION_PIN (5u)
+#define Y_DIRECTION_PIN (33u)
+#define Z_DIRECTION_PIN (13u)
+
+// Define stepper driver enable/disable output pin(s).
+#define STEPPERS_ENABLE_PIN (24u)
+#define Z_ENABLE_PIN (8u)
+
+// Define homing/hard limit switch input pins.
+#define X_LIMIT_PIN (10u)
+#define Y_LIMIT_PIN (1u)
+#define Z_LIMIT_PIN (0u)
+
+// Define spindle enable and spindle direction output pins.
+#define SPINDLE_ENABLE_PIN (12u)
+#define SPINDLE_DIRECTION_PIN (16u)
+#define SPINDLEPWMPIN (12u)
+
+// Define flood and mist coolant enable output pins.
+#define COOLANT_FLOOD_PIN (4u)
+#define COOLANT_MIST_PIN (31u)
+
+// Define user-control CONTROLs (cycle start, reset, feed hold) input pins.
+#define RESET_PIN (11u)
+#define FEED_HOLD_PIN (7u)
+#define CYCLE_START_PIN (6u)
+#define SAFETY_DOOR_PIN (9u)
+
+// Define probe switch input pin.
+#define PROBE_PIN (15U)
+
+#if KEYPAD_ENABLE
+#define KEYPAD_STROBE_PIN (28U)
+#endif
+
+#if EEPROM_ENABLE || KEYPAD_ENABLE
+#define I2C_PORT 0
+#define I2C_SCL0 (19u) // Not used, for info only
+#define I2C_SDA0 (18u) // Not used, for info only
+#endif
+
+#define UART_PORT 5
+#define UART_RX5 (21u) // Not used, for info only
+#define UART_TX5 (20u) // Not used, for info only
+
+#define GPIO0_PIN (3u)
+#define GPIO1_PIN (29u)
+#define GPIO2_PIN (27u)
+#define GPIO3_PIN (2u)
+
+#if QEI_ENABLE
+ #define QEI_A_PIN GPIO0_PIN
+ #define QEI_B_PIN GPIO3_PIN
+// #define QEI_INDEX_PIN GPIO2_PIN
+ #define QEI_SELECT_PIN GPIO1_PIN
+#endif
+
+/* EOF */
diff --git a/grblHAL_Teensy4/src/driver.c b/grblHAL_Teensy4/src/driver.c
new file mode 100644
index 0000000..0de63c7
--- /dev/null
+++ b/grblHAL_Teensy4/src/driver.c
@@ -0,0 +1,2536 @@
+/*
+ driver.c - driver code for IMXRT1062 processor (on Teensy 4.0/4.1 board)
+
+ Part of grblHAL
+
+ Copyright (c) 2020-2021 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+#include
+#include
+#include
+
+#include "uart.h"
+#include "driver.h"
+#include "grbl/protocol.h"
+#include "grbl/limits.h"
+
+#ifdef I2C_PORT
+#include "i2c.h"
+#endif
+
+#if EEPROM_ENABLE
+#include "eeprom/eeprom.h"
+#else
+#include "avr/eeprom.h"
+#endif
+
+#if IOPORTS_ENABLE
+#include "ioports.h"
+#endif
+
+#if SDCARD_ENABLE
+#include "sdcard/sdcard.h"
+#endif
+
+#if KEYPAD_ENABLE
+#include "keypad/keypad.h"
+#endif
+
+#if PLASMA_ENABLE
+#include "plasma/thc.h"
+#endif
+
+#if PPI_ENABLE
+#include "laser/ppi.h"
+static void ppi_timeout_isr (void);
+#endif
+
+#if ODOMETER_ENABLE
+#include "odometer/odometer.h"
+#endif
+
+#if ETHERNET_ENABLE
+ #include "enet.h"
+ #if TELNET_ENABLE
+ #include "networking/TCPStream.h"
+ #endif
+ #if WEBSOCKET_ENABLE
+ #include "networking/WsStream.h"
+ #endif
+#endif
+
+#if USB_SERIAL_CDC == 1
+#include "usb_serial_ard.h"
+#elif USB_SERIAL_CDC == 2
+#include "usb_serial_pjrc.h"
+#endif
+
+#if defined(ENABLE_SAFETY_DOOR_INPUT_PIN) && defined(SAFETY_DOOR_PIN)
+#define SAFETY_DOOR_ENABLE 1
+#else
+#define SAFETY_DOOR_ENABLE 0
+#ifdef SAFETY_DOOR_PIN
+//#define LIMITS_OVERRIDE_PIN SAFETY_DOOR_PIN
+#endif
+#endif
+
+#define DEBOUNCE_QUEUE 8 // Must be a power of 2
+
+#define F_BUS_MHZ (F_BUS_ACTUAL / 1000000)
+
+#if X_AUTO_SQUARE || Y_AUTO_SQUARE || Z_AUTO_SQUARE
+#define SQUARING_ENABLED
+#endif
+
+#if defined(X2_LIMIT_PIN) || defined(Y2_LIMIT_PIN) || defined(Z2_LIMIT_PIN) || defined(A2_LIMIT_PIN) || defined(B2_LIMIT_PIN)
+#define DUAL_LIMIT_SWITCHES
+#else
+ #ifdef SQUARING_ENABLED
+ #error "Squaring requires at least one axis with dual switch inputs!"
+ #endif
+#endif
+
+#define INPUT_GROUP_CONTROL (1 << 0)
+#define INPUT_GROUP_PROBE (1 << 1)
+#define INPUT_GROUP_LIMIT (1 << 2)
+#define INPUT_GROUP_KEYPAD (1 << 3)
+#define INPUT_GROUP_MPG (1 << 4)
+#define INPUT_GROUP_QEI (1 << 5)
+#define INPUT_GROUP_QEI_SELECT (1 << 6)
+#define INPUT_GROUP_SPINDLE_INDEX (1 << 7)
+
+typedef struct {
+ volatile uint_fast8_t head;
+ volatile uint_fast8_t tail;
+ input_signal_t *signal[DEBOUNCE_QUEUE];
+} debounce_queue_t;
+
+#if QEI_ENABLE
+
+#define QEI_DEBOUNCE 3
+#define QEI_VELOCITY_TIMEOUT 100
+
+typedef union {
+ uint_fast8_t pins;
+ struct {
+ uint_fast8_t a :1,
+ b :1;
+ };
+} qei_state_t;
+
+typedef struct {
+ encoder_t encoder;
+ int32_t count;
+ int32_t vel_count;
+ uint_fast16_t state;
+ volatile uint32_t dbl_click_timeout;
+ volatile uint32_t vel_timeout;
+ uint32_t vel_timestamp;
+} qei_t;
+
+static qei_t qei = {0};
+
+#endif
+
+static debounce_queue_t debounce_queue = {0};
+
+// Standard inputs
+static gpio_t Reset, FeedHold, CycleStart, Probe, LimitX, LimitY, LimitZ;
+
+// Standard outputs
+static gpio_t Mist, Flood, stepX, stepY, stepZ, dirX, dirY, dirZ;
+
+#if !VFD_SPINDLE
+static gpio_t spindleEnable, spindleDir;
+#endif
+
+// Optional I/O
+#if SAFETY_DOOR_ENABLE
+static gpio_t SafetyDoor;
+#endif
+#ifdef LIMITS_OVERRIDE_PIN
+static gpio_t LimitsOverride;
+#endif
+#ifdef A_AXIS
+static gpio_t stepA, dirA, LimitA;
+#ifdef A_ENABLE_PIN
+static gpio_t enableA;
+#endif
+#endif
+#ifdef B_AXIS
+static gpio_t stepB, dirB, LimitB;
+#ifdef B_ENABLE_PIN
+static gpio_t enableB;
+#endif
+#endif
+#ifdef C_AXIS
+static gpio_t stepC, dirC;
+#ifdef C_ENABLE_PIN
+static gpio_t enableC;
+#endif
+#ifdef C_LIMIT_PIN
+static gpio_t LimitC;
+#endif
+#endif
+#ifdef STEPPERS_ENABLE_PIN
+static gpio_t steppersEnable;
+#endif
+#ifdef X_ENABLE_PIN
+static gpio_t enableX;
+#endif
+#ifdef Y_ENABLE_PIN
+static gpio_t enableY;
+#endif
+#ifdef Z_ENABLE_PIN
+static gpio_t enableZ;
+#endif
+#if KEYPAD_ENABLE
+static gpio_t KeypadStrobe;
+#endif
+#if MPG_MODE_ENABLE
+static gpio_t ModeSelect;
+#endif
+#if QEI_ENABLE
+static bool qei_enable = false;
+static gpio_t QEI_A, QEI_B;
+ #ifdef QEI_SELECT_PIN
+ #define QEI_SELECT_ENABLED 1
+ static gpio_t QEI_Select;
+ #endif
+ #ifdef QEI_INDEX_PIN
+ #define QEI_INDEX_ENABLED 1
+ static gpio_t QEI_Index;
+ #endif
+#endif
+
+#ifdef X2_STEP_PIN
+ static gpio_t stepX2;
+#endif
+#ifdef X2_DIRECTION_PIN
+ static gpio_t dirX2;
+#endif
+#ifdef X2_ENABLE_PIN
+ static gpio_t enableX2;
+#endif
+#ifdef X2_LIMIT_PIN
+ static gpio_t LimitX2;
+#endif
+
+#ifdef Y2_STEP_PIN
+ static gpio_t stepY2;
+#endif
+#ifdef Y2_DIRECTION_PIN
+ static gpio_t dirY2;
+#endif
+#ifdef Y2_ENABLE_PIN
+ static gpio_t enableY2;
+#endif
+#ifdef Y2_LIMIT_PIN
+ static gpio_t LimitY2;
+#endif
+
+#ifdef Z2_STEP_PIN
+ static gpio_t stepZ2;
+#endif
+#ifdef Z2_DIRECTION_PIN
+ static gpio_t dirZ2;
+#endif
+#ifdef Z2_ENABLE_PIN
+ static gpio_t enableZ2;
+#endif
+#ifdef Z2_LIMIT_PIN
+ static gpio_t LimitZ2;
+#endif
+#ifdef SPINDLE_INDEX_PIN
+ static gpio_t SpindleIndex;
+#endif
+
+static input_signal_t inputpin[] = {
+#if ESTOP_ENABLE
+ { .id = Input_EStop, .port = &Reset, .pin = RESET_PIN, .group = INPUT_GROUP_CONTROL },
+#else
+ { .id = Input_Reset, .port = &Reset, .pin = RESET_PIN, .group = INPUT_GROUP_CONTROL },
+#endif
+ { .id = Input_FeedHold, .port = &FeedHold, .pin = FEED_HOLD_PIN, .group = INPUT_GROUP_CONTROL },
+ { .id = Input_CycleStart, .port = &CycleStart, .pin = CYCLE_START_PIN, .group = INPUT_GROUP_CONTROL },
+#if SAFETY_DOOR_ENABLE
+ { .id = Input_SafetyDoor, .port = &SafetyDoor, .pin = SAFETY_DOOR_PIN, .group = INPUT_GROUP_CONTROL },
+#endif
+#if defined(LIMITS_OVERRIDE_PIN)
+ { .id = Input_LimitsOverride, .port = &LimitsOverride, .pin = LIMITS_OVERRIDE_PIN, .group = INPUT_GROUP_CONTROL },
+#endif
+ { .id = Input_Probe, .port = &Probe, .pin = PROBE_PIN, .group = INPUT_GROUP_PROBE },
+ { .id = Input_LimitX, .port = &LimitX, .pin = X_LIMIT_PIN, .group = INPUT_GROUP_LIMIT },
+#ifdef X2_LIMIT_PIN
+ { .id = Input_LimitX_Max, .port = &LimitX2, .pin = X2_LIMIT_PIN, .group = INPUT_GROUP_LIMIT },
+#endif
+ { .id = Input_LimitY, .port = &LimitY, .pin = Y_LIMIT_PIN, .group = INPUT_GROUP_LIMIT },
+#ifdef Y2_LIMIT_PIN
+ { .id = Input_LimitY_Max, .port = &LimitY2, .pin = Y2_LIMIT_PIN, .group = INPUT_GROUP_LIMIT },
+#endif
+ { .id = Input_LimitZ, .port = &LimitZ, .pin = Z_LIMIT_PIN, .group = INPUT_GROUP_LIMIT }
+#ifdef Z2_LIMIT_PIN
+ , { .id = Input_LimitZ_Max, .port = &LimitZ2, .pin = Z2_LIMIT_PIN, .group = INPUT_GROUP_LIMIT }
+#endif
+#ifdef A_LIMIT_PIN
+ , { .id = Input_LimitA, .port = &LimitA, .pin = A_LIMIT_PIN, .group = INPUT_GROUP_LIMIT }
+#endif
+#ifdef B_LIMIT_PIN
+ , { .id = Input_LimitB, .port = &LimitB, .pin = B_LIMIT_PIN, .group = INPUT_GROUP_LIMIT }
+#endif
+#ifdef C_LIMIT_PIN
+ , { .id = Input_LimitC, .port = &LimitC, .pin = C_LIMIT_PIN, .group = INPUT_GROUP_LIMIT }
+#endif
+#if MPG_MODE_ENABLE
+ , { .id = Input_ModeSelect, .port = &ModeSelect, .pin = MODE_PIN, .group = INPUT_GROUP_MPG }
+#endif
+#if KEYPAD_ENABLE && defined(KEYPAD_STROBE_PIN)
+ , { .id = Input_KeypadStrobe, .port = &KeypadStrobe, .pin = KEYPAD_STROBE_PIN, .group = INPUT_GROUP_KEYPAD }
+#endif
+#ifdef SPINDLE_INDEX_PIN
+ , { .id = Input_SpindleIndex, .port = &SpindleIndex, .pin = SPINDLE_INDEX_PIN, .group = INPUT_GROUP_SPINDLE_INDEX }
+#endif
+#if QEI_ENABLE
+ , { .id = Input_QEI_A, .port = &QEI_A, .pin = QEI_A_PIN, .group = INPUT_GROUP_QEI }
+ , { .id = Input_QEI_B, .port = &QEI_B, .pin = QEI_B_PIN, .group = INPUT_GROUP_QEI }
+ #if QEI_SELECT_ENABLED
+ , { .id = Input_QEI_Select, .port = &QEI_Select, .pin = QEI_SELECT_PIN, .group = INPUT_GROUP_QEI_SELECT }
+ #endif
+ #if QEI_INDEX_ENABLED
+ , { .id = Input_QEI_Index, .port = &QEI_Index, .pin = QEI_INDEX_PIN, .group = INPUT_GROUP_QEI }
+ #endif
+#endif
+};
+
+#if USB_SERIAL_CDC || QEI_ENABLE
+#define ADD_MSEVENT 1
+static volatile bool ms_event = false;
+#else
+#define ADD_MSEVENT 0
+#endif
+static bool IOInitDone = false;
+static uint16_t pulse_length, pulse_delay;
+static axes_signals_t next_step_outbits;
+static delay_t grbl_delay = { .ms = 0, .callback = NULL };
+static probe_state_t probe = {
+ .connected = On
+};
+#ifdef SQUARING_ENABLED
+static axes_signals_t motors_1 = {AXES_BITMASK}, motors_2 = {AXES_BITMASK};
+#endif
+
+#if !VFD_SPINDLE
+static bool pwmEnabled = false;
+static spindle_pwm_t spindle_pwm;
+
+static void spindle_set_speed (uint_fast16_t pwm_value);
+#endif
+
+#if MODBUS_ENABLE
+static modbus_stream_t modbus_stream = {0};
+#endif
+
+#ifdef SPINDLE_SYNC_ENABLE
+
+#include "grbl/spindle_sync.h"
+
+static spindle_data_t spindle_data;
+static spindle_encoder_t spindle_encoder = {
+ .tics_per_irq = 4
+};
+static spindle_sync_t spindle_tracker;
+static volatile bool spindleLock = false;
+
+static void stepperPulseStartSynchronized (stepper_t *stepper);
+static void spindleDataReset (void);
+static spindle_data_t *spindleGetData (spindle_data_request_t request);
+static void spindle_pulse_isr (void);
+
+#endif
+
+#if ETHERNET_ENABLE
+static network_services_t services = {0};
+
+static void enetStreamWriteS (const char *data)
+{
+#if TELNET_ENABLE
+ if(services.telnet)
+ TCPStreamWriteS(data);
+#endif
+#if WEBSOCKET_ENABLE
+ if(services.websocket)
+ WsStreamWriteS(data);
+#endif
+#if USB_SERIAL_CDC
+ if(!(services.telnet || services.websocket)) // TODO: check if usb connection is up?
+ usb_serialWriteS(data);
+#else
+ serialWriteS(data);
+#endif
+}
+
+ #if TELNET_ENABLE
+ const io_stream_t ethernet_stream = {
+ .type = StreamType_Telnet,
+ .read = TCPStreamGetC,
+ .write = TCPStreamWriteS,
+ .write_all = enetStreamWriteS,
+ .get_rx_buffer_available = TCPStreamRxFree,
+ .reset_read_buffer = TCPStreamRxFlush,
+ .cancel_read_buffer = TCPStreamRxCancel,
+ .enqueue_realtime_command = protocol_enqueue_realtime_command,
+ #if M6_ENABLE
+ .suspend_read = NULL // for now...
+ #else
+ .suspend_read = NULL
+ #endif
+ };
+ #endif
+
+ #if WEBSOCKET_ENABLE
+ const io_stream_t websocket_stream = {
+ .type = StreamType_WebSocket,
+ .read = WsStreamGetC,
+ .write = WsStreamWriteS,
+ .write_all = enetStreamWriteS,
+ .get_rx_buffer_available = WsStreamRxFree,
+ .reset_read_buffer = WsStreamRxFlush,
+ .cancel_read_buffer = WsStreamRxCancel,
+ .enqueue_realtime_command = protocol_enqueue_realtime_command,
+ #if M6_ENABLE
+ .suspend_read = NULL // for now...
+ #else
+ .suspend_read = NULL
+ #endif
+ };
+ #endif
+
+#endif // ETHERNET_ENABLE
+
+#if USB_SERIAL_CDC
+ const io_stream_t serial_stream = {
+ .type = StreamType_Serial,
+ .read = usb_serialGetC,
+ .write = usb_serialWriteS,
+ #if ETHERNET_ENABLE
+ .write_all = enetStreamWriteS,
+ #else
+ .write_all = usb_serialWriteS,
+ #endif
+ .get_rx_buffer_available = usb_serialRxFree,
+ .reset_read_buffer = usb_serialRxFlush,
+ .cancel_read_buffer = usb_serialRxCancel,
+ .suspend_read = usb_serialSuspendInput,
+ .enqueue_realtime_command = protocol_enqueue_realtime_command
+ };
+#else
+const io_stream_t serial_stream = {
+ .type = StreamType_Serial,
+ .read = serialGetC,
+ .write = serialWriteS,
+#if ETHERNET_ENABLE
+ .write_all = enetStreamWriteS,
+#else
+ .write_all = serialWriteS,
+#endif
+ .get_rx_buffer_available = serialRxFree,
+ .reset_read_buffer = serialRxFlush,
+ .cancel_read_buffer = serialRxCancel,
+ .suspend_read = serialSuspendInput,
+ .enqueue_realtime_command = protocol_enqueue_realtime_command
+};
+#endif
+
+// Interrupt handler prototypes
+// Interrupt handlers needs to be registered, possibly by modifying a system specific startup file.
+// It is possible to relocate the interrupt dispatch table from flash to RAM and programatically attach handlers.
+// See the driver for SAMD21 for an example, relocation is done in the driver_init() function.
+// Also, if a MCU specific driver library is used this might have functions to programatically attach handlers.
+
+static void stepper_driver_isr (void);
+static void stepper_pulse_isr (void);
+static void stepper_pulse_isr_delayed (void);
+static void gpio_isr (void);
+static void debounce_isr (void);
+static void systick_isr (void);
+
+static void (*systick_isr_org)(void) = NULL;
+
+// Millisecond resolution delay function
+// Will return immediately if a callback function is provided
+static void driver_delay_ms (uint32_t ms, void (*callback)(void))
+{
+ if(ms) {
+ grbl_delay.ms = ms;
+ if(!(grbl_delay.callback = callback))
+ while(grbl_delay.ms);
+ } else {
+ if(grbl_delay.ms) {
+ grbl_delay.callback = NULL;
+ grbl_delay.ms = 1;
+ }
+ if(callback)
+ callback();
+ }
+}
+
+void selectStream (stream_type_t stream)
+{
+ static stream_type_t active_stream = StreamType_Serial;
+
+ switch(stream) {
+
+#if TELNET_ENABLE
+ case StreamType_Telnet:
+ hal.stream.write_all("[MSG:TELNET STREAM ACTIVE]" ASCII_EOL);
+ memcpy(&hal.stream, ðernet_stream, sizeof(io_stream_t));
+ services.telnet = On;
+ break;
+#endif
+#if WEBSOCKET_ENABLE
+ case StreamType_WebSocket:
+ hal.stream.write_all("[MSG:WEBSOCKET STREAM ACTIVE]" ASCII_EOL);
+ memcpy(&hal.stream, &websocket_stream, sizeof(io_stream_t));
+ services.websocket = On;
+ break;
+#endif
+ case StreamType_Serial:
+ memcpy(&hal.stream, &serial_stream, sizeof(io_stream_t));
+#if ETHERNET_ENABLE
+ services.mask = 0;
+#endif
+ if(active_stream != StreamType_Serial)
+ hal.stream.write_all("[MSG:SERIAL STREAM ACTIVE]" ASCII_EOL);
+ break;
+
+ default:
+ break;
+ }
+
+ active_stream = stream;
+}
+
+// Set stepper pulse output pins.
+// step_outbits.value (or step_outbits.mask) are: bit0 -> X, bit1 -> Y...
+// Individual step bits can be accessed by step_outbits.x, step_outbits.y, ...
+#ifdef SQUARING_ENABLED
+inline static __attribute__((always_inline)) void set_step_outputs (axes_signals_t step_outbits_1)
+{
+ axes_signals_t step_outbits_2;
+
+ step_outbits_2.mask = (step_outbits_1.mask & motors_2.mask) ^ settings.steppers.step_invert.mask;
+ step_outbits_1.mask = (step_outbits_1.mask & motors_1.mask) ^ settings.steppers.step_invert.mask;
+
+ DIGITAL_OUT(stepX, step_outbits_1.x);
+#ifdef X2_STEP_PIN
+ DIGITAL_OUT(stepX2, step_outbits_2.x);
+#endif
+
+ DIGITAL_OUT(stepY, step_outbits_1.y);
+#ifdef Y2_STEP_PIN
+ DIGITAL_OUT(stepY2, step_outbits_2.y);
+#endif
+
+ DIGITAL_OUT(stepZ, step_outbits_1.z);
+#ifdef Z2_STEP_PIN
+ DIGITAL_OUT(stepZ2, step_outbits_2.z);
+#endif
+
+#ifdef A_AXIS
+ DIGITAL_OUT(stepA, step_outbits_1.a);
+#endif
+#ifdef B_AXIS
+ DIGITAL_OUT(stepB, step_outbits_1.b);
+#endif
+}
+#else
+inline static __attribute__((always_inline)) void set_step_outputs (axes_signals_t step_outbits)
+{
+ step_outbits.value ^= settings.steppers.step_invert.mask;
+
+ DIGITAL_OUT(stepX, step_outbits.x);
+#ifdef X2_STEP_PIN
+ DIGITAL_OUT(stepX2, step_outbits.x);
+#endif
+
+ DIGITAL_OUT(stepY, step_outbits.y);
+#ifdef Y2_STEP_PIN
+ DIGITAL_OUT(stepY2, step_outbits.y);
+#endif
+
+ DIGITAL_OUT(stepZ, step_outbits.z);
+#ifdef Z2_STEP_PIN
+ DIGITAL_OUT(stepZ2, step_outbits.z);
+#endif
+
+#ifdef A_AXIS
+ DIGITAL_OUT(stepA, step_outbits.a);
+#endif
+#ifdef B_AXIS
+ DIGITAL_OUT(stepB, step_outbits.b);
+#endif
+#ifdef C_AXIS
+ DIGITAL_OUT(stepC, step_outbits.c);
+#endif
+}
+#endif
+
+// Set stepper direction ouput pins.
+// dir_outbits.value (or dir_outbits.mask) are: bit0 -> X, bit1 -> Y...
+// Individual direction bits can be accessed by dir_outbits.x, dir_outbits.y, ...
+inline static __attribute__((always_inline)) void set_dir_outputs (axes_signals_t dir_outbits)
+{
+ dir_outbits.value ^= settings.steppers.dir_invert.mask;
+
+ DIGITAL_OUT(dirX, dir_outbits.x);
+#ifdef X2_DIRECTION_PIN
+ DIGITAL_OUT(dirX2, dir_outbits.x);
+#endif
+
+ DIGITAL_OUT(dirY, dir_outbits.y);
+#ifdef Y2_DIRECTION_PIN
+ DIGITAL_OUT(dirY2, dir_outbits.y);
+#endif
+
+ DIGITAL_OUT(dirZ, dir_outbits.z);
+#ifdef Z2_DIRECTION_PIN
+ DIGITAL_OUT(dirZ2, dir_outbits.z);
+#endif
+
+#ifdef A_AXIS
+ DIGITAL_OUT(dirA, dir_outbits.a);
+#endif
+#ifdef B_AXIS
+ DIGITAL_OUT(dirB, dir_outbits.b);
+#endif
+#ifdef C_AXIS
+ DIGITAL_OUT(dirC, dir_outbits.c);
+#endif
+}
+
+// Enable steppers.
+// enable.value (or enable.mask) are: bit0 -> X, bit1 -> Y...
+// Individual enable bits can be accessed by enable.x, enable.y, ...
+// NOTE: if a common signal is used to enable all drivers enable.x should be used to set the signal.
+static void stepperEnable (axes_signals_t enable)
+{
+ enable.value ^= settings.steppers.enable_invert.mask;
+
+#ifdef STEPPERS_ENABLE_PIN
+ DIGITAL_OUT(steppersEnable, enable.x)
+#endif
+
+#ifdef X_ENABLE_PIN
+ DIGITAL_OUT(enableX, enable.x)
+#endif
+#ifdef X2_ENABLE_PIN
+ DIGITAL_OUT(enableX2, enable.x)
+#endif
+
+#ifdef Y_ENABLE_PIN
+ DIGITAL_OUT(enableY, enable.y)
+#endif
+#ifdef Y2_ENABLE_PIN
+ DIGITAL_OUT(enableY2, enable.y)
+#endif
+
+#ifdef Z_ENABLE_PIN
+ DIGITAL_OUT(enableZ, enable.z)
+#endif
+#ifdef Z2_ENABLE_PIN
+ DIGITAL_OUT(enableZ2, enable.z)
+#endif
+
+#ifdef A_ENABLE_PIN
+ DIGITAL_OUT(enableA, enable.a)
+#endif
+#ifdef B_ENABLE_PIN
+ DIGITAL_OUT(enableB, enable.b)
+#endif
+#ifdef C_ENABLE_PIN
+ DIGITAL_OUT(enableC, enable.c)
+#endif
+}
+
+// Starts stepper driver timer and forces a stepper driver interrupt callback.
+static void stepperWakeUp (void)
+{
+ // Enable stepper drivers.
+ stepperEnable((axes_signals_t){AXES_BITMASK});
+
+ PIT_LDVAL0 = 5000;
+ PIT_TFLG0 |= PIT_TFLG_TIF;
+ PIT_TCTRL0 |= (PIT_TCTRL_TIE|PIT_TCTRL_TEN);
+}
+
+// Disables stepper driver interrupts and reset outputs.
+static void stepperGoIdle (bool clear_signals)
+{
+ PIT_TCTRL0 &= ~(PIT_TCTRL_TIE|PIT_TCTRL_TEN);
+
+ if(clear_signals) {
+ set_step_outputs((axes_signals_t){0});
+ set_dir_outputs((axes_signals_t){0});
+ }
+}
+
+// Sets up stepper driver interrupt timeout.
+// Called at the start of each segment.
+// NOTE: If a 32-bit timer is used it is advisable to limit max step time to about 2 seconds
+// in order to avoid excessive delays on completion of motions
+// NOTE: If a 16 bit timer is used it may be neccesary to adjust the timer clock frequency (prescaler)
+// to cover the needed range. Refer to actual drivers for code examples.
+static void stepperCyclesPerTick (uint32_t cycles_per_tick)
+{
+ PIT_TCTRL0 &= ~PIT_TCTRL_TEN;
+ PIT_LDVAL0 = cycles_per_tick < (1UL << 20) ? cycles_per_tick : 0x000FFFFFUL;
+ PIT_TFLG0 |= PIT_TFLG_TIF;
+ PIT_TCTRL0 |= PIT_TCTRL_TEN;
+}
+
+// Start a stepper pulse, no delay version.
+// stepper_t struct is defined in grbl/stepper.h
+static void stepperPulseStart (stepper_t *stepper)
+{
+#ifdef SPINDLE_SYNC_ENABLE
+ if(stepper->new_block && stepper->exec_segment->spindle_sync) {
+ spindle_tracker.stepper_pulse_start_normal = hal.stepper.pulse_start;
+ hal.stepper.pulse_start = stepperPulseStartSynchronized;
+ hal.stepper.pulse_start(stepper);
+ return;
+ }
+#endif
+
+ if(stepper->dir_change)
+ set_dir_outputs(stepper->dir_outbits);
+
+ if(stepper->step_outbits.value) {
+ set_step_outputs(stepper->step_outbits);
+ TMR4_CTRL0 |= TMR_CTRL_CM(0b001);
+ }
+}
+
+// Start a stepper pulse, delay version.
+// Note: delay is only added when there is a direction change and a pulse to be output.
+// In the delayed step pulse interrupt handler the pulses are output and
+// normal (no delay) operation is resumed.
+// stepper_t struct is defined in grbl/stepper.h
+static void stepperPulseStartDelayed (stepper_t *stepper)
+{
+#ifdef SPINDLE_SYNC_ENABLE
+ if(stepper->new_block && stepper->exec_segment->spindle_sync) {
+ spindle_tracker.stepper_pulse_start_normal = hal.stepper.pulse_start;
+ hal.stepper.pulse_start = stepperPulseStartSynchronized;
+ hal.stepper.pulse_start(stepper);
+ return;
+ }
+#endif
+
+ if(stepper->dir_change) {
+
+ set_dir_outputs(stepper->dir_outbits);
+
+ if(stepper->step_outbits.value) {
+
+ next_step_outbits = stepper->step_outbits; // Store out_bits
+
+ attachInterruptVector(IRQ_QTIMER4, stepper_pulse_isr_delayed);
+
+ TMR4_COMP10 = pulse_delay;
+ TMR4_CTRL0 |= TMR_CTRL_CM(0b001);
+ }
+
+ return;
+ }
+
+ if(stepper->step_outbits.value) {
+ set_step_outputs(stepper->step_outbits);
+ TMR4_CTRL0 |= TMR_CTRL_CM(0b001);
+ }
+}
+
+#ifdef SPINDLE_SYNC_ENABLE
+
+// Spindle sync version: sets stepper direction and pulse pins and starts a step pulse.
+// Switches back to "normal" version if spindle synchronized motion is finished.
+// TODO: add delayed pulse handling...
+static void stepperPulseStartSynchronized (stepper_t *stepper)
+{
+ static bool sync = false;
+ static float block_start;
+
+ if(stepper->new_block) {
+ if(!stepper->exec_segment->spindle_sync) {
+ hal.stepper.pulse_start = spindle_tracker.stepper_pulse_start_normal;
+ hal.stepper.pulse_start(stepper);
+ return;
+ }
+ sync = true;
+ set_dir_outputs(stepper->dir_outbits);
+ spindle_tracker.programmed_rate = stepper->exec_block->programmed_rate;
+ spindle_tracker.steps_per_mm = stepper->exec_block->steps_per_mm;
+ spindle_tracker.segment_id = 0;
+ spindle_tracker.prev_pos = 0.0f;
+ block_start = spindleGetData(SpindleData_AngularPosition)->angular_position * spindle_tracker.programmed_rate;
+ pidf_reset(&spindle_tracker.pid);
+#ifdef PID_LOG
+ sys.pid_log.idx = 0;
+ sys.pid_log.setpoint = 100.0f;
+#endif
+ }
+
+ if(stepper->step_outbits.value) {
+ set_step_outputs(stepper->step_outbits);
+ TMR4_CTRL0 |= TMR_CTRL_CM(0b001);
+ }
+
+ if(spindle_tracker.segment_id != stepper->exec_segment->id) {
+
+ spindle_tracker.segment_id = stepper->exec_segment->id;
+
+ if(!stepper->new_block) { // adjust this segments total time for any positional error since last segment
+
+ float actual_pos;
+
+ if(stepper->exec_segment->cruising) {
+
+ float dt = (float)hal.f_step_timer / (float)(stepper->exec_segment->cycles_per_tick * stepper->exec_segment->n_step);
+ actual_pos = spindleGetData(SpindleData_AngularPosition)->angular_position * spindle_tracker.programmed_rate;
+
+ if(sync) {
+ spindle_tracker.pid.sample_rate_prev = dt;
+// block_start += (actual_pos - spindle_tracker.block_start) - spindle_tracker.prev_pos;
+// block_start += spindle_tracker.prev_pos;
+ sync = false;
+ }
+
+ actual_pos -= block_start;
+ int32_t step_delta = (int32_t)(pidf(&spindle_tracker.pid, spindle_tracker.prev_pos, actual_pos, dt) * spindle_tracker.steps_per_mm);
+
+
+ int32_t ticks = (((int32_t)stepper->step_count + step_delta) * (int32_t)stepper->exec_segment->cycles_per_tick) / (int32_t)stepper->step_count;
+
+ stepper->exec_segment->cycles_per_tick = (uint32_t)max(ticks, spindle_tracker.min_cycles_per_tick);
+
+ stepperCyclesPerTick(stepper->exec_segment->cycles_per_tick);
+ } else
+ actual_pos = spindle_tracker.prev_pos;
+
+#ifdef PID_LOG
+ if(sys.pid_log.idx < PID_LOG) {
+
+ sys.pid_log.target[sys.pid_log.idx] = spindle_tracker.prev_pos;
+ sys.pid_log.actual[sys.pid_log.idx] = actual_pos; // - spindle_tracker.prev_pos;
+
+ // spindle_tracker.log[sys.pid_log.idx] = STEPPER_TIMER->BGLOAD << stepper->amass_level;
+ // spindle_tracker.pos[sys.pid_log.idx] = stepper->exec_segment->cycles_per_tick stepper->amass_level;
+ // spindle_tracker.pos[sys.pid_log.idx] = stepper->exec_segment->cycles_per_tick * stepper->step_count;
+ // STEPPER_TIMER->BGLOAD = STEPPER_TIMER->LOAD;
+
+ // spindle_tracker.pos[sys.pid_log.idx] = spindle_tracker.prev_pos;
+
+ sys.pid_log.idx++;
+ }
+#endif
+ }
+
+ spindle_tracker.prev_pos = stepper->exec_segment->target_position;
+ }
+}
+
+#endif
+
+#if PLASMA_ENABLE
+
+#if PLASMA_ENABLE
+static void output_pulse_isr(void);
+#endif
+
+static axes_signals_t pulse_output = {0};
+
+void stepperOutputStep (axes_signals_t step_outbits, axes_signals_t dir_outbits)
+{
+ pulse_output = step_outbits;
+ dir_outbits.value ^= settings.steppers.dir_invert.mask;
+
+ DIGITAL_OUT(dirZ, dir_outbits.z);
+ TMR2_CTRL0 |= TMR_CTRL_CM(0b001);
+}
+
+#endif
+
+#ifdef DUAL_LIMIT_SWITCHES
+
+// Returns limit state as an axes_signals_t variable.
+// Each bitfield bit indicates an axis limit, where triggered is 1 and not triggered is 0.
+// Dual limit switch inputs per axis version. Only one needs to be dual input!
+inline static limit_signals_t limitsGetState()
+{
+ limit_signals_t signals = {0};
+
+ signals.min.mask = signals.min2.mask = settings.limits.invert.mask;
+
+ signals.min.x = (LimitX.reg->DR & LimitX.bit) != 0;
+#ifdef X2_LIMIT_PIN
+ signals.min2.x = (LimitX2.reg->DR & LimitX2.bit) != 0;
+#endif
+
+ signals.min.y = (LimitY.reg->DR & LimitY.bit) != 0;
+#ifdef Y2_LIMIT_PIN
+ signals.min2.y = (LimitY2.reg->DR & LimitY2.bit) != 0;
+#endif
+
+ signals.min.z = (LimitZ.reg->DR & LimitZ.bit) != 0;
+#ifdef Z2_LIMIT_PIN
+ signals.min2.z = (LimitZ2.reg->DR & LimitZ2.bit) != 0;
+#endif
+
+#ifdef A_LIMIT_PIN
+ signals.min.a = (LimitA.reg->DR & LimitA.bit) != 0;
+#endif
+#ifdef B_LIMIT_PIN
+ signals.min.b = (LimitB.reg->DR & LimitB.bit) != 0;
+#endif
+#ifdef C_LIMIT_PIN
+ signals.min.c = (LimitC.reg->DR & LimitC.bit) != 0;
+#endif
+
+ if(settings.limits.invert.mask) {
+ signals.min.value ^= settings.limits.invert.mask;
+ signals.min2.value ^= settings.limits.invert.mask;
+ }
+
+ return signals;
+}
+#else // SINGLE INPUT LIMIT SWITCHES
+
+// Returns limit state as an axes_signals_t bitmap variable.
+// signals.value (or signals.mask) are: bit0 -> X, bit1 -> Y...
+// Individual signals bits can be accessed by signals.x, signals.y, ...
+// Each bit indicates a limit signal, where triggered is 1 and not triggered is 0.
+// axes_signals_t is defined in grbl/nuts_bolts.h.
+inline static limit_signals_t limitsGetState()
+{
+ limit_signals_t signals = {0};
+
+ signals.min.mask = settings.limits.invert.mask;
+
+ signals.min.x = (LimitX.reg->DR & LimitX.bit) != 0;
+ signals.min.y = (LimitY.reg->DR & LimitY.bit) != 0;
+ signals.min.z = (LimitZ.reg->DR & LimitZ.bit) != 0;
+#ifdef A_LIMIT_PIN
+ signals.min.a = (LimitA.reg->DR & LimitA.bit) != 0;
+#endif
+#ifdef B_LIMIT_PIN
+ signals.min.b = (LimitB.reg->DR & LimitB.bit) != 0;
+#endif
+#ifdef C_LIMIT_PIN
+ signals.min.c = (LimitC.reg->DR & LimitC.bit) != 0;
+#endif
+
+ if (settings.limits.invert.mask)
+ signals.min.value ^= settings.limits.invert.mask;
+
+ return signals;
+}
+
+#endif
+
+#ifdef SQUARING_ENABLED
+
+static axes_signals_t getAutoSquaredAxes (void)
+{
+ axes_signals_t ganged = {0};
+
+ #if X_AUTO_SQUARE
+ ganged.x = On;
+#endif
+#if Y_AUTO_SQUARE
+ ganged.y = On;
+#endif
+#if Z_AUTO_SQUARE
+ ganged.z = On;
+#endif
+
+ return ganged;
+}
+
+// Enable/disable motors for auto squaring of ganged axes
+static void StepperDisableMotors (axes_signals_t axes, squaring_mode_t mode)
+{
+ motors_1.mask = (mode == SquaringMode_A || mode == SquaringMode_Both ? axes.mask : 0) ^ AXES_BITMASK;
+ motors_2.mask = (mode == SquaringMode_B || mode == SquaringMode_Both ? axes.mask : 0) ^ AXES_BITMASK;
+}
+
+#endif
+
+// Enable/disable limit pins interrupt.
+// NOTE: the homing parameter is indended for configuring advanced
+// stepper drivers for sensorless homing.
+static void limitsEnable (bool on, bool homing)
+{
+ uint32_t i = sizeof(inputpin) / sizeof(input_signal_t);
+
+ on &= settings.limits.flags.hard_enabled;
+
+ do {
+ if(inputpin[--i].group == INPUT_GROUP_LIMIT) {
+ inputpin[i].gpio.reg->ISR = inputpin[i].gpio.bit; // Clear interrupt.
+ if(on)
+ inputpin[i].gpio.reg->IMR |= inputpin[i].gpio.bit; // Enable interrupt.
+ else
+ inputpin[i].gpio.reg->IMR &= ~inputpin[i].gpio.bit; // Disable interrupt.
+ }
+ } while(i);
+}
+
+// Returns system state as a control_signals_t bitmap variable.
+// signals.value (or signals.mask) are: bit0 -> reset, bit1 -> feed_hold, ...
+// Individual enable bits can be accessed by signals.reset, signals.feed_hold, ...
+// Each bit indicates a control signal, where triggered is 1 and not triggered is 0.
+// axes_signals_t is defined in grbl/system.h.
+inline static control_signals_t systemGetState (void)
+{
+ control_signals_t signals;
+
+ signals.value = settings.control_invert.value;
+
+#if ESTOP_ENABLE
+ signals.e_stop = (Reset.reg->DR & Reset.bit) != 0;
+#else
+ signals.reset = (Reset.reg->DR & Reset.bit) != 0;
+#endif
+ signals.feed_hold = (FeedHold.reg->DR & FeedHold.bit) != 0;
+ signals.cycle_start = (CycleStart.reg->DR & CycleStart.bit) != 0;
+#if SAFETY_DOOR_ENABLE
+ signals.safety_door_ajar = (SafetyDoor.reg->DR & SafetyDoor.bit) != 0;
+#endif
+
+ if(settings.control_invert.value)
+ signals.value ^= settings.control_invert.value;
+
+#ifdef LIMITS_OVERRIDE_PIN
+ signals.limits_override = (LimitsOverride.reg->DR & LimitsOverride.bit) == 0;
+#endif
+
+ return signals;
+}
+
+// Sets up the probe pin invert mask to
+// appropriately set the pin logic according to setting for normal-high/normal-low operation
+// and the probing cycle modes for toward-workpiece/away-from-workpiece.
+static void probeConfigure(bool is_probe_away, bool probing)
+{
+ probe.triggered = Off;
+ probe.is_probing = probing;
+ probe.inverted = is_probe_away ? !settings.probe.invert_probe_pin : settings.probe.invert_probe_pin;
+}
+
+// Returns the probe connected and triggered pin states.
+probe_state_t probeGetState (void)
+{
+ probe_state_t state = {0};
+
+ state.connected = probe.connected;
+ state.triggered = !!(Probe.reg->DR & Probe.bit) ^ probe.inverted;
+
+ return state;
+}
+
+#if !VFD_SPINDLE
+
+// Static spindle (off, on cw & on ccw)
+
+inline static void spindle_off ()
+{
+ DIGITAL_OUT(spindleEnable, settings.spindle.invert.on);
+}
+
+inline static void spindle_on ()
+{
+ DIGITAL_OUT(spindleEnable, !settings.spindle.invert.on);
+#ifdef SPINDLE_SYNC_ENABLE
+ spindleDataReset();
+#endif
+}
+
+inline static void spindle_dir (bool ccw)
+{
+ DIGITAL_OUT(spindleDir, ccw ^ settings.spindle.invert.ccw);
+}
+
+// Start or stop spindle.
+static void spindleSetState (spindle_state_t state, float rpm)
+{
+ if (!state.on)
+ spindle_off();
+ else {
+ if(hal.driver_cap.spindle_dir)
+ spindle_dir(state.ccw);
+ spindle_on();
+ }
+}
+
+// Variable spindle control functions
+
+// Set spindle speed.
+static void spindle_set_speed (uint_fast16_t pwm_value)
+{
+ if (pwm_value == spindle_pwm.off_value) {
+ if(settings.spindle.flags.pwm_action == SpindleAction_DisableWithZeroSPeed)
+ spindle_off();
+ pwmEnabled = false;
+ if(spindle_pwm.always_on) {
+#if SPINDLEPWMPIN == 12
+ TMR1_COMP21 = spindle_pwm.off_value;
+ TMR1_CMPLD11 = spindle_pwm.period - spindle_pwm.off_value;
+ TMR1_CTRL1 |= TMR_CTRL_CM(0b001);
+#else // 13
+ TMR2_COMP20 = spindle_pwm.off_value;
+ TMR2_CMPLD10 = spindle_pwm.period - spindle_pwm.off_value;
+ TMR2_CTRL0 |= TMR_CTRL_CM(0b001);
+#endif
+
+ } else {
+#if SPINDLEPWMPIN == 12
+ TMR1_CTRL1 &= ~TMR_CTRL_CM(0b111);
+ TMR1_SCTRL1 &= ~TMR_SCTRL_VAL; // set TMR_SCTRL_VAL || TMR_SCTRL_OPS if inverted PWM
+ TMR1_SCTRL1 |= TMR_SCTRL_FORCE;
+#else // 13
+ TMR2_CTRL0 &= ~TMR_CTRL_CM(0b111);
+ TMR2_SCTRL0 &= ~TMR_SCTRL_VAL; // set TMR_SCTRL_VAL || TMR_SCTRL_OPS if inverted PWM
+ TMR2_SCTRL0 |= TMR_SCTRL_FORCE;
+#endif
+ }
+ } else {
+ if(!pwmEnabled) {
+ spindle_on();
+ pwmEnabled = true;
+ }
+#if SPINDLEPWMPIN == 12
+ TMR1_COMP21 = pwm_value;
+ TMR1_CMPLD11 = spindle_pwm.period - pwm_value;
+ TMR1_CTRL1 |= TMR_CTRL_CM(0b001);
+#else // 13
+ TMR2_COMP20 = pwm_value;
+ TMR2_CMPLD10 = spindle_pwm.period - pwm_value;
+ TMR2_CTRL0 |= TMR_CTRL_CM(0b001);
+#endif
+ }
+}
+
+#ifdef SPINDLE_PWM_DIRECT
+
+// Convert spindle speed to PWM value.
+static uint_fast16_t spindleGetPWM (float rpm)
+{
+ return spindle_compute_pwm_value(&spindle_pwm, rpm, false);
+}
+
+#else
+
+// Update spindle speed.
+static void spindleUpdateRPM (float rpm)
+{
+ spindle_set_speed(spindle_compute_pwm_value(&spindle_pwm, rpm, false));
+}
+
+#endif
+
+// Start or stop spindle.
+static void spindleSetStateVariable (spindle_state_t state, float rpm)
+{
+ if (!state.on || rpm == 0.0f) {
+ spindle_set_speed(spindle_pwm.off_value);
+ spindle_off();
+ } else {
+ if(hal.driver_cap.spindle_dir)
+ spindle_dir(state.ccw);
+ spindle_set_speed(spindle_compute_pwm_value(&spindle_pwm, rpm, false));
+ }
+
+#ifdef SPINDLE_SYNC_ENABLE
+ if(settings.spindle.at_speed_tolerance > 0.0f) {
+ float tolerance = rpm * settings.spindle.at_speed_tolerance / 100.0f;
+ spindle_data.rpm_low_limit = rpm - tolerance;
+ spindle_data.rpm_high_limit = rpm + tolerance;
+ }
+ spindle_data.rpm_programmed = spindle_data.rpm = rpm;
+#endif
+}
+
+// Returns spindle state in a spindle_state_t variable.
+// spindle_state_t is defined in grbl/spindle_control.h
+static spindle_state_t spindleGetState (void)
+{
+ spindle_state_t state = {settings.spindle.invert.mask};
+
+ state.on = (spindleEnable.reg->DR & spindleEnable.bit) != 0;
+
+ if(hal.driver_cap.spindle_dir)
+ state.ccw = (spindleDir.reg->DR & spindleDir.bit) != 0;
+
+ state.value ^= settings.spindle.invert.mask;
+
+ if(pwmEnabled)
+ state.on |= pwmEnabled;
+
+#ifdef SPINDLE_SYNC_ENABLE
+ float rpm = spindleGetData(SpindleData_RPM)->rpm;
+ state.at_speed = settings.spindle.at_speed_tolerance <= 0.0f || (rpm >= spindle_data.rpm_low_limit && rpm <= spindle_data.rpm_high_limit);
+#endif
+
+ return state;
+}
+
+#if PPI_ENABLE
+
+static void spindlePulseOn (uint_fast16_t pulse_length)
+{
+ static uint_fast16_t plen = 0;
+
+ if(plen != pulse_length) {
+ plen = pulse_length;
+ PPI_TIMER.CH[0].COMP1 = (uint16_t)((pulse_length * F_BUS_MHZ) / 128);
+ }
+
+ spindle_on();
+ PPI_TIMER.CH[0].CTRL |= TMR_CTRL_CM(0b001);
+}
+
+#endif
+
+#ifdef SPINDLE_SYNC_ENABLE
+
+static spindle_data_t *spindleGetData (spindle_data_request_t request)
+{
+ bool stopped;
+ uint32_t pulse_length, rpm_timer_delta;
+ spindle_encoder_counter_t encoder;
+
+ __disable_irq();
+
+ memcpy(&encoder, &spindle_encoder.counter, sizeof(spindle_encoder_counter_t));
+
+ pulse_length = spindle_encoder.timer.pulse_length / spindle_encoder.tics_per_irq;
+ rpm_timer_delta = GPT1_CNT - spindle_encoder.timer.last_pulse;
+
+ __enable_irq();
+
+ // If no (4) spindle pulses during last 250 ms assume RPM is 0
+ if((stopped = ((pulse_length == 0) || (rpm_timer_delta > spindle_encoder.maximum_tt)))) {
+ spindle_data.rpm = 0.0f;
+ rpm_timer_delta = (GPT2_CNT - spindle_encoder.counter.last_count) * pulse_length;
+ }
+
+ switch(request) {
+
+ case SpindleData_Counters:
+ spindle_data.pulse_count = GPT2_CNT;
+ spindle_data.index_count = encoder.index_count;
+ spindle_data.error_count = spindle_encoder.error_count;
+ break;
+
+ case SpindleData_RPM:
+ if(!stopped)
+ spindle_data.rpm = spindle_encoder.rpm_factor / (float)pulse_length;
+ break;
+
+ case SpindleData_AngularPosition:;
+ while(spindleLock);
+ int32_t d = encoder.last_count - encoder.last_index;
+ spindle_data.angular_position = (float)encoder.index_count +
+ ((float)(d) +
+ (pulse_length == 0 ? 0.0f : (float)rpm_timer_delta / (float)pulse_length)) *
+ spindle_encoder.pulse_distance;
+ break;
+ }
+
+ return &spindle_data;
+}
+
+static void spindleDataReset (void)
+{
+ while(spindleLock);
+
+ uint32_t timeout = millis() + 1000; // 1 second
+
+ uint32_t index_count = spindle_data.index_count + 2;
+ if(spindleGetData(SpindleData_RPM)->rpm > 0.0f) { // wait for index pulse if running
+
+ while(index_count != spindle_data.index_count && millis() <= timeout);
+
+// if(uwTick > timeout)
+// alarm?
+ }
+
+ GPT2_CR &= ~GPT_CR_EN; // Reset timer
+ GPT1_CR &= ~GPT_CR_EN; // Reset timer
+ GPT1_PR = 24;
+ GPT1_CR |= GPT_CR_EN;
+
+ spindle_encoder.timer.last_pulse =
+ spindle_encoder.timer.last_index = GPT1_CNT;
+
+ spindle_encoder.timer.pulse_length =
+ spindle_encoder.counter.last_count =
+ spindle_encoder.counter.last_index =
+ spindle_encoder.counter.pulse_count =
+ spindle_encoder.counter.index_count =
+ spindle_encoder.error_count = 0;
+
+ // Spindle pulse counter
+ GPT2_OCR1 = spindle_encoder.tics_per_irq;
+ GPT2_CR |= GPT_CR_EN;
+}
+
+#endif
+
+// end spindle code
+
+#endif
+
+// Start/stop coolant (and mist if enabled).
+// coolant_state_t is defined in grbl/coolant_control.h.
+static void coolantSetState (coolant_state_t mode)
+{
+ mode.value ^= settings.coolant_invert.mask;
+
+ DIGITAL_OUT(Flood, mode.flood);
+ DIGITAL_OUT(Mist, mode.mist);
+}
+
+// Returns coolant state in a coolant_state_t variable.
+// coolant_state_t is defined in grbl/coolant_control.h.
+static coolant_state_t coolantGetState (void)
+{
+ coolant_state_t state = {0};
+
+ state.flood = (Flood.reg->DR & Flood.bit) != 0;
+ state.mist = (Mist.reg->DR & Mist.bit) != 0;
+
+ state.value ^= settings.coolant_invert.mask;
+
+ return state;
+}
+
+#if ETHERNET_ENABLE
+static void reportIP (bool newopt)
+{
+ if(!newopt && (services.telnet || services.websocket)) {
+ hal.stream.write("[NETCON:");
+ hal.stream.write(services.telnet ? "Telnet" : "Websocket");
+ hal.stream.write("]" ASCII_EOL);
+ }
+}
+#endif
+
+// Helper functions for setting/clearing/inverting individual bits atomically (uninterruptable)
+static void bitsSetAtomic (volatile uint_fast16_t *ptr, uint_fast16_t bits)
+{
+ __disable_irq();
+ *ptr |= bits;
+ __enable_irq();
+}
+
+static uint_fast16_t bitsClearAtomic (volatile uint_fast16_t *ptr, uint_fast16_t bits)
+{
+ __disable_irq();
+ uint_fast16_t prev = *ptr;
+ *ptr &= ~bits;
+ __enable_irq();
+
+ return prev;
+}
+
+static uint_fast16_t valueSetAtomic (volatile uint_fast16_t *ptr, uint_fast16_t value)
+{
+ __disable_irq();
+ uint_fast16_t prev = *ptr;
+ *ptr = value;
+ __enable_irq();
+
+ return prev;
+}
+
+static void enable_irq (void)
+{
+ __enable_irq();
+}
+
+static void disable_irq (void)
+{
+ __disable_irq();
+}
+
+// Configures perhipherals when settings are initialized or changed
+static void settings_changed (settings_t *settings)
+{
+ if(IOInitDone) {
+
+ stepperEnable(settings->steppers.deenergize);
+
+#ifdef SQUARING_ENABLED
+ hal.stepper.disable_motors((axes_signals_t){0}, SquaringMode_Both);
+#endif
+
+#if !PLASMA_ENABLE && !defined(SPINDLE_RPM_CONTROLLED)
+
+ if(hal.driver_cap.variable_spindle && spindle_precompute_pwm_values(&spindle_pwm, F_BUS_ACTUAL / 2)) {
+ #if SPINDLEPWMPIN == 12
+ TMR1_COMP11 = spindle_pwm.period;
+ TMR1_CMPLD11 = spindle_pwm.period;
+ #else // 13
+ TMR2_COMP10 = spindle_pwm.period;
+ TMR2_CMPLD10 = spindle_pwm.period;
+ #endif
+ hal.spindle.set_state = spindleSetStateVariable;
+ } else
+ hal.spindle.set_state = spindleSetState;
+#elif !VFD_SPINDLE
+ hal.spindle.set_state = spindleSetState;
+#endif
+
+#ifdef SPINDLE_SYNC_ENABLE
+
+ if((hal.spindle.get_data = settings->spindle.ppr > 0 ? spindleGetData : NULL) && spindle_encoder.ppr != settings->spindle.ppr) {
+
+ hal.spindle.reset_data = spindleDataReset;
+ hal.spindle.set_state((spindle_state_t){0}, 0.0f);
+
+ pidf_init(&spindle_tracker.pid, &settings->position.pid);
+
+ float timer_resolution = 1.0f / 1000000.0f; // 1 us resolution
+
+ spindle_tracker.min_cycles_per_tick = (int32_t)ceilf(settings->steppers.pulse_microseconds * 2.0f + settings->steppers.pulse_delay_microseconds);
+ spindle_encoder.ppr = settings->spindle.ppr;
+ spindle_encoder.tics_per_irq = max(1, spindle_encoder.ppr / 32);
+ spindle_encoder.pulse_distance = 1.0f / spindle_encoder.ppr;
+ spindle_encoder.maximum_tt = (uint32_t)(2.0f / timer_resolution) / spindle_encoder.tics_per_irq;
+ spindle_encoder.rpm_factor = 60.0f / ((timer_resolution * (float)spindle_encoder.ppr));
+ spindleDataReset();
+ }
+
+#endif
+
+ // Stepper pulse timeout setup.
+ TMR4_CSCTRL0 &= ~(TMR_CSCTRL_TCF1|TMR_CSCTRL_TCF2);
+
+ pulse_length = (uint16_t)((float)F_BUS_MHZ * (settings->steppers.pulse_microseconds - STEP_PULSE_LATENCY));
+
+ if(hal.driver_cap.step_pulse_delay && settings->steppers.pulse_delay_microseconds > 0.0f) {
+ float delay = settings->steppers.pulse_delay_microseconds - STEP_PULSE_LATENCY;
+ if(delay <= STEP_PULSE_LATENCY)
+ delay = STEP_PULSE_LATENCY + 0.2f;
+ pulse_delay = (uint16_t)((float)F_BUS_MHZ * delay);
+ hal.stepper.pulse_start = stepperPulseStartDelayed;
+ } else
+ hal.stepper.pulse_start = stepperPulseStart;
+
+ TMR4_COMP10 = pulse_length;
+ TMR4_CSCTRL0 &= ~TMR_CSCTRL_TCF2EN;
+ TMR4_CTRL0 &= ~TMR_CTRL_OUTMODE(0b000);
+ attachInterruptVector(IRQ_QTIMER4, stepper_pulse_isr);
+
+#if PLASMA_ENABLE
+ TMR2_CSCTRL0 &= ~(TMR_CSCTRL_TCF1|TMR_CSCTRL_TCF2);
+ TMR2_COMP10 = pulse_length;
+ TMR2_CSCTRL0 &= ~TMR_CSCTRL_TCF2EN;
+ TMR2_CTRL0 &= ~TMR_CTRL_OUTMODE(0b000);
+#endif
+
+ /****************************************
+ * Control, limit & probe pins config *
+ ****************************************/
+
+ control_signals_t control_fei;
+ control_fei.mask = settings->control_disable_pullup.mask ^ settings->control_invert.mask;
+
+ axes_signals_t limit_fei;
+ limit_fei.mask = settings->limits.disable_pullup.mask ^ settings->limits.invert.mask;
+
+ bool pullup;
+ uint32_t i = sizeof(inputpin) / sizeof(input_signal_t);
+ input_signal_t *signal;
+
+ NVIC_DISABLE_IRQ(IRQ_GPIO6789);
+
+ do {
+
+ pullup = true;
+ signal = &inputpin[--i];
+ signal->irq_mode = IRQ_Mode_None;
+
+ switch(signal->id) {
+#if ESTOP_ENABLE
+ case Input_EStop:
+ pullup = !settings->control_disable_pullup.e_stop;
+ signal->debounce = hal.driver_cap.software_debounce;
+ signal->irq_mode = control_fei.e_stop ? IRQ_Mode_Falling : IRQ_Mode_Rising;
+ break;
+#else
+ case Input_Reset:
+ pullup = !settings->control_disable_pullup.reset;
+ signal->debounce = hal.driver_cap.software_debounce;
+ signal->irq_mode = control_fei.reset ? IRQ_Mode_Falling : IRQ_Mode_Rising;
+ break;
+#endif
+ case Input_FeedHold:
+ pullup = !settings->control_disable_pullup.feed_hold;
+ signal->debounce = hal.driver_cap.software_debounce;
+ signal->irq_mode = control_fei.feed_hold ? IRQ_Mode_Falling : IRQ_Mode_Rising;
+ break;
+
+ case Input_CycleStart:
+ pullup = !settings->control_disable_pullup.cycle_start;
+ signal->debounce = hal.driver_cap.software_debounce;
+ signal->irq_mode = control_fei.cycle_start ? IRQ_Mode_Falling : IRQ_Mode_Rising;
+ break;
+
+#if SAFETY_DOOR_ENABLE
+ case Input_SafetyDoor:
+ pullup = !settings->control_disable_pullup.safety_door_ajar;
+ signal->debounce = hal.driver_cap.software_debounce;
+ signal->irq_mode = control_fei.safety_door_ajar ? IRQ_Mode_Falling : IRQ_Mode_Rising;
+ break;
+#endif
+#ifdef LIMITS_OVERRIDE_PIN
+ case Input_LimitsOverride:
+ pullup = true;
+ signal->debounce = false;
+ break;
+#endif
+ case Input_Probe:
+ pullup = hal.driver_cap.probe_pull_up;
+ break;
+
+ case Input_LimitX:
+ case Input_LimitX_Max:
+ pullup = !settings->limits.disable_pullup.x;
+ signal->debounce = hal.driver_cap.software_debounce;
+ signal->irq_mode = limit_fei.x ? IRQ_Mode_Falling : IRQ_Mode_Rising;
+ break;
+
+ case Input_LimitY:
+ case Input_LimitY_Max:
+ pullup = !settings->limits.disable_pullup.y;
+ signal->irq_mode = limit_fei.y ? IRQ_Mode_Falling : IRQ_Mode_Rising;
+ break;
+
+ case Input_LimitZ:
+ case Input_LimitZ_Max:
+ pullup = !settings->limits.disable_pullup.z;
+ signal->irq_mode = limit_fei.z ? IRQ_Mode_Falling : IRQ_Mode_Rising;
+ break;
+#ifdef A_LIMIT_PIN
+ case Input_LimitA:
+ case Input_LimitA_Max:
+ pullup = !settings->limits.disable_pullup.a;
+ signal->irq_mode = limit_fei.a ? IRQ_Mode_Falling : IRQ_Mode_Rising;
+ break;
+#endif
+#ifdef B_LIMIT_PIN
+ case Input_LimitB:
+ case Input_LimitB_Max:
+ pullup = !settings->limits.disable_pullup.b;
+ signal->irq_mode = limit_fei.b ? IRQ_Mode_Falling : IRQ_Mode_Rising;
+ break;
+#endif
+#ifdef C_LIMIT_PIN
+ case Input_LimitC:
+ case Input_LimitC_Max:
+ pullup = !settings->limits.disable_pullup.b;
+ signal->irq_mode = limit_fei.b ? IRQ_Mode_Falling : IRQ_Mode_Rising;
+ break;
+#endif
+#if MPG_MODE_ENABLE
+ case Input_ModeSelect:
+ signal->irq_mode = IRQ_Mode_Change;
+ break;
+#endif
+#if KEYPAD_ENABLE
+ case Input_KeypadStrobe:
+ pullup = true;
+ signal->irq_mode = IRQ_Mode_Change;
+ break;
+#endif
+#ifdef SPINDLE_INDEX_PIN
+ case Input_SpindleIndex:
+ pullup = false;
+ signal->irq_mode = IRQ_Mode_Rising;
+ break;
+#endif
+#if QEI_ENABLE
+ case Input_QEI_A:
+ if(qei_enable)
+ signal->irq_mode = IRQ_Mode_Change;
+ break;
+
+ case Input_QEI_B:
+ if(qei_enable)
+ signal->irq_mode = IRQ_Mode_Change;
+ break;
+
+ #if QEI_INDEX_ENABLED
+ case Input_QEI_Index:
+ if(qei_enable)
+ signal->irq_mode = IRQ_Mode_None;
+ break;
+ #endif
+
+ #if QEI_SELECT_ENABLED
+ case Input_QEI_Select:
+ signal->debounce = hal.driver_cap.software_debounce;
+ if(qei_enable)
+ signal->irq_mode = IRQ_Mode_Falling;
+ break;
+ #endif
+#endif
+ default:
+ break;
+ }
+
+ pinMode(signal->pin, pullup ? INPUT_PULLUP : INPUT_PULLDOWN);
+ signal->gpio.reg = (gpio_reg_t *)digital_pin_to_info_PGM[signal->pin].reg;
+ signal->gpio.bit = digital_pin_to_info_PGM[signal->pin].mask;
+
+ if(signal->port != NULL)
+ memcpy(signal->port, &signal->gpio, sizeof(gpio_t));
+
+ if(signal->irq_mode != IRQ_Mode_None) {
+
+ if(signal->gpio.reg == (gpio_reg_t *)&GPIO6_DR)
+ signal->offset = 0;
+ else if(signal->gpio.reg == (gpio_reg_t *)&GPIO7_DR)
+ signal->offset = 1;
+ else if(signal->gpio.reg == (gpio_reg_t *)&GPIO8_DR)
+ signal->offset = 2;
+ else
+ signal->offset = 3;
+
+ if(signal->irq_mode == IRQ_Mode_Change)
+ signal->gpio.reg->EDGE_SEL |= signal->gpio.bit;
+ else {
+ signal->gpio.reg->EDGE_SEL &= ~signal->gpio.bit;
+ uint32_t iopin = __builtin_ctz(signal->gpio.bit);
+ if(iopin < 16) {
+ uint32_t shift = iopin << 1;
+ signal->gpio.reg->ICR1 = (signal->gpio.reg->ICR1 & ~(0b11 << shift)) | (signal->irq_mode << shift);
+ } else {
+ uint32_t shift = (iopin - 16) << 1;
+ signal->gpio.reg->ICR2 = (signal->gpio.reg->ICR2 & ~(0b11 << shift)) | (signal->irq_mode << shift);
+ }
+ }
+
+ signal->gpio.reg->ISR = signal->gpio.bit; // Clear interrupt.
+
+ if(signal->group != INPUT_GROUP_LIMIT) // If pin is not a limit pin
+ signal->gpio.reg->IMR |= signal->gpio.bit; // enable interrupt
+
+ signal->active = (signal->gpio.reg->DR & signal->gpio.bit) != 0;
+
+ if(signal->irq_mode != IRQ_Mode_Change)
+ signal->active = signal->active ^ (signal->irq_mode == IRQ_Mode_Falling ? 0 : 1);
+ }
+ } while(i);
+
+ NVIC_ENABLE_IRQ(IRQ_GPIO6789);
+ }
+}
+
+void pinModeOutput (gpio_t *gpio, uint8_t pin)
+{
+ pinMode(pin, OUTPUT);
+ gpio->reg = (gpio_reg_t *)digital_pin_to_info_PGM[pin].reg;
+ gpio->bit = digital_pin_to_info_PGM[pin].mask;
+}
+
+#if QEI_ENABLE
+
+static void qei_update (void)
+{
+ const uint8_t encoder_valid_state[] = {0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0};
+
+ uint_fast8_t idx;
+ qei_state_t state = {0};
+
+ state.a = (QEI_A.reg->DR & QEI_A.bit) != 0;
+ state.b = (QEI_B.reg->DR & QEI_B.bit) != 0;
+
+ idx = (((qei.state << 2) & 0x0F) | state.pins);
+
+ if(encoder_valid_state[idx] ) {
+
+ qei.state = ((qei.state << 4) | idx) & 0xFF;
+
+ if (qei.state == 0x42 || qei.state == 0xD4 || qei.state == 0x2B || qei.state == 0xBD) {
+ qei.count--;
+ if(qei.vel_timeout == 0) {
+ qei.encoder.event.position_changed = hal.encoder.on_event != NULL;
+ hal.encoder.on_event(&qei.encoder, qei.count);
+ }
+ } else if(qei.state == 0x81 || qei.state == 0x17 || qei.state == 0xE8 || qei.state == 0x7E) {
+ qei.count++;
+ if(qei.vel_timeout == 0) {
+ qei.encoder.event.position_changed = hal.encoder.on_event != NULL;
+ hal.encoder.on_event(&qei.encoder, qei.count);
+ }
+ }
+ }
+
+}
+
+static void qei_reset (uint_fast8_t id)
+{
+ qei.vel_timeout = 0;
+ qei.count = qei.vel_count = 0;
+ qei.vel_timestamp = millis();
+ qei.vel_timeout = qei.encoder.axis != 0xFF ? QEI_VELOCITY_TIMEOUT : 0;
+}
+
+// dummy handler, called on events if plugin init fails
+static void encoder_event (encoder_t *encoder, int32_t position)
+{
+ UNUSED(position);
+ encoder->event.events = 0;
+}
+
+#endif
+
+// Initializes MCU peripherals for Grbl use
+static bool driver_setup (settings_t *settings)
+{
+#if TRINAMIC_ENABLE && defined(BOARD_CNC_BOOSTERPACK) // Trinamic BoosterPack does not support mixed drivers
+ driver_settings.trinamic.driver_enable.mask = AXES_BITMASK;
+#endif
+
+ /******************
+ * Stepper init *
+ ******************/
+
+ PIT_MCR = 0x00;
+ CCM_CCGR1 |= CCM_CCGR1_PIT(CCM_CCGR_ON);
+
+ attachInterruptVector(IRQ_PIT, stepper_driver_isr);
+ NVIC_SET_PRIORITY(IRQ_PIT, 2);
+ NVIC_ENABLE_IRQ(IRQ_PIT);
+
+ TMR4_ENBL = 0;
+ TMR4_LOAD0 = 0;
+ TMR4_CTRL0 = TMR_CTRL_PCS(0b1000) | TMR_CTRL_ONCE | TMR_CTRL_LENGTH;
+ TMR4_CSCTRL0 = TMR_CSCTRL_TCF1EN;
+
+ attachInterruptVector(IRQ_QTIMER4, stepper_pulse_isr);
+ NVIC_SET_PRIORITY(IRQ_QTIMER4, 0);
+ NVIC_ENABLE_IRQ(IRQ_QTIMER4);
+
+ TMR4_ENBL = 1;
+
+#if PLASMA_ENABLE
+ TMR2_ENBL = 0;
+ TMR2_LOAD0 = 0;
+ TMR2_CTRL0 = TMR_CTRL_PCS(0b1000) | TMR_CTRL_ONCE | TMR_CTRL_LENGTH;
+ TMR2_CSCTRL0 = TMR_CSCTRL_TCF1EN;
+
+ attachInterruptVector(IRQ_QTIMER2, output_pulse_isr);
+ NVIC_SET_PRIORITY(IRQ_QTIMER2, 0);
+ NVIC_ENABLE_IRQ(IRQ_QTIMER2);
+
+ TMR2_ENBL = 1;
+#endif
+
+ pinModeOutput(&stepX, X_STEP_PIN);
+ pinModeOutput(&dirX, X_DIRECTION_PIN);
+#ifdef X_ENABLE_PIN
+ pinModeOutput(&enableX, X_ENABLE_PIN);
+#endif
+#ifdef X2_STEP_PIN
+ pinModeOutput(&stepX2, X2_STEP_PIN);
+#endif
+#ifdef X2_DIRECTION_PIN
+ pinModeOutput(&dirX2, X2_DIRECTION_PIN);
+#endif
+#ifdef X2_ENABLE_PIN
+ pinModeOutput(&enableX2, X2_ENABLE_PIN);
+#endif
+
+ pinModeOutput(&stepY, Y_STEP_PIN);
+ pinModeOutput(&dirY, Y_DIRECTION_PIN);
+#ifdef Y_ENABLE_PIN
+ pinModeOutput(&enableY, Y_ENABLE_PIN);
+#endif
+#ifdef Y2_STEP_PIN
+ pinModeOutput(&stepY2, Y2_STEP_PIN);
+#endif
+#ifdef Y2_DIRECTION_PIN
+ pinModeOutput(&dirY2, Y2_DIRECTION_PIN);
+#endif
+#ifdef Y2_ENABLE_PIN
+ pinModeOutput(&enableY2, Y2_ENABLE_PIN);
+#endif
+
+ pinModeOutput(&stepZ, Z_STEP_PIN);
+ pinModeOutput(&dirZ, Z_DIRECTION_PIN);
+#ifdef Z_ENABLE_PIN
+ pinModeOutput(&enableZ, Z_ENABLE_PIN);
+#endif
+#ifdef Z2_STEP_PIN
+ pinModeOutput(&stepZ2, Z2_STEP_PIN);
+#endif
+#ifdef Z2_DIRECTION_PIN
+ pinModeOutput(&dirZ2, Z2_DIRECTION_PIN);
+#endif
+#ifdef Z2_ENABLE_PIN
+ pinModeOutput(&enableZ2, Z2_ENABLE_PIN);
+#endif
+
+#ifdef A_AXIS
+ pinModeOutput(&stepA, A_STEP_PIN);
+ pinModeOutput(&dirA, A_DIRECTION_PIN);
+ #ifdef A_ENABLE_PIN
+ pinModeOutput(&enableA, A_ENABLE_PIN);
+ #endif
+#endif
+
+#ifdef B_AXIS
+ pinModeOutput(&stepB, B_STEP_PIN);
+ pinModeOutput(&dirB, B_DIRECTION_PIN);
+ #ifdef B_ENABLE_PIN
+ pinModeOutput(&enableB, B_ENABLE_PIN);
+ #endif
+#endif
+
+#ifdef C_AXIS
+ pinModeOutput(&stepC, C_STEP_PIN);
+ pinModeOutput(&dirC, C_DIRECTION_PIN);
+ #ifdef C_ENABLE_PIN
+ pinModeOutput(&enableC, C_ENABLE_PIN);
+ #endif
+#endif
+
+#ifdef STEPPERS_ENABLE_PIN
+ pinModeOutput(&steppersEnable, STEPPERS_ENABLE_PIN);
+#endif
+
+ /****************************
+ * Software debounce init *
+ ****************************/
+
+ if(hal.driver_cap.software_debounce) {
+
+ TMR3_ENBL = 0;
+ TMR3_LOAD0 = 0;
+ TMR3_CTRL0 = TMR_CTRL_PCS(0b1111) | TMR_CTRL_ONCE | TMR_CTRL_LENGTH;
+ TMR3_COMP10 = (uint16_t)((40000UL * F_BUS_MHZ) / 128); // 150 MHz -> 40ms
+ TMR3_CSCTRL0 = TMR_CSCTRL_TCF1EN;
+
+ attachInterruptVector(IRQ_QTIMER3, debounce_isr);
+ NVIC_SET_PRIORITY(IRQ_QTIMER3, 4);
+ NVIC_ENABLE_IRQ(IRQ_QTIMER3);
+
+ TMR3_ENBL = 1;
+ }
+
+ /***********************
+ * Control pins init *
+ ***********************/
+
+ attachInterruptVector(IRQ_GPIO6789, gpio_isr);
+
+ /***********************
+ * Coolant pins init *
+ ***********************/
+
+ pinModeOutput(&Flood, COOLANT_FLOOD_PIN);
+ pinModeOutput(&Mist, COOLANT_MIST_PIN);
+
+#if !VFD_SPINDLE
+
+ /******************
+ * Spindle init *
+ ******************/
+
+ pinModeOutput(&spindleEnable, SPINDLE_ENABLE_PIN);
+ pinModeOutput(&spindleDir, SPINDLE_DIRECTION_PIN);
+
+#if !PLASMA_ENABLE
+
+#if SPINDLEPWMPIN == 12
+ TMR1_ENBL = 0;
+ TMR1_LOAD1 = 0;
+ TMR1_CTRL1 = TMR_CTRL_PCS(0b1001) | TMR_CTRL_OUTMODE(0b100) | TMR_CTRL_LENGTH;
+ TMR1_SCTRL1 = TMR_SCTRL_OEN | TMR_SCTRL_FORCE; // set TMR_SCTRL_VAL || TMR_SCTRL_OPS if inverted PWM
+ TMR1_ENBL = 1 << 1;
+#else // 13
+ TMR2_ENBL = 0;
+ TMR2_LOAD0 = 0;
+ TMR2_CTRL0 = TMR_CTRL_PCS(0b1001) | TMR_CTRL_OUTMODE(0b100) | TMR_CTRL_LENGTH;
+ TMR2_SCTRL0 = TMR_SCTRL_OEN | TMR_SCTRL_FORCE; // set TMR_SCTRL_VAL || TMR_SCTRL_OPS if inverted PWM
+ TMR2_ENBL = 1;
+#endif
+
+ *(portConfigRegister(SPINDLEPWMPIN)) = 1;
+
+#ifdef SPINDLE_SYNC_ENABLE
+
+ CCM_CCGR1 |= CCM_CCGR1_GPT1_BUS(CCM_CCGR_ON);
+ CCM_CMEOR |= CCM_CMEOR_MOD_EN_OV_GPT;
+
+ // Free running timer
+ GPT1_CR = 0;
+ GPT1_CR |= GPT_CR_SWR;
+ while(GPT1_CR & GPT_CR_SWR);
+ GPT1_CR = GPT_CR_CLKSRC(1);
+ GPT1_CR |= GPT_CR_FRR|GPT_CR_ENMOD|GPT_CR_EN;
+ GPT1_PR = 150;
+
+ #if SPINDLE_PULSE_PIN == 14
+
+ CCM_CCGR0 |= CCM_CCGR0_GPT2_BUS(CCM_CCGR_ON);
+
+ IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B1_02 = 8;
+ IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B1_02 = IOMUXC_PAD_DSE(7) | IOMUXC_PAD_PKE | IOMUXC_PAD_PUE | IOMUXC_PAD_PUS(3) | IOMUXC_PAD_HYS;
+// IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B1_02 = 0x001d0b0;
+ IOMUXC_GPT2_IPP_IND_CLKIN_SELECT_INPUT = 1;
+
+ // Spindle pulse counter
+ GPT2_CR = 0;
+ GPT2_CR |= GPT_CR_SWR;
+ while(GPT2_CR & GPT_CR_SWR);
+ GPT2_CR = GPT_CR_CLKSRC(3);
+ GPT2_CR |= GPT_CR_ENMOD;
+ GPT2_CR |= GPT_CR_FRR|GPT_CR_EN;
+ GPT2_OCR1 = spindle_encoder.tics_per_irq;
+ GPT2_IR = GPT_IR_OF1IE;
+
+ attachInterruptVector(IRQ_GPT2, spindle_pulse_isr);
+ NVIC_SET_PRIORITY(IRQ_GPT2, 1);
+ NVIC_ENABLE_IRQ(IRQ_GPT2);
+
+ #endif
+
+#endif // SPINDLE_SYNC_ENABLE
+
+#if PPI_ENABLE
+
+ PPI_TIMER.ENBL = 0;
+ PPI_TIMER.CH[0].LOAD = 0;
+ PPI_TIMER.CH[0].COMP1 = (uint16_t)((1500UL * F_BUS_MHZ) / 128);
+ PPI_TIMER.CH[0].CTRL = TMR_CTRL_PCS(0b1111) | TMR_CTRL_ONCE | TMR_CTRL_LENGTH;
+ PPI_TIMER.CH[0].CSCTRL = TMR_CSCTRL_TCF1EN;
+
+ attachInterruptVector(PPI_TIMERIRQ, ppi_timeout_isr);
+ NVIC_SET_PRIORITY(PPI_TIMERIRQ, 3);
+ NVIC_ENABLE_IRQ(PPI_TIMERIRQ);
+
+ PPI_TIMER.ENBL = 1;
+
+ ppi_init();
+
+ #endif // PPI_ENABLE
+
+#endif // !PLASMA_ENABLE
+
+#endif // !VFD_SPINDLE
+
+ // Set defaults
+
+ IOInitDone = settings->version == 19;
+
+ hal.settings_changed(settings);
+ hal.stepper.go_idle(true);
+
+#if IOPORTS_ENABLE
+ ioports_init();
+#endif
+
+#if SDCARD_ENABLE
+ sdcard_init();
+#endif
+
+#if ETHERNET_ENABLE
+ grbl_enet_start();
+#endif
+
+#if QEI_ENABLE
+ if(qei_enable)
+ encoder_start(&qei.encoder);
+#endif
+
+ return IOInitDone;
+}
+
+#if EEPROM_ENABLE == 0
+
+// EEPROM emulation - stores settings in flash
+
+bool nvsRead (uint8_t *dest)
+{
+// assert size ? E2END
+
+ eeprom_read_block(dest, 0, hal.nvs.size);
+
+ return true; //?;
+}
+
+bool nvsWrite (uint8_t *source)
+{
+ eeprom_write_block(source, 0, hal.nvs.size);
+
+ return true; //?;
+}
+
+// End EEPROM emulation
+
+#endif
+
+#if ETHERNET_ENABLE || ADD_MSEVENT
+
+static void execute_realtime (uint_fast16_t state)
+{
+#if ADD_MSEVENT
+ if(ms_event) {
+
+ ms_event = false;
+
+ #if USB_SERIAL_CDC
+ usb_execute_realtime(state);
+ #endif
+
+ #if QEI_ENABLE
+ if(qei.vel_timeout && !(--qei.vel_timeout)) {
+ qei.encoder.velocity = abs(qei.count - qei.vel_count) * 1000 / (millis() - qei.vel_timestamp);
+ qei.vel_timestamp = millis();
+ qei.vel_timeout = QEI_VELOCITY_TIMEOUT;
+ if((qei.encoder.event.position_changed = !qei.dbl_click_timeout || qei.encoder.velocity == 0))
+ hal.encoder.on_event(&qei.encoder, qei.count);
+ qei.vel_count = qei.count;
+ }
+
+ if(qei.dbl_click_timeout && !(--qei.dbl_click_timeout)) {
+ qei.encoder.event.click = On;
+ hal.encoder.on_event(&qei.encoder, qei.count);
+ }
+ #endif
+ }
+#endif // ADD_MSEVENT
+
+#if ETHERNET_ENABLE
+ grbl_enet_poll();
+#endif
+}
+
+#endif
+
+#ifdef DEBUGOUT
+void debugOut (bool on)
+{
+ digitalWrite(13, on); // LED
+}
+#endif
+
+#ifdef UART_DEBUG
+void uart_debug_write (char *s)
+{
+ serialWriteS(s);
+ while(serialTxCount()); // Wait until message is delivered
+}
+#endif
+
+// Cold restart (T4.x has no reset button)
+static void reboot (void)
+{
+ SCB_AIRCR = 0x05FA0004;
+}
+
+// Initialize HAL pointers, setup serial comms and enable EEPROM.
+// NOTE: Grbl is not yet configured (from EEPROM data), driver_setup() will be called when done.
+bool driver_init (void)
+{
+ static char options[30];
+
+ // Chain our systick isr to the Arduino handler
+
+ if(systick_isr_org == NULL)
+ systick_isr_org = _VectorsRam[15];
+ _VectorsRam[15] = systick_isr;
+
+ // Enable lazy stacking of FPU registers here if a FPU is available.
+
+ // FPU->FPCCR = (FPU->FPCCR & ~FPU_FPCCR_LSPEN_Msk) | FPU_FPCCR_ASPEN_Msk; // enable lazy stacking
+
+ options[0] = '\0';
+
+#if USB_SERIAL_CDC == 1
+ strcat(options, "USB.1 ");
+#endif
+#if USB_SERIAL_CDC == 2
+ strcat(options, "USB.2 ");
+#endif
+
+ if(*options != '\0')
+ options[strlen(options) - 1] = '\0';
+
+ hal.info = "iMXRT1062";
+ hal.driver_version = "210221";
+#ifdef BOARD_NAME
+ hal.board = BOARD_NAME;
+#endif
+ hal.driver_options = *options == '\0' ? NULL : options;
+ hal.driver_setup = driver_setup;
+ hal.f_step_timer = 24000000;
+ hal.rx_buffer_size = RX_BUFFER_SIZE;
+ hal.delay_ms = driver_delay_ms;
+ hal.settings_changed = settings_changed;
+
+ hal.stepper.wake_up = stepperWakeUp;
+ hal.stepper.go_idle = stepperGoIdle;
+ hal.stepper.enable = stepperEnable;
+ hal.stepper.cycles_per_tick = stepperCyclesPerTick;
+ hal.stepper.pulse_start = stepperPulseStart;
+#ifdef SQUARING_ENABLED
+ hal.stepper.get_auto_squared = getAutoSquaredAxes;
+ hal.stepper.disable_motors = StepperDisableMotors;
+#endif
+
+ hal.limits.enable = limitsEnable;
+ hal.limits.get_state = limitsGetState;
+ hal.homing.get_state = limitsGetState;
+
+ hal.coolant.set_state = coolantSetState;
+ hal.coolant.get_state = coolantGetState;
+
+ hal.probe.configure = probeConfigure;
+ hal.probe.get_state = probeGetState;
+
+#if !VFD_SPINDLE
+ hal.spindle.set_state = spindleSetState;
+ hal.spindle.get_state = spindleGetState;
+ #ifdef SPINDLE_PWM_DIRECT
+ hal.spindle.get_pwm = spindleGetPWM;
+ hal.spindle.update_pwm = spindle_set_speed;
+ #else
+ hal.spindle.update_rpm = spindleUpdateRPM;
+ #endif
+ #if PPI_ENABLE
+ hal.spindle.pulse_on = spindlePulseOn;
+ #endif
+#endif
+#ifdef SPINDLE_SYNC_ENABLE
+ hal.driver_cap.spindle_sync = On;
+ hal.driver_cap.spindle_at_speed = On;
+#endif
+ hal.control.get_state = systemGetState;
+
+#if ETHERNET_ENABLE
+ grbl.on_report_options = reportIP;
+#endif
+
+#if USB_SERIAL_CDC
+ usb_serialInit();
+#else
+ serialInit(115200);
+#endif
+
+ selectStream(StreamType_Serial);
+
+#ifdef I2C_PORT
+ i2c_init();
+#endif
+
+#if EEPROM_ENABLE
+ i2c_eeprom_init();
+#else // use Arduino emulated EEPROM in flash
+ eeprom_initialize();
+ hal.nvs.type = NVS_Flash;
+ hal.nvs.memcpy_from_flash = nvsRead;
+ hal.nvs.memcpy_to_flash = nvsWrite;
+#endif
+
+ hal.reboot = reboot;
+ hal.irq_enable = enable_irq;
+ hal.irq_disable = disable_irq;
+ hal.set_bits_atomic = bitsSetAtomic;
+ hal.clear_bits_atomic = bitsClearAtomic;
+ hal.set_value_atomic = valueSetAtomic;
+ hal.get_elapsed_ticks = millis;
+
+#if ETHERNET_ENABLE || ADD_MSEVENT
+ grbl.on_execute_realtime = execute_realtime;
+#endif
+
+#if QEI_ENABLE
+ hal.encoder.reset = qei_reset;
+ hal.encoder.on_event = encoder_event;
+#endif
+
+#if MODBUS_ENABLE
+ modbus_stream.write = serialWrite;
+ modbus_stream.read = serialGetC;
+ modbus_stream.flush_rx_buffer = serialRxFlush;
+ modbus_stream.flush_tx_buffer = serialTxFlush;
+ modbus_stream.get_rx_buffer_count = serialRxCount;
+ modbus_stream.get_tx_buffer_count = serialTxCount;
+ modbus_stream.set_baud_rate = serialSetBaudRate;
+
+ bool modbus = modbus_init(&modbus_stream);
+
+#if SPINDLE_HUANYANG
+ if(modbus)
+ huanyang_init(&modbus_stream);
+#endif
+
+#endif
+
+ // Driver capabilities, used for announcing and negotiating (with Grbl) driver functionality.
+ // See driver_cap_t union i grbl/hal.h for available flags.
+
+#if ESTOP_ENABLE
+ hal.signals_cap.e_stop = On;
+#endif
+#if SAFETY_DOOR_ENABLE
+ hal.signals_cap.safety_door_ajar = On;
+#endif
+#ifdef LIMITS_OVERRIDE_PIN
+ hal.signals_cap.limits_override = On;
+#endif
+
+#if !VFD_SPINDLE && !PLASMA_ENABLE
+ #ifdef SPINDLE_DIRECTION_PIN
+ hal.driver_cap.spindle_dir = On;
+ #endif
+ hal.driver_cap.variable_spindle = On;
+#endif
+#ifdef COOLANT_MIST_PIN
+ hal.driver_cap.mist_control = On;
+#endif
+ hal.driver_cap.software_debounce = On;
+ hal.driver_cap.step_pulse_delay = On;
+ hal.driver_cap.amass_level = 3;
+ hal.driver_cap.control_pull_up = On;
+ hal.driver_cap.limits_pull_up = On;
+ hal.driver_cap.probe_pull_up = On;
+
+#ifdef DEBUGOUT
+ hal.debug_out = debugOut;
+#endif
+
+#ifdef UART_DEBUG
+ serialInit(115200);
+ uart_debug_write(ASCII_EOL "UART Debug:" ASCII_EOL);
+#endif
+
+#ifdef HAS_BOARD_INIT
+ board_init();
+#endif
+
+#if ETHERNET_ENABLE
+ grbl_enet_init();
+#endif
+
+#if KEYPAD_ENABLE
+ keypad_init();
+#endif
+
+#if QEI_ENABLE
+ qei_enable = encoder_init(QEI_ENABLE);
+#endif
+
+#if PLASMA_ENABLE
+ hal.stepper.output_step = stepperOutputStep;
+ plasma_init();
+#endif
+
+ my_plugin_init();
+
+#if ODOMETER_ENABLE
+ odometer_init(); // NOTE: this *must* be last plugin to be initialized as it claims storage at the end of NVS.
+#endif
+
+ // No need to move version check before init.
+ // Compiler will fail any signature mismatch for existing entries.
+ return hal.version == 8;
+}
+
+/* interrupt handlers */
+
+// Main stepper driver.
+static void stepper_driver_isr (void)
+{
+ if(PIT_TFLG0 & PIT_TFLG_TIF) {
+ PIT_TFLG0 |= PIT_TFLG_TIF;
+ hal.stepper.interrupt_callback();
+ }
+}
+
+/* The Stepper Port Reset Interrupt: This interrupt handles the falling edge of the step
+ pulse. This should always trigger before the next general stepper driver interrupt and independently
+ finish, if stepper driver interrupts is disabled after completing a move.
+ NOTE: Interrupt collisions between the serial and stepper interrupts can cause delays by
+ a few microseconds, if they execute right before one another. Not a big deal, but can
+ cause issues at high step rates if another high frequency asynchronous interrupt is
+ added to Grbl.
+*/
+// This interrupt is enabled when Grbl sets the motor port bits to execute
+// a step. This ISR resets the motor port after a short period (settings.pulse_microseconds)
+// completing one step cycle.
+static void stepper_pulse_isr (void)
+{
+ TMR4_CSCTRL0 &= ~TMR_CSCTRL_TCF1;
+
+ set_step_outputs((axes_signals_t){0});
+}
+
+static void stepper_pulse_isr_delayed (void)
+{
+ TMR4_CSCTRL0 &= ~TMR_CSCTRL_TCF1;
+
+ set_step_outputs(next_step_outbits);
+
+ attachInterruptVector(IRQ_QTIMER4, stepper_pulse_isr);
+ TMR4_COMP10 = pulse_length;
+ TMR4_CTRL0 |= TMR_CTRL_CM(0b001);
+}
+
+#if defined(SPINDLE_SYNC_ENABLE) && SPINDLE_PULSE_PIN == 14
+
+static void spindle_pulse_isr (void)
+{
+ uint32_t tval = GPT1_CNT;
+
+ GPT2_SR |= GPT_SR_OF1; // clear interrupt flag
+ GPT2_OCR1 += spindle_encoder.tics_per_irq;
+
+ spindleLock = true;
+
+ spindle_encoder.counter.pulse_count = GPT2_CNT;
+ spindle_encoder.counter.last_count = spindle_encoder.counter.pulse_count;
+ spindle_encoder.timer.pulse_length = tval - spindle_encoder.timer.last_pulse;
+ spindle_encoder.timer.last_pulse = tval;
+
+ spindleLock = false;
+}
+
+#endif
+
+#if PLASMA_ENABLE
+static void output_pulse_isr(void)
+{
+ axes_signals_t output = {pulse_output.mask ^ settings.steppers.dir_invert.mask};
+
+ TMR2_CSCTRL0 &= ~TMR_CSCTRL_TCF1;
+
+ DIGITAL_OUT(stepZ, output.z);
+
+ if(pulse_output.value) {
+ pulse_output.value = 0;
+ TMR2_CTRL0 |= TMR_CTRL_CM(0b001);
+ }
+}
+#endif
+
+#if PPI_ENABLE
+// Switches off the spindle (laser) after laser.pulse_length time has elapsed
+static void ppi_timeout_isr (void)
+{
+ PPI_TIMER.CH[0].CSCTRL &= ~TMR_CSCTRL_TCF1;
+ spindle_off();
+}
+#endif
+
+inline static bool enqueue_debounce (input_signal_t *signal)
+{
+ bool ok;
+ uint_fast8_t bptr = (debounce_queue.head + 1) & (DEBOUNCE_QUEUE - 1);
+
+ if((ok = bptr != debounce_queue.tail)) {
+ debounce_queue.signal[debounce_queue.head] = signal;
+ debounce_queue.head = bptr;
+ }
+
+ return ok;
+}
+
+// Returns NULL if no debounce checks enqueued
+inline static input_signal_t *get_debounce (void)
+{
+ input_signal_t *signal = NULL;
+ uint_fast8_t bptr = debounce_queue.tail;
+
+ if(bptr != debounce_queue.head) {
+ signal = debounce_queue.signal[bptr++];
+ debounce_queue.tail = bptr & (DEBOUNCE_QUEUE - 1);
+ }
+
+ return signal;
+}
+
+static void debounce_isr (void)
+{
+ uint8_t grp = 0;
+ input_signal_t *signal;
+
+ TMR3_CSCTRL0 &= ~TMR_CSCTRL_TCF1;
+
+ while((signal = get_debounce())) {
+
+ signal->gpio.reg->IMR |= signal->gpio.bit;
+
+ if(((signal->gpio.reg->DR & signal->gpio.bit) != 0) == (signal->irq_mode == IRQ_Mode_Falling ? 0 : 1))
+ grp |= signal->group;
+ }
+
+ if(grp & INPUT_GROUP_LIMIT)
+ hal.limits.interrupt_callback(limitsGetState());
+
+ if(grp & INPUT_GROUP_CONTROL)
+ hal.control.interrupt_callback(systemGetState());
+
+#if QEI_SELECT_ENABLED
+
+ if(grp & INPUT_GROUP_QEI_SELECT) {
+ if(!qei.dbl_click_timeout)
+ qei.dbl_click_timeout = qei.encoder.settings->dbl_click_window;
+ else if(qei.dbl_click_timeout < qei.encoder.settings->dbl_click_window - 40) {
+ qei.dbl_click_timeout = 0;
+ qei.encoder.event.dbl_click = On;
+ hal.encoder.on_event(&qei.encoder, qei.count);
+ }
+ }
+
+#endif
+}
+
+ //GPIO intr process
+static void gpio_isr (void)
+{
+ bool debounce = false;
+ uint8_t grp = 0;
+ uint32_t intr_status[4];
+
+ // Get masked interrupt status
+ intr_status[0] = ((gpio_reg_t *)&GPIO6_DR)->ISR & ((gpio_reg_t *)&GPIO6_DR)->IMR;
+ intr_status[1] = ((gpio_reg_t *)&GPIO7_DR)->ISR & ((gpio_reg_t *)&GPIO7_DR)->IMR;
+ intr_status[2] = ((gpio_reg_t *)&GPIO8_DR)->ISR & ((gpio_reg_t *)&GPIO8_DR)->IMR;
+ intr_status[3] = ((gpio_reg_t *)&GPIO9_DR)->ISR & ((gpio_reg_t *)&GPIO9_DR)->IMR;
+
+ // Clear interrupts
+ ((gpio_reg_t *)&GPIO6_DR)->ISR = intr_status[0];
+ ((gpio_reg_t *)&GPIO7_DR)->ISR = intr_status[1];
+ ((gpio_reg_t *)&GPIO8_DR)->ISR = intr_status[2];
+ ((gpio_reg_t *)&GPIO9_DR)->ISR = intr_status[3];
+
+ uint32_t i = sizeof(inputpin) / sizeof(input_signal_t);
+ do {
+ if(inputpin[--i].irq_mode != IRQ_Mode_None) {
+
+ if(intr_status[inputpin[i].offset] & inputpin[i].gpio.bit) {
+ inputpin[i].active = true;
+ if(inputpin[i].debounce && enqueue_debounce(&inputpin[i])) {
+ inputpin[i].gpio.reg->IMR &= ~inputpin[i].gpio.bit;
+ debounce = true;
+ } else {
+#if QEI_ENABLE
+ if(inputpin[i].group & INPUT_GROUP_QEI) {
+ qei_update();
+ /*
+ QEI_A.reg->IMR &= ~QEI_A.bit; // Switch off
+ QEI_B.reg->IMR &= ~QEI_B.bit; // encoder interrupts.
+ qei.iflags.a = inputpin[i].port == &QEI_A;
+ qei.iflags.b = inputpin[i].port == &QEI_B;
+ qei.debounce = QEI_DEBOUNCE;
+ qei.initial_debounce = true;
+ */
+ } else
+#endif
+
+#if defined(SPINDLE_SYNC_ENABLE) && defined(SPINDLE_INDEX_PIN)
+ if(inputpin[i].group & INPUT_GROUP_SPINDLE_INDEX) {
+ spindleLock = true;
+ spindle_encoder.counter.index_count++;
+ spindle_encoder.counter.last_index = GPT2_CNT;
+ spindle_encoder.timer.last_index = GPT1_CNT;
+ spindleLock = false;
+ } else
+#endif
+ grp |= inputpin[i].group;
+ }
+ }
+ }
+ } while(i);
+
+ if(debounce) {
+ TMR3_CTRL0 |= TMR_CTRL_CM(0b001);
+ }
+
+ if(grp & INPUT_GROUP_LIMIT) {
+ limit_signals_t state = limitsGetState();
+ if(limit_signals_merge(state).value) //TODO: add check for limit switches having same state as when limit_isr were invoked?
+ hal.limits.interrupt_callback(state);
+ }
+
+ if(grp & INPUT_GROUP_CONTROL)
+ hal.control.interrupt_callback(systemGetState());
+
+#if QEI_SELECT_ENABLED
+
+ if(grp & INPUT_GROUP_QEI_SELECT) {
+ if(!qei.dbl_click_timeout)
+ qei.dbl_click_timeout = qei.encoder.settings->dbl_click_window;
+ else if(qei.dbl_click_timeout < qei.encoder.settings->dbl_click_window - 40) {
+ qei.dbl_click_timeout = 0;
+ qei.encoder.event.dbl_click = On;
+ hal.encoder.on_event(&qei.encoder, qei.count);
+ }
+ }
+
+#endif
+
+#if MPG_MODE_ENABLE
+
+ static bool mpg_mutex = false;
+
+ if((grp & INPUT_GROUP_MPG) && !mpg_mutex) {
+ mpg_mutex = true;
+ modeChange();
+ // hal.delay_ms(50, modeChange);
+ mpg_mutex = false;
+ }
+#endif
+
+#if KEYPAD_ENABLE
+ if(grp & INPUT_GROUP_KEYPAD)
+ keypad_keyclick_handler(!(KeypadStrobe.reg->DR & KeypadStrobe.bit));
+#endif
+}
+
+// Interrupt handler for 1 ms interval timer
+static void systick_isr (void)
+{
+ systick_isr_org();
+
+#if ADD_MSEVENT
+ ms_event = true;
+#endif
+
+ if(grbl_delay.ms && !(--grbl_delay.ms)) {
+ if(grbl_delay.callback) {
+ grbl_delay.callback();
+ grbl_delay.callback = NULL;
+ }
+ }
+}
diff --git a/grblHAL_Teensy4/src/driver.h b/grblHAL_Teensy4/src/driver.h
new file mode 100644
index 0000000..8a9641f
--- /dev/null
+++ b/grblHAL_Teensy4/src/driver.h
@@ -0,0 +1,340 @@
+/*
+ driver.h - driver code for IMXRT1062 processor (on Teensy 4.0 board)
+
+ Part of grblHAL
+
+ Copyright (c) 2020-2021 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+//
+// NOTE: do NOT change configuration here - edit my_machine.h instead!
+//
+
+#ifndef __DRIVER_H__
+#define __DRIVER_H__
+
+#include "imxrt.h"
+#include "core_pins.h"
+#include "pins_arduino.h"
+
+#ifndef OVERRIDE_MY_MACHINE
+#include "my_machine.h"
+#endif
+
+#include "grbl/hal.h"
+#include "grbl/nuts_bolts.h"
+
+
+#define DIGITAL_IN(gpio) (!!(gpio.reg->DR & gpio.bit))
+#define DIGITAL_OUT(gpio, on) { if(on) gpio.reg->DR_SET = gpio.bit; else gpio.reg->DR_CLEAR = gpio.bit; }
+
+#if USB_SERIAL_CDC > 0
+//#define UART_DEBUG // For development only - enable only with USB_SERIAL_CDC enabled and SPINDLE_HUANYANG disabled
+#endif
+
+#ifndef USB_SERIAL_CDC
+#define USB_SERIAL_CDC 0 // for UART comms
+#endif
+#ifndef USB_SERIAL_WAIT
+#define USB_SERIAL_WAIT 0
+#endif
+#ifndef PLASMA_ENABLE
+#define PLASMA_ENABLE 0
+#endif
+#ifndef PPI_ENABLE
+#define PPI_ENABLE 0
+#endif
+#ifndef SPINDLE_HUANYANG
+#define SPINDLE_HUANYANG 0
+#endif
+#ifndef QEI_ENABLE
+#define QEI_ENABLE 0
+#endif
+#ifndef ODOMETER_ENABLE
+#define ODOMETER_ENABLE 0
+#endif
+
+#ifndef ETHERNET_ENABLE
+#define ETHERNET_ENABLE 0
+#endif
+#ifndef TELNET_ENABLE
+#define TELNET_ENABLE 0
+#endif
+#ifndef WEBSOCKET_ENABLE
+#define WEBSOCKET_ENABLE 0
+#endif
+
+#ifndef SDCARD_ENABLE
+#define SDCARD_ENABLE 0
+#endif
+#ifndef KEYPAD_ENABLE
+#define KEYPAD_ENABLE 0
+#endif
+#ifndef EEPROM_ENABLE
+#define EEPROM_ENABLE 0
+#endif
+#ifndef EEPROM_IS_FRAM
+#define EEPROM_IS_FRAM 0
+#endif
+#ifndef TRINAMIC_ENABLE
+#define TRINAMIC_ENABLE 0
+#endif
+#ifndef TRINAMIC_I2C
+#define TRINAMIC_I2C 0
+#endif
+#ifndef TRINAMIC_DEV
+#define TRINAMIC_DEV 0
+#endif
+
+#ifndef ESTOP_ENABLE
+ #if COMPATIBILITY_LEVEL <= 1
+ #define ESTOP_ENABLE 1
+ #else
+ #define ESTOP_ENABLE 0
+ #endif
+#elif ESTOP_ENABLE && COMPATIBILITY_LEVEL > 1
+ #warning "Enabling ESTOP may not work with all senders!"
+#endif
+
+#if ETHERNET_ENABLE
+#ifndef NETWORK_HOSTNAME
+#define NETWORK_HOSTNAME "GRBL"
+#endif
+#ifndef NETWORK_IPMODE
+#define NETWORK_IPMODE 1 // 0 = static, 1 = DHCP, 2 = AutoIP
+#endif
+#ifndef NETWORK_IP
+#define NETWORK_IP "192.168.5.1"
+#endif
+#ifndef NETWORK_GATEWAY
+#define NETWORK_GATEWAY "192.168.5.1"
+#endif
+#ifndef NETWORK_MASK
+#define NETWORK_MASK "255.255.255.0"
+#endif
+#ifndef NETWORK_TELNET_PORT
+#define NETWORK_TELNET_PORT 23
+#endif
+#ifndef NETWORK_WEBSOCKET_PORT
+#define NETWORK_WEBSOCKET_PORT 80
+#endif
+#ifndef NETWORK_HTTP_PORT
+#define NETWORK_HTTP_PORT 80
+#endif
+#if NETWORK_IPMODE < 0 || NETWORK_IPMODE > 2
+#error "Invalid IP mode selected!"
+#endif
+#endif
+
+// Timer assignments (for reference, Arduino libs does not follow the CMSIS style...)
+
+//#define STEPPER_TIMER PIT0 (32 bit)
+//#define PULSE_TIMER TMR4
+//#define SPINDLE_PWM_TIMER TMR1 (pin 12) or TMR2 (pin 3)
+//#define DEBOUNCE_TIMER TMR3
+//#define PLASMA_TIMER TMR2
+//#define PPI_TIMER inverse of SPINDLE_PWM_TIMER
+
+// Timers used for spindle encoder if spindle sync is enabled:
+//#define RPM_TIMER GPT1
+//#define RPM_COUNTER GPT2
+
+// End configuration
+
+#ifdef BOARD_CNC_BOOSTERPACK
+ #include "cnc_boosterpack_map.h"
+#elif defined(BOARD_T40X101)
+ #include "T40X101_map.h"
+#elif defined(BOARD_T41U5XBB)
+ #include "T41U5XBB_map.h"
+#elif defined(BOARD_T41U5XSS)
+ #include "T41U5XSS_map.h"
+#elif defined(BOARD_T41PROBB)
+ #include "T41ProBB_map.h"
+#elif defined(BOARD_MY_MACHINE)
+ #include "my_machine_map.h"
+#else // default board
+#include "generic_map.h"
+#endif
+
+#if SPINDLEPWMPIN == 12
+#define PPI_TIMER (IMXRT_TMR2)
+#define PPI_TIMERIRQ IRQ_QTIMER2
+#else
+#define PPI_TIMER (IMXRT_TMR1)
+#define PPI_TIMERIRQ IRQ_QTIMER1
+#endif
+
+// Adjust STEP_PULSE_LATENCY to get accurate step pulse length when required, e.g if using high step rates.
+// The default value is calibrated for 10 microseconds length.
+// NOTE: step output mode, number of axes and compiler optimization settings may all affect this value.
+#ifndef STEP_PULSE_LATENCY
+#define STEP_PULSE_LATENCY 0.2f // microseconds
+#endif
+
+#ifndef IOPORTS_ENABLE
+#define IOPORTS_ENABLE 0
+#endif
+
+#if EEPROM_ENABLE && !defined(EEPROM_IS_FRAM)
+#define EEPROM_IS_FRAM 0
+#endif
+
+#if TRINAMIC_ENABLE
+#include "tmc2130/trinamic.h"
+#endif
+
+#if QEI_ENABLE
+#include "encoder/encoder.h"
+#endif
+
+#if SPINDLE_HUANYANG
+#if USB_SERIAL_CDC == 0
+#error "Huanyang VFD cannot be used with UART communications enabled!"
+#endif
+#include "spindle/huanyang.h"
+#endif
+
+#ifndef VFD_SPINDLE
+#define VFD_SPINDLE 0
+#endif
+
+#if PLASMA_ENABLE
+#include "plasma/thc.h"
+#endif
+
+#if ODOMETER_ENABLE
+#include "odometer/odometer.h"
+#endif
+
+#if KEYPAD_ENABLE && !defined(KEYPAD_STROBE_PIN)
+#error "KEYPAD_ENABLE requires KEYPAD_STROBE_PIN to be defined!"
+#endif
+
+#ifndef I2C_PORT
+ #if EEPROM_ENABLE
+ #error "EEPROM_ENABLE requires I2C_PORT to be defined!"
+ #endif
+ #if KEYPAD_ENABLE
+ #error "KEYPAD_ENABLE requires I2C_PORT to be defined!"
+ #endif
+#endif
+
+#if !(SPINDLEPWMPIN == 12 || SPINDLEPWMPIN == 13)
+ #error "SPINDLEPWMPIN can only be routed to pin 12 or 13!"
+#endif
+
+#if QEI_ENABLE > 1
+ #error "Max number of quadrature interfaces is 1!"
+#endif
+
+#if QEI_ENABLE > 0 && !(defined(QEI_A_PIN) && defined(QEI_B_PIN))
+ #error "QEI_ENABLE requires encoder input pins A and B to be defined!"
+#endif
+
+typedef enum {
+ Input_Probe = 0,
+ Input_Reset,
+ Input_FeedHold,
+ Input_CycleStart,
+ Input_SafetyDoor,
+ Input_LimitsOverride,
+ Input_EStop,
+ Input_ModeSelect,
+ Input_LimitX,
+ Input_LimitX_Max,
+ Input_LimitY,
+ Input_LimitY_Max,
+ Input_LimitZ,
+ Input_LimitZ_Max,
+ Input_LimitA,
+ Input_LimitA_Max,
+ Input_LimitB,
+ Input_LimitB_Max,
+ Input_LimitC,
+ Input_LimitC_Max,
+ Input_KeypadStrobe,
+ Input_QEI_A,
+ Input_QEI_B,
+ Input_QEI_Select,
+ Input_QEI_Index,
+ Input_SpindleIndex,
+ Input_Aux0,
+ Input_Aux1,
+ Input_Aux2,
+ Input_Aux3
+} input_t;
+
+typedef enum {
+ IRQ_Mode_None = 0b00,
+ IRQ_Mode_Change = 0b01,
+ IRQ_Mode_Rising = 0b10,
+ IRQ_Mode_Falling = 0b11
+} irq_mode_t;
+
+typedef struct {
+ volatile uint32_t DR;
+ volatile uint32_t GDIR;
+ volatile uint32_t PSR;
+ volatile uint32_t ICR1;
+ volatile uint32_t ICR2;
+ volatile uint32_t IMR;
+ volatile uint32_t ISR;
+ volatile uint32_t EDGE_SEL;
+ uint32_t unused[25];
+ volatile uint32_t DR_SET;
+ volatile uint32_t DR_CLEAR;
+ volatile uint32_t DR_TOGGLE;
+} gpio_reg_t;
+
+typedef struct {
+ gpio_reg_t *reg;
+ uint32_t bit;
+} gpio_t;
+
+typedef struct {
+ input_t id;
+ uint8_t group;
+ uint8_t pin;
+ gpio_t *port;
+ gpio_t gpio; // doubled up for now for speed...
+ irq_mode_t irq_mode;
+ uint8_t offset;
+ volatile bool active;
+ volatile bool debounce;
+} input_signal_t;
+
+// The following struct is pulled from the Teensy Library core, Copyright (c) 2019 PJRC.COM, LLC.
+
+typedef struct {
+ const uint8_t pin; // The pin number
+ const uint32_t mux_val; // Value to set for mux;
+ volatile uint32_t *select_reg; // Which register controls the selection
+ const uint32_t select_val; // Value for that selection
+} pin_info_t;
+
+//
+
+void selectStream (stream_type_t stream);
+void pinModeOutput (gpio_t *gpio, uint8_t pin);
+
+uint32_t xTaskGetTickCount();
+
+#ifdef UART_DEBUG
+void uart_debug_write (char *s);
+#endif
+
+#endif // __DRIVER_H__
diff --git a/grblHAL_Teensy4/src/enet.c b/grblHAL_Teensy4/src/enet.c
new file mode 100644
index 0000000..34deb85
--- /dev/null
+++ b/grblHAL_Teensy4/src/enet.c
@@ -0,0 +1,325 @@
+/*
+ enet.c - lwIP driver glue code for IMXRT1062 processor (on Teensy 4.1 board)
+
+ Part of grblHAL
+
+ Copyright (c) 2020-2021 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+#include "driver.h"
+
+#if ETHERNET_ENABLE
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include "lwip/dhcp.h"
+
+#include "grbl/report.h"
+#include "grbl/nvs_buffer.h"
+
+#include "networking/TCPStream.h"
+#include "networking/WsStream.h"
+
+static volatile bool linkUp = false;
+static char IPAddress[IP4ADDR_STRLEN_MAX];
+static network_services_t services = {0};
+static nvs_address_t nvs_address;
+static network_settings_t ethernet, network;
+static on_report_options_ptr on_report_options;
+
+static void report_options (bool newopt)
+{
+ on_report_options(newopt);
+
+ if(newopt)
+ hal.stream.write(",ETH");
+ else {
+ hal.stream.write("[IP:");
+ hal.stream.write(IPAddress);
+ hal.stream.write("]" ASCII_EOL);
+ }
+}
+
+static void link_status_callback (struct netif *netif)
+{
+ bool isLinkUp = netif_is_link_up(netif);
+
+ if(isLinkUp != linkUp) {
+ linkUp = isLinkUp;
+ TCPStreamNotifyLinkStatus(linkUp);
+ }
+}
+
+static void netif_status_callback (struct netif *netif)
+{
+ ip4addr_ntoa_r(netif_ip_addr4(netif), IPAddress, IP4ADDR_STRLEN_MAX);
+
+#if TELNET_ENABLE
+ if(network.services.telnet && !services.telnet) {
+ TCPStreamInit();
+ TCPStreamListen(network.telnet_port == 0 ? 23 : network.telnet_port);
+ services.telnet = On;
+ }
+#endif
+
+#if WEBSOCKET_ENABLE
+ if(network.services.websocket && !services.websocket) {
+ WsStreamInit();
+ WsStreamListen(network.websocket_port == 0 ? 80 : network.websocket_port);
+ services.websocket = On;
+ }
+#endif
+}
+
+void grbl_enet_poll (void)
+{
+ static uint32_t last_ms0, last_ms1;
+ uint32_t ms;
+
+ enet_proc_input();
+
+ ms = millis();
+
+ if(ms - last_ms0 > 1) {
+ last_ms0 = ms;
+#if TELNET_ENABLE
+ if(services.telnet)
+ TCPStreamPoll();
+#endif
+ #if WEBSOCKET_ENABLE
+ if(services.websocket)
+ WsStreamPoll();
+#endif
+ }
+
+ if (ms - last_ms1 > 25)
+ {
+ last_ms1 = ms;
+ enet_poll();
+ }
+}
+
+bool grbl_enet_start (void)
+{
+ if(nvs_address != 0) {
+
+ *IPAddress = '\0';
+
+ memcpy(&network, ðernet, sizeof(network_settings_t));
+
+ if(network.ip_mode == IpMode_Static)
+ enet_init((ip_addr_t *)&network.ip, (ip_addr_t *)&network.mask, (ip_addr_t *)&network.gateway);
+ else
+ enet_init(NULL, NULL, NULL);
+
+ netif_set_status_callback(netif_default, netif_status_callback);
+ netif_set_link_callback(netif_default, link_status_callback);
+
+ netif_set_up(netif_default);
+ #if LWIP_NETIF_HOSTNAME
+ netif_set_hostname(netif_default, network.hostname);
+ #endif
+ if(network.ip_mode == IpMode_DHCP)
+ dhcp_start(netif_default);
+ }
+
+ return nvs_address != 0;
+}
+
+static inline void set_addr (char *ip, ip4_addr_t *addr)
+{
+ memcpy(ip, addr, sizeof(ip4_addr_t));
+}
+
+static void ethernet_settings_load (void);
+static void ethernet_settings_restore (void);
+static status_code_t ethernet_set_ip (setting_id_t setting, char *value);
+static char *ethernet_get_ip (setting_id_t setting);
+
+static const setting_group_detail_t ethernet_groups [] = {
+ { Group_Root, Group_Networking, "Networking" }
+};
+
+#if TELNET_ENABLE && WEBSOCKET_ENABLE && HTTP_ENABLE
+static const char netservices[] = "Telnet,Websocket,HTTP";
+#endif
+#if TELNET_ENABLE && WEBSOCKET_ENABLE && !HTTP_ENABLE
+static const char netservices[] = "Telnet,Websocket";
+#endif
+#if TELNET_ENABLE && !WEBSOCKET_ENABLE && !HTTP_ENABLE
+static const char netservices[] = "Telnet";
+#endif
+
+PROGMEM static const setting_detail_t ethernet_settings[] = {
+ { Setting_NetworkServices, Group_Networking, "Network Services", NULL, Format_Bitfield, netservices, NULL, NULL, Setting_NonCore, ðernet.services.mask, NULL, NULL },
+ { Setting_Hostname, Group_Networking, "Hostname", NULL, Format_String, "x(64)", NULL, "64", Setting_NonCore, ethernet.hostname, NULL, NULL },
+ { Setting_IpMode, Group_Networking, "IP Mode", NULL, Format_RadioButtons, "Static,DHCP,AutoIP", NULL, NULL, Setting_NonCore, ðernet.ip_mode, NULL, NULL },
+ { Setting_IpAddress, Group_Networking, "IP Address", NULL, Format_IPv4, NULL, NULL, NULL, Setting_NonCoreFn, ethernet_set_ip, ethernet_get_ip, NULL },
+ { Setting_Gateway, Group_Networking, "Gateway", NULL, Format_IPv4, NULL, NULL, NULL, Setting_NonCoreFn, ethernet_set_ip, ethernet_get_ip, NULL },
+ { Setting_NetMask, Group_Networking, "Netmask", NULL, Format_IPv4, NULL, NULL, NULL, Setting_NonCoreFn, ethernet_set_ip, ethernet_get_ip, NULL },
+ { Setting_TelnetPort, Group_Networking, "Telnet port", NULL, Format_Int16, "####0", "1", "65535", Setting_NonCore, ðernet.telnet_port, NULL, NULL },
+#if HTTP_ENABLE
+ { Setting_HttpPort, Group_Networking, "HTTP port", NULL, Format_Int16, "####0", "1", "65535", Setting_NonCore, ðernet.http_port, NULL, NULL },
+#endif
+ { Setting_WebSocketPort, Group_Networking, "Websocket port", NULL, Format_Int16, "####0", "1", "65535", Setting_NonCore, ðernet.websocket_port, NULL, NULL }
+};
+
+static void ethernet_settings_save (void)
+{
+ hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)ðernet, sizeof(network_settings_t), true);
+}
+
+static setting_details_t details = {
+ .groups = ethernet_groups,
+ .n_groups = sizeof(ethernet_groups) / sizeof(setting_group_detail_t),
+ .settings = ethernet_settings,
+ .n_settings = sizeof(ethernet_settings) / sizeof(setting_detail_t),
+ .save = ethernet_settings_save,
+ .load = ethernet_settings_load,
+ .restore = ethernet_settings_restore
+};
+
+static setting_details_t *on_get_settings (void)
+{
+ return &details;
+}
+
+static status_code_t ethernet_set_ip (setting_id_t setting, char *value)
+{
+ ip_addr_t addr;
+
+ if(ip4addr_aton(value, &addr) != 1)
+ return Status_InvalidStatement;
+
+ status_code_t status = Status_OK;
+
+ switch(setting) {
+
+ case Setting_IpAddress:
+ set_addr(ethernet.ip, &addr);
+ break;
+
+ case Setting_Gateway:
+ set_addr(ethernet.gateway, &addr);
+ break;
+
+ case Setting_NetMask:
+ set_addr(ethernet.mask, &addr);
+ break;
+
+ default:
+ status = Status_Unhandled;
+ break;
+ }
+
+ return status;
+}
+
+static char *ethernet_get_ip (setting_id_t setting)
+{
+ static char ip[IPADDR_STRLEN_MAX];
+
+ switch(setting) {
+
+ case Setting_IpAddress:
+ ip4addr_ntoa_r((const ip_addr_t *)ðernet.ip, ip, IPADDR_STRLEN_MAX);
+ break;
+
+ case Setting_Gateway:
+ ip4addr_ntoa_r((const ip_addr_t *)ðernet.gateway, ip, IPADDR_STRLEN_MAX);
+ break;
+
+ case Setting_NetMask:
+ ip4addr_ntoa_r((const ip_addr_t *)ðernet.mask, ip, IPADDR_STRLEN_MAX);
+ break;
+
+ default:
+ *ip = '\0';
+ break;
+ }
+
+ return ip;
+}
+
+void ethernet_settings_restore (void)
+{
+ strcpy(ethernet.hostname, NETWORK_HOSTNAME);
+
+ ip4_addr_t addr;
+
+ ethernet.ip_mode = (ip_mode_t)NETWORK_IPMODE;
+
+ if(ip4addr_aton(NETWORK_IP, &addr) == 1)
+ set_addr(ethernet.ip, &addr);
+
+ if(ip4addr_aton(NETWORK_GATEWAY, &addr) == 1)
+ set_addr(ethernet.gateway, &addr);
+
+#if NETWORK_IPMODE == 0
+ if(ip4addr_aton(NETWORK_MASK, &addr) == 1)
+ set_addr(ethernet.mask, &addr);
+#else
+ if(ip4addr_aton("255.255.255.0", &addr) == 1)
+ set_addr(ethernet.mask, &addr);
+#endif
+
+ ethernet.services.mask = 0;
+ ethernet.telnet_port = NETWORK_TELNET_PORT;
+ ethernet.http_port = NETWORK_HTTP_PORT;
+ ethernet.websocket_port = NETWORK_WEBSOCKET_PORT;
+
+#if TELNET_ENABLE
+ ethernet.services.telnet = On;
+#endif
+
+#if HTTP_ENABLE
+ ethernet.services.http = On;
+#endif
+
+#if WEBSOCKET_ENABLE
+ ethernet.services.websocket = On;
+#endif
+
+ hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)ðernet, sizeof(network_settings_t), true);
+}
+
+static void ethernet_settings_load (void)
+{
+ if(hal.nvs.memcpy_from_nvs((uint8_t *)ðernet, nvs_address, sizeof(network_settings_t), true) != NVS_TransferResult_OK)
+ ethernet_settings_restore();
+}
+
+bool grbl_enet_init (network_settings_t *settings)
+{
+ if((nvs_address = nvs_alloc(sizeof(network_settings_t)))) {
+
+ on_report_options = grbl.on_report_options;
+ grbl.on_report_options = report_options;
+
+ details.on_get_settings = grbl.on_get_settings;
+ grbl.on_get_settings = on_get_settings;
+ }
+
+ return nvs_address != 0;
+}
+
+#endif
diff --git a/grblHAL_Teensy4/src/enet.h b/grblHAL_Teensy4/src/enet.h
new file mode 100644
index 0000000..95233e1
--- /dev/null
+++ b/grblHAL_Teensy4/src/enet.h
@@ -0,0 +1,31 @@
+/*
+ enet.h - lwIP driver glue code for IMXRT1062 processor (on Teensy 4.1 board)
+
+ Part of grblHAL
+
+ Copyright (c) 2020 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+#ifndef __ENET_H__
+#define __ENET_H__
+
+#include "driver.h"
+
+bool grbl_enet_init (void);
+bool grbl_enet_start (void);
+void grbl_enet_poll (void);
+
+#endif
diff --git a/grblHAL_Teensy4/src/generic_map.h b/grblHAL_Teensy4/src/generic_map.h
new file mode 100644
index 0000000..bd051dc
--- /dev/null
+++ b/grblHAL_Teensy4/src/generic_map.h
@@ -0,0 +1,82 @@
+/*
+ generic_map.h - driver code for IMXRT1062 processor (on Teensy 4.0 board)
+
+ Part of grblHAL
+
+ Copyright (c) 2020 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+// Define step pulse output pins.
+#define X_STEP_PIN (2u)
+#define Y_STEP_PIN (4u)
+#define Z_STEP_PIN (6u)
+
+// Define step direction output pins.
+#define X_DIRECTION_PIN (3u)
+#define Y_DIRECTION_PIN (5u)
+#define Z_DIRECTION_PIN (7u)
+
+// Define stepper driver enable/disable output pin(s).
+#define STEPPERS_ENABLE_PIN (10u)
+
+// Define homing/hard limit switch input pins.
+#define X_LIMIT_PIN (20u)
+#define Y_LIMIT_PIN (21u)
+#define Z_LIMIT_PIN (22u)
+
+#if N_AXIS > 3
+#define A_STEP_PIN (8u)
+#define A_DIRECTION_PIN (9u)
+#define A_LIMIT_PIN (23u)
+#endif
+
+#if N_AXIS > 4
+#define B_STEP_PIN (26u)
+#define B_DIRECTION_PIN (27u)
+#define B_LIMIT_PIN (28u)
+#endif
+
+// Define spindle enable and spindle direction output pins.
+#define SPINDLE_ENABLE_PIN (12u)
+#define SPINDLE_DIRECTION_PIN (11u)
+#define SPINDLEPWMPIN (13u) // NOTE: only pin 12 or pin 13 can be assigned!
+
+// Define flood and mist coolant enable output pins.
+#define COOLANT_FLOOD_PIN (19u)
+#define COOLANT_MIST_PIN (18u)
+
+// Define user-control CONTROLs (cycle start, reset, feed hold, door) input pins.
+#define RESET_PIN (14u)
+#define FEED_HOLD_PIN (16u)
+#define CYCLE_START_PIN (17u)
+#define SAFETY_DOOR_PIN (29u)
+
+// Define probe switch input pin.
+#define PROBE_PIN (15U)
+
+#if EEPROM_ENABLE || KEYPAD_ENABLE
+#define I2C_PORT 4
+#define I2C_SCL4 (24u) // Not used, for info only
+#define I2C_SDA4 (25u) // Not used, for info only
+#endif
+
+#if QEI_ENABLE
+#define QEI_A_PIN (0)
+#define QEI_B_PIN (3)
+// #define QEI_INDEX_PIN GPIO2_PIN
+#define QEI_SELECT_PIN (1)
+#endif
+
diff --git a/grblHAL_Teensy4/src/grblHAL_Teensy4.h b/grblHAL_Teensy4/src/grblHAL_Teensy4.h
new file mode 100644
index 0000000..34d7c62
--- /dev/null
+++ b/grblHAL_Teensy4/src/grblHAL_Teensy4.h
@@ -0,0 +1,22 @@
+/*
+ grblHAL_Teensy4.h - Arduino library wrapper (Teensy4.x version)
+
+ Part of grblHAL
+
+ Copyright (c) 2019-2020 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+#include "grbl/grbllib.h"
diff --git a/grblHAL_Teensy4/src/i2c.c b/grblHAL_Teensy4/src/i2c.c
new file mode 100644
index 0000000..900d28c
--- /dev/null
+++ b/grblHAL_Teensy4/src/i2c.c
@@ -0,0 +1,511 @@
+/*
+ i2c.c - driver code for IMXRT1062 processor (on Teensy 4.0 board)
+
+ Part of grblHAL
+
+ Some parts of this code is Copyright (c) 2020 Terje Io
+
+ Some parts are derived/pulled from WireIMXRT.cpp in the Teensyduino Core Library (no copyright header)
+
+*/
+
+#include "driver.h"
+
+#ifdef I2C_PORT
+
+#include
+
+#include "i2c.h"
+
+#if EEPROM_ENABLE
+#include "eeprom/eeprom.h"
+#endif
+
+#define i2cIsBusy (!(i2c.state == I2CState_Idle || i2c.state == I2CState_Error) || (port->MSR & (LPI2C_MSR_BBF|LPI2C_MSR_MBF)))
+
+// Timeout if a device stretches SCL this long, in microseconds
+#define CLOCK_STRETCH_TIMEOUT 15000
+#define PINCONFIG (IOMUXC_PAD_ODE | IOMUXC_PAD_SRE | IOMUXC_PAD_DSE(4) | IOMUXC_PAD_SPEED(1) | IOMUXC_PAD_PKE | IOMUXC_PAD_PUE | IOMUXC_PAD_PUS(3))
+
+typedef struct {
+ volatile uint32_t *clock_gate_register;
+ uint32_t clock_gate_mask;
+ pin_info_t sda_pin;
+ pin_info_t scl_pin;
+ IMXRT_LPI2C_t *port;
+ enum IRQ_NUMBER_t irq;
+} i2c_hardware_t;
+
+static const i2c_hardware_t i2c1_hardware = {
+ .clock_gate_register = &CCM_CCGR2,
+ .clock_gate_mask = CCM_CCGR2_LPI2C1(CCM_CCGR_ON),
+ .port = &IMXRT_LPI2C1,
+ .irq = IRQ_LPI2C1,
+ .sda_pin = {
+ .pin = 18,
+ .mux_val = 3 | 0x10,
+ .select_reg = &IOMUXC_LPI2C1_SDA_SELECT_INPUT,
+ .select_val = 1
+ },
+ .scl_pin = {
+ .pin = 19,
+ .mux_val = 3 | 0x10,
+ .select_reg = &IOMUXC_LPI2C1_SCL_SELECT_INPUT,
+ .select_val = 1
+ }
+};
+
+// NOTE: port 3 has alternative mapping to pin 36 and 37
+static const i2c_hardware_t i2c3_hardware = {
+ .clock_gate_register = &CCM_CCGR2,
+ .clock_gate_mask = CCM_CCGR2_LPI2C3(CCM_CCGR_ON),
+ .port = &IMXRT_LPI2C3,
+ .irq = IRQ_LPI2C3,
+ .sda_pin = {
+ .pin = 16,
+ .mux_val = 1 | 0x10,
+ .select_reg = &IOMUXC_LPI2C3_SDA_SELECT_INPUT,
+ .select_val = 1
+ },
+ .scl_pin = {
+ .pin = 17,
+ .mux_val = 1 | 0x10,
+ .select_reg = &IOMUXC_LPI2C3_SCL_SELECT_INPUT,
+ .select_val = 1
+ }
+};
+
+static const i2c_hardware_t i2c4_hardware = {
+ .clock_gate_register = &CCM_CCGR6,
+ .clock_gate_mask = CCM_CCGR6_LPI2C4_SERIAL(CCM_CCGR_ON),
+ .port = &IMXRT_LPI2C4,
+ .irq = IRQ_LPI2C4,
+ .sda_pin = {
+ .pin = 25,
+ .mux_val = 0 | 0x10,
+ .select_reg = &IOMUXC_LPI2C4_SDA_SELECT_INPUT,
+ .select_val = 1
+ },
+ .scl_pin = {
+ .pin = 24,
+ .mux_val = 0 | 0x10,
+ .select_reg = &IOMUXC_LPI2C4_SCL_SELECT_INPUT,
+ .select_val = 1
+ }
+};
+
+static bool force_clock (i2c_hardware_t *hardware)
+{
+ bool ret = false;
+ uint32_t sda_pin = hardware->sda_pin.pin;
+ uint32_t scl_pin = hardware->scl_pin.pin;
+ uint32_t sda_mask = digitalPinToBitMask(sda_pin);
+ uint32_t scl_mask = digitalPinToBitMask(scl_pin);
+ // take control of pins with GPIO
+ *portConfigRegister(sda_pin) = 5 | 0x10;
+ *portSetRegister(sda_pin) = sda_mask;
+ *portModeRegister(sda_pin) |= sda_mask;
+ *portConfigRegister(scl_pin) = 5 | 0x10;
+ *portSetRegister(scl_pin) = scl_mask;
+ *portModeRegister(scl_pin) |= scl_mask;
+ delayMicroseconds(10);
+ for (int i=0; i < 9; i++) {
+ if ((*portInputRegister(sda_pin) & sda_mask) && (*portInputRegister(scl_pin) & scl_mask)) {
+ // success, both pins are high
+ ret = true;
+ break;
+ }
+ *portClearRegister(scl_pin) = scl_mask;
+ delayMicroseconds(5);
+ *portSetRegister(scl_pin) = scl_mask;
+ delayMicroseconds(5);
+ }
+ // return control of pins to I2C
+ *(portConfigRegister(sda_pin)) = hardware->sda_pin.mux_val;
+ *(portConfigRegister(scl_pin)) = hardware->scl_pin.mux_val;
+
+ return ret;
+}
+
+static void setClock (IMXRT_LPI2C_t *port, uint32_t frequency)
+{
+ port->MCR = 0;
+
+ if (frequency < 400000) {
+ // 100 kHz
+ port->MCCR0 = LPI2C_MCCR0_CLKHI(55) | LPI2C_MCCR0_CLKLO(59) | LPI2C_MCCR0_DATAVD(25) | LPI2C_MCCR0_SETHOLD(40);
+ port->MCFGR1 = LPI2C_MCFGR1_PRESCALE(1);
+ port->MCFGR2 = LPI2C_MCFGR2_FILTSDA(5) | LPI2C_MCFGR2_FILTSCL(5) | LPI2C_MCFGR2_BUSIDLE(3000); // idle timeout 250 us
+ port->MCFGR3 = LPI2C_MCFGR3_PINLOW(CLOCK_STRETCH_TIMEOUT * 12 / 256 + 1);
+ } else if (frequency < 1000000) {
+ // 400 kHz
+ port->MCCR0 = LPI2C_MCCR0_CLKHI(26) | LPI2C_MCCR0_CLKLO(28) | LPI2C_MCCR0_DATAVD(12) | LPI2C_MCCR0_SETHOLD(18);
+ port->MCFGR1 = LPI2C_MCFGR1_PRESCALE(0);
+ port->MCFGR2 = LPI2C_MCFGR2_FILTSDA(2) | LPI2C_MCFGR2_FILTSCL(2) | LPI2C_MCFGR2_BUSIDLE(3600); // idle timeout 150 us
+ port->MCFGR3 = LPI2C_MCFGR3_PINLOW(CLOCK_STRETCH_TIMEOUT * 24 / 256 + 1);
+ } else {
+ // 1 MHz
+ port->MCCR0 = LPI2C_MCCR0_CLKHI(9) | LPI2C_MCCR0_CLKLO(10) | LPI2C_MCCR0_DATAVD(4) | LPI2C_MCCR0_SETHOLD(7);
+ port->MCFGR1 = LPI2C_MCFGR1_PRESCALE(0);
+ port->MCFGR2 = LPI2C_MCFGR2_FILTSDA(1) | LPI2C_MCFGR2_FILTSCL(1) | LPI2C_MCFGR2_BUSIDLE(2400); // idle timeout 100 us
+ port->MCFGR3 = LPI2C_MCFGR3_PINLOW(CLOCK_STRETCH_TIMEOUT * 24 / 256 + 1);
+ }
+ port->MCCR1 = port->MCCR0;
+ port->MCFGR0 = 0;
+ port->MFCR = LPI2C_MFCR_RXWATER(0) | LPI2C_MFCR_TXWATER(1);
+ port->MCR = LPI2C_MCR_MEN;
+}
+
+typedef enum {
+ I2CState_Idle = 0,
+ I2CState_Restart,
+ I2CState_SendAddr,
+ I2CState_SendNext,
+ I2CState_SendLast,
+ I2CState_AwaitCompletion,
+ I2CState_ReceiveNext,
+ I2CState_ReceiveNextToLast,
+ I2CState_ReceiveLast,
+ I2CState_Error
+} i2c_state_t;
+
+typedef struct {
+ volatile i2c_state_t state;
+ uint8_t addr;
+ volatile uint16_t count;
+ volatile uint16_t rcount;
+ volatile uint8_t acount;
+ uint8_t *data;
+ uint8_t regaddr[2];
+#if KEYPAD_ENABLE
+ keycode_callback_ptr keycode_callback;
+#endif
+ uint8_t buffer[8];
+} i2c_trans_t;
+
+static i2c_trans_t i2c;
+static uint8_t tx_fifo_size;
+static const i2c_hardware_t *hardware;
+static IMXRT_LPI2C_t *port = NULL;
+
+static void I2C_interrupt_handler (void);
+
+void i2c_init (void)
+{
+ static bool init_ok = false;
+
+ if(!init_ok) {
+
+ init_ok = true;
+
+#ifndef I2C_PORT
+#error "I2C port is undefined!"
+#endif
+
+#if I2C_PORT == 0
+ hardware = &i2c1_hardware;
+#elif I2C_PORT == 3
+ hardware = &i2c3_hardware;
+#elif I2C_PORT == 4
+ hardware = &i2c4_hardware;
+#else
+#error "No such I2C port!"
+#endif
+
+ port = hardware->port;
+ tx_fifo_size = 1 << (port->PARAM & 0b1111);
+
+ CCM_CSCDR2 = (CCM_CSCDR2 & ~CCM_CSCDR2_LPI2C_CLK_PODF(63)) | CCM_CSCDR2_LPI2C_CLK_SEL;
+ *hardware->clock_gate_register |= hardware->clock_gate_mask;
+ port->MCR = LPI2C_MCR_RST;
+
+ setClock(port, 100000);
+
+ // Setup SDA register
+ *(portControlRegister(hardware->sda_pin.pin)) = PINCONFIG;
+ *(portConfigRegister(hardware->sda_pin.pin)) = hardware->sda_pin.mux_val;
+ *(hardware->sda_pin.select_reg) = hardware->sda_pin.select_val;
+
+ // setup SCL register
+ *(portControlRegister(hardware->scl_pin.pin)) = PINCONFIG;
+ *(portConfigRegister(hardware->scl_pin.pin)) = hardware->scl_pin.mux_val;
+ *(hardware->scl_pin.select_reg) = hardware->scl_pin.select_val;
+
+ attachInterruptVector(hardware->irq, I2C_interrupt_handler);
+
+ NVIC_SET_PRIORITY(hardware->irq, 1);
+ NVIC_ENABLE_IRQ(hardware->irq);
+ }
+}
+
+// get bytes (max 8 if local buffer, else max 255), waits for result
+uint8_t *I2C_Receive (uint32_t i2cAddr, uint8_t *buf, uint16_t bytes, bool block)
+{
+ i2c.data = buf ? buf : i2c.buffer;
+ i2c.count = bytes;
+ i2c.rcount = 0;
+ i2c.state = bytes == 1 ? I2CState_ReceiveLast : (bytes == 2 ? I2CState_ReceiveNextToLast : I2CState_ReceiveNext);
+
+ port->MSR = 0;
+ port->MTDR = LPI2C_MTDR_CMD_START | (i2cAddr << 1) | 1;
+ port->MTDR = LPI2C_MTDR_CMD_RECEIVE | ((uint32_t)i2c.count - 1);
+ port->MIER = LPI2C_MIER_NDIE|LPI2C_MIER_RDIE;
+
+ if(block)
+ while(i2cIsBusy);
+
+ return i2c.buffer;
+}
+
+void I2C_Send (uint32_t i2cAddr, uint8_t *buf, uint16_t bytes, bool block)
+{
+ i2c.count = bytes;
+ i2c.data = buf ? buf : i2c.buffer;
+
+ port->MSR = 0;
+ port->MTDR = LPI2C_MTDR_CMD_START | (i2cAddr << 1);
+
+ while(i2c.count && (port->MFSR & 0b111) < tx_fifo_size) {
+ port->MTDR = LPI2C_MTDR_CMD_TRANSMIT | (uint32_t)(*i2c.data++);
+ i2c.count--;
+ }
+
+ port->MIER = LPI2C_MIER_NDIE|LPI2C_MIER_TDIE;
+
+ i2c.state = i2c.count == 0 ? I2CState_AwaitCompletion : (i2c.count == 1 ? I2CState_SendLast : I2CState_SendNext);
+
+ if(block)
+ while(i2cIsBusy);
+}
+
+uint8_t *I2C_ReadRegister (uint32_t i2cAddr, uint8_t *buf, uint8_t abytes, uint16_t bytes, bool block)
+{
+ while(i2cIsBusy);
+
+ i2c.addr = i2cAddr;
+ i2c.count = bytes;
+ i2c.rcount = 0;
+ i2c.acount = abytes;
+ i2c.data = buf ? buf : i2c.buffer;
+ i2c.state = I2CState_SendAddr;
+
+ port->MSR = 0;
+ port->MTDR = LPI2C_MTDR_CMD_START | (i2cAddr << 1);
+ port->MIER = LPI2C_MIER_NDIE|LPI2C_MIER_TDIE;
+
+ if(block)
+ while(i2cIsBusy);
+
+ return i2c.buffer;
+}
+
+#if EEPROM_ENABLE
+
+nvs_transfer_result_t i2c_nvs_transfer (nvs_transfer_t *transfer, bool read)
+{
+ static uint8_t txbuf[NVS_SIZE + 2];
+
+ while(i2cIsBusy);
+
+ if(read) {
+ if(transfer->word_addr_bytes == 1)
+ i2c.regaddr[0] = transfer->word_addr;
+ else {
+ i2c.regaddr[0] = transfer->word_addr & 0xFF;
+ i2c.regaddr[1] = transfer->word_addr >> 8;
+ }
+ I2C_ReadRegister(transfer->address, transfer->data, transfer->word_addr_bytes, transfer->count, true);
+ } else {
+ memcpy(&txbuf[transfer->word_addr_bytes], transfer->data, transfer->count);
+ if(transfer->word_addr_bytes == 1)
+ txbuf[0] = transfer->word_addr;
+ else {
+ txbuf[0] = transfer->word_addr >> 8;
+ txbuf[1] = transfer->word_addr & 0xFF;
+ }
+ I2C_Send(transfer->address, txbuf, transfer->count + transfer->word_addr_bytes, true);
+#if !EEPROM_IS_FRAM
+ hal.delay_ms(7, NULL);
+#endif
+ }
+
+ return NVS_TransferResult_OK;
+}
+
+#endif
+
+#if KEYPAD_ENABLE
+
+void I2C_GetKeycode (uint32_t i2cAddr, keycode_callback_ptr callback)
+{
+ while(i2cIsBusy);
+
+ i2c.keycode_callback = callback;
+
+ I2C_Receive(i2cAddr, NULL, 1, false);
+}
+
+#endif
+
+#if TRINAMIC_ENABLE && TRINAMIC_I2C
+
+static TMC2130_status_t I2C_TMC_ReadRegister (TMC2130_t *driver, TMC2130_datagram_t *reg)
+{
+ uint8_t *res, i2creg;
+ TMC2130_status_t status = {0};
+
+ if((i2creg = TMCI2C_GetMapAddress((uint8_t)(driver ? (uint32_t)driver->cs_pin : 0), reg->addr).value) == 0xFF)
+ return status; // unsupported register
+
+ while(i2cIsBusy);
+
+ i2c.buffer[0] = i2creg;
+ i2c.buffer[1] = 0;
+ i2c.buffer[2] = 0;
+ i2c.buffer[3] = 0;
+ i2c.buffer[4] = 0;
+
+ res = I2C_ReadRegister(I2C_ADR_I2CBRIDGE, NULL, 5, true);
+
+ status.value = (uint8_t)*res++;
+ reg->payload.value = ((uint8_t)*res++ << 24);
+ reg->payload.value |= ((uint8_t)*res++ << 16);
+ reg->payload.value |= ((uint8_t)*res++ << 8);
+ reg->payload.value |= (uint8_t)*res++;
+
+ return status;
+}
+
+static TMC2130_status_t I2C_TMC_WriteRegister (TMC2130_t *driver, TMC2130_datagram_t *reg)
+{
+ TMC2130_status_t status = {0};
+
+ while(i2cIsBusy);
+
+ reg->addr.write = 1;
+ i2c.buffer[0] = TMCI2C_GetMapAddress((uint8_t)(driver ? (uint32_t)driver->cs_pin : 0), reg->addr).value;
+ reg->addr.write = 0;
+
+ if(i2c.buffer[0] == 0xFF)
+ return status; // unsupported register
+
+ i2c.buffer[1] = (reg->payload.value >> 24) & 0xFF;
+ i2c.buffer[2] = (reg->payload.value >> 16) & 0xFF;
+ i2c.buffer[3] = (reg->payload.value >> 8) & 0xFF;
+ i2c.buffer[4] = reg->payload.value & 0xFF;
+
+ I2C_Send(I2C_ADR_I2CBRIDGE, NULL, 5, true);
+
+ return status;
+}
+
+void I2C_DriverInit (TMC_io_driver_t *driver)
+{
+ i2c_init();
+ driver->WriteRegister = I2C_TMC_WriteRegister;
+ driver->ReadRegister = I2C_TMC_ReadRegister;
+}
+
+#endif
+
+static void I2C_interrupt_handler (void)
+{
+ uint32_t ifg = port->MSR & 0xFFFF;
+
+ port->MSR &= ~ifg;
+
+if((ifg & port->MIER) == 0) return;
+
+//if(port->MSR & LPI2C_MSR_MBF) return;
+/*
+hal.stream.write("I:");
+hal.stream.write(uitoa(ifg));
+hal.stream.write(" ");
+hal.stream.write(uitoa(i2c.state));
+hal.stream.write(ASCII_EOL);
+*/
+ if(ifg & LPI2C_MSR_ALF) {
+ port->MTDR = LPI2C_MTDR_CMD_STOP;
+ i2c.state = I2CState_Error;
+ }
+
+ if(ifg & LPI2C_MSR_NDF)
+ i2c.state = I2CState_Error;
+
+ switch(i2c.state) {
+
+ case I2CState_Idle:
+ case I2CState_Error:
+ port->MIER = 0;
+ port->MCR |= (LPI2C_MCR_RTF|LPI2C_MCR_RRF);
+ break;
+
+ case I2CState_SendNext:
+ port->MTDR = LPI2C_MTDR_CMD_TRANSMIT | (uint32_t)(*i2c.data++);
+ if(--i2c.count == 1)
+ i2c.state = I2CState_SendLast;
+ break;
+
+ case I2CState_SendLast:
+ port->MTDR = LPI2C_MTDR_CMD_TRANSMIT | (uint32_t)(*i2c.data++);
+ i2c.state = I2CState_AwaitCompletion;
+ break;
+
+ case I2CState_AwaitCompletion:
+ port->MIER &= ~LPI2C_MIER_TDIE;
+ port->MTDR = LPI2C_MTDR_CMD_STOP;
+ i2c.count = 0;
+ i2c.state = I2CState_Idle;
+ break;
+
+ case I2CState_SendAddr:
+ port->MTDR = LPI2C_MTDR_CMD_TRANSMIT | (uint32_t)(i2c.regaddr[--i2c.acount]);
+ if(i2c.acount == 0)
+ i2c.state = I2CState_Restart;
+ break;
+
+ case I2CState_Restart:
+ if(port->MIER & LPI2C_MIER_TDIE) {
+ port->MIER &= ~LPI2C_MIER_TDIE;
+ port->MIER |= LPI2C_MIER_EPIE;
+ port->MTDR = LPI2C_MTDR_CMD_START | (i2c.addr << 1) | 1;
+ } else if(port->MIER & LPI2C_MIER_EPIE) {
+ port->MIER &= ~LPI2C_MIER_EPIE;
+ port->MIER |= LPI2C_MIER_RDIE;
+ port->MTDR = LPI2C_MTDR_CMD_RECEIVE | ((uint32_t)i2c.count - 1);
+ i2c.state = i2c.count == 1 ? I2CState_ReceiveLast : (i2c.count == 2 ? I2CState_ReceiveNextToLast : I2CState_ReceiveNext);
+ }
+ break;
+
+ case I2CState_ReceiveNext: // superfluous, to be removed...
+ *i2c.data++ = port->MRDR & 0xFF;
+ if(--i2c.count == 1) {
+ i2c.state = I2CState_ReceiveLast;
+ }
+ ++i2c.rcount;
+ break;
+
+ case I2CState_ReceiveNextToLast:
+ *i2c.data++ = port->MRDR & 0xFF;
+// port->MTDR = LPI2C_MTDR_CMD_STOP;
+ i2c.count--;
+ i2c.state = I2CState_ReceiveLast;
+ break;
+
+ case I2CState_ReceiveLast:
+ *i2c.data = port->MRDR & 0xFF;
+ i2c.count = 0;
+ i2c.state = I2CState_Idle;
+ port->MTDR = LPI2C_MTDR_CMD_STOP;
+ #if KEYPAD_ENABLE
+ if(i2c.keycode_callback) {
+ i2c.keycode_callback(*i2c.data);
+ i2c.keycode_callback = NULL;
+ }
+ #endif
+ break;
+default: break;
+ }
+}
+
+#endif
diff --git a/grblHAL_Teensy4/src/i2c.h b/grblHAL_Teensy4/src/i2c.h
new file mode 100644
index 0000000..9f1c461
--- /dev/null
+++ b/grblHAL_Teensy4/src/i2c.h
@@ -0,0 +1,54 @@
+/*
+ i2c.h - I2C interface
+
+ Driver code for IMXRT1062 processor (on Teensy 4.0 board)
+
+ Part of grblHAL
+
+ Copyright (c) 2020-2021 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+#ifndef __I2C_DRIVER_H__
+#define __I2C_DRIVER_H__
+
+#include "driver.h"
+#include "grbl/plugins.h"
+
+void i2c_init (void);
+uint8_t *I2C_Receive (uint32_t i2cAddr, uint8_t *buf, uint16_t bytes, bool block);
+void I2C_Send (uint32_t i2cAddr, uint8_t *buf, uint16_t bytes, bool block);
+uint8_t *I2C_ReadRegister (uint32_t i2cAddr, uint8_t *buf, uint8_t abytes, uint16_t bytes, bool block);
+
+#if TRINAMIC_ENABLE && TRINAMIC_I2C
+
+#include "trinamic/trinamic2130.h"
+#include "trinamic/TMC2130_I2C_map.h"
+
+#define I2C_ADR_I2CBRIDGE 0x47
+
+void I2C_DriverInit (TMC_io_driver_t *drv);
+
+#endif
+
+#if KEYPAD_ENABLE
+
+#include "keypad/keypad.h"
+
+void I2C_GetKeycode (uint32_t i2cAddr, keycode_callback_ptr callback);
+
+#endif
+
+#endif
diff --git a/grblHAL_Teensy4/src/main.c b/grblHAL_Teensy4/src/main.c
new file mode 100644
index 0000000..47e953d
--- /dev/null
+++ b/grblHAL_Teensy4/src/main.c
@@ -0,0 +1,35 @@
+/*
+
+ main.ino - startup code for iMXRT1062 processor (on Teensy 4.0 board)
+
+ Part of Grbl
+
+ Copyright (c) 2020 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,ss
+ 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 Grbl. If not, see .
+
+*/
+
+// Double tap reset to enter bootloader mode - select bootloader port for programming
+
+#include "grbl/grbllib.h"
+
+void setup ()
+{
+ grbl_enter();
+}
+
+void loop ()
+{
+}
diff --git a/grblHAL_Teensy4/src/my_machine.h b/grblHAL_Teensy4/src/my_machine.h
new file mode 100644
index 0000000..ce499b6
--- /dev/null
+++ b/grblHAL_Teensy4/src/my_machine.h
@@ -0,0 +1,76 @@
+/*
+ my_machine.h - configuration for IMXRT1062 processor (on Teensy 4.x board)
+
+ Part of grblHAL
+
+ Copyright (c) 2020 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+// BOARD_T40X101 and BOARD_T41U5XBB by Phil Barrett: https://github.com/phil-barrett/grblHAL-teensy-4.x
+
+// NOTE: Only one board may be enabled!
+// If none is enabled pin mappings from generic_map.h will be used
+//#define BOARD_T40X101
+#define BOARD_T41U5XBB
+//#define BOARD_CNC_BOOSTERPACK
+//#define BOARD_MY_MACHINE // Add my_machine_map.h before enabling this!
+
+// Configuration
+// Uncomment to enable, for some a value > 1 may be assigned, if so the default value is shown.
+
+/*
+ Plugin: | ETHERNET¹ | SDCARD¹ | KEYPAD | EEPROM | N_AXIS |
+----------------------|-----------|---------|--------|--------|--------|
+BOARD_T40X101 | no | no | yes | yes³ | max 4 |
+BOARD_T41U5XBB | yes | yes | yes | yes³ | max 5 |
+BOARD_CNC_BOOSTERPACK | yes² | yes | yes | yes | max 3 |
+
+¹ Teensy 4.1 only.
+² External magjack.
+³ EEPROM is optional and must be added to the board.
+
+N_AXIS has a default value of 3, edit grbl\config.h to increase.
+
+*/
+
+#define USB_SERIAL_CDC 2 // 1 for Arduino class library and 2 for PJRC C library. Comment out to use UART communication.
+//#define USB_SERIAL_WAIT 1 // Wait for USB connection before starting grblHAL.
+//#define SPINDLE_HUANYANG 1 // Set to 1 or 2 for Huanyang VFD spindle. Requires spindle plugin.
+//#define QEI_ENABLE 1 // Enable quadrature encoder interfaces. Max value is 1. Requires encoder plugin.
+//#define ETHERNET_ENABLE 1 // Ethernet streaming. Requires networking plugin.
+//#define SDCARD_ENABLE 1 // Run gcode programs from SD card, requires sdcard plugin.
+//#define KEYPAD_ENABLE 1 // I2C keypad for jogging etc., requires keypad plugin.
+//#define PLASMA_ENABLE 1 // Plasma/THC plugin. To be completed.
+//#define PPI_ENABLE 1 // Laser PPI plugin. To be completed.
+//#define ODOMETER_ENABLE 1 // Odometer plugin. To be completed.
+//#define EEPROM_ENABLE 1 // I2C EEPROM support. Set to 1 for 24LC16(2K), 2 for larger sizes. Requires eeprom plugin.
+//#define EEPROM_IS_FRAM 1 // Uncomment when EEPROM is enabled and chip is FRAM, this to remove write delay.
+
+//#define ESTOP_ENABLE 0 // When enabled only real-time report requests will be executed when the reset pin is asserted.
+ // Note: if left commented out the default setting is determined from COMPATIBILITY_LEVEL.
+
+#if ETHERNET_ENABLE > 0
+#define TELNET_ENABLE 1 // Telnet daemon - requires Ethernet streaming enabled.
+#define WEBSOCKET_ENABLE 1 // Websocket daemon - requires Ethernet streaming enabled.
+#define NETWORK_HOSTNAME "GRBL"
+#define NETWORK_IPMODE 1 // 0 = static, 1 = DHCP, 2 = AutoIP
+#define NETWORK_IP "192.168.5.1"
+#define NETWORK_GATEWAY "192.168.5.1"
+#define NETWORK_MASK "255.255.255.0"
+#define NETWORK_TELNET_PORT 23
+#define NETWORK_WEBSOCKET_PORT 80
+#define NETWORK_HTTP_PORT 80
+#endif
diff --git a/grblHAL_Teensy4/src/uart.c b/grblHAL_Teensy4/src/uart.c
new file mode 100644
index 0000000..22b0dcc
--- /dev/null
+++ b/grblHAL_Teensy4/src/uart.c
@@ -0,0 +1,431 @@
+/*
+ uart.c - driver code for IMXRT1062 processor (on Teensy 4.0 board)
+
+ Part of grblHAL
+
+ Some parts of this code is Copyright (c) 2020 Terje Io
+
+ Some parts are derived from HardwareSerial.cpp in the Teensyduino Core Library
+
+*/
+
+/* Teensyduino Core Library
+ * http://www.pjrc.com/teensy/
+ * Copyright (c) 2019 PJRC.COM, LLC.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * 1. The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * 2. If the Software is incorporated into a build system that allows
+ * selection among a list of target devices, then similar target
+ * devices manufactured by PJRC.COM must be included in the list of
+ * target devices and selectable in the same manner.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include
+
+#include "driver.h"
+
+#define UART_CLOCK 24000000
+#define CTRL_ENABLE (LPUART_CTRL_TE | LPUART_CTRL_RE | LPUART_CTRL_RIE | LPUART_CTRL_ILIE)
+#define CTRL_TX_ACTIVE (CTRL_ENABLE | LPUART_CTRL_TIE)
+#define CTRL_TX_COMPLETING (CTRL_ENABLE | LPUART_CTRL_TCIE)
+#define CTRL_TX_INACTIVE CTRL_ENABLE
+
+#ifndef UART_PORT
+#define UART uart1_hardware
+#elif UART_PORT == 5
+#define UART uart5_hardware
+#else
+#error "UART port not available!"
+#endif
+
+typedef struct {
+ IMXRT_LPUART_t *port;
+ volatile uint32_t *ccm_register;
+ const uint32_t ccm_value;
+ enum IRQ_NUMBER_t irq;
+ void (*irq_handler)(void);
+ pin_info_t rx_pin;
+ pin_info_t tx_pin;
+} uart_hardware_t;
+
+static void uart_interrupt_handler (void);
+
+static const uart_hardware_t uart1_hardware =
+{
+ .port = &IMXRT_LPUART6,
+ .ccm_register = &CCM_CCGR3,
+ .ccm_value = CCM_CCGR3_LPUART6(CCM_CCGR_ON),
+ .irq = IRQ_LPUART6,
+ .irq_handler = uart_interrupt_handler,
+ .rx_pin = {
+ .pin = 0,
+ .mux_val = 2,
+ .select_reg = &IOMUXC_LPUART6_RX_SELECT_INPUT,
+ .select_val = 1
+ },
+ .tx_pin = {
+ .pin = 1,
+ .mux_val = 2,
+ .select_reg = NULL,
+ .select_val = 0
+ }
+};
+
+static const uart_hardware_t uart5_hardware =
+{
+ .port = &IMXRT_LPUART8,
+ .ccm_register = &CCM_CCGR6,
+ .ccm_value = CCM_CCGR6_LPUART8(CCM_CCGR_ON),
+ .irq = IRQ_LPUART8,
+ .irq_handler = uart_interrupt_handler,
+ .rx_pin = {
+ .pin = 21,
+ .mux_val = 2,
+ .select_reg = &IOMUXC_LPUART8_RX_SELECT_INPUT,
+ .select_val = 1
+ },
+ .tx_pin = {
+ .pin = 20,
+ .mux_val = 2,
+ .select_reg = &IOMUXC_LPUART8_TX_SELECT_INPUT,
+ .select_val = 1
+ }
+};
+
+/*
+
+#define PIN_TO_BASEREG(pin) (portOutputRegister(pin))
+#define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin))
+#define IO_REG_TYPE uint32_t
+#define IO_REG_BASE_ATTR
+#define IO_REG_MASK_ATTR
+#define DIRECT_READ(base, mask) ((*((base)+2) & (mask)) ? 1 : 0)
+#define DIRECT_MODE_INPUT(base, mask) (*((base)+1) &= ~(mask))
+#define DIRECT_MODE_OUTPUT(base, mask) (*((base)+1) |= (mask))
+#define DIRECT_WRITE_LOW(base, mask) (*((base)+34) = (mask))
+#define DIRECT_WRITE_HIGH(base, mask) (*((base)+33) = (mask))
+
+
+static bool transmitting_ = 0;
+volatile uint32_t *transmit_pin_baseReg_ = 0;
+uint32_t transmit_pin_bitmask_ = 0;
+
+void transmitterEnable(uint8_t pin)
+{
+ while (transmitting_) ;
+ pinMode(pin, OUTPUT);
+ transmit_pin_baseReg_ = PIN_TO_BASEREG(pin);
+ transmit_pin_bitmask_ = PIN_TO_BITMASK(pin);
+ DIRECT_WRITE_LOW(transmit_pin_baseReg_, transmit_pin_bitmask_);
+}
+*/
+
+static uint16_t tx_fifo_size;
+static stream_tx_buffer_t txbuffer = {0};
+static stream_rx_buffer_t rxbuffer = {0}, rxbackup;
+
+void serialInit (uint32_t baud_rate)
+{
+// uart_hardware_t *hardware = &UART;
+ float base = (float)UART_CLOCK / (float)baud_rate;
+ float besterr = 1e20;
+ int bestdiv = 1;
+ int bestosr = 4;
+ for (int osr = 4; osr <= 32; osr++) {
+ float div = base / (float)osr;
+ int divint = (int)(div + 0.5f);
+ if (divint < 1)
+ divint = 1;
+ else if (divint > 8191)
+ divint = 8191;
+ float err = ((float)divint - div) / div;
+ if (err < 0.0f)
+ err = -err;
+ if (err <= besterr) {
+ besterr = err;
+ bestdiv = divint;
+ bestosr = osr;
+ }
+ }
+
+ *UART.ccm_register |= UART.ccm_value;
+
+ *(portControlRegister(UART.rx_pin.pin)) = IOMUXC_PAD_DSE(7) | IOMUXC_PAD_PKE | IOMUXC_PAD_PUE | IOMUXC_PAD_PUS(3) | IOMUXC_PAD_HYS;
+ *(portConfigRegister(UART.rx_pin.pin)) = UART.rx_pin.mux_val;
+ if (UART.rx_pin.select_reg)
+ *(UART.rx_pin.select_reg) = UART.rx_pin.select_val;
+
+ *(portControlRegister(UART.tx_pin.pin)) = IOMUXC_PAD_SRE | IOMUXC_PAD_DSE(3) | IOMUXC_PAD_SPEED(3);
+ *(portConfigRegister(UART.tx_pin.pin)) = UART.tx_pin.mux_val;
+ if (UART.tx_pin.select_reg)
+ *(UART.tx_pin.select_reg) = UART.tx_pin.select_val;
+
+ UART.port->BAUD = LPUART_BAUD_OSR(bestosr - 1) | LPUART_BAUD_SBR(bestdiv) | (bestosr <= 8 ? LPUART_BAUD_BOTHEDGE : 0);
+ UART.port->PINCFG = 0;
+
+ // Enable the transmitter, receiver and enable receiver interrupt
+ NVIC_DISABLE_IRQ(UART.irq);
+ attachInterruptVector(UART.irq, UART.irq_handler);
+ NVIC_SET_PRIORITY(UART.irq, 0);
+ NVIC_ENABLE_IRQ(UART.irq);
+
+ tx_fifo_size = (UART.port->FIFO >> 4) & 0x7;
+ tx_fifo_size = tx_fifo_size ? (2 << tx_fifo_size) : 1;
+
+ uint8_t tx_water = (tx_fifo_size < 16) ? tx_fifo_size >> 1 : 7;
+ uint16_t rx_fifo_size = (((UART.port->FIFO >> 0) & 0x7) << 2);
+ uint8_t rx_water = (rx_fifo_size < 16) ? rx_fifo_size >> 1 : 7;
+ /*
+ Serial.printf("SerialX::begin stat:%x ctrl:%x fifo:%x water:%x\n", UART.port->STAT, UART.port->CTRL, UART.port->FIFO, UART.port->WATER );
+ Serial.printf(" FIFO sizes: tx:%d rx:%d\n",tx_fifo_size, rx_fifo_size);
+ Serial.printf(" Watermark tx:%d, rx: %d\n", tx_water, rx_water);
+ */
+ UART.port->WATER = LPUART_WATER_RXWATER(rx_water) | LPUART_WATER_TXWATER(tx_water);
+ UART.port->FIFO |= LPUART_FIFO_TXFE | LPUART_FIFO_RXFE;
+ // lets configure up our CTRL register value
+ uint32_t ctrl = CTRL_TX_INACTIVE;
+
+uint16_t format = 0;
+
+ // Now process the bits in the Format value passed in
+ // Bits 0-2 - Parity plus 9 bit.
+ ctrl |= (format & (LPUART_CTRL_PT | LPUART_CTRL_PE) ); // configure parity - turn off PT, PE, M and configure PT, PE
+ if (format & 0x04) ctrl |= LPUART_CTRL_M; // 9 bits (might include parity)
+ if ((format & 0x0F) == 0x04) ctrl |= LPUART_CTRL_R9T8; // 8N2 is 9 bit with 9th bit always 1
+
+ // Bit 5 TXINVERT
+ if (format & 0x20) ctrl |= LPUART_CTRL_TXINV; // tx invert
+
+ // write out computed CTRL
+ UART.port->CTRL = ctrl;
+
+ // Bit 3 10 bit - Will assume that begin already cleared it.
+ // process some other bits which change other registers.
+ if (format & 0x08) UART.port->BAUD |= LPUART_BAUD_M10;
+
+ // Bit 4 RXINVERT
+ uint32_t c = UART.port->STAT & ~LPUART_STAT_RXINV;
+ if (format & 0x10) c |= LPUART_STAT_RXINV; // rx invert
+ UART.port->STAT = c;
+
+ // bit 8 can turn on 2 stop bit mote
+ if ( format & 0x100) UART.port->BAUD |= LPUART_BAUD_SBNS;
+
+//transmitterEnable(1);
+}
+
+bool serialSetBaudRate (uint32_t baud_rate)
+{
+ static bool init_ok = false;
+
+ if(!init_ok) {
+ serialInit(baud_rate);
+ init_ok = true;
+ }
+
+ return true;
+}
+
+//
+// serialGetC - returns -1 if no data available
+//
+int16_t serialGetC (void)
+{
+ int16_t data;
+ uint_fast16_t bptr = rxbuffer.tail;
+
+ if(bptr == rxbuffer.head)
+ return -1; // no data available else EOF
+
+ data = rxbuffer.data[bptr++]; // Get next character, increment tmp pointer
+ rxbuffer.tail = bptr & (RX_BUFFER_SIZE - 1); // and update pointer
+
+ return data;
+}
+
+void serialTxFlush (void)
+{
+ txbuffer.tail = txbuffer.head;
+}
+
+uint16_t serialRxCount (void)
+{
+ uint_fast16_t head = rxbuffer.head, tail = rxbuffer.tail;
+
+ return BUFCOUNT(head, tail, RX_BUFFER_SIZE);
+}
+
+uint16_t serialRxFree (void)
+{
+ return (RX_BUFFER_SIZE - 1) - serialRxCount();
+}
+
+void serialRxFlush (void)
+{
+ rxbuffer.tail = rxbuffer.head;
+ rxbuffer.overflow = false;
+}
+
+void serialRxCancel (void)
+{
+ serialRxFlush();
+ rxbuffer.data[rxbuffer.head] = ASCII_CAN;
+ rxbuffer.head = (rxbuffer.tail + 1) & (RX_BUFFER_SIZE - 1);
+}
+
+bool serialPutC (const char c)
+{
+ uint_fast16_t next_head;
+
+// swr(c); return true;
+
+// if (transmit_pin_baseReg_) DIRECT_WRITE_HIGH(transmit_pin_baseReg_, transmit_pin_bitmask_);
+
+ if(txbuffer.head == txbuffer.tail && ((UART.port->WATER >> 8) & 0x7) < tx_fifo_size) {
+ UART.port->DATA = c;
+ return true;
+ }
+
+ next_head = (txbuffer.head + 1) & (TX_BUFFER_SIZE - 1); // Get and update head pointer
+
+ while(txbuffer.tail == next_head) { // Buffer full, block until space is available...
+ if(!hal.stream_blocking_callback())
+ return false;
+ }
+
+ txbuffer.data[txbuffer.head] = c; // Add data to buffer
+ txbuffer.head = next_head; // and update head pointer
+
+ __disable_irq();
+// transmitting_ = 1;
+ UART.port->CTRL |= LPUART_CTRL_TIE; // (may need to handle this issue)BITBAND_SET_BIT(LPUART0_CTRL, TIE_BIT); // Enable TX interrupts
+ __enable_irq();
+
+ return true;
+}
+
+void serialWriteS (const char *data)
+{
+ char c, *ptr = (char *)data;
+
+ while((c = *ptr++) != '\0')
+ serialPutC(c);
+}
+
+void serialWrite(const char *s, uint16_t length)
+{
+ char *ptr = (char *)s;
+
+ while(length--)
+ serialPutC(*ptr++);
+}
+
+// "dummy" version of serialGetC
+static int16_t serialGetNull (void)
+{
+ return -1;
+}
+
+bool serialSuspendInput (bool suspend)
+{
+ if(suspend)
+ hal.stream.read = serialGetNull;
+ else if(rxbuffer.backup)
+ memcpy(&rxbuffer, &rxbackup, sizeof(stream_rx_buffer_t));
+
+ return rxbuffer.tail != rxbuffer.head;
+}
+
+uint16_t serialTxCount(void) {
+
+ uint_fast16_t head = txbuffer.head, tail = txbuffer.tail;
+
+ return BUFCOUNT(head, tail, TX_BUFFER_SIZE) + ((UART.port->WATER >> 8) & 0x7) + ((UART.port->STAT & LPUART_STAT_TC) ? 0 : 1);
+}
+
+static void uart_interrupt_handler (void)
+{
+ uint_fast16_t bptr;
+ uint32_t data, ctrl = UART.port->CTRL;
+
+ if ((ctrl & LPUART_CTRL_TIE) && (UART.port->STAT & LPUART_STAT_TDRE))
+ {
+ bptr = txbuffer.tail;
+
+ do {
+ if(txbuffer.head != bptr) {
+
+ UART.port->DATA = txbuffer.data[bptr++]; // Put character in TXT register
+ bptr &= (TX_BUFFER_SIZE - 1); // and update tmp tail pointer
+
+ } else
+ break;
+
+ } while(((UART.port->WATER >> 8) & 0x7) < tx_fifo_size);
+
+ txbuffer.tail = bptr; // Update tail pinter
+
+ if(bptr == txbuffer.head) { // Disable TX interrups
+ UART.port->CTRL &= ~LPUART_CTRL_TIE;
+// UART.port->CTRL |= LPUART_CTRL_TCIE; // Actually wondering if we can just leave this one on...
+ }
+ }
+
+ if ((ctrl & LPUART_CTRL_TCIE) && (UART.port->STAT & LPUART_STAT_TC))
+ {
+// transmitting_ = 0;
+// if (transmit_pin_baseReg_) DIRECT_WRITE_LOW(transmit_pin_baseReg_, transmit_pin_bitmask_);
+
+ UART.port->CTRL &= ~LPUART_CTRL_TCIE;
+ }
+
+ if (UART.port->STAT & (LPUART_STAT_RDRF | LPUART_STAT_IDLE)) {
+
+ while ((UART.port->WATER >> 24) & 0x7) {
+
+ bptr = (rxbuffer.head + 1) & (RX_BUFFER_SIZE - 1); // Get next head pointer
+ data = UART.port->DATA & 0xFF; // and read input (use only 8 bits of data)
+
+ if(bptr == rxbuffer.tail) { // If buffer full
+ rxbuffer.overflow = true; // flag overflow
+ } else {
+#if MODBUS_ENABLE
+ rxbuffer.data[rxbuffer.head] = (char)data; // Add data to buffer
+ rxbuffer.head = bptr; // and update pointer
+#else
+ if(data == CMD_TOOL_ACK && !rxbuffer.backup) {
+ memcpy(&rxbackup, &rxbuffer, sizeof(stream_rx_buffer_t));
+ rxbuffer.backup = true;
+ rxbuffer.tail = rxbuffer.head;
+ hal.stream.read = serialGetC; // restore normal input
+ } else if(!hal.stream.enqueue_realtime_command((char)data)) {
+ rxbuffer.data[rxbuffer.head] = (char)data; // Add data to buffer
+ rxbuffer.head = bptr; // and update pointer
+ }
+#endif
+ }
+ }
+
+ if (UART.port->STAT & LPUART_STAT_IDLE)
+ UART.port->STAT |= LPUART_STAT_IDLE; // writing a 1 to idle should clear it.
+ }
+}
diff --git a/grblHAL_Teensy4/src/uart.h b/grblHAL_Teensy4/src/uart.h
new file mode 100644
index 0000000..1847bf4
--- /dev/null
+++ b/grblHAL_Teensy4/src/uart.h
@@ -0,0 +1,43 @@
+/*
+
+ uart.h - driver code for IMXRT1062 processor (on Teensy 4.0 board)
+
+ Part of grblHAL
+
+ Copyright (c) 2020 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+*/
+
+#ifndef _HAL_SERIAL_H_
+#define _HAL_SERIAL_H_
+
+#include
+#include
+
+void serialInit (uint32_t baud_rate);
+bool serialSetBaudRate (uint32_t baud_rate);
+int16_t serialGetC (void);
+bool serialPutC (const char c);
+void serialWrite(const char *s, uint16_t length);
+void serialWriteS (const char *data);
+bool serialSuspendInput (bool suspend);
+uint16_t serialRxFree (void);
+uint16_t serialRxCount (void);
+uint16_t serialTxCount (void);
+void serialRxFlush (void);
+void serialTxFlush (void);
+void serialRxCancel (void);
+
+#endif
diff --git a/grblHAL_Teensy4/src/usb_serial_ard.cpp b/grblHAL_Teensy4/src/usb_serial_ard.cpp
new file mode 100644
index 0000000..ef30e97
--- /dev/null
+++ b/grblHAL_Teensy4/src/usb_serial_ard.cpp
@@ -0,0 +1,245 @@
+/*
+
+ usb_serial_ard.cpp - driver code for IMXRT1062 processor (on Teensy 4.0 board) : USB serial port wrapper
+
+ Part of grblHAL
+
+ Copyright (c) 2018-2020 Terje Io
+
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+
+*/
+
+#include
+
+#include "Arduino.h"
+
+#include "driver.h"
+
+#if USB_SERIAL_CDC == 1
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLOCK_RX_BUFFER_SIZE 20
+
+static stream_block_tx_buffer_t txbuf = {0};
+static char rxbuf[BLOCK_RX_BUFFER_SIZE];
+static stream_rx_buffer_t usb_rxbuffer, usb_rxbackup;
+
+void usb_serialInit(void)
+{
+ txbuf.s = txbuf.data;
+
+ SerialUSB.begin(BAUD_RATE);
+
+#if USB_SERIAL_WAIT
+ while(!SerialUSB); // Wait for connection
+#endif
+
+ txbuf.max_length = SerialUSB.availableForWrite(); // 6144 bytes
+ txbuf.max_length = (txbuf.max_length > BLOCK_TX_BUFFER_SIZE ? BLOCK_TX_BUFFER_SIZE : txbuf.max_length) - 20;
+}
+
+//
+// Returns number of characters in serial input buffer
+//
+uint16_t usb_serialRxCount (void)
+{
+ uint_fast16_t tail = usb_rxbuffer.tail, head = usb_rxbuffer.head;
+ return (uint16_t)BUFCOUNT(head, tail, RX_BUFFER_SIZE);
+}
+
+//
+// Returns number of free characters in serial input buffer
+//
+uint16_t usb_serialRxFree (void)
+{
+ uint_fast16_t tail = usb_rxbuffer.tail, head = usb_rxbuffer.head;
+ return (uint16_t)((RX_BUFFER_SIZE - 1) - BUFCOUNT(head, tail, RX_BUFFER_SIZE));
+}
+
+//
+// Flushes the serial input buffer (including the USB buffer)
+//
+void usb_serialRxFlush (void)
+{
+ SerialUSB.flush();
+ usb_rxbuffer.tail = usb_rxbuffer.head;
+}
+
+//
+// Flushes and adds a CAN character to the serial input buffer
+//
+void usb_serialRxCancel (void)
+{
+ usb_rxbuffer.data[usb_rxbuffer.head] = CMD_RESET;
+ usb_rxbuffer.tail = usb_rxbuffer.head;
+ usb_rxbuffer.head = (usb_rxbuffer.tail + 1) & (RX_BUFFER_SIZE - 1);
+}
+
+//
+// Writes a character to the serial output stream
+//
+bool usb_serialPutC (const char c)
+{
+ SerialUSB.write(c);
+
+ return true;
+}
+
+//
+// Writes a null terminated string to the serial output stream, blocks if buffer full.
+// Buffers locally up to 40 characters or until the string is terminated with a ASCII_LF character.
+// NOTE: grbl always sends ASCII_LF terminated strings!
+//
+void usb_serialWriteS (const char *s)
+{
+ if(*s == '\0')
+ return;
+
+ size_t length = strlen(s);
+
+ if((length + txbuf.length) < BLOCK_TX_BUFFER_SIZE) {
+
+ memcpy(txbuf.s, s, length);
+ txbuf.length += length;
+ txbuf.s += length;
+
+ if(s[length - 1] == ASCII_LF || txbuf.length > txbuf.max_length) {
+
+ size_t txfree;
+ txbuf.s = txbuf.data;
+
+ while(txbuf.length) {
+
+ if((txfree = SerialUSB.availableForWrite()) > 10) {
+
+ length = txfree < txbuf.length ? txfree : txbuf.length;
+
+ SerialUSB.write((uint8_t *)txbuf.s, length); // doc is wrong - does not return bytes sent!
+
+ txbuf.length -= length;
+ txbuf.s += length;
+ }
+
+ if(txbuf.length && !hal.stream_blocking_callback()) {
+ txbuf.length = 0;
+ txbuf.s = txbuf.data;
+ return;
+ }
+ }
+ txbuf.s = txbuf.data;
+ }
+ }
+}
+
+//
+// Writes a null terminated string to the serial output stream followed by EOL, blocks if buffer full
+//
+void usb_serialWriteLn (const char *s)
+{
+ usb_serialWriteS(s);
+ usb_serialWriteS(ASCII_EOL);
+}
+
+//
+// Writes a number of characters from string to the serial output stream followed by EOL, blocks if buffer full
+//
+void usb_serialWrite (const char *s, uint16_t length)
+{
+ char *ptr = (char *)s;
+
+ while(length--)
+ usb_serialPutC(*ptr++);
+}
+
+//
+// serialGetC - returns -1 if no data available
+//
+int16_t usb_serialGetC (void)
+{
+ uint16_t bptr = usb_rxbuffer.tail;
+
+ if(bptr == usb_rxbuffer.head)
+ return -1; // no data available else EOF
+
+ char data = usb_rxbuffer.data[bptr++]; // Get next character, increment tmp pointer
+ usb_rxbuffer.tail = bptr & (RX_BUFFER_SIZE - 1); // and update pointer
+
+ return (int16_t)data;
+}
+
+// "dummy" version of serialGetC
+static int16_t serialGetNull (void)
+{
+ return -1;
+}
+
+bool usb_serialSuspendInput (bool suspend)
+{
+ if(suspend)
+ hal.stream.read = serialGetNull;
+ else if(usb_rxbuffer.backup)
+ memcpy(&usb_rxbuffer, &usb_rxbackup, sizeof(stream_rx_buffer_t));
+
+ return usb_rxbuffer.tail != usb_rxbuffer.head;
+}
+
+//
+// This function get called from the systick interrupt handler,
+// used here to get characters off the USB serial input stream and buffer
+// them for processing by grbl. Real time command characters are stripped out
+// and submitted for realtime processing.
+//
+void usb_execute_realtime (uint_fast16_t state)
+{
+ char c, *dp;
+ int avail, free;
+
+ if((avail = SerialUSB.available())) {
+
+ dp = rxbuf;
+ free = usb_serialRxFree();
+ free = free > BLOCK_RX_BUFFER_SIZE ? BLOCK_RX_BUFFER_SIZE : free;
+
+ avail = SerialUSB.readBytes(rxbuf, avail > free ? free : avail);
+
+ while(avail--) {
+ c = *dp++;
+ if(c == CMD_TOOL_ACK && !usb_rxbuffer.backup) {
+ memcpy(&usb_rxbackup, &usb_rxbuffer, sizeof(stream_rx_buffer_t));
+ usb_rxbuffer.backup = true;
+ usb_rxbuffer.tail = usb_rxbuffer.head;
+ hal.stream.read = usb_serialGetC; // restore normal input
+ } else if(!hal.stream.enqueue_realtime_command(c)) {
+ uint32_t bptr = (usb_rxbuffer.head + 1) & (RX_BUFFER_SIZE - 1); // Get next head pointer
+ if(bptr == usb_rxbuffer.tail) // If buffer full
+ usb_rxbuffer.overflow = On; // flag overflow,
+ else {
+ usb_rxbuffer.data[usb_rxbuffer.head] = c; // else add character data to buffer
+ usb_rxbuffer.head = bptr; // and update pointer
+ }
+ }
+ }
+ }
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/grblHAL_Teensy4/src/usb_serial_ard.h b/grblHAL_Teensy4/src/usb_serial_ard.h
new file mode 100644
index 0000000..71f6285
--- /dev/null
+++ b/grblHAL_Teensy4/src/usb_serial_ard.h
@@ -0,0 +1,48 @@
+/*
+
+ usb_serial_ard.h - driver code for IMXRT1062 processor (on Teensy 4.0 board)
+
+ Part of grblHAL
+
+ Copyright (c) 2018-2020 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+
+*/
+
+#ifndef _USB_SERIAL_H_
+#define _USB_SERIAL_H_
+
+#include
+#include
+
+extern void usb_execute_realtime (uint_fast16_t state);
+
+#define usb_serial_poll() usb_execute_realtime(0)
+
+void usb_serialInit(void);
+int16_t usb_serialGetC(void);
+bool usb_serialPutC(const char c);
+void usb_serialWriteS(const char *s);
+void usb_serialWriteLn(const char *s);
+void usb_serialWrite(const char *s, uint16_t length);
+bool usb_serialSuspendInput (bool suspend);
+
+uint16_t usb_serialTxCount(void);
+uint16_t usb_serialRxCount(void);
+uint16_t usb_serialRxFree(void);
+void usb_serialRxFlush(void);
+void usb_serialRxCancel(void);
+
+#endif
diff --git a/grblHAL_Teensy4/src/usb_serial_pjrc.c b/grblHAL_Teensy4/src/usb_serial_pjrc.c
new file mode 100644
index 0000000..a3c91e0
--- /dev/null
+++ b/grblHAL_Teensy4/src/usb_serial_pjrc.c
@@ -0,0 +1,230 @@
+/*
+
+ usb_serial_pjrc.c - driver code for IMXRT1062 processor (on Teensy 4.0 board) : USB serial port wrapper, PJRC version
+
+ Part of grblHAL
+
+ Copyright (c) 2018-2020 Terje Io
+
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+
+*/
+
+#include
+
+#include "usb_serial.h"
+
+#include "driver.h"
+
+#if USB_SERIAL_CDC == 2
+
+#define BLOCK_RX_BUFFER_SIZE 20
+
+static stream_block_tx_buffer_t txbuf = {0};
+static char rxbuf[BLOCK_RX_BUFFER_SIZE];
+static stream_rx_buffer_t usb_rxbuffer, usb_rxbackup;
+
+void usb_serialInit(void)
+{
+// usb_serial_configure(); // Done somewhere already - do not call again
+ txbuf.s = txbuf.data;
+ txbuf.max_length = usb_serial_write_buffer_free(); // 6144
+ txbuf.max_length = (txbuf.max_length > BLOCK_TX_BUFFER_SIZE ? BLOCK_TX_BUFFER_SIZE : txbuf.max_length) - 20;
+
+}
+
+//
+// Returns number of characters in serial input buffer
+//
+uint16_t usb_serialRxCount (void)
+{
+ uint_fast16_t tail = usb_rxbuffer.tail, head = usb_rxbuffer.head;
+ return (uint16_t)BUFCOUNT(head, tail, RX_BUFFER_SIZE);
+}
+
+//
+// Returns number of free characters in serial input buffer
+//
+uint16_t usb_serialRxFree (void)
+{
+ uint_fast16_t tail = usb_rxbuffer.tail, head = usb_rxbuffer.head;
+ return (uint16_t)((RX_BUFFER_SIZE - 1) - BUFCOUNT(head, tail, RX_BUFFER_SIZE));
+}
+
+//
+// Flushes the serial input buffer (including the USB buffer)
+//
+void usb_serialRxFlush (void)
+{
+ usb_serial_flush_input();
+ usb_rxbuffer.tail = usb_rxbuffer.head;
+}
+
+//
+// Flushes and adds a CAN character to the serial input buffer
+//
+void usb_serialRxCancel (void)
+{
+ usb_rxbuffer.data[usb_rxbuffer.head] = CMD_RESET;
+ usb_rxbuffer.tail = usb_rxbuffer.head;
+ usb_rxbuffer.head = (usb_rxbuffer.tail + 1) & (RX_BUFFER_SIZE - 1);
+}
+
+//
+// Writes a character to the serial output stream
+//
+bool usb_serialPutC (const char c)
+{
+ usb_serial_putchar(c);
+
+ return true;
+}
+
+//
+// Writes a null terminated string to the serial output stream, blocks if buffer full
+//
+void usb_serialWriteS (const char *s)
+{
+ if(*s == '\0')
+ return;
+
+ size_t length = strlen(s);
+
+ if((length + txbuf.length) < BLOCK_TX_BUFFER_SIZE) {
+
+ memcpy(txbuf.s, s, length);
+ txbuf.length += length;
+ txbuf.s += length;
+
+ if(s[length - 1] == ASCII_LF || txbuf.length > txbuf.max_length) {
+
+ size_t txfree;
+ txbuf.s = txbuf.data;
+
+ while(txbuf.length) {
+
+ if((txfree = usb_serial_write_buffer_free()) > 10) {
+
+ length = txfree < txbuf.length ? txfree : txbuf.length;
+
+ usb_serial_write(txbuf.s, length); //
+
+ txbuf.length -= length;
+ txbuf.s += length;
+ }
+
+ if(txbuf.length && !hal.stream_blocking_callback()) {
+ txbuf.length = 0;
+ txbuf.s = txbuf.data;
+ return;
+ }
+ }
+ txbuf.s = txbuf.data;
+ }
+ }
+}
+
+//
+// Writes a null terminated string to the serial output stream followed by EOL, blocks if buffer full
+//
+void usb_serialWriteLn (const char *s)
+{
+ usb_serialWriteS(s);
+ usb_serialWriteS(ASCII_EOL);
+}
+
+//
+// Writes a number of characters from string to the serial output stream followed by EOL, blocks if buffer full
+//
+void usb_serialWrite (const char *s, uint16_t length)
+{
+ char *ptr = (char *)s;
+
+ while(length--)
+ usb_serialPutC(*ptr++);
+}
+
+//
+// serialGetC - returns -1 if no data available
+//
+int16_t usb_serialGetC (void)
+{
+ uint16_t bptr = usb_rxbuffer.tail;
+
+ if(bptr == usb_rxbuffer.head)
+ return -1; // no data available else EOF
+
+ char data = usb_rxbuffer.data[bptr++]; // Get next character, increment tmp pointer
+ usb_rxbuffer.tail = bptr & (RX_BUFFER_SIZE - 1); // and update pointer
+
+ return (int16_t)data;
+}
+
+// "dummy" version of serialGetC
+static int16_t serialGetNull (void)
+{
+ return -1;
+}
+
+bool usb_serialSuspendInput (bool suspend)
+{
+ if(suspend)
+ hal.stream.read = serialGetNull;
+ else if(usb_rxbuffer.backup)
+ memcpy(&usb_rxbuffer, &usb_rxbackup, sizeof(stream_rx_buffer_t));
+
+ return usb_rxbuffer.tail != usb_rxbuffer.head;
+}
+
+//
+// This function get called from the systick interrupt handler,
+// used here to get characters off the USB serial input stream and buffer
+// them for processing by grbl. Real time command characters are stripped out
+// and submitted for realtime processing.
+//
+void usb_execute_realtime (uint_fast16_t state)
+{
+ char c, *dp;
+ int avail, free;
+
+ if((avail = usb_serial_available())) {
+
+ dp = rxbuf;
+ free = usb_serialRxFree();
+ free = free > BLOCK_RX_BUFFER_SIZE ? BLOCK_RX_BUFFER_SIZE : free;
+
+ avail = usb_serial_read(rxbuf, avail > free ? free : avail);
+
+ while(avail--) {
+ c = *dp++;
+ if(c == CMD_TOOL_ACK && !usb_rxbuffer.backup) {
+ memcpy(&usb_rxbackup, &usb_rxbuffer, sizeof(stream_rx_buffer_t));
+ usb_rxbuffer.backup = true;
+ usb_rxbuffer.tail = usb_rxbuffer.head;
+ hal.stream.read = usb_serialGetC; // restore normal input
+ } else if(!hal.stream.enqueue_realtime_command(c)) {
+ uint32_t bptr = (usb_rxbuffer.head + 1) & (RX_BUFFER_SIZE - 1); // Get next head pointer
+ if(bptr == usb_rxbuffer.tail) // If buffer full
+ usb_rxbuffer.overflow = On; // flag overflow,
+ else {
+ usb_rxbuffer.data[usb_rxbuffer.head] = c; // else add character data to buffer
+ usb_rxbuffer.head = bptr; // and update pointer
+ }
+ }
+ }
+ }
+}
+
+#endif
diff --git a/grblHAL_Teensy4/src/usb_serial_pjrc.h b/grblHAL_Teensy4/src/usb_serial_pjrc.h
new file mode 100644
index 0000000..ea7d675
--- /dev/null
+++ b/grblHAL_Teensy4/src/usb_serial_pjrc.h
@@ -0,0 +1,48 @@
+/*
+
+ usb_serial.h - driver code for IMXRT1062 processor (on Teensy 4.0 board)
+
+ Part of grblHAL
+
+ Copyright (c) 2018-2020 Terje Io
+
+ Grbl 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 3 of the License, or
+ (at your option) any later version.
+
+ Grbl 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 Grbl. If not, see .
+
+*/
+
+#ifndef _USB_SERIAL_H_
+#define _USB_SERIAL_H_
+
+#include
+#include
+
+extern void usb_execute_realtime (uint_fast16_t state);
+
+#define usb_serial_poll() usb_execute_realtime(0)
+
+void usb_serialInit(void);
+int16_t usb_serialGetC(void);
+bool usb_serialPutC(const char c);
+void usb_serialWriteS(const char *s);
+void usb_serialWriteLn(const char *s);
+void usb_serialWrite(const char *s, uint16_t length);
+bool usb_serialSuspendInput (bool suspend);
+
+uint16_t usb_serialTxCount(void);
+uint16_t usb_serialRxCount(void);
+uint16_t usb_serialRxFree(void);
+void usb_serialRxFlush(void);
+void usb_serialRxCancel(void);
+
+#endif
diff --git a/src/eeprom b/src/eeprom
deleted file mode 160000
index 292df8b..0000000
--- a/src/eeprom
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 292df8b7cc2a7d9df7a647483eef6ba8c68c250d
diff --git a/src/encoder b/src/encoder
deleted file mode 160000
index 8ee8eea..0000000
--- a/src/encoder
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 8ee8eea4e4974ae54787b01aa4a17fa744580246
diff --git a/src/grbl b/src/grbl
deleted file mode 160000
index cdca5ad..0000000
--- a/src/grbl
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit cdca5ad55cc9b6076401a841583bd53e3baf1481
diff --git a/src/keypad b/src/keypad
deleted file mode 160000
index 0ae3a11..0000000
--- a/src/keypad
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 0ae3a110b6064596022aa978a5d0a1f14abaa0d7
diff --git a/src/laser b/src/laser
deleted file mode 160000
index 942f560..0000000
--- a/src/laser
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 942f5607ba4d6428d8a2f5b690e251d80d462c61
diff --git a/src/networking b/src/networking
deleted file mode 160000
index 3652e14..0000000
--- a/src/networking
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 3652e142c228c7a168cfa929497f570d541511f5
diff --git a/src/odometer b/src/odometer
deleted file mode 160000
index 3ba87d7..0000000
--- a/src/odometer
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 3ba87d7450bb6c4f4ded4cce96a0c71cffc48fa5
diff --git a/src/plasma b/src/plasma
deleted file mode 160000
index b6240a9..0000000
--- a/src/plasma
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit b6240a9f9d88b7ef8ac368474589495cf2f43d24
diff --git a/src/sdcard b/src/sdcard
deleted file mode 160000
index 1f5a0df..0000000
--- a/src/sdcard
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 1f5a0df4e0a58480ab7c4e80f0f018152aa2da01
diff --git a/src/spindle b/src/spindle
deleted file mode 160000
index 2f81fc6..0000000
--- a/src/spindle
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 2f81fc6ce340133a48bd5a8209c34220d269308a