diff --git a/fw/Core/Inc/main.h b/fw/Core/Inc/main.h
index 9172851..7e5429f 100644
--- a/fw/Core/Inc/main.h
+++ b/fw/Core/Inc/main.h
@@ -52,6 +52,7 @@ extern "C" {
 #include "scd4x.h"
 #include "sht4x.h"
 #include "sps30.h"
+#include "modbus.h"
 /* USER CODE END Includes */
 
 /* Exported types ------------------------------------------------------------*/
@@ -98,7 +99,6 @@ void Error_Handler(void);
 /* USER CODE BEGIN Private defines */
 #define MEASUREMENT_PERIOD_MS		600000
 
-extern uint8_t lpuart1_rx_message[255];
 extern uint8_t lpuart1_rx_message_index;
 extern uint8_t lpuart1_rx_message_len;
 extern uint8_t lpuart1_rx_done;
diff --git a/fw/Core/Inc/modbus.h b/fw/Core/Inc/modbus.h
new file mode 100644
index 0000000..7eb2e4a
--- /dev/null
+++ b/fw/Core/Inc/modbus.h
@@ -0,0 +1,178 @@
+/*
+ * modbus.h
+ *
+ *  Created on: Jul 18, 2021
+ *      Author: user
+ *
+ *  Modbus slave RTU library (does NOT support ASCII and TCP)
+ *
+ *  Useful links:
+ *  https://www.picotech.com/library/oscilloscopes/modbus-serial-protocol-decoding
+ *  https://ipc2u.com/articles/knowledge-base/modbus-rtu-made-simple-with-detailed-descriptions-and-examples/
+ *  https://modbus.org/docs/Modbus_over_serial_line_V1_02.pdf
+ *  https://www.modbus.org/docs/Modbus_Application_Protocol_V1_1b.pdf
+ *
+ *  Note that byte order is big endian.
+ *
+ * USAGE:
+ *
+ * 1) Implement functions modbus_callback_function() and modbus_uart_transmit_function()
+ *      - modbus_uart_transmit_function() sends data via UART
+ *      - modbus_callback_function() does the real work: read sensors, set outputs...
+ *        note that when filling buffers (e.g. input_registers[]) user must
+ *        ensure that all data is big-endian
+ *    These functions are implementation-specific.
+ * 2) Set device address (variable modbus_device_address); you can do this either
+ *      - setting modbus_device_address directly (modbus.h needs to be included, duh)
+ *      - using modbus_set_device_address(uint8_t address) function
+ *    Or you can leave address as-is (MODBUS_DEFAULT_SLAVE_ADDRESS) and set it via
+ *    Modbus during runtime
+ * 3) Call modbus_process_msg() after message reception; you need to observe Modbus RTU timing:
+ *      - pauses between chars in frame are less or equal to 1.5 char
+ *      - pauses between frames are at least 3.5 chars (of silence)
+ *    For more information see section 2.5.1.1 (MODBUS Message RTU Framing)
+ *    in "MODBUS over Serial Line: Specification and Implementation Guide"
+ *
+ */
+
+#ifndef SRC_MODBUS_H_
+#define SRC_MODBUS_H_
+
+#include "stdint.h"
+
+/*
+ * Defines & macros
+ */
+
+#define MODBUS_BROADCAST_ADDR 0
+#define MODBUS_DEFAULT_SLAVE_ADDRESS 254 /* 255 may be used for bridge device */
+/* minimal frame length is 4 bytes: 1 B slave address, 1 B function code, 2 B CRC */
+#define MODBUS_MINIMAL_FRAME_LEN 4
+#define MODBUS_MAX_RTU_FRAME_SIZE 256
+#define MODBUS_BUFFER_SIZE MODBUS_MAX_RTU_FRAME_SIZE /* alias */
+#define MODBUS_ERROR_FLAG 0x80
+#define MODBUS_MAX_REGISTERS 125
+
+/*
+ * Return values
+ */
+
+#define MODBUS_OK 0
+#define MODBUS_ERROR -1 // generic error
+#define MODBUS_ERROR_CRC -2 // checksum failed
+#define MODBUS_ERROR_FRAME_INVALID -3 // invalid frame format / length
+#define MODBUS_ERROR_OUT_OF_BOUNDS -4 // requested register is out of bounds
+#define MODBUS_ERROR_FUNCTION_NOT_IMPLEMENTED -5 // function not implemented in callback
+#define MODBUS_ERROR_REGISTER_NOT_IMPLEMENTED -6 // register not implemented in callback
+
+/*
+ * Data types
+ */
+
+/* Public functions codes (Modbus Application protocol specification, section 5.1) */
+typedef enum {
+	/* single bit access functions */
+	MODBUS_READ_COILS = 1,
+	MODBUS_READ_DO = 1, // alias
+	MODBUS_READ_DISCRETE_INPUTS = 2,
+	MODBUS_READ_DI = 2, // alias
+	MODBUS_WRITE_SINGLE_COIL = 5,
+	MODBUS_WRITE_SINGLE_DO = 5, // alias
+	MODBUS_WRITE_MULTIPLE_COILS = 15,
+	MODBUS_WRITE_MULTIPLE_DO = 15, // alias
+	/* 16-bit access functions */
+	MODBUS_READ_HOLDING_REGISTERS = 3,
+	MODBUS_READ_AO = 3, // alias
+	MODBUS_READ_INPUT_REGISTERS = 4,
+	MODBUS_READ_AI = 4, // alias
+	MODBUS_WRITE_SINGLE_REGISTER = 6,
+	MODBUS_WRITE_SINGLE_AO = 6, // alias
+	MODBUS_WRITE_MULTIPLE_REGISTERS = 16,
+	MODBUS_WRITE_MULTIPLE_AO = 16, // alias
+	MODBUS_MASK_WRITE_REGISTER = 22,
+	MODBUS_READ_WRITE_MULTIPLE_REGISTERS = 23,
+	MODBUS_READ_FIFO_QUEUE = 24,
+	/* file record access */
+	MODBUS_READ_FILE_RECORD = 20,
+	MODBUS_WRITE_FILE_RECORD = 21,
+	/* diagnostics */
+	MODBUS_READ_EXCEPTION_STATUS = 7,
+	MODBUS_DIAGNOSTIC = 8, /* sub codes: 00-18,20 */
+	MODBUS_GET_COM_EVENT_COUNTER = 11,
+	MODBUS_GET_COM_EVENT_LOG = 12,
+	MODBUS_REPORT_SLAVE_ID = 17,
+	MODBUS_READ_DEVICE_IDENTIFICATION = 43, /* sub codes: 14 */
+} modbus_function_code_t;
+
+typedef enum {
+	MODBUS_EXCEPTION_ILLEGAL_FUNCTION = 1,
+	MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS = 2,
+	MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE = 3,
+	MODBUS_EXCEPTION_SLAVE_DEVICE_FAILURE = 4,
+	MODBUS_EXCEPTION_ACKNOWLEDGE = 5,
+	MODBUS_EXCEPTION_SLAVE_DEVICE_BUSY = 6,
+	MODBUS_EXCEPTION_MEMORY_PARITY_ERROR = 8,
+	MODBUS_EXCEPTION_GATEWAY_PATH_UNAVAILABLE = 10,
+	MODBUS_EXCEPTION_GATEWAY_TARGET_DEVICE_FAILED_TO_RESPOND = 11,
+} modbus_exception_code_t;
+
+typedef struct {
+	uint8_t exception_code;
+} exception_t;
+
+typedef struct {
+	modbus_function_code_t function_code : 8;
+	uint16_t register_address; // e.g. first register of A0: 0
+	uint16_t register_number;  // e.g. first register of A0: 40001
+	uint8_t  register_count; // number of registers to be read/written
+
+	exception_t exception;
+
+	union {
+		uint8_t buffer8b[MODBUS_MAX_RTU_FRAME_SIZE];
+		uint16_t buffer16b[MODBUS_MAX_RTU_FRAME_SIZE/2];
+		uint16_t input_registers[MODBUS_MAX_REGISTERS];
+		uint16_t holding_registers[MODBUS_MAX_REGISTERS];
+	};
+} modbus_transaction_t;
+
+typedef enum {
+	MODBUS_DO_START_NUMBER = 1, // Discrete output coils
+	MODBUS_DO_END_NUMBER = 9999,
+	MODBUS_DI_START_NUMBER = 10001, // Discrete input contacts
+	MODBUS_DI_END_NUMBER = 19999,
+	MODBUS_AI_START_NUMBER = 30001, // Analog input registers
+	MODBUS_AI_END_NUMBER = 39999,
+	MODBUS_AO_START_NUMBER = 40001, // Analog output (holding registers)
+	MODBUS_AO_END_NUMBER = 49999
+} modbus_register_number_t;
+
+
+/*
+ * Global variables
+ */
+
+/* device address: declared in modbus.c */
+extern uint8_t modbus_slave_address;
+
+/* shared modbus buffer; defined in modbus.c; may be used elsewhere in code */
+extern uint8_t modbus_buffer[];
+
+/*
+ * Function prototypes
+ */
+
+/* process message: should be called in when modbus message was received (e.g. in main.c)
+ * modbus_process_msg() may call following functions:
+ *     - modbus_callback_function() if data readout is requested
+ *     - modbus_uart_transmit_function() if response is required
+ * Both functions have to be implemented by user.
+ */
+int8_t modbus_slave_process_msg(const uint8_t *buffer, int len);
+int8_t modbus_slave_set_address(uint8_t address);
+/* modbus callback function type - should be implemented by user (e.g. in main.c) */
+int8_t modbus_slave_callback(modbus_transaction_t *transaction);
+/* UART transmit function type - should be implemented by user (e.g. in main.c) */
+int8_t modbus_transmit_function(uint8_t *buffer, int data_len);
+
+#endif /* SRC_MODBUS_H_ */
diff --git a/fw/Core/Src/main.c b/fw/Core/Src/main.c
index 879c817..56f3423 100644
--- a/fw/Core/Src/main.c
+++ b/fw/Core/Src/main.c
@@ -64,6 +64,16 @@ static void MX_TIM21_Init(void);
 
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
+int8_t modbus_slave_callback(modbus_transaction_t *transaction)
+{
+	return MODBUS_ERROR_FUNCTION_NOT_IMPLEMENTED; /* nothing implemented yet! TODO */
+}
+
+int8_t modbus_transmit_function(uint8_t *buffer, int data_len)
+{
+	return MODBUS_OK; /* TODO */
+}
+
 
 /* USER CODE END 0 */
 
@@ -157,13 +167,13 @@ int main(void)
 	  /* UART RX is done */
 	  if (lpuart1_rx_done == 1)
 	  {
-		  /* Process the message */
-		  uint8_t buff_pom[255];
-		  for (uint8_t j = 0; j < lpuart1_rx_message_len; j ++)
-		  {
-			  buff_pom[j] = lpuart1_rx_message[j];
-		  }
-		  /* process_modbus_message(lpuart1_rx_message, lpuart1_rx_message_len); */
+		  /* Process the message:
+		   * message is stored in modbus_buffer[], no copying necessary;
+		   * but we need to make sure that modbus_buffer[] will not be used while
+		   * processing the message: this can be done by disabling RX interrupt */
+		  LL_LPUART_DisableIT_RXNE(LPUART1);
+		  modbus_slave_process_msg(modbus_buffer, lpuart1_rx_message_len);
+		  LL_LPUART_EnableIT_RXNE(LPUART1);
 
 		  /* Reset the RX DONE flag */
 		  lpuart1_rx_done = 0;
diff --git a/fw/Core/Src/modbus.c b/fw/Core/Src/modbus.c
new file mode 100644
index 0000000..854c7e8
--- /dev/null
+++ b/fw/Core/Src/modbus.c
@@ -0,0 +1,223 @@
+/*
+ * modbus.c
+ *
+ *  Created on: Jul 18, 2021
+ *      Author: user
+ */
+
+#include "modbus.h"
+
+/*
+ * Global variables
+ */
+
+/* Modbus TX buffer; can be also used for RX in memory constrained systems (e.g. in main.c);
+ * NOTE if shared buffer is used for TX/RX, care must be taken to prevent writing into buffer
+ * during execution of modbus_process_message() */
+uint8_t modbus_buffer[MODBUS_MAX_RTU_FRAME_SIZE];
+
+/* device address: declared  */
+uint8_t modbus_slave_address = MODBUS_DEFAULT_SLAVE_ADDRESS;
+
+/*
+ * CRC16 functions
+ * see https://modbus.org/docs/Modbus_over_serial_line_V1_02.pdf
+ * section 6.2.2
+ */
+
+/* CRC16 (without memory mapped values)
+ * taken from https://ctlsys.com/support/how_to_compute_the_modbus_rtu_message_crc/ */
+uint16_t modbus_CRC16(const uint8_t *buf, int len)
+{
+	uint16_t crc = 0xFFFF;
+
+	for (int pos = 0; pos < len; pos++) {
+		crc ^= (uint16_t)buf[pos];          // XOR byte into least sig. byte of crc
+       
+		for (int i = 8; i != 0; i--) {    // Loop over each bit
+			if ((crc & 0x0001) != 0) {      // If the LSB is set
+				crc >>= 1;                    // Shift right and XOR 0xA001
+				crc ^= 0xA001;
+			} else {                            // Else LSB is not set
+				crc >>= 1;                    // Just shift right
+			}
+		}
+	}
+	// Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes)
+	return crc;  
+}
+
+/*
+ * Private functions
+ */
+
+/* here we assume buffer has minimal size of MODBUS_MAX_RTU_FRAME_SIZE;
+ * this function is private, so hopefully it's going to be ok */
+int8_t modbus_copy_reply_to_buffer(uint8_t *buffer, uint8_t *msg_len, modbus_transaction_t *transaction)
+{
+	uint16_t crc16;
+	uint8_t byte_count;
+
+	buffer[0] = modbus_slave_address;
+	buffer[1] = transaction->function_code;
+	*msg_len = 5;
+
+	if (transaction->function_code | MODBUS_ERROR_FLAG) {
+		/* sending error reply */
+		buffer[2] = transaction->exception.exception_code;
+	}
+	switch (transaction->function_code) {
+		case MODBUS_READ_HOLDING_REGISTERS:
+		case MODBUS_READ_INPUT_REGISTERS:
+			byte_count = transaction->register_count * 2;
+			buffer[2] = byte_count;
+			*msg_len = byte_count + 5;
+			for (int i = 0; i < transaction->register_count; i++) {
+				// TODO endianness handling
+				/* buffer16b is alias for both holding and input register buffers */
+				buffer[3 + 2*i] = transaction->buffer16b[i] >> 8;
+				buffer[4 + 2*i] = transaction->buffer16b[i] & 0xff;
+			}
+			break;
+	}
+	crc16 = modbus_CRC16(buffer, *msg_len - 2); /* last two bytes is the checksum itself */
+	buffer[*msg_len - 2] = crc16 & 0xff;
+	buffer[*msg_len - 1] = crc16 >> 8;
+}
+
+/*
+ * Public function definitions
+ */
+
+int8_t modbus_slave_set_address(uint8_t address)
+{
+	if (address == 0) {
+		/* address 0 is broadcast address */
+		return MODBUS_ERROR;
+	}
+	modbus_slave_address = address;
+	return MODBUS_OK;
+}
+
+int8_t modbus_slave_process_msg(const uint8_t *buffer, int len)
+{
+	/* transaction holds message context and content:
+	 * it wraps all necessary buffers and variables */
+	modbus_transaction_t transaction;
+	int8_t callback_result;
+	uint8_t buffer_pos = 0;
+
+	if (len < MODBUS_MINIMAL_FRAME_LEN) {
+		/* frame too short; return error */
+		return MODBUS_ERROR_FRAME_INVALID;
+	}
+	/* check CRC first */
+	uint16_t crc_received = (buffer[len - 1] << 8) | buffer[len - 2];
+	uint16_t crc_calculated = modbus_CRC16(buffer, len - 2);
+	if (crc_received != crc_calculated) {
+		/* CRC mismatch, return error */
+		//printf("crc mismatch: received 0x%x, calculated 0x%x\n", crc_received, crc_calculated);
+		return MODBUS_ERROR_CRC;
+	}
+	/* check if address matches ours */
+	uint8_t address = buffer[buffer_pos++];
+	if (address != modbus_slave_address && address != MODBUS_BROADCAST_ADDR) {
+		/* Message is not for us */
+		return MODBUS_OK;
+	}
+	/* get function code */
+	transaction.function_code = buffer[buffer_pos++];
+	transaction.exception.exception_code = 0;
+
+	if (transaction.function_code == MODBUS_READ_DEVICE_IDENTIFICATION) {
+		// TODO
+		goto modbus_send;
+	}
+
+	/* set starting register number */
+	switch (transaction.function_code) {
+	/* coils */
+	case MODBUS_READ_DO:
+	case MODBUS_WRITE_SINGLE_DO:
+	case MODBUS_WRITE_MULTIPLE_DO:
+		transaction.register_number = MODBUS_DO_START_NUMBER;
+		break;
+	/* discrete inputs */
+	case MODBUS_READ_DI:
+		transaction.register_number = MODBUS_DI_START_NUMBER;
+		break;
+	/* input registers */
+	case MODBUS_READ_AI:
+		transaction.register_number = MODBUS_AI_START_NUMBER;
+		break;
+	/* holding registers */
+	case MODBUS_READ_AO:
+	case MODBUS_WRITE_SINGLE_AO:
+	case MODBUS_WRITE_MULTIPLE_AO:
+	case MODBUS_READ_WRITE_MULTIPLE_REGISTERS:
+		transaction.register_number = MODBUS_AO_START_NUMBER;
+		break;	
+	}
+
+	#define MODBUS_FLAG_WRITE  0x01
+	#define MODBUS_FLAG_SINGLE 0x02
+	uint8_t flags = 0x00;
+
+	/* process message */
+	switch (transaction.function_code) {
+	case MODBUS_WRITE_SINGLE_COIL:
+	case MODBUS_WRITE_SINGLE_REGISTER: /* holding register */
+		flags |= MODBUS_FLAG_SINGLE;
+	case MODBUS_WRITE_MULTIPLE_COILS:
+	case MODBUS_WRITE_MULTIPLE_REGISTERS:
+		flags |= MODBUS_FLAG_WRITE;
+	case MODBUS_READ_DISCRETE_INPUTS:
+	case MODBUS_READ_COILS:
+	case MODBUS_READ_INPUT_REGISTERS:
+	case MODBUS_READ_HOLDING_REGISTERS:
+		if (len < (MODBUS_MINIMAL_FRAME_LEN + 4)) {
+			/* buffer too short to contain everything we need */
+			return MODBUS_ERROR;
+		}
+		transaction.register_address = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++];
+		transaction.register_count = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++];
+		if (
+				transaction.register_count < 1 ||
+				transaction.register_count > MODBUS_MAX_REGISTERS
+		   ) {
+			transaction.exception.exception_code = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE;
+		}
+		// add offset to register number
+		transaction.register_number += transaction.register_address;
+		break;
+	default:
+		/* function code not known / not implemented, reply with
+		 * ExceptionCode 1 */
+		transaction.exception.exception_code = MODBUS_EXCEPTION_ILLEGAL_FUNCTION;
+		break;
+	}
+	/* data in modbus_buffer have been processed and buffer can be re-used for TX */
+	/* handle reply */
+	if (transaction.exception.exception_code != 0) {
+		/* indicate error */
+		transaction.function_code |= MODBUS_ERROR_FLAG;
+	} else {
+		callback_result = modbus_slave_callback(&transaction);
+		/* error handling */
+		if (callback_result != MODBUS_OK) {
+			transaction.function_code |= MODBUS_ERROR_FLAG;
+			if (callback_result == MODBUS_ERROR_FUNCTION_NOT_IMPLEMENTED) {
+				transaction.exception.exception_code = MODBUS_EXCEPTION_ILLEGAL_FUNCTION;
+			} else if (callback_result == MODBUS_ERROR_REGISTER_NOT_IMPLEMENTED) {
+				transaction.exception.exception_code = MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS;
+			}
+		}
+	}
+	uint8_t msg_len = 0;
+modbus_send:
+	if (address != MODBUS_BROADCAST_ADDR) {
+		/* send only if master request was not broadcast */
+		modbus_copy_reply_to_buffer(modbus_buffer, &msg_len, &transaction);
+		modbus_transmit_function(modbus_buffer, msg_len);
+	}
+}
diff --git a/fw/Core/Src/stm32l0xx_it.c b/fw/Core/Src/stm32l0xx_it.c
index fd2c6ca..16bfd27 100644
--- a/fw/Core/Src/stm32l0xx_it.c
+++ b/fw/Core/Src/stm32l0xx_it.c
@@ -42,7 +42,6 @@
 
 /* Private variables ---------------------------------------------------------*/
 /* USER CODE BEGIN PV */
-uint8_t lpuart1_rx_message[255];
 uint8_t lpuart1_rx_message_index = 0;
 uint8_t lpuart1_rx_message_len = 0;
 uint8_t lpuart1_rx_done = 0;
@@ -196,8 +195,10 @@ void LPUART1_IRQHandler(void)
 void LPUART1_CharReception_Callback( void )
 {
 	uint16_t lpuart1_rx_bit = LL_LPUART_ReceiveData9(LPUART1);
-	lpuart1_rx_message[lpuart1_rx_message_index] = (uint8_t)lpuart1_rx_bit;
-    lpuart1_rx_message_index++;
+	if (lpuart1_rx_message_index < (MODBUS_BUFFER_SIZE - 1)) {
+		/* buffer (defined in modbus.c) is shared for TX and RX */
+		modbus_buffer[lpuart1_rx_message_index++] = (uint8_t)lpuart1_rx_bit;
+	}
 }
 /* USER CODE END 1 */
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/