diff --git a/grblHAL_Teensy4/src/driver.h b/grblHAL_Teensy4/src/driver.h
index bdd164c..644aeab 100644
--- a/grblHAL_Teensy4/src/driver.h
+++ b/grblHAL_Teensy4/src/driver.h
@@ -39,6 +39,10 @@
 #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; }
 
+#ifndef MCP3221_ENABLE
+#define MCP3221_ENABLE 0
+#endif
+
 #if USB_SERIAL_CDC > 0
 //#define UART_DEBUG // For development only - enable only with USB_SERIAL_CDC enabled and SPINDLE_HUANYANG disabled
 #endif
@@ -137,6 +141,9 @@
   #if KEYPAD_ENABLE
   #error "KEYPAD_ENABLE requires I2C_PORT to be defined!"
   #endif
+  #if MCP3221_ENABLE
+  #error "MCP3221_ENABLE requires I2C_PORT to be defined!"
+  #endif
 #endif
 
 #if !(SPINDLEPWMPIN == 12 || SPINDLEPWMPIN == 13)
diff --git a/grblHAL_Teensy4/src/i2c.c b/grblHAL_Teensy4/src/i2c.c
index 0cd831e..7ce4355 100644
--- a/grblHAL_Teensy4/src/i2c.c
+++ b/grblHAL_Teensy4/src/i2c.c
@@ -3,7 +3,7 @@
 
   Part of grblHAL
 
-  Some parts of this code is Copyright (c) 2020 Terje Io
+  Some parts of this code is Copyright (c) 2020-2021 Terje Io
 
   Some parts are derived/pulled from WireIMXRT.cpp in the Teensyduino Core Library (no copyright header)
 
@@ -166,6 +166,7 @@ typedef enum {
     I2CState_ReceiveNext,
     I2CState_ReceiveNextToLast,
     I2CState_ReceiveLast,
+    I2CState_Poll,
     I2CState_Error
 } i2c_state_t;
 
@@ -273,7 +274,7 @@ uint8_t *I2C_Receive (uint32_t i2cAddr, uint8_t *buf, uint16_t bytes, bool block
     return i2c.buffer;
 }
 
-void I2C_Send (uint32_t i2cAddr, uint8_t *buf, uint16_t bytes, bool block)
+bool I2C_Send (uint32_t i2cAddr, uint8_t *buf, uint16_t bytes, bool block)
 {
     i2c.count = bytes;
     i2c.data  = buf ? buf : i2c.buffer;
@@ -290,8 +291,19 @@ void I2C_Send (uint32_t i2cAddr, uint8_t *buf, uint16_t bytes, bool block)
 
     i2c.state = i2c.count == 0 ? I2CState_AwaitCompletion : (i2c.count == 1 ? I2CState_SendLast : I2CState_SendNext);
 
-    if(block)
-        while(i2cIsBusy);
+    if(block) {
+        while(i2cIsBusy) {
+            if(bytes == 0) {
+                hal.delay_ms(2, 0);
+                if(port->MSR & LPI2C_MSR_PLTF) {
+                    wait_ready();
+                    i2c.state = I2CState_Error;
+                }
+            }
+        }
+    }
+
+    return !block || i2c.state != I2CState_Error;
 }
 
 uint8_t *I2C_ReadRegister (uint32_t i2cAddr, uint8_t *buf, uint8_t abytes, uint16_t bytes, bool block)
diff --git a/grblHAL_Teensy4/src/i2c.h b/grblHAL_Teensy4/src/i2c.h
index 9f1c461..a8eb565 100644
--- a/grblHAL_Teensy4/src/i2c.h
+++ b/grblHAL_Teensy4/src/i2c.h
@@ -1,7 +1,7 @@
 /*
   i2c.h - I2C interface
 
-  Driver code for IMXRT1062 processor (on Teensy 4.0 board)
+  Driver code for IMXRT1062 processor (on Teensy 4.x board)
 
   Part of grblHAL
 
@@ -29,7 +29,7 @@
 
 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);
+bool 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
diff --git a/grblHAL_Teensy4/src/ioports.c b/grblHAL_Teensy4/src/ioports.c
index 37cf4e6..79feb97 100644
--- a/grblHAL_Teensy4/src/ioports.c
+++ b/grblHAL_Teensy4/src/ioports.c
@@ -22,6 +22,7 @@
 */
 
 #include "driver.h"
+#include "mcp3221.h"
 
 #ifdef HAS_IOPORTS
 
