main.c and corresponding makefile

3 files changed, 798 insertions, 1468 deletions

diff --git a/makefile b/makefile
index 7374892..ed9fb02 100644
--- a/makefile
+++ b/makefile
@@ -36,7 +36,7 @@ BUILD_DIR = build
 ######################################

 # C sources

 C_SOURCES =  \

-src/main-hs.c \

+src/main.c \

 src/stm32f4xx_it.c \

 src/stm32f4xx_hal_msp.c \

 src/system_stm32f4xx.c \

diff --git a/src/main-hs.c b/src/main-hs.c
deleted file mode 100644
index 9a98fd4..0000000
--- a/src/main-hs.c
+++ /dev/null
@@ -1,797 +0,0 @@
-/* USER CODE BEGIN Header */
-/**
-******************************************************************************
-* @file           : main.c
-* @brief          : Main program body
-******************************************************************************
-* @attention
-*
-* 
-******************************************************************************
-*/
-
-/* Standard library includes */
-#include <stdio.h>
-
-/* Library includes */
-#include <pb_encode.h>
-#include <pb_decode.h>
-#include "handshake.pb.h"
-
-/* Project includes */
-#include "devices.h"
-#include "config.h"
-#include "main.h"
-
-/* Private Macros */
-#define device_MDR s2m_MDR_response
-#define GET_IDX_FROM_ADDR(i2c_addr) i2c_addr-1
-#define GET_BIT_FROM_IDX(a, b) a[b>>5]&(1<<(b%32))
-#define SET_BIT_FROM_IDX(a, b) a[b>>5]|=(1<<(b%32))
-#define COUNTOF(__BUFFER__)   (sizeof(__BUFFER__) / sizeof(*(__BUFFER__)))
-
-#define I2C_ADDRESS 0x05
-#define BUS_DEVICE_LIMIT 128
-
-/* Macro to toggle between master and slave firmware */
-/* #define MASTER */
-
-/* Private globals */
-I2C_HandleTypeDef hi2c1;
-UART_HandleTypeDef huart1;
-
-device_info_t *device_info[BUS_DEVICE_LIMIT] = {NULL};
-subscription_info_t* subs_info[BUS_DEVICE_LIMIT];
-uint32_t allocated[4]={0};
-uint8_t dev_sts[BUS_DEVICE_LIMIT] = {OFFLINE};
-
-/* Function prototypes */
-void SystemClock_Config(void);
-static void MX_GPIO_Init(void);
-static void MX_I2C1_Init(void);
-static void MX_USART1_UART_Init(void);
-
-bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
-hs_status_t handshake(uint32_t i2c_addr);
-bool todo_hs_or_not_todo_hs(uint8_t i2c_addr);
-state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status);
-bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
-
-
-/**
- * @brief  The application entry point.
- * @retval int
- */
-int main(void)
-{ 
-    /* MCU Configuration */
-
-    /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
-    HAL_Init();
-
-    /* Configure the system clock */
-    SystemClock_Config();
-
-    /* Initialize all configured peripherals */
-    MX_GPIO_Init();
-    MX_I2C1_Init();
-    MX_USART1_UART_Init();
-
-#ifdef TESTING_ENABLE
-#ifdef MASTER
-    uint8_t reset_string[] = "\r\n\n==========MASTER RESET=========\r\n\n";
-    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
-#else
-    uint8_t reset_string[] = "\r\n\n==========SLAVE RESET=========\r\n\n";
-    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
-#endif /* MASTER */
-#endif /* TESTING_ENABLE */
-
-#ifdef MASTER
-    hs_status_t hs_status;
-    for (int curr_addr=0; curr_addr < 10; curr_addr++) {
-	if (todo_hs_or_not_todo_hs(curr_addr)) {
-	    hs_status = handshake(curr_addr);
-	    dev_sts[GET_IDX_FROM_ADDR(curr_addr)] = get_state_from_hs_status(curr_addr, hs_status);
-	}
-    }
-
-#else /* Slave code*/
-    {
-	uint8_t MDR_buf[128], debug_buf[128], term[]="\r\n";
-	size_t MDR_enc_size;
-	s2m_MDR_response res;
-	res.MDR_version=1.1;
-	res.module_id = 1;
-	res.module_class=1;
-	res.entity_id=32;
-
-	res.subscriptions.funcs.encode=encode_subscription_callback;
-	pb_ostream_t ostream = pb_ostream_from_buffer(MDR_buf, sizeof(MDR_buf));
-	if(!pb_encode(&ostream, s2m_MDR_response_fields, &res)){
-#ifdef DEBUG_ENABLE
-	    uint8_t err_buf[] = "MDR encoding error\r\n";
-	    HAL_UART_Transmit(&huart1, err_buf, sizeof(err_buf), 100);
-#endif
-	    while(1) {
-		HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
-		HAL_Delay(500);
-	    }
-	}
-	MDR_enc_size = ostream.bytes_written;
-#ifdef TESTING_ENABLE	    
-	sprintf((char*)debug_buf, "MDR Encoded size: %d\r\n", MDR_enc_size);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	
-	uint8_t bufbuf[] = "MDR Buffer: ";	  
-	HAL_UART_Transmit(&huart1, bufbuf, sizeof(bufbuf), 100);
-	for(int x=0; x<MDR_enc_size; x++) {
-	    sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);
-	}
-	HAL_UART_Transmit(&huart1, debug_buf, MDR_enc_size, 100);  
-	HAL_UART_Transmit(&huart1, term, 2, 100);
-	memset(debug_buf, 0, 128);
-#endif
-	    
-	uint8_t MDR_ACK_buf[8] = {0};
-	s2m_MDR_req_ACK ack;
-	ack.MDR_res_length = MDR_enc_size;
-	pb_ostream_t MDR_ack_ostream = pb_ostream_from_buffer(MDR_ACK_buf,
-							      sizeof(MDR_ACK_buf));
-	if(!pb_encode(&MDR_ack_ostream, s2m_MDR_req_ACK_fields, &ack)) {
-#ifdef DEBUG_ENABLE
-	    uint8_t errbuf[] = "MDR ACK encoding error\r\n";
-	    HAL_UART_Transmit(&huart1, errbuf, sizeof(errbuf), 100);
-#endif /* DEBUG_ENABLE */
-	    while(1) {
-		HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
-		HAL_Delay(500);
-	    }
-	}
-	size_t MDR_ack_size = MDR_ack_ostream.bytes_written;
-#ifdef TESTING_ENABLE
-	sprintf((char*)debug_buf, "MDR ACK Encoded size: %d\r\n", MDR_ack_size);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-#endif /* TESTING_ENABLE */
-
-	uint8_t MDR_req_buf[8]; 
-	m2s_MDR_request MDR_req;
-	
-	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)MDR_req_buf, 2, 10000) != HAL_OK) {
-#ifdef DEBUG_ENABLE
-	    uint8_t debug_buf[] = "Failed to get MDR req\r\n";
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-#endif /* DEBUG_ENABLE */
-	}
-
-#ifdef TESTING_ENABLE
-	uint8_t bufbuf2[] = "MDR ACK buffer: ";
-	HAL_UART_Transmit(&huart1, bufbuf2, sizeof(bufbuf2), 100);
-	for(int x=0; x<2; x++) {
-	    sprintf((char*)debug_buf+x, "%x", MDR_ACK_buf[x]);
-	}
-	HAL_UART_Transmit(&huart1, debug_buf, MDR_enc_size, 100);  
-	HAL_UART_Transmit(&huart1, term, 2, 100);
-	memset(debug_buf, 0, 128);
-#endif
-	
-	pb_istream_t MDR_req_istream = pb_istream_from_buffer(MDR_req_buf, 2);
-	if(!pb_decode(&MDR_req_istream, m2s_MDR_request_fields, &MDR_req)) {
-#ifdef DEBUG_ENABLE
-	    uint8_t errbuf[] = "MDR request decoding error\r\n";
-	    HAL_UART_Transmit(&huart1, errbuf, sizeof(errbuf), 100);
-#endif /* DEBUG_ENABLE */
-	}
-	
-#ifdef TESTING_ENABLE
-	sprintf((char*)debug_buf, "Got requested record type: %ld\r\n", MDR_req.record_type);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-#endif /* TESTING_ENABLE */
-
-	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
-
-	if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_ACK_buf, s2m_MDR_req_ACK_size, 10000) != HAL_OK) {
-#ifdef DEBUG_ENABLE
-	    sprintf((char*)debug_buf, "Unable to send MDR ACK. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-#endif /* DEBUG_ENABLE */
-	}
-
-	uint8_t MDR_CTS_buf[8];
-	m2s_MDR_res_CTS MDR_CTS;
-	
-	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {
-#ifdef DEBUG_ENABLE
-	    sprintf((char*)debug_buf, "Failed to get MDR CTS\r\n");
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-#endif /* DEBUG_ENABLE */
-	}
-#ifdef TESTING_ENABLE
-	uint8_t ctsbuf[] = "Got CTS buffer: ";
-	for(int x=0; x<2; x++) {
-	    sprintf((char*)debug_buf+x, "%x", MDR_CTS_buf[x]);
-	}
-	HAL_UART_Transmit(&huart1, ctsbuf, sizeof(ctsbuf), 100);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	HAL_UART_Transmit(&huart1, term, 2, 100);
-	memset(debug_buf, 0, 128);
-#endif /* TESTING_ENABLE */
-	
-	pb_istream_t MDR_CTS_istream = pb_istream_from_buffer(MDR_CTS_buf, 2);
-	if (!pb_decode(&MDR_CTS_istream, m2s_MDR_res_CTS_fields, &MDR_CTS)) {
-#ifdef DEBUG_ENABLE
-	    sprintf((char*)debug_buf, "Failed to decode MDR CTS\r\n");
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-#endif /* DEBUG_ENABLE */
-	    Error_Handler();
-	}
-
-	uint32_t MDR_timeout = MDR_CTS.timeout;
-	if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_buf, MDR_enc_size, 10000) !=
-	    HAL_OK) {
-#ifdef DEBUG_ENABLE
-	    sprintf((char*)debug_buf, "Unable to send MDR. I2C error: %ld\r\n",
-		    HAL_I2C_GetError(&hi2c1));
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-#endif /* DEBUG_ENABLE */
-	}
-	else {
-	    sprintf((char*)debug_buf, "Successfully sent MDR\r\n");
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-	}
-    }	
-#endif /* MASTER */
-    
-    while (1)
-    {
-
-    }
-}
-
-hs_status_t handshake(uint32_t i2c_addr)
-{
-
-    /* Handshake variables */
-
-    uint8_t hs_sts = IDLE;
-    uint8_t *MDR_req_buf, *MDR_ACK_buf, *MDR_CTS_buf, *MDR_buf;
-    uint32_t AF_error_counter = 0;
-    uint32_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);
-    uint32_t MDR_len = 0;
-    
-    m2s_MDR_request MDR_req_message;
-    s2m_MDR_req_ACK MDR_ACK;
-    m2s_MDR_res_CTS MDR_CTS;
-    s2m_MDR_response MDR_res_message;
-
-#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
-    uint8_t debug_buf[128];
-#endif
-#ifdef TESTING_ENABLE
-    uint8_t term[] = "\r\n";
-    size_t MDR_req_size, MDR_CTS_size;
-#endif
-    
-    while (hs_sts != HS_FAILED && hs_sts != HS_REGISTERED) {
-	switch (hs_sts) {
-	case (IDLE):
-	{
-	    MDR_req_buf = malloc(8);	    
-	    pb_ostream_t MDR_req_stream = pb_ostream_from_buffer(MDR_req_buf, 2);
-	    MDR_req_message.record_type = 7; /* Placeholder for default record type */
-	    if(!pb_encode(&MDR_req_stream, m2s_MDR_request_fields, &MDR_req_message)) {
-		hs_sts = HS_FAILED;
-#ifdef DEBUG_ENABLE
-		goto __MDR_REQ_ENC_FAIL;
-	    __MDR_REQ_ENC_FAIL_END:
-		__asm__("nop");
-#endif
-	    }
-	    else {
-#ifdef TESTING_ENABLE
-		MDR_req_size = MDR_req_stream.bytes_written;
-		goto __HS_IDLE_TESTING;
-	    __HS_IDLE_TESTING_END:
-		__asm__("nop");
-#endif	    
-		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)MDR_req_buf,
-					    MDR_req_buf_len, 10000) != HAL_OK) {
-		    hs_sts = HS_FAILED;
-#ifdef DEBUG_ENABLE
-		    goto __HS_MDR_REQ_I2C_ERROR;
-		__HS_MDR_REQ_I2C_ERROR_END:
-		    __asm__("nop");
-#endif
-		}
-		else {
-		    hs_sts = HS_MDR_ACK;
-		}
-		free(MDR_req_buf);
-		break;
-	    }
-	}
-	case (HS_MDR_ACK):
-	{
-	    MDR_ACK_buf = malloc(8);
-	    AF_error_counter = 0;
-	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)MDR_ACK_buf,
-					  s2m_MDR_req_ACK_size, 100) != HAL_OK) {
-		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
-		    hs_sts = HS_FAILED;
-		}
-		if (++AF_error_counter > 1500) {
-		    hs_sts = HS_FAILED;
-		}
-		if (hs_sts == HS_FAILED) {
-#ifdef DEBUG_ENABLE
-		    goto __HS_MDR_ACK_I2C_ERROR;
-		__HS_MDR_ACK_I2C_ERROR_END:
-		    __asm__("nop");
-#endif
-		    break;
-		}
-	    }
-	    if (hs_sts != HS_FAILED) {
-		pb_istream_t MDR_ACK_istream = pb_istream_from_buffer(MDR_ACK_buf, 2);
-		if (!pb_decode(&MDR_ACK_istream, s2m_MDR_req_ACK_fields, &MDR_ACK)) {
-		    hs_sts = HS_FAILED;
-#ifdef DEBUG_ENABLE
-		    goto __MDR_ACK_DEC_ERROR;
-		__MDR_ACK_DEC_ERROR_END:
-		    __asm__("nop");
-#endif
-		}
-		else {
-		    MDR_len = MDR_ACK.MDR_res_length;
-		    hs_sts = HS_MDR_CTS;
-#ifdef TESTING_ENABLE
-		    goto __HS_MDR_ACK_TESTING;
-		__HS_MDR_ACK_TESTING_END:
-		    __asm__("nop");
-#endif
-		}
-		free(MDR_ACK_buf);
-	    }
-	    break;
-	}
-	case (HS_MDR_CTS):
-	{
-	    MDR_CTS_buf = (uint8_t*)malloc(8);
-	    pb_ostream_t MDR_CTS_ostream = pb_ostream_from_buffer(MDR_CTS_buf, sizeof(MDR_CTS_buf));
-	    MDR_CTS.timeout = 100;
-	    if (!pb_encode(&MDR_CTS_ostream, m2s_MDR_res_CTS_fields, &MDR_CTS)) {
-		hs_sts = HS_FAILED;
-#ifdef DEBUG_ENABLE
-		goto __MDR_CTS_ENC_ERROR;
-	    __MDR_CTS_ENC_ERROR_END:
-		__asm__("nop");
-#endif
-	    }
-	    else {
-#ifdef TESTING_ENABLE
-		MDR_CTS_size = MDR_CTS_ostream.bytes_written;
-		goto __HS_MDR_CTS_TESTING;
-	    __HS_MDR_CTS_TESTING_END:
-		__asm__("nop");
-#endif
-		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr,
-					    (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {
-		    hs_sts = HS_FAILED;
-#ifdef DEBUG_ENABLE
-		    goto __HS_CTS_I2C_ERROR;
-		__HS_CTS_I2C_ERROR_END:
-		    __asm__("nop");
-#endif
-		}
-		else {
-		    hs_sts = HS_MDR_MDR;
-		    free(MDR_CTS_buf);
-		}
-	    }
-	    break;
-	}
-	case (HS_MDR_MDR):
-	{
-	    MDR_buf = (uint8_t*)malloc(MDR_len);
-	    AF_error_counter = 0;
-	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
-					  (uint8_t*)MDR_buf, MDR_len, 1000) != HAL_OK) {
-		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
-		    hs_sts = HS_FAILED;
-#ifdef DEBUG_ENABLE
-		    goto __HS_MDR_I2C_ERROR;
-		__HS_MDR_I2C_ERROR_END:
-		    __asm__("nop");
-#endif
-		    break;
-		}
-		else if (++AF_error_counter > 1500) {
-		    hs_sts = HS_FAILED;
-		    break;
-		}
-	    }
-	    if (hs_sts != HS_FAILED) {
-#ifdef TESTING_ENABLE
-		goto __HS_MDR_MDR_TESTING;
-	    __HS_MDR_MDR_TESTING_END:
-		__asm__("nop");
-#endif
-		MDR_res_message.subscriptions.funcs.decode = decode_subscriptions_callback;
-		MDR_res_message.subscriptions.arg = (void*)dev_idx;
-		pb_istream_t MDR_res_stream = pb_istream_from_buffer(MDR_buf, MDR_len);
-		if (!pb_decode(&MDR_res_stream, s2m_MDR_response_fields, &MDR_res_message)) {
-#ifdef DEBUG_ENABLE
-		    goto __HS_MDR_DEC_ERROR;
-		__HS_MDR_DEC_ERROR_END:
-		    __asm__("nop");
-#endif
-		}
-		else {
-#ifdef TESTING_ENABLE
-		    goto __MDR_DEC_TESTING;
-		__MDR_DEC_TESTING_END:
-		    __asm__("nop");
-#endif
-		    hs_sts = HS_REGISTERED;
-		}	       		
-	    }
-	    break;
-	}
-	}
-
-    }
-
-#ifdef TESTING_ENABLE
-    {
-	goto __TESTING_BLOCK_END;
-    __HS_IDLE_TESTING:
-	sprintf((char*)debug_buf, "MDR req length: %d\r\n", MDR_req_size);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	uint8_t bufbuf[] = "MDR req buffer: ";
-	HAL_UART_Transmit(&huart1, bufbuf, sizeof(bufbuf), 100);
-	for(int x=0; x<MDR_req_size; x++) {
-	    sprintf((char*)debug_buf+x, "%x", MDR_req_buf[x]);
-	}
-	HAL_UART_Transmit(&huart1, debug_buf, MDR_req_size, 100);
-	HAL_UART_Transmit(&huart1, term, 2, 100);
-	memset(debug_buf, 0, 128);
-	goto __HS_IDLE_TESTING_END;
-    __HS_MDR_ACK_TESTING:
-	sprintf((char*)debug_buf, "Got MDR message length: %ld\r\n", MDR_ACK.MDR_res_length);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __HS_MDR_ACK_TESTING_END;
-    __HS_MDR_CTS_TESTING:
-	sprintf((char*)debug_buf, "CTS size: %d\r\n", MDR_CTS_size);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	uint8_t ctsbuf[] = "\tCTS buffer: ";
-	for(int x=0; x<2; x++) {
-	    sprintf((char*)debug_buf+x, "%x", MDR_CTS_buf[x]);
-	}
-	HAL_UART_Transmit(&huart1, ctsbuf, sizeof(ctsbuf), 100);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	HAL_UART_Transmit(&huart1, term, 2, 100);
-	memset(debug_buf, 0, 128);
-	goto __HS_MDR_CTS_TESTING_END;
-    __HS_MDR_MDR_TESTING:
-	for (int x=0; x<MDR_len; x++) {
-	    sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);
-	}
-	uint8_t mdrbuf[] = "Got MDR: ";
-	HAL_UART_Transmit(&huart1, mdrbuf, sizeof(mdrbuf), 100);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);  
-	HAL_UART_Transmit(&huart1, term, 2, 100);
-	memset(debug_buf, 0, 128);
-	goto __HS_MDR_MDR_TESTING_END;
-    __MDR_DEC_TESTING:
-	sprintf((char*)debug_buf, "MDR Decode success\r\n\tFirst subscibed module: %d\r\n",
-		subs_info[dev_idx]->module_ids[1]);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __MDR_DEC_TESTING_END;
-    }
-__TESTING_BLOCK_END:
-    __asm__("nop");
-#endif
-
-#ifdef DEBUG_ENABLE
-    {
-	goto __DEBUG_BLOCK_END;
-    __MDR_REQ_ENC_FAIL:
-	sprintf((char*)debug_buf, "MDR reqest encoding error\r\n");
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __MDR_REQ_ENC_FAIL_END;
-    __HS_MDR_REQ_I2C_ERROR:
-	sprintf((char*)debug_buf, "Unable to send MDR request. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __HS_MDR_REQ_I2C_ERROR_END;
-    __HS_MDR_ACK_I2C_ERROR:
-	sprintf((char*)debug_buf, "Unable to get MDR ACK. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __HS_MDR_ACK_I2C_ERROR_END;
-    __MDR_ACK_DEC_ERROR:
-	sprintf((char*)debug_buf, "MDR ACK decoding error\r\n");
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __MDR_ACK_DEC_ERROR_END;
-    __MDR_CTS_ENC_ERROR:	
-	sprintf((char*)debug_buf, "MDR encoding error\r\n");
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __MDR_CTS_ENC_ERROR_END;
-    __HS_CTS_I2C_ERROR:
-	sprintf((char*)debug_buf, "Unable to send MDR CTS. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __HS_CTS_I2C_ERROR_END;
-    __HS_MDR_I2C_ERROR:
-	sprintf((char*)debug_buf, "Unable to get MDR. I2C error: %ld\n\tError counter: %ld\r\n", HAL_I2C_GetError(&hi2c1), AF_error_counter);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __HS_MDR_I2C_ERROR_END;	
-    }
-__HS_MDR_DEC_ERROR:
-    sprintf((char*)debug_buf, "MDR decode error\r\n");
-    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-    memset(debug_buf, 0, 128);
-    goto __HS_MDR_DEC_ERROR_END;
-__DEBUG_BLOCK_END:
-    __asm__("nop");
-#endif
-    
-    return hs_sts;
-}
-
-bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
-{
-    _subscriptions subs;
-    int *subs_idx = (int*)args;
-
-    /* Check is storage is allocated; if not, allocate it */
-    if ((GET_BIT_FROM_IDX(allocated, *subs_idx)) == 0) {
-	subs_info[*subs_idx] = (subscription_info_t*)malloc(sizeof(subscription_info_t));
-	SET_BIT_FROM_IDX(allocated, *subs_idx);
-	subs_info[*subs_idx]->mod_idx = subs_info[*subs_idx]->entity_idx =
-	    subs_info[*subs_idx]->class_idx = subs_info[*subs_idx]->i2c_idx = 0;
-    }
-    
-    if(!pb_decode(istream, _subscriptions_fields, &subs))
-    	return false;
-
-    /* Parse all fields if they're included */
-    if (subs.has_module_id)
-	subs_info[*subs_idx]->module_ids[subs_info[*subs_idx]->mod_idx++] =
-	    subs.module_id;
-    if (subs.has_entity_id)
-	subs_info[*subs_idx]->entity_ids[subs_info[*subs_idx]->entity_idx++] =
-	    subs.entity_id;
-    if (subs.has_module_class)
-	subs_info[*subs_idx]->module_class[subs_info[*subs_idx]->class_idx++] =
-	    subs.module_class;
-    if (subs.has_i2c_address)
-	subs_info[*subs_idx]->i2c_address[subs_info[*subs_idx]->i2c_idx++] =
-	    subs.i2c_address;
-    return true;
-}
-
-bool todo_hs_or_not_todo_hs(uint8_t i2c_addr)
-{
-    uint8_t device_idx = GET_IDX_FROM_ADDR(i2c_addr);
-    state_t device_curr_state = dev_sts[device_idx];
-    bool do_hs = false;
-    switch(device_curr_state) {
-    case NO_HS:
-    case CONNECTED:
-    case FAILED:
-    case OFFLINE:	
-	do_hs = true;
-	break;
-    case REGISTERED:
-    case NO_DATA:
-	do_hs = false;
-	break;	
-    }
-    return do_hs;
-}
-
-state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status)
-{
-    state_t device_state = OFFLINE;
-    switch(hs_status) {
-    case IDLE:
-    case HS_FAILED:
-	device_state = OFFLINE;
-	break;
-    case HS_MDR_ACK:
-    case HS_MDR_CTS:
-    case HS_MDR_MDR:
-	device_state = FAILED;
-	break;
-    case HS_REGISTERED:
-	device_state = REGISTERED;
-	break;    
-    }
-    return device_state;
-}
-
-bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
-{
-    if(ostream!=NULL && field->tag == s2m_MDR_response_subscriptions_tag) {
-	for (int x=0; x<2; x++) {
-	    _subscriptions subs;
-	    subs.module_id = x+10*x;
-	    subs.i2c_address = x+1;
-	    subs.has_entity_id=false;
-	    subs.has_module_class=false;
-	    subs.has_module_id=true;
-	    subs.has_i2c_address=true;
-	    if(!pb_encode_tag_for_field(ostream, field)){
-		printf("ERR1\n");
-		return false;
-	    }
-	    if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)){
-		printf("ERR2\n");
-		return false;
-	    }
-	}
-    }
-    else{
-	return false;
-    }
-    return true;
-}
-
-/**
- * @brief System Clock Configuration
- * @retval None
- */
-void SystemClock_Config(void)
-{
-    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
-    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
-
-    /** Configure the main internal regulator output voltage 
-     */
-    __HAL_RCC_PWR_CLK_ENABLE();
-    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);
-    /** Initializes the CPU, AHB and APB busses clocks 
-     */
-    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
-    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
-    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
-    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
-    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
-    {
-	Error_Handler();
-    }
-    /** Initializes the CPU, AHB and APB busses clocks 
-     */
-    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
-	|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
-    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
-    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
-    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
-    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
-
-    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
-    {
-	Error_Handler();
-    }
-}
-
-/**
- * @brief I2C1 Initialization Function
- * @param None
- * @retval None
- */
-static void MX_I2C1_Init(void)
-{
-    hi2c1.Instance = I2C1;
-    hi2c1.Init.ClockSpeed = 100000;
-    hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
-    hi2c1.Init.OwnAddress1 = I2C_ADDRESS;
-    hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
-    hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
-    hi2c1.Init.OwnAddress2 = 0xFF;
-    hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
-    hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
-    if (HAL_I2C_Init(&hi2c1) != HAL_OK)
-    {
-	Error_Handler();
-    }
-
-}
-
-/**
- * @brief USART1 Initialization Function
- * @param None
- * @retval None
- */
-static void MX_USART1_UART_Init(void)
-{
-    huart1.Instance = USART1;
-    huart1.Init.BaudRate = 9600;
-    huart1.Init.WordLength = UART_WORDLENGTH_8B;
-    huart1.Init.StopBits = UART_STOPBITS_1;
-    huart1.Init.Parity = UART_PARITY_NONE;
-    huart1.Init.Mode = UART_MODE_TX_RX;
-    huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
-    huart1.Init.OverSampling = UART_OVERSAMPLING_16;
-    if (HAL_UART_Init(&huart1) != HAL_OK)
-    {
-	Error_Handler();
-    }
-
-}
-
-/**
- * @brief GPIO Initialization Function
- * @param None
- * @retval None
- */
-static void MX_GPIO_Init(void)
-{
-    GPIO_InitTypeDef GPIO_InitStruct = {0};
-
-    /* GPIO Ports Clock Enable */
-    __HAL_RCC_GPIOC_CLK_ENABLE();
-    __HAL_RCC_GPIOH_CLK_ENABLE();
-    __HAL_RCC_GPIOA_CLK_ENABLE();
-    __HAL_RCC_GPIOB_CLK_ENABLE();
-
-    /*Configure GPIO pin Output Level */
-    HAL_GPIO_WritePin(led_GPIO_Port, led_Pin, GPIO_PIN_RESET);
-
-    /*Configure GPIO pin : led_Pin */
-    GPIO_InitStruct.Pin = led_Pin;
-    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-    HAL_GPIO_Init(led_GPIO_Port, &GPIO_InitStruct);
-
-}
-
-/**
- * @brief  This function is executed in case of error occurrence.
- * @retval None
- */
-void Error_Handler(void)
-{
-    /* USER CODE BEGIN Error_Handler_Debug */
-    /* User can add his own implementation to report the HAL error return state */
-    while (1) {
-	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
-	HAL_Delay(1000);
-    }
-    /* USER CODE END Error_Handler_Debug */
-}
-
-#ifdef  USE_FULL_ASSERT
-/**
- * @brief  Reports the name of the source file and the source line number
- *         where the assert_param error has occurred.
- * @param  file: pointer to the source file name
- * @param  line: assert_param error line source number
- * @retval None
- */
-void assert_failed(uint8_t *file, uint32_t line)
-{ 
-    /* USER CODE BEGIN 6 */
-    /* User can add his own implementation to report the file name and line number,
-       tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
-    /* USER CODE END 6 */
-}
-#endif /* USE_FULL_ASSERT */
diff --git a/src/main.c b/src/main.c
index 760e8eb..9a98fd4 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,670 +1,797 @@
-/* USER CODE BEGIN Header */

