diff --git a/kn/bridge.go b/kn/bridge.go
index 0032c93..79f47f8 100644
--- a/kn/bridge.go
+++ b/kn/bridge.go
@@ -3,6 +3,7 @@ package kn
 import (
 	"encoding/json"
 	"fmt"
+	"koolnova2mqtt/modbus"
 	"koolnova2mqtt/watcher"
 	"log"
 	"strconv"
@@ -12,14 +13,13 @@ type Publish func(topic string, qos byte, retained bool, payload string)
 type Subscribe func(topic string, callback func(message string)) error
 
 type Config struct {
-	ModuleName    string
-	SlaveID       byte
-	Publish       Publish
-	Subscribe     Subscribe
-	TopicPrefix   string
-	HassPrefix    string
-	ReadRegister  watcher.ReadRegister
-	WriteRegister watcher.WriteRegister
+	ModuleName  string
+	SlaveID     byte
+	Publish     Publish
+	Subscribe   Subscribe
+	TopicPrefix string
+	HassPrefix  string
+	Modbus      modbus.Modbus
 }
 
 type Bridge struct {
@@ -54,8 +54,7 @@ func NewBridge(config *Config) *Bridge {
 		Quantity:     TOTAL_ZONE_REGISTERS,
 		RegisterSize: 2,
 		SlaveID:      config.SlaveID,
-		Read:         config.ReadRegister,
-		Write:        config.WriteRegister,
+		Modbus:       config.Modbus,
 	})
 
 	sysw := watcher.New(&watcher.Config{
@@ -63,8 +62,7 @@ func NewBridge(config *Config) *Bridge {
 		Quantity:     TOTAL_SYS_REGISTERS,
 		RegisterSize: 2,
 		SlaveID:      config.SlaveID,
-		Read:         config.ReadRegister,
-		Write:        config.WriteRegister,
+		Modbus:       config.Modbus,
 	})
 
 	b := &Bridge{
@@ -146,8 +144,18 @@ func (b *Bridge) Start() error {
 		fanModeTopic := b.getZoneTopic(zone.ZoneNumber, "fanMode")
 		fanModeSetTopic := fanModeTopic + "/set"
 		hvacModeTopic := b.getZoneTopic(zone.ZoneNumber, "hvacMode")
-		hvacModeTopicSet := hvacModeTopic + "/set"
+		hvacModeSetTopic := hvacModeTopic + "/set"
 
+		zone.OnEnabledChange = func() {
+			hvacModeTopic := b.getZoneTopic(zone.ZoneNumber, "hvacMode")
+			var mode string
+			if zone.IsOn() {
+				mode = getHVACMode()
+			} else {
+				mode = HVAC_MODE_OFF
+			}
+			b.Publish(hvacModeTopic, 0, true, mode)
+		}
 		zone.OnCurrentTempChange = func(currentTemp float32) {
 			b.Publish(currentTempTopic, 0, true, fmt.Sprintf("%g", currentTemp))
 		}
@@ -190,6 +198,42 @@ func (b *Bridge) Start() error {
 			return err
 		}
 
+		err = b.Subscribe(hvacModeSetTopic, func(message string) {
+			if message == HVAC_MODE_OFF {
+				err := zone.SetOn(false)
+				if err != nil {
+					log.Printf("Cannot set zone %d to off", zone.ZoneNumber)
+				}
+				return
+			}
+			err := zone.SetOn(true)
+			if err != nil {
+				log.Printf("Cannot set zone %d to on", zone.ZoneNumber)
+				return
+			}
+			knMode := sys.GetSystemKNMode()
+			knMode = ApplyHvacMode(knMode, message)
+			err = sys.SetSystemKNMode(knMode)
+			if err != nil {
+				log.Printf("Cannot set knmode mode to %x in zone %d", knMode, zone.ZoneNumber)
+			}
+		})
+		if err != nil {
+			return err
+		}
+
+		err = b.Subscribe(holdModeSetTopic, func(message string) {
+			knMode := sys.GetSystemKNMode()
+			knMode = ApplyHoldMode(knMode, message)
+			err := sys.SetSystemKNMode(knMode)
+			if err != nil {
+				log.Printf("Cannot set knmode mode to %x in zone %d", knMode, zone.ZoneNumber)
+			}
+		})
+		if err != nil {
+			return err
+		}
+
 		name := fmt.Sprintf("%s_zone%d", b.ModuleName, zone.ZoneNumber)
 		config := map[string]interface{}{
 			"name":                      name,
@@ -204,7 +248,7 @@ func (b *Bridge) Start() error {
 			"max_temp":                  35,
 			"modes":                     []string{HVAC_MODE_COOL, HVAC_MODE_HEAT, HVAC_MODE_OFF},
 			"mode_state_topic":          hvacModeTopic,
-			"mode_command_topic":        hvacModeTopicSet,
+			"mode_command_topic":        hvacModeSetTopic,
 			"fan_modes":                 []string{"auto", "low", "medium", "high"},
 			"fan_mode_state_topic":      fanModeTopic,
 			"fan_mode_command_topic":    fanModeSetTopic,
diff --git a/kn/constants.go b/kn/constants.go
index 3ec7902..28006dd 100644
--- a/kn/constants.go
+++ b/kn/constants.go
@@ -99,3 +99,47 @@ func KnMode2Str(hm KnMode) string {
 	}
 	return st.(string)
 }
+
+func ApplyHvacMode(knMode KnMode, hvacMode string) KnMode {
+	switch knMode {
+	case MODE_AIR_COOLING:
+		if hvacMode == HVAC_MODE_HEAT {
+			return MODE_AIR_HEATING
+		}
+	case MODE_AIR_HEATING:
+		if hvacMode == HVAC_MODE_COOL {
+			return MODE_AIR_COOLING
+		}
+	case MODE_UNDERFLOOR_AIR_COOLING:
+		if hvacMode == HVAC_MODE_HEAT {
+			return MODE_UNDERFLOOR_AIR_HEATING
+		}
+	case MODE_UNDERFLOOR_AIR_HEATING, MODE_UNDERFLOOR_HEATING:
+		if hvacMode == HVAC_MODE_COOL {
+			return MODE_UNDERFLOOR_AIR_COOLING
+		}
+	}
+	return knMode
+}
+
+func ApplyHoldMode(knMode KnMode, holdMode string) KnMode {
+	cool := knMode == MODE_AIR_COOLING || knMode == MODE_UNDERFLOOR_AIR_COOLING
+	switch holdMode {
+	case HOLD_MODE_FAN_ONLY:
+		if cool {
+			return MODE_AIR_COOLING
+		}
+		return MODE_AIR_HEATING
+	case HOLD_MODE_UNDERFLOOR_ONLY:
+		if cool {
+			return MODE_UNDERFLOOR_AIR_COOLING
+		}
+		return MODE_UNDERFLOOR_HEATING
+	case HOLD_MODE_UNDERFLOOR_AND_FAN:
+		if cool {
+			return MODE_UNDERFLOOR_AIR_COOLING
+		}
+		return MODE_UNDERFLOOR_AIR_HEATING
+	}
+	return knMode
+}
diff --git a/kn/sys.go b/kn/sys.go
index d83994a..e246317 100644
--- a/kn/sys.go
+++ b/kn/sys.go
@@ -68,6 +68,10 @@ func (s *Sys) ReadRegister(n int) int {
 	return int(r[1])
 }
 
+func (s *Sys) WriteRegister(n int, value uint16) error {
+	return s.Watcher.WriteRegister(uint16(n), value)
+}
+
 func (s *Sys) GetAirflow(ac ACMachine) int {
 	r := s.ReadRegister(REG_AIRFLOW + int(ac) - 1)
 	return r
@@ -124,3 +128,7 @@ func (s *Sys) GetSystemKNMode() KnMode {
 	r := s.ReadRegister(REG_SYS_KN_MODE)
 	return KnMode(r)
 }
+
+func (s *Sys) SetSystemKNMode(knMode KnMode) error {
+	return s.WriteRegister(REG_SYS_KN_MODE, uint16(knMode))
+}
diff --git a/kn/zone.go b/kn/zone.go
index b08ad95..749f993 100644
--- a/kn/zone.go
+++ b/kn/zone.go
@@ -17,6 +17,7 @@ type ZoneConfig struct {
 
 type Zone struct {
 	ZoneConfig
+	OnEnabledChange     func()
 	OnCurrentTempChange func(newTemp float32)
 	OnTargetTempChange  func(newTemp float32)
 	OnFanModeChange     func(newMode FanMode)
@@ -27,6 +28,11 @@ func NewZone(config *ZoneConfig) *Zone {
 	z := &Zone{
 		ZoneConfig: *config,
 	}
+	z.RegisterCallback(REG_ENABLED, func() {
+		if z.OnEnabledChange != nil {
+			z.OnEnabledChange()
+		}
+	})
 	z.RegisterCallback(REG_CURRENT_TEMP, func() {
 		if z.OnCurrentTempChange == nil {
 			return
@@ -45,7 +51,7 @@ func NewZone(config *ZoneConfig) *Zone {
 	})
 	z.RegisterCallback(REG_MODE, func() {
 		fanMode := z.GetFanMode()
-		hvacMode := z.GetHvacMode()
+		hvacMode := z.GetKnMode()
 		if z.OnFanModeChange != nil {
 			z.OnFanModeChange(fanMode)
 		}
@@ -76,6 +82,16 @@ func (z *Zone) IsOn() bool {
 	return r1&0x1 != 0
 }
 
+func (z *Zone) SetOn(on bool) error {
+	var r1 uint16
+	if on {
+		r1 = 0x3
+	} else {
+		r1 = 0x2
+	}
+	return z.WriteRegister(REG_ENABLED, r1)
+}
+
 func (z *Zone) IsPresent() bool {
 	r1 := z.ReadRegister(REG_ENABLED)
 	return r1&0x2 != 0
@@ -106,7 +122,7 @@ func (z *Zone) SetFanMode(fanMode FanMode) error {
 	return z.WriteRegister(REG_MODE, r2|fm)
 }
 
-func (z *Zone) GetHvacMode() KnMode {
+func (z *Zone) GetKnMode() KnMode {
 	r2 := z.ReadRegister(REG_MODE)
 	return (KnMode)(r2 & 0x000F)
 }
diff --git a/main.go b/main.go
index 5fef39b..df4a954 100644
--- a/main.go
+++ b/main.go
@@ -6,41 +6,19 @@ import (
 	"flag"
 	"fmt"
 	"koolnova2mqtt/kn"
-	"koolnova2mqtt/watcher"
+	"koolnova2mqtt/modbus"
 	"log"
 	"os"
 	"os/signal"
 	"regexp"
 	"strconv"
 	"strings"
-	"sync"
 	"syscall"
 	"time"
 
 	MQTT "github.com/eclipse/paho.mqtt.golang"
-	"github.com/goburrow/modbus"
 )
 
-func buildReader(handler *modbus.RTUClientHandler, client modbus.Client, lock *sync.RWMutex) watcher.ReadRegister {
-	return func(slaveID byte, address uint16, quantity uint16) (results []byte, err error) {
-		lock.Lock()
-		defer lock.Unlock()
-		handler.SlaveId = slaveID
-		results, err = client.ReadHoldingRegisters(address-1, quantity)
-		return results, err
-	}
-}
-
-func buildWriter(handler *modbus.RTUClientHandler, client modbus.Client, lock *sync.RWMutex) watcher.WriteRegister {
-	return func(slaveID byte, address uint16, value uint16) (results []byte, err error) {
-		lock.Lock()
-		defer lock.Unlock()
-		handler.SlaveId = slaveID
-		results, err = client.WriteSingleRegister(address-1, value)
-		return results, err
-	}
-}
-
 func generateNodeName(slaveID string, port string) string {
 	reg, err := regexp.Compile("[^a-zA-Z0-9]+")
 	if err != nil {
@@ -79,24 +57,18 @@ func main() {
 
 	flag.Parse()
 
-	handler := modbus.NewRTUClientHandler(*modbusPort)
-	handler.BaudRate = *modbusPortBaudRate
-	handler.DataBits = *modbusDataBits
-	handler.Parity = *modbusPortParity
-	handler.StopBits = *modbusStopBits
-	handler.Timeout = 5 * time.Second
-
-	err := handler.Connect()
+	mb, err := modbus.New(&modbus.Config{
+		Port:     *modbusPort,
+		BaudRate: *modbusPortBaudRate,
+		DataBits: *modbusDataBits,
+		Parity:   *modbusPortParity,
+		StopBits: *modbusStopBits,
+		Timeout:  5 * time.Second,
+	})
 	if err != nil {
-		log.Fatalf("Error connecting modbus: %s", err)
+		log.Fatalf("Error initializing modbus: %s", err)
 	}
-	defer handler.Close()
-
-	modbusClient := modbus.NewClient(handler)
-
-	lock := &sync.RWMutex{}
-	registerReader := buildReader(handler, modbusClient, lock)
-	registerWriter := buildWriter(handler, modbusClient, lock)
+	defer mb.Close()
 
 	var mqttClient MQTT.Client
 	publish := func(topic string, qos byte, retained bool, payload string) {
@@ -147,14 +119,13 @@ func main() {
 			log.Fatalf("Error parsing slaveID list")
 		}
 		bridge := kn.NewBridge(&kn.Config{
-			ModuleName:    slaveName,
-			SlaveID:       byte(slaveID),
-			Publish:       publish,
-			Subscribe:     subscribe,
-			TopicPrefix:   *prefix,
-			HassPrefix:    *hassPrefix,
-			ReadRegister:  registerReader,
-			WriteRegister: registerWriter,
+			ModuleName:  slaveName,
+			SlaveID:     byte(slaveID),
+			Publish:     publish,
+			Subscribe:   subscribe,
+			TopicPrefix: *prefix,
+			HassPrefix:  *hassPrefix,
+			Modbus:      mb,
 		})
 		bridges = append(bridges, bridge)
 	}
diff --git a/watcher/watcher.go b/watcher/watcher.go
index 21b625b..528e9a4 100644
--- a/watcher/watcher.go
+++ b/watcher/watcher.go
@@ -3,18 +3,15 @@ package watcher
 import (
 	"bytes"
 	"errors"
+	"koolnova2mqtt/modbus"
 	"sync"
 )
 
-type ReadRegister func(slaveID byte, address uint16, quantity uint16) (results []byte, err error)
-type WriteRegister func(slaveID byte, address uint16, value uint16) (results []byte, err error)
-
 type Config struct {
 	Address      uint16
 	Quantity     uint16
 	SlaveID      byte
-	Read         ReadRegister
-	Write        WriteRegister
+	Modbus       modbus.Modbus
 	RegisterSize int
 }
 
@@ -43,7 +40,7 @@ func (w *Watcher) RegisterCallback(address uint16, callback func(address uint16)
 
 func (w *Watcher) Poll() error {
 	w.lock.Lock()
-	newState, err := w.Read(w.SlaveID, w.Address, w.Quantity)
+	newState, err := w.Modbus.ReadRegister(w.SlaveID, w.Address, w.Quantity)
 	if err != nil {
 		w.lock.Unlock()
 		return err
@@ -100,7 +97,7 @@ func (w *Watcher) ReadRegister(address uint16) (value []byte) {
 
 func (w *Watcher) WriteRegister(address uint16, value uint16) error {
 	w.lock.Lock()
-	results, err := w.Write(w.SlaveID, address, value)
+	results, err := w.Modbus.WriteRegister(w.SlaveID, address, value)
 	if err != nil {
 		w.lock.Unlock()
 		return err