path: root/src/main-data.c

/**
******************************************************************************
* @file           : main.c
* @brief          : Main program body
******************************************************************************
* @attention
*
* 
******************************************************************************
*/

/* Standard library includes */
#include <stdio.h>

/* Library includes */
#include <pb_encode.h>
#include <pb_decode.h>

/* Project includes */
#include "main.h"
#include "devices.h"
#include "config.h"
#include "dataflow.h"
#include "handshake.pb.h"
#include "data.pb.h"

/* Private Macros */
#define device_MDR s2m_MDR_response
#define GET_IDX_FROM_ADDR(i2c_addr) i2c_addr-1
#define GET_ADDR_FROM_IDX(idx)      idx+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};
uint8_t data_idx;

_datapoint routing_buffer[ROUTING_BUFSIZE];
/* Index information for each datapoint */
uint8_t    routing_idx_buffer[ROUTING_BUFSIZE];
/* Pointer to tail of both data and idx buffers */
uint32_t   routing_ptr = 0;

/* 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);

hs_status_t handshake(uint32_t i2c_addr);
dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t routing_buf_idx);
bool routing(void);
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 decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
bool encode_datapoint_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
bool decode_data_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
bool master_encode_MDR_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
    /* Add MDR */
    
    uint8_t dev_idx = GET_IDX_FROM_ADDR(I2C_ADDRESS);
    subs_info[dev_idx] = malloc(sizeof(subscription_info_t));
    subs_info[dev_idx]->mod_idx = subs_info[dev_idx]->entity_idx =
	subs_info[dev_idx]->class_idx = subs_info[dev_idx]->i2c_idx = 0;
    SET_BIT_FROM_IDX(allocated, dev_idx);

    subs_info[dev_idx]->module_ids[subs_info[dev_idx]->mod_idx++] = 1; //subscribe to self

    
    _MDR module_MDR = s2m_MDR_response_init_default;

    device_info[dev_idx] = malloc(sizeof(device_info_t));
    device_info[dev_idx]->i2c_addr     = I2C_ADDRESS;
    device_info[dev_idx]->device_id    = dev_idx;

    module_MDR.MDR_version  = 1.1;
    module_MDR.module_id    = 1;
    module_MDR.module_class = 1;
    module_MDR.entity_id    = 32;

    device_info[dev_idx]->MDR = module_MDR;
    
    /* dataflow */
    /* while (1) { */
    device_dataflow(0x05, 1, 0);
    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
    routing();
	/* HAL_Delay(500); */
    /* } */
    