-/**

-******************************************************************************

-* @file           : main.c

-* @brief          : Main program body

-******************************************************************************

-* @attention

-*

-* <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.

-* All rights reserved.</center></h2>

-*

-* This software component is licensed by ST under BSD 3-Clause license,

-* the "License"; You may not use this file except in compliance with the

-* License. You may obtain a copy of the License at:

-*                        opensource.org/licenses/BSD-3-Clause

-*

-******************************************************************************

-*/

-/* USER CODE END Header */

-

-/* Includes ------------------------------------------------------------------*/

-#include "main.h"

-

-/* Private includes ----------------------------------------------------------*/

-/* USER CODE BEGIN Includes */

-#include <stdio.h>

-#include <pb_encode.h>

-#include <pb_decode.h>

-#include "handshake.pb.h"

-#include "devices.h"

-#include "config.h"

-/* USER CODE END Includes */

-

-/* Private typedef -----------------------------------------------------------*/

-/* USER CODE BEGIN PTD */

-

-/* USER CODE END PTD */

-

-/* Private define ------------------------------------------------------------*/

-/* USER CODE BEGIN PD */

-/* USER CODE END PD */

-

-/* Private macro -------------------------------------------------------------*/

-/* USER CODE BEGIN PM */

-#define device_MDR s2m_MDR_response

