From 20a8acb365b5041701f7e26633c3ae2f4a77dfb7 Mon Sep 17 00:00:00 2001
From: andig <cpuidle@gmx.de>
Date: Mon, 3 Jan 2022 19:06:55 +0100
Subject: [PATCH] Modbus: add bool16 type and supporting bitmask (#2196)

---
 util/modbus/functions.go | 47 +++++++++++++++++++++++++++++++++++++++-
 util/modbus/modbus.go    |  8 +++++++
 2 files changed, 54 insertions(+), 1 deletion(-)

diff --git a/util/modbus/functions.go b/util/modbus/functions.go
index 4b6e63062..58d54ae2a 100644
--- a/util/modbus/functions.go
+++ b/util/modbus/functions.go
@@ -1,6 +1,12 @@
 package modbus
 
-import "strings"
+import (
+	"encoding/binary"
+	"encoding/hex"
+	"fmt"
+	"strconv"
+	"strings"
+)
 
 // ReadingName formats MBMD reading names
 func ReadingName(val string) string {
@@ -9,3 +15,42 @@ func ReadingName(val string) string {
 	}
 	return val
 }
+
+// decodeMask converts a bit mask in decimal or hex format to uint64
+func decodeMask(mask string) (uint64, error) {
+	mask = strings.ToLower(mask)
+
+	if strings.HasPrefix(mask, "0x") {
+		if len(mask) < 3 {
+			return 0, fmt.Errorf("invalid mask: %s", mask)
+		}
+
+		b, err := hex.DecodeString(mask[2:])
+		if err != nil {
+			return 0, fmt.Errorf("invalid mask: %w", err)
+		}
+
+		var u uint64
+		for _, v := range b {
+			u = u<<8 | uint64(v)
+		}
+
+		return u, nil
+	}
+
+	return strconv.ParseUint(mask, 10, 64)
+}
+
+// decodeBool16 converts a masked uint16 to a bool
+func decodeBool16(mask uint64) func(b []byte) float64 {
+	return func(b []byte) float64 {
+		u := binary.BigEndian.Uint16(b)
+		if mask > 0 {
+			u = u & uint16(mask)
+		}
+		if u > 0 {
+			return 1
+		}
+		return 0
+	}
+}
diff --git a/util/modbus/modbus.go b/util/modbus/modbus.go
index 848305f1d..cd9f49c44 100644
--- a/util/modbus/modbus.go
+++ b/util/modbus/modbus.go
@@ -243,6 +243,7 @@ type Register struct {
 	Address uint16 // Length  uint16
 	Type    string
 	Decode  string
+	BitMask string
 }
 
 // RegisterOperation creates a read operation from a register definition
@@ -288,6 +289,13 @@ func RegisterOperation(r Register) (rs485.Operation, error) {
 		op.Transform = rs485.RTUInt32ToFloat64
 	case "int32s":
 		op.Transform = rs485.RTUInt32ToFloat64Swapped
+	case "bool16":
+		mask, err := decodeMask(r.BitMask)
+		if err != nil {
+			return op, err
+		}
+		op.Transform = decodeBool16(mask)
+		op.ReadLen = 1
 	default:
 		return rs485.Operation{}, fmt.Errorf("invalid register decoding: %s", r.Decode)
 	}