new hid protocol with crc

12 files changed, 394 insertions, 162 deletions

diff --git a/configs/kvmd/platforms/kvmd.v1-hdmi.yaml b/configs/kvmd/platforms/kvmd.v1-hdmi.yaml
index 63f146a3..c181ceb5 100644
--- a/configs/kvmd/platforms/kvmd.v1-hdmi.yaml
+++ b/configs/kvmd/platforms/kvmd.v1-hdmi.yaml
@@ -19,9 +19,16 @@ kvmd:
         pinout:
             reset: 4
 
+        reset_delay: 0.1
+
         device: "/dev/kvmd-hid"
         speed: 115200
-        reset_delay: 0.1
+        read_timeout: 2.0
+        read_retries: 10
+        common_retries: 100
+        retries_delay: 0.1
+
+        state_poll: 1.0
 
     atx:
         pinout:
diff --git a/configs/kvmd/platforms/kvmd.v1-vga.yaml b/configs/kvmd/platforms/kvmd.v1-vga.yaml
index 9e401733..5fc4a44c 100644
--- a/configs/kvmd/platforms/kvmd.v1-vga.yaml
+++ b/configs/kvmd/platforms/kvmd.v1-vga.yaml
@@ -19,9 +19,16 @@ kvmd:
         pinout:
             reset: 4
 
+        reset_delay: 0.1
+
         device: "/dev/kvmd-hid"
         speed: 115200
-        reset_delay: 0.1
+        read_timeout: 2.0
+        read_retries: 10
+        common_retries: 100
+        retries_delay: 0.1
+
+        state_poll: 1.0
 
     atx:
         pinout:
diff --git a/hid/platformio.ini b/hid/platformio.ini
index 13036b9d..f70cd66c 100644
--- a/hid/platformio.ini
+++ b/hid/platformio.ini
@@ -17,3 +17,4 @@ monitor_speed = 115200
 
 lib_deps =
 	[email protected]
+	[email protected]
diff --git a/hid/src/main.cpp b/hid/src/main.cpp
index 93a90b8b..a7867981 100644
--- a/hid/src/main.cpp
+++ b/hid/src/main.cpp
@@ -1,5 +1,6 @@
 #include <Arduino.h>
 #include <HID-Project.h>
+#include <TimerOne.h>
 
 #include "inline.h"
 #include "keymap.h"
@@ -7,35 +8,42 @@
 
 #define CMD_SERIAL			Serial1
 #define CMD_SERIAL_SPEED	115200
-
-#define CMD_MOUSE_LEFT			0b10000000
-#define CMD_MOUSE_LEFT_STATE	0b00001000
-#define CMD_MOUSE_RIGHT			0b01000000
-#define CMD_MOUSE_RIGHT_STATE	0b00000100
-
-#define REPORT_INTERVAL 100
+#define CMD_RECV_TIMEOUT	100000
+
+#define PROTO_MAGIC						0x33
+#define PROTO_CRC_POLINOM				0xA001
+// -----------------------------------------
+#define PROTO_RESP_OK					0x20
+#define PROTO_RESP_NONE					0x24
+#define PROTO_RESP_CRC_ERROR			0x40
+#define PROTO_RESP_INVALID_ERROR		0x45
+#define PROTO_RESP_TIMEOUT_ERROR		0x48
+// -----------------------------------------
+#define PROTO_CMD_PING					0x01
+#define PROTO_CMD_REPEAT				0x02
+#define PROTO_CMD_RESET_HID				0x10
+#define PROTO_CMD_KEY_EVENT				0x11
+#define PROTO_CMD_MOUSE_MOVE_EVENT		0x12
+#define PROTO_CMD_MOUSE_BUTTON_EVENT	0x13
+#define PROTO_CMD_MOUSE_WHEEL_EVENT		0x14
+// -----------------------------------------
+#define PROTO_CMD_MOUSE_BUTTON_LEFT_SELECT	0b10000000
+#define PROTO_CMD_MOUSE_BUTTON_LEFT_STATE	0b00001000
+#define PROTO_CMD_MOUSE_BUTTON_RIGHT_SELECT	0b01000000
+#define PROTO_CMD_MOUSE_BUTTON_RIGHT_STATE	0b00000100
 
 
 // -----------------------------------------------------------------------------