-#define GET_IDX_FROM_ADDR(i2c_addr) i2c_addr-1

-#define GET_BIT_FROM_IDX(a, b) a[b>>5]&(1<<(b%32))

-#define SET_BIT_FROM_IDX(a, b) a[b>>5]|=(1<<(b%32))

-#define COUNTOF(__BUFFER__)   (sizeof(__BUFFER__) / sizeof(*(__BUFFER__)))

-

-#define I2C_ADDRESS 0x05

-

-/* Macro to toggle between master and slave firmware */

-/* #define MASTER */

-/* USER CODE END PM */

-

-/* Private variables ---------------------------------------------------------*/

-I2C_HandleTypeDef hi2c1;

-

-UART_HandleTypeDef huart1;

-

-/* USER CODE BEGIN PV */

-_subscriptions gsubs[10];

-int subs_idx = 0;

-/* USER CODE END PV */

-

-/* Private function prototypes -----------------------------------------------*/

-void SystemClock_Config(void);

-static void MX_GPIO_Init(void);

-static void MX_I2C1_Init(void);

-static void MX_USART1_UART_Init(void);

-/* USER CODE BEGIN PFP */

-bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);

-bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args);

-/* USER CODE END PFP */

-

-/* Private user code ---------------------------------------------------------*/

-/* USER CODE BEGIN 0 */

