Merge branch 'master' of github.com:webhat/modbustcp

.gitignore

@@ -15,3 +15,4 @@ modbustcp
 *.out
 modbus-demo-*.tar.bz2
 modbus-demo-*
+modbustcp

README.md

@@ -3,4 +3,19 @@ ModBus TCP Server
 
 This is a basic server based of the code by [tbrandon](https://github.com/tbrandon/mbserver)
 
+# Usage
 
+To use this demo you need to get the dependencies and compile the tool.
+
+```
+$ go get
+$ go build
+$ ./modbustcp
+```
+
+Once the server is running you can connect with a *Modbus over TCP* client.
+
+
+# Issues
+
+If you find an issue please [report it](https://github.com/webhat/modbustcp/issues).

modbusserver.go

@@ -21,9 +21,11 @@ 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.
 */
+
 package main
 
 import (
+	"fmt"
 	"log"
 	"time"
 
@@ -36,6 +38,7 @@ func main() {
 	serv.RegisterFunctionHandler(3, ReadRegisters)
 	serv.RegisterFunctionHandler(4, ReadRegisters)
 	serv.RegisterFunctionHandler(6, WriteRegisters)
+	serv.RegisterFunctionHandler(16, WriteHoldingRegisters)
 
 	err := serv.ListenTCP(":502")
 	if err != nil {
@@ -66,6 +69,7 @@ func ReadRegisters(s *mbserver.Server, frame mbserver.Framer) ([]byte, *mbserver
 		shift := uint(i) % 8
 		data[1+i/8] |= byte(1 << shift)
 	}
+	fmt.Println("READ: ", data)
 	return data, &mbserver.Success
 }
 
@@ -82,20 +86,51 @@ func WriteRegisters(s *mbserver.Server, frame mbserver.Framer) ([]byte, *mbserve
 	data[2] = byte(int(value / 256))
 	data[3] = byte(value % 256)
 
+	fmt.Println("WRITE: ", data)
+
+	return data, &mbserver.Success
+}
+
+func WriteHoldingRegisters(s *mbserver.Server, frame mbserver.Framer) ([]byte, *mbserver.Exception) {
+	register, numregs, _ := registerAddressAndNumber(frame)
+	value := frame.GetData()[5:]
+	// Check the request is within the allocated memory
+	if register > 65535 {
+		return []byte{}, &mbserver.IllegalDataAddress
+	}
+
+	if len(value)/2 != numregs {
+		return []byte{}, &mbserver.IllegalDataAddress
+	}
+
+	for i := 0; i < len(value); i += 2 {
+		b := value[i : i+2]
+		fmt.Println("Data ", register+(i/2), b)
+	}
+
+	data := make([]byte, 4)
+	data = frame.GetData()[0:4]
+
+	fmt.Println("WRITE: ", data)
+
 	return data, &mbserver.Success
 }
 
 func registerAddressAndNumber(frame mbserver.Framer) (register int, numRegs int, endRegister int) {
 	data := frame.GetData()
+	fmt.Println("DATA: ", data)
 	register = int(binary.BigEndian.Uint16(data[0:2]))
+	fmt.Println("REG: ", register)
 	numRegs = int(binary.BigEndian.Uint16(data[2:4]))
 	endRegister = register + numRegs
 	return register, numRegs, endRegister
 }
 
 func registerAddressAndValue(frame mbserver.Framer) (int, uint16) {
 	data := frame.GetData()
+	fmt.Println("DATA: ", data)
 	register := int(binary.BigEndian.Uint16(data[0:2]))
+	fmt.Println("REG: ", register)
 	value := binary.BigEndian.Uint16(data[2:4])
 	return register, value
 }