#else /* Slave code*/
    {
	while (1) {
	    uint8_t SOR_buf[m2s_SOR_size] = {0}, debug_buf[128];
	    if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, m2s_SOR_size, 10000) != HAL_OK) {
		sprintf((char*)debug_buf, "Failed to get SOR\r\n");
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
	    }
	    else {
		sprintf((char*)debug_buf, "Got SOR\r\n");
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
	    }
	    m2s_SOR SOR_message;
	    pb_istream_t SOR_istream = pb_istream_from_buffer(SOR_buf, 2);
	    if (!pb_decode(&SOR_istream, m2s_SOR_fields, &SOR_message)) {
		sprintf((char*)debug_buf, "SOR decode error\r\n");
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
	    }
	    else {
		sprintf((char*)debug_buf, "SOR decoded; code: %ld\r\n", SOR_message.SOR_code);
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
	    }

	    if (SOR_message.SOR_code == 1) {
		uint8_t data_buf[128];
		size_t data_enc_size;
		s2m_data data;
		data.datapoints.funcs.encode = encode_datapoint_callback;
		pb_ostream_t data_ostream = pb_ostream_from_buffer(data_buf, sizeof(data_buf));
		if (!pb_encode(&data_ostream, s2m_data_fields, &data)) {
		    sprintf((char*)debug_buf, "Data encoding error\r\n");
		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		    memset(debug_buf, 0, 128);
		    Error_Handler();
		}
		data_enc_size = data_ostream.bytes_written;
	    
		s2m_DOC doc = s2m_DOC_init_zero;
		uint8_t doc_buf[s2m_DOC_size];
		doc.DOC_code  = 5;
		doc.tx_length = data_enc_size;
		pb_ostream_t doc_ostream = pb_ostream_from_buffer(doc_buf, 4);

		if (!pb_encode(&doc_ostream, s2m_DOC_fields, &doc)) {
		    sprintf((char*)debug_buf, "DOC encoding error\r\n");
		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		    memset(debug_buf, 0, 128);
		    Error_Handler();
		}
	    
		sprintf((char*)debug_buf, "s2m_DOC encoded length: %d\r\n", doc_ostream.bytes_written);
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);

		if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)doc_buf, 4, 10000) != HAL_OK) {
		    sprintf((char*)debug_buf, "DOC I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		    memset(debug_buf, 0, 128);
		    Error_Handler();
		}
		else {
		    sprintf((char*)debug_buf, "SENT DOC\r\n");
		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		    memset(debug_buf, 0, 128);
		}

		uint8_t CTS_buf[8];
	    
		if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)CTS_buf, 2, 10000) != HAL_OK) {
		    sprintf((char*)debug_buf, "Failed to get CTS: %ld\r\n", HAL_I2C_GetError(&hi2c1));
		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		    memset(debug_buf, 0, 128);
		    Error_Handler();
		}
		else {
		    sprintf((char*)debug_buf, "Got CTS\r\n");
		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		    memset(debug_buf, 0, 128);
		}

		if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)data_buf, data_enc_size, 10000) != HAL_OK) {
		    sprintf((char*)debug_buf, "Data I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		    memset(debug_buf, 0, 128);
		    Error_Handler();
		}
		else {
		    sprintf((char*)debug_buf, "SENT DATA\r\n");
		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		    memset(debug_buf, 0, 128);
		}
	    }
	    else if (SOR_message.SOR_code == 2) {
		uint8_t CTS_buf[] = {0x0, 0x1};
		uint8_t len_buf[4], *MDR_buf, *data_buf;
		/* _datapoint datapoints[16]; */
	    
		HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
		sprintf((char*)debug_buf, "Sent CTS\r\n");
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
		
		HAL_I2C_Slave_Receive(&hi2c1, len_buf, 4, 1000);
		
		uint16_t MDR_len = len_buf[1]+(len_buf[0]<<8);
		MDR_buf = malloc(MDR_len);
		uint16_t data_len = len_buf[3]+(len_buf[2]<<8);
		data_buf = malloc(data_len);

		sprintf((char*)debug_buf, "Got lengths. MDR: %d, data: %d\r\n", MDR_len, data_len);
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
		
		HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
		HAL_I2C_Slave_Receive(&hi2c1, MDR_buf, MDR_len, 10000);
		HAL_I2C_Slave_Receive(&hi2c1, data_buf, data_len, 10000);
		
		_datapoint datapoint_message;
		s2m_MDR_response MDR_message;
		pb_istream_t MDR_istream  = pb_istream_from_buffer(MDR_buf, MDR_len);
		pb_istream_t data_istream = pb_istream_from_buffer(data_buf, data_len);
		
		pb_decode(&MDR_istream, s2m_MDR_response_fields, &MDR_message);
		pb_decode(&data_istream, _datapoint_fields, &datapoint_message);

		sprintf((char*)debug_buf, "Got data from %ld, running version %f\r\n\tdata 0: %f\r\n", MDR_message.module_id, MDR_message.MDR_version, datapoint_message.data);
		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;
}

dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t rbuf_data_idx)
{
    uint8_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);    
    dataflow_status_t df_status = DF_IDLE;
   
    uint8_t *SOR_buf, *DOC_buf, *CTS_buf, *data_buf;
    uint32_t AF_error_counter = 0;
    uint32_t data_len         = 0;
    _datapoint datapoints[16];
    
    m2s_SOR  SOR_message  = m2s_SOR_init_default;
    s2m_DOC  DOC_message  = s2m_DOC_init_zero;
    /* TODO Add default values to the CTS message in proto */
    m2s_CTS  CTS_message  = m2s_CTS_init_default;
    s2m_data data_message = s2m_data_init_zero;
#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
    uint8_t debug_buf[128];
#endif
#ifdef TESTING_ENABLE
    uint8_t term[] = "\r\n";
#endif
    
    while (df_status != DF_SUCCESS && df_status != DF_FAIL) {
	switch (df_status) {
	case (DF_IDLE):
	{
	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
	    SOR_buf = malloc(sizeof(m2s_SOR));
	    pb_ostream_t SOR_stream = pb_ostream_from_buffer(SOR_buf, sizeof(SOR_buf));
	    SOR_message.SOR_code = SOR_code;
	    if (!pb_encode(&SOR_stream, m2s_SOR_fields, &SOR_message)) {
		df_status = DF_FAIL;
#ifdef DEBUG_ENABLE
		goto __DF_SOR_ENC_FAIL;
	    __DF_SOR_ENC_FAIL_END:
		__asm__("nop");
#endif
	    }
	    else {
		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)SOR_buf,
					    m2s_SOR_size, 1000) != HAL_OK) {
		    df_status = DF_FAIL;
#ifdef DEBUG_ENABLE
		    goto __DF_SOR_I2C_ERROR;
		__DF_SOR_I2C_ERROR_END:
		    __asm__("nop");
#endif
		}
		else {
		    if (SOR_code == SLAVE_TX) {
			df_status = DF_RX_DOC;
		    }
		    else if (SOR_code == SLAVE_RX_DATAPOINT) {
			/* TODO */
			df_status = DF_LEN_TX;
		    }
		    else if (SOR_code == SLAVE_RX_COMMAND) {
			/* TODO */
		    }
		}
	    }
	    break;
	}
	case (DF_RX_DOC):
	{
	    DOC_buf = (uint8_t*)malloc(4);
	    AF_error_counter = 0;
	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
					  (uint8_t*)DOC_buf, 4, 1000) != HAL_OK) {
		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
		    df_status = DF_FAIL;
#ifdef DEBUG_ENABLE
		    goto __DF_DOC_I2C_ERROR;
		__DF_DOC_I2C_ERROR_END:
		    __asm__("nop");
#endif
		    break;
		}
		else if (++AF_error_counter > 4000) {
		    df_status = DF_FAIL;
		    break;
		}
	    }
	    if (df_status != DF_FAIL) {
		pb_istream_t DOC_istream = pb_istream_from_buffer(DOC_buf, 4);
		if (!pb_decode(&DOC_istream, s2m_DOC_fields, &DOC_message)) {
		    df_status = DF_FAIL;
#ifdef DEBUG_ENABLE
		    goto __DF_DOC_DECODE_ERROR;
		__DF_DOC_DECODE_ERROR_END:
		    __asm__("nop");
#endif
		}
		else {
		    if (DOC_message.DOC_code == DATA) {
			df_status = DF_CTS;
			data_len = DOC_message.tx_length;
		    }
		    else if (DOC_message.DOC_code == CMD_UNICAST) {
			/* TODO */
		    }
		    else if (DOC_message.DOC_code == CMD_MULTICAST) {
			/* TODO */
		    }
		    else if (DOC_message.DOC_code == CMD_BROADCAST) {
			/* TODO */
		    }
		}
	    }
	    break;
	}
	case (DF_CTS):
	{
	    CTS_buf = (uint8_t*)malloc(8);
	    pb_ostream_t CTS_ostream = pb_ostream_from_buffer(CTS_buf, 8);
	    CTS_message.timeout = 100;
	    
	    if (!pb_encode(&CTS_ostream, m2s_CTS_fields, &CTS_message)) {
		df_status = DF_FAIL;
#ifdef DEBUG_ENABLE
		goto __DF_CTS_ENC_FAIL;
	    __DF_CTS_ENC_FAIL_END:
		__asm__("nop");
#endif
	    }
	    else {
		HAL_Delay(MASTER_I2C_BUS_INTERVAL);
		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)CTS_buf,
					    2, 10000) != HAL_OK) {
		    df_status = DF_FAIL;
#ifdef DEBUG_ENABLE
		    goto __DF_CTS_I2C_ERROR;
		__DF_CTS_I2C_ERROR_END:
		    __asm__("nop");
#endif
		}
		else {
		    if (DOC_message.DOC_code == DATA) {
			df_status = DF_RX_DATA;
		    }
		    else {
			/* TODO */
		    }
		}
	    }
	    break;
	}
	case (DF_RX_DATA):
	{
	    data_buf = (uint8_t*)malloc(128);
	    AF_error_counter = 0;
	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
					  (uint8_t*)data_buf, data_len, 10000) != HAL_OK) {
		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
		    df_status = DF_FAIL;
#ifdef DEBUG_ENABLE
		    goto __DF_DATA_I2C_ERROR;
		__DF_DATA_I2C_ERROR_END:
		    __asm__("nop");
#endif
		    break;
		}
		else if (++AF_error_counter > 1500) {
		    df_status = DF_FAIL;
		    break;
		}
	    }
	    if (df_status != DF_FAIL) {
		data_idx = 0;
		data_message.datapoints.funcs.decode = decode_data_callback;
		data_message.datapoints.arg = (void*)datapoints;
		pb_istream_t data_istream = pb_istream_from_buffer(data_buf, data_len);
		if (!pb_decode(&data_istream, s2m_data_fields, &data_message)) {
		    df_status = DF_FAIL;
#ifdef DEBUG_ENABLE
		    goto __DF_DATA_DECODE_ERROR;
		__DF_DATA_DECODE_ERROR_END:
		    __asm__("nop");
#endif
		}
		else {
		    /* This could be done in the callback itself */
		    for (int i = 0; i < data_idx && routing_ptr < ROUTING_BUFSIZE; i++) {
			routing_idx_buffer[routing_ptr] = dev_idx;
			routing_buffer[routing_ptr++]   = datapoints[i];
		    }
		    df_status = DF_SUCCESS;
		}
	    }
	    break;
	}
	case (DF_LEN_TX):
	{
	    /* TODO error checking  */
	    /* Will need to package datapoint and MDR to know their lengths
	       Once cached, will not need to do this */

	    /* Do this after handshake to cache ================================================== */
	    uint8_t MDR_buf[128], data_buf[128], CTS_buf[2];
	    uint8_t src_device_idx = routing_idx_buffer[rbuf_data_idx];
	    s2m_MDR_response data_src_MDR = device_info[src_device_idx]->MDR;
	    pb_ostream_t MDR_ostream = pb_ostream_from_buffer(MDR_buf, sizeof(MDR_buf));
	    data_src_MDR.subscriptions.funcs.encode=master_encode_MDR_callback;
	    pb_encode(&MDR_ostream, s2m_MDR_response_fields, &data_src_MDR);	    
	    uint8_t MDR_len = MDR_ostream.bytes_written;
	    /* =================================================================================== */
	    
	    _datapoint data = routing_buffer[rbuf_data_idx];
	    pb_ostream_t data_ostream = pb_ostream_from_buffer(data_buf, sizeof(data_buf));
	    pb_encode(&data_ostream, _datapoint_fields, &data);
	    uint8_t data_len = data_ostream.bytes_written;

	    uint8_t data_MDR_len_buf[4] = {0, MDR_len, 0, data_len};

	    AF_error_counter = 0;
	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 10000) != HAL_OK) {
		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
		    df_status = DF_FAIL;
		}
		if (++AF_error_counter > 3000) {
		    df_status = DF_FAIL;
		}
		if (df_status == DF_FAIL) {
		    sprintf((char*)debug_buf, "Failed to get LEN CTS\r\n");
		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		    memset(debug_buf, 0, 128);
		    break;
		}
	    }

	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
	    if (df_status != DF_FAIL &&
		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, data_MDR_len_buf, 4, 10000) == HAL_OK) {
		sprintf((char*)debug_buf, "MDR len: %d data len: %d SENT\r\n", MDR_len, data_len);
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
	    }
	    else {
		sprintf((char*)debug_buf, "Failed to send lengths\r\n");
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
	    }
	    AF_error_counter = 0;
	    while (df_status != DF_FAIL &&
		   HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 10000) != HAL_OK) {
		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
		    df_status = DF_FAIL;
		}
		if (++AF_error_counter > 3000) {
		    df_status = DF_FAIL;
		}
		if (df_status == DF_FAIL) {
		    sprintf((char*)debug_buf, "Failed to get TX CTS\r\n");
		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		    memset(debug_buf, 0, 128);
		    break;
		}
	    }

	    