@@ -31,7 +32,7 @@
 
 #include "grbl/protocol.h"
 
-static uint_fast8_t aux_n_in, aux_n_out;
+static uint_fast8_t aux_n_in, aux_n_out, analog_n_in;
 static volatile uint32_t event_bits;
 static volatile bool spin_lock = false;
 static input_signal_t *aux_in;
@@ -202,8 +203,10 @@ static int32_t wait_on_input (bool digital, uint8_t port, wait_mode_t wait_mode,
         if(port < aux_n_in)
             value = get_input(&aux_in[port], (settings.ioport.invert_in.mask << port) & 0x01, wait_mode, timeout);
     }
-//    else if(port == 0)
-//        value = analogRead(41);
+#if MCP3221_ENABLE
+    else if(port < analog_n_in)
+        value = (int32_t)MCP3221_read();
+#endif
 
     return value;
 }
@@ -257,6 +260,11 @@ void ioports_init (pin_group_pins_t *aux_inputs, pin_group_pins_t *aux_outputs)
     if((hal.port.num_digital_out = aux_n_out = aux_outputs->n_pins))
         hal.port.digital_out = digital_out;
 
+#if MCP3221_ENABLE
+    if(MCP3221_init())
+        hal.port.num_analog_in = analog_n_in = 1;
+#endif
+
     hal.port.set_pin_description = set_pin_description;
 
     details.on_get_settings = grbl.on_get_settings;
diff --git a/grblHAL_Teensy4/src/mcp3221.c b/grblHAL_Teensy4/src/mcp3221.c
new file mode 100644
index 0000000..7c1dcf1
--- /dev/null
+++ b/grblHAL_Teensy4/src/mcp3221.c
@@ -0,0 +1,41 @@
+/*
+  mcp3221.c - analog input from a MCP3221 I2C ADC
+
+  Driver code for IMXRT1062 processor (on Teensy 4.x board)
+
+  Part of grblHAL
+
+  Copyright (c) 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 <http://www.gnu.org/licenses/>.
+*/
+
+#include "i2c.h"
+#include "mcp3221.h"
+
+uint16_t MCP3221_read (void)
+{
+    uint8_t value[2];
+
+    I2C_Receive(MCP3221_ADDRESS, value, 2, true);
+
+    return value[0] << 8 | value[1];
+}
+
+bool MCP3221_init (void)
+{
+    i2c_init();
+
+    return I2C_Send(MCP3221_ADDRESS, NULL, 0, true);
+}
diff --git a/grblHAL_Teensy4/src/mcp3221.h b/grblHAL_Teensy4/src/mcp3221.h
new file mode 100644
index 0000000..bae3ff6
--- /dev/null
+++ b/grblHAL_Teensy4/src/mcp3221.h
@@ -0,0 +1,31 @@
+/*
+  mcp3221.h - analog input from a MCP3221 I2C ADC
+
+  Driver code for IMXRT1062 processor (on Teensy 4.x board)
+
+  Part of grblHAL
+
+  Copyright (c) 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 <http://www.gnu.org/licenses/>.
+*/
+
+#pragma once
+
+#define MCP3221_ADDRESS 0b1001101
+
+uint16_t MCP3221_read (void);
+bool MCP3221_init (void);
+
+/*EOF*/
diff --git a/grblHAL_Teensy4/src/my_machine.h b/grblHAL_Teensy4/src/my_machine.h
index f46a475..fa5aa14 100644
--- a/grblHAL_Teensy4/src/my_machine.h
+++ b/grblHAL_Teensy4/src/my_machine.h
@@ -49,18 +49,20 @@ 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_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 BLUETOOTH_ENABLE   1 // Set to 1 for HC-05 module. Requires Bluetooth plugin.
+//#define BLUETOOTH_ENABLE    1 // Set to 1 for HC-05 module. Requires Bluetooth plugin.
 //#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 MCP3221_ENABLE      1 // Enable analog input via MCP3221 ADC.
 //#define PPI_ENABLE          1 // Laser PPI plugin. To be completed.
 //#define ODOMETER_ENABLE     1 // Odometer plugin.
 //#define OPENPNP_ENABLE      1 // OpenPNP plugin. To be completed.
+//#define FANS_ENABLE         1 // Enable fan control via M106/M107. Requires fans plugin.
 //#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 SPINDLE_SYNC_ENABLE 1 // Enable spindle sync support (G33, G76). !! NOTE: Alpha quality - enable only for test or verification.