diff --git a/framework/unit.h b/framework/unit.h
index 791da70..0cb86fa 100644
--- a/framework/unit.h
+++ b/framework/unit.h
@@ -11,6 +11,11 @@
 #include "utils/payload_builder.h"
 #include "utils/payload_parser.h"
 
+#define CHECK_TYPE(_unit, _driver) do { \
+    if ((_unit->driver) != (_driver)) \
+        return E_BAD_UNIT_TYPE; \
+} while (0)
+
 extern char unit_tmp512[512]; // temporary static buffer - not expected to be accessed asynchronously
 // TODO add mutex?
 
diff --git a/units/digital_in/unit_din.c b/units/digital_in/unit_din.c
index 1cca3b7..642e546 100644
--- a/units/digital_in/unit_din.c
+++ b/units/digital_in/unit_din.c
@@ -187,6 +187,14 @@ static void DI_deInit(Unit *unit)
 
 // ------------------------------------------------------------------------
 
+error_t UU_DI_Read(Unit *unit, uint16_t *packed)
+{
+    CHECK_TYPE(unit, &UNIT_DIN);
+    struct priv *priv = unit->data;
+    *packed = port_pack((uint16_t) priv->port->IDR, priv->pins);
+    return E_SUCCESS;
+}
+
 enum PinCmd_ {
     CMD_READ = 0,
 };