-

-/* USER CODE END 0 */

-

-/**

- * @brief  The application entry point.

- * @retval int

- */

-int main(void)

-{

-    /* USER CODE BEGIN 1 */

-

-    /* USER CODE END 1 */

-  

-

-    /* MCU Configuration--------------------------------------------------------*/

-

-    /* Reset of all peripherals, Initializes the Flash interface and the Systick. */

-    HAL_Init();

-

-    /* USER CODE BEGIN Init */

-

-    /* USER CODE END Init */

-

-    /* Configure the system clock */

-    SystemClock_Config();

-

-    /* USER CODE BEGIN SysInit */

-

-    /* USER CODE END SysInit */

-

-    /* Initialize all configured peripherals */

-    MX_GPIO_Init();

-    MX_I2C1_Init();

-    MX_USART1_UART_Init();

-    /* USER CODE BEGIN 2 */

-#ifdef TESTING_ENABLE

-#ifdef MASTER

-    uint8_t reset_string[] = "\r\n\n==========MASTER RESET=========\r\n\n";

-    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);

-#else

-    uint8_t reset_string[] = "\r\n\n==========SLAVE RESET=========\r\n\n";

-    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);

-#endif /* MASTER */

-#endif /* TESTING_ENABLE */

-	

-#ifdef MASTER

-	uint8_t MDR_req_buf[64], debug_buf[128] = {0}, term[]="\r\n";

-	m2s_MDR_request MDR_req_message;

-	pb_ostream_t MDR_req_stream = pb_ostream_from_buffer(MDR_req_buf, sizeof(MDR_req_buf));

-	MDR_req_message.record_type = 7; /* Placeholder for default record type */

-	if(!pb_encode(&MDR_req_stream, m2s_MDR_request_fields, &MDR_req_message)) {

-#ifdef DEBUG_ENABLE

-	    uint8_t err_buf[] = "MDR reqest encoding error\r\n";

-	    HAL_UART_Transmit(&huart1, err_buf, sizeof(err_buf), 100);

-#endif /* DEBUG_ENABLE */

-	    Error_Handler();

-	}

-	uint16_t enc_size = MDR_req_stream.bytes_written;

-#ifdef TESTING_ENABLE

-	sprintf((char*)debug_buf, "Encoded length: %d\r\n", enc_size);

-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	memset(debug_buf, 0, 128);

-	uint8_t bufbuf[] = "MDR req buffer: ";	  

-	HAL_UART_Transmit(&huart1, bufbuf, sizeof(bufbuf), 100);

-	for(int x=0; x<enc_size; x++) {

-	    sprintf((char*)debug_buf+x, "%x", MDR_req_buf[x]);

-	}

-	HAL_UART_Transmit(&huart1, debug_buf, enc_size, 100);

-	HAL_UART_Transmit(&huart1, term, 2, 100);

-	memset(debug_buf, 0, 128);

-#endif /* TESTING_ENABLE */

-

-	if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)I2C_ADDRESS, (uint8_t*)MDR_req_buf, 2, 10000) != HAL_OK) {

-#ifdef DEBUG_ENABLE

-	    sprintf((char*)debug_buf, "Unable to send MDR request. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));

-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	    memset(debug_buf, 0, 128);

-#endif /* DEBUG_ENABLE */

-	}

-	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);

-

-	uint8_t MDR_ACK_buf[8] = {0};

-	HAL_Delay(100);

-

-	uint32_t AF_error_counter = 0;

-	while (HAL_I2C_Master_Receive(&hi2c1, (uint8_t)I2C_ADDRESS, (uint8_t*)MDR_ACK_buf, s2m_MDR_req_ACK_size, 100) != HAL_OK) {

-	    if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {

-#ifdef DEBUG_ENABLE

-		sprintf((char*)debug_buf, "Unable to get MDR ACK. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));

-		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-		memset(debug_buf, 0, 128);

-#endif /* DEBUG_ENABLE */

-		break;

-	    }

-	    if (++AF_error_counter > 1000) {

-		break;

-	    }

-	}

-	

-#ifdef TESTING_ENABLE

-	uint8_t bufbuf2[] = "MDR ACK buffer: ";

-	HAL_UART_Transmit(&huart1, bufbuf2, sizeof(bufbuf2), 100);

-	for (int x=0; x<2; x++) {

-	    sprintf((char*)debug_buf+x, "%x|", MDR_ACK_buf[x]);

-	}

-	HAL_UART_Transmit(&huart1, debug_buf, 2, 100);  

-	HAL_UART_Transmit(&huart1, term, 2, 100);

-	memset(debug_buf, 0, 128);

-#endif /* TESTING_ENABLE */

-

-	uint32_t MDR_len = 0;

-	pb_istream_t MDR_ACK_istream = pb_istream_from_buffer(MDR_ACK_buf, 2);

-	s2m_MDR_req_ACK MDR_ACK;

-	if (!pb_decode(&MDR_ACK_istream, s2m_MDR_req_ACK_fields, &MDR_ACK)) {

-#ifdef DEBUG_ENABLE

-	    uint8_t errbuf[] = "MDR ACK decoding error\r\n";

-	    HAL_UART_Transmit(&huart1, errbuf, sizeof(errbuf), 100);

-#endif /* DEBUG_ENABLE */

-	}

-	else {

-	    MDR_len = MDR_ACK.MDR_res_length;

-#ifdef TESTING_ENABLE

-	    sprintf((char*)debug_buf, "Got MDR message length: %ld\r\n", MDR_ACK.MDR_res_length);

-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	    memset(debug_buf, 0, 128);

-#endif /* TESTING_ENABLE */

-	}

-

-	uint8_t MDR_CTS_buf[8] = {0};

-	m2s_MDR_res_CTS MDR_CTS;

-	pb_ostream_t MDR_CTS_ostream = pb_ostream_from_buffer(MDR_CTS_buf, sizeof(MDR_CTS_buf));

-	

-	MDR_CTS.timeout = 100;

-	if (!pb_encode(&MDR_CTS_ostream, m2s_MDR_res_CTS_fields, &MDR_CTS)) {

-#ifdef DEBUG_ENABLE

-	    sprintf((char*)debug_buf, "MDR encoding error\r\n");

-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	    memset(debug_buf, 0, 128);

-#endif

-	}

-#ifdef TESTING_ENABLE

-	sprintf((char*)debug_buf, "CTS size: %d\r\n", MDR_CTS_ostream.bytes_written);

-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	memset(debug_buf, 0, 128);

-	uint8_t ctsbuf[] = "\tCTS buffer: ";

-	for(int x=0; x<2; x++) {

-	    sprintf((char*)debug_buf+x, "%x", MDR_CTS_buf[x]);

-	}

-	HAL_UART_Transmit(&huart1, ctsbuf, sizeof(ctsbuf), 100);

-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	HAL_UART_Transmit(&huart1, term, 2, 100);

-	memset(debug_buf, 0, 128);

-#endif /* TESTING_ENABLE */

-	

-	if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)I2C_ADDRESS, (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {

-#ifdef DEBUG_ENABLE

-	    sprintf((char*)debug_buf, "Unable to send MDR CTS. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));

-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	    memset(debug_buf, 0, 128);

-#endif /* DEBUG_ENABLE */

-	}

-	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);

-

-	uint8_t MDR_buf[256];

-	AF_error_counter = 0;

-	while (HAL_I2C_Master_Receive(&hi2c1, (uint8_t)I2C_ADDRESS, (uint8_t*)MDR_buf, MDR_len, 1000) != HAL_OK) {

-	    if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {

-#ifdef DEBUG_ENABLE

-		sprintf((char*)debug_buf, "Unable to get MDR. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));

-		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-		memset(debug_buf, 0, 128);

-#endif /* DEBUG_ENABLE */

-		break;

-	    }

-	    AF_error_counter++;

-	    /* if (++AF_error_counter > 1000) { */

-	    /* 	break; */

-	    /* } */

-	}

-	

-#ifdef TESTING_ENABLE

-	sprintf((char*)debug_buf, "MDR trials: %ld\r\n", AF_error_counter);

-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	memset(debug_buf, 0, 128);

-#endif

-	

-#ifdef TESTING_ENABLE

-	uint8_t mdrbuf[] = "Got MDR: ";

-	for (int x=0; x<MDR_len; x++) {

-	    sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);

-	}

-	HAL_UART_Transmit(&huart1, mdrbuf, sizeof(mdrbuf), 100);

-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);  

-	HAL_UART_Transmit(&huart1, term, 2, 100);

-	memset(debug_buf, 0, 128);

-#endif /* TESTING_ENABLE */

-

-	s2m_MDR_response MDR_res;

-	MDR_res.subscriptions.funcs.decode=decode_subscriptions_callback;

-	pb_istream_t MDR_istream = pb_istream_from_buffer(MDR_buf, MDR_len);

-	// Decode the mdr

-	if (!pb_decode(&MDR_istream, s2m_MDR_response_fields, &MDR_res)) {

-	    sprintf((char*)debug_buf, "MDR decode error\r\n");

-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	    memset(debug_buf, 0, 128);

-	}

-	else {

-	    sprintf((char*)debug_buf, "MDR:\n\tVersion: %f\nID: %ld\nSub 1 ID: %ld\r\n", MDR_res.MDR_version, MDR_res.module_id, gsubs[1].module_id);

-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	    memset(debug_buf, 0, 128);

-	}