#ifdef TESTING_ENABLE
	    uint8_t buf_title[64];
	    sprintf((char*)buf_title, "MDR buffer: ");	    
	    HAL_UART_Transmit(&huart1, buf_title, sizeof(buf_title), 100);
	    memset(buf_title, 0, 64);
	    for(int x=0; x<MDR_len; x++)
		sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    HAL_UART_Transmit(&huart1, term, 2, 100);
	    memset(debug_buf, 0, 128);

	    sprintf((char*)buf_title, "Data buffer: ");	    
	    HAL_UART_Transmit(&huart1, buf_title, sizeof(buf_title), 100);
	    memset(buf_title, 0, 64);
	    for(int x=0; x<data_len; x++)
		sprintf((char*)debug_buf+x, "%x", data_buf[x]);
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    HAL_UART_Transmit(&huart1, term, 2, 100);
	    memset(debug_buf, 0, 128);
#endif
	    
	    if (df_status != DF_FAIL &&
		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, MDR_buf, MDR_len, 10000) == HAL_OK &&
		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, data_buf, data_len, 10000) == HAL_OK) {
		sprintf((char*)debug_buf, "Data and MDR SENT\r\n");
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);

		df_status = DF_SUCCESS;
	    }
	    break;
	}
	case DF_SUCCESS:
	case DF_FAIL:
	    break;	    
	}
    }
    
