diff --git a/Phpmodbus/ModbusMaster.php b/Phpmodbus/ModbusMaster.php
index e115d11..a03229f 100644
--- a/Phpmodbus/ModbusMaster.php
+++ b/Phpmodbus/ModbusMaster.php
@@ -32,6 +32,7 @@ require_once dirname(__FILE__) . '/PhpType.php';
  *   - FC  6: write single register
  *   - FC 15: write multiple coils
  *   - FC 16: write multiple registers
+ *   - FC 22: mask write register
  *   - FC 23: read write registers
  *   
  * @author Jan Krakora
@@ -1049,6 +1050,106 @@ class ModbusMaster {
     return true;
   }
 
+
+  /**
+   * maskWriteRegister
+   *
+   * Modbus function FC22(0x16) - Mask Write Register.
+   *
+   * This function alter single bit(s) at {@link $reference} position of
+   * memory of a Modbus device given by {@link $unitId}.
+   *
+   * Result = (Current Contents AND And_Mask) OR (Or_Mask AND (NOT And_Mask))
+   *
+   * @param int $unitId usually ID of Modbus device
+   * @param int $reference Reference in the device memory (e.g. in device WAGO 750-841, memory MW0 starts at address 12288)
+   * @param int $andMask
+   * @param int $orMask 
+   * @return bool Success flag
+   */
+  function maskWriteRegister($unitId, $reference, $andMask, $orMask){
+    $this->status .= "maskWriteRegister: START\n";
+    // connect
+    $this->connect();
+    // send FC22
+    $packet = $this->maskWriteRegisterPacketBuilder($unitId, $reference, $andMask, $orMask);
+    $this->status .= $this->printPacket($packet);
+    $this->send($packet);
+    // receive response
+    $rpacket = $this->rec();
+    $this->status .= $this->printPacket($rpacket);
+    // parse packet
+    $this->maskWriteRegisterParser($rpacket);
+    // disconnect
+    $this->disconnect();
+    $this->status .= "maskWriteRegister: DONE\n";
+    return true;
+  }
+
+
+  /**
+   * fc22
+   *
+   * Alias to {@link maskWriteRegister} method
+   *
+   * @param int $unitId
+   * @param int $reference
+   * @param int $andMask
+   * @param int $orMask
+   * @return bool
+   */
+  function fc22($unitId, $reference, $andMask, $orMask){
+    return $this->maskWriteRegister($unitId, $reference, $andMask, $orMask);
+  }
+
+
+  /**
+   * maskWriteRegisterPacketBuilder
+   *
+   * Packet builder FC22 - MASK WRITE register
+   *
+   * @param int $unitId
+   * @param int $reference
+   * @param int $andMask
+   * @param int $orMask
+   * @return string
+   */
+  private function maskWriteRegisterPacketBuilder($unitId, $reference, $andMask, $orMask){
+    $dataLen = 0;
+    // build data section
+    $buffer1 = "";
+    // build body
+    $buffer2 = "";
+    $buffer2 .= iecType::iecBYTE(22);             // FC 22 = 22(0x16)
+    $buffer2 .= iecType::iecINT($reference);      // refnumber = 12288
+    $buffer2 .= iecType::iecINT($andMask);        // AND mask
+    $buffer2 .= iecType::iecINT($orMask);     	  // OR mask
+    $dataLen += 7;
+    // build header
+    $buffer3 = '';
+    $buffer3 .= iecType::iecINT(rand(0,65000));   // transaction ID
+    $buffer3 .= iecType::iecINT(0);               // protocol ID
+    $buffer3 .= iecType::iecINT($dataLen + 1);    // lenght
+    $buffer3 .= iecType::iecBYTE($unitId);        //unit ID
+    // return packet string
+    return $buffer3. $buffer2. $buffer1;
+  }
+
+
+  /**
+   * maskWriteRegisterParser
+   *
+   * FC22 response parser
+   *
+   * @param string $packet
+   * @return bool
+   */
+  private function maskWriteRegisterParser($packet){
+    $this->responseCode($packet);
+    return true;
+  }
+
+
   /**
    * readWriteRegisters
    *
diff --git a/README.md b/README.md
index c189d18..422a9bc 100644
--- a/README.md
+++ b/README.md
@@ -16,6 +16,7 @@ Features
     * FC6 - Write single register
     * FC15 - Write multiple coils
     * FC16 - Write multiple registers
+    * FC22 - Mask Write register
     * FC23 - Read/Write multiple registers
 
 Example
diff --git a/examples/example_fc22.php b/examples/example_fc22.php
new file mode 100644
index 0000000..24e235f
--- /dev/null
+++ b/examples/example_fc22.php
@@ -0,0 +1,28 @@
+<?php
+
+require_once dirname(__FILE__) . '/../Phpmodbus/ModbusMaster.php';
+
+// Create Modbus object
+$modbus = new ModbusMaster("192.192.15.51", "UDP");
+
+// Data to be writen
+$bitValue = true;
+$bitNumber = 2;
+$andMask =  0xFFFF ^ pow(2, $bitNumber) ;
+$orMask =  0x0000 ^ (pow(2, $bitNumber) * $bitValue ) ;
+
+try {
+    // FC22
+    $modbus->maskWriteRegister(0, 12288, $andMask, $orMask);
+}
+catch (Exception $e) {
+    // Print error information if any
+    echo $modbus;
+    echo $e;
+    exit;
+}
+
+// Print status information
+echo $modbus;
+
+?>