-	

-#else /* MASTER */

-	uint8_t MDR_buf[128], debug_buf[128], term[]="\r\n";

-	size_t MDR_enc_size;

-	s2m_MDR_response res;

-	res.MDR_version=1.1;

-	res.module_id = 1;

-	res.module_class=1;

-	res.entity_id=32;

-

-	res.subscriptions.funcs.encode=encode_subscription_callback;

-	pb_ostream_t ostream = pb_ostream_from_buffer(MDR_buf, sizeof(MDR_buf));

-	if(!pb_encode(&ostream, s2m_MDR_response_fields, &res)){

-#ifdef DEBUG_ENABLE

-	    uint8_t err_buf[] = "MDR encoding error\r\n";

-	    HAL_UART_Transmit(&huart1, err_buf, sizeof(err_buf), 100);

-#endif

-	    while(1) {

-		HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);

-		HAL_Delay(500);

-	    }

-	}

-	MDR_enc_size = ostream.bytes_written;

-#ifdef TESTING_ENABLE	    

-	sprintf((char*)debug_buf, "MDR Encoded size: %d\r\n", MDR_enc_size);

-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	memset(debug_buf, 0, 128);

-	

-	uint8_t bufbuf[] = "MDR Buffer: ";	  

-	HAL_UART_Transmit(&huart1, bufbuf, sizeof(bufbuf), 100);

-	for(int x=0; x<MDR_enc_size; x++) {

-	    sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);

-	}

-	HAL_UART_Transmit(&huart1, debug_buf, MDR_enc_size, 100);  

-	HAL_UART_Transmit(&huart1, term, 2, 100);

-	memset(debug_buf, 0, 128);

-#endif

-	    

-	uint8_t MDR_ACK_buf[8] = {0};

-	s2m_MDR_req_ACK ack;

-	ack.MDR_res_length = MDR_enc_size;

-	pb_ostream_t MDR_ack_ostream = pb_ostream_from_buffer(MDR_ACK_buf,

-							      sizeof(MDR_ACK_buf));

-	if(!pb_encode(&MDR_ack_ostream, s2m_MDR_req_ACK_fields, &ack)) {

-#ifdef DEBUG_ENABLE

-	    uint8_t errbuf[] = "MDR ACK encoding error\r\n";

-	    HAL_UART_Transmit(&huart1, errbuf, sizeof(errbuf), 100);

-#endif /* DEBUG_ENABLE */

-	    while(1) {

-		HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);

-		HAL_Delay(500);

-	    }

-	}

-	size_t MDR_ack_size = MDR_ack_ostream.bytes_written;

-#ifdef TESTING_ENABLE

-	sprintf((char*)debug_buf, "MDR ACK Encoded size: %d\r\n", MDR_ack_size);

-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	memset(debug_buf, 0, 128);

-#endif /* TESTING_ENABLE */

-

-	uint8_t MDR_req_buf[8]; 

-	m2s_MDR_request MDR_req;

-	

-	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)MDR_req_buf, 2, 10000) != HAL_OK) {

-#ifdef DEBUG_ENABLE

-	    uint8_t debug_buf[] = "Failed to get MDR req\r\n";

-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-#endif /* DEBUG_ENABLE */

-	}

-

-#ifdef TESTING_ENABLE

-	uint8_t bufbuf2[] = "MDR ACK buffer: ";

-	HAL_UART_Transmit(&huart1, bufbuf2, sizeof(bufbuf2), 100);

-	for(int x=0; x<2; x++) {

-	    sprintf((char*)debug_buf+x, "%x", MDR_ACK_buf[x]);

-	}

-	HAL_UART_Transmit(&huart1, debug_buf, MDR_enc_size, 100);  

-	HAL_UART_Transmit(&huart1, term, 2, 100);

-	memset(debug_buf, 0, 128);

-#endif

-	

-	pb_istream_t MDR_req_istream = pb_istream_from_buffer(MDR_req_buf, 2);

-	if(!pb_decode(&MDR_req_istream, m2s_MDR_request_fields, &MDR_req)) {

-#ifdef DEBUG_ENABLE

-	    uint8_t errbuf[] = "MDR request decoding error\r\n";

-	    HAL_UART_Transmit(&huart1, errbuf, sizeof(errbuf), 100);

-#endif /* DEBUG_ENABLE */

-	}

-	

-#ifdef TESTING_ENABLE

-	sprintf((char*)debug_buf, "Got requested record type: %ld\r\n", MDR_req.record_type);

-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	memset(debug_buf, 0, 128);

-#endif /* TESTING_ENABLE */

-

-	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);

-

-	if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_ACK_buf, s2m_MDR_req_ACK_size, 10000) != HAL_OK) {

-#ifdef DEBUG_ENABLE

-	    sprintf((char*)debug_buf, "Unable to send MDR ACK. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));

-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	    memset(debug_buf, 0, 128);

-#endif /* DEBUG_ENABLE */

-	}

-

-	uint8_t MDR_CTS_buf[8];

-	m2s_MDR_res_CTS MDR_CTS;

-	

-	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {

-#ifdef DEBUG_ENABLE

-	    sprintf((char*)debug_buf, "Failed to get MDR CTS\r\n");

-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	    memset(debug_buf, 0, 128);

-#endif /* DEBUG_ENABLE */

-	}

-#ifdef TESTING_ENABLE

-	uint8_t ctsbuf[] = "Got CTS buffer: ";

-	for(int x=0; x<2; x++) {

-	    sprintf((char*)debug_buf+x, "%x", MDR_CTS_buf[x]);

-	}

-	HAL_UART_Transmit(&huart1, ctsbuf, sizeof(ctsbuf), 100);

-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	HAL_UART_Transmit(&huart1, term, 2, 100);

-	memset(debug_buf, 0, 128);

-#endif /* TESTING_ENABLE */

-	

-	pb_istream_t MDR_CTS_istream = pb_istream_from_buffer(MDR_CTS_buf, 2);

-	if (!pb_decode(&MDR_CTS_istream, m2s_MDR_res_CTS_fields, &MDR_CTS)) {

-#ifdef DEBUG_ENABLE

-	    sprintf((char*)debug_buf, "Failed to decode MDR CTS\r\n");

-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	    memset(debug_buf, 0, 128);

-#endif /* DEBUG_ENABLE */

-	    Error_Handler();

-	}

-

-	uint32_t MDR_timeout = MDR_CTS.timeout;

-	if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_buf, MDR_enc_size, 10000) !=

-	    HAL_OK) {

-#ifdef DEBUG_ENABLE

-	    sprintf((char*)debug_buf, "Unable to send MDR. I2C error: %ld\r\n",

-		    HAL_I2C_GetError(&hi2c1));

-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	    memset(debug_buf, 0, 128);

-#endif /* DEBUG_ENABLE */

-	}

-	else {

-	    sprintf((char*)debug_buf, "Successfully sent MDR\r\n");

-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-	    memset(debug_buf, 0, 128);

-	}

-	

-#endif /* MASTER */

-	

-	/* USER CODE END 2 */

-	/* Infinite loop */

-	/* USER CODE BEGIN WHILE */

-	while (1)

-	{

-	    /* USER CODE END WHILE */

-	    /* USER CODE BEGIN 3 */

-	}

-	/* USER CODE END 3 */

-}

-

-/**

- * @brief System Clock Configuration

- * @retval None

- */

-void SystemClock_Config(void)

-{

-    RCC_OscInitTypeDef RCC_OscInitStruct = {0};

-    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

-

-    /** Configure the main internal regulator output voltage 

-     */

-    __HAL_RCC_PWR_CLK_ENABLE();

-    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);

-    /** Initializes the CPU, AHB and APB busses clocks 

-     */

-    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;

-    RCC_OscInitStruct.HSIState = RCC_HSI_ON;

-    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;

-    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;

-    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)

-    {

-	Error_Handler();

-    }

-    /** Initializes the CPU, AHB and APB busses clocks 

-     */

-    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK

-	|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;

-    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;

-    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;

-    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;

-    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

-

-    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)

-    {

-	Error_Handler();

-    }

-}

-

-/**

- * @brief I2C1 Initialization Function

- * @param None

- * @retval None

- */

-static void MX_I2C1_Init(void)

-{

-

-    /* USER CODE BEGIN I2C1_Init 0 */

-

-    /* USER CODE END I2C1_Init 0 */

-

-    /* USER CODE BEGIN I2C1_Init 1 */

-

-    /* USER CODE END I2C1_Init 1 */

-    hi2c1.Instance = I2C1;

-    hi2c1.Init.ClockSpeed = 100000;

-    hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;

-    hi2c1.Init.OwnAddress1 = I2C_ADDRESS;

-    hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;

-    hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;

-    hi2c1.Init.OwnAddress2 = 0xFF;

-    hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;

-    hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;

-    if (HAL_I2C_Init(&hi2c1) != HAL_OK)

-    {

-	Error_Handler();

-    }

-    /* USER CODE BEGIN I2C1_Init 2 */

-

-    /* USER CODE END I2C1_Init 2 */

-

-}

-

-/**

- * @brief USART1 Initialization Function

- * @param None

- * @retval None

- */

-static void MX_USART1_UART_Init(void)

-{

-

-    /* USER CODE BEGIN USART1_Init 0 */

-

-    /* USER CODE END USART1_Init 0 */

-

-    /* USER CODE BEGIN USART1_Init 1 */

-

-    /* USER CODE END USART1_Init 1 */

-    huart1.Instance = USART1;

-    huart1.Init.BaudRate = 9600;

-    huart1.Init.WordLength = UART_WORDLENGTH_8B;

-    huart1.Init.StopBits = UART_STOPBITS_1;

-    huart1.Init.Parity = UART_PARITY_NONE;

-    huart1.Init.Mode = UART_MODE_TX_RX;

-    huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;

-    huart1.Init.OverSampling = UART_OVERSAMPLING_16;

-    if (HAL_UART_Init(&huart1) != HAL_OK)

-    {

-	Error_Handler();

-    }

-    /* USER CODE BEGIN USART1_Init 2 */

-

-    /* USER CODE END USART1_Init 2 */

-

-}

-

-/**

- * @brief GPIO Initialization Function

- * @param None

- * @retval None

- */

-static void MX_GPIO_Init(void)