-INLINE void readNoop() {
-	for (int count = 0; count < 4; ++count) {
-		CMD_SERIAL.read();
-	}
-}
-
-INLINE void cmdResetHid() { // 0 bytes
-	readNoop();
+INLINE void cmdResetHid(const uint8_t *buffer) { // 0 bytes
 	BootKeyboard.releaseAll();
 	SingleAbsoluteMouse.releaseAll();
 }
 
-INLINE void cmdKeyEvent() { // 2 bytes
-	KeyboardKeycode code = keymap((uint8_t)CMD_SERIAL.read());
-	uint8_t state = CMD_SERIAL.read();
-	CMD_SERIAL.read(); // unused
-	CMD_SERIAL.read(); // unused
+INLINE void cmdKeyEvent(const uint8_t *buffer) { // 2 bytes
+	KeyboardKeycode code = keymap(buffer[0]);
+
 	if (code != KEY_ERROR_UNDEFINED) {
-		if (state) {
+		if (buffer[1]) {
 			BootKeyboard.press(code);
 		} else {
 			BootKeyboard.release(code);
@@ -43,28 +51,29 @@ INLINE void cmdKeyEvent() { // 2 bytes
 	}
 }
 
-INLINE void cmdMouseMoveEvent() { // 4 bytes
-	int x = (int)CMD_SERIAL.read() << 8;
-	x |= (int)CMD_SERIAL.read();
-	int y = (int)CMD_SERIAL.read() << 8;
-	y |= (int)CMD_SERIAL.read();
+INLINE void cmdMouseMoveEvent(const uint8_t *buffer) { // 4 bytes
+	int x = (int)buffer[0] << 8;
+	x |= (int)buffer[1];
+
+	int y = (int)buffer[2] << 8;
+	y |= (int)buffer[3];
+
 	SingleAbsoluteMouse.moveTo(x, y);
 }
 
-INLINE void cmdMouseButtonEvent() { // 1 byte
-	uint8_t state = CMD_SERIAL.read();
-	CMD_SERIAL.read(); // unused
-	CMD_SERIAL.read(); // unused
-	CMD_SERIAL.read(); // unused
-	if (state & CMD_MOUSE_LEFT) {
-		if (state & CMD_MOUSE_LEFT_STATE) {
+INLINE void cmdMouseButtonEvent(const uint8_t *buffer) { // 1 byte
+	uint8_t state = buffer[0];
+
+	if (state & PROTO_CMD_MOUSE_BUTTON_LEFT_SELECT) {
+		if (state & PROTO_CMD_MOUSE_BUTTON_LEFT_STATE) {
 			SingleAbsoluteMouse.press(MOUSE_LEFT);
 		} else {
 			SingleAbsoluteMouse.release(MOUSE_LEFT);
 		}
 	}
-	if (state & CMD_MOUSE_RIGHT) {
-		if (state & CMD_MOUSE_RIGHT_STATE) {
+
+	if (state & PROTO_CMD_MOUSE_BUTTON_RIGHT_SELECT) {
+		if (state & PROTO_CMD_MOUSE_BUTTON_RIGHT_STATE) {
 			SingleAbsoluteMouse.press(MOUSE_RIGHT);
 		} else {
 			SingleAbsoluteMouse.release(MOUSE_RIGHT);
@@ -72,45 +81,113 @@ INLINE void cmdMouseButtonEvent() { // 1 byte
 	}
 }
 
-INLINE void cmdMouseWheelEvent() { // 2 bytes
-	CMD_SERIAL.read(); // delta_x is not supported by hid-project now
-	signed char delta_y = CMD_SERIAL.read();
-	CMD_SERIAL.read(); // unused
-	CMD_SERIAL.read(); // unused
+INLINE void cmdMouseWheelEvent(const uint8_t *buffer) { // 2 bytes
+	// delta_x is not supported by hid-project now
+	signed char delta_y = buffer[1];
+
 	SingleAbsoluteMouse.move(0, 0, delta_y);
 }
 
 
 // -----------------------------------------------------------------------------
+INLINE uint16_t makeCrc16(const uint8_t *buffer, const unsigned length) {
+	uint16_t crc = 0xFFFF;
+
+	for (unsigned byte_count = 0; byte_count < length; ++byte_count) {
+		crc = crc ^ buffer[byte_count];
+		for (unsigned bit_count = 0; bit_count < 8; ++bit_count) {
+			if ((crc & 0x0001) == 0) {
+				crc = crc >> 1;
+			} else {
+				crc = crc >> 1;
+				crc = crc ^ PROTO_CRC_POLINOM;
+			}
+		}
+	}
+	return crc;
+}
+
+
+// -----------------------------------------------------------------------------
+volatile bool cmd_recv_timed_out = false;
+
+INLINE void recvTimerStop(bool flag) {
+	Timer1.stop();
+	cmd_recv_timed_out = flag;
+}
+
+INLINE void resetCmdRecvTimeout() {
+	recvTimerStop(false);
+	Timer1.initialize(CMD_RECV_TIMEOUT);
+}
+
+INLINE void sendCmdResponse(uint8_t code=0) {
+	static uint8_t prev_code = PROTO_RESP_NONE;
+	if (code == 0) {
+		code = prev_code; // Repeat the last code
+	} else {
+		prev_code = code;
+	}
+
+	uint8_t buffer[4];
+	buffer[0] = PROTO_MAGIC;
+	buffer[1] = code;
+	uint16_t crc = makeCrc16(buffer, 2);
+	buffer[2] = (uint8_t)(crc >> 8);
+	buffer[3] = (uint8_t)(crc & 0xFF);
+
+	recvTimerStop(false);
+	CMD_SERIAL.write(buffer, 4);
+}
+
+void intRecvTimedOut() {
+	recvTimerStop(true);
+}
+
 void setup() {
-	CMD_SERIAL.begin(CMD_SERIAL_SPEED);
 	BootKeyboard.begin();
 	SingleAbsoluteMouse.begin();
+
+	Timer1.attachInterrupt(intRecvTimedOut);
+	CMD_SERIAL.begin(CMD_SERIAL_SPEED);
 }
 
 void loop() {
-	static unsigned long last_report = 0;
-	bool cmd_processed = false;
-
-	if (CMD_SERIAL.available() >= 5) {
-		switch ((uint8_t)CMD_SERIAL.read()) {
-			case 0: cmdResetHid(); break;
-			case 1: cmdKeyEvent(); break;
-			case 2: cmdMouseMoveEvent(); break;
-			case 3: cmdMouseButtonEvent(); break;
-			case 4: cmdMouseWheelEvent(); break;
-			default: readNoop(); break;
+	uint8_t buffer[8];
+	unsigned index = 0;
+
+	while (true) {
+		if (CMD_SERIAL.available() > 0) {
+			buffer[index] = (uint8_t)CMD_SERIAL.read();
+			if (index == 7) {
+				uint16_t crc = (uint16_t)buffer[6] << 8;
+				crc |= (uint16_t)buffer[7];
+
+				if (makeCrc16(buffer, 6) == crc) {
+#	define HANDLE(_handler) { _handler(buffer + 2); sendCmdResponse(PROTO_RESP_OK); break; }
+					switch (buffer[1]) {
+						case PROTO_CMD_RESET_HID:			HANDLE(cmdResetHid);
+						case PROTO_CMD_KEY_EVENT:			HANDLE(cmdKeyEvent);
+						case PROTO_CMD_MOUSE_MOVE_EVENT:	HANDLE(cmdMouseMoveEvent);
+						case PROTO_CMD_MOUSE_BUTTON_EVENT:	HANDLE(cmdMouseButtonEvent);
+						case PROTO_CMD_MOUSE_WHEEL_EVENT:	HANDLE(cmdMouseWheelEvent);
+
+						case PROTO_CMD_PING:	sendCmdResponse(PROTO_RESP_OK); break;
+						case PROTO_CMD_REPEAT:	sendCmdResponse(); break;
+						default:				sendCmdResponse(PROTO_RESP_INVALID_ERROR); break;
+					}
+#	undef HANDLE
+				} else {
+					sendCmdResponse(PROTO_RESP_CRC_ERROR);
+				}
+				index = 0;
+			} else {
+				resetCmdRecvTimeout();
+				index += 1;
+			}
+		} else if (index > 0 && cmd_recv_timed_out) {
+			sendCmdResponse(PROTO_RESP_TIMEOUT_ERROR);
+			index = 0;
 		}
-		cmd_processed = true;
-	}
-
-	unsigned long now = millis();
-	if (
-		cmd_processed
-		|| (now >= last_report && now - last_report >= REPORT_INTERVAL)
-		|| (now < last_report && ((unsigned long) -1) - last_report + now >= REPORT_INTERVAL)
-	) {
-		CMD_SERIAL.write(0);
-		last_report = now;
 	}
 }
diff --git a/kvmd/apps/kvmd/__init__.py b/kvmd/apps/kvmd/__init__.py
index e8b74abc..e9daea32 100644
--- a/kvmd/apps/kvmd/__init__.py
+++ b/kvmd/apps/kvmd/__init__.py
@@ -35,9 +35,18 @@ def main() -> None:
 
         hid = Hid(
             reset=int(config["hid"]["pinout"]["reset"]),
+
+            reset_delay=float(config["hid"]["reset_delay"]),
+
             device_path=str(config["hid"]["device"]),
             speed=int(config["hid"]["speed"]),
-            reset_delay=float(config["hid"]["reset_delay"]),
+            read_timeout=float(config["hid"]["read_timeout"]),
+            read_retries=int(config["hid"]["read_retries"]),
+            common_retries=int(config["hid"]["common_retries"]),
+            retries_delay=float(config["hid"]["retries_delay"]),
+            noop=bool(config["hid"].get("noop", False)),
+
+            state_poll=float(config["hid"]["state_poll"]),
         )
 
         atx = Atx(
diff --git a/kvmd/apps/kvmd/hid.py b/kvmd/apps/kvmd/hid.py
index 26cfa898..3f329c64 100644
--- a/kvmd/apps/kvmd/hid.py
+++ b/kvmd/apps/kvmd/hid.py
@@ -11,6 +11,7 @@ import time
 from typing import Dict
 from typing import Set
 from typing import NamedTuple
+from typing import AsyncGenerator
 
 import yaml
 import serial
@@ -33,41 +34,92 @@ class _KeyEvent(NamedTuple):
     key: str
     state: bool
 
+    @staticmethod
+    def is_valid(key: str) -> bool:
+        return (key in _KEYMAP)
+
+    def make_command(self) -> bytes:
+        code = _KEYMAP[self.key]
+        key_bytes = bytes([code])
+        assert len(key_bytes) == 1, (self, key_bytes, code)
+        state_bytes = (b"\x01" if self.state else b"\x00")
+        return b"\x11" + key_bytes + state_bytes + b"\x00\x00"
+
 
 class _MouseMoveEvent(NamedTuple):
     to_x: int
     to_y: int
 
+    def make_command(self) -> bytes:
+        to_x = min(max(-32768, self.to_x), 32767)
+        to_y = min(max(-32768, self.to_y), 32767)
+        return b"\x12" + struct.pack(">hh", to_x, to_y)
+
 
 class _MouseButtonEvent(NamedTuple):
     button: str
     state: bool
 
+    @staticmethod
+    def is_valid(button: str) -> bool:
+        return (button in ["left", "right"])
+
+    def make_command(self) -> bytes:
+        code = 0
+        if self.button == "left":
+            code = (0b10000000 | (0b00001000 if self.state else 0))
+        elif self.button == "right":
+            code = (0b01000000 | (0b00000100 if self.state else 0))
+        assert code, self
+        return b"\x13" + bytes([code]) + b"\x00\x00\x00"
+
 
 class _MouseWheelEvent(NamedTuple):
     delta_y: int
 
+    def make_command(self) -> bytes:
+        delta_y = min(max(-128, self.delta_y), 127)
+        return b"\x14\x00" + struct.pack(">b", delta_y) + b"\x00\x00"
+
 
 class Hid(multiprocessing.Process):  # pylint: disable=too-many-instance-attributes
-    def __init__(
+    def __init__(  # pylint: disable=too-many-arguments
         self,
         reset: int,
+        reset_delay: float,
+
         device_path: str,
         speed: int,
-        reset_delay: float,
+        read_timeout: float,
+        read_retries: int,
+        common_retries: int,
+        retries_delay: float,
+        noop: bool,
+
+        state_poll: float,
     ) -> None:
 
         super().__init__(daemon=True)
 
         self.__reset = gpio.set_output(reset)
+        self.__reset_delay = reset_delay
+
         self.__device_path = device_path
         self.__speed = speed
-        self.__reset_delay = reset_delay
+        self.__read_timeout = read_timeout
+        self.__read_retries = read_retries
+        self.__common_retries = common_retries
+        self.__retries_delay = retries_delay
+        self.__noop = noop
+
+        self.__state_poll = state_poll
 
         self.__pressed_keys: Set[str] = set()
         self.__pressed_mouse_buttons: Set[str] = set()
         self.__lock = asyncio.Lock()
-        self.__queue: multiprocessing.queues.Queue = multiprocessing.Queue()
+        self.__events_queue: multiprocessing.queues.Queue = multiprocessing.Queue()
+
+        self.__ok_shared = multiprocessing.Value("i", 1)
 
         self.__stop_event = multiprocessing.Event()
 
@@ -75,6 +127,14 @@ class Hid(multiprocessing.Process):  # pylint: disable=too-many-instance-attribu
         get_logger().info("Starting HID daemon ...")
         super().start()
 
+    def get_state(self) -> Dict:
+        return {"ok": bool(self.__ok_shared.value)}
+
+    async def poll_state(self) -> AsyncGenerator[Dict, None]:
+        while self.is_alive():
+            yield self.get_state()
+            await asyncio.sleep(self.__state_poll)
+
     async def reset(self) -> None:
         async with self.__lock:
             gpio.write(self.__reset, True)
@@ -84,32 +144,34 @@ class Hid(multiprocessing.Process):  # pylint: disable=too-many-instance-attribu
     async def send_key_event(self, key: str, state: bool) -> None:
         if not self.__stop_event.is_set():
             async with self.__lock:
-                if state and key not in self.__pressed_keys:
-                    self.__pressed_keys.add(key)
-                    self.__queue.put(_KeyEvent(key, state))
-                elif not state and key in self.__pressed_keys:
-                    self.__pressed_keys.remove(key)
-                    self.__queue.put(_KeyEvent(key, state))
+                if _KeyEvent.is_valid(key):
+                    if state and key not in self.__pressed_keys:
+                        self.__pressed_keys.add(key)
+                        self.__events_queue.put(_KeyEvent(key, state))
+                    elif not state and key in self.__pressed_keys:
+                        self.__pressed_keys.remove(key)
+                        self.__events_queue.put(_KeyEvent(key, state))
 
     async def send_mouse_move_event(self, to_x: int, to_y: int) -> None:
         if not self.__stop_event.is_set():
             async with self.__lock:
-                self.__queue.put(_MouseMoveEvent(to_x, to_y))
+                self.__events_queue.put(_MouseMoveEvent(to_x, to_y))
 
     async def send_mouse_button_event(self, button: str, state: bool) -> None:
         if not self.__stop_event.is_set():
             async with self.__lock:
-                if state and button not in self.__pressed_mouse_buttons:
-                    self.__pressed_mouse_buttons.add(button)
-                    self.__queue.put(_MouseButtonEvent(button, state))
-                elif not state and button in self.__pressed_mouse_buttons:
-                    self.__pressed_mouse_buttons.remove(button)
-                    self.__queue.put(_MouseButtonEvent(button, state))
+                if _MouseButtonEvent.is_valid(button):
+                    if state and button not in self.__pressed_mouse_buttons:
+                        self.__pressed_mouse_buttons.add(button)
+                        self.__events_queue.put(_MouseButtonEvent(button, state))
+                    elif not state and button in self.__pressed_mouse_buttons:
+                        self.__pressed_mouse_buttons.remove(button)
+                        self.__events_queue.put(_MouseButtonEvent(button, state))
 
     async def send_mouse_wheel_event(self, delta_y: int) -> None:
         if not self.__stop_event.is_set():
             async with self.__lock:
-                self.__queue.put(_MouseWheelEvent(delta_y))
+                self.__events_queue.put(_MouseWheelEvent(delta_y))
 
     async def clear_events(self) -> None:
         if not self.__stop_event.is_set():
@@ -120,7 +182,7 @@ class Hid(multiprocessing.Process):  # pylint: disable=too-many-instance-attribu
         async with self.__lock:
             if self.is_alive():
                 self.__unsafe_clear_events()
-                get_logger().info("Stopping keyboard daemon ...")
+                get_logger().info("Stopping HID daemon ...")
                 self.__stop_event.set()
                 self.join()
             else:
@@ -130,17 +192,17 @@ class Hid(multiprocessing.Process):  # pylint: disable=too-many-instance-attribu
 
     def __unsafe_clear_events(self) -> None:
         for button in self.__pressed_mouse_buttons:
-            self.__queue.put(_MouseButtonEvent(button, False))
+            self.__events_queue.put(_MouseButtonEvent(button, False))
         self.__pressed_mouse_buttons.clear()
         for key in self.__pressed_keys:
-            self.__queue.put(_KeyEvent(key, False))
+            self.__events_queue.put(_KeyEvent(key, False))
         self.__pressed_keys.clear()
 
     def __emergency_clear_events(self) -> None:
         if os.path.exists(self.__device_path):
             try:
-                with serial.Serial(self.__device_path, self.__speed) as tty:
-                    self.__send_clear_hid(tty)
+                with self.__get_serial() as tty:
+                    self.__process_request(tty, b"\x10\x00\x00\x00\x00")
             except Exception:
                 get_logger().exception("Can't execute emergency clear HID events")
 
@@ -148,70 +210,102 @@ class Hid(multiprocessing.Process):  # pylint: disable=too-many-instance-attribu
         signal.signal(signal.SIGINT, signal.SIG_IGN)
         setproctitle.setproctitle("[hid] " + setproctitle.getproctitle())
         try:
-            with serial.Serial(self.__device_path, self.__speed) as tty:
-                hid_ready = False
-                while True:
-                    if hid_ready:
-                        try:
-                            event = self.__queue.get(timeout=0.05)
-                        except queue.Empty:
-                            pass
+            with self.__get_serial() as tty:
+                passed = 0
+                while not (self.__stop_event.is_set() and self.__events_queue.qsize() == 0):
+                    try:
+                        event = self.__events_queue.get(timeout=0.05)
+                    except queue.Empty:
+                        if passed >= 20:  # 20 * 0.05 = 1 sec
+                            self.__process_request(tty, b"\x01\x00\x00\x00\x00")  # Ping
+                            passed = 0
                         else:
-                            if isinstance(event, _KeyEvent):
-                                self.__send_key_event(tty, event)
-                            elif isinstance(event, _MouseMoveEvent):
-                                self.__send_mouse_move_event(tty, event)
-                            elif isinstance(event, _MouseButtonEvent):
-                                self.__send_mouse_button_event(tty, event)
-                            elif isinstance(event, _MouseWheelEvent):
-                                self.__send_mouse_wheel_event(tty, event)
-                            else:
-                                raise RuntimeError("Unknown HID event")
-                            hid_ready = False
-
-                    if tty.in_waiting:
-                        while tty.in_waiting:
-                            tty.read(tty.in_waiting)
-                        hid_ready = True
+                            passed += 1
                     else:
-                        time.sleep(0.05)
-
-                    if self.__stop_event.is_set() and self.__queue.qsize() == 0:
-                        break
+                        self.__process_request(tty, event.make_command())
+                        passed = 0
         except Exception:
             get_logger().exception("Unhandled exception")
             raise
 
-    def __send_key_event(self, tty: serial.Serial, event: _KeyEvent) -> None:
-        code = _KEYMAP.get(event.key)
-        if code:
-            key_bytes = bytes([code])
-            assert len(key_bytes) == 1, (event, key_bytes)
-            tty.write(
-                b"\01"
-                + key_bytes
-                + (b"\01" if event.state else b"\00")
-                + b"\00\00"
-            )
-
-    def __send_mouse_move_event(self, tty: serial.Serial, event: _MouseMoveEvent) -> None:
-        to_x = min(max(-32768, event.to_x), 32767)
-        to_y = min(max(-32768, event.to_y), 32767)
-        tty.write(b"\02" + struct.pack(">hh", to_x, to_y))
-
-    def __send_mouse_button_event(self, tty: serial.Serial, event: _MouseButtonEvent) -> None:
-        if event.button == "left":
-            code = (0b10000000 | (0b00001000 if event.state else 0))
-        elif event.button == "right":
-            code = (0b01000000 | (0b00000100 if event.state else 0))
-        else:
-            code = 0
-        if code:
-            tty.write(b"\03" + bytes([code]) + b"\00\00\00")
-
-    def __send_mouse_wheel_event(self, tty: serial.Serial, event: _MouseWheelEvent) -> None:
-        delta_y = min(max(-128, event.delta_y), 127)
-        tty.write(b"\04\00" + struct.pack(">b", delta_y) + b"\00\00")
-
-    def __send_clear_hid(self, tty: serial.Serial) -> None:
-        tty.write(b"\00\00\00\00\00")
+    def __get_serial(self) -> serial.Serial:
+        return serial.Serial(self.__device_path, self.__speed, timeout=self.__read_timeout)
+
+    def __process(self, tty: serial.Serial, command: bytes) -> None:
+        self.__process_request(tty, self.__make_request(command))
+
+    def __process_request(self, tty: serial.Serial, request: bytes) -> None:  # pylint: disable=too-many-branches
+        logger = get_logger()
+
+        common_retries = self.__common_retries
+        read_retries = self.__read_retries
+        error_occured = False
+
+        while common_retries and read_retries:
+            if not self.__noop:
+                if tty.in_waiting:
+                    tty.read(tty.in_waiting)
+
+                assert tty.write(request) == len(request)
+                response = tty.read(4)
+            else:
+                response = b"\x33\x20"  # Magic + OK
+                response += struct.pack(">H", self.__make_crc16(response))
+
+            if len(response) < 4:
+                logger.error("No response from HID: request=%r", request)
+                read_retries -= 1
+            else:
+                assert len(response) == 4, response
+                if self.__make_crc16(response[-4:-2]) != struct.unpack(">H", response[-2:])[0]:
+                    get_logger().error("Invalid response CRC; requesting response again ...")
+                    request = self.__make_request(b"\x02\x00\x00\x00\x00")  # Repeat an answer
+                else:
+                    code = response[1]
+                    if code == 0x48:  # Request timeout
+                        logger.error("Got request timeout from HID: request=%r", request)
+                    elif code == 0x40:  # CRC Error
+                        logger.error("Got CRC error of request from HID: request=%r", request)
+                    elif code == 0x45:  # Unknown command
+                        logger.error("HID did not recognize the request=%r", request)
+                        self.__ok_shared.value = 1
+                        return
+                    elif code == 0x24:  # Rebooted?
+                        logger.error("No previous command state inside HID, seems it was rebooted")
+                        self.__ok_shared.value = 1
+                        return
+                    elif code == 0x20:  # Done
+                        if error_occured:
+                            logger.info("Success!")
+                        self.__ok_shared.value = 1
+                        return
+                    else:
+                        logger.error("Invalid response from HID: request=%r; code=0x%x", request, code)
+
+                common_retries -= 1
+            error_occured = True
+            self.__ok_shared.value = 0
+
+            if common_retries and read_retries:
+                logger.error("Retries left: common_retries=%d; read_retries=%d", common_retries, read_retries)
+                time.sleep(self.__retries_delay)
+
+        logger.error("Can't process HID request due many errors: %r", request)
+
+    def __make_request(self, command: bytes) -> bytes:
+        request = b"\x33" + command
+        request += struct.pack(">H", self.__make_crc16(request))
+        assert len(request) == 8, (request, command)
+        return request
+
+    def __make_crc16(self, data: bytes) -> int:
+        crc = 0xFFFF
+        for byte in data:
+            crc = crc ^ byte
+            for _ in range(8):
+                if crc & 0x0001 == 0:
+                    crc = crc >> 1
+                else:
+                    crc = crc >> 1
+                    crc = crc ^ 0xA001
+        return crc
diff --git a/kvmd/apps/kvmd/server.py b/kvmd/apps/kvmd/server.py
index f3e7ddc8..37e6ca5b 100644
--- a/kvmd/apps/kvmd/server.py
+++ b/kvmd/apps/kvmd/server.py
@@ -144,6 +144,7 @@ def _system_task(method: Callable) -> Callable:
     async def wrap(self: "Server") -> None:
         try:
             await method(self)
+            raise RuntimeError("Dead system task: %s" % (method))
         except asyncio.CancelledError:
             pass
         except Exception:
@@ -201,6 +202,7 @@ def _valid_int(name: str, value: Optional[str], min_value: Optional[int]=None, m
 
 class _Events(Enum):
     INFO_STATE = "info_state"
+    HID_STATE = "hid_state"
     ATX_STATE = "atx_state"
     MSD_STATE = "msd_state"
     STREAMER_STATE = "streamer_state"
@@ -357,6 +359,7 @@ class Server:  # pylint: disable=too-many-instance-attributes
         await self.__register_socket(ws)
         await asyncio.gather(*[
             self.__broadcast_event(_Events.INFO_STATE, (await self.__make_info())),
+            self.__broadcast_event(_Events.HID_STATE, self.__hid.get_state()),
             self.__broadcast_event(_Events.ATX_STATE, self.__atx.get_state()),
             self.__broadcast_event(_Events.MSD_STATE, self.__msd.get_state()),
             self.__broadcast_event(_Events.STREAMER_STATE, (await self.__streamer.get_state())),
@@ -568,12 +571,6 @@ class Server:  # pylint: disable=too-many-instance-attributes
     # ===== SYSTEM TASKS
 
     @_system_task
-    async def __hid_watchdog(self) -> None:
-        while self.__hid.is_alive():
-            await asyncio.sleep(0.1)
-        raise RuntimeError("HID is dead")
-
-    @_system_task
     async def __stream_controller(self) -> None:
         prev = 0
         shutdown_at = 0.0
@@ -607,6 +604,11 @@ class Server:  # pylint: disable=too-many-instance-attributes
             await asyncio.sleep(0.1)
 
     @_system_task
+    async def __poll_hid_state(self) -> None:
+        async for state in self.__hid.poll_state():
+            await self.__broadcast_event(_Events.HID_STATE, state)
+
+    @_system_task
     async def __poll_atx_state(self) -> None:
         async for state in self.__atx.poll_state():
             await self.__broadcast_event(_Events.ATX_STATE, state)
diff --git a/testenv/kvmd.yaml b/testenv/kvmd.yaml
index b2c91c3e..1ca4e490 100644
--- a/testenv/kvmd.yaml
+++ b/testenv/kvmd.yaml
@@ -16,9 +16,17 @@ kvmd:
         pinout:
             reset: 4
 
+        reset_delay: 0.1
+
         device: /dev/ttyS10
         speed: 115200
-        reset_delay: 0.1
+        read_timeout: 2.0
+        read_retries: 10
+        common_retries: 100
+        retries_delay: 0.1
+        noop: true
+
+        state_poll: 1.0
 
     atx:
         pinout:
@@ -36,7 +44,7 @@ kvmd:
             target: 12
             reset: 13
 
-        device: "/dev/kvmd-msd"
+        device: /dev/kvmd-msd
         init_delay: 2.0
         reset_delay: 1.0
         write_meta: true
diff --git a/web/share/js/kvm/hid.js b/web/share/js/kvm/hid.js
index 1c60b099..b6896955 100644
--- a/web/share/js/kvm/hid.js
+++ b/web/share/js/kvm/hid.js
@@ -62,6 +62,11 @@ function Hid() {
 		__mouse.setSocket(ws);
 	};
 
+	self.setState = function(state) {
+		__keyboard.setState(state);
+		__mouse.setState(state);
+	};
+
 	var __releaseAll = function() {
 		__keyboard.releaseAll();
 	};
diff --git a/web/share/js/kvm/keyboard.js b/web/share/js/kvm/keyboard.js
index c424c659..d5ccd80f 100644
--- a/web/share/js/kvm/keyboard.js
+++ b/web/share/js/kvm/keyboard.js
@@ -4,6 +4,7 @@ function Keyboard() {
 	/********************************************************************************/
 
 	var __ws = null;
+	var __ok = true;
 
 	var __keys = [].slice.call($$$("div#keyboard-desktop div.keyboard-block div.keyboard-row div.key"));
 	var __modifiers = [].slice.call($$$("div#keyboard-desktop div.keyboard-block div.keyboard-row div.modifier"));
@@ -53,6 +54,10 @@ function Keyboard() {
 		__updateLeds();
 	};
 
+	self.setState = function(state) {
+		__ok = state.ok;
+	};
+
 	self.releaseAll = function() {
 		__keys.concat(__modifiers).forEach(function(el_key) {
 			if (__isActive(el_key)) {
@@ -73,8 +78,13 @@ function Keyboard() {
 				|| $("keyboard-window").classList.contains("window-active")
 			)
 		) {
-			$("hid-keyboard-led").className = "led-green";
-			$("hid-keyboard-led").title = "Keyboard captured";
+			if (__ok) {
+				$("hid-keyboard-led").className = "led-green";
+				$("hid-keyboard-led").title = "Keyboard captured";
+			} else {
+				$("hid-keyboard-led").className = "led-yellow";
+				$("hid-keyboard-led").title = "Keyboard captured, HID offline";
+			}
 		} else {
 			$("hid-keyboard-led").className = "led-gray";
 			$("hid-keyboard-led").title = "Keyboard free";
diff --git a/web/share/js/kvm/mouse.js b/web/share/js/kvm/mouse.js
index 59b3fc73..75836b1d 100644
--- a/web/share/js/kvm/mouse.js
+++ b/web/share/js/kvm/mouse.js
@@ -4,6 +4,7 @@ function Mouse() {
 	/********************************************************************************/
 
 	var __ws = null;
+	var __ok = true;
 
 	var __current_pos = {x: 0, y:0};
 	var __sent_pos = {x: 0, y:0};
@@ -44,6 +45,10 @@ function Mouse() {
 		__updateLeds();
 	};
 
+	self.setState = function(state) {
+		__ok = state.ok;
+	};
+
 	var __hoverStream = function() {
 		__stream_hovered = true;
 		__updateLeds();
@@ -57,8 +62,13 @@ function Mouse() {
 	var __updateLeds = function() {
 		if (__ws && (__stream_hovered || tools.browser.is_ios)) {
 			// Mouse is always available on iOS via touchscreen
-			$("hid-mouse-led").className = "led-green";
-			$("hid-mouse-led").title = "Mouse tracked";
+			if (__ok) { 
+				$("hid-mouse-led").className = "led-green";
+				$("hid-mouse-led").title = "Mouse tracked";
+			} else {
+				$("hid-mouse-led").className = "led-yellow";
+				$("hid-mouse-led").title = "Mouse tracked, HID offline";
+			}
 		} else {
 			$("hid-mouse-led").className = "led-gray";
 			$("hid-mouse-led").title = "Mouse free";
diff --git a/web/share/js/kvm/session.js b/web/share/js/kvm/session.js
index 95f8fd88..7f2100bd 100644
--- a/web/share/js/kvm/session.js
+++ b/web/share/js/kvm/session.js
@@ -84,6 +84,8 @@ function Session() {
 		} else if (event.msg_type === "event") {
 			if (event.msg.event === "info_state") {
 				__setKvmdInfo(event.msg.event_attrs);
+			} else if (event.msg.event === "hid_state") {
+				__hid.setState(event.msg.event_attrs);
 			} else if (event.msg.event === "atx_state") {
 				__atx.setState(event.msg.event_attrs);
 			} else if (event.msg.event === "msd_state") {