@@ -194,14 +202,12 @@ enum PinCmd_ {
 /** Handle a request message */
 static error_t DI_handleRequest(Unit *unit, TF_ID frame_id, uint8_t command, PayloadParser *pp)
 {
-    (void)pp;
-
-    struct priv *priv = unit->data;
-
-    uint16_t packed = port_pack((uint16_t) priv->port->IDR, priv->pins);
+    uint16_t packed = 0;
 
     switch (command) {
         case CMD_READ:;
+            TRY(UU_DI_Read(unit, &packed));
+
             PayloadBuilder pb = pb_start((uint8_t*)unit_tmp512, 64, NULL);
             pb_u16(&pb, packed);
             com_respond_buf(frame_id, MSG_SUCCESS, (uint8_t *) unit_tmp512, pb_length(&pb));
diff --git a/units/digital_out/unit_dout.c b/units/digital_out/unit_dout.c
index 1724204..850de4f 100644
--- a/units/digital_out/unit_dout.c
+++ b/units/digital_out/unit_dout.c
@@ -179,6 +179,60 @@ static void DO_deInit(Unit *unit)
 
 // ------------------------------------------------------------------------
 
+error_t UU_DO_Write(Unit *unit, uint16_t packed)
+{
+    CHECK_TYPE(unit, &UNIT_DOUT);
+
+    struct priv *priv = unit->data;
+    uint16_t mask = priv->pins;
+    uint16_t spread = port_spread(packed, mask);
+
+    uint16_t set = spread;
+    uint16_t reset = ((~spread) & mask);
+    priv->port->BSRR = set | (reset << 16);
+    return E_SUCCESS;
+}
+
+error_t UU_DO_Set(Unit *unit, uint16_t packed)
+{
+    CHECK_TYPE(unit, &UNIT_DOUT);
+
+    struct priv *priv = unit->data;
+    uint16_t mask = priv->pins;
+    uint16_t spread = port_spread(packed, mask);
+
+    priv->port->BSRR = spread;
+    return E_SUCCESS;
+}
+
+error_t UU_DO_Clear(Unit *unit, uint16_t packed)
+{
+    CHECK_TYPE(unit, &UNIT_DOUT);
+
+    struct priv *priv = unit->data;
+    uint16_t mask = priv->pins;
+    uint16_t spread = port_spread(packed, mask);
+
+    priv->port->BSRR = (spread<<16);
+    return E_SUCCESS;
+}
+
+error_t UU_DO_Toggle(Unit *unit, uint16_t packed)
+{
+    CHECK_TYPE(unit, &UNIT_DOUT);
+
+    struct priv *priv = unit->data;
+    uint16_t mask = priv->pins;
+    uint16_t spread = port_spread(packed, mask);
+
+    uint16_t flipped = (uint16_t) (~priv->port->ODR) & mask;
+    uint16_t set = flipped & spread;
+    uint16_t reset = ((~flipped) & mask) & spread;
+    priv->port->BSRR = set | (reset<<16);
+    return E_SUCCESS;
+}
+
+
 enum PinCmd_ {
     CMD_WRITE = 0,
     CMD_SET = 1,
@@ -193,32 +247,18 @@ static error_t DO_handleRequest(Unit *unit, TF_ID frame_id, uint8_t command, Pay
 
     uint16_t packed = pp_u16(pp);
 
-    uint16_t mask = priv->pins;
-    uint16_t spread = port_spread(packed, mask);
-
-    uint16_t set, reset;
-
     switch (command) {
-        case CMD_WRITE:;
-            set = spread;
-            reset = (~spread) & mask;
-            priv->port->BSRR = set | (reset<<16);
-            return E_SUCCESS;
+        case CMD_WRITE:
+            return UU_DO_Write(unit, packed);
 
         case CMD_SET:
-            priv->port->BSRR = spread;
-            return E_SUCCESS;
+            return UU_DO_Set(unit, packed);
 
         case CMD_CLEAR:
-            priv->port->BSRR = (spread<<16);
-            return E_SUCCESS;
-
-        case CMD_TOGGLE:;
-            spread = (uint16_t) (~priv->port->ODR) & mask;
-            set = spread;
-            reset = (~spread) & mask;
-            priv->port->BSRR = set | (reset<<16);
-            return E_SUCCESS;
+            return UU_DO_Clear(unit, packed);
+
+        case CMD_TOGGLE:
+            return UU_DO_Toggle(unit, packed);
 
         default:
             return E_UNKNOWN_COMMAND;
diff --git a/units/i2c/unit_i2c.c b/units/i2c/unit_i2c.c
index 2dd4b0e..30d2d57 100644
--- a/units/i2c/unit_i2c.c
+++ b/units/i2c/unit_i2c.c
@@ -293,6 +293,8 @@ static error_t i2c_wait_until_flag(struct priv *priv, uint32_t flag, bool stop_s
 
 error_t UU_I2C_Write(Unit *unit, uint16_t addr, const uint8_t *bytes, uint32_t bcount)
 {
+    CHECK_TYPE(unit, &UNIT_I2C);
+
     struct priv *priv = unit->data;
 
     uint8_t addrsize = (uint8_t) (((addr & 0x8000) == 0) ? 7 : 10);
@@ -326,6 +328,8 @@ error_t UU_I2C_Write(Unit *unit, uint16_t addr, const uint8_t *bytes, uint32_t b
 
 error_t UU_I2C_Read(Unit *unit, uint16_t addr, uint8_t *dest, uint32_t bcount)
 {
+    CHECK_TYPE(unit, &UNIT_I2C);
+
     struct priv *priv = unit->data;
 
     uint8_t addrsize = (uint8_t) (((addr & 0x8000) == 0) ? 7 : 10);
@@ -370,6 +374,8 @@ error_t UU_I2C_ReadReg(Unit *unit, uint16_t addr, uint8_t regnum, uint8_t *dest,
 
 error_t UU_I2C_WriteReg(Unit *unit, uint16_t addr, uint8_t regnum, const uint8_t *bytes, uint32_t width)
 {
+    CHECK_TYPE(unit, &UNIT_I2C);
+
     // we have to insert the address first - needs a buffer (XXX realistically the buffer needs 1-4 bytes + addr)
     PayloadBuilder pb = pb_start((uint8_t*)unit_tmp512, 512, NULL);
     pb_u8(&pb, regnum);
diff --git a/units/neopixel/unit_neopixel.c b/units/neopixel/unit_neopixel.c
index 7c15754..ef46fa2 100644
--- a/units/neopixel/unit_neopixel.c
+++ b/units/neopixel/unit_neopixel.c
@@ -141,6 +141,8 @@ static void Npx_deInit(Unit *unit)
 /* Clear the strip */
 error_t UU_NEOPIXEL_Clear(Unit *unit)
 {
+    CHECK_TYPE(unit, &UNIT_NEOPIXEL);
+
     struct priv *priv = unit->data;
     ws2812_clear(priv->port, priv->ll_pin, priv->pixels);
     return E_SUCCESS;
@@ -149,6 +151,8 @@ error_t UU_NEOPIXEL_Clear(Unit *unit)
 /* Load packed */
 error_t UU_NEOPIXEL_Load(Unit *unit, const uint8_t *packed_rgb, uint32_t nbytes)
 {
+    CHECK_TYPE(unit, &UNIT_NEOPIXEL);
+
     struct priv *priv = unit->data;
     if (nbytes != 3*priv->pixels) return E_BAD_COUNT;
     ws2812_load_raw(priv->port, priv->ll_pin, packed_rgb, priv->pixels);
@@ -158,6 +162,8 @@ error_t UU_NEOPIXEL_Load(Unit *unit, const uint8_t *packed_rgb, uint32_t nbytes)
 /* Load U32, LE or BE */
 static error_t load_u32(Unit *unit, const uint8_t *bytes, uint32_t nbytes, bool bige)
 {
+    CHECK_TYPE(unit, &UNIT_NEOPIXEL);
+
     struct priv *priv = unit->data;
     if (nbytes != 4*priv->pixels) return E_BAD_COUNT;
     ws2812_load_sparse(priv->port, priv->ll_pin, bytes, priv->pixels, bige);
@@ -177,10 +183,13 @@ inline error_t UU_NEOPIXEL_LoadU32BE(Unit *unit, const uint8_t *bytes, uint32_t
 }
 
 /* Get the pixel count */
-inline uint16_t UU_NEOPIXEL_GetCount(Unit *unit)
+error_t UU_NEOPIXEL_GetCount(Unit *unit, uint16_t *count)
 {
+    CHECK_TYPE(unit, &UNIT_NEOPIXEL);
+
     struct priv *priv = unit->data;
-    return priv->pixels;
+    *count = priv->pixels;
+    return E_SUCCESS;
 }
 
 enum PinCmd_ {
@@ -218,8 +227,10 @@ static error_t Npx_handleRequest(Unit *unit, TF_ID frame_id, uint8_t command, Pa
             return UU_NEOPIXEL_LoadU32BE(unit, bytes, len);
 
         /** Get the Neopixel strip length */
-        case CMD_GET_LEN:
-            com_respond_u16(frame_id, UU_NEOPIXEL_GetCount(unit));
+        case CMD_GET_LEN:;
+            uint16_t count;
+            TRY(UU_NEOPIXEL_GetCount(unit, &count));
+            com_respond_u16(frame_id, count);
             return E_SUCCESS;
 
         default:
diff --git a/units/neopixel/unit_neopixel.h b/units/neopixel/unit_neopixel.h
index 24613ef..408abd4 100644
--- a/units/neopixel/unit_neopixel.h
+++ b/units/neopixel/unit_neopixel.h
@@ -49,4 +49,13 @@ error_t UU_NEOPIXEL_LoadU32LE(Unit *unit, const uint8_t *bytes, uint32_t nbytes)
  */
 error_t UU_NEOPIXEL_LoadU32BE(Unit *unit, const uint8_t *bytes, uint32_t nbytes);
 
+/**
+ * Get number of pixels on the strip
+ *
+ * @param unit
+ * @param count - destination for the count value
+ * @return success
+ */
+error_t UU_NEOPIXEL_GetCount(Unit *unit, uint16_t *count);
+
 #endif //U_NEOPIXEL_H
diff --git a/utils/error.h b/utils/error.h
index 0d1b403..2e9cc99 100644
--- a/utils/error.h
+++ b/utils/error.h
@@ -41,6 +41,7 @@
     X(NO_SUCH_UNIT, NULL) \
     X(OVERRUN, NULL) /* used in bulk transfer */ \
     X(PROTOCOL_BREACH, NULL) /* eating with the wrong spoon */  \
+    X(BAD_UNIT_TYPE, NULL) \
     \
     /* VFS user errors (those are meant to be shown to user) */ \
     X(ERROR_DURING_TRANSFER, "Error during transfer") \