-{

-    GPIO_InitTypeDef GPIO_InitStruct = {0};

-

-    /* GPIO Ports Clock Enable */

-    __HAL_RCC_GPIOC_CLK_ENABLE();

-    __HAL_RCC_GPIOH_CLK_ENABLE();

-    __HAL_RCC_GPIOA_CLK_ENABLE();

-    __HAL_RCC_GPIOB_CLK_ENABLE();

-

-    /*Configure GPIO pin Output Level */

-    HAL_GPIO_WritePin(led_GPIO_Port, led_Pin, GPIO_PIN_RESET);

-

-    /*Configure GPIO pin : led_Pin */

-    GPIO_InitStruct.Pin = led_Pin;

-    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;

-    GPIO_InitStruct.Pull = GPIO_NOPULL;

-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

-    HAL_GPIO_Init(led_GPIO_Port, &GPIO_InitStruct);

-

-}

-

-/* USER CODE BEGIN 4 */

-bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)

-{

-    if(ostream!=NULL && field->tag == s2m_MDR_response_subscriptions_tag) {

-	for (int x=0; x<2; x++) {

-	    _subscriptions subs;

-	    subs.module_id = x+10*x;

-	    subs.i2c_address = x+1;

-	    subs.has_entity_id=false;

-	    subs.has_module_class=false;

-	    subs.has_module_id=true;

-	    subs.has_i2c_address=true;

-	    if(!pb_encode_tag_for_field(ostream, field)){

-		printf("ERR1\n");

-		return false;

-	    }

-	    if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)){

-		printf("ERR2\n");

-		return false;

-	    }

-	}

-    }

-    else{

-	return false;

-    }

-    return true;

-}

-

-bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args)

-{

-    _subscriptions subs;

-    if(!pb_decode(istream, _subscriptions_fields, &subs)) {

-	return false;

-    }

-    else {

-	/* Parse all fields if they're included */

-	if (subs.has_module_id)

-	    gsubs[subs_idx].module_id = subs.module_id;

-	if (subs.has_entity_id)

-	    gsubs[subs_idx].entity_id = subs.entity_id;

-	if (subs.has_module_class)

-	    gsubs[subs_idx].module_class = subs.module_class;

-	if (subs.has_i2c_address)

-	    gsubs[subs_idx].i2c_address = subs.i2c_address;

-	subs_idx++;

-	subs_idx%=10;

-	return true;

-    }

-}

-/* USER CODE END 4 */

-

-/**

- * @brief  This function is executed in case of error occurrence.

- * @retval None

- */

-void Error_Handler(void)

-{

-    /* USER CODE BEGIN Error_Handler_Debug */

-    /* User can add his own implementation to report the HAL error return state */

-    while (1) {

-	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);

-	HAL_Delay(1000);

-    }

-    /* USER CODE END Error_Handler_Debug */

-}

-

-#ifdef  USE_FULL_ASSERT

-/**

- * @brief  Reports the name of the source file and the source line number

- *         where the assert_param error has occurred.

- * @param  file: pointer to the source file name

- * @param  line: assert_param error line source number

- * @retval None

- */

-void assert_failed(uint8_t *file, uint32_t line)

-{ 

-    /* USER CODE BEGIN 6 */

-    /* User can add his own implementation to report the file name and line number,

-       tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

-    /* USER CODE END 6 */

-}

-#endif /* USE_FULL_ASSERT */

-

-/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

