From 24d16bc1aa93daaffab1165e8bbf01158a3c2d69 Mon Sep 17 00:00:00 2001
From: FabrizioC <contif@hotmail.com>
Date: Fri, 21 Nov 2014 10:07:38 +0100
Subject: [PATCH 1/4] Added FC 22 Mask Write Register function

---
 Phpmodbus/ModbusMaster.php | 100 +++++++++++++++++++++++++++++++++++++
 1 file changed, 100 insertions(+)

diff --git a/Phpmodbus/ModbusMaster.php b/Phpmodbus/ModbusMaster.php
index e115d11..b9a35cb 100644
--- a/Phpmodbus/ModbusMaster.php
+++ b/Phpmodbus/ModbusMaster.php
@@ -1049,6 +1049,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
    *

From a2bde03574f4f88b06eadd533dccf832bb760c00 Mon Sep 17 00:00:00 2001
From: FabrizioC <contif@hotmail.com>
Date: Fri, 21 Nov 2014 10:08:44 +0100
Subject: [PATCH 2/4] Added FC 22 Mask Write Register function

---
 README.md | 1 +
 1 file changed, 1 insertion(+)

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

From cd52263b8effd15abdeb0c9ce12a3bfd0d07b690 Mon Sep 17 00:00:00 2001
From: FabrizioC <contif@hotmail.com>
Date: Fri, 21 Nov 2014 10:31:45 +0100
Subject: [PATCH 3/4] Added FC22 Mask Write Register example

---
 examples/example_fc22.php | 28 ++++++++++++++++++++++++++++
 1 file changed, 28 insertions(+)
 create mode 100644 examples/example_fc22.php

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;
+
+?>

From 697daedba386fca8f184e641d0f8965a11f44645 Mon Sep 17 00:00:00 2001
From: FabrizioC <contif@hotmail.com>
Date: Fri, 21 Nov 2014 10:38:17 +0100
Subject: [PATCH 4/4] Update ModbusMaster.php

---
 Phpmodbus/ModbusMaster.php | 1 +
 1 file changed, 1 insertion(+)

diff --git a/Phpmodbus/ModbusMaster.php b/Phpmodbus/ModbusMaster.php
index b9a35cb..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