#ifdef TESTING_ENABLE
    {
	goto __DF_TESTING_BLOCK_END;
    __DF_TESTING_BLOCK_END:
	__asm__("nop");
    }
#endif

#ifdef DEBUG_ENABLE
    {
	goto __DF_DEBUG_BLOCK_END;
    __DF_SOR_ENC_FAIL:
	sprintf((char*)debug_buf, "SOR encoding error\r\n");
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __DF_SOR_ENC_FAIL_END;
    __DF_SOR_I2C_ERROR:
	sprintf((char*)debug_buf, "Unable to send SOR 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 __DF_SOR_I2C_ERROR_END;
    __DF_DOC_I2C_ERROR:
	sprintf((char*)debug_buf, "Unable to receive DOC. 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 __DF_DOC_I2C_ERROR_END;
    __DF_DOC_DECODE_ERROR:
	sprintf((char*)debug_buf, "DOC decoding error\r\n");
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __DF_DOC_DECODE_ERROR_END;
    __DF_CTS_ENC_FAIL:
	sprintf((char*)debug_buf, "CTS encoding error\r\n");
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __DF_CTS_ENC_FAIL_END;
    __DF_CTS_I2C_ERROR:
	sprintf((char*)debug_buf, "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 __DF_CTS_I2C_ERROR_END;
    __DF_DATA_I2C_ERROR:
	sprintf((char*)debug_buf, "Unable to receive data. 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 __DF_DATA_I2C_ERROR_END;
    __DF_DATA_DECODE_ERROR:
	sprintf((char*)debug_buf, "Data decoding error\r\n");
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __DF_DATA_DECODE_ERROR_END;
    __DF_DEBUG_BLOCK_END:
	__asm__("nop");
    }
#endif

    return df_status;
}

bool routing(void)
{
    /* This table holds information on where to send each datapoint in the routing buffer  */
    uint32_t routing_table[ROUTING_BUFSIZE][4] = {{0, 0}};
    
    /* Build table with routing information */
    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < routing_ptr; rbuf_data_idx++) {
	uint8_t module_idx = routing_idx_buffer[rbuf_data_idx];
	for (uint8_t dev_idx = 0; dev_idx < BUS_DEVICE_LIMIT; dev_idx++) {
	    if (!(GET_BIT_FROM_IDX(allocated, dev_idx)&&1)) { // No module at this index
	        continue;
	    }
	    bool alloc = false;
	    for (uint8_t dev_sub_idx = 0; dev_sub_idx < subs_info[dev_idx]->mod_idx && !alloc; dev_sub_idx++) {
		if (subs_info[dev_idx]->module_ids[dev_sub_idx] == device_info[module_idx]->MDR.module_id) {
		    SET_BIT_FROM_IDX(routing_table[rbuf_data_idx], module_idx);
		    alloc = true;
		}
	    }
	    /* TODO entity ID, I2C addr and class routing, should go in the if condition above */
	}
    }

    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < routing_ptr; rbuf_data_idx++) {
	for (uint8_t device_idx = 0; device_idx < BUS_DEVICE_LIMIT; device_idx++) {
	    if (GET_BIT_FROM_IDX(allocated, device_idx) &&
		GET_BIT_FROM_IDX(routing_table[rbuf_data_idx], device_idx)) {
		device_dataflow(GET_ADDR_FROM_IDX(device_idx), SLAVE_RX_DATAPOINT, rbuf_data_idx);
	    }
	}
    }

    /* Reset the routing pointer, since all data in buffer should have been routed */    
    routing_ptr = 0;
    return true;
}

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 master_encode_MDR_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
{
    if (!pb_encode_tag_for_field(ostream, field)) {
	return false;
    }
    return true;
}

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 encode_datapoint_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
{
    if (ostream != NULL && field->tag == s2m_data_datapoints_tag) {
	for (int i = 0; i < 4; i++) {
	    _datapoint datapoint = _datapoint_init_zero;
	    datapoint.entity_id  = 1;
	    datapoint.data = 20.70+((float)i/100);
	    if (!pb_encode_tag_for_field(ostream, field))
		return false;
	    if (!pb_encode_submessage(ostream, _datapoint_fields, &datapoint))
		return false;
	}
    }
    else
	return false;
    return true;
}

bool decode_data_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
{
    _datapoint loc_datapoint = _datapoint_init_zero;
    _datapoint *datapoint = *args;

    if (!pb_decode(istream, _datapoint_fields, &loc_datapoint))
	return false;

    datapoint[data_idx].data = datapoint[data_idx].entity_id = 0;
    
    datapoint[data_idx].entity_id = loc_datapoint.entity_id;
    datapoint[data_idx].data      = loc_datapoint.data;
    
    if (loc_datapoint.has_channel_id) {
    	datapoint[data_idx].has_channel_id = true;
    	datapoint[data_idx].channel_id     = loc_datapoint.channel_id;
    }
    if (loc_datapoint.has_unit_id) {
    	datapoint[data_idx].has_unit_id = true;
    	datapoint[data_idx].unit_id     = loc_datapoint.unit_id;
    }
    if (loc_datapoint.has_timestamp) {
    	datapoint[data_idx].has_timestamp = true;
    	datapoint[data_idx].timestamp    = loc_datapoint.timestamp;
    }

    data_idx++;
    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 */