+/* USER CODE BEGIN Header */
+/**
+******************************************************************************
+* @file           : main.c
+* @brief          : Main program body
+******************************************************************************
+* @attention
+*
+* 
+******************************************************************************
+*/
+
+/* Standard library includes */
+#include <stdio.h>
+
+/* Library includes */
+#include <pb_encode.h>
+#include <pb_decode.h>
+#include "handshake.pb.h"
+
+/* Project includes */
+#include "devices.h"
+#include "config.h"
+#include "main.h"
+
+/* Private Macros */
+#define device_MDR s2m_MDR_response
+#define GET_IDX_FROM_ADDR(i2c_addr) i2c_addr-1
+#define GET_BIT_FROM_IDX(a, b) a[b>>5]&(1<<(b%32))
+#define SET_BIT_FROM_IDX(a, b) a[b>>5]|=(1<<(b%32))
+#define COUNTOF(__BUFFER__)   (sizeof(__BUFFER__) / sizeof(*(__BUFFER__)))
+
+#define I2C_ADDRESS 0x05
+#define BUS_DEVICE_LIMIT 128
+
+/* Macro to toggle between master and slave firmware */
+/* #define MASTER */
+
+/* Private globals */
+I2C_HandleTypeDef hi2c1;
+UART_HandleTypeDef huart1;
+
+device_info_t *device_info[BUS_DEVICE_LIMIT] = {NULL};
+subscription_info_t* subs_info[BUS_DEVICE_LIMIT];
+uint32_t allocated[4]={0};
+uint8_t dev_sts[BUS_DEVICE_LIMIT] = {OFFLINE};
+
+/* Function prototypes */
+void SystemClock_Config(void);
+static void MX_GPIO_Init(void);
+static void MX_I2C1_Init(void);
+static void MX_USART1_UART_Init(void);
+
+bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
+hs_status_t handshake(uint32_t i2c_addr);
+bool todo_hs_or_not_todo_hs(uint8_t i2c_addr);
+state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status);
+bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
+
+
+/**
+ * @brief  The application entry point.
+ * @retval int
+ */
+int main(void)
+{ 
+    /* MCU Configuration */
+
+    /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
+    HAL_Init();
+
+    /* Configure the system clock */
+    SystemClock_Config();
+
+    /* Initialize all configured peripherals */
+    MX_GPIO_Init();
+    MX_I2C1_Init();
+    MX_USART1_UART_Init();
+
+#ifdef TESTING_ENABLE
+#ifdef MASTER
+    uint8_t reset_string[] = "\r\n\n==========MASTER RESET=========\r\n\n";
+    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
+#else
+    uint8_t reset_string[] = "\r\n\n==========SLAVE RESET=========\r\n\n";
+    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
+#endif /* MASTER */
+#endif /* TESTING_ENABLE */
+
+#ifdef MASTER
+    hs_status_t hs_status;
+    for (int curr_addr=0; curr_addr < 10; curr_addr++) {
+	if (todo_hs_or_not_todo_hs(curr_addr)) {
+	    hs_status = handshake(curr_addr);
+	    dev_sts[GET_IDX_FROM_ADDR(curr_addr)] = get_state_from_hs_status(curr_addr, hs_status);
+	}
+    }
+
+#else /* Slave code*/
+    {
+	uint8_t MDR_buf[128], debug_buf[128], term[]="\r\n";
+	size_t MDR_enc_size;
+	s2m_MDR_response res;
+	res.MDR_version=1.1;
+	res.module_id = 1;
+	res.module_class=1;
+	res.entity_id=32;
+
+	res.subscriptions.funcs.encode=encode_subscription_callback;
+	pb_ostream_t ostream = pb_ostream_from_buffer(MDR_buf, sizeof(MDR_buf));
+	if(!pb_encode(&ostream, s2m_MDR_response_fields, &res)){
+#ifdef DEBUG_ENABLE
+	    uint8_t err_buf[] = "MDR encoding error\r\n";
+	    HAL_UART_Transmit(&huart1, err_buf, sizeof(err_buf), 100);
+#endif
+	    while(1) {
+		HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
+		HAL_Delay(500);
+	    }
+	}
+	MDR_enc_size = ostream.bytes_written;
+#ifdef TESTING_ENABLE	    
+	sprintf((char*)debug_buf, "MDR Encoded size: %d\r\n", MDR_enc_size);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	
+	uint8_t bufbuf[] = "MDR Buffer: ";	  
+	HAL_UART_Transmit(&huart1, bufbuf, sizeof(bufbuf), 100);
+	for(int x=0; x<MDR_enc_size; x++) {
+	    sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);
+	}
+	HAL_UART_Transmit(&huart1, debug_buf, MDR_enc_size, 100);  
+	HAL_UART_Transmit(&huart1, term, 2, 100);
+	memset(debug_buf, 0, 128);
+#endif
+	    
+	uint8_t MDR_ACK_buf[8] = {0};
+	s2m_MDR_req_ACK ack;
+	ack.MDR_res_length = MDR_enc_size;
+	pb_ostream_t MDR_ack_ostream = pb_ostream_from_buffer(MDR_ACK_buf,
+							      sizeof(MDR_ACK_buf));
+	if(!pb_encode(&MDR_ack_ostream, s2m_MDR_req_ACK_fields, &ack)) {
+#ifdef DEBUG_ENABLE
+	    uint8_t errbuf[] = "MDR ACK encoding error\r\n";
+	    HAL_UART_Transmit(&huart1, errbuf, sizeof(errbuf), 100);
+#endif /* DEBUG_ENABLE */
+	    while(1) {
+		HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
+		HAL_Delay(500);
+	    }
+	}
+	size_t MDR_ack_size = MDR_ack_ostream.bytes_written;
+#ifdef TESTING_ENABLE
+	sprintf((char*)debug_buf, "MDR ACK Encoded size: %d\r\n", MDR_ack_size);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+#endif /* TESTING_ENABLE */
+
+	uint8_t MDR_req_buf[8]; 
+	m2s_MDR_request MDR_req;
+	
+	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)MDR_req_buf, 2, 10000) != HAL_OK) {
+#ifdef DEBUG_ENABLE
+	    uint8_t debug_buf[] = "Failed to get MDR req\r\n";
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+#endif /* DEBUG_ENABLE */
+	}
+
+#ifdef TESTING_ENABLE
+	uint8_t bufbuf2[] = "MDR ACK buffer: ";
+	HAL_UART_Transmit(&huart1, bufbuf2, sizeof(bufbuf2), 100);
+	for(int x=0; x<2; x++) {
+	    sprintf((char*)debug_buf+x, "%x", MDR_ACK_buf[x]);
+	}
+	HAL_UART_Transmit(&huart1, debug_buf, MDR_enc_size, 100);  
+	HAL_UART_Transmit(&huart1, term, 2, 100);
+	memset(debug_buf, 0, 128);
+#endif
+	
+	pb_istream_t MDR_req_istream = pb_istream_from_buffer(MDR_req_buf, 2);
+	if(!pb_decode(&MDR_req_istream, m2s_MDR_request_fields, &MDR_req)) {
+#ifdef DEBUG_ENABLE
+	    uint8_t errbuf[] = "MDR request decoding error\r\n";
+	    HAL_UART_Transmit(&huart1, errbuf, sizeof(errbuf), 100);
+#endif /* DEBUG_ENABLE */
+	}
+	
+#ifdef TESTING_ENABLE
+	sprintf((char*)debug_buf, "Got requested record type: %ld\r\n", MDR_req.record_type);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+#endif /* TESTING_ENABLE */
+
+	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
+
+	if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_ACK_buf, s2m_MDR_req_ACK_size, 10000) != HAL_OK) {
+#ifdef DEBUG_ENABLE
+	    sprintf((char*)debug_buf, "Unable to send MDR ACK. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);
+#endif /* DEBUG_ENABLE */
+	}
+
+	uint8_t MDR_CTS_buf[8];
+	m2s_MDR_res_CTS MDR_CTS;
+	
+	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {
+#ifdef DEBUG_ENABLE
+	    sprintf((char*)debug_buf, "Failed to get MDR CTS\r\n");
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);
+#endif /* DEBUG_ENABLE */
+	}
+#ifdef TESTING_ENABLE
+	uint8_t ctsbuf[] = "Got CTS buffer: ";
+	for(int x=0; x<2; x++) {
+	    sprintf((char*)debug_buf+x, "%x", MDR_CTS_buf[x]);
+	}
+	HAL_UART_Transmit(&huart1, ctsbuf, sizeof(ctsbuf), 100);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	HAL_UART_Transmit(&huart1, term, 2, 100);
+	memset(debug_buf, 0, 128);
+#endif /* TESTING_ENABLE */
+	
+	pb_istream_t MDR_CTS_istream = pb_istream_from_buffer(MDR_CTS_buf, 2);
+	if (!pb_decode(&MDR_CTS_istream, m2s_MDR_res_CTS_fields, &MDR_CTS)) {
+#ifdef DEBUG_ENABLE
+	    sprintf((char*)debug_buf, "Failed to decode MDR CTS\r\n");
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);
+#endif /* DEBUG_ENABLE */
+	    Error_Handler();
+	}
+
+	uint32_t MDR_timeout = MDR_CTS.timeout;
+	if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_buf, MDR_enc_size, 10000) !=
+	    HAL_OK) {
+#ifdef DEBUG_ENABLE
+	    sprintf((char*)debug_buf, "Unable to send MDR. I2C error: %ld\r\n",
+		    HAL_I2C_GetError(&hi2c1));
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);
+#endif /* DEBUG_ENABLE */
+	}
+	else {
+	    sprintf((char*)debug_buf, "Successfully sent MDR\r\n");
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);
+	}
+    }	
+#endif /* MASTER */
+    
+    while (1)
+    {
+
+    }
+}
+
+hs_status_t handshake(uint32_t i2c_addr)
+{
+
+    /* Handshake variables */
+
+    uint8_t hs_sts = IDLE;
+    uint8_t *MDR_req_buf, *MDR_ACK_buf, *MDR_CTS_buf, *MDR_buf;
+    uint32_t AF_error_counter = 0;
+    uint32_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);
+    uint32_t MDR_len = 0;
+    
+    m2s_MDR_request MDR_req_message;
+    s2m_MDR_req_ACK MDR_ACK;
+    m2s_MDR_res_CTS MDR_CTS;
+    s2m_MDR_response MDR_res_message;
+
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
+    uint8_t debug_buf[128];
+#endif
+#ifdef TESTING_ENABLE
+    uint8_t term[] = "\r\n";
+    size_t MDR_req_size, MDR_CTS_size;
+#endif
+    
+    while (hs_sts != HS_FAILED && hs_sts != HS_REGISTERED) {
+	switch (hs_sts) {
+	case (IDLE):
+	{
+	    MDR_req_buf = malloc(8);	    
+	    pb_ostream_t MDR_req_stream = pb_ostream_from_buffer(MDR_req_buf, 2);
+	    MDR_req_message.record_type = 7; /* Placeholder for default record type */
+	    if(!pb_encode(&MDR_req_stream, m2s_MDR_request_fields, &MDR_req_message)) {
+		hs_sts = HS_FAILED;
+#ifdef DEBUG_ENABLE
+		goto __MDR_REQ_ENC_FAIL;
+	    __MDR_REQ_ENC_FAIL_END:
+		__asm__("nop");
+#endif
+	    }
+	    else {
+#ifdef TESTING_ENABLE
+		MDR_req_size = MDR_req_stream.bytes_written;
+		goto __HS_IDLE_TESTING;
+	    __HS_IDLE_TESTING_END:
+		__asm__("nop");
+#endif	    
+		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)MDR_req_buf,
+					    MDR_req_buf_len, 10000) != HAL_OK) {
+		    hs_sts = HS_FAILED;
+#ifdef DEBUG_ENABLE
+		    goto __HS_MDR_REQ_I2C_ERROR;
+		__HS_MDR_REQ_I2C_ERROR_END:
+		    __asm__("nop");
+#endif
+		}
+		else {
+		    hs_sts = HS_MDR_ACK;
+		}
+		free(MDR_req_buf);
+		break;
+	    }
+	}
+	case (HS_MDR_ACK):
+	{
+	    MDR_ACK_buf = malloc(8);
+	    AF_error_counter = 0;
+	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)MDR_ACK_buf,
+					  s2m_MDR_req_ACK_size, 100) != HAL_OK) {
+		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
+		    hs_sts = HS_FAILED;
+		}
+		if (++AF_error_counter > 1500) {
+		    hs_sts = HS_FAILED;
+		}
+		if (hs_sts == HS_FAILED) {
+#ifdef DEBUG_ENABLE
+		    goto __HS_MDR_ACK_I2C_ERROR;
+		__HS_MDR_ACK_I2C_ERROR_END:
+		    __asm__("nop");
+#endif
+		    break;
+		}
+	    }
+	    if (hs_sts != HS_FAILED) {
+		pb_istream_t MDR_ACK_istream = pb_istream_from_buffer(MDR_ACK_buf, 2);
+		if (!pb_decode(&MDR_ACK_istream, s2m_MDR_req_ACK_fields, &MDR_ACK)) {
+		    hs_sts = HS_FAILED;
+#ifdef DEBUG_ENABLE
+		    goto __MDR_ACK_DEC_ERROR;
+		__MDR_ACK_DEC_ERROR_END:
+		    __asm__("nop");
+#endif
+		}
+		else {
+		    MDR_len = MDR_ACK.MDR_res_length;
+		    hs_sts = HS_MDR_CTS;
+#ifdef TESTING_ENABLE
+		    goto __HS_MDR_ACK_TESTING;
+		__HS_MDR_ACK_TESTING_END:
+		    __asm__("nop");
+#endif
+		}
+		free(MDR_ACK_buf);
+	    }
+	    break;
+	}
+	case (HS_MDR_CTS):
+	{
+	    MDR_CTS_buf = (uint8_t*)malloc(8);
+	    pb_ostream_t MDR_CTS_ostream = pb_ostream_from_buffer(MDR_CTS_buf, sizeof(MDR_CTS_buf));
+	    MDR_CTS.timeout = 100;
+	    if (!pb_encode(&MDR_CTS_ostream, m2s_MDR_res_CTS_fields, &MDR_CTS)) {
+		hs_sts = HS_FAILED;
+#ifdef DEBUG_ENABLE
+		goto __MDR_CTS_ENC_ERROR;
+	    __MDR_CTS_ENC_ERROR_END:
+		__asm__("nop");
+#endif
+	    }
+	    else {
+#ifdef TESTING_ENABLE
+		MDR_CTS_size = MDR_CTS_ostream.bytes_written;
+		goto __HS_MDR_CTS_TESTING;
+	    __HS_MDR_CTS_TESTING_END:
+		__asm__("nop");
+#endif
+		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr,
+					    (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {
+		    hs_sts = HS_FAILED;
+#ifdef DEBUG_ENABLE
+		    goto __HS_CTS_I2C_ERROR;
+		__HS_CTS_I2C_ERROR_END:
+		    __asm__("nop");
+#endif
+		}
+		else {
+		    hs_sts = HS_MDR_MDR;
+		    free(MDR_CTS_buf);
+		}
+	    }
+	    break;
+	}
+	case (HS_MDR_MDR):
+	{
+	    MDR_buf = (uint8_t*)malloc(MDR_len);
+	    AF_error_counter = 0;
+	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
+					  (uint8_t*)MDR_buf, MDR_len, 1000) != HAL_OK) {
+		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
+		    hs_sts = HS_FAILED;
+#ifdef DEBUG_ENABLE
+		    goto __HS_MDR_I2C_ERROR;
+		__HS_MDR_I2C_ERROR_END:
+		    __asm__("nop");
+#endif
+		    break;
+		}
+		else if (++AF_error_counter > 1500) {
+		    hs_sts = HS_FAILED;
+		    break;
+		}
+	    }
+	    if (hs_sts != HS_FAILED) {
+#ifdef TESTING_ENABLE
+		goto __HS_MDR_MDR_TESTING;
+	    __HS_MDR_MDR_TESTING_END:
+		__asm__("nop");
+#endif
+		MDR_res_message.subscriptions.funcs.decode = decode_subscriptions_callback;
+		MDR_res_message.subscriptions.arg = (void*)dev_idx;
+		pb_istream_t MDR_res_stream = pb_istream_from_buffer(MDR_buf, MDR_len);
+		if (!pb_decode(&MDR_res_stream, s2m_MDR_response_fields, &MDR_res_message)) {
+#ifdef DEBUG_ENABLE
+		    goto __HS_MDR_DEC_ERROR;
+		__HS_MDR_DEC_ERROR_END:
+		    __asm__("nop");
+#endif
+		}
+		else {
+#ifdef TESTING_ENABLE
+		    goto __MDR_DEC_TESTING;
+		__MDR_DEC_TESTING_END:
+		    __asm__("nop");
+#endif
+		    hs_sts = HS_REGISTERED;
+		}	       		
+	    }
+	    break;
+	}
+	}
+
+    }
+
+#ifdef TESTING_ENABLE
+    {
+	goto __TESTING_BLOCK_END;
+    __HS_IDLE_TESTING:
+	sprintf((char*)debug_buf, "MDR req length: %d\r\n", MDR_req_size);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	uint8_t bufbuf[] = "MDR req buffer: ";
+	HAL_UART_Transmit(&huart1, bufbuf, sizeof(bufbuf), 100);
+	for(int x=0; x<MDR_req_size; x++) {
+	    sprintf((char*)debug_buf+x, "%x", MDR_req_buf[x]);
+	}
+	HAL_UART_Transmit(&huart1, debug_buf, MDR_req_size, 100);
+	HAL_UART_Transmit(&huart1, term, 2, 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_IDLE_TESTING_END;
+    __HS_MDR_ACK_TESTING:
+	sprintf((char*)debug_buf, "Got MDR message length: %ld\r\n", MDR_ACK.MDR_res_length);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_ACK_TESTING_END;
+    __HS_MDR_CTS_TESTING:
+	sprintf((char*)debug_buf, "CTS size: %d\r\n", MDR_CTS_size);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	uint8_t ctsbuf[] = "\tCTS buffer: ";
+	for(int x=0; x<2; x++) {
+	    sprintf((char*)debug_buf+x, "%x", MDR_CTS_buf[x]);
+	}
+	HAL_UART_Transmit(&huart1, ctsbuf, sizeof(ctsbuf), 100);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	HAL_UART_Transmit(&huart1, term, 2, 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_CTS_TESTING_END;
+    __HS_MDR_MDR_TESTING:
+	for (int x=0; x<MDR_len; x++) {
+	    sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);
+	}
+	uint8_t mdrbuf[] = "Got MDR: ";
+	HAL_UART_Transmit(&huart1, mdrbuf, sizeof(mdrbuf), 100);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);  
+	HAL_UART_Transmit(&huart1, term, 2, 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_MDR_TESTING_END;
+    __MDR_DEC_TESTING:
+	sprintf((char*)debug_buf, "MDR Decode success\r\n\tFirst subscibed module: %d\r\n",
+		subs_info[dev_idx]->module_ids[1]);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __MDR_DEC_TESTING_END;
+    }
+__TESTING_BLOCK_END:
+    __asm__("nop");
+#endif
+
+#ifdef DEBUG_ENABLE
+    {
+	goto __DEBUG_BLOCK_END;
+    __MDR_REQ_ENC_FAIL:
+	sprintf((char*)debug_buf, "MDR reqest encoding error\r\n");
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __MDR_REQ_ENC_FAIL_END;
+    __HS_MDR_REQ_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to send MDR request. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_REQ_I2C_ERROR_END;
+    __HS_MDR_ACK_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to get MDR ACK. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_ACK_I2C_ERROR_END;
+    __MDR_ACK_DEC_ERROR:
+	sprintf((char*)debug_buf, "MDR ACK decoding error\r\n");
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __MDR_ACK_DEC_ERROR_END;
+    __MDR_CTS_ENC_ERROR:	
+	sprintf((char*)debug_buf, "MDR encoding error\r\n");
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __MDR_CTS_ENC_ERROR_END;
+    __HS_CTS_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to send MDR CTS. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_CTS_I2C_ERROR_END;
+    __HS_MDR_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to get MDR. I2C error: %ld\n\tError counter: %ld\r\n", HAL_I2C_GetError(&hi2c1), AF_error_counter);
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_I2C_ERROR_END;	
+    }
+__HS_MDR_DEC_ERROR:
+    sprintf((char*)debug_buf, "MDR decode error\r\n");
+    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+    memset(debug_buf, 0, 128);
+    goto __HS_MDR_DEC_ERROR_END;
+__DEBUG_BLOCK_END:
+    __asm__("nop");
+#endif
+    
+    return hs_sts;
+}
+
+bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
+{
+    _subscriptions subs;
+    int *subs_idx = (int*)args;
+
+    /* Check is storage is allocated; if not, allocate it */
+    if ((GET_BIT_FROM_IDX(allocated, *subs_idx)) == 0) {
+	subs_info[*subs_idx] = (subscription_info_t*)malloc(sizeof(subscription_info_t));
+	SET_BIT_FROM_IDX(allocated, *subs_idx);
+	subs_info[*subs_idx]->mod_idx = subs_info[*subs_idx]->entity_idx =
+	    subs_info[*subs_idx]->class_idx = subs_info[*subs_idx]->i2c_idx = 0;
+    }
+    
+    if(!pb_decode(istream, _subscriptions_fields, &subs))
+    	return false;
+
+    /* Parse all fields if they're included */
+    if (subs.has_module_id)
+	subs_info[*subs_idx]->module_ids[subs_info[*subs_idx]->mod_idx++] =
+	    subs.module_id;
+    if (subs.has_entity_id)
+	subs_info[*subs_idx]->entity_ids[subs_info[*subs_idx]->entity_idx++] =
+	    subs.entity_id;
+    if (subs.has_module_class)
+	subs_info[*subs_idx]->module_class[subs_info[*subs_idx]->class_idx++] =
+	    subs.module_class;
+    if (subs.has_i2c_address)
+	subs_info[*subs_idx]->i2c_address[subs_info[*subs_idx]->i2c_idx++] =
+	    subs.i2c_address;
+    return true;
+}
+
+bool todo_hs_or_not_todo_hs(uint8_t i2c_addr)
+{
+    uint8_t device_idx = GET_IDX_FROM_ADDR(i2c_addr);
+    state_t device_curr_state = dev_sts[device_idx];
+    bool do_hs = false;
+    switch(device_curr_state) {
+    case NO_HS:
+    case CONNECTED:
+    case FAILED:
+    case OFFLINE:	
+	do_hs = true;
+	break;
+    case REGISTERED:
+    case NO_DATA:
+	do_hs = false;
+	break;	
+    }
+    return do_hs;
+}
+
+state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status)
+{
+    state_t device_state = OFFLINE;
+    switch(hs_status) {
+    case IDLE:
+    case HS_FAILED:
+	device_state = OFFLINE;
+	break;
+    case HS_MDR_ACK:
+    case HS_MDR_CTS:
+    case HS_MDR_MDR:
+	device_state = FAILED;
+	break;
+    case HS_REGISTERED:
+	device_state = REGISTERED;
+	break;    
+    }
+    return device_state;
+}
+
+bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
+{
+    if(ostream!=NULL && field->tag == s2m_MDR_response_subscriptions_tag) {
+	for (int x=0; x<2; x++) {
+	    _subscriptions subs;
+	    subs.module_id = x+10*x;
+	    subs.i2c_address = x+1;
+	    subs.has_entity_id=false;
+	    subs.has_module_class=false;
+	    subs.has_module_id=true;
+	    subs.has_i2c_address=true;
+	    if(!pb_encode_tag_for_field(ostream, field)){
+		printf("ERR1\n");
+		return false;
+	    }
+	    if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)){
+		printf("ERR2\n");
+		return false;
+	    }
+	}
+    }
+    else{
+	return false;
+    }
+    return true;
+}
+
+/**
+ * @brief System Clock Configuration
+ * @retval None
+ */
+void SystemClock_Config(void)
+{
+    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
+    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
+
+    /** Configure the main internal regulator output voltage 
+     */
+    __HAL_RCC_PWR_CLK_ENABLE();
+    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);
+    /** Initializes the CPU, AHB and APB busses clocks 
+     */
+    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
+    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
+    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
+    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
+    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
+    {
+	Error_Handler();
+    }
+    /** Initializes the CPU, AHB and APB busses clocks 
+     */
+    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
+	|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
+    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
+    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
+    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
+    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
+
+    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
+    {
+	Error_Handler();
+    }
+}
+
+/**
+ * @brief I2C1 Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_I2C1_Init(void)
+{
+    hi2c1.Instance = I2C1;
+    hi2c1.Init.ClockSpeed = 100000;
+    hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
+    hi2c1.Init.OwnAddress1 = I2C_ADDRESS;
+    hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+    hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+    hi2c1.Init.OwnAddress2 = 0xFF;
+    hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+    hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+    if (HAL_I2C_Init(&hi2c1) != HAL_OK)
+    {
+	Error_Handler();
+    }
+
+}
+
+/**
+ * @brief USART1 Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_USART1_UART_Init(void)
+{
+    huart1.Instance = USART1;
+    huart1.Init.BaudRate = 9600;
+    huart1.Init.WordLength = UART_WORDLENGTH_8B;
+    huart1.Init.StopBits = UART_STOPBITS_1;
+    huart1.Init.Parity = UART_PARITY_NONE;
+    huart1.Init.Mode = UART_MODE_TX_RX;
+    huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
+    huart1.Init.OverSampling = UART_OVERSAMPLING_16;
+    if (HAL_UART_Init(&huart1) != HAL_OK)
+    {
+	Error_Handler();
+    }
+
+}
+
+/**
+ * @brief GPIO Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_GPIO_Init(void)
+{
+    GPIO_InitTypeDef GPIO_InitStruct = {0};
+
+    /* GPIO Ports Clock Enable */
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+    __HAL_RCC_GPIOH_CLK_ENABLE();
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+
+    /*Configure GPIO pin Output Level */
+    HAL_GPIO_WritePin(led_GPIO_Port, led_Pin, GPIO_PIN_RESET);
+
+    /*Configure GPIO pin : led_Pin */
+    GPIO_InitStruct.Pin = led_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    HAL_GPIO_Init(led_GPIO_Port, &GPIO_InitStruct);
+
+}
+
+/**
+ * @brief  This function is executed in case of error occurrence.
+ * @retval None
+ */
+void Error_Handler(void)
+{
+    /* USER CODE BEGIN Error_Handler_Debug */
+    /* User can add his own implementation to report the HAL error return state */
+    while (1) {
+	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
+	HAL_Delay(1000);
+    }
+    /* USER CODE END Error_Handler_Debug */
+}
+
+#ifdef  USE_FULL_ASSERT
+/**
+ * @brief  Reports the name of the source file and the source line number
+ *         where the assert_param error has occurred.
+ * @param  file: pointer to the source file name
+ * @param  line: assert_param error line source number
+ * @retval None
+ */
+void assert_failed(uint8_t *file, uint32_t line)
+{ 
+    /* USER CODE BEGIN 6 */
+    /* User can add his own implementation to report the file name and line number,
+       tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
+    /* USER CODE END 6 */
+}
+#endif /* USE_FULL_ASSERT */