Master mergerefactor

21 files changed, 916 insertions, 6983 deletions

diff --git a/src/config.h b/src/config.h
index ad035c9..e4a6590 100644
--- a/src/config.h
+++ b/src/config.h
@@ -1,9 +1,9 @@
 
 /* Enable debug mode */
-#define DEBUG_ENABLE 1
+/* #define DEBUG_ENABLE 1 */
 
 /* Enable testing mode */
-#define TESTING_ENABLE 1
+/* #define TESTING_ENABLE 1 */
 
 /* Buffer size for routing */
 #define ROUTING_BUFSIZE 128
@@ -19,4 +19,4 @@
  * hardware.
  *
 */
-#define MASTER_I2C_BUS_INTERVAL 150
+#define MASTER_I2C_BUS_INTERVAL 5
diff --git a/src/data.pb.c b/src/data.pb.c
index fb1e203..76db00a 100644
--- a/src/data.pb.c
+++ b/src/data.pb.c
@@ -15,10 +15,7 @@ PB_BIND(s2m_DOC, s2m_DOC, AUTO)
 PB_BIND(m2s_CTS, m2s_CTS, AUTO)
 
 
-PB_BIND(m2s_command, m2s_command, AUTO)
-
-
-PB_BIND(s2m_command, s2m_command, AUTO)
+PB_BIND(command, command, AUTO)
 
 
 PB_BIND(_datapoint, _datapoint, AUTO)
diff --git a/src/data.pb.h b/src/data.pb.h
index cc271a3..ce1395d 100644
--- a/src/data.pb.h
+++ b/src/data.pb.h
@@ -1,8 +1,8 @@
 /* Automatically generated nanopb header */
 /* Generated by nanopb-0.4.2-dev */
 
-#ifndef PB_DATA_PB_H_INCLUDED
-#define PB_DATA_PB_H_INCLUDED
+#ifndef PB_PROTO_DATA_PB_H_INCLUDED
+#define PB_PROTO_DATA_PB_H_INCLUDED
 #include <pb.h>
 
 #if PB_PROTO_HEADER_VERSION != 40
@@ -29,6 +29,13 @@ typedef struct __datapoint {
     int32_t timestamp;
 } _datapoint;
 
+typedef struct _command {
+    uint32_t source_module_id;
+    uint32_t dest_module_id;
+    pb_callback_t cmd_bytes;
+    pb_callback_t cmd_str;
+} command;
+
 typedef struct _m2s_CTS {
     uint32_t timeout;
 } m2s_CTS;
@@ -39,42 +46,23 @@ typedef struct _m2s_SOR {
     uint32_t rx_length;
 } m2s_SOR;
 
-typedef struct _m2s_command {
-    bool has_source_module_id;
-    uint32_t source_module_id;
-    bool has_source_i2c_address;
-    uint32_t source_i2c_address;
-    pb_callback_t cmd_bytes;
-    pb_callback_t cmd_str;
-} m2s_command;
-
 typedef struct _s2m_DOC {
     uint32_t DOC_code;
     uint32_t tx_length;
 } s2m_DOC;
 
-typedef struct _s2m_command {
-    bool has_dest_module_id;
-    uint32_t dest_module_id;
-    pb_callback_t dest_module_id_multicast;
-    pb_callback_t cmd_bytes;
-    pb_callback_t cmd_str;
-} s2m_command;
-
 
 /* Initializer values for message structs */
 #define m2s_SOR_init_default                     {1u, false, 0}
 #define s2m_DOC_init_default                     {1u, 0u}
 #define m2s_CTS_init_default                     {0}
-#define m2s_command_init_default                 {false, 1u, false, 1u, {{NULL}, NULL}, {{NULL}, NULL}}
-#define s2m_command_init_default                 {false, 0, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}}
+#define command_init_default                     {0, 0, {{NULL}, NULL}, {{NULL}, NULL}}
 #define _datapoint_init_default                  {0u, 0, false, 1u, false, 1u, false, 1}
 #define s2m_data_init_default                    {{{NULL}, NULL}}
 #define m2s_SOR_init_zero                        {0, false, 0}
 #define s2m_DOC_init_zero                        {0, 0}
 #define m2s_CTS_init_zero                        {0}
-#define m2s_command_init_zero                    {false, 0, false, 0, {{NULL}, NULL}, {{NULL}, NULL}}
-#define s2m_command_init_zero                    {false, 0, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}}
+#define command_init_zero                        {0, 0, {{NULL}, NULL}, {{NULL}, NULL}}
 #define _datapoint_init_zero                     {0, 0, false, 0, false, 0, false, 0}
 #define s2m_data_init_zero                       {{{NULL}, NULL}}
 
@@ -85,19 +73,15 @@ typedef struct _s2m_command {
 #define _datapoint_channel_id_tag                3
 #define _datapoint_unit_id_tag                   4
 #define _datapoint_timestamp_tag                 5
+#define command_source_module_id_tag             1
+#define command_dest_module_id_tag               2
+#define command_cmd_bytes_tag                    3
+#define command_cmd_str_tag                      4
 #define m2s_CTS_timeout_tag                      1
 #define m2s_SOR_SOR_code_tag                     1
 #define m2s_SOR_rx_length_tag                    2
-#define m2s_command_source_module_id_tag         1
-#define m2s_command_source_i2c_address_tag       2
-#define m2s_command_cmd_bytes_tag                3
-#define m2s_command_cmd_str_tag                  4
 #define s2m_DOC_DOC_code_tag                     1
 #define s2m_DOC_tx_length_tag                    2
-#define s2m_command_dest_module_id_tag           1
-#define s2m_command_dest_module_id_multicast_tag 2
-#define s2m_command_cmd_bytes_tag                3
-#define s2m_command_cmd_str_tag                  4
 
 /* Struct field encoding specification for nanopb */
 #define m2s_SOR_FIELDLIST(X, a) \
@@ -117,21 +101,13 @@ X(a, STATIC,   REQUIRED, UINT32,   timeout,           1)
 #define m2s_CTS_CALLBACK NULL
 #define m2s_CTS_DEFAULT NULL
 
-#define m2s_command_FIELDLIST(X, a) \
-X(a, STATIC,   OPTIONAL, UINT32,   source_module_id,   1) \
-X(a, STATIC,   OPTIONAL, UINT32,   source_i2c_address,   2) \
-X(a, CALLBACK, OPTIONAL, BYTES,    cmd_bytes,         3) \
-X(a, CALLBACK, OPTIONAL, STRING,   cmd_str,           4)
-#define m2s_command_CALLBACK pb_default_field_callback
-#define m2s_command_DEFAULT (const pb_byte_t*)"\x08\x01\x10\x01\x00"
-
-#define s2m_command_FIELDLIST(X, a) \
-X(a, STATIC,   OPTIONAL, UINT32,   dest_module_id,    1) \
-X(a, CALLBACK, REPEATED, UINT32,   dest_module_id_multicast,   2) \
+#define command_FIELDLIST(X, a) \
+X(a, STATIC,   REQUIRED, UINT32,   source_module_id,   1) \
+X(a, STATIC,   REQUIRED, UINT32,   dest_module_id,    2) \
 X(a, CALLBACK, OPTIONAL, BYTES,    cmd_bytes,         3) \
 X(a, CALLBACK, OPTIONAL, STRING,   cmd_str,           4)
-#define s2m_command_CALLBACK pb_default_field_callback
-#define s2m_command_DEFAULT NULL
+#define command_CALLBACK pb_default_field_callback
+#define command_DEFAULT NULL
 
 #define _datapoint_FIELDLIST(X, a) \
 X(a, STATIC,   REQUIRED, UINT32,   entity_id,         1) \
@@ -151,8 +127,7 @@ X(a, CALLBACK, REPEATED, MESSAGE,  datapoints,        1)
 extern const pb_msgdesc_t m2s_SOR_msg;
 extern const pb_msgdesc_t s2m_DOC_msg;
 extern const pb_msgdesc_t m2s_CTS_msg;
-extern const pb_msgdesc_t m2s_command_msg;
-extern const pb_msgdesc_t s2m_command_msg;
+extern const pb_msgdesc_t command_msg;
 extern const pb_msgdesc_t _datapoint_msg;
 extern const pb_msgdesc_t s2m_data_msg;
 
@@ -160,8 +135,7 @@ extern const pb_msgdesc_t s2m_data_msg;
 #define m2s_SOR_fields &m2s_SOR_msg
 #define s2m_DOC_fields &s2m_DOC_msg
 #define m2s_CTS_fields &m2s_CTS_msg
-#define m2s_command_fields &m2s_command_msg
-#define s2m_command_fields &s2m_command_msg
+#define command_fields &command_msg
 #define _datapoint_fields &_datapoint_msg
 #define s2m_data_fields &s2m_data_msg
 
@@ -169,8 +143,7 @@ extern const pb_msgdesc_t s2m_data_msg;
 #define m2s_SOR_size                             12
 #define s2m_DOC_size                             12
 #define m2s_CTS_size                             6
-/* m2s_command_size depends on runtime parameters */
-/* s2m_command_size depends on runtime parameters */
+/* command_size depends on runtime parameters */
 #define _datapoint_size                          38
 /* s2m_data_size depends on runtime parameters */
 
diff --git a/src/dataflow.h b/src/dataflow.h
index d39126e..2aed40a 100644
--- a/src/dataflow.h
+++ b/src/dataflow.h
@@ -28,7 +28,8 @@ typedef enum dataflow_status {
     DF_RX_CMD    = 4,
     DF_SUCCESS   = 5,
     DF_FAIL      = 6,
-    DF_LEN_TX    = 7
+    DF_TX_DATA   = 7,
+    DF_TX_CMD    = 8
 } dataflow_status_t;
 
 typedef enum SOR_codes {
@@ -38,9 +39,9 @@ typedef enum SOR_codes {
 } SOR_codes_t;
 
 typedef enum DOC_codes {
-    CMD_UNICAST    = 1,
-    CMD_MULTICAST  = 2,
-    CMD_BROADCAST  = 3,
-    RESERVED       = 4,
+    CMD_UNICAST    = 0x1,
+    CMD_MULTICAST  = 0x2,
+    CMD_BROADCAST  = 0x3,
+    RESERVED       = 0x4,
     DATA           = 0x5
 } DOC_codes_t;
diff --git a/src/main.c b/src/main.c
deleted file mode 100644
index 3ab25ac..0000000
--- a/src/main.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.h b/src/main.h
index 5d4be41..6579484 100644
--- a/src/main.h
+++ b/src/main.h
@@ -28,7 +28,7 @@ extern "C" {
 #endif

 

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

-#include "stm32f4xx_hal.h"

+#include "port.h"

 

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

 /* USER CODE BEGIN Includes */

diff --git a/src/main-master.c b/src/master.c
index c627a45..28ff4bb 100644
--- a/src/main-master.c
+++ b/src/master.c
@@ -40,7 +40,9 @@
 
 /* Private globals */
 I2C_HandleTypeDef hi2c1;
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
 UART_HandleTypeDef huart1;
+#endif
 
 device_info_t *device_info[BUS_DEVICE_LIMIT] = {NULL};
 subscription_info_t* subs_info[BUS_DEVICE_LIMIT];
@@ -48,21 +50,32 @@ 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;
+uint8_t *data_rbuf[ROUTING_BUFSIZE];        /*< Buffer to store data to be routed */
+uint8_t *cmd_routing_buf[ROUTING_BUFSIZE];  /*< Buffer to store commands to be routed */
+
+uint8_t    data_src_idx_rbuf[ROUTING_BUFSIZE];   /*< Index information for data source */
+uint8_t    cmd_src_idx_rbuf[ROUTING_BUFSIZE];    /*< Index information for command source */
+uint8_t    cmd_dst_idx_rbuf[ROUTING_BUFSIZE];    /*< Index information for command dest */
 
+uint32_t    data_len_buf[ROUTING_BUFSIZE];
+
+uint32_t   data_routing_ptr = 0;     /*< Pointer to tail of both data and data index buffers */
+uint32_t   cmd_routing_ptr = 0;      /*< Pointer to tail of cmd and cmd index buffers */
+    
 /* Function prototypes */
 void SystemClock_Config(void);
 static void MX_GPIO_Init(void);
 static void MX_I2C1_Init(void);
+
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
 static void MX_USART1_UART_Init(void);
+#endif
 
 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 cmd_routing(void);
+uint8_t get_CTS(uint8_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 decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
@@ -88,8 +101,11 @@ int main(void)
     /* Initialize all configured peripherals */
     MX_GPIO_Init();
     MX_I2C1_Init();
-    MX_USART1_UART_Init();
 
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
+    MX_USART1_UART_Init();
+#endif
+    
 #ifdef TESTING_ENABLE
 #ifdef MASTER
     uint8_t reset_string[] = "\r\n\n==========MASTER RESET=========\r\n\n";
@@ -100,35 +116,32 @@ int main(void)
 #endif /* MASTER */
 #endif /* TESTING_ENABLE */
     
-    uint8_t priority_counter = 0, debug_buf[128];
+    uint8_t priority_counter = 0;
     /* Handshake */
     while (1) {	
 	if (priority_counter == 0) {
 	    hs_status_t hs_status;
-	    /* for (uint8_t curr_addr=5; curr_addr == 5; curr_addr++) { */
-	    for (uint8_t curr_addr=0x1; curr_addr <= BUS_DEVICE_LIMIT; 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);
+	    for (int dev_idx = 0; dev_idx < BUS_DEVICE_LIMIT-1; dev_idx++) {
+		if (todo_hs_or_not_todo_hs(GET_ADDR_FROM_IDX(dev_idx))) {
+		    hs_status = handshake(GET_ADDR_FROM_IDX(dev_idx));
+		    dev_sts[dev_idx] = get_state_from_hs_status(GET_ADDR_FROM_IDX(dev_idx), hs_status);
 		}
 	    }
 	}
 
-	else if (priority_counter == 5 && routing_ptr > 0) {
-	    routing();
-	}
+	/* else if (priority_counter == 5 && routing_ptr > 0) { */
+	/*     routing(); */
+	/* } */
 	else {
 	    for (int device_idx = 0; device_idx < BUS_DEVICE_LIMIT-1; device_idx++) {
 		if (dev_sts[device_idx] == REGISTERED) {
 		    device_dataflow(GET_ADDR_FROM_IDX(device_idx), SLAVE_TX, 0);
 		}
 	    }
+	    routing();
+	    cmd_routing();
 	}
 	priority_counter = ((priority_counter+1)%10);
-
-	sprintf((char*)debug_buf, "routing ptr: %ld\r\n", routing_ptr);
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
     }   
 }
 
@@ -146,7 +159,7 @@ hs_status_t handshake(uint32_t i2c_addr)
     s2m_MDR_response MDR_res_message = s2m_MDR_response_init_default;
 
 #if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
-    uint8_t debug_buf[128];
+    uint8_t debug_buf[128] = {0};
 #endif
 #ifdef TESTING_ENABLE
     uint8_t term[] = "\r\n";
@@ -290,8 +303,8 @@ hs_status_t handshake(uint32_t i2c_addr)
 	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]);
+	sprintf((char*)debug_buf, "MDR Decode success\r\n\tFirst subscibed module: %x\r\n",
+		subs_info[dev_idx]->module_ids[0]);
 	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 	memset(debug_buf, 0, 128);
 	goto __MDR_DEC_TESTING_END;
@@ -304,7 +317,7 @@ __TESTING_BLOCK_END:
     {
 	goto __DEBUG_BLOCK_END;
     __HS_MDR_REQ_I2C_ERROR:
-	sprintf((char*)debug_buf, "Unable to send MDR request to %lx. I2C error: %ld\r\n", i2c_addr, HAL_I2C_GetError(&hi2c1));
+	sprintf((char*)debug_buf, "Unable to send MDR request to %lx. I2C error: %ld\r\n", i2c_addr>>1, 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;
@@ -336,24 +349,21 @@ __TESTING_BLOCK_END:
     return hs_sts;
 }
 
-dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t rbuf_data_idx)
+dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, volatile uint8_t rbuf_idx)
 {
     uint8_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);    
     dataflow_status_t df_status = DF_IDLE;
-
+    uint8_t CTS_buf[2] = {0x2, 0xFF};
+    
     uint8_t DOC_buf[4];
-    uint8_t *data_buf;
+    uint8_t cmd_dest;
+    
     uint32_t AF_error_counter = 0;
     uint32_t data_len         = 0;
-    _datapoint datapoints[16];
     
     /* TODO Add default values to the CTS message in proto */
-    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";
+    uint8_t debug_buf[128]={0};
 #endif
     
     while (df_status != DF_SUCCESS && df_status != DF_FAIL) {
@@ -375,11 +385,10 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		    df_status = DF_RX_DOC;
 		}
 		else if (SOR_code == SLAVE_RX_DATAPOINT) {
-		    /* TODO */
-		    df_status = DF_LEN_TX;
+		    df_status = DF_TX_DATA;
 		}
 		else if (SOR_code == SLAVE_RX_COMMAND) {
-		    /* TODO */
+		    df_status = DF_TX_CMD;
 		}
 	    }
 	    break;
@@ -391,7 +400,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
 	    AF_error_counter = 0;
 	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
-					  (uint8_t*)DOC_buf, 4, 10000) != HAL_OK) {
+					  (uint8_t*)DOC_buf, 4, 0xffff) != HAL_OK) {
 		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
 		    df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
@@ -407,12 +416,18 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		}
 	    }
 	    if (df_status != DF_FAIL) {
-		if (DOC_buf[1] == DATA) {
+		if (DOC_buf[0] == 0x0) {
+		    /* Do nothing DOC; should become redundant once dataflow is initiated using classes */
+		    df_status = DF_SUCCESS;
+		}
+		else if (DOC_buf[1] == DATA) {
 		    df_status = DF_CTS;
 		    data_len = DOC_buf[3];
 		}
 		else if (DOC_buf[1] == CMD_UNICAST) {
-		    /* TODO */
+		    df_status = DF_CTS;
+		    cmd_dest  = DOC_buf[0];
+		    data_len = DOC_buf[3];
 		}
 		else if (DOC_buf[1] == CMD_MULTICAST) {
 		    /* TODO */
@@ -429,8 +444,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	case (DF_CTS):
 	{
 	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
-	    uint8_t CTS_buf[2] = {0x2, 0xFF};
-	    if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 10000) != HAL_OK) {
+	    if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 1000) != HAL_OK) {
 		df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
 		goto __DF_CTS_I2C_ERROR;
@@ -443,22 +457,21 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		    df_status = DF_RX_DATA;
 		}
 		else {
-		    /* TODO RX CMD stuff */
+		    if (DOC_buf[1] == CMD_UNICAST) {
+			df_status = DF_RX_CMD;
+		    }
 		}
 	    }
 	    break;
 	}
 	case (DF_RX_DATA):
 	{
-	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
-	    sprintf((char*)debug_buf, "data len: %ld\r\n", data_len);
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-	    
-	    data_buf = (uint8_t*)malloc(128);
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);	    
+	    data_rbuf[data_routing_ptr] = malloc(sizeof(uint8_t)*data_len);
 	    AF_error_counter = 0;
 	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
-					  (uint8_t*)data_buf, data_len, 1000) != HAL_OK) {
+					  (uint8_t*)data_rbuf[data_routing_ptr],
+					  data_len, 1000) != HAL_OK) {
 		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
 		    df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
@@ -473,85 +486,81 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		    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;
-		}
+	    if (df_status != DF_FAIL) {		
+		data_src_idx_rbuf[data_routing_ptr] = dev_idx;
+		data_len_buf[data_routing_ptr] = (uint8_t)data_len;
+		data_routing_ptr++;
+		df_status = DF_SUCCESS;
+	    }
+	    else {
+		free(data_rbuf[data_routing_ptr]);
 	    }
 	    break;
 	}
-	case (DF_LEN_TX):
+	case (DF_TX_DATA):
 	{
 	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
 	    /* 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];
+	    /* Do this after handshake to cache ==================================== */
+	    uint8_t MDR_buf[128];
+	    uint8_t src_device_idx = data_src_idx_rbuf[rbuf_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_len = data_len_buf[rbuf_idx];
 	    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;
-		}
-	    }
-
+	    uint8_t status = get_CTS(i2c_addr);
 	    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) {
+	    if (status != 0 &&
+		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr,
+					data_MDR_len_buf, 4, 10000) == HAL_OK) {
+#ifdef DEBUG_ENABLE
 		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);
+#endif
 	    }
 	    else {
+#ifdef DEBUG_ENABLE
 		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);
+#endif
+	    }	    
+	    status = get_CTS(i2c_addr);
+	    if (status != 0 &&
+		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_rbuf[rbuf_idx],
+					data_len, 10000) == HAL_OK) {
+		df_status = DF_SUCCESS;
+#ifdef DEBUG_ENABLE
+		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);
+#endif
 	    }
+	    else {
+		df_status = DF_FAIL;
+	    }
+	    break;
+	}
+	case (DF_RX_CMD):
+	{
+	    uint8_t *cmd_buf;
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+	    cmd_buf = (uint8_t*)malloc(data_len);
 	    AF_error_counter = 0;
-	    while (df_status != DF_FAIL &&
-		   HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 10000) != HAL_OK) {
+	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, cmd_buf,
+					  data_len, 10000) != HAL_OK) {
 		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
 		    df_status = DF_FAIL;
 		}
@@ -559,49 +568,78 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		    df_status = DF_FAIL;
 		}
 		if (df_status == DF_FAIL) {
-		    sprintf((char*)debug_buf, "Failed to get TX CTS\r\n");
+		    free(cmd_buf);
+#ifdef DEBUG_ENABLE
+		    sprintf((char*)debug_buf, "Failed to get command, expected len: %ld\r\n", data_len);
 		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		    memset(debug_buf, 0, 128);
+#endif
 		    break;
 		}
 	    }
+	    if (df_status != DF_FAIL) {
+		cmd_routing_buf[cmd_routing_ptr] = malloc(sizeof(uint8_t)*data_len);
+		memcpy(cmd_routing_buf[cmd_routing_ptr], cmd_buf, data_len);
+		free(cmd_buf);
+		
+		cmd_src_idx_rbuf[cmd_routing_ptr] = dev_idx;
+		cmd_dst_idx_rbuf[cmd_routing_ptr] = cmd_dest;
+		cmd_routing_ptr++;
 
-	    
-#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);
+#ifdef DEBUG_ENABLE
+		sprintf((char*)debug_buf, "Got command\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
 #endif
+		df_status = DF_SUCCESS;
+	    }
+	    break;
+	}
+	case (DF_TX_CMD):
+	{
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
+	    uint8_t data_len = sizeof(cmd_routing_buf[rbuf_idx]);
+	    uint8_t len_buf[] = {0x0, data_len, 0x0, 0x0};
 	    
-	    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");
+	    uint8_t status = get_CTS(i2c_addr);	    
+	    if (status != 0 &&
+		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, len_buf,
+					4, 10000) != HAL_OK) {
+		df_status = DF_FAIL;
+#ifdef DEBUG_ENABLE
+		sprintf((char*)debug_buf, "Failed to send cmd len buf to %d\r\n", i2c_addr>>1);
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-
-		df_status = DF_SUCCESS;
+#endif
+	    }
+	    
+	    if (df_status != DF_FAIL) {
+		uint8_t status = get_CTS(i2c_addr);
+		if (status != 0 &&
+		    HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr,
+					    cmd_routing_buf[rbuf_idx],
+					    4, 10000) == HAL_OK) {
+		    df_status = DF_SUCCESS;
+#ifdef DEBUG_ENABLE
+		    sprintf((char*)debug_buf, "Routed cmd to %d\r\n", i2c_addr>>1);
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+#endif
+		}
+		else {
+		    df_status = DF_FAIL;
+#ifdef DEBUG_ENABLE
+		    sprintf((char*)debug_buf, "Failed to send cmd to %d\r\n", i2c_addr>>1);
+		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		    memset(debug_buf, 0, 128);
+#endif
+		}
 	    }
 	    break;
 	}
 	case DF_SUCCESS:
 	case DF_FAIL:
-	    break;	    
+	    break;
 	}
     }
     
@@ -618,7 +656,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	goto __DF_DEBUG_BLOCK_END;
     __DF_SOR_I2C_ERROR:
 	sprintf((char*)debug_buf, "Unable to send SOR request to %d. I2C error: %ld\r\n",
-		i2c_addr, HAL_I2C_GetError(&hi2c1));
+		i2c_addr>>1, 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;
@@ -637,11 +675,6 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	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");
     }
@@ -650,21 +683,54 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
     return df_status;
 }
 
+uint8_t get_CTS(uint8_t i2c_addr)
+{
+    uint8_t CTS_buf[2], AF_error_counter = 0;
+
+    #if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
+	debug_buf[20] = {0};
+    #endif
+	
+    uint8_t status = 1;
+    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) {
+	    status = 0;
+	}
+	if (++AF_error_counter > 1000) {
+	    status = 0;
+	}
+	if (status == 0) {
+#ifdef DEBUG_ENABLE
+	    sprintf((char*)debug_buf, "Failed to get CTS\r\n");
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+#endif
+	    break;
+	}
+    }
+    return status;
+}
+
+/**
+ * @brief Dataflow routing
+ *
+ */
 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 src_module_idx = routing_idx_buffer[rbuf_data_idx];
+    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < data_routing_ptr; rbuf_data_idx++) {
+	uint8_t src_module_idx = data_src_idx_rbuf[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
+	    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[src_module_idx]->MDR.module_id) {
+	    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[src_module_idx]->MDR.module_id) {
 		    SET_BIT_FROM_IDX(routing_table[rbuf_data_idx], dev_idx);
 		    alloc = true;
 		}
@@ -673,17 +739,40 @@ bool routing(void)
 	}
     }
 
-    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < routing_ptr; rbuf_data_idx++) {
+    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < data_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);
 	    }
 	}
+	free(data_rbuf[rbuf_data_idx]);
     }
 
     /* Reset the routing pointer, since all data in buffer should have been routed */    
-    routing_ptr = 0;
+    data_routing_ptr = 0;
+    return true;
+}
+
+/**
+ * @brief Routing specific to commands. Currently only supports Unicast commands, multicast and 
+ * broadcast support to be added in a later version if deemed necessary.
+ *
+ */
+bool cmd_routing(void)
+{
+    /* uint32_t routing_table[4]; */
+    for (uint8_t rbuf_cmd_idx = 0; rbuf_cmd_idx < cmd_routing_ptr; rbuf_cmd_idx++) {
+	uint8_t dst_module_idx = cmd_dst_idx_rbuf[rbuf_cmd_idx];
+	for (int dev_idx = 0; dev_idx < BUS_DEVICE_LIMIT; dev_idx++) {
+	    if (GET_BIT_FROM_IDX(allocated, dev_idx) &&
+		device_info[dev_idx]->MDR.module_id == dst_module_idx) {
+	    	device_dataflow(GET_ADDR_FROM_IDX(dev_idx), SLAVE_RX_COMMAND, rbuf_cmd_idx);
+	    }
+	}
+	free(cmd_routing_buf[rbuf_cmd_idx]);
+    }
+    cmd_routing_ptr = 0;
     return true;
 }
 
@@ -888,7 +977,7 @@ void SystemClock_Config(void)
 static void MX_I2C1_Init(void)
 {
     hi2c1.Instance = I2C1;
-    hi2c1.Init.ClockSpeed = 100000;
+    hi2c1.Init.ClockSpeed = 400000;
     hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
     hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
     hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
@@ -908,9 +997,10 @@ static void MX_I2C1_Init(void)
  * @param None
  * @retval None
  */
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
 static void MX_USART1_UART_Init(void)
 {
-    huart1.Instance = USART1;
+    huart1.Instance = USART2;
     huart1.Init.BaudRate = 9600;
     huart1.Init.WordLength = UART_WORDLENGTH_8B;
     huart1.Init.StopBits = UART_STOPBITS_1;
@@ -924,7 +1014,7 @@ static void MX_USART1_UART_Init(void)
     }
 
 }
-
+#endif
 /**
  * @brief GPIO Initialization Function
  * @param None
diff --git a/src/pb.h b/src/pb.h
deleted file mode 100644
index fc0f387..0000000
--- a/src/pb.h
+++ /dev/null
@@ -1,830 +0,0 @@
-/* Common parts of the nanopb library. Most of these are quite low-level
- * stuff. For the high-level interface, see pb_encode.h and pb_decode.h.
- */
-
-#ifndef PB_H_INCLUDED
-#define PB_H_INCLUDED
-
-/*****************************************************************
- * Nanopb compilation time options. You can change these here by *
- * uncommenting the lines, or on the compiler command line.      *
- *****************************************************************/
-
-/* Enable support for dynamically allocated fields */
-#define PB_ENABLE_MALLOC 1
-
-/* Define this if your CPU / compiler combination does not support
- * unaligned memory access to packed structures. */
-/* #define PB_NO_PACKED_STRUCTS 1 */
-
-/* Increase the number of required fields that are tracked.
- * A compiler warning will tell if you need this. */
-/* #define PB_MAX_REQUIRED_FIELDS 256 */
-
-/* Add support for tag numbers > 65536 and fields larger than 65536 bytes. */
-/* #define PB_FIELD_32BIT 1 */
-
-/* Disable support for error messages in order to save some code space. */
-/* #define PB_NO_ERRMSG 1 */
-
-/* Disable support for custom streams (support only memory buffers). */
-#define PB_BUFFER_ONLY 1
-
-/* Disable support for 64-bit datatypes, for compilers without int64_t
-   or to save some code space. */
-/* #define PB_WITHOUT_64BIT 1 */
-
-/* Don't encode scalar arrays as packed. This is only to be used when
- * the decoder on the receiving side cannot process packed scalar arrays.
- * Such example is older protobuf.js. */
-/* #define PB_ENCODE_ARRAYS_UNPACKED 1 */
-
-/* Enable conversion of doubles to floats for platforms that do not
- * support 64-bit doubles. Most commonly AVR. */
-/* #define PB_CONVERT_DOUBLE_FLOAT 1 */
-
-/* Check whether incoming strings are valid UTF-8 sequences. Slows down
- * the string processing slightly and slightly increases code size. */
-/* #define PB_VALIDATE_UTF8 1 */
-
-/******************************************************************
- * You usually don't need to change anything below this line.     *
- * Feel free to look around and use the defined macros, though.   *
- ******************************************************************/
-
-
-/* Version of the nanopb library. Just in case you want to check it in
- * your own program. */
-#define NANOPB_VERSION nanopb-0.4.2-dev
-
-/* Include all the system headers needed by nanopb. You will need the
- * definitions of the following:
- * - strlen, memcpy, memset functions
- * - [u]int_least8_t, uint_fast8_t, [u]int_least16_t, [u]int32_t, [u]int64_t
- * - size_t
- * - bool
- *
- * If you don't have the standard header files, you can instead provide
- * a custom header that defines or includes all this. In that case,
- * define PB_SYSTEM_HEADER to the path of this file.
- */
-#ifdef PB_SYSTEM_HEADER
-#include PB_SYSTEM_HEADER
-#else
-#include <stdint.h>
-#include <stddef.h>
-#include <stdbool.h>
-#include <string.h>
-#include <limits.h>
-
-#ifdef PB_ENABLE_MALLOC
-#include <stdlib.h>
-#endif
-#endif
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* Macro for defining packed structures (compiler dependent).
- * This just reduces memory requirements, but is not required.
- */
-#if defined(PB_NO_PACKED_STRUCTS)
-    /* Disable struct packing */
-#   define PB_PACKED_STRUCT_START
-#   define PB_PACKED_STRUCT_END
-#   define pb_packed
-#elif defined(__GNUC__) || defined(__clang__)
-    /* For GCC and clang */
-#   define PB_PACKED_STRUCT_START
-#   define PB_PACKED_STRUCT_END
-#   define pb_packed __attribute__((packed))
-#elif defined(__ICCARM__) || defined(__CC_ARM)
-    /* For IAR ARM and Keil MDK-ARM compilers */
-#   define PB_PACKED_STRUCT_START _Pragma("pack(push, 1)")
-#   define PB_PACKED_STRUCT_END _Pragma("pack(pop)")
-#   define pb_packed
-#elif defined(_MSC_VER) && (_MSC_VER >= 1500)
-    /* For Microsoft Visual C++ */
-#   define PB_PACKED_STRUCT_START __pragma(pack(push, 1))
-#   define PB_PACKED_STRUCT_END __pragma(pack(pop))
-#   define pb_packed
-#else
-    /* Unknown compiler */
-#   define PB_PACKED_STRUCT_START
-#   define PB_PACKED_STRUCT_END
-#   define pb_packed
-#endif
-
-/* Handly macro for suppressing unreferenced-parameter compiler warnings. */
-#ifndef PB_UNUSED
-#define PB_UNUSED(x) (void)(x)
-#endif
-
-/* Harvard-architecture processors may need special attributes for storing
- * field information in program memory. */
-#ifndef PB_PROGMEM
-#ifdef __AVR__
-#include <avr/pgmspace.h>
-#define PB_PROGMEM             PROGMEM
-#define PB_PROGMEM_READU32(x)  pgm_read_dword(&x)
-#else
-#define PB_PROGMEM
-#define PB_PROGMEM_READU32(x)  (x)
-#endif
-#endif
-
-/* Compile-time assertion, used for checking compatible compilation options.
- * If this does not work properly on your compiler, use
- * #define PB_NO_STATIC_ASSERT to disable it.
- *
- * But before doing that, check carefully the error message / place where it
- * comes from to see if the error has a real cause. Unfortunately the error
- * message is not always very clear to read, but you can see the reason better
- * in the place where the PB_STATIC_ASSERT macro was called.
- */
-#ifndef PB_NO_STATIC_ASSERT
-#  ifndef PB_STATIC_ASSERT
-#    if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L
-       /* C11 standard _Static_assert mechanism */
-#      define PB_STATIC_ASSERT(COND,MSG) _Static_assert(COND,#MSG);
-#    else
-       /* Classic negative-size-array static assert mechanism */
-#      define PB_STATIC_ASSERT(COND,MSG) typedef char PB_STATIC_ASSERT_MSG(MSG, __LINE__, __COUNTER__)[(COND)?1:-1];
-#      define PB_STATIC_ASSERT_MSG(MSG, LINE, COUNTER) PB_STATIC_ASSERT_MSG_(MSG, LINE, COUNTER)
-#      define PB_STATIC_ASSERT_MSG_(MSG, LINE, COUNTER) pb_static_assertion_##MSG##_##LINE##_##COUNTER
-#    endif
-#  endif
-#else
-   /* Static asserts disabled by PB_NO_STATIC_ASSERT */
-#  define PB_STATIC_ASSERT(COND,MSG)
-#endif
-
-/* Number of required fields to keep track of. */
-#ifndef PB_MAX_REQUIRED_FIELDS
-#define PB_MAX_REQUIRED_FIELDS 64
-#endif
-
-#if PB_MAX_REQUIRED_FIELDS < 64
-#error You should not lower PB_MAX_REQUIRED_FIELDS from the default value (64).
-#endif
-
-#ifdef PB_WITHOUT_64BIT
-#ifdef PB_CONVERT_DOUBLE_FLOAT
-/* Cannot use doubles without 64-bit types */
-#undef PB_CONVERT_DOUBLE_FLOAT
-#endif
-#endif
-
-/* List of possible field types. These are used in the autogenerated code.
- * Least-significant 4 bits tell the scalar type
- * Most-significant 4 bits specify repeated/required/packed etc.
- */
-
-typedef uint_least8_t pb_type_t;
-
-/**** Field data types ****/
-
-/* Numeric types */
-#define PB_LTYPE_BOOL    0x00U /* bool */
-#define PB_LTYPE_VARINT  0x01U /* int32, int64, enum, bool */
-#define PB_LTYPE_UVARINT 0x02U /* uint32, uint64 */
-#define PB_LTYPE_SVARINT 0x03U /* sint32, sint64 */
-#define PB_LTYPE_FIXED32 0x04U /* fixed32, sfixed32, float */
-#define PB_LTYPE_FIXED64 0x05U /* fixed64, sfixed64, double */
-
-/* Marker for last packable field type. */
-#define PB_LTYPE_LAST_PACKABLE 0x05U
-
-/* Byte array with pre-allocated buffer.
- * data_size is the length of the allocated PB_BYTES_ARRAY structure. */
-#define PB_LTYPE_BYTES 0x06U
-
-/* String with pre-allocated buffer.
- * data_size is the maximum length. */
-#define PB_LTYPE_STRING 0x07U
-
-/* Submessage
- * submsg_fields is pointer to field descriptions */
-#define PB_LTYPE_SUBMESSAGE 0x08U
-
-/* Submessage with pre-decoding callback
- * The pre-decoding callback is stored as pb_callback_t right before pSize.
- * submsg_fields is pointer to field descriptions */
-#define PB_LTYPE_SUBMSG_W_CB 0x09U
-
-/* Extension pseudo-field
- * The field contains a pointer to pb_extension_t */
-#define PB_LTYPE_EXTENSION 0x0AU
-
-/* Byte array with inline, pre-allocated byffer.
- * data_size is the length of the inline, allocated buffer.
- * This differs from PB_LTYPE_BYTES by defining the element as
- * pb_byte_t[data_size] rather than pb_bytes_array_t. */
-#define PB_LTYPE_FIXED_LENGTH_BYTES 0x0BU
-
-/* Number of declared LTYPES */
-#define PB_LTYPES_COUNT 0x0CU
-#define PB_LTYPE_MASK 0x0FU
-
-/**** Field repetition rules ****/
-
-#define PB_HTYPE_REQUIRED 0x00U
-#define PB_HTYPE_OPTIONAL 0x10U
-#define PB_HTYPE_SINGULAR 0x10U
-#define PB_HTYPE_REPEATED 0x20U
-#define PB_HTYPE_FIXARRAY 0x20U
-#define PB_HTYPE_ONEOF    0x30U
-#define PB_HTYPE_MASK     0x30U
-
-/**** Field allocation types ****/
- 
-#define PB_ATYPE_STATIC   0x00U
-#define PB_ATYPE_POINTER  0x80U
-#define PB_ATYPE_CALLBACK 0x40U
-#define PB_ATYPE_MASK     0xC0U
-
-#define PB_ATYPE(x) ((x) & PB_ATYPE_MASK)
-#define PB_HTYPE(x) ((x) & PB_HTYPE_MASK)
-#define PB_LTYPE(x) ((x) & PB_LTYPE_MASK)
-#define PB_LTYPE_IS_SUBMSG(x) (PB_LTYPE(x) == PB_LTYPE_SUBMESSAGE || \
-                               PB_LTYPE(x) == PB_LTYPE_SUBMSG_W_CB)
-
-/* Data type used for storing sizes of struct fields
- * and array counts.
- */
-#if defined(PB_FIELD_32BIT)
-    typedef uint32_t pb_size_t;
-    typedef int32_t pb_ssize_t;
-#else
-    typedef uint_least16_t pb_size_t;
-    typedef int_least16_t pb_ssize_t;
-#endif
-#define PB_SIZE_MAX ((pb_size_t)-1)
-
-/* Data type for storing encoded data and other byte streams.
- * This typedef exists to support platforms where uint8_t does not exist.
- * You can regard it as equivalent on uint8_t on other platforms.
- */
-typedef uint_least8_t pb_byte_t;
-
-/* Forward declaration of struct types */
-typedef struct pb_istream_s pb_istream_t;
-typedef struct pb_ostream_s pb_ostream_t;
-typedef struct pb_field_iter_s pb_field_iter_t;
-
-/* This structure is used in auto-generated constants
- * to specify struct fields.
- */
-PB_PACKED_STRUCT_START
-typedef struct pb_msgdesc_s pb_msgdesc_t;
-struct pb_msgdesc_s {
-    pb_size_t field_count;
-    const uint32_t *field_info;
-    const pb_msgdesc_t * const * submsg_info;
-    const pb_byte_t *default_value;
-
-    bool (*field_callback)(pb_istream_t *istream, pb_ostream_t *ostream, const pb_field_iter_t *field);
-} pb_packed;
-PB_PACKED_STRUCT_END
-
-/* Iterator for message descriptor */
-struct pb_field_iter_s {
-    const pb_msgdesc_t *descriptor;  /* Pointer to message descriptor constant */
-    void *message;                   /* Pointer to start of the structure */
-
-    pb_size_t index;                 /* Index of the field */
-    pb_size_t field_info_index;      /* Index to descriptor->field_info array */
-    pb_size_t required_field_index;  /* Index that counts only the required fields */
-    pb_size_t submessage_index;      /* Index that counts only submessages */
-
-    pb_size_t tag;                   /* Tag of current field */
-    pb_size_t data_size;             /* sizeof() of a single item */
-    pb_size_t array_size;            /* Number of array entries */
-    pb_type_t type;                  /* Type of current field */
-
-    void *pField;                    /* Pointer to current field in struct */
-    void *pData;                     /* Pointer to current data contents. Different than pField for arrays and pointers. */
-    void *pSize;                     /* Pointer to count/has field */
-
-    const pb_msgdesc_t *submsg_desc; /* For submessage fields, pointer to field descriptor for the submessage. */
-};
-
-/* For compatibility with legacy code */
-typedef pb_field_iter_t pb_field_t;
-
-/* Make sure that the standard integer types are of the expected sizes.
- * Otherwise fixed32/fixed64 fields can break.
- *
- * If you get errors here, it probably means that your stdint.h is not
- * correct for your platform.
- */
-#ifndef PB_WITHOUT_64BIT
-PB_STATIC_ASSERT(sizeof(int64_t) == 2 * sizeof(int32_t), INT64_T_WRONG_SIZE)
-PB_STATIC_ASSERT(sizeof(uint64_t) == 2 * sizeof(uint32_t), UINT64_T_WRONG_SIZE)
-#endif
-
-/* This structure is used for 'bytes' arrays.
- * It has the number of bytes in the beginning, and after that an array.
- * Note that actual structs used will have a different length of bytes array.
- */
-#define PB_BYTES_ARRAY_T(n) struct { pb_size_t size; pb_byte_t bytes[n]; }
-#define PB_BYTES_ARRAY_T_ALLOCSIZE(n) ((size_t)n + offsetof(pb_bytes_array_t, bytes))
-
-struct pb_bytes_array_s {
-    pb_size_t size;
-    pb_byte_t bytes[1];
-};
-typedef struct pb_bytes_array_s pb_bytes_array_t;
-
-/* This structure is used for giving the callback function.
- * It is stored in the message structure and filled in by the method that
- * calls pb_decode.
- *
- * The decoding callback will be given a limited-length stream
- * If the wire type was string, the length is the length of the string.
- * If the wire type was a varint/fixed32/fixed64, the length is the length
- * of the actual value.
- * The function may be called multiple times (especially for repeated types,
- * but also otherwise if the message happens to contain the field multiple
- * times.)
- *
- * The encoding callback will receive the actual output stream.
- * It should write all the data in one call, including the field tag and
- * wire type. It can write multiple fields.
- *
- * The callback can be null if you want to skip a field.
- */
-typedef struct pb_callback_s pb_callback_t;
-struct pb_callback_s {
-    /* Callback functions receive a pointer to the arg field.
-     * You can access the value of the field as *arg, and modify it if needed.
-     */
-    union {
-        bool (*decode)(pb_istream_t *stream, const pb_field_t *field, void **arg);
-        bool (*encode)(pb_ostream_t *stream, const pb_field_t *field, void * const *arg);
-    } funcs;
-    
-    /* Free arg for use by callback */
-    void *arg;
-};
-
-extern bool pb_default_field_callback(pb_istream_t *istream, pb_ostream_t *ostream, const pb_field_t *field);
-
-/* Wire types. Library user needs these only in encoder callbacks. */
-typedef enum {
-    PB_WT_VARINT = 0,
-    PB_WT_64BIT  = 1,
-    PB_WT_STRING = 2,
-    PB_WT_32BIT  = 5
-} pb_wire_type_t;
-
-/* Structure for defining the handling of unknown/extension fields.
- * Usually the pb_extension_type_t structure is automatically generated,
- * while the pb_extension_t structure is created by the user. However,
- * if you want to catch all unknown fields, you can also create a custom
- * pb_extension_type_t with your own callback.
- */
-typedef struct pb_extension_type_s pb_extension_type_t;
-typedef struct pb_extension_s pb_extension_t;
-struct pb_extension_type_s {
-    /* Called for each unknown field in the message.
-     * If you handle the field, read off all of its data and return true.
-     * If you do not handle the field, do not read anything and return true.
-     * If you run into an error, return false.
-     * Set to NULL for default handler.
-     */
-    bool (*decode)(pb_istream_t *stream, pb_extension_t *extension,
-                   uint32_t tag, pb_wire_type_t wire_type);
-    
-    /* Called once after all regular fields have been encoded.
-     * If you have something to write, do so and return true.
-     * If you do not have anything to write, just return true.
-     * If you run into an error, return false.
-     * Set to NULL for default handler.
-     */
-    bool (*encode)(pb_ostream_t *stream, const pb_extension_t *extension);
-    
-    /* Free field for use by the callback. */
-    const void *arg;
-};
-
-struct pb_extension_s {
-    /* Type describing the extension field. Usually you'll initialize
-     * this to a pointer to the automatically generated structure. */
-    const pb_extension_type_t *type;
-    
-    /* Destination for the decoded data. This must match the datatype
-     * of the extension field. */
-    void *dest;
-    
-    /* Pointer to the next extension handler, or NULL.
-     * If this extension does not match a field, the next handler is
-     * automatically called. */
-    pb_extension_t *next;
-
-    /* The decoder sets this to true if the extension was found.
-     * Ignored for encoding. */
-    bool found;
-};
-
-#define pb_extension_init_zero {NULL,NULL,NULL,false}
-
-/* Memory allocation functions to use. You can define pb_realloc and
- * pb_free to custom functions if you want. */
-#ifdef PB_ENABLE_MALLOC
-#   ifndef pb_realloc
-#       define pb_realloc(ptr, size) realloc(ptr, size)
-#   endif
-#   ifndef pb_free
-#       define pb_free(ptr) free(ptr)
-#   endif
-#endif
-
-/* This is used to inform about need to regenerate .pb.h/.pb.c files. */
-#define PB_PROTO_HEADER_VERSION 40
-
-/* These macros are used to declare pb_field_t's in the constant array. */
-/* Size of a structure member, in bytes. */
-#define pb_membersize(st, m) (sizeof ((st*)0)->m)
-/* Number of entries in an array. */
-#define pb_arraysize(st, m) (pb_membersize(st, m) / pb_membersize(st, m[0]))
-/* Delta from start of one member to the start of another member. */
-#define pb_delta(st, m1, m2) ((int)offsetof(st, m1) - (int)offsetof(st, m2))
-
-/* Force expansion of macro value */
-#define PB_EXPAND(x) x
-
-/* Binding of a message field set into a specific structure */
-#define PB_BIND(msgname, structname, width) \
-    const uint32_t structname ## _field_info[] PB_PROGMEM = \
-    { \
-        msgname ## _FIELDLIST(PB_GEN_FIELD_INFO_ ## width, structname) \
-        0 \
-    }; \
-    const pb_msgdesc_t* const structname ## _submsg_info[] = \
-    { \
-        msgname ## _FIELDLIST(PB_GEN_SUBMSG_INFO, structname) \
-        NULL \
-    }; \
-    const pb_msgdesc_t structname ## _msg = \
-    { \
-       0 msgname ## _FIELDLIST(PB_GEN_FIELD_COUNT, structname), \
-       structname ## _field_info, \
-       structname ## _submsg_info, \
-       msgname ## _DEFAULT, \
-       msgname ## _CALLBACK, \
-    }; \
-    msgname ## _FIELDLIST(PB_GEN_FIELD_INFO_ASSERT_ ## width, structname)
-
-#define PB_GEN_FIELD_COUNT(structname, atype, htype, ltype, fieldname, tag) +1
-
-#define PB_GEN_FIELD_INFO_1(structname, atype, htype, ltype, fieldname, tag) \
-    PB_GEN_FIELD_INFO(1, structname, atype, htype, ltype, fieldname, tag)
-
-#define PB_GEN_FIELD_INFO_2(structname, atype, htype, ltype, fieldname, tag) \
-    PB_GEN_FIELD_INFO(2, structname, atype, htype, ltype, fieldname, tag)
-
-#define PB_GEN_FIELD_INFO_4(structname, atype, htype, ltype, fieldname, tag) \
-    PB_GEN_FIELD_INFO(4, structname, atype, htype, ltype, fieldname, tag)
-
-#define PB_GEN_FIELD_INFO_8(structname, atype, htype, ltype, fieldname, tag) \
-    PB_GEN_FIELD_INFO(8, structname, atype, htype, ltype, fieldname, tag)
-
-#define PB_GEN_FIELD_INFO_AUTO(structname, atype, htype, ltype, fieldname, tag) \
-    PB_GEN_FIELD_INFO_AUTO2(PB_FIELDINFO_WIDTH_AUTO(atype, htype, ltype), structname, atype, htype, ltype, fieldname, tag)
-
-#define PB_GEN_FIELD_INFO_AUTO2(width, structname, atype, htype, ltype, fieldname, tag) \
-    PB_GEN_FIELD_INFO(width, structname, atype, htype, ltype, fieldname, tag)
-
-#define PB_GEN_FIELD_INFO(width, structname, atype, htype, ltype, fieldname, tag) \
-    PB_FIELDINFO_ ## width(tag, PB_ATYPE_ ## atype | PB_HTYPE_ ## htype | PB_LTYPE_MAP_ ## ltype, \
-                   PB_DATA_OFFSET_ ## atype(htype, structname, fieldname), \
-                   PB_DATA_SIZE_ ## atype(htype, structname, fieldname), \
-                   PB_SIZE_OFFSET_ ## atype(htype, structname, fieldname), \
-                   PB_ARRAY_SIZE_ ## atype(htype, structname, fieldname))
-
-#define PB_GEN_FIELD_INFO_ASSERT_1(structname, atype, htype, ltype, fieldname, tag) \
-    PB_GEN_FIELD_INFO_ASSERT(1, structname, atype, htype, ltype, fieldname, tag)
-
-#define PB_GEN_FIELD_INFO_ASSERT_2(structname, atype, htype, ltype, fieldname, tag) \
-    PB_GEN_FIELD_INFO_ASSERT(2, structname, atype, htype, ltype, fieldname, tag)
-
-#define PB_GEN_FIELD_INFO_ASSERT_4(structname, atype, htype, ltype, fieldname, tag) \
-    PB_GEN_FIELD_INFO_ASSERT(4, structname, atype, htype, ltype, fieldname, tag)
-
-#define PB_GEN_FIELD_INFO_ASSERT_8(structname, atype, htype, ltype, fieldname, tag) \
-    PB_GEN_FIELD_INFO_ASSERT(8, structname, atype, htype, ltype, fieldname, tag)
-
-#define PB_GEN_FIELD_INFO_ASSERT_AUTO(structname, atype, htype, ltype, fieldname, tag) \
-    PB_GEN_FIELD_INFO_ASSERT_AUTO2(PB_FIELDINFO_WIDTH_AUTO(atype, htype, ltype), structname, atype, htype, ltype, fieldname, tag)
-
-#define PB_GEN_FIELD_INFO_ASSERT_AUTO2(width, structname, atype, htype, ltype, fieldname, tag) \
-    PB_GEN_FIELD_INFO_ASSERT(width, structname, atype, htype, ltype, fieldname, tag)
-
-#define PB_GEN_FIELD_INFO_ASSERT(width, structname, atype, htype, ltype, fieldname, tag) \
-    PB_FIELDINFO_ASSERT_ ## width(tag, PB_ATYPE_ ## atype | PB_HTYPE_ ## htype | PB_LTYPE_MAP_ ## ltype, \
-                   PB_DATA_OFFSET_ ## atype(htype, structname, fieldname), \
-                   PB_DATA_SIZE_ ## atype(htype, structname, fieldname), \
-                   PB_SIZE_OFFSET_ ## atype(htype, structname, fieldname), \
-                   PB_ARRAY_SIZE_ ## atype(htype, structname, fieldname))
-
-#define PB_DATA_OFFSET_STATIC(htype, structname, fieldname) PB_DATA_OFFSET_ ## htype(structname, fieldname)
-#define PB_DATA_OFFSET_POINTER(htype, structname, fieldname) PB_DATA_OFFSET_ ## htype(structname, fieldname)
-#define PB_DATA_OFFSET_CALLBACK(htype, structname, fieldname) PB_DATA_OFFSET_ ## htype(structname, fieldname)
-#define PB_DATA_OFFSET_REQUIRED(structname, fieldname) offsetof(structname, fieldname)
-#define PB_DATA_OFFSET_SINGULAR(structname, fieldname) offsetof(structname, fieldname)
-#define PB_DATA_OFFSET_ONEOF(structname, fieldname) offsetof(structname, PB_ONEOF_NAME(FULL, fieldname))
-#define PB_DATA_OFFSET_OPTIONAL(structname, fieldname) offsetof(structname, fieldname)
-#define PB_DATA_OFFSET_REPEATED(structname, fieldname) offsetof(structname, fieldname)
-#define PB_DATA_OFFSET_FIXARRAY(structname, fieldname) offsetof(structname, fieldname)
-
-#define PB_SIZE_OFFSET_STATIC(htype, structname, fieldname) PB_SIZE_OFFSET_ ## htype(structname, fieldname)
-#define PB_SIZE_OFFSET_POINTER(htype, structname, fieldname) PB_SIZE_OFFSET_PTR_ ## htype(structname, fieldname)
-#define PB_SIZE_OFFSET_CALLBACK(htype, structname, fieldname) PB_SIZE_OFFSET_CB_ ## htype(structname, fieldname)
-#define PB_SIZE_OFFSET_REQUIRED(structname, fieldname) 0
-#define PB_SIZE_OFFSET_SINGULAR(structname, fieldname) 0
-#define PB_SIZE_OFFSET_ONEOF(structname, fieldname) PB_SIZE_OFFSET_ONEOF2(structname, PB_ONEOF_NAME(FULL, fieldname), PB_ONEOF_NAME(UNION, fieldname))
-#define PB_SIZE_OFFSET_ONEOF2(structname, fullname, unionname) PB_SIZE_OFFSET_ONEOF3(structname, fullname, unionname)
-#define PB_SIZE_OFFSET_ONEOF3(structname, fullname, unionname) pb_delta(structname, fullname, which_ ## unionname)
-#define PB_SIZE_OFFSET_OPTIONAL(structname, fieldname) pb_delta(structname, fieldname, has_ ## fieldname)
-#define PB_SIZE_OFFSET_REPEATED(structname, fieldname) pb_delta(structname, fieldname, fieldname ## _count)
-#define PB_SIZE_OFFSET_FIXARRAY(structname, fieldname) 0
-#define PB_SIZE_OFFSET_PTR_REQUIRED(structname, fieldname) 0
-#define PB_SIZE_OFFSET_PTR_SINGULAR(structname, fieldname) 0
-#define PB_SIZE_OFFSET_PTR_ONEOF(structname, fieldname) PB_SIZE_OFFSET_ONEOF(structname, fieldname)
-#define PB_SIZE_OFFSET_PTR_OPTIONAL(structname, fieldname) 0
-#define PB_SIZE_OFFSET_PTR_REPEATED(structname, fieldname) PB_SIZE_OFFSET_REPEATED(structname, fieldname)
-#define PB_SIZE_OFFSET_PTR_FIXARRAY(structname, fieldname) 0
-#define PB_SIZE_OFFSET_CB_REQUIRED(structname, fieldname) 0
-#define PB_SIZE_OFFSET_CB_SINGULAR(structname, fieldname) 0
-#define PB_SIZE_OFFSET_CB_ONEOF(structname, fieldname) PB_SIZE_OFFSET_ONEOF(structname, fieldname)
-#define PB_SIZE_OFFSET_CB_OPTIONAL(structname, fieldname) 0
-#define PB_SIZE_OFFSET_CB_REPEATED(structname, fieldname) 0
-#define PB_SIZE_OFFSET_CB_FIXARRAY(structname, fieldname) 0
-
-#define PB_ARRAY_SIZE_STATIC(htype, structname, fieldname) PB_ARRAY_SIZE_ ## htype(structname, fieldname)
-#define PB_ARRAY_SIZE_POINTER(htype, structname, fieldname) PB_ARRAY_SIZE_PTR_ ## htype(structname, fieldname)
-#define PB_ARRAY_SIZE_CALLBACK(htype, structname, fieldname) 1
-#define PB_ARRAY_SIZE_REQUIRED(structname, fieldname) 1
-#define PB_ARRAY_SIZE_SINGULAR(structname, fieldname) 1
-#define PB_ARRAY_SIZE_OPTIONAL(structname, fieldname) 1
-#define PB_ARRAY_SIZE_ONEOF(structname, fieldname) 1
-#define PB_ARRAY_SIZE_REPEATED(structname, fieldname) pb_arraysize(structname, fieldname)
-#define PB_ARRAY_SIZE_FIXARRAY(structname, fieldname) pb_arraysize(structname, fieldname)
-#define PB_ARRAY_SIZE_PTR_REQUIRED(structname, fieldname) 1
-#define PB_ARRAY_SIZE_PTR_SINGULAR(structname, fieldname) 1
-#define PB_ARRAY_SIZE_PTR_OPTIONAL(structname, fieldname) 1
-#define PB_ARRAY_SIZE_PTR_ONEOF(structname, fieldname) 1
-#define PB_ARRAY_SIZE_PTR_REPEATED(structname, fieldname) 1
-#define PB_ARRAY_SIZE_PTR_FIXARRAY(structname, fieldname) pb_arraysize(structname, fieldname[0])
-
-#define PB_DATA_SIZE_STATIC(htype, structname, fieldname) PB_DATA_SIZE_ ## htype(structname, fieldname)
-#define PB_DATA_SIZE_POINTER(htype, structname, fieldname) PB_DATA_SIZE_PTR_ ## htype(structname, fieldname)
-#define PB_DATA_SIZE_CALLBACK(htype, structname, fieldname) PB_DATA_SIZE_CB_ ## htype(structname, fieldname)
-#define PB_DATA_SIZE_REQUIRED(structname, fieldname) pb_membersize(structname, fieldname)
-#define PB_DATA_SIZE_SINGULAR(structname, fieldname) pb_membersize(structname, fieldname)
-#define PB_DATA_SIZE_OPTIONAL(structname, fieldname) pb_membersize(structname, fieldname)
-#define PB_DATA_SIZE_ONEOF(structname, fieldname) pb_membersize(structname, PB_ONEOF_NAME(FULL, fieldname))
-#define PB_DATA_SIZE_REPEATED(structname, fieldname) pb_membersize(structname, fieldname[0])
-#define PB_DATA_SIZE_FIXARRAY(structname, fieldname) pb_membersize(structname, fieldname[0])
-#define PB_DATA_SIZE_PTR_REQUIRED(structname, fieldname) pb_membersize(structname, fieldname[0])
-#define PB_DATA_SIZE_PTR_SINGULAR(structname, fieldname) pb_membersize(structname, fieldname[0])
-#define PB_DATA_SIZE_PTR_OPTIONAL(structname, fieldname) pb_membersize(structname, fieldname[0])
-#define PB_DATA_SIZE_PTR_ONEOF(structname, fieldname) pb_membersize(structname, PB_ONEOF_NAME(FULL, fieldname)[0])
-#define PB_DATA_SIZE_PTR_REPEATED(structname, fieldname) pb_membersize(structname, fieldname[0])
-#define PB_DATA_SIZE_PTR_FIXARRAY(structname, fieldname) pb_membersize(structname, fieldname[0][0])
-#define PB_DATA_SIZE_CB_REQUIRED(structname, fieldname) pb_membersize(structname, fieldname)
-#define PB_DATA_SIZE_CB_SINGULAR(structname, fieldname) pb_membersize(structname, fieldname)
-#define PB_DATA_SIZE_CB_OPTIONAL(structname, fieldname) pb_membersize(structname, fieldname)
-#define PB_DATA_SIZE_CB_ONEOF(structname, fieldname) pb_membersize(structname, PB_ONEOF_NAME(FULL, fieldname))
-#define PB_DATA_SIZE_CB_REPEATED(structname, fieldname) pb_membersize(structname, fieldname)
-#define PB_DATA_SIZE_CB_FIXARRAY(structname, fieldname) pb_membersize(structname, fieldname)
-
-#define PB_ONEOF_NAME(type, tuple) PB_EXPAND(PB_ONEOF_NAME_ ## type tuple)
-#define PB_ONEOF_NAME_UNION(unionname,membername,fullname) unionname
-#define PB_ONEOF_NAME_MEMBER(unionname,membername,fullname) membername
-#define PB_ONEOF_NAME_FULL(unionname,membername,fullname) fullname
-
-#define PB_GEN_SUBMSG_INFO(structname, atype, htype, ltype, fieldname, tag) \
-    PB_SUBMSG_INFO_ ## htype(ltype, structname, fieldname)
-
-#define PB_SUBMSG_INFO_REQUIRED(ltype, structname, fieldname) PB_SUBMSG_INFO_ ## ltype(structname ## _ ## fieldname ## _MSGTYPE)
-#define PB_SUBMSG_INFO_SINGULAR(ltype, structname, fieldname) PB_SUBMSG_INFO_ ## ltype(structname ## _ ## fieldname ## _MSGTYPE)
-#define PB_SUBMSG_INFO_OPTIONAL(ltype, structname, fieldname) PB_SUBMSG_INFO_ ## ltype(structname ## _ ## fieldname ## _MSGTYPE)
-#define PB_SUBMSG_INFO_ONEOF(ltype, structname, fieldname) PB_SUBMSG_INFO_ONEOF2(ltype, structname, PB_ONEOF_NAME(UNION, fieldname), PB_ONEOF_NAME(MEMBER, fieldname))
-#define PB_SUBMSG_INFO_ONEOF2(ltype, structname, unionname, membername) PB_SUBMSG_INFO_ONEOF3(ltype, structname, unionname, membername)
-#define PB_SUBMSG_INFO_ONEOF3(ltype, structname, unionname, membername) PB_SUBMSG_INFO_ ## ltype(structname ## _ ## unionname ## _ ## membername ## _MSGTYPE)
-#define PB_SUBMSG_INFO_REPEATED(ltype, structname, fieldname) PB_SUBMSG_INFO_ ## ltype(structname ## _ ## fieldname ## _MSGTYPE)
-#define PB_SUBMSG_INFO_FIXARRAY(ltype, structname, fieldname) PB_SUBMSG_INFO_ ## ltype(structname ## _ ## fieldname ## _MSGTYPE)
-#define PB_SUBMSG_INFO_BOOL(t)
-#define PB_SUBMSG_INFO_BYTES(t)
-#define PB_SUBMSG_INFO_DOUBLE(t)
-#define PB_SUBMSG_INFO_ENUM(t)
-#define PB_SUBMSG_INFO_UENUM(t)
-#define PB_SUBMSG_INFO_FIXED32(t)
-#define PB_SUBMSG_INFO_FIXED64(t)
-#define PB_SUBMSG_INFO_FLOAT(t)
-#define PB_SUBMSG_INFO_INT32(t)
-#define PB_SUBMSG_INFO_INT64(t)
-#define PB_SUBMSG_INFO_MESSAGE(t)  PB_SUBMSG_DESCRIPTOR(t)
-#define PB_SUBMSG_INFO_MSG_W_CB(t) PB_SUBMSG_DESCRIPTOR(t)
-#define PB_SUBMSG_INFO_SFIXED32(t)
-#define PB_SUBMSG_INFO_SFIXED64(t)
-#define PB_SUBMSG_INFO_SINT32(t)
-#define PB_SUBMSG_INFO_SINT64(t)
-#define PB_SUBMSG_INFO_STRING(t)
-#define PB_SUBMSG_INFO_UINT32(t)
-#define PB_SUBMSG_INFO_UINT64(t)
-#define PB_SUBMSG_INFO_EXTENSION(t)
-#define PB_SUBMSG_INFO_FIXED_LENGTH_BYTES(t)
-#define PB_SUBMSG_DESCRIPTOR(t)    &(t ## _msg),
-
-/* The field descriptors use a variable width format, with width of either
- * 1, 2, 4 or 8 of 32-bit words. The two lowest bytes of the first byte always
- * encode the descriptor size, 6 lowest bits of field tag number, and 8 bits
- * of the field type.
- *
- * Descriptor size is encoded as 0 = 1 word, 1 = 2 words, 2 = 4 words, 3 = 8 words.
- *
- * Formats, listed starting with the least significant bit of the first word.
- * 1 word:  [2-bit len] [6-bit tag] [8-bit type] [8-bit data_offset] [4-bit size_offset] [4-bit data_size]
- *
- * 2 words: [2-bit len] [6-bit tag] [8-bit type] [12-bit array_size] [4-bit size_offset]
- *          [16-bit data_offset] [12-bit data_size] [4-bit tag>>6]
- *
- * 4 words: [2-bit len] [6-bit tag] [8-bit type] [16-bit array_size]
- *          [8-bit size_offset] [24-bit tag>>6]
- *          [32-bit data_offset]
- *          [32-bit data_size]
- *
- * 8 words: [2-bit len] [6-bit tag] [8-bit type] [16-bit reserved]
- *          [8-bit size_offset] [24-bit tag>>6]
- *          [32-bit data_offset]
- *          [32-bit data_size]
- *          [32-bit array_size]
- *          [32-bit reserved]
- *          [32-bit reserved]
- *          [32-bit reserved]
- */
-
-#define PB_FIELDINFO_1(tag, type, data_offset, data_size, size_offset, array_size) \
-    (0 | (((tag) << 2) & 0xFF) | ((type) << 8) | (((uint32_t)(data_offset) & 0xFF) << 16) | \
-     (((uint32_t)(size_offset) & 0x0F) << 24) | (((uint32_t)(data_size) & 0x0F) << 28)),
-
-#define PB_FIELDINFO_2(tag, type, data_offset, data_size, size_offset, array_size) \
-    (1 | (((tag) << 2) & 0xFF) | ((type) << 8) | (((uint32_t)(array_size) & 0xFFF) << 16) | (((uint32_t)(size_offset) & 0x0F) << 28)), \
-    (((uint32_t)(data_offset) & 0xFFFF) | (((uint32_t)(data_size) & 0xFFF) << 16) | (((uint32_t)(tag) & 0x3c0) << 22)),
-
-#define PB_FIELDINFO_4(tag, type, data_offset, data_size, size_offset, array_size) \
-    (2 | (((tag) << 2) & 0xFF) | ((type) << 8) | (((uint32_t)(array_size) & 0xFFFF) << 16)), \
-    ((uint32_t)(int_least8_t)(size_offset) | (((uint32_t)(tag) << 2) & 0xFFFFFF00)), \
-    (data_offset), (data_size),
-
-#define PB_FIELDINFO_8(tag, type, data_offset, data_size, size_offset, array_size) \
-    (3 | (((tag) << 2) & 0xFF) | ((type) << 8)), \
-    ((uint32_t)(int_least8_t)(size_offset) | (((uint32_t)(tag) << 2) & 0xFFFFFF00)), \
-    (data_offset), (data_size), (array_size), 0, 0, 0,
-
-/* These assertions verify that the field information fits in the allocated space.
- * The generator tries to automatically determine the correct width that can fit all
- * data associated with a message. These asserts will fail only if there has been a
- * problem in the automatic logic - this may be worth reporting as a bug. As a workaround,
- * you can increase the descriptor width by defining PB_FIELDINFO_WIDTH or by setting
- * descriptorsize option in .options file.
- */
-#define PB_FITS(value,bits) ((uint32_t)(value) < ((uint32_t)1<<bits))
-#define PB_FIELDINFO_ASSERT_1(tag, type, data_offset, data_size, size_offset, array_size) \
-    PB_STATIC_ASSERT(PB_FITS(tag,6) && PB_FITS(data_offset,8) && PB_FITS(size_offset,4) && PB_FITS(data_size,4) && PB_FITS(array_size,1), FIELDINFO_DOES_NOT_FIT_width1_field ## tag)
-
-#define PB_FIELDINFO_ASSERT_2(tag, type, data_offset, data_size, size_offset, array_size) \
-    PB_STATIC_ASSERT(PB_FITS(tag,10) && PB_FITS(data_offset,16) && PB_FITS(size_offset,4) && PB_FITS(data_size,12) && PB_FITS(array_size,12), FIELDINFO_DOES_NOT_FIT_width2_field ## tag)
-
-#ifndef PB_FIELD_32BIT
-/* Maximum field sizes are still 16-bit if pb_size_t is 16-bit */
-#define PB_FIELDINFO_ASSERT_4(tag, type, data_offset, data_size, size_offset, array_size) \
-    PB_STATIC_ASSERT(PB_FITS(tag,16) && PB_FITS(data_offset,16) && PB_FITS((int_least8_t)size_offset,8) && PB_FITS(data_size,16) && PB_FITS(array_size,16), FIELDINFO_DOES_NOT_FIT_width4_field ## tag)
-
-#define PB_FIELDINFO_ASSERT_8(tag, type, data_offset, data_size, size_offset, array_size) \
-    PB_STATIC_ASSERT(PB_FITS(tag,16) && PB_FITS(data_offset,16) && PB_FITS((int_least8_t)size_offset,8) && PB_FITS(data_size,16) && PB_FITS(array_size,16), FIELDINFO_DOES_NOT_FIT_width8_field ## tag)
-#else
-/* Up to 32-bit fields supported.
- * Note that the checks are against 31 bits to avoid compiler warnings about shift wider than type in the test.
- * I expect that there is no reasonable use for >2GB messages with nanopb anyway.
- */
-#define PB_FIELDINFO_ASSERT_4(tag, type, data_offset, data_size, size_offset, array_size) \
-    PB_STATIC_ASSERT(PB_FITS(tag,30) && PB_FITS(data_offset,31) && PB_FITS(size_offset,8) && PB_FITS(data_size,31) && PB_FITS(array_size,16), FIELDINFO_DOES_NOT_FIT_width4_field ## tag)
-
-#define PB_FIELDINFO_ASSERT_8(tag, type, data_offset, data_size, size_offset, array_size) \
-    PB_STATIC_ASSERT(PB_FITS(tag,30) && PB_FITS(data_offset,31) && PB_FITS(size_offset,8) && PB_FITS(data_size,31) && PB_FITS(array_size,31), FIELDINFO_DOES_NOT_FIT_width8_field ## tag)
-#endif
-
-
-/* Automatic picking of FIELDINFO width:
- * Uses width 1 when possible, otherwise resorts to width 2.
- * This is used when PB_BIND() is called with "AUTO" as the argument.
- * The generator will give explicit size argument when it knows that a message
- * structure grows beyond 1-word format limits.
- */
-#define PB_FIELDINFO_WIDTH_AUTO(atype, htype, ltype) PB_FIELDINFO_WIDTH_ ## atype(htype, ltype)
-#define PB_FIELDINFO_WIDTH_STATIC(htype, ltype) PB_FIELDINFO_WIDTH_ ## htype(ltype)
-#define PB_FIELDINFO_WIDTH_POINTER(htype, ltype) PB_FIELDINFO_WIDTH_ ## htype(ltype)
-#define PB_FIELDINFO_WIDTH_CALLBACK(htype, ltype) 2
-#define PB_FIELDINFO_WIDTH_REQUIRED(ltype) PB_FIELDINFO_WIDTH_ ## ltype
-#define PB_FIELDINFO_WIDTH_SINGULAR(ltype) PB_FIELDINFO_WIDTH_ ## ltype
-#define PB_FIELDINFO_WIDTH_OPTIONAL(ltype) PB_FIELDINFO_WIDTH_ ## ltype
-#define PB_FIELDINFO_WIDTH_ONEOF(ltype) PB_FIELDINFO_WIDTH_ ## ltype
-#define PB_FIELDINFO_WIDTH_REPEATED(ltype) 2
-#define PB_FIELDINFO_WIDTH_FIXARRAY(ltype) 2
-#define PB_FIELDINFO_WIDTH_BOOL      1
-#define PB_FIELDINFO_WIDTH_BYTES     2
-#define PB_FIELDINFO_WIDTH_DOUBLE    1
-#define PB_FIELDINFO_WIDTH_ENUM      1
-#define PB_FIELDINFO_WIDTH_UENUM     1
-#define PB_FIELDINFO_WIDTH_FIXED32   1
-#define PB_FIELDINFO_WIDTH_FIXED64   1
-#define PB_FIELDINFO_WIDTH_FLOAT     1
-#define PB_FIELDINFO_WIDTH_INT32     1
-#define PB_FIELDINFO_WIDTH_INT64     1
-#define PB_FIELDINFO_WIDTH_MESSAGE   2
-#define PB_FIELDINFO_WIDTH_MSG_W_CB  2
-#define PB_FIELDINFO_WIDTH_SFIXED32  1
-#define PB_FIELDINFO_WIDTH_SFIXED64  1
-#define PB_FIELDINFO_WIDTH_SINT32    1
-#define PB_FIELDINFO_WIDTH_SINT64    1
-#define PB_FIELDINFO_WIDTH_STRING    2
-#define PB_FIELDINFO_WIDTH_UINT32    1
-#define PB_FIELDINFO_WIDTH_UINT64    1
-#define PB_FIELDINFO_WIDTH_EXTENSION 1
-#define PB_FIELDINFO_WIDTH_FIXED_LENGTH_BYTES 2
-
-/* The mapping from protobuf types to LTYPEs is done using these macros. */
-#define PB_LTYPE_MAP_BOOL               PB_LTYPE_BOOL
-#define PB_LTYPE_MAP_BYTES              PB_LTYPE_BYTES
-#define PB_LTYPE_MAP_DOUBLE             PB_LTYPE_FIXED64
-#define PB_LTYPE_MAP_ENUM               PB_LTYPE_VARINT
-#define PB_LTYPE_MAP_UENUM              PB_LTYPE_UVARINT
-#define PB_LTYPE_MAP_FIXED32            PB_LTYPE_FIXED32
-#define PB_LTYPE_MAP_FIXED64            PB_LTYPE_FIXED64
-#define PB_LTYPE_MAP_FLOAT              PB_LTYPE_FIXED32
-#define PB_LTYPE_MAP_INT32              PB_LTYPE_VARINT
-#define PB_LTYPE_MAP_INT64              PB_LTYPE_VARINT
-#define PB_LTYPE_MAP_MESSAGE            PB_LTYPE_SUBMESSAGE
-#define PB_LTYPE_MAP_MSG_W_CB           PB_LTYPE_SUBMSG_W_CB
-#define PB_LTYPE_MAP_SFIXED32           PB_LTYPE_FIXED32
-#define PB_LTYPE_MAP_SFIXED64           PB_LTYPE_FIXED64
-#define PB_LTYPE_MAP_SINT32             PB_LTYPE_SVARINT
-#define PB_LTYPE_MAP_SINT64             PB_LTYPE_SVARINT
-#define PB_LTYPE_MAP_STRING             PB_LTYPE_STRING
-#define PB_LTYPE_MAP_UINT32             PB_LTYPE_UVARINT
-#define PB_LTYPE_MAP_UINT64             PB_LTYPE_UVARINT
-#define PB_LTYPE_MAP_EXTENSION          PB_LTYPE_EXTENSION
-#define PB_LTYPE_MAP_FIXED_LENGTH_BYTES PB_LTYPE_FIXED_LENGTH_BYTES
-
-/* These macros are used for giving out error messages.
- * They are mostly a debugging aid; the main error information
- * is the true/false return value from functions.
- * Some code space can be saved by disabling the error
- * messages if not used.
- *
- * PB_SET_ERROR() sets the error message if none has been set yet.
- *                msg must be a constant string literal.
- * PB_GET_ERROR() always returns a pointer to a string.
- * PB_RETURN_ERROR() sets the error and returns false from current
- *                   function.
- */
-#ifdef PB_NO_ERRMSG
-#define PB_SET_ERROR(stream, msg) PB_UNUSED(stream)
-#define PB_GET_ERROR(stream) "(errmsg disabled)"
-#else
-#define PB_SET_ERROR(stream, msg) (stream->errmsg = (stream)->errmsg ? (stream)->errmsg : (msg))
-#define PB_GET_ERROR(stream) ((stream)->errmsg ? (stream)->errmsg : "(none)")
-#endif
-
-#define PB_RETURN_ERROR(stream, msg) return PB_SET_ERROR(stream, msg), false
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#ifdef __cplusplus
-#if __cplusplus >= 201103L
-#define PB_CONSTEXPR constexpr
-#else  // __cplusplus >= 201103L
-#define PB_CONSTEXPR
-#endif  // __cplusplus >= 201103L
-
-#if __cplusplus >= 201703L
-#define PB_INLINE_CONSTEXPR inline constexpr
-#else  // __cplusplus >= 201703L
-#define PB_INLINE_CONSTEXPR PB_CONSTEXPR
-#endif  // __cplusplus >= 201703L
-
-namespace nanopb {
-// Each type will be partially specialized by the generator.
-template <typename GenMessageT> struct MessageDescriptor;
-}  // namespace nanopb
-#endif  /* __cplusplus */
-
-#endif
-
diff --git a/src/pb_common.c b/src/pb_common.c
deleted file mode 100644
index dfc5a05..0000000
--- a/src/pb_common.c
+++ /dev/null
@@ -1,336 +0,0 @@
-/* pb_common.c: Common support functions for pb_encode.c and pb_decode.c.
- *
- * 2014 Petteri Aimonen <jpa@kapsi.fi>
- */
-
-#include "pb_common.h"
-
-static bool load_descriptor_values(pb_field_iter_t *iter)
-{
-    uint32_t word0;
-    uint32_t data_offset;
-    uint_least8_t format;
-    int_least8_t size_offset;
-
-    if (iter->index >= iter->descriptor->field_count)
-        return false;
-
-    word0 = PB_PROGMEM_READU32(iter->descriptor->field_info[iter->field_info_index]);
-    format = word0 & 3;
-    iter->tag = (pb_size_t)((word0 >> 2) & 0x3F);
-    iter->type = (pb_type_t)((word0 >> 8) & 0xFF);
-
-    if (format == 0)
-    {
-        /* 1-word format */
-        iter->array_size = 1;
-        size_offset = (int_least8_t)((word0 >> 24) & 0x0F);
-        data_offset = (word0 >> 16) & 0xFF;
-        iter->data_size = (pb_size_t)((word0 >> 28) & 0x0F);
-    }
-    else if (format == 1)
-    {
-        /* 2-word format */
-        uint32_t word1 = PB_PROGMEM_READU32(iter->descriptor->field_info[iter->field_info_index + 1]);
-
-        iter->array_size = (pb_size_t)((word0 >> 16) & 0x0FFF);
-        iter->tag = (pb_size_t)(iter->tag | ((word1 >> 28) << 6));
-        size_offset = (int_least8_t)((word0 >> 28) & 0x0F);
-        data_offset = word1 & 0xFFFF;
-        iter->data_size = (pb_size_t)((word1 >> 16) & 0x0FFF);
-    }
-    else if (format == 2)
-    {
-        /* 4-word format */
-        uint32_t word1 = PB_PROGMEM_READU32(iter->descriptor->field_info[iter->field_info_index + 1]);
-        uint32_t word2 = PB_PROGMEM_READU32(iter->descriptor->field_info[iter->field_info_index + 2]);
-        uint32_t word3 = PB_PROGMEM_READU32(iter->descriptor->field_info[iter->field_info_index + 3]);
-
-        iter->array_size = (pb_size_t)(word0 >> 16);
-        iter->tag = (pb_size_t)(iter->tag | ((word1 >> 8) << 6));
-        size_offset = (int_least8_t)(word1 & 0xFF);
-        data_offset = word2;
-        iter->data_size = (pb_size_t)word3;
-    }
-    else
-    {
-        /* 8-word format */
-        uint32_t word1 = PB_PROGMEM_READU32(iter->descriptor->field_info[iter->field_info_index + 1]);
-        uint32_t word2 = PB_PROGMEM_READU32(iter->descriptor->field_info[iter->field_info_index + 2]);
-        uint32_t word3 = PB_PROGMEM_READU32(iter->descriptor->field_info[iter->field_info_index + 3]);
-        uint32_t word4 = PB_PROGMEM_READU32(iter->descriptor->field_info[iter->field_info_index + 4]);
-
-        iter->array_size = (pb_size_t)word4;
-        iter->tag = (pb_size_t)(iter->tag | ((word1 >> 8) << 6));
-        size_offset = (int_least8_t)(word1 & 0xFF);
-        data_offset = word2;
-        iter->data_size = (pb_size_t)word3;
-    }
-
-    iter->pField = (char*)iter->message + data_offset;
-
-    if (size_offset)
-    {
-        iter->pSize = (char*)iter->pField - size_offset;
-    }
-    else if (PB_HTYPE(iter->type) == PB_HTYPE_REPEATED &&
-             (PB_ATYPE(iter->type) == PB_ATYPE_STATIC ||
-              PB_ATYPE(iter->type) == PB_ATYPE_POINTER))
-    {
-        /* Fixed count array */
-        iter->pSize = &iter->array_size;
-    }
-    else
-    {
-        iter->pSize = NULL;
-    }
-
-    if (PB_ATYPE(iter->type) == PB_ATYPE_POINTER && iter->pField != NULL)
-    {
-        iter->pData = *(void**)iter->pField;
-    }
-    else
-    {
-        iter->pData = iter->pField;
-    }
-
-    if (PB_LTYPE_IS_SUBMSG(iter->type))
-    {
-        iter->submsg_desc = iter->descriptor->submsg_info[iter->submessage_index];
-    }
-    else
-    {
-        iter->submsg_desc = NULL;
-    }
-
-    return true;
-}
-
-static void advance_iterator(pb_field_iter_t *iter)
-{
-    iter->index++;
-
-    if (iter->index >= iter->descriptor->field_count)
-    {
-        /* Restart */
-        iter->index = 0;
-        iter->field_info_index = 0;
-        iter->submessage_index = 0;
-        iter->required_field_index = 0;
-    }
-    else
-    {
-        /* Increment indexes based on previous field type.
-         * All field info formats have the following fields:
-         * - lowest 2 bits tell the amount of words in the descriptor (2^n words)
-         * - bits 2..7 give the lowest bits of tag number.
-         * - bits 8..15 give the field type.
-         */
-        uint32_t prev_descriptor = PB_PROGMEM_READU32(iter->descriptor->field_info[iter->field_info_index]);
-        pb_type_t prev_type = (prev_descriptor >> 8) & 0xFF;
-        pb_size_t descriptor_len = (pb_size_t)(1 << (prev_descriptor & 3));
-
-        iter->field_info_index = (pb_size_t)(iter->field_info_index + descriptor_len);
-
-        if (PB_HTYPE(prev_type) == PB_HTYPE_REQUIRED)
-        {
-            iter->required_field_index++;
-        }
-
-        if (PB_LTYPE_IS_SUBMSG(prev_type))
-        {
-            iter->submessage_index++;
-        }
-    }
-}
-
-bool pb_field_iter_begin(pb_field_iter_t *iter, const pb_msgdesc_t *desc, void *message)
-{
-    memset(iter, 0, sizeof(*iter));
-
-    iter->descriptor = desc;
-    iter->message = message;
-
-    return load_descriptor_values(iter);
-}
-
-bool pb_field_iter_begin_extension(pb_field_iter_t *iter, pb_extension_t *extension)
-{
-    const pb_msgdesc_t *msg = (const pb_msgdesc_t*)extension->type->arg;
-    bool status;
-
-    uint32_t word0 = PB_PROGMEM_READU32(msg->field_info[0]);
-    if (PB_ATYPE(word0 >> 8) == PB_ATYPE_POINTER)
-    {
-        /* For pointer extensions, the pointer is stored directly
-         * in the extension structure. This avoids having an extra
-         * indirection. */
-        status = pb_field_iter_begin(iter, msg, &extension->dest);
-    }
-    else
-    {
-        status = pb_field_iter_begin(iter, msg, extension->dest);
-    }
-
-    iter->pSize = &extension->found;
-    return status;
-}
-
-bool pb_field_iter_next(pb_field_iter_t *iter)
-{
-    advance_iterator(iter);
-    (void)load_descriptor_values(iter);
-    return iter->index != 0;
-}
-
-bool pb_field_iter_find(pb_field_iter_t *iter, uint32_t tag)
-{
-    if (iter->tag == tag)
-    {
-        return true; /* Nothing to do, correct field already. */
-    }
-    else
-    {
-        pb_size_t start = iter->index;
-        uint32_t fieldinfo;
-
-        do
-        {
-            /* Advance iterator but don't load values yet */
-            advance_iterator(iter);
-
-            /* Do fast check for tag number match */
-            fieldinfo = PB_PROGMEM_READU32(iter->descriptor->field_info[iter->field_info_index]);
-
-            if (((fieldinfo >> 2) & 0x3F) == (tag & 0x3F))
-            {
-                /* Good candidate, check further */
-                (void)load_descriptor_values(iter);
-
-                if (iter->tag == tag &&
-                    PB_LTYPE(iter->type) != PB_LTYPE_EXTENSION)
-                {
-                    /* Found it */
-                    return true;
-                }
-            }
-        } while (iter->index != start);
-
-        /* Searched all the way back to start, and found nothing. */
-        (void)load_descriptor_values(iter);
-        return false;
-    }
-}
-
-static void *pb_const_cast(const void *p)
-{
-    /* Note: this casts away const, in order to use the common field iterator
-     * logic for both encoding and decoding. The cast is done using union
-     * to avoid spurious compiler warnings. */
-    union {
-        void *p1;
-        const void *p2;
-    } t;
-    t.p2 = p;
-    return t.p1;
-}
-
-bool pb_field_iter_begin_const(pb_field_iter_t *iter, const pb_msgdesc_t *desc, const void *message)
-{
-    return pb_field_iter_begin(iter, desc, pb_const_cast(message));
-}
-
-bool pb_field_iter_begin_extension_const(pb_field_iter_t *iter, const pb_extension_t *extension)
-{
-    return pb_field_iter_begin_extension(iter, (pb_extension_t*)pb_const_cast(extension));
-}
-
-bool pb_default_field_callback(pb_istream_t *istream, pb_ostream_t *ostream, const pb_field_t *field)
-{
-    if (field->data_size == sizeof(pb_callback_t))
-    {
-        pb_callback_t *pCallback = (pb_callback_t*)field->pData;
-
-        if (pCallback != NULL)
-        {
-            if (istream != NULL && pCallback->funcs.decode != NULL)
-            {
-                return pCallback->funcs.decode(istream, field, &pCallback->arg);
-            }
-
-            if (ostream != NULL && pCallback->funcs.encode != NULL)
-            {
-                return pCallback->funcs.encode(ostream, field, &pCallback->arg);
-            }
-        }
-    }
-
-    return true; /* Success, but didn't do anything */
-
-}
-
-#ifdef PB_VALIDATE_UTF8
-
-/* This function checks whether a string is valid UTF-8 text.
- *
- * Algorithm is adapted from https://www.cl.cam.ac.uk/~mgk25/ucs/utf8_check.c
- * Original copyright: Markus Kuhn <http://www.cl.cam.ac.uk/~mgk25/> 2005-03-30
- * Licensed under "Short code license", which allows use under MIT license or
- * any compatible with it.
- */
-
-bool pb_validate_utf8(const char *str)
-{
-    const pb_byte_t *s = (const pb_byte_t*)str;
-    while (*s)
-    {
-        if (*s < 0x80)
-        {
-            /* 0xxxxxxx */
-            s++;
-        }
-        else if ((s[0] & 0xe0) == 0xc0)
-        {
-            /* 110XXXXx 10xxxxxx */
-            if ((s[1] & 0xc0) != 0x80 ||
-                (s[0] & 0xfe) == 0xc0)                        /* overlong? */
-                return false;
-            else
-                s += 2;
-        }
-        else if ((s[0] & 0xf0) == 0xe0)
-        {
-            /* 1110XXXX 10Xxxxxx 10xxxxxx */
-            if ((s[1] & 0xc0) != 0x80 ||
-                (s[2] & 0xc0) != 0x80 ||
-                (s[0] == 0xe0 && (s[1] & 0xe0) == 0x80) ||    /* overlong? */
-                (s[0] == 0xed && (s[1] & 0xe0) == 0xa0) ||    /* surrogate? */
-                (s[0] == 0xef && s[1] == 0xbf &&
-                (s[2] & 0xfe) == 0xbe))                 /* U+FFFE or U+FFFF? */
-                return false;
-            else
-                s += 3;
-        }
-        else if ((s[0] & 0xf8) == 0xf0)
-        {
-            /* 11110XXX 10XXxxxx 10xxxxxx 10xxxxxx */
-            if ((s[1] & 0xc0) != 0x80 ||
-                (s[2] & 0xc0) != 0x80 ||
-                (s[3] & 0xc0) != 0x80 ||
-                (s[0] == 0xf0 && (s[1] & 0xf0) == 0x80) ||    /* overlong? */
-                (s[0] == 0xf4 && s[1] > 0x8f) || s[0] > 0xf4) /* > U+10FFFF? */
-                return false;
-            else
-                s += 4;
-        }
-        else
-        {
-            return false;
-        }
-    }
-
-    return true;
-}
-
-#endif
-
diff --git a/src/pb_common.h b/src/pb_common.h
deleted file mode 100644
index 47fa2c9..0000000
--- a/src/pb_common.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/* pb_common.h: Common support functions for pb_encode.c and pb_decode.c.
- * These functions are rarely needed by applications directly.
- */
-
-#ifndef PB_COMMON_H_INCLUDED
-#define PB_COMMON_H_INCLUDED
-
-#include "pb.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* Initialize the field iterator structure to beginning.
- * Returns false if the message type is empty. */
-bool pb_field_iter_begin(pb_field_iter_t *iter, const pb_msgdesc_t *desc, void *message);
-
-/* Get a field iterator for extension field. */
-bool pb_field_iter_begin_extension(pb_field_iter_t *iter, pb_extension_t *extension);
-
-/* Same as pb_field_iter_begin(), but for const message pointer.
- * Note that the pointers in pb_field_iter_t will be non-const but shouldn't
- * be written to when using these functions. */
-bool pb_field_iter_begin_const(pb_field_iter_t *iter, const pb_msgdesc_t *desc, const void *message);
-bool pb_field_iter_begin_extension_const(pb_field_iter_t *iter, const pb_extension_t *extension);
-
-/* Advance the iterator to the next field.
- * Returns false when the iterator wraps back to the first field. */
-bool pb_field_iter_next(pb_field_iter_t *iter);
-
-/* Advance the iterator until it points at a field with the given tag.
- * Returns false if no such field exists. */
-bool pb_field_iter_find(pb_field_iter_t *iter, uint32_t tag);
-
-#ifdef PB_VALIDATE_UTF8
-/* Validate UTF-8 text string */
-bool pb_validate_utf8(const char *s);
-#endif
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif
-
diff --git a/src/pb_decode.c b/src/pb_decode.c
deleted file mode 100644
index f936412..0000000
--- a/src/pb_decode.c
+++ /dev/null
@@ -1,1735 +0,0 @@
-/* pb_decode.c -- decode a protobuf using minimal resources
- *
- * 2011 Petteri Aimonen <jpa@kapsi.fi>
- */
-
-/* Use the GCC warn_unused_result attribute to check that all return values
- * are propagated correctly. On other compilers and gcc before 3.4.0 just
- * ignore the annotation.
- */
-#if !defined(__GNUC__) || ( __GNUC__ < 3) || (__GNUC__ == 3 && __GNUC_MINOR__ < 4)
-    #define checkreturn
-#else
-    #define checkreturn __attribute__((warn_unused_result))
-#endif
-
-#include "pb.h"
-#include "pb_decode.h"
-#include "pb_common.h"
-
-/**************************************
- * Declarations internal to this file *
- **************************************/
-
-static bool checkreturn buf_read(pb_istream_t *stream, pb_byte_t *buf, size_t count);
-static bool checkreturn pb_decode_varint32_eof(pb_istream_t *stream, uint32_t *dest, bool *eof);
-static bool checkreturn read_raw_value(pb_istream_t *stream, pb_wire_type_t wire_type, pb_byte_t *buf, size_t *size);
-static bool checkreturn check_wire_type(pb_wire_type_t wire_type, pb_field_iter_t *field);
-static bool checkreturn decode_basic_field(pb_istream_t *stream, pb_field_iter_t *field);
-static bool checkreturn decode_static_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iter_t *field);
-static bool checkreturn decode_pointer_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iter_t *field);
-static bool checkreturn decode_callback_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iter_t *field);
-static bool checkreturn decode_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iter_t *field);
-static bool checkreturn default_extension_decoder(pb_istream_t *stream, pb_extension_t *extension, uint32_t tag, pb_wire_type_t wire_type);
-static bool checkreturn decode_extension(pb_istream_t *stream, uint32_t tag, pb_wire_type_t wire_type, pb_field_iter_t *iter);
-static bool checkreturn find_extension_field(pb_field_iter_t *iter);
-static bool pb_message_set_to_defaults(pb_field_iter_t *iter);
-static bool checkreturn pb_dec_bool(pb_istream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_dec_varint(pb_istream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_dec_fixed(pb_istream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_dec_bytes(pb_istream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_dec_string(pb_istream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_dec_submessage(pb_istream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_dec_fixed_length_bytes(pb_istream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_skip_varint(pb_istream_t *stream);
-static bool checkreturn pb_skip_string(pb_istream_t *stream);
-
-#ifdef PB_ENABLE_MALLOC
-static bool checkreturn allocate_field(pb_istream_t *stream, void *pData, size_t data_size, size_t array_size);
-static void initialize_pointer_field(void *pItem, pb_field_iter_t *field);
-static bool checkreturn pb_release_union_field(pb_istream_t *stream, pb_field_iter_t *field);
-static void pb_release_single_field(pb_field_iter_t *field);
-#endif
-
-#ifdef PB_WITHOUT_64BIT
-#define pb_int64_t int32_t
-#define pb_uint64_t uint32_t
-#else
-#define pb_int64_t int64_t
-#define pb_uint64_t uint64_t
-#endif
-
-typedef struct {
-    uint32_t bitfield[(PB_MAX_REQUIRED_FIELDS + 31) / 32];
-} pb_fields_seen_t;
-
-/*******************************
- * pb_istream_t implementation *
- *******************************/
-
-static bool checkreturn buf_read(pb_istream_t *stream, pb_byte_t *buf, size_t count)
-{
-    size_t i;
-    const pb_byte_t *source = (const pb_byte_t*)stream->state;
-    stream->state = (pb_byte_t*)stream->state + count;
-    
-    if (buf != NULL)
-    {
-        for (i = 0; i < count; i++)
-            buf[i] = source[i];
-    }
-    
-    return true;
-}
-
-bool checkreturn pb_read(pb_istream_t *stream, pb_byte_t *buf, size_t count)
-{
-    if (count == 0)
-        return true;
-
-#ifndef PB_BUFFER_ONLY
-	if (buf == NULL && stream->callback != buf_read)
-	{
-		/* Skip input bytes */
-		pb_byte_t tmp[16];
-		while (count > 16)
-		{
-			if (!pb_read(stream, tmp, 16))
-				return false;
-			
-			count -= 16;
-		}
-		
-		return pb_read(stream, tmp, count);
-	}
-#endif
-
-    if (stream->bytes_left < count)
-        PB_RETURN_ERROR(stream, "end-of-stream");
-    
-#ifndef PB_BUFFER_ONLY
-    if (!stream->callback(stream, buf, count))
-        PB_RETURN_ERROR(stream, "io error");
-#else
-    if (!buf_read(stream, buf, count))
-        return false;
-#endif
-    
-    stream->bytes_left -= count;
-    return true;
-}
-
-/* Read a single byte from input stream. buf may not be NULL.
- * This is an optimization for the varint decoding. */
-static bool checkreturn pb_readbyte(pb_istream_t *stream, pb_byte_t *buf)
-{
-    if (stream->bytes_left == 0)
-        PB_RETURN_ERROR(stream, "end-of-stream");
-
-#ifndef PB_BUFFER_ONLY
-    if (!stream->callback(stream, buf, 1))
-        PB_RETURN_ERROR(stream, "io error");
-#else
-    *buf = *(const pb_byte_t*)stream->state;
-    stream->state = (pb_byte_t*)stream->state + 1;
-#endif
-
-    stream->bytes_left--;
-    
-    return true;    
-}
-
-pb_istream_t pb_istream_from_buffer(const pb_byte_t *buf, size_t bufsize)
-{
-    pb_istream_t stream;
-    /* Cast away the const from buf without a compiler error.  We are
-     * careful to use it only in a const manner in the callbacks.
-     */
-    union {
-        void *state;
-        const void *c_state;
-    } state;
-#ifdef PB_BUFFER_ONLY
-    stream.callback = NULL;
-#else
-    stream.callback = &buf_read;
-#endif
-    state.c_state = buf;
-    stream.state = state.state;
-    stream.bytes_left = bufsize;
-#ifndef PB_NO_ERRMSG
-    stream.errmsg = NULL;
-#endif
-    return stream;
-}
-
-/********************
- * Helper functions *
- ********************/
-
-static bool checkreturn pb_decode_varint32_eof(pb_istream_t *stream, uint32_t *dest, bool *eof)
-{
-    pb_byte_t byte;
-    uint32_t result;
-    
-    if (!pb_readbyte(stream, &byte))
-    {
-        if (stream->bytes_left == 0)
-        {
-            if (eof)
-            {
-                *eof = true;
-            }
-        }
-
-        return false;
-    }
-    
-    if ((byte & 0x80) == 0)
-    {
-        /* Quick case, 1 byte value */
-        result = byte;
-    }
-    else
-    {
-        /* Multibyte case */
-        uint_fast8_t bitpos = 7;
-        result = byte & 0x7F;
-        
-        do
-        {
-            if (!pb_readbyte(stream, &byte))
-                return false;
-            
-            if (bitpos >= 32)
-            {
-                /* Note: The varint could have trailing 0x80 bytes, or 0xFF for negative. */
-                pb_byte_t sign_extension = (bitpos < 63) ? 0xFF : 0x01;
-                
-                if ((byte & 0x7F) != 0x00 && ((result >> 31) == 0 || byte != sign_extension))
-                {
-                    PB_RETURN_ERROR(stream, "varint overflow");
-                }
-            }
-            else
-            {
-                result |= (uint32_t)(byte & 0x7F) << bitpos;
-            }
-            bitpos = (uint_fast8_t)(bitpos + 7);
-        } while (byte & 0x80);
-        
-        if (bitpos == 35 && (byte & 0x70) != 0)
-        {
-            /* The last byte was at bitpos=28, so only bottom 4 bits fit. */
-            PB_RETURN_ERROR(stream, "varint overflow");
-        }
-   }
-   
-   *dest = result;
-   return true;
-}
-
-bool checkreturn pb_decode_varint32(pb_istream_t *stream, uint32_t *dest)
-{
-    return pb_decode_varint32_eof(stream, dest, NULL);
-}
-
-#ifndef PB_WITHOUT_64BIT
-bool checkreturn pb_decode_varint(pb_istream_t *stream, uint64_t *dest)
-{
-    pb_byte_t byte;
-    uint_fast8_t bitpos = 0;
-    uint64_t result = 0;
-    
-    do
-    {
-        if (bitpos >= 64)
-            PB_RETURN_ERROR(stream, "varint overflow");
-        
-        if (!pb_readbyte(stream, &byte))
-            return false;
-
-        result |= (uint64_t)(byte & 0x7F) << bitpos;
-        bitpos = (uint_fast8_t)(bitpos + 7);
-    } while (byte & 0x80);
-    
-    *dest = result;
-    return true;
-}
-#endif
-
-bool checkreturn pb_skip_varint(pb_istream_t *stream)
-{
-    pb_byte_t byte;
-    do
-    {
-        if (!pb_read(stream, &byte, 1))
-            return false;
-    } while (byte & 0x80);
-    return true;
-}
-
-bool checkreturn pb_skip_string(pb_istream_t *stream)
-{
-    uint32_t length;
-    if (!pb_decode_varint32(stream, &length))
-        return false;
-    
-    if ((size_t)length != length)
-    {
-        PB_RETURN_ERROR(stream, "size too large");
-    }
-
-    return pb_read(stream, NULL, (size_t)length);
-}
-
-bool checkreturn pb_decode_tag(pb_istream_t *stream, pb_wire_type_t *wire_type, uint32_t *tag, bool *eof)
-{
-    uint32_t temp;
-    *eof = false;
-    *wire_type = (pb_wire_type_t) 0;
-    *tag = 0;
-    
-    if (!pb_decode_varint32_eof(stream, &temp, eof))
-    {
-        return false;
-    }
-    
-    *tag = temp >> 3;
-    *wire_type = (pb_wire_type_t)(temp & 7);
-    return true;
-}
-
-bool checkreturn pb_skip_field(pb_istream_t *stream, pb_wire_type_t wire_type)
-{
-    switch (wire_type)
-    {
-        case PB_WT_VARINT: return pb_skip_varint(stream);
-        case PB_WT_64BIT: return pb_read(stream, NULL, 8);
-        case PB_WT_STRING: return pb_skip_string(stream);
-        case PB_WT_32BIT: return pb_read(stream, NULL, 4);
-        default: PB_RETURN_ERROR(stream, "invalid wire_type");
-    }
-}
-
-/* Read a raw value to buffer, for the purpose of passing it to callback as
- * a substream. Size is maximum size on call, and actual size on return.
- */
-static bool checkreturn read_raw_value(pb_istream_t *stream, pb_wire_type_t wire_type, pb_byte_t *buf, size_t *size)
-{
-    size_t max_size = *size;
-    switch (wire_type)
-    {
-        case PB_WT_VARINT:
-            *size = 0;
-            do
-            {
-                (*size)++;
-                if (*size > max_size)
-                    PB_RETURN_ERROR(stream, "varint overflow");
-
-                if (!pb_read(stream, buf, 1))
-                    return false;
-            } while (*buf++ & 0x80);
-            return true;
-            
-        case PB_WT_64BIT:
-            *size = 8;
-            return pb_read(stream, buf, 8);
-        
-        case PB_WT_32BIT:
-            *size = 4;
-            return pb_read(stream, buf, 4);
-        
-        case PB_WT_STRING:
-            /* Calling read_raw_value with a PB_WT_STRING is an error.
-             * Explicitly handle this case and fallthrough to default to avoid
-             * compiler warnings.
-             */
-
-        default: PB_RETURN_ERROR(stream, "invalid wire_type");
-    }
-}
-
-/* Decode string length from stream and return a substream with limited length.
- * Remember to close the substream using pb_close_string_substream().
- */
-bool checkreturn pb_make_string_substream(pb_istream_t *stream, pb_istream_t *substream)
-{
-    uint32_t size;
-    if (!pb_decode_varint32(stream, &size))
-        return false;
-    
-    *substream = *stream;
-    if (substream->bytes_left < size)
-        PB_RETURN_ERROR(stream, "parent stream too short");
-    
-    substream->bytes_left = (size_t)size;
-    stream->bytes_left -= (size_t)size;
-    return true;
-}
-
-bool checkreturn pb_close_string_substream(pb_istream_t *stream, pb_istream_t *substream)
-{
-    if (substream->bytes_left) {
-        if (!pb_read(substream, NULL, substream->bytes_left))
-            return false;
-    }
-
-    stream->state = substream->state;
-
-#ifndef PB_NO_ERRMSG
-    stream->errmsg = substream->errmsg;
-#endif
-    return true;
-}
-
-/*************************
- * Decode a single field *
- *************************/
-
-static bool checkreturn check_wire_type(pb_wire_type_t wire_type, pb_field_iter_t *field)
-{
-    switch (PB_LTYPE(field->type))
-    {
-        case PB_LTYPE_BOOL:
-        case PB_LTYPE_VARINT:
-        case PB_LTYPE_UVARINT:
-        case PB_LTYPE_SVARINT:
-            return wire_type == PB_WT_VARINT;
-
-        case PB_LTYPE_FIXED32:
-            return wire_type == PB_WT_32BIT;
-
-        case PB_LTYPE_FIXED64:
-            return wire_type == PB_WT_64BIT;
-
-        case PB_LTYPE_BYTES:
-        case PB_LTYPE_STRING:
-        case PB_LTYPE_SUBMESSAGE:
-        case PB_LTYPE_SUBMSG_W_CB:
-        case PB_LTYPE_FIXED_LENGTH_BYTES:
-            return wire_type == PB_WT_STRING;
-
-        default:
-            return false;
-    }
-}
-
-static bool checkreturn decode_basic_field(pb_istream_t *stream, pb_field_iter_t *field)
-{
-    switch (PB_LTYPE(field->type))
-    {
-        case PB_LTYPE_BOOL:
-            return pb_dec_bool(stream, field);
-
-        case PB_LTYPE_VARINT:
-        case PB_LTYPE_UVARINT:
-        case PB_LTYPE_SVARINT:
-            return pb_dec_varint(stream, field);
-
-        case PB_LTYPE_FIXED32:
-        case PB_LTYPE_FIXED64:
-            return pb_dec_fixed(stream, field);
-
-        case PB_LTYPE_BYTES:
-            return pb_dec_bytes(stream, field);
-
-        case PB_LTYPE_STRING:
-            return pb_dec_string(stream, field);
-
-        case PB_LTYPE_SUBMESSAGE:
-        case PB_LTYPE_SUBMSG_W_CB:
-            return pb_dec_submessage(stream, field);
-
-        case PB_LTYPE_FIXED_LENGTH_BYTES:
-            return pb_dec_fixed_length_bytes(stream, field);
-
-        default:
-            PB_RETURN_ERROR(stream, "invalid field type");
-    }
-}
-
-static bool checkreturn decode_static_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iter_t *field)
-{
-    switch (PB_HTYPE(field->type))
-    {
-        case PB_HTYPE_REQUIRED:
-            if (!check_wire_type(wire_type, field))
-                PB_RETURN_ERROR(stream, "wrong wire type");
-
-            return decode_basic_field(stream, field);
-            
-        case PB_HTYPE_OPTIONAL:
-            if (!check_wire_type(wire_type, field))
-                PB_RETURN_ERROR(stream, "wrong wire type");
-
-            if (field->pSize != NULL)
-                *(bool*)field->pSize = true;
-            return decode_basic_field(stream, field);
-    
-        case PB_HTYPE_REPEATED:
-            if (wire_type == PB_WT_STRING
-                && PB_LTYPE(field->type) <= PB_LTYPE_LAST_PACKABLE)
-            {
-                /* Packed array */
-                bool status = true;
-                pb_istream_t substream;
-                pb_size_t *size = (pb_size_t*)field->pSize;
-                field->pData = (char*)field->pField + field->data_size * (*size);
-
-                if (!pb_make_string_substream(stream, &substream))
-                    return false;
-
-                while (substream.bytes_left > 0 && *size < field->array_size)
-                {
-                    if (!decode_basic_field(&substream, field))
-                    {
-                        status = false;
-                        break;
-                    }
-                    (*size)++;
-                    field->pData = (char*)field->pData + field->data_size;
-                }
-
-                if (substream.bytes_left != 0)
-                    PB_RETURN_ERROR(stream, "array overflow");
-                if (!pb_close_string_substream(stream, &substream))
-                    return false;
-
-                return status;
-            }
-            else
-            {
-                /* Repeated field */
-                pb_size_t *size = (pb_size_t*)field->pSize;
-                field->pData = (char*)field->pField + field->data_size * (*size);
-
-                if (!check_wire_type(wire_type, field))
-                    PB_RETURN_ERROR(stream, "wrong wire type");
-
-                if ((*size)++ >= field->array_size)
-                    PB_RETURN_ERROR(stream, "array overflow");
-
-                return decode_basic_field(stream, field);
-            }
-
-        case PB_HTYPE_ONEOF:
-            *(pb_size_t*)field->pSize = field->tag;
-            if (PB_LTYPE_IS_SUBMSG(field->type))
-            {
-                /* We memset to zero so that any callbacks are set to NULL.
-                 * This is because the callbacks might otherwise have values
-                 * from some other union field.
-                 * If callbacks are needed inside oneof field, use .proto
-                 * option submsg_callback to have a separate callback function
-                 * that can set the fields before submessage is decoded.
-                 * pb_dec_submessage() will set any default values. */
-                memset(field->pData, 0, (size_t)field->data_size);
-            }
-
-            if (!check_wire_type(wire_type, field))
-                PB_RETURN_ERROR(stream, "wrong wire type");
-
-            return decode_basic_field(stream, field);
-
-        default:
-            PB_RETURN_ERROR(stream, "invalid field type");
-    }
-}
-
-#ifdef PB_ENABLE_MALLOC
-/* Allocate storage for the field and store the pointer at iter->pData.
- * array_size is the number of entries to reserve in an array.
- * Zero size is not allowed, use pb_free() for releasing.
- */
-static bool checkreturn allocate_field(pb_istream_t *stream, void *pData, size_t data_size, size_t array_size)
-{    
-    void *ptr = *(void**)pData;
-    
-    if (data_size == 0 || array_size == 0)
-        PB_RETURN_ERROR(stream, "invalid size");
-    
-#ifdef __AVR__
-    /* Workaround for AVR libc bug 53284: http://savannah.nongnu.org/bugs/?53284
-     * Realloc to size of 1 byte can cause corruption of the malloc structures.
-     */
-    if (data_size == 1 && array_size == 1)
-    {
-        data_size = 2;
-    }
-#endif
-
-    /* Check for multiplication overflows.
-     * This code avoids the costly division if the sizes are small enough.
-     * Multiplication is safe as long as only half of bits are set
-     * in either multiplicand.
-     */
-    {
-        const size_t check_limit = (size_t)1 << (sizeof(size_t) * 4);
-        if (data_size >= check_limit || array_size >= check_limit)
-        {
-            const size_t size_max = (size_t)-1;
-            if (size_max / array_size < data_size)
-            {
-                PB_RETURN_ERROR(stream, "size too large");
-            }
-        }
-    }
-    
-    /* Allocate new or expand previous allocation */
-    /* Note: on failure the old pointer will remain in the structure,
-     * the message must be freed by caller also on error return. */
-    ptr = pb_realloc(ptr, array_size * data_size);
-    if (ptr == NULL)
-        PB_RETURN_ERROR(stream, "realloc failed");
-    
-    *(void**)pData = ptr;
-    return true;
-}
-
-/* Clear a newly allocated item in case it contains a pointer, or is a submessage. */
-static void initialize_pointer_field(void *pItem, pb_field_iter_t *field)
-{
-    if (PB_LTYPE(field->type) == PB_LTYPE_STRING ||
-        PB_LTYPE(field->type) == PB_LTYPE_BYTES)
-    {
-        *(void**)pItem = NULL;
-    }
-    else if (PB_LTYPE_IS_SUBMSG(field->type))
-    {
-        /* We memset to zero so that any callbacks are set to NULL.
-         * Then set any default values. */
-        pb_field_iter_t submsg_iter;
-        memset(pItem, 0, field->data_size);
-
-        if (pb_field_iter_begin(&submsg_iter, field->submsg_desc, pItem))
-        {
-            (void)pb_message_set_to_defaults(&submsg_iter);
-        }
-    }
-}
-#endif
-
-static bool checkreturn decode_pointer_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iter_t *field)
-{
-#ifndef PB_ENABLE_MALLOC
-    PB_UNUSED(wire_type);
-    PB_UNUSED(field);
-    PB_RETURN_ERROR(stream, "no malloc support");
-#else
-    switch (PB_HTYPE(field->type))
-    {
-        case PB_HTYPE_REQUIRED:
-        case PB_HTYPE_OPTIONAL:
-        case PB_HTYPE_ONEOF:
-            if (!check_wire_type(wire_type, field))
-                PB_RETURN_ERROR(stream, "wrong wire type");
-
-            if (PB_LTYPE_IS_SUBMSG(field->type) && *(void**)field->pField != NULL)
-            {
-                /* Duplicate field, have to release the old allocation first. */
-                /* FIXME: Does this work correctly for oneofs? */
-                pb_release_single_field(field);
-            }
-        
-            if (PB_HTYPE(field->type) == PB_HTYPE_ONEOF)
-            {
-                *(pb_size_t*)field->pSize = field->tag;
-            }
-
-            if (PB_LTYPE(field->type) == PB_LTYPE_STRING ||
-                PB_LTYPE(field->type) == PB_LTYPE_BYTES)
-            {
-                /* pb_dec_string and pb_dec_bytes handle allocation themselves */
-                field->pData = field->pField;
-                return decode_basic_field(stream, field);
-            }
-            else
-            {
-                if (!allocate_field(stream, field->pField, field->data_size, 1))
-                    return false;
-                
-                field->pData = *(void**)field->pField;
-                initialize_pointer_field(field->pData, field);
-                return decode_basic_field(stream, field);
-            }
-    
-        case PB_HTYPE_REPEATED:
-            if (wire_type == PB_WT_STRING
-                && PB_LTYPE(field->type) <= PB_LTYPE_LAST_PACKABLE)
-            {
-                /* Packed array, multiple items come in at once. */
-                bool status = true;
-                pb_size_t *size = (pb_size_t*)field->pSize;
-                size_t allocated_size = *size;
-                pb_istream_t substream;
-                
-                if (!pb_make_string_substream(stream, &substream))
-                    return false;
-                
-                while (substream.bytes_left)
-                {
-                    if (*size == PB_SIZE_MAX)
-                    {
-#ifndef PB_NO_ERRMSG
-                        stream->errmsg = "too many array entries";
-#endif
-                        status = false;
-                        break;
-                    }
-
-                    if ((size_t)*size + 1 > allocated_size)
-                    {
-                        /* Allocate more storage. This tries to guess the
-                         * number of remaining entries. Round the division
-                         * upwards. */
-                        size_t remain = (substream.bytes_left - 1) / field->data_size + 1;
-                        if (remain < PB_SIZE_MAX - allocated_size)
-                            allocated_size += remain;
-                        else
-                            allocated_size += 1;
-                        
-                        if (!allocate_field(&substream, field->pField, field->data_size, allocated_size))
-                        {
-                            status = false;
-                            break;
-                        }
-                    }
-
-                    /* Decode the array entry */
-                    field->pData = *(char**)field->pField + field->data_size * (*size);
-                    initialize_pointer_field(field->pData, field);
-                    if (!decode_basic_field(&substream, field))
-                    {
-                        status = false;
-                        break;
-                    }
-                    
-                    (*size)++;
-                }
-                if (!pb_close_string_substream(stream, &substream))
-                    return false;
-                
-                return status;
-            }
-            else
-            {
-                /* Normal repeated field, i.e. only one item at a time. */
-                pb_size_t *size = (pb_size_t*)field->pSize;
-
-                if (*size == PB_SIZE_MAX)
-                    PB_RETURN_ERROR(stream, "too many array entries");
-                
-                if (!check_wire_type(wire_type, field))
-                    PB_RETURN_ERROR(stream, "wrong wire type");
-
-                if (!allocate_field(stream, field->pField, field->data_size, (size_t)(*size + 1)))
-                    return false;
-            
-                field->pData = *(char**)field->pField + field->data_size * (*size);
-                (*size)++;
-                initialize_pointer_field(field->pData, field);
-                return decode_basic_field(stream, field);
-            }
-
-        default:
-            PB_RETURN_ERROR(stream, "invalid field type");
-    }
-#endif
-}
-
-static bool checkreturn decode_callback_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iter_t *field)
-{
-    if (!field->descriptor->field_callback)
-        return pb_skip_field(stream, wire_type);
-
-    if (wire_type == PB_WT_STRING)
-    {
-        pb_istream_t substream;
-        size_t prev_bytes_left;
-        
-        if (!pb_make_string_substream(stream, &substream))
-            return false;
-        
-        do
-        {
-            prev_bytes_left = substream.bytes_left;
-            if (!field->descriptor->field_callback(&substream, NULL, field))
-                PB_RETURN_ERROR(stream, "callback failed");
-        } while (substream.bytes_left > 0 && substream.bytes_left < prev_bytes_left);
-        
-        if (!pb_close_string_substream(stream, &substream))
-            return false;
-
-        return true;
-    }
-    else
-    {
-        /* Copy the single scalar value to stack.
-         * This is required so that we can limit the stream length,
-         * which in turn allows to use same callback for packed and
-         * not-packed fields. */
-        pb_istream_t substream;
-        pb_byte_t buffer[10];
-        size_t size = sizeof(buffer);
-        
-        if (!read_raw_value(stream, wire_type, buffer, &size))
-            return false;
-        substream = pb_istream_from_buffer(buffer, size);
-        
-        return field->descriptor->field_callback(&substream, NULL, field);
-    }
-}
-
-static bool checkreturn decode_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iter_t *field)
-{
-#ifdef PB_ENABLE_MALLOC
-    /* When decoding an oneof field, check if there is old data that must be
-     * released first. */
-    if (PB_HTYPE(field->type) == PB_HTYPE_ONEOF)
-    {
-        if (!pb_release_union_field(stream, field))
-            return false;
-    }
-#endif
-
-    switch (PB_ATYPE(field->type))
-    {
-        case PB_ATYPE_STATIC:
-            return decode_static_field(stream, wire_type, field);
-        
-        case PB_ATYPE_POINTER:
-            return decode_pointer_field(stream, wire_type, field);
-        
-        case PB_ATYPE_CALLBACK:
-            return decode_callback_field(stream, wire_type, field);
-        
-        default:
-            PB_RETURN_ERROR(stream, "invalid field type");
-    }
-}
-
-/* Default handler for extension fields. Expects to have a pb_msgdesc_t
- * pointer in the extension->type->arg field, pointing to a message with
- * only one field in it.  */
-static bool checkreturn default_extension_decoder(pb_istream_t *stream,
-    pb_extension_t *extension, uint32_t tag, pb_wire_type_t wire_type)
-{
-    pb_field_iter_t iter;
-
-    if (!pb_field_iter_begin_extension(&iter, extension))
-        PB_RETURN_ERROR(stream, "invalid extension");
-
-    if (iter.tag != tag)
-        return true;
-
-    extension->found = true;
-    return decode_field(stream, wire_type, &iter);
-}
-
-/* Try to decode an unknown field as an extension field. Tries each extension
- * decoder in turn, until one of them handles the field or loop ends. */
-static bool checkreturn decode_extension(pb_istream_t *stream,
-    uint32_t tag, pb_wire_type_t wire_type, pb_field_iter_t *iter)
-{
-    pb_extension_t *extension = *(pb_extension_t* const *)iter->pData;
-    size_t pos = stream->bytes_left;
-    
-    while (extension != NULL && pos == stream->bytes_left)
-    {
-        bool status;
-        if (extension->type->decode)
-            status = extension->type->decode(stream, extension, tag, wire_type);
-        else
-            status = default_extension_decoder(stream, extension, tag, wire_type);
-
-        if (!status)
-            return false;
-        
-        extension = extension->next;
-    }
-    
-    return true;
-}
-
-/* Step through the iterator until an extension field is found or until all
- * entries have been checked. There can be only one extension field per
- * message. Returns false if no extension field is found. */
-static bool checkreturn find_extension_field(pb_field_iter_t *iter)
-{
-    pb_size_t start = iter->index;
-
-    do {
-        if (PB_LTYPE(iter->type) == PB_LTYPE_EXTENSION)
-            return true;
-        (void)pb_field_iter_next(iter);
-    } while (iter->index != start);
-    
-    return false;
-}
-
-/* Initialize message fields to default values, recursively */
-static bool pb_field_set_to_default(pb_field_iter_t *field)
-{
-    pb_type_t type;
-    type = field->type;
-
-    if (PB_LTYPE(type) == PB_LTYPE_EXTENSION)
-    {
-        pb_extension_t *ext = *(pb_extension_t* const *)field->pData;
-        while (ext != NULL)
-        {
-            pb_field_iter_t ext_iter;
-            if (pb_field_iter_begin_extension(&ext_iter, ext))
-            {
-                ext->found = false;
-                if (!pb_message_set_to_defaults(&ext_iter))
-                    return false;
-            }
-            ext = ext->next;
-        }
-    }
-    else if (PB_ATYPE(type) == PB_ATYPE_STATIC)
-    {
-        bool init_data = true;
-        if (PB_HTYPE(type) == PB_HTYPE_OPTIONAL && field->pSize != NULL)
-        {
-            /* Set has_field to false. Still initialize the optional field
-             * itself also. */
-            *(bool*)field->pSize = false;
-        }
-        else if (PB_HTYPE(type) == PB_HTYPE_REPEATED ||
-                 PB_HTYPE(type) == PB_HTYPE_ONEOF)
-        {
-            /* REPEATED: Set array count to 0, no need to initialize contents.
-               ONEOF: Set which_field to 0. */
-            *(pb_size_t*)field->pSize = 0;
-            init_data = false;
-        }
-
-        if (init_data)
-        {
-            if (PB_LTYPE_IS_SUBMSG(field->type))
-            {
-                /* Initialize submessage to defaults */
-                pb_field_iter_t submsg_iter;
-                if (pb_field_iter_begin(&submsg_iter, field->submsg_desc, field->pData))
-                {
-                    if (!pb_message_set_to_defaults(&submsg_iter))
-                        return false;
-                }
-            }
-            else
-            {
-                /* Initialize to zeros */
-                memset(field->pData, 0, (size_t)field->data_size);
-            }
-        }
-    }
-    else if (PB_ATYPE(type) == PB_ATYPE_POINTER)
-    {
-        /* Initialize the pointer to NULL. */
-        *(void**)field->pField = NULL;
-
-        /* Initialize array count to 0. */
-        if (PB_HTYPE(type) == PB_HTYPE_REPEATED ||
-            PB_HTYPE(type) == PB_HTYPE_ONEOF)
-        {
-            *(pb_size_t*)field->pSize = 0;
-        }
-    }
-    else if (PB_ATYPE(type) == PB_ATYPE_CALLBACK)
-    {
-        /* Don't overwrite callback */
-    }
-
-    return true;
-}
-
-static bool pb_message_set_to_defaults(pb_field_iter_t *iter)
-{
-    pb_istream_t defstream = PB_ISTREAM_EMPTY;
-    uint32_t tag = 0;
-    pb_wire_type_t wire_type = PB_WT_VARINT;
-    bool eof;
-
-    if (iter->descriptor->default_value)
-    {
-        defstream = pb_istream_from_buffer(iter->descriptor->default_value, (size_t)-1);
-        if (!pb_decode_tag(&defstream, &wire_type, &tag, &eof))
-            return false;
-    }
-
-    do
-    {
-        if (!pb_field_set_to_default(iter))
-            return false;
-
-        if (tag != 0 && iter->tag == tag)
-        {
-            /* We have a default value for this field in the defstream */
-            if (!decode_field(&defstream, wire_type, iter))
-                return false;
-            if (!pb_decode_tag(&defstream, &wire_type, &tag, &eof))
-                return false;
-
-            if (iter->pSize)
-                *(bool*)iter->pSize = false;
-        }
-    } while (pb_field_iter_next(iter));
-
-    return true;
-}
-
-/*********************
- * Decode all fields *
- *********************/
-
-static bool checkreturn pb_decode_inner(pb_istream_t *stream, const pb_msgdesc_t *fields, void *dest_struct, unsigned int flags)
-{
-    uint32_t extension_range_start = 0;
-
-    /* 'fixed_count_field' and 'fixed_count_size' track position of a repeated fixed
-     * count field. This can only handle _one_ repeated fixed count field that
-     * is unpacked and unordered among other (non repeated fixed count) fields.
-     */
-    pb_size_t fixed_count_field = PB_SIZE_MAX;
-    pb_size_t fixed_count_size = 0;
-    pb_size_t fixed_count_total_size = 0;
-
-    pb_fields_seen_t fields_seen = {{0, 0}};
-    const uint32_t allbits = ~(uint32_t)0;
-    pb_field_iter_t iter;
-
-    if (pb_field_iter_begin(&iter, fields, dest_struct))
-    {
-        if ((flags & PB_DECODE_NOINIT) == 0)
-        {
-            if (!pb_message_set_to_defaults(&iter))
-                PB_RETURN_ERROR(stream, "failed to set defaults");
-        }
-    }
-
-    while (stream->bytes_left)
-    {
-        uint32_t tag;
-        pb_wire_type_t wire_type;
-        bool eof;
-
-        if (!pb_decode_tag(stream, &wire_type, &tag, &eof))
-        {
-            if (eof)
-                break;
-            else
-                return false;
-        }
-
-        if (tag == 0)
-        {
-          if (flags & PB_DECODE_NULLTERMINATED)
-          {
-            break;
-          }
-          else
-          {
-            PB_RETURN_ERROR(stream, "zero tag");
-          }
-        }
-
-        if (!pb_field_iter_find(&iter, tag) || PB_LTYPE(iter.type) == PB_LTYPE_EXTENSION)
-        {
-            /* No match found, check if it matches an extension. */
-            if (tag >= extension_range_start)
-            {
-                if (!find_extension_field(&iter))
-                    extension_range_start = (uint32_t)-1;
-                else
-                    extension_range_start = iter.tag;
-
-                if (tag >= extension_range_start)
-                {
-                    size_t pos = stream->bytes_left;
-
-                    if (!decode_extension(stream, tag, wire_type, &iter))
-                        return false;
-
-                    if (pos != stream->bytes_left)
-                    {
-                        /* The field was handled */
-                        continue;
-                    }
-                }
-            }
-
-            /* No match found, skip data */
-            if (!pb_skip_field(stream, wire_type))
-                return false;
-            continue;
-        }
-
-        /* If a repeated fixed count field was found, get size from
-         * 'fixed_count_field' as there is no counter contained in the struct.
-         */
-        if (PB_HTYPE(iter.type) == PB_HTYPE_REPEATED && iter.pSize == &iter.array_size)
-        {
-            if (fixed_count_field != iter.index) {
-                /* If the new fixed count field does not match the previous one,
-                 * check that the previous one is NULL or that it finished
-                 * receiving all the expected data.
-                 */
-                if (fixed_count_field != PB_SIZE_MAX &&
-                    fixed_count_size != fixed_count_total_size)
-                {
-                    PB_RETURN_ERROR(stream, "wrong size for fixed count field");
-                }
-
-                fixed_count_field = iter.index;
-                fixed_count_size = 0;
-                fixed_count_total_size = iter.array_size;
-            }
-
-            iter.pSize = &fixed_count_size;
-        }
-
-        if (PB_HTYPE(iter.type) == PB_HTYPE_REQUIRED
-            && iter.required_field_index < PB_MAX_REQUIRED_FIELDS)
-        {
-            uint32_t tmp = ((uint32_t)1 << (iter.required_field_index & 31));
-            fields_seen.bitfield[iter.required_field_index >> 5] |= tmp;
-        }
-
-        if (!decode_field(stream, wire_type, &iter))
-            return false;
-    }
-
-    /* Check that all elements of the last decoded fixed count field were present. */
-    if (fixed_count_field != PB_SIZE_MAX &&
-        fixed_count_size != fixed_count_total_size)
-    {
-        PB_RETURN_ERROR(stream, "wrong size for fixed count field");
-    }
-
-    /* Check that all required fields were present. */
-    {
-        /* First figure out the number of required fields by
-         * seeking to the end of the field array. Usually we
-         * are already close to end after decoding.
-         */
-        pb_size_t req_field_count;
-        pb_type_t last_type;
-        pb_size_t i;
-        do {
-            req_field_count = iter.required_field_index;
-            last_type = iter.type;
-        } while (pb_field_iter_next(&iter));
-
-        /* Fixup if last field was also required. */
-        if (PB_HTYPE(last_type) == PB_HTYPE_REQUIRED && iter.tag != 0)
-            req_field_count++;
-
-        if (req_field_count > PB_MAX_REQUIRED_FIELDS)
-            req_field_count = PB_MAX_REQUIRED_FIELDS;
-
-        if (req_field_count > 0)
-        {
-            /* Check the whole words */
-            for (i = 0; i < (req_field_count >> 5); i++)
-            {
-                if (fields_seen.bitfield[i] != allbits)
-                    PB_RETURN_ERROR(stream, "missing required field");
-            }
-
-            /* Check the remaining bits (if any) */
-            if ((req_field_count & 31) != 0)
-            {
-                if (fields_seen.bitfield[req_field_count >> 5] !=
-                    (allbits >> (uint_least8_t)(32 - (req_field_count & 31))))
-                {
-                    PB_RETURN_ERROR(stream, "missing required field");
-                }
-            }
-        }
-    }
-
-    return true;
-}
-
-bool checkreturn pb_decode_ex(pb_istream_t *stream, const pb_msgdesc_t *fields, void *dest_struct, unsigned int flags)
-{
-    bool status;
-
-    if ((flags & PB_DECODE_DELIMITED) == 0)
-    {
-      status = pb_decode_inner(stream, fields, dest_struct, flags);
-    }
-    else
-    {
-      pb_istream_t substream;
-      if (!pb_make_string_substream(stream, &substream))
-        return false;
-
-      status = pb_decode_inner(&substream, fields, dest_struct, flags);
-
-      if (!pb_close_string_substream(stream, &substream))
-        return false;
-    }
-    
-#ifdef PB_ENABLE_MALLOC
-    if (!status)
-        pb_release(fields, dest_struct);
-#endif
-    
-    return status;
-}
-
-bool checkreturn pb_decode(pb_istream_t *stream, const pb_msgdesc_t *fields, void *dest_struct)
-{
-    bool status;
-
-    status = pb_decode_inner(stream, fields, dest_struct, 0);
-
-#ifdef PB_ENABLE_MALLOC
-    if (!status)
-        pb_release(fields, dest_struct);
-#endif
-
-    return status;
-}
-
-#ifdef PB_ENABLE_MALLOC
-/* Given an oneof field, if there has already been a field inside this oneof,
- * release it before overwriting with a different one. */
-static bool pb_release_union_field(pb_istream_t *stream, pb_field_iter_t *field)
-{
-    pb_field_iter_t old_field = *field;
-    pb_size_t old_tag = *(pb_size_t*)field->pSize; /* Previous which_ value */
-    pb_size_t new_tag = field->tag; /* New which_ value */
-
-    if (old_tag == 0)
-        return true; /* Ok, no old data in union */
-
-    if (old_tag == new_tag)
-        return true; /* Ok, old data is of same type => merge */
-
-    /* Release old data. The find can fail if the message struct contains
-     * invalid data. */
-    if (!pb_field_iter_find(&old_field, old_tag))
-        PB_RETURN_ERROR(stream, "invalid union tag");
-
-    pb_release_single_field(&old_field);
-
-    return true;
-}
-
-static void pb_release_single_field(pb_field_iter_t *field)
-{
-    pb_type_t type;
-    type = field->type;
-
-    if (PB_HTYPE(type) == PB_HTYPE_ONEOF)
-    {
-        if (*(pb_size_t*)field->pSize != field->tag)
-            return; /* This is not the current field in the union */
-    }
-
-    /* Release anything contained inside an extension or submsg.
-     * This has to be done even if the submsg itself is statically
-     * allocated. */
-    if (PB_LTYPE(type) == PB_LTYPE_EXTENSION)
-    {
-        /* Release fields from all extensions in the linked list */
-        pb_extension_t *ext = *(pb_extension_t**)field->pData;
-        while (ext != NULL)
-        {
-            pb_field_iter_t ext_iter;
-            if (pb_field_iter_begin_extension(&ext_iter, ext))
-            {
-                pb_release_single_field(&ext_iter);
-            }
-            ext = ext->next;
-        }
-    }
-    else if (PB_LTYPE_IS_SUBMSG(type) && PB_ATYPE(type) != PB_ATYPE_CALLBACK)
-    {
-        /* Release fields in submessage or submsg array */
-        pb_size_t count = 1;
-        
-        if (PB_ATYPE(type) == PB_ATYPE_POINTER)
-        {
-            field->pData = *(void**)field->pField;
-        }
-        else
-        {
-            field->pData = field->pField;
-        }
-        
-        if (PB_HTYPE(type) == PB_HTYPE_REPEATED)
-        {
-            count = *(pb_size_t*)field->pSize;
-
-            if (PB_ATYPE(type) == PB_ATYPE_STATIC && count > field->array_size)
-            {
-                /* Protect against corrupted _count fields */
-                count = field->array_size;
-            }
-        }
-        
-        if (field->pData)
-        {
-            while (count--)
-            {
-                pb_release(field->submsg_desc, field->pData);
-                field->pData = (char*)field->pData + field->data_size;
-            }
-        }
-    }
-    
-    if (PB_ATYPE(type) == PB_ATYPE_POINTER)
-    {
-        if (PB_HTYPE(type) == PB_HTYPE_REPEATED &&
-            (PB_LTYPE(type) == PB_LTYPE_STRING ||
-             PB_LTYPE(type) == PB_LTYPE_BYTES))
-        {
-            /* Release entries in repeated string or bytes array */
-            void **pItem = *(void***)field->pField;
-            pb_size_t count = *(pb_size_t*)field->pSize;
-            while (count--)
-            {
-                pb_free(*pItem);
-                *pItem++ = NULL;
-            }
-        }
-        
-        if (PB_HTYPE(type) == PB_HTYPE_REPEATED)
-        {
-            /* We are going to release the array, so set the size to 0 */
-            *(pb_size_t*)field->pSize = 0;
-        }
-        
-        /* Release main pointer */
-        pb_free(*(void**)field->pField);
-        *(void**)field->pField = NULL;
-    }
-}
-
-void pb_release(const pb_msgdesc_t *fields, void *dest_struct)
-{
-    pb_field_iter_t iter;
-    
-    if (!dest_struct)
-        return; /* Ignore NULL pointers, similar to free() */
-
-    if (!pb_field_iter_begin(&iter, fields, dest_struct))
-        return; /* Empty message type */
-    
-    do
-    {
-        pb_release_single_field(&iter);
-    } while (pb_field_iter_next(&iter));
-}
-#endif
-
-/* Field decoders */
-
-bool pb_decode_bool(pb_istream_t *stream, bool *dest)
-{
-    uint32_t value;
-    if (!pb_decode_varint32(stream, &value))
-        return false;
-
-    *(bool*)dest = (value != 0);
-    return true;
-}
-
-bool pb_decode_svarint(pb_istream_t *stream, pb_int64_t *dest)
-{
-    pb_uint64_t value;
-    if (!pb_decode_varint(stream, &value))
-        return false;
-    
-    if (value & 1)
-        *dest = (pb_int64_t)(~(value >> 1));
-    else
-        *dest = (pb_int64_t)(value >> 1);
-    
-    return true;
-}
-
-bool pb_decode_fixed32(pb_istream_t *stream, void *dest)
-{
-    union {
-        uint32_t fixed32;
-        pb_byte_t bytes[4];
-    } u;
-
-    if (!pb_read(stream, u.bytes, 4))
-        return false;
-
-#if defined(__BYTE_ORDER) && __BYTE_ORDER == __LITTLE_ENDIAN && CHAR_BIT == 8
-    /* fast path - if we know that we're on little endian, assign directly */
-    *(uint32_t*)dest = u.fixed32;
-#else
-    *(uint32_t*)dest = ((uint32_t)u.bytes[0] << 0) |
-                       ((uint32_t)u.bytes[1] << 8) |
-                       ((uint32_t)u.bytes[2] << 16) |
-                       ((uint32_t)u.bytes[3] << 24);
-#endif
-    return true;
-}
-
-#ifndef PB_WITHOUT_64BIT
-bool pb_decode_fixed64(pb_istream_t *stream, void *dest)
-{
-    union {
-        uint64_t fixed64;
-        pb_byte_t bytes[8];
-    } u;
-
-    if (!pb_read(stream, u.bytes, 8))
-        return false;
-
-#if defined(__BYTE_ORDER) && __BYTE_ORDER == __LITTLE_ENDIAN && CHAR_BIT == 8
-    /* fast path - if we know that we're on little endian, assign directly */
-    *(uint64_t*)dest = u.fixed64;
-#else
-    *(uint64_t*)dest = ((uint64_t)u.bytes[0] << 0) |
-                       ((uint64_t)u.bytes[1] << 8) |
-                       ((uint64_t)u.bytes[2] << 16) |
-                       ((uint64_t)u.bytes[3] << 24) |
-                       ((uint64_t)u.bytes[4] << 32) |
-                       ((uint64_t)u.bytes[5] << 40) |
-                       ((uint64_t)u.bytes[6] << 48) |
-                       ((uint64_t)u.bytes[7] << 56);
-#endif
-    return true;
-}
-#endif
-
-static bool checkreturn pb_dec_bool(pb_istream_t *stream, const pb_field_iter_t *field)
-{
-    return pb_decode_bool(stream, (bool*)field->pData);
-}
-
-static bool checkreturn pb_dec_varint(pb_istream_t *stream, const pb_field_iter_t *field)
-{
-    if (PB_LTYPE(field->type) == PB_LTYPE_UVARINT)
-    {
-        pb_uint64_t value, clamped;
-        if (!pb_decode_varint(stream, &value))
-            return false;
-
-        /* Cast to the proper field size, while checking for overflows */
-        if (field->data_size == sizeof(pb_uint64_t))
-            clamped = *(pb_uint64_t*)field->pData = value;
-        else if (field->data_size == sizeof(uint32_t))
-            clamped = *(uint32_t*)field->pData = (uint32_t)value;
-        else if (field->data_size == sizeof(uint_least16_t))
-            clamped = *(uint_least16_t*)field->pData = (uint_least16_t)value;
-        else if (field->data_size == sizeof(uint_least8_t))
-            clamped = *(uint_least8_t*)field->pData = (uint_least8_t)value;
-        else
-            PB_RETURN_ERROR(stream, "invalid data_size");
-
-        if (clamped != value)
-            PB_RETURN_ERROR(stream, "integer too large");
-
-        return true;
-    }
-    else
-    {
-        pb_uint64_t value;
-        pb_int64_t svalue;
-        pb_int64_t clamped;
-
-        if (PB_LTYPE(field->type) == PB_LTYPE_SVARINT)
-        {
-            if (!pb_decode_svarint(stream, &svalue))
-                return false;
-        }
-        else
-        {
-            if (!pb_decode_varint(stream, &value))
-                return false;
-
-            /* See issue 97: Google's C++ protobuf allows negative varint values to
-            * be cast as int32_t, instead of the int64_t that should be used when
-            * encoding. Previous nanopb versions had a bug in encoding. In order to
-            * not break decoding of such messages, we cast <=32 bit fields to
-            * int32_t first to get the sign correct.
-            */
-            if (field->data_size == sizeof(pb_int64_t))
-                svalue = (pb_int64_t)value;
-            else
-                svalue = (int32_t)value;
-        }
-
-        /* Cast to the proper field size, while checking for overflows */
-        if (field->data_size == sizeof(pb_int64_t))
-            clamped = *(pb_int64_t*)field->pData = svalue;
-        else if (field->data_size == sizeof(int32_t))
-            clamped = *(int32_t*)field->pData = (int32_t)svalue;
-        else if (field->data_size == sizeof(int_least16_t))
-            clamped = *(int_least16_t*)field->pData = (int_least16_t)svalue;
-        else if (field->data_size == sizeof(int_least8_t))
-            clamped = *(int_least8_t*)field->pData = (int_least8_t)svalue;
-        else
-            PB_RETURN_ERROR(stream, "invalid data_size");
-
-        if (clamped != svalue)
-            PB_RETURN_ERROR(stream, "integer too large");
-
-        return true;
-    }
-}
-
-static bool checkreturn pb_dec_fixed(pb_istream_t *stream, const pb_field_iter_t *field)
-{
-#ifdef PB_CONVERT_DOUBLE_FLOAT
-    if (field->data_size == sizeof(float) && PB_LTYPE(field->type) == PB_LTYPE_FIXED64)
-    {
-        return pb_decode_double_as_float(stream, (float*)field->pData);
-    }
-#endif
-
-    if (field->data_size == sizeof(uint32_t))
-    {
-        return pb_decode_fixed32(stream, field->pData);
-    }
-#ifndef PB_WITHOUT_64BIT
-    else if (field->data_size == sizeof(uint64_t))
-    {
-        return pb_decode_fixed64(stream, field->pData);
-    }
-#endif
-    else
-    {
-        PB_RETURN_ERROR(stream, "invalid data_size");
-    }
-}
-
-static bool checkreturn pb_dec_bytes(pb_istream_t *stream, const pb_field_iter_t *field)
-{
-    uint32_t size;
-    size_t alloc_size;
-    pb_bytes_array_t *dest;
-    
-    if (!pb_decode_varint32(stream, &size))
-        return false;
-    
-    if (size > PB_SIZE_MAX)
-        PB_RETURN_ERROR(stream, "bytes overflow");
-    
-    alloc_size = PB_BYTES_ARRAY_T_ALLOCSIZE(size);
-    if (size > alloc_size)
-        PB_RETURN_ERROR(stream, "size too large");
-    
-    if (PB_ATYPE(field->type) == PB_ATYPE_POINTER)
-    {
-#ifndef PB_ENABLE_MALLOC
-        PB_RETURN_ERROR(stream, "no malloc support");
-#else
-        if (stream->bytes_left < size)
-            PB_RETURN_ERROR(stream, "end-of-stream");
-
-        if (!allocate_field(stream, field->pData, alloc_size, 1))
-            return false;
-        dest = *(pb_bytes_array_t**)field->pData;
-#endif
-    }
-    else
-    {
-        if (alloc_size > field->data_size)
-            PB_RETURN_ERROR(stream, "bytes overflow");
-        dest = (pb_bytes_array_t*)field->pData;
-    }
-
-    dest->size = (pb_size_t)size;
-    return pb_read(stream, dest->bytes, (size_t)size);
-}
-
-static bool checkreturn pb_dec_string(pb_istream_t *stream, const pb_field_iter_t *field)
-{
-    uint32_t size;
-    size_t alloc_size;
-    pb_byte_t *dest = (pb_byte_t*)field->pData;
-
-    if (!pb_decode_varint32(stream, &size))
-        return false;
-
-    if (size == (uint32_t)-1)
-        PB_RETURN_ERROR(stream, "size too large");
-
-    /* Space for null terminator */
-    alloc_size = (size_t)(size + 1);
-
-    if (alloc_size < size)
-        PB_RETURN_ERROR(stream, "size too large");
-
-    if (PB_ATYPE(field->type) == PB_ATYPE_POINTER)
-    {
-#ifndef PB_ENABLE_MALLOC
-        PB_RETURN_ERROR(stream, "no malloc support");
-#else
-        if (stream->bytes_left < size)
-            PB_RETURN_ERROR(stream, "end-of-stream");
-
-        if (!allocate_field(stream, field->pData, alloc_size, 1))
-            return false;
-        dest = *(pb_byte_t**)field->pData;
-#endif
-    }
-    else
-    {
-        if (alloc_size > field->data_size)
-            PB_RETURN_ERROR(stream, "string overflow");
-    }
-    
-    dest[size] = 0;
-
-    if (!pb_read(stream, dest, (size_t)size))
-        return false;
-
-#ifdef PB_VALIDATE_UTF8
-    if (!pb_validate_utf8((const char*)dest))
-        PB_RETURN_ERROR(stream, "invalid utf8");
-#endif
-
-    return true;
-}
-
-static bool checkreturn pb_dec_submessage(pb_istream_t *stream, const pb_field_iter_t *field)
-{
-    bool status = true;
-    pb_istream_t substream;
-
-    if (!pb_make_string_substream(stream, &substream))
-        return false;
-    
-    if (field->submsg_desc == NULL)
-        PB_RETURN_ERROR(stream, "invalid field descriptor");
-    
-    /* New array entries need to be initialized, while required and optional
-     * submessages have already been initialized in the top-level pb_decode. */
-    if (PB_HTYPE(field->type) == PB_HTYPE_REPEATED ||
-        PB_HTYPE(field->type) == PB_HTYPE_ONEOF)
-    {
-        pb_field_iter_t submsg_iter;
-        if (pb_field_iter_begin(&submsg_iter, field->submsg_desc, field->pData))
-        {
-            if (!pb_message_set_to_defaults(&submsg_iter))
-                PB_RETURN_ERROR(stream, "failed to set defaults");
-        }
-    }
-
-    /* Submessages can have a separate message-level callback that is called
-     * before decoding the message. Typically it is used to set callback fields
-     * inside oneofs. */
-    if (PB_LTYPE(field->type) == PB_LTYPE_SUBMSG_W_CB && field->pSize != NULL)
-    {
-        /* Message callback is stored right before pSize. */
-        pb_callback_t *callback = (pb_callback_t*)field->pSize - 1;
-        if (callback->funcs.decode)
-        {
-            status = callback->funcs.decode(&substream, field, &callback->arg);
-        }
-    }
-
-    /* Now decode the submessage contents */
-    if (status)
-    {
-        status = pb_decode_inner(&substream, field->submsg_desc, field->pData, 0);
-    }
-    
-    if (!pb_close_string_substream(stream, &substream))
-        return false;
-
-    return status;
-}
-
-static bool checkreturn pb_dec_fixed_length_bytes(pb_istream_t *stream, const pb_field_iter_t *field)
-{
-    uint32_t size;
-
-    if (!pb_decode_varint32(stream, &size))
-        return false;
-
-    if (size > PB_SIZE_MAX)
-        PB_RETURN_ERROR(stream, "bytes overflow");
-
-    if (size == 0)
-    {
-        /* As a special case, treat empty bytes string as all zeros for fixed_length_bytes. */
-        memset(field->pData, 0, (size_t)field->data_size);
-        return true;
-    }
-
-    if (size != field->data_size)
-        PB_RETURN_ERROR(stream, "incorrect fixed length bytes size");
-
-    return pb_read(stream, (pb_byte_t*)field->pData, (size_t)field->data_size);
-}
-
-#ifdef PB_CONVERT_DOUBLE_FLOAT
-bool pb_decode_double_as_float(pb_istream_t *stream, float *dest)
-{
-    uint_least8_t sign;
-    int exponent;
-    uint32_t mantissa;
-    uint64_t value;
-    union { float f; uint32_t i; } out;
-
-    if (!pb_decode_fixed64(stream, &value))
-        return false;
-
-    /* Decompose input value */
-    sign = (uint_least8_t)((value >> 63) & 1);
-    exponent = (int)((value >> 52) & 0x7FF) - 1023;
-    mantissa = (value >> 28) & 0xFFFFFF; /* Highest 24 bits */
-
-    /* Figure if value is in range representable by floats. */
-    if (exponent == 1024)
-    {
-        /* Special value */
-        exponent = 128;
-    }
-    else if (exponent > 127)
-    {
-        /* Too large, convert to infinity */
-        exponent = 128;
-        mantissa = 0;
-    }
-    else if (exponent < -150)
-    {
-        /* Too small, convert to zero */
-        exponent = -127;
-        mantissa = 0;
-    }
-    else if (exponent < -126)
-    {
-        /* Denormalized */
-        mantissa |= 0x1000000;
-        mantissa >>= (-126 - exponent);
-        exponent = -127;
-    }
-
-    /* Round off mantissa */
-    mantissa = (mantissa + 1) >> 1;
-
-    /* Check if mantissa went over 2.0 */
-    if (mantissa & 0x800000)
-    {
-        exponent += 1;
-        mantissa &= 0x7FFFFF;
-        mantissa >>= 1;
-    }
-
-    /* Combine fields */
-    out.i = mantissa;
-    out.i |= (uint32_t)(exponent + 127) << 23;
-    out.i |= (uint32_t)sign << 31;
-
-    *dest = out.f;
-    return true;
-}
-#endif
diff --git a/src/pb_decode.h b/src/pb_decode.h
deleted file mode 100644
index b64d95a..0000000
--- a/src/pb_decode.h
+++ /dev/null
@@ -1,193 +0,0 @@
-/* pb_decode.h: Functions to decode protocol buffers. Depends on pb_decode.c.
- * The main function is pb_decode. You also need an input stream, and the
- * field descriptions created by nanopb_generator.py.
- */
-
-#ifndef PB_DECODE_H_INCLUDED
-#define PB_DECODE_H_INCLUDED
-
-#include "pb.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* Structure for defining custom input streams. You will need to provide
- * a callback function to read the bytes from your storage, which can be
- * for example a file or a network socket.
- * 
- * The callback must conform to these rules:
- *
- * 1) Return false on IO errors. This will cause decoding to abort.
- * 2) You can use state to store your own data (e.g. buffer pointer),
- *    and rely on pb_read to verify that no-body reads past bytes_left.
- * 3) Your callback may be used with substreams, in which case bytes_left
- *    is different than from the main stream. Don't use bytes_left to compute
- *    any pointers.
- */
-struct pb_istream_s
-{
-#ifdef PB_BUFFER_ONLY
-    /* Callback pointer is not used in buffer-only configuration.
-     * Having an int pointer here allows binary compatibility but
-     * gives an error if someone tries to assign callback function.
-     */
-    int *callback;
-#else
-    bool (*callback)(pb_istream_t *stream, pb_byte_t *buf, size_t count);
-#endif
-
-    void *state; /* Free field for use by callback implementation */
-    size_t bytes_left;
-    
-#ifndef PB_NO_ERRMSG
-    const char *errmsg;
-#endif
-};
-
-#ifndef PB_NO_ERRMSG
-#define PB_ISTREAM_EMPTY {0,0,0,0}
-#else
-#define PB_ISTREAM_EMPTY {0,0,0}
-#endif
-
-/***************************
- * Main decoding functions *
- ***************************/
- 
-/* Decode a single protocol buffers message from input stream into a C structure.
- * Returns true on success, false on any failure.
- * The actual struct pointed to by dest must match the description in fields.
- * Callback fields of the destination structure must be initialized by caller.
- * All other fields will be initialized by this function.
- *
- * Example usage:
- *    MyMessage msg = {};
- *    uint8_t buffer[64];
- *    pb_istream_t stream;
- *    
- *    // ... read some data into buffer ...
- *
- *    stream = pb_istream_from_buffer(buffer, count);
- *    pb_decode(&stream, MyMessage_fields, &msg);
- */
-bool pb_decode(pb_istream_t *stream, const pb_msgdesc_t *fields, void *dest_struct);
-
-/* Extended version of pb_decode, with several options to control
- * the decoding process:
- *
- * PB_DECODE_NOINIT:         Do not initialize the fields to default values.
- *                           This is slightly faster if you do not need the default
- *                           values and instead initialize the structure to 0 using
- *                           e.g. memset(). This can also be used for merging two
- *                           messages, i.e. combine already existing data with new
- *                           values.
- *
- * PB_DECODE_DELIMITED:      Input message starts with the message size as varint.
- *                           Corresponds to parseDelimitedFrom() in Google's
- *                           protobuf API.
- *
- * PB_DECODE_NULLTERMINATED: Stop reading when field tag is read as 0. This allows
- *                           reading null terminated messages.
- *                           NOTE: Until nanopb-0.4.0, pb_decode() also allows
- *                           null-termination. This behaviour is not supported in
- *                           most other protobuf implementations, so PB_DECODE_DELIMITED
- *                           is a better option for compatibility.
- *
- * Multiple flags can be combined with bitwise or (| operator)
- */
-#define PB_DECODE_NOINIT          0x01U
-#define PB_DECODE_DELIMITED       0x02U
-#define PB_DECODE_NULLTERMINATED  0x04U
-bool pb_decode_ex(pb_istream_t *stream, const pb_msgdesc_t *fields, void *dest_struct, unsigned int flags);
-
-/* Defines for backwards compatibility with code written before nanopb-0.4.0 */
-#define pb_decode_noinit(s,f,d) pb_decode_ex(s,f,d, PB_DECODE_NOINIT)
-#define pb_decode_delimited(s,f,d) pb_decode_ex(s,f,d, PB_DECODE_DELIMITED)
-#define pb_decode_delimited_noinit(s,f,d) pb_decode_ex(s,f,d, PB_DECODE_DELIMITED | PB_DECODE_NOINIT)
-#define pb_decode_nullterminated(s,f,d) pb_decode_ex(s,f,d, PB_DECODE_NULLTERMINATED)
-
-#ifdef PB_ENABLE_MALLOC
-/* Release any allocated pointer fields. If you use dynamic allocation, you should
- * call this for any successfully decoded message when you are done with it. If
- * pb_decode() returns with an error, the message is already released.
- */
-void pb_release(const pb_msgdesc_t *fields, void *dest_struct);
-#endif
-
-
-/**************************************
- * Functions for manipulating streams *
- **************************************/
-
-/* Create an input stream for reading from a memory buffer.
- *
- * Alternatively, you can use a custom stream that reads directly from e.g.
- * a file or a network socket.
- */
-pb_istream_t pb_istream_from_buffer(const pb_byte_t *buf, size_t bufsize);
-
-/* Function to read from a pb_istream_t. You can use this if you need to
- * read some custom header data, or to read data in field callbacks.
- */
-bool pb_read(pb_istream_t *stream, pb_byte_t *buf, size_t count);
-
-
-/************************************************
- * Helper functions for writing field callbacks *
- ************************************************/
-
-/* Decode the tag for the next field in the stream. Gives the wire type and
- * field tag. At end of the message, returns false and sets eof to true. */
-bool pb_decode_tag(pb_istream_t *stream, pb_wire_type_t *wire_type, uint32_t *tag, bool *eof);
-
-/* Skip the field payload data, given the wire type. */
-bool pb_skip_field(pb_istream_t *stream, pb_wire_type_t wire_type);
-
-/* Decode an integer in the varint format. This works for enum, int32,
- * int64, uint32 and uint64 field types. */
-#ifndef PB_WITHOUT_64BIT
-bool pb_decode_varint(pb_istream_t *stream, uint64_t *dest);
-#else
-#define pb_decode_varint pb_decode_varint32
-#endif
-
-/* Decode an integer in the varint format. This works for enum, int32,
- * and uint32 field types. */
-bool pb_decode_varint32(pb_istream_t *stream, uint32_t *dest);
-
-/* Decode a bool value in varint format. */
-bool pb_decode_bool(pb_istream_t *stream, bool *dest);
-
-/* Decode an integer in the zig-zagged svarint format. This works for sint32
- * and sint64. */
-#ifndef PB_WITHOUT_64BIT
-bool pb_decode_svarint(pb_istream_t *stream, int64_t *dest);
-#else
-bool pb_decode_svarint(pb_istream_t *stream, int32_t *dest);
-#endif
-
-/* Decode a fixed32, sfixed32 or float value. You need to pass a pointer to
- * a 4-byte wide C variable. */
-bool pb_decode_fixed32(pb_istream_t *stream, void *dest);
-
-#ifndef PB_WITHOUT_64BIT
-/* Decode a fixed64, sfixed64 or double value. You need to pass a pointer to
- * a 8-byte wide C variable. */
-bool pb_decode_fixed64(pb_istream_t *stream, void *dest);
-#endif
-
-#ifdef PB_CONVERT_DOUBLE_FLOAT
-/* Decode a double value into float variable. */
-bool pb_decode_double_as_float(pb_istream_t *stream, float *dest);
-#endif
-
-/* Make a limited-length substream for reading a PB_WT_STRING field. */
-bool pb_make_string_substream(pb_istream_t *stream, pb_istream_t *substream);
-bool pb_close_string_substream(pb_istream_t *stream, pb_istream_t *substream);
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif
diff --git a/src/pb_encode.c b/src/pb_encode.c
deleted file mode 100644
index 409fec3..0000000
--- a/src/pb_encode.c
+++ /dev/null
@@ -1,958 +0,0 @@
-/* pb_encode.c -- encode a protobuf using minimal resources
- *
- * 2011 Petteri Aimonen <jpa@kapsi.fi>
- */
-
-#include "pb.h"
-#include "pb_encode.h"
-#include "pb_common.h"
-
-/* Use the GCC warn_unused_result attribute to check that all return values
- * are propagated correctly. On other compilers and gcc before 3.4.0 just
- * ignore the annotation.
- */
-#if !defined(__GNUC__) || ( __GNUC__ < 3) || (__GNUC__ == 3 && __GNUC_MINOR__ < 4)
-    #define checkreturn
-#else
-    #define checkreturn __attribute__((warn_unused_result))
-#endif
-
-/**************************************
- * Declarations internal to this file *
- **************************************/
-static bool checkreturn buf_write(pb_ostream_t *stream, const pb_byte_t *buf, size_t count);
-static bool checkreturn encode_array(pb_ostream_t *stream, pb_field_iter_t *field);
-static bool checkreturn pb_check_proto3_default_value(const pb_field_iter_t *field);
-static bool checkreturn encode_basic_field(pb_ostream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn encode_callback_field(pb_ostream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn encode_field(pb_ostream_t *stream, pb_field_iter_t *field);
-static bool checkreturn encode_extension_field(pb_ostream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn default_extension_encoder(pb_ostream_t *stream, const pb_extension_t *extension);
-static bool checkreturn pb_encode_varint_32(pb_ostream_t *stream, uint32_t low, uint32_t high);
-static bool checkreturn pb_enc_bool(pb_ostream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_enc_varint(pb_ostream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_enc_fixed(pb_ostream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_enc_bytes(pb_ostream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_enc_string(pb_ostream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_enc_submessage(pb_ostream_t *stream, const pb_field_iter_t *field);
-static bool checkreturn pb_enc_fixed_length_bytes(pb_ostream_t *stream, const pb_field_iter_t *field);
-
-#ifdef PB_WITHOUT_64BIT
-#define pb_int64_t int32_t
-#define pb_uint64_t uint32_t
-#else
-#define pb_int64_t int64_t
-#define pb_uint64_t uint64_t
-#endif
-
-/*******************************
- * pb_ostream_t implementation *
- *******************************/
-
-static bool checkreturn buf_write(pb_ostream_t *stream, const pb_byte_t *buf, size_t count)
-{
-    size_t i;
-    pb_byte_t *dest = (pb_byte_t*)stream->state;
-    stream->state = dest + count;
-    
-    for (i = 0; i < count; i++)
-        dest[i] = buf[i];
-    
-    return true;
-}
-
-pb_ostream_t pb_ostream_from_buffer(pb_byte_t *buf, size_t bufsize)
-{
-    pb_ostream_t stream;
-#ifdef PB_BUFFER_ONLY
-    stream.callback = (void*)1; /* Just a marker value */
-#else
-    stream.callback = &buf_write;
-#endif
-    stream.state = buf;
-    stream.max_size = bufsize;
-    stream.bytes_written = 0;
-#ifndef PB_NO_ERRMSG
-    stream.errmsg = NULL;
-#endif
-    return stream;
-}
-
-bool checkreturn pb_write(pb_ostream_t *stream, const pb_byte_t *buf, size_t count)
-{
-    if (count > 0 && stream->callback != NULL)
-    {
-        if (stream->bytes_written + count > stream->max_size)
-            PB_RETURN_ERROR(stream, "stream full");
-
-#ifdef PB_BUFFER_ONLY
-        if (!buf_write(stream, buf, count))
-            PB_RETURN_ERROR(stream, "io error");
-#else        
-        if (!stream->callback(stream, buf, count))
-            PB_RETURN_ERROR(stream, "io error");
-#endif
-    }
-    
-    stream->bytes_written += count;
-    return true;
-}
-
-/*************************
- * Encode a single field *
- *************************/
-
-/* Read a bool value without causing undefined behavior even if the value
- * is invalid. See issue #434 and
- * https://stackoverflow.com/questions/27661768/weird-results-for-conditional
- */
-static bool safe_read_bool(const void *pSize)
-{
-    const char *p = (const char *)pSize;
-    size_t i;
-    for (i = 0; i < sizeof(bool); i++)
-    {
-        if (p[i] != 0)
-            return true;
-    }
-    return false;
-}
-
-/* Encode a static array. Handles the size calculations and possible packing. */
-static bool checkreturn encode_array(pb_ostream_t *stream, pb_field_iter_t *field)
-{
-    pb_size_t i;
-    pb_size_t count;
-#ifndef PB_ENCODE_ARRAYS_UNPACKED
-    size_t size;
-#endif
-
-    count = *(pb_size_t*)field->pSize;
-
-    if (count == 0)
-        return true;
-
-    if (PB_ATYPE(field->type) != PB_ATYPE_POINTER && count > field->array_size)
-        PB_RETURN_ERROR(stream, "array max size exceeded");
-    
-#ifndef PB_ENCODE_ARRAYS_UNPACKED
-    /* We always pack arrays if the datatype allows it. */
-    if (PB_LTYPE(field->type) <= PB_LTYPE_LAST_PACKABLE)
-    {
-        if (!pb_encode_tag(stream, PB_WT_STRING, field->tag))
-            return false;
-        
-        /* Determine the total size of packed array. */
-        if (PB_LTYPE(field->type) == PB_LTYPE_FIXED32)
-        {
-            size = 4 * (size_t)count;
-        }
-        else if (PB_LTYPE(field->type) == PB_LTYPE_FIXED64)
-        {
-            size = 8 * (size_t)count;
-        }
-        else
-        { 
-            pb_ostream_t sizestream = PB_OSTREAM_SIZING;
-            void *pData_orig = field->pData;
-            for (i = 0; i < count; i++)
-            {
-                if (!pb_enc_varint(&sizestream, field))
-                    PB_RETURN_ERROR(stream, PB_GET_ERROR(&sizestream));
-                field->pData = (char*)field->pData + field->data_size;
-            }
-            field->pData = pData_orig;
-            size = sizestream.bytes_written;
-        }
-        
-        if (!pb_encode_varint(stream, (pb_uint64_t)size))
-            return false;
-        
-        if (stream->callback == NULL)
-            return pb_write(stream, NULL, size); /* Just sizing.. */
-        
-        /* Write the data */
-        for (i = 0; i < count; i++)
-        {
-            if (PB_LTYPE(field->type) == PB_LTYPE_FIXED32 || PB_LTYPE(field->type) == PB_LTYPE_FIXED64)
-            {
-                if (!pb_enc_fixed(stream, field))
-                    return false;
-            }
-            else
-            {
-                if (!pb_enc_varint(stream, field))
-                    return false;
-            }
-
-            field->pData = (char*)field->pData + field->data_size;
-        }
-    }
-    else /* Unpacked fields */
-#endif
-    {
-        for (i = 0; i < count; i++)
-        {
-            /* Normally the data is stored directly in the array entries, but
-             * for pointer-type string and bytes fields, the array entries are
-             * actually pointers themselves also. So we have to dereference once
-             * more to get to the actual data. */
-            if (PB_ATYPE(field->type) == PB_ATYPE_POINTER &&
-                (PB_LTYPE(field->type) == PB_LTYPE_STRING ||
-                 PB_LTYPE(field->type) == PB_LTYPE_BYTES))
-            {
-                bool status;
-                void *pData_orig = field->pData;
-                field->pData = *(void* const*)field->pData;
-
-                if (!field->pData)
-                {
-                    /* Null pointer in array is treated as empty string / bytes */
-                    status = pb_encode_tag_for_field(stream, field) &&
-                             pb_encode_varint(stream, 0);
-                }
-                else
-                {
-                    status = encode_basic_field(stream, field);
-                }
-
-                field->pData = pData_orig;
-
-                if (!status)
-                    return false;
-            }
-            else
-            {
-                if (!encode_basic_field(stream, field))
-                    return false;
-            }
-            field->pData = (char*)field->pData + field->data_size;
-        }
-    }
-    
-    return true;
-}
-
-/* In proto3, all fields are optional and are only encoded if their value is "non-zero".
- * This function implements the check for the zero value. */
-static bool checkreturn pb_check_proto3_default_value(const pb_field_iter_t *field)
-{
-    pb_type_t type = field->type;
-
-    if (PB_ATYPE(type) == PB_ATYPE_STATIC)
-    {
-        if (PB_HTYPE(type) == PB_HTYPE_REQUIRED)
-        {
-            /* Required proto2 fields inside proto3 submessage, pretty rare case */
-            return false;
-        }
-        else if (PB_HTYPE(type) == PB_HTYPE_REPEATED)
-        {
-            /* Repeated fields inside proto3 submessage: present if count != 0 */
-            return *(const pb_size_t*)field->pSize == 0;
-        }
-        else if (PB_HTYPE(type) == PB_HTYPE_ONEOF)
-        {
-            /* Oneof fields */
-            return *(const pb_size_t*)field->pSize == 0;
-        }
-        else if (PB_HTYPE(type) == PB_HTYPE_OPTIONAL && field->pSize != NULL)
-        {
-            /* Proto2 optional fields inside proto3 message, or proto3
-             * submessage fields. */
-            return safe_read_bool(field->pSize) == false;
-        }
-
-        /* Rest is proto3 singular fields */
-        if (PB_LTYPE(type) == PB_LTYPE_BYTES)
-        {
-            const pb_bytes_array_t *bytes = (const pb_bytes_array_t*)field->pData;
-            return bytes->size == 0;
-        }
-        else if (PB_LTYPE(type) == PB_LTYPE_STRING)
-        {
-            return *(const char*)field->pData == '\0';
-        }
-        else if (PB_LTYPE(type) == PB_LTYPE_FIXED_LENGTH_BYTES)
-        {
-            /* Fixed length bytes is only empty if its length is fixed
-             * as 0. Which would be pretty strange, but we can check
-             * it anyway. */
-            return field->data_size == 0;
-        }
-        else if (PB_LTYPE_IS_SUBMSG(type))
-        {
-            /* Check all fields in the submessage to find if any of them
-             * are non-zero. The comparison cannot be done byte-per-byte
-             * because the C struct may contain padding bytes that must
-             * be skipped. Note that usually proto3 submessages have
-             * a separate has_field that is checked earlier in this if.
-             */
-            pb_field_iter_t iter;
-            if (pb_field_iter_begin(&iter, field->submsg_desc, field->pData))
-            {
-                do
-                {
-                    if (!pb_check_proto3_default_value(&iter))
-                    {
-                        return false;
-                    }
-                } while (pb_field_iter_next(&iter));
-            }
-            return true;
-        }
-    }
-    
-    {
-        /* Catch-all branch that does byte-per-byte comparison for zero value.
-         *
-         * This is for all pointer fields, and for static PB_LTYPE_VARINT,
-         * UVARINT, SVARINT, FIXED32, FIXED64, EXTENSION fields, and also
-         * callback fields. These all have integer or pointer value which
-         * can be compared with 0.
-         */
-        pb_size_t i;
-        const char *p = (const char*)field->pData;
-        for (i = 0; i < field->data_size; i++)
-        {
-            if (p[i] != 0)
-            {
-                return false;
-            }
-        }
-
-        return true;
-    }
-}
-
-/* Encode a field with static or pointer allocation, i.e. one whose data
- * is available to the encoder directly. */
-static bool checkreturn encode_basic_field(pb_ostream_t *stream, const pb_field_iter_t *field)
-{
-    if (!field->pData)
-    {
-        /* Missing pointer field */
-        return true;
-    }
-
-    if (!pb_encode_tag_for_field(stream, field))
-        return false;
-
-    switch (PB_LTYPE(field->type))
-    {
-        case PB_LTYPE_BOOL:
-            return pb_enc_bool(stream, field);
-
-        case PB_LTYPE_VARINT:
-        case PB_LTYPE_UVARINT:
-        case PB_LTYPE_SVARINT:
-            return pb_enc_varint(stream, field);
-
-        case PB_LTYPE_FIXED32:
-        case PB_LTYPE_FIXED64:
-            return pb_enc_fixed(stream, field);
-
-        case PB_LTYPE_BYTES:
-            return pb_enc_bytes(stream, field);
-
-        case PB_LTYPE_STRING:
-            return pb_enc_string(stream, field);
-
-        case PB_LTYPE_SUBMESSAGE:
-        case PB_LTYPE_SUBMSG_W_CB:
-            return pb_enc_submessage(stream, field);
-
-        case PB_LTYPE_FIXED_LENGTH_BYTES:
-            return pb_enc_fixed_length_bytes(stream, field);
-
-        default:
-            PB_RETURN_ERROR(stream, "invalid field type");
-    }
-}
-
-/* Encode a field with callback semantics. This means that a user function is
- * called to provide and encode the actual data. */
-static bool checkreturn encode_callback_field(pb_ostream_t *stream, const pb_field_iter_t *field)
-{
-    if (field->descriptor->field_callback != NULL)
-    {
-        if (!field->descriptor->field_callback(NULL, stream, field))
-            PB_RETURN_ERROR(stream, "callback error");
-    }
-    return true;
-}
-
-/* Encode a single field of any callback, pointer or static type. */
-static bool checkreturn encode_field(pb_ostream_t *stream, pb_field_iter_t *field)
-{
-    /* Check field presence */
-    if (PB_HTYPE(field->type) == PB_HTYPE_ONEOF)
-    {
-        if (*(const pb_size_t*)field->pSize != field->tag)
-        {
-            /* Different type oneof field */
-            return true;
-        }
-    }
-    else if (PB_HTYPE(field->type) == PB_HTYPE_OPTIONAL)
-    {
-        if (field->pSize)
-        {
-            if (safe_read_bool(field->pSize) == false)
-            {
-                /* Missing optional field */
-                return true;
-            }
-        }
-        else if (PB_ATYPE(field->type) == PB_ATYPE_STATIC)
-        {
-            /* Proto3 singular field */
-            if (pb_check_proto3_default_value(field))
-                return true;
-        }
-    }
-
-    if (!field->pData)
-    {
-        if (PB_HTYPE(field->type) == PB_HTYPE_REQUIRED)
-            PB_RETURN_ERROR(stream, "missing required field");
-
-        /* Pointer field set to NULL */
-        return true;
-    }
-
-    /* Then encode field contents */
-    if (PB_ATYPE(field->type) == PB_ATYPE_CALLBACK)
-    {
-        return encode_callback_field(stream, field);
-    }
-    else if (PB_HTYPE(field->type) == PB_HTYPE_REPEATED)
-    {
-        return encode_array(stream, field);
-    }
-    else
-    {
-        return encode_basic_field(stream, field);
-    }
-}
-
-/* Default handler for extension fields. Expects to have a pb_msgdesc_t
- * pointer in the extension->type->arg field, pointing to a message with
- * only one field in it.  */
-static bool checkreturn default_extension_encoder(pb_ostream_t *stream, const pb_extension_t *extension)
-{
-    pb_field_iter_t iter;
-
-    if (!pb_field_iter_begin_extension_const(&iter, extension))
-        PB_RETURN_ERROR(stream, "invalid extension");
-
-    return encode_field(stream, &iter);
-}
-
-
-/* Walk through all the registered extensions and give them a chance
- * to encode themselves. */
-static bool checkreturn encode_extension_field(pb_ostream_t *stream, const pb_field_iter_t *field)
-{
-    const pb_extension_t *extension = *(const pb_extension_t* const *)field->pData;
-
-    while (extension)
-    {
-        bool status;
-        if (extension->type->encode)
-            status = extension->type->encode(stream, extension);
-        else
-            status = default_extension_encoder(stream, extension);
-
-        if (!status)
-            return false;
-        
-        extension = extension->next;
-    }
-    
-    return true;
-}
-
-/*********************
- * Encode all fields *
- *********************/
-
-bool checkreturn pb_encode(pb_ostream_t *stream, const pb_msgdesc_t *fields, const void *src_struct)
-{
-    pb_field_iter_t iter;
-    if (!pb_field_iter_begin_const(&iter, fields, src_struct))
-        return true; /* Empty message type */
-    
-    do {
-        if (PB_LTYPE(iter.type) == PB_LTYPE_EXTENSION)
-        {
-            /* Special case for the extension field placeholder */
-            if (!encode_extension_field(stream, &iter))
-                return false;
-        }
-        else
-        {
-            /* Regular field */
-            if (!encode_field(stream, &iter))
-                return false;
-        }
-    } while (pb_field_iter_next(&iter));
-    
-    return true;
-}
-
-bool checkreturn pb_encode_ex(pb_ostream_t *stream, const pb_msgdesc_t *fields, const void *src_struct, unsigned int flags)
-{
-  if ((flags & PB_ENCODE_DELIMITED) != 0)
-  {
-    return pb_encode_submessage(stream, fields, src_struct);
-  }
-  else if ((flags & PB_ENCODE_NULLTERMINATED) != 0)
-  {
-    const pb_byte_t zero = 0;
-
-    if (!pb_encode(stream, fields, src_struct))
-        return false;
-
-    return pb_write(stream, &zero, 1);
-  }
-  else
-  {
-    return pb_encode(stream, fields, src_struct);
-  }
-}
-
-bool pb_get_encoded_size(size_t *size, const pb_msgdesc_t *fields, const void *src_struct)
-{
-    pb_ostream_t stream = PB_OSTREAM_SIZING;
-    
-    if (!pb_encode(&stream, fields, src_struct))
-        return false;
-    
-    *size = stream.bytes_written;
-    return true;
-}
-
-/********************
- * Helper functions *
- ********************/
-
-/* This function avoids 64-bit shifts as they are quite slow on many platforms. */
-static bool checkreturn pb_encode_varint_32(pb_ostream_t *stream, uint32_t low, uint32_t high)
-{
-    size_t i = 0;
-    pb_byte_t buffer[10];
-    pb_byte_t byte = (pb_byte_t)(low & 0x7F);
-    low >>= 7;
-
-    while (i < 4 && (low != 0 || high != 0))
-    {
-        byte |= 0x80;
-        buffer[i++] = byte;
-        byte = (pb_byte_t)(low & 0x7F);
-        low >>= 7;
-    }
-
-    if (high)
-    {
-        byte = (pb_byte_t)(byte | ((high & 0x07) << 4));
-        high >>= 3;
-
-        while (high)
-        {
-            byte |= 0x80;
-            buffer[i++] = byte;
-            byte = (pb_byte_t)(high & 0x7F);
-            high >>= 7;
-        }
-    }
-
-    buffer[i++] = byte;
-
-    return pb_write(stream, buffer, i);
-}
-
-bool checkreturn pb_encode_varint(pb_ostream_t *stream, pb_uint64_t value)
-{
-    if (value <= 0x7F)
-    {
-        /* Fast path: single byte */
-        pb_byte_t byte = (pb_byte_t)value;
-        return pb_write(stream, &byte, 1);
-    }
-    else
-    {
-#ifdef PB_WITHOUT_64BIT
-        return pb_encode_varint_32(stream, value, 0);
-#else
-        return pb_encode_varint_32(stream, (uint32_t)value, (uint32_t)(value >> 32));
-#endif
-    }
-}
-
-bool checkreturn pb_encode_svarint(pb_ostream_t *stream, pb_int64_t value)
-{
-    pb_uint64_t zigzagged;
-    if (value < 0)
-        zigzagged = ~((pb_uint64_t)value << 1);
-    else
-        zigzagged = (pb_uint64_t)value << 1;
-    
-    return pb_encode_varint(stream, zigzagged);
-}
-
-bool checkreturn pb_encode_fixed32(pb_ostream_t *stream, const void *value)
-{
-    uint32_t val = *(const uint32_t*)value;
-    pb_byte_t bytes[4];
-    bytes[0] = (pb_byte_t)(val & 0xFF);
-    bytes[1] = (pb_byte_t)((val >> 8) & 0xFF);
-    bytes[2] = (pb_byte_t)((val >> 16) & 0xFF);
-    bytes[3] = (pb_byte_t)((val >> 24) & 0xFF);
-    return pb_write(stream, bytes, 4);
-}
-
-#ifndef PB_WITHOUT_64BIT
-bool checkreturn pb_encode_fixed64(pb_ostream_t *stream, const void *value)
-{
-    uint64_t val = *(const uint64_t*)value;
-    pb_byte_t bytes[8];
-    bytes[0] = (pb_byte_t)(val & 0xFF);
-    bytes[1] = (pb_byte_t)((val >> 8) & 0xFF);
-    bytes[2] = (pb_byte_t)((val >> 16) & 0xFF);
-    bytes[3] = (pb_byte_t)((val >> 24) & 0xFF);
-    bytes[4] = (pb_byte_t)((val >> 32) & 0xFF);
-    bytes[5] = (pb_byte_t)((val >> 40) & 0xFF);
-    bytes[6] = (pb_byte_t)((val >> 48) & 0xFF);
-    bytes[7] = (pb_byte_t)((val >> 56) & 0xFF);
-    return pb_write(stream, bytes, 8);
-}
-#endif
-
-bool checkreturn pb_encode_tag(pb_ostream_t *stream, pb_wire_type_t wiretype, uint32_t field_number)
-{
-    pb_uint64_t tag = ((pb_uint64_t)field_number << 3) | wiretype;
-    return pb_encode_varint(stream, tag);
-}
-
-bool pb_encode_tag_for_field ( pb_ostream_t* stream, const pb_field_iter_t* field )
-{
-    pb_wire_type_t wiretype;
-    switch (PB_LTYPE(field->type))
-    {
-        case PB_LTYPE_BOOL:
-        case PB_LTYPE_VARINT:
-        case PB_LTYPE_UVARINT:
-        case PB_LTYPE_SVARINT:
-            wiretype = PB_WT_VARINT;
-            break;
-        
-        case PB_LTYPE_FIXED32:
-            wiretype = PB_WT_32BIT;
-            break;
-        
-        case PB_LTYPE_FIXED64:
-            wiretype = PB_WT_64BIT;
-            break;
-        
-        case PB_LTYPE_BYTES:
-        case PB_LTYPE_STRING:
-        case PB_LTYPE_SUBMESSAGE:
-        case PB_LTYPE_SUBMSG_W_CB:
-        case PB_LTYPE_FIXED_LENGTH_BYTES:
-            wiretype = PB_WT_STRING;
-            break;
-        
-        default:
-            PB_RETURN_ERROR(stream, "invalid field type");
-    }
-    
-    return pb_encode_tag(stream, wiretype, field->tag);
-}
-
-bool checkreturn pb_encode_string(pb_ostream_t *stream, const pb_byte_t *buffer, size_t size)
-{
-    if (!pb_encode_varint(stream, (pb_uint64_t)size))
-        return false;
-    
-    return pb_write(stream, buffer, size);
-}
-
-bool checkreturn pb_encode_submessage(pb_ostream_t *stream, const pb_msgdesc_t *fields, const void *src_struct)
-{
-    /* First calculate the message size using a non-writing substream. */
-    pb_ostream_t substream = PB_OSTREAM_SIZING;
-    size_t size;
-    bool status;
-    
-    if (!pb_encode(&substream, fields, src_struct))
-    {
-#ifndef PB_NO_ERRMSG
-        stream->errmsg = substream.errmsg;
-#endif
-        return false;
-    }
-    
-    size = substream.bytes_written;
-    
-    if (!pb_encode_varint(stream, (pb_uint64_t)size))
-        return false;
-    
-    if (stream->callback == NULL)
-        return pb_write(stream, NULL, size); /* Just sizing */
-    
-    if (stream->bytes_written + size > stream->max_size)
-        PB_RETURN_ERROR(stream, "stream full");
-        
-    /* Use a substream to verify that a callback doesn't write more than
-     * what it did the first time. */
-    substream.callback = stream->callback;
-    substream.state = stream->state;
-    substream.max_size = size;
-    substream.bytes_written = 0;
-#ifndef PB_NO_ERRMSG
-    substream.errmsg = NULL;
-#endif
-    
-    status = pb_encode(&substream, fields, src_struct);
-    
-    stream->bytes_written += substream.bytes_written;
-    stream->state = substream.state;
-#ifndef PB_NO_ERRMSG
-    stream->errmsg = substream.errmsg;
-#endif
-    
-    if (substream.bytes_written != size)
-        PB_RETURN_ERROR(stream, "submsg size changed");
-    
-    return status;
-}
-
-/* Field encoders */
-
-static bool checkreturn pb_enc_bool(pb_ostream_t *stream, const pb_field_iter_t *field)
-{
-    uint32_t value = safe_read_bool(field->pData) ? 1 : 0;
-    PB_UNUSED(field);
-    return pb_encode_varint(stream, value);
-}
-
-static bool checkreturn pb_enc_varint(pb_ostream_t *stream, const pb_field_iter_t *field)
-{
-    if (PB_LTYPE(field->type) == PB_LTYPE_UVARINT)
-    {
-        /* Perform unsigned integer extension */
-        pb_uint64_t value = 0;
-
-        if (field->data_size == sizeof(uint_least8_t))
-            value = *(const uint_least8_t*)field->pData;
-        else if (field->data_size == sizeof(uint_least16_t))
-            value = *(const uint_least16_t*)field->pData;
-        else if (field->data_size == sizeof(uint32_t))
-            value = *(const uint32_t*)field->pData;
-        else if (field->data_size == sizeof(pb_uint64_t))
-            value = *(const pb_uint64_t*)field->pData;
-        else
-            PB_RETURN_ERROR(stream, "invalid data_size");
-
-        return pb_encode_varint(stream, value);
-    }
-    else
-    {
-        /* Perform signed integer extension */
-        pb_int64_t value = 0;
-
-        if (field->data_size == sizeof(int_least8_t))
-            value = *(const int_least8_t*)field->pData;
-        else if (field->data_size == sizeof(int_least16_t))
-            value = *(const int_least16_t*)field->pData;
-        else if (field->data_size == sizeof(int32_t))
-            value = *(const int32_t*)field->pData;
-        else if (field->data_size == sizeof(pb_int64_t))
-            value = *(const pb_int64_t*)field->pData;
-        else
-            PB_RETURN_ERROR(stream, "invalid data_size");
-
-        if (PB_LTYPE(field->type) == PB_LTYPE_SVARINT)
-            return pb_encode_svarint(stream, value);
-#ifdef PB_WITHOUT_64BIT
-        else if (value < 0)
-            return pb_encode_varint_32(stream, (uint32_t)value, (uint32_t)-1);
-#endif
-        else
-            return pb_encode_varint(stream, (pb_uint64_t)value);
-
-    }
-}
-
-static bool checkreturn pb_enc_fixed(pb_ostream_t *stream, const pb_field_iter_t *field)
-{
-#ifdef PB_CONVERT_DOUBLE_FLOAT
-    if (field->data_size == sizeof(float) && PB_LTYPE(field->type) == PB_LTYPE_FIXED64)
-    {
-        return pb_encode_float_as_double(stream, *(float*)field->pData);
-    }
-#endif
-
-    if (field->data_size == sizeof(uint32_t))
-    {
-        return pb_encode_fixed32(stream, field->pData);
-    }
-#ifndef PB_WITHOUT_64BIT
-    else if (field->data_size == sizeof(uint64_t))
-    {
-        return pb_encode_fixed64(stream, field->pData);
-    }
-#endif
-    else
-    {
-        PB_RETURN_ERROR(stream, "invalid data_size");
-    }
-}
-
-static bool checkreturn pb_enc_bytes(pb_ostream_t *stream, const pb_field_iter_t *field)
-{
-    const pb_bytes_array_t *bytes = NULL;
-
-    bytes = (const pb_bytes_array_t*)field->pData;
-    
-    if (bytes == NULL)
-    {
-        /* Treat null pointer as an empty bytes field */
-        return pb_encode_string(stream, NULL, 0);
-    }
-    
-    if (PB_ATYPE(field->type) == PB_ATYPE_STATIC &&
-        PB_BYTES_ARRAY_T_ALLOCSIZE(bytes->size) > field->data_size)
-    {
-        PB_RETURN_ERROR(stream, "bytes size exceeded");
-    }
-    
-    return pb_encode_string(stream, bytes->bytes, (size_t)bytes->size);
-}
-
-static bool checkreturn pb_enc_string(pb_ostream_t *stream, const pb_field_iter_t *field)
-{
-    size_t size = 0;
-    size_t max_size = (size_t)field->data_size;
-    const char *str = (const char*)field->pData;
-    
-    if (PB_ATYPE(field->type) == PB_ATYPE_POINTER)
-    {
-        max_size = (size_t)-1;
-    }
-    else
-    {
-        /* pb_dec_string() assumes string fields end with a null
-         * terminator when the type isn't PB_ATYPE_POINTER, so we
-         * shouldn't allow more than max-1 bytes to be written to
-         * allow space for the null terminator.
-         */
-        if (max_size == 0)
-            PB_RETURN_ERROR(stream, "zero-length string");
-
-        max_size -= 1;
-    }
-
-
-    if (str == NULL)
-    {
-        size = 0; /* Treat null pointer as an empty string */
-    }
-    else
-    {
-        const char *p = str;
-
-        /* strnlen() is not always available, so just use a loop */
-        while (size < max_size && *p != '\0')
-        {
-            size++;
-            p++;
-        }
-
-        if (*p != '\0')
-        {
-            PB_RETURN_ERROR(stream, "unterminated string");
-        }
-    }
-
-#ifdef PB_VALIDATE_UTF8
-    if (!pb_validate_utf8(str))
-        PB_RETURN_ERROR(stream, "invalid utf8");
-#endif
-
-    return pb_encode_string(stream, (const pb_byte_t*)str, size);
-}
-
-static bool checkreturn pb_enc_submessage(pb_ostream_t *stream, const pb_field_iter_t *field)
-{
-    if (field->submsg_desc == NULL)
-        PB_RETURN_ERROR(stream, "invalid field descriptor");
-
-    if (PB_LTYPE(field->type) == PB_LTYPE_SUBMSG_W_CB && field->pSize != NULL)
-    {
-        /* Message callback is stored right before pSize. */
-        pb_callback_t *callback = (pb_callback_t*)field->pSize - 1;
-        if (callback->funcs.encode)
-        {
-            if (!callback->funcs.encode(stream, field, &callback->arg))
-                return false;
-        }
-    }
-    
-    return pb_encode_submessage(stream, field->submsg_desc, field->pData);
-}
-
-static bool checkreturn pb_enc_fixed_length_bytes(pb_ostream_t *stream, const pb_field_iter_t *field)
-{
-    return pb_encode_string(stream, (const pb_byte_t*)field->pData, (size_t)field->data_size);
-}
-
-#ifdef PB_CONVERT_DOUBLE_FLOAT
-bool pb_encode_float_as_double(pb_ostream_t *stream, float value)
-{
-    union { float f; uint32_t i; } in;
-    uint_least8_t sign;
-    int exponent;
-    uint64_t mantissa;
-
-    in.f = value;
-
-    /* Decompose input value */
-    sign = (uint_least8_t)((in.i >> 31) & 1);
-    exponent = (int)((in.i >> 23) & 0xFF) - 127;
-    mantissa = in.i & 0x7FFFFF;
-
-    if (exponent == 128)
-    {
-        /* Special value (NaN etc.) */
-        exponent = 1024;
-    }
-    else if (exponent == -127)
-    {
-        if (!mantissa)
-        {
-            /* Zero */
-            exponent = -1023;
-        }
-        else
-        {
-            /* Denormalized */
-            mantissa <<= 1;
-            while (!(mantissa & 0x800000))
-            {
-                mantissa <<= 1;
-                exponent--;
-            }
-            mantissa &= 0x7FFFFF;
-        }
-    }
-
-    /* Combine fields */
-    mantissa <<= 29;
-    mantissa |= (uint64_t)(exponent + 1023) << 52;
-    mantissa |= (uint64_t)sign << 63;
-
-    return pb_encode_fixed64(stream, &mantissa);
-}
-#endif
diff --git a/src/pb_encode.h b/src/pb_encode.h
deleted file mode 100644
index 88e246a..0000000
--- a/src/pb_encode.h
+++ /dev/null
@@ -1,185 +0,0 @@
-/* pb_encode.h: Functions to encode protocol buffers. Depends on pb_encode.c.
- * The main function is pb_encode. You also need an output stream, and the
- * field descriptions created by nanopb_generator.py.
- */
-
-#ifndef PB_ENCODE_H_INCLUDED
-#define PB_ENCODE_H_INCLUDED
-
-#include "pb.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* Structure for defining custom output streams. You will need to provide
- * a callback function to write the bytes to your storage, which can be
- * for example a file or a network socket.
- *
- * The callback must conform to these rules:
- *
- * 1) Return false on IO errors. This will cause encoding to abort.
- * 2) You can use state to store your own data (e.g. buffer pointer).
- * 3) pb_write will update bytes_written after your callback runs.
- * 4) Substreams will modify max_size and bytes_written. Don't use them
- *    to calculate any pointers.
- */
-struct pb_ostream_s
-{
-#ifdef PB_BUFFER_ONLY
-    /* Callback pointer is not used in buffer-only configuration.
-     * Having an int pointer here allows binary compatibility but
-     * gives an error if someone tries to assign callback function.
-     * Also, NULL pointer marks a 'sizing stream' that does not
-     * write anything.
-     */
-    int *callback;
-#else
-    bool (*callback)(pb_ostream_t *stream, const pb_byte_t *buf, size_t count);
-#endif
-    void *state;          /* Free field for use by callback implementation. */
-    size_t max_size;      /* Limit number of output bytes written (or use SIZE_MAX). */
-    size_t bytes_written; /* Number of bytes written so far. */
-    
-#ifndef PB_NO_ERRMSG
-    const char *errmsg;
-#endif
-};
-
-/***************************
- * Main encoding functions *
- ***************************/
-
-/* Encode a single protocol buffers message from C structure into a stream.
- * Returns true on success, false on any failure.
- * The actual struct pointed to by src_struct must match the description in fields.
- * All required fields in the struct are assumed to have been filled in.
- *
- * Example usage:
- *    MyMessage msg = {};
- *    uint8_t buffer[64];
- *    pb_ostream_t stream;
- *
- *    msg.field1 = 42;
- *    stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
- *    pb_encode(&stream, MyMessage_fields, &msg);
- */
-bool pb_encode(pb_ostream_t *stream, const pb_msgdesc_t *fields, const void *src_struct);
-
-/* Extended version of pb_encode, with several options to control the
- * encoding process:
- *
- * PB_ENCODE_DELIMITED:      Prepend the length of message as a varint.
- *                           Corresponds to writeDelimitedTo() in Google's
- *                           protobuf API.
- *
- * PB_ENCODE_NULLTERMINATED: Append a null byte to the message for termination.
- *                           NOTE: This behaviour is not supported in most other
- *                           protobuf implementations, so PB_ENCODE_DELIMITED
- *                           is a better option for compatibility.
- */
-#define PB_ENCODE_DELIMITED       0x02U
-#define PB_ENCODE_NULLTERMINATED  0x04U
-bool pb_encode_ex(pb_ostream_t *stream, const pb_msgdesc_t *fields, const void *src_struct, unsigned int flags);
-
-/* Defines for backwards compatibility with code written before nanopb-0.4.0 */
-#define pb_encode_delimited(s,f,d) pb_encode_ex(s,f,d, PB_ENCODE_DELIMITED)
-#define pb_encode_nullterminated(s,f,d) pb_encode_ex(s,f,d, PB_ENCODE_NULLTERMINATED)
-
-/* Encode the message to get the size of the encoded data, but do not store
- * the data. */
-bool pb_get_encoded_size(size_t *size, const pb_msgdesc_t *fields, const void *src_struct);
-
-/**************************************
- * Functions for manipulating streams *
- **************************************/
-
-/* Create an output stream for writing into a memory buffer.
- * The number of bytes written can be found in stream.bytes_written after
- * encoding the message.
- *
- * Alternatively, you can use a custom stream that writes directly to e.g.
- * a file or a network socket.
- */
-pb_ostream_t pb_ostream_from_buffer(pb_byte_t *buf, size_t bufsize);
-
-/* Pseudo-stream for measuring the size of a message without actually storing
- * the encoded data.
- * 
- * Example usage:
- *    MyMessage msg = {};
- *    pb_ostream_t stream = PB_OSTREAM_SIZING;
- *    pb_encode(&stream, MyMessage_fields, &msg);
- *    printf("Message size is %d\n", stream.bytes_written);
- */
-#ifndef PB_NO_ERRMSG
-#define PB_OSTREAM_SIZING {0,0,0,0,0}
-#else
-#define PB_OSTREAM_SIZING {0,0,0,0}
-#endif
-
-/* Function to write into a pb_ostream_t stream. You can use this if you need
- * to append or prepend some custom headers to the message.
- */
-bool pb_write(pb_ostream_t *stream, const pb_byte_t *buf, size_t count);
-
-
-/************************************************
- * Helper functions for writing field callbacks *
- ************************************************/
-
-/* Encode field header based on type and field number defined in the field
- * structure. Call this from the callback before writing out field contents. */
-bool pb_encode_tag_for_field(pb_ostream_t *stream, const pb_field_iter_t *field);
-
-/* Encode field header by manually specifing wire type. You need to use this
- * if you want to write out packed arrays from a callback field. */
-bool pb_encode_tag(pb_ostream_t *stream, pb_wire_type_t wiretype, uint32_t field_number);
-
-/* Encode an integer in the varint format.
- * This works for bool, enum, int32, int64, uint32 and uint64 field types. */
-#ifndef PB_WITHOUT_64BIT
-bool pb_encode_varint(pb_ostream_t *stream, uint64_t value);
-#else
-bool pb_encode_varint(pb_ostream_t *stream, uint32_t value);
-#endif
-
-/* Encode an integer in the zig-zagged svarint format.
- * This works for sint32 and sint64. */
-#ifndef PB_WITHOUT_64BIT
-bool pb_encode_svarint(pb_ostream_t *stream, int64_t value);
-#else
-bool pb_encode_svarint(pb_ostream_t *stream, int32_t value);
-#endif
-
-/* Encode a string or bytes type field. For strings, pass strlen(s) as size. */
-bool pb_encode_string(pb_ostream_t *stream, const pb_byte_t *buffer, size_t size);
-
-/* Encode a fixed32, sfixed32 or float value.
- * You need to pass a pointer to a 4-byte wide C variable. */
-bool pb_encode_fixed32(pb_ostream_t *stream, const void *value);
-
-#ifndef PB_WITHOUT_64BIT
-/* Encode a fixed64, sfixed64 or double value.
- * You need to pass a pointer to a 8-byte wide C variable. */
-bool pb_encode_fixed64(pb_ostream_t *stream, const void *value);
-#endif
-
-#ifdef PB_CONVERT_DOUBLE_FLOAT
-/* Encode a float value so that it appears like a double in the encoded
- * message. */
-bool pb_encode_float_as_double(pb_ostream_t *stream, float value);
-#endif
-
-/* Encode a submessage field.
- * You need to pass the pb_field_t array and pointer to struct, just like
- * with pb_encode(). This internally encodes the submessage twice, first to
- * calculate message size and then to actually write it out.
- */
-bool pb_encode_submessage(pb_ostream_t *stream, const pb_msgdesc_t *fields, const void *src_struct);
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif
diff --git a/src/slave-cmd.c b/src/slave-cmd.c
new file mode 100644
index 0000000..c8425fb
--- /dev/null
+++ b/src/slave-cmd.c
@@ -0,0 +1,538 @@
+/**
+******************************************************************************
+* @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 0x09<<1
+#define BUS_DEVICE_LIMIT 128
+
+/* Macro to toggle between master and slave firmware */
+
+/* Private globals */
+I2C_HandleTypeDef hi2c1;
+UART_HandleTypeDef huart1;
+uint8_t cmd_str[128];
+uint8_t str_ptr = 0;
+pb_byte_t str_buf[18];
+
+/* 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);
+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 encode_cmd_string_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
+bool decode_cmd_string_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
+bool handshake(void);
+/**
+ * @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();
+
+    uint8_t reset_string[] = "\r\n\n==========COMMAND SLAVE RESET=========\r\n\n";
+    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
+
+
+    while (handshake() == false);
+    uint8_t debug_buf[128] = {0};
+    HAL_Delay(1000);
+    while (1) {
+	uint8_t SOR_buf[2] = {0};
+	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, 2, 1000) != 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);
+	    continue;
+	}
+	else {
+	    sprintf((char*)debug_buf, "Got SOR: %d \r\n", SOR_buf[0]);
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);
+	}
+
+	if (SOR_buf[0] == 1) {
+
+	    uint8_t cmd_buf[128];
+	    size_t cmd_enc_size;
+	    cmd_str[0] = 0x1; cmd_str[1] = 0x2; cmd_str[2] = 0x3; cmd_str[3] = 0x6;
+	    command cmd = command_init_zero;
+	    cmd.dest_module_id = 0x4;
+	    cmd.cmd_str.funcs.encode = encode_cmd_string_callback;
+	    pb_ostream_t cmd_ostream = pb_ostream_from_buffer(cmd_buf, sizeof(cmd_buf));
+
+	    
+	    if (!pb_encode(&cmd_ostream, command_fields, &cmd)) {
+		sprintf((char*)debug_buf, "Command encoding error\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+		continue;
+	    }
+	    cmd_enc_size = cmd_ostream.bytes_written;
+	    
+	    uint8_t doc_buf[4] = {cmd.dest_module_id, 0x1, 0x0, cmd_enc_size};
+	    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);
+		continue;
+	    }
+	    else {
+		sprintf((char*)debug_buf, "Sent command DOC\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+	    }
+
+	    uint8_t CTS_buf[2] = {0x0, 0x0};
+	    if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)CTS_buf, 2, 1000) != 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);
+		continue;
+	    }
+	    else {
+		sprintf((char*)debug_buf, "Got CTS: %d, data size: %d\r\n", CTS_buf[0], cmd_enc_size);
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+	    }
+
+	    if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)cmd_buf,
+				       cmd_enc_size, 1000) != 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);
+		continue;
+	    }
+	    else {
+		sprintf((char*)debug_buf, "Sent command\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+		continue;
+	    }
+	}
+	else if (SOR_buf[0] == 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);
+
+	    s2m_MDR_response MDR_message;
+	    s2m_data data_message;
+	    data_idx = 0;
+	    data_message.datapoints.funcs.decode = decode_data_callback;
+	    data_message.datapoints.arg = (void*)datapoints;
+	    
+	    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);
+	    if (!pb_decode(&data_istream, s2m_data_fields, &data_message)){
+		sprintf((char*)debug_buf, "Data decode error\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+	    }
+	    else {
+		sprintf((char*)debug_buf, "Got data from %ld\r\n\tEntity: %ld, data : %f, unit: %ld\r\n", MDR_message.module_id, datapoints[0].entity_id, datapoints[0].data, datapoints[0].unit_id);
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+	    }
+	}
+	else if (SOR_buf[0] == 3) {
+	    uint8_t CTS_buf[] = {0x0, 0x1}, len_buf[4], *cmd_buf;
+	    
+	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
+	    HAL_I2C_Slave_Receive(&hi2c1, len_buf, 4, 10000);
+	    uint8_t cmd_len = len_buf[1];
+	    cmd_buf = malloc(cmd_len*sizeof(uint8_t));
+	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
+	    HAL_I2C_Slave_Receive(&hi2c1, cmd_buf, cmd_len, 10000);
+
+	    command cmd_decode=command_init_zero;
+	    pb_istream_t cmd_istream = pb_istream_from_buffer(cmd_buf, cmd_len);
+	    cmd_decode.cmd_str.funcs.decode=decode_cmd_string_callback;
+	    pb_decode(&cmd_istream, command_fields, &cmd_decode);
+	    sprintf((char*)debug_buf, "Got cmd\r\n");
+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	    memset(debug_buf, 0, 128);	    
+	}
+	else {
+	    uint8_t doc_buf[4] = {0x0, 0x0, 0x0, 0x0};
+	    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);
+		continue;
+	    }
+	    else {
+		sprintf((char*)debug_buf, "Sent idle DOC\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+		continue;
+	    }
+	}
+    }   
+}
+
+
+bool handshake(void)
+{
+    uint8_t MDR_buf[128], debug_buf[128]={0}, term[]="\r\n";
+    size_t MDR_enc_size;
+    s2m_MDR_response res;
+    res.MDR_version = 0.1;
+    res.module_id = 0x5;
+    res.module_class = 1;
+    res.entity_id=1;
+
+    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
+	return false;
+    }
+    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[2] = {MDR_enc_size, 0xFF};
+    uint8_t MDR_req_buf[2] = {0x0, 0x0};
+	
+    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
+	return false;
+    }
+		
+#ifdef TESTING_ENABLE
+    sprintf((char*)debug_buf, "Got requested record type: %d\r\n", MDR_req_buf[1]);
+    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+    memset(debug_buf, 0, 128);
+#endif
+
+    if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_ACK_buf, 2, 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
+	return false;
+    }
+
+    uint8_t MDR_CTS_buf[2] = {0x0, 0x0};
+    
+    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
+	return false;
+    }	
+
+    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
+	return false;
+    }
+#ifdef DEBUG_ENABLE
+    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
+
+    /* TODO Final ACK from master */
+    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<5; x++) {
+	    _subscriptions subs;
+	    subs.module_id = x+1;
+	    subs.has_module_id=true;
+	    subs.has_entity_id=false;
+	    subs.has_module_class=false;
+	    subs.has_i2c_address=false;
+	    if(!pb_encode_tag_for_field(ostream, field)) {
+		return false;
+	    }
+	    if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)) {
+		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 encode_cmd_string_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
+{
+    if (ostream != NULL && field->tag == command_cmd_str_tag) {
+	if (!pb_encode_tag_for_field(ostream, field))
+	    return false;
+	return pb_encode_string(ostream, cmd_str, strlen((char*)cmd_str));
+    }
+    return true;
+}
+
+bool decode_cmd_string_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
+{
+    while (istream->bytes_left) {
+	if (!pb_read(istream, &str_buf[str_ptr++], 1))
+	    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;
+    /* hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_ENABLE; */
+    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 = USART2;
+    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-slave.c b/src/slave.c
index e2364e3..f5096f3 100644
--- a/src/main-slave.c
+++ b/src/slave.c
@@ -32,7 +32,7 @@
 #define SET_BIT_FROM_IDX(a, b) a[b>>5]|=(1<<(b%32))
 #define COUNTOF(__BUFFER__)   (sizeof(__BUFFER__) / sizeof(*(__BUFFER__)))
 
-#define I2C_ADDRESS 0x05<<1
+#define I2C_ADDRESS 0x07<<1
 #define BUS_DEVICE_LIMIT 128
 
 /* Macro to toggle between master and slave firmware */
@@ -40,6 +40,10 @@
 /* Private globals */
 I2C_HandleTypeDef hi2c1;
 UART_HandleTypeDef huart1;
+uint8_t cmd_str[128];
+uint8_t str_ptr = 0;
+pb_byte_t str_buf[18];
+uint8_t data_idx = 0;
 
 /* Function prototypes */
 void SystemClock_Config(void);
@@ -51,6 +55,8 @@ bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *fiel
 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 encode_cmd_string_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
+bool decode_cmd_string_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
 bool handshake(void);
 /**
  * @brief  The application entry point.
@@ -76,11 +82,10 @@ int main(void)
 
 
     while (handshake() == false);
-    uint8_t SOR_buf[2] = {0}, debug_buf[128];
+    uint8_t SOR_buf[2] = {0}, debug_buf[128] = {0};
     HAL_Delay(1000);
     while (1) {
-
-	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, 2, 500) != HAL_OK) {
+	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, 2, 1000) != 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);
@@ -146,7 +151,7 @@ int main(void)
 	else if (SOR_buf[0] == 2) {
 	    uint8_t CTS_buf[] = {0x0, 0x1};
 	    uint8_t len_buf[4], *MDR_buf, *data_buf;
-	    /* _datapoint datapoints[16]; */
+	    _datapoint datapoints[16];
 	    
 	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
 	    sprintf((char*)debug_buf, "Sent CTS\r\n");
@@ -167,30 +172,61 @@ int main(void)
 	    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;
+	    s2m_data data_message;
+	    data_idx = 0;
+	    data_message.datapoints.funcs.decode = decode_data_callback;
+	    data_message.datapoints.arg = (void*)datapoints;
+	    
 	    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);
+	    if (!pb_decode(&data_istream, s2m_data_fields, &data_message)){
+		sprintf((char*)debug_buf, "Data decode error\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+	    }
+	    else {
+		sprintf((char*)debug_buf, "Got data from %ld\r\n\tEntity: %ld, data : %f, unit: %ld\r\n", MDR_message.module_id, datapoints[0].entity_id, datapoints[0].data, datapoints[0].unit_id);
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+	    }
+	}
+	else if (SOR_buf[0] == 3) {
+	    uint8_t CTS_buf[] = {0x0, 0x1}, len_buf[4], *cmd_buf;
+	    
+	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
+	    HAL_I2C_Slave_Receive(&hi2c1, len_buf, 4, 10000);
+	    uint8_t cmd_len = len_buf[1];
+	    cmd_buf = malloc(cmd_len*sizeof(uint8_t));
+	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
+	    HAL_I2C_Slave_Receive(&hi2c1, cmd_buf, cmd_len, 10000);
+
+	    command cmd_decode=command_init_zero;
+	    pb_istream_t cmd_istream = pb_istream_from_buffer(cmd_buf, cmd_len);
+	    cmd_decode.cmd_str.funcs.decode=decode_cmd_string_callback;
+	    if (pb_decode(&cmd_istream, command_fields, &cmd_decode))
+		sprintf((char*)debug_buf, "Got cmd, decode successful\r\n");
+	    else
+		sprintf((char*)debug_buf, "Got cmd, decode unsuccessful\r\n");
 
-	    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);
 	}
+
     }   
 }
 
 
 bool handshake(void)
 {
-    uint8_t MDR_buf[128], debug_buf[128], term[]="\r\n";
+    uint8_t MDR_buf[128], debug_buf[128] = {0}, term[]="\r\n";
     size_t MDR_enc_size;
     s2m_MDR_response res;
     res.MDR_version = 0.1;
-    res.module_id = 1;
+    res.module_id = 0x4;
     res.module_class = 2;
     res.entity_id=1;
 
@@ -280,19 +316,17 @@ bool handshake(void)
 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<5; x++) {
-	    _subscriptions subs;
-	    subs.module_id = x+1;
-	    subs.has_module_id=true;
-	    subs.has_entity_id=false;
-	    subs.has_module_class=false;
-	    subs.has_i2c_address=false;
-	    if(!pb_encode_tag_for_field(ostream, field)) {
-		return false;
-	    }
-	    if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)) {
-		return false;
-	    }
+	_subscriptions subs;
+	subs.module_id = 0x4;
+	subs.has_module_id=true;
+	subs.has_entity_id=false;
+	subs.has_module_class=false;
+	subs.has_i2c_address=false;
+	if(!pb_encode_tag_for_field(ostream, field)) {
+	    return false;
+	}
+	if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)) {
+	    return false;
 	}
     }
     else{
@@ -308,6 +342,8 @@ bool encode_datapoint_callback(pb_ostream_t *ostream, const pb_field_t *field, v
 	    _datapoint datapoint = _datapoint_init_zero;
 	    datapoint.entity_id  = 1;
 	    datapoint.data = 20.70+((float)i/100);
+	    datapoint.unit_id = 1;
+	    datapoint.has_unit_id = true;
 	    if (!pb_encode_tag_for_field(ostream, field))
 		return false;
 	    if (!pb_encode_submessage(ostream, _datapoint_fields, &datapoint))
@@ -319,6 +355,55 @@ bool encode_datapoint_callback(pb_ostream_t *ostream, const pb_field_t *field, v
     return true;
 }
 
+bool encode_cmd_string_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
+{
+    if (ostream != NULL && field->tag == command_cmd_str_tag) {
+	if (!pb_encode_tag_for_field(ostream, field))
+	    return false;
+	return pb_encode_string(ostream, cmd_str, strlen((char*)cmd_str));
+    }
+    return true;
+}
+
+bool decode_cmd_string_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
+{
+    while (istream->bytes_left) {
+	if (!pb_read(istream, &str_buf[str_ptr++], 1))
+	    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
@@ -388,7 +473,7 @@ static void MX_I2C1_Init(void)
  */
 static void MX_USART1_UART_Init(void)
 {
-    huart1.Instance = USART1;
+    huart1.Instance = USART2;
     huart1.Init.BaudRate = 9600;
     huart1.Init.WordLength = UART_WORDLENGTH_8B;
     huart1.Init.StopBits = UART_STOPBITS_1;
diff --git a/src/stm32f4xx_hal_conf.h b/src/stm32f4xx_hal_conf.h
deleted file mode 100644
index e7e342a..0000000
--- a/src/stm32f4xx_hal_conf.h
+++ /dev/null
@@ -1,443 +0,0 @@
-/**

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

-  * @file    stm32f4xx_hal_conf_template.h

-  * @author  MCD Application Team

-  * @brief   HAL configuration template file. 

-  *          This file should be copied to the application folder and renamed

-  *          to stm32f4xx_hal_conf.h.

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

-  * @attention

-  *

-  * <h2><center>&copy; Copyright (c) 2017 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

-  *

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

-  */ 

-

-/* Define to prevent recursive inclusion -------------------------------------*/

-#ifndef __STM32F4xx_HAL_CONF_H

-#define __STM32F4xx_HAL_CONF_H

-

-#ifdef __cplusplus

- extern "C" {

-#endif

-

-/* Exported types ------------------------------------------------------------*/

-/* Exported constants --------------------------------------------------------*/

-

-/* ########################## Module Selection ############################## */

-/**

-  * @brief This is the list of modules to be used in the HAL driver 

-  */

-#define HAL_MODULE_ENABLED  

-

-  /* #define HAL_ADC_MODULE_ENABLED   */

-/* #define HAL_CRYP_MODULE_ENABLED   */

-/* #define HAL_CAN_MODULE_ENABLED   */

-/* #define HAL_CRC_MODULE_ENABLED   */

-/* #define HAL_CRYP_MODULE_ENABLED   */

-/* #define HAL_DAC_MODULE_ENABLED   */

-/* #define HAL_DCMI_MODULE_ENABLED   */

-/* #define HAL_DMA2D_MODULE_ENABLED   */

-/* #define HAL_ETH_MODULE_ENABLED   */

-/* #define HAL_NAND_MODULE_ENABLED   */

-/* #define HAL_NOR_MODULE_ENABLED   */

-/* #define HAL_PCCARD_MODULE_ENABLED   */

-/* #define HAL_SRAM_MODULE_ENABLED   */

-/* #define HAL_SDRAM_MODULE_ENABLED   */

-/* #define HAL_HASH_MODULE_ENABLED   */

-#define HAL_I2C_MODULE_ENABLED

-/* #define HAL_I2S_MODULE_ENABLED   */

-/* #define HAL_IWDG_MODULE_ENABLED   */

-/* #define HAL_LTDC_MODULE_ENABLED   */

-/* #define HAL_RNG_MODULE_ENABLED   */

-/* #define HAL_RTC_MODULE_ENABLED   */

-/* #define HAL_SAI_MODULE_ENABLED   */

-/* #define HAL_SD_MODULE_ENABLED   */

-/* #define HAL_MMC_MODULE_ENABLED   */

-/* #define HAL_SPI_MODULE_ENABLED   */

-/* #define HAL_TIM_MODULE_ENABLED   */

-#define HAL_UART_MODULE_ENABLED

-/* #define HAL_USART_MODULE_ENABLED   */

-/* #define HAL_IRDA_MODULE_ENABLED   */

-/* #define HAL_SMARTCARD_MODULE_ENABLED   */

-/* #define HAL_SMBUS_MODULE_ENABLED   */

-/* #define HAL_WWDG_MODULE_ENABLED   */

-/* #define HAL_PCD_MODULE_ENABLED   */

-/* #define HAL_HCD_MODULE_ENABLED   */

-/* #define HAL_DSI_MODULE_ENABLED   */

-/* #define HAL_QSPI_MODULE_ENABLED   */

-/* #define HAL_QSPI_MODULE_ENABLED   */

-/* #define HAL_CEC_MODULE_ENABLED   */

-/* #define HAL_FMPI2C_MODULE_ENABLED   */

-/* #define HAL_SPDIFRX_MODULE_ENABLED   */

-/* #define HAL_DFSDM_MODULE_ENABLED   */

-/* #define HAL_LPTIM_MODULE_ENABLED   */

-#define HAL_GPIO_MODULE_ENABLED

-#define HAL_EXTI_MODULE_ENABLED

-#define HAL_DMA_MODULE_ENABLED

-#define HAL_RCC_MODULE_ENABLED

-#define HAL_FLASH_MODULE_ENABLED

-#define HAL_PWR_MODULE_ENABLED

-#define HAL_CORTEX_MODULE_ENABLED

-

-/* ########################## HSE/HSI Values adaptation ##################### */

-/**

-  * @brief Adjust the value of External High Speed oscillator (HSE) used in your application.

-  *        This value is used by the RCC HAL module to compute the system frequency

-  *        (when HSE is used as system clock source, directly or through the PLL).  

-  */

-#if !defined  (HSE_VALUE) 

-  #define HSE_VALUE    ((uint32_t)25000000U) /*!< Value of the External oscillator in Hz */

-#endif /* HSE_VALUE */

-

-#if !defined  (HSE_STARTUP_TIMEOUT)

-  #define HSE_STARTUP_TIMEOUT    ((uint32_t)100U)   /*!< Time out for HSE start up, in ms */

-#endif /* HSE_STARTUP_TIMEOUT */

-

-/**

-  * @brief Internal High Speed oscillator (HSI) value.

-  *        This value is used by the RCC HAL module to compute the system frequency

-  *        (when HSI is used as system clock source, directly or through the PLL). 

-  */

-#if !defined  (HSI_VALUE)

-  #define HSI_VALUE    ((uint32_t)16000000U) /*!< Value of the Internal oscillator in Hz*/

-#endif /* HSI_VALUE */

-

-/**

-  * @brief Internal Low Speed oscillator (LSI) value.

-  */

-#if !defined  (LSI_VALUE) 

- #define LSI_VALUE  ((uint32_t)32000U)       /*!< LSI Typical Value in Hz*/

-#endif /* LSI_VALUE */                      /*!< Value of the Internal Low Speed oscillator in Hz

-                                             The real value may vary depending on the variations

-                                             in voltage and temperature.*/

-/**

-  * @brief External Low Speed oscillator (LSE) value.

-  */

-#if !defined  (LSE_VALUE)

- #define LSE_VALUE  ((uint32_t)32768U)    /*!< Value of the External Low Speed oscillator in Hz */

-#endif /* LSE_VALUE */

-

-#if !defined  (LSE_STARTUP_TIMEOUT)

-  #define LSE_STARTUP_TIMEOUT    ((uint32_t)5000U)   /*!< Time out for LSE start up, in ms */

-#endif /* LSE_STARTUP_TIMEOUT */

-

-/**

-  * @brief External clock source for I2S peripheral

-  *        This value is used by the I2S HAL module to compute the I2S clock source 

-  *        frequency, this source is inserted directly through I2S_CKIN pad. 

-  */

-#if !defined  (EXTERNAL_CLOCK_VALUE)

-  #define EXTERNAL_CLOCK_VALUE    ((uint32_t)12288000U) /*!< Value of the External audio frequency in Hz*/

-#endif /* EXTERNAL_CLOCK_VALUE */

-

-/* Tip: To avoid modifying this file each time you need to use different HSE,

-   ===  you can define the HSE value in your toolchain compiler preprocessor. */

-

-/* ########################### System Configuration ######################### */

-/**

-  * @brief This is the HAL system configuration section

-  */

-#define  VDD_VALUE		      ((uint32_t)3300U) /*!< Value of VDD in mv */           

-#define  TICK_INT_PRIORITY            ((uint32_t)0U)   /*!< tick interrupt priority */            

-#define  USE_RTOS                     0U     

-#define  PREFETCH_ENABLE              1U

-#define  INSTRUCTION_CACHE_ENABLE     1U

-#define  DATA_CACHE_ENABLE            1U

-

-/* ########################## Assert Selection ############################## */

-/**

-  * @brief Uncomment the line below to expanse the "assert_param" macro in the 

-  *        HAL drivers code

-  */

-/* #define USE_FULL_ASSERT    1U */

-

-/* ################## Ethernet peripheral configuration ##################### */

-

-/* Section 1 : Ethernet peripheral configuration */

-

-/* MAC ADDRESS: MAC_ADDR0:MAC_ADDR1:MAC_ADDR2:MAC_ADDR3:MAC_ADDR4:MAC_ADDR5 */

-#define MAC_ADDR0   2U

-#define MAC_ADDR1   0U

-#define MAC_ADDR2   0U

-#define MAC_ADDR3   0U

-#define MAC_ADDR4   0U

-#define MAC_ADDR5   0U

-

-/* Definition of the Ethernet driver buffers size and count */   

-#define ETH_RX_BUF_SIZE                ETH_MAX_PACKET_SIZE /* buffer size for receive               */

-#define ETH_TX_BUF_SIZE                ETH_MAX_PACKET_SIZE /* buffer size for transmit              */

-#define ETH_RXBUFNB                    ((uint32_t)4U)       /* 4 Rx buffers of size ETH_RX_BUF_SIZE  */

-#define ETH_TXBUFNB                    ((uint32_t)4U)       /* 4 Tx buffers of size ETH_TX_BUF_SIZE  */

-

-/* Section 2: PHY configuration section */

-

-/* DP83848_PHY_ADDRESS Address*/ 

-#define DP83848_PHY_ADDRESS           0x01U

-/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/ 

-#define PHY_RESET_DELAY                 ((uint32_t)0x000000FFU)

-/* PHY Configuration delay */

-#define PHY_CONFIG_DELAY                ((uint32_t)0x00000FFFU)

-

-#define PHY_READ_TO                     ((uint32_t)0x0000FFFFU)

-#define PHY_WRITE_TO                    ((uint32_t)0x0000FFFFU)

-

-/* Section 3: Common PHY Registers */

-

-#define PHY_BCR                         ((uint16_t)0x0000U)    /*!< Transceiver Basic Control Register   */

-#define PHY_BSR                         ((uint16_t)0x0001U)    /*!< Transceiver Basic Status Register    */

- 

-#define PHY_RESET                       ((uint16_t)0x8000U)  /*!< PHY Reset */

-#define PHY_LOOPBACK                    ((uint16_t)0x4000U)  /*!< Select loop-back mode */

-#define PHY_FULLDUPLEX_100M             ((uint16_t)0x2100U)  /*!< Set the full-duplex mode at 100 Mb/s */

-#define PHY_HALFDUPLEX_100M             ((uint16_t)0x2000U)  /*!< Set the half-duplex mode at 100 Mb/s */

-#define PHY_FULLDUPLEX_10M              ((uint16_t)0x0100U)  /*!< Set the full-duplex mode at 10 Mb/s  */

-#define PHY_HALFDUPLEX_10M              ((uint16_t)0x0000U)  /*!< Set the half-duplex mode at 10 Mb/s  */

-#define PHY_AUTONEGOTIATION             ((uint16_t)0x1000U)  /*!< Enable auto-negotiation function     */

-#define PHY_RESTART_AUTONEGOTIATION     ((uint16_t)0x0200U)  /*!< Restart auto-negotiation function    */

-#define PHY_POWERDOWN                   ((uint16_t)0x0800U)  /*!< Select the power down mode           */

-#define PHY_ISOLATE                     ((uint16_t)0x0400U)  /*!< Isolate PHY from MII                 */

-

-#define PHY_AUTONEGO_COMPLETE           ((uint16_t)0x0020U)  /*!< Auto-Negotiation process completed   */

-#define PHY_LINKED_STATUS               ((uint16_t)0x0004U)  /*!< Valid link established               */

-#define PHY_JABBER_DETECTION            ((uint16_t)0x0002U)  /*!< Jabber condition detected            */

-  

-/* Section 4: Extended PHY Registers */

-#define PHY_SR                          ((uint16_t)0x10U)    /*!< PHY status register Offset                      */

-

-#define PHY_SPEED_STATUS                ((uint16_t)0x0002U)  /*!< PHY Speed mask                                  */

-#define PHY_DUPLEX_STATUS               ((uint16_t)0x0004U)  /*!< PHY Duplex mask                                 */

-

-/* ################## SPI peripheral configuration ########################## */

-

-/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver

-* Activated: CRC code is present inside driver

-* Deactivated: CRC code cleaned from driver

-*/

-

-#define USE_SPI_CRC                     0U

-

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

-/**

-  * @brief Include module's header file 

-  */

-

-#ifdef HAL_RCC_MODULE_ENABLED

-  #include "stm32f4xx_hal_rcc.h"

-#endif /* HAL_RCC_MODULE_ENABLED */

-

-#ifdef HAL_EXTI_MODULE_ENABLED

-  #include "stm32f4xx_hal_exti.h"

-#endif /* HAL_EXTI_MODULE_ENABLED */

-

-#ifdef HAL_GPIO_MODULE_ENABLED

-  #include "stm32f4xx_hal_gpio.h"

-#endif /* HAL_GPIO_MODULE_ENABLED */

-

-#ifdef HAL_DMA_MODULE_ENABLED

-  #include "stm32f4xx_hal_dma.h"

-#endif /* HAL_DMA_MODULE_ENABLED */

-   

-#ifdef HAL_CORTEX_MODULE_ENABLED

-  #include "stm32f4xx_hal_cortex.h"

-#endif /* HAL_CORTEX_MODULE_ENABLED */

-

-#ifdef HAL_ADC_MODULE_ENABLED

-  #include "stm32f4xx_hal_adc.h"

-#endif /* HAL_ADC_MODULE_ENABLED */

-

-#ifdef HAL_CAN_MODULE_ENABLED

-  #include "stm32f4xx_hal_can.h"

-#endif /* HAL_CAN_MODULE_ENABLED */

-

-#ifdef HAL_CRC_MODULE_ENABLED

-  #include "stm32f4xx_hal_crc.h"

-#endif /* HAL_CRC_MODULE_ENABLED */

-

-#ifdef HAL_CRYP_MODULE_ENABLED

-  #include "stm32f4xx_hal_cryp.h" 

-#endif /* HAL_CRYP_MODULE_ENABLED */

-

-#ifdef HAL_SMBUS_MODULE_ENABLED

-#include "stm32f4xx_hal_smbus.h"

-#endif /* HAL_SMBUS_MODULE_ENABLED */

-

-#ifdef HAL_DMA2D_MODULE_ENABLED

-  #include "stm32f4xx_hal_dma2d.h"

-#endif /* HAL_DMA2D_MODULE_ENABLED */

-

-#ifdef HAL_DAC_MODULE_ENABLED

-  #include "stm32f4xx_hal_dac.h"

-#endif /* HAL_DAC_MODULE_ENABLED */

-

-#ifdef HAL_DCMI_MODULE_ENABLED

-  #include "stm32f4xx_hal_dcmi.h"

-#endif /* HAL_DCMI_MODULE_ENABLED */

-

-#ifdef HAL_ETH_MODULE_ENABLED

-  #include "stm32f4xx_hal_eth.h"

-#endif /* HAL_ETH_MODULE_ENABLED */

-

-#ifdef HAL_FLASH_MODULE_ENABLED

-  #include "stm32f4xx_hal_flash.h"

-#endif /* HAL_FLASH_MODULE_ENABLED */

- 

-#ifdef HAL_SRAM_MODULE_ENABLED

-  #include "stm32f4xx_hal_sram.h"

-#endif /* HAL_SRAM_MODULE_ENABLED */

-

-#ifdef HAL_NOR_MODULE_ENABLED

-  #include "stm32f4xx_hal_nor.h"

-#endif /* HAL_NOR_MODULE_ENABLED */

-

-#ifdef HAL_NAND_MODULE_ENABLED

-  #include "stm32f4xx_hal_nand.h"

-#endif /* HAL_NAND_MODULE_ENABLED */

-

-#ifdef HAL_PCCARD_MODULE_ENABLED

-  #include "stm32f4xx_hal_pccard.h"

-#endif /* HAL_PCCARD_MODULE_ENABLED */ 

-  

-#ifdef HAL_SDRAM_MODULE_ENABLED

-  #include "stm32f4xx_hal_sdram.h"

-#endif /* HAL_SDRAM_MODULE_ENABLED */      

-

-#ifdef HAL_HASH_MODULE_ENABLED

- #include "stm32f4xx_hal_hash.h"

-#endif /* HAL_HASH_MODULE_ENABLED */

-

-#ifdef HAL_I2C_MODULE_ENABLED

- #include "stm32f4xx_hal_i2c.h"

-#endif /* HAL_I2C_MODULE_ENABLED */

-

-#ifdef HAL_I2S_MODULE_ENABLED

- #include "stm32f4xx_hal_i2s.h"

-#endif /* HAL_I2S_MODULE_ENABLED */

-

-#ifdef HAL_IWDG_MODULE_ENABLED

- #include "stm32f4xx_hal_iwdg.h"

-#endif /* HAL_IWDG_MODULE_ENABLED */

-

-#ifdef HAL_LTDC_MODULE_ENABLED

- #include "stm32f4xx_hal_ltdc.h"

-#endif /* HAL_LTDC_MODULE_ENABLED */

-

-#ifdef HAL_PWR_MODULE_ENABLED

- #include "stm32f4xx_hal_pwr.h"

-#endif /* HAL_PWR_MODULE_ENABLED */

-

-#ifdef HAL_RNG_MODULE_ENABLED

- #include "stm32f4xx_hal_rng.h"

-#endif /* HAL_RNG_MODULE_ENABLED */

-

-#ifdef HAL_RTC_MODULE_ENABLED

- #include "stm32f4xx_hal_rtc.h"

-#endif /* HAL_RTC_MODULE_ENABLED */

-

-#ifdef HAL_SAI_MODULE_ENABLED

- #include "stm32f4xx_hal_sai.h"

-#endif /* HAL_SAI_MODULE_ENABLED */

-

-#ifdef HAL_SD_MODULE_ENABLED

- #include "stm32f4xx_hal_sd.h"

-#endif /* HAL_SD_MODULE_ENABLED */

-

-#ifdef HAL_MMC_MODULE_ENABLED

- #include "stm32f4xx_hal_mmc.h"

-#endif /* HAL_MMC_MODULE_ENABLED */

-

-#ifdef HAL_SPI_MODULE_ENABLED

- #include "stm32f4xx_hal_spi.h"

-#endif /* HAL_SPI_MODULE_ENABLED */

-

-#ifdef HAL_TIM_MODULE_ENABLED

- #include "stm32f4xx_hal_tim.h"

-#endif /* HAL_TIM_MODULE_ENABLED */

-

-#ifdef HAL_UART_MODULE_ENABLED

- #include "stm32f4xx_hal_uart.h"

-#endif /* HAL_UART_MODULE_ENABLED */

-

-#ifdef HAL_USART_MODULE_ENABLED

- #include "stm32f4xx_hal_usart.h"

-#endif /* HAL_USART_MODULE_ENABLED */

-

-#ifdef HAL_IRDA_MODULE_ENABLED

- #include "stm32f4xx_hal_irda.h"

-#endif /* HAL_IRDA_MODULE_ENABLED */

-

-#ifdef HAL_SMARTCARD_MODULE_ENABLED

- #include "stm32f4xx_hal_smartcard.h"

-#endif /* HAL_SMARTCARD_MODULE_ENABLED */

-

-#ifdef HAL_WWDG_MODULE_ENABLED

- #include "stm32f4xx_hal_wwdg.h"

-#endif /* HAL_WWDG_MODULE_ENABLED */

-

-#ifdef HAL_PCD_MODULE_ENABLED

- #include "stm32f4xx_hal_pcd.h"

-#endif /* HAL_PCD_MODULE_ENABLED */

-

-#ifdef HAL_HCD_MODULE_ENABLED

- #include "stm32f4xx_hal_hcd.h"

-#endif /* HAL_HCD_MODULE_ENABLED */

-   

-#ifdef HAL_DSI_MODULE_ENABLED

- #include "stm32f4xx_hal_dsi.h"

-#endif /* HAL_DSI_MODULE_ENABLED */

-

-#ifdef HAL_QSPI_MODULE_ENABLED

- #include "stm32f4xx_hal_qspi.h"

-#endif /* HAL_QSPI_MODULE_ENABLED */

-

-#ifdef HAL_CEC_MODULE_ENABLED

- #include "stm32f4xx_hal_cec.h"

-#endif /* HAL_CEC_MODULE_ENABLED */

-

-#ifdef HAL_FMPI2C_MODULE_ENABLED

- #include "stm32f4xx_hal_fmpi2c.h"

-#endif /* HAL_FMPI2C_MODULE_ENABLED */

-

-#ifdef HAL_SPDIFRX_MODULE_ENABLED

- #include "stm32f4xx_hal_spdifrx.h"

-#endif /* HAL_SPDIFRX_MODULE_ENABLED */

-

-#ifdef HAL_DFSDM_MODULE_ENABLED

- #include "stm32f4xx_hal_dfsdm.h"

-#endif /* HAL_DFSDM_MODULE_ENABLED */

-

-#ifdef HAL_LPTIM_MODULE_ENABLED

- #include "stm32f4xx_hal_lptim.h"

-#endif /* HAL_LPTIM_MODULE_ENABLED */

-   

-/* Exported macro ------------------------------------------------------------*/

-#ifdef  USE_FULL_ASSERT

-/**

-  * @brief  The assert_param macro is used for function's parameters check.

-  * @param  expr: If expr is false, it calls assert_failed function

-  *         which reports the name of the source file and the source

-  *         line number of the call that failed. 

-  *         If expr is true, it returns no value.

-  * @retval None

-  */

-  #define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__))

-/* Exported functions ------------------------------------------------------- */

-  void assert_failed(uint8_t* file, uint32_t line);

-#else

-  #define assert_param(expr) ((void)0U)

-#endif /* USE_FULL_ASSERT */    

-

-#ifdef __cplusplus

-}

-#endif

-

-#endif /* __STM32F4xx_HAL_CONF_H */

- 

-

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

diff --git a/src/stm32f4xx_hal_msp.c b/src/stm32f4xx_hal_msp.c
deleted file mode 100644
index d19bcec..0000000
--- a/src/stm32f4xx_hal_msp.c
+++ /dev/null
@@ -1,214 +0,0 @@
-/* USER CODE BEGIN Header */

-/**

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

-  * File Name          : stm32f4xx_hal_msp.c

-  * Description        : This file provides code for the MSP Initialization 

-  *                      and de-Initialization codes.

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

-  * @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"

-/* USER CODE BEGIN Includes */

-

-/* USER CODE END Includes */

-

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

-/* USER CODE BEGIN TD */

-

-/* USER CODE END TD */

-

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

-/* USER CODE BEGIN Define */

- 

-/* USER CODE END Define */

-

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

-/* USER CODE BEGIN Macro */

-

-/* USER CODE END Macro */

-

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

-/* USER CODE BEGIN PV */

-

-/* USER CODE END PV */

-

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

-/* USER CODE BEGIN PFP */

-

-/* USER CODE END PFP */

-

-/* External functions --------------------------------------------------------*/

-/* USER CODE BEGIN ExternalFunctions */

-

-/* USER CODE END ExternalFunctions */

-

-/* USER CODE BEGIN 0 */

-

-/* USER CODE END 0 */

-/**

-  * Initializes the Global MSP.

-  */

-void HAL_MspInit(void)

-{

-  /* USER CODE BEGIN MspInit 0 */

-

-  /* USER CODE END MspInit 0 */

-

-  __HAL_RCC_SYSCFG_CLK_ENABLE();

-  __HAL_RCC_PWR_CLK_ENABLE();

-

-  /* System interrupt init*/

-

-  /* USER CODE BEGIN MspInit 1 */

-

-  /* USER CODE END MspInit 1 */

-}

-

-/**

-* @brief I2C MSP Initialization

-* This function configures the hardware resources used in this example

-* @param hi2c: I2C handle pointer

-* @retval None

-*/

-void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c)

-{

-  GPIO_InitTypeDef GPIO_InitStruct = {0};

-  if(hi2c->Instance==I2C1)

-  {

-  /* USER CODE BEGIN I2C1_MspInit 0 */

-

-  /* USER CODE END I2C1_MspInit 0 */

-  

-    __HAL_RCC_GPIOB_CLK_ENABLE();

-    /**I2C1 GPIO Configuration    

-    PB6     ------> I2C1_SCL

-    PB7     ------> I2C1_SDA 

-    */

-    GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7;

-    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;

-    GPIO_InitStruct.Pull = GPIO_PULLUP;

-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;

-    GPIO_InitStruct.Alternate = GPIO_AF4_I2C1;

-    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

-

-    /* Peripheral clock enable */

-    __HAL_RCC_I2C1_CLK_ENABLE();

-  /* USER CODE BEGIN I2C1_MspInit 1 */

-

-  /* USER CODE END I2C1_MspInit 1 */

-  }

-

-}

-

-/**

-* @brief I2C MSP De-Initialization

-* This function freeze the hardware resources used in this example

-* @param hi2c: I2C handle pointer

-* @retval None

-*/

-void HAL_I2C_MspDeInit(I2C_HandleTypeDef* hi2c)

-{

-  if(hi2c->Instance==I2C1)

-  {

-  /* USER CODE BEGIN I2C1_MspDeInit 0 */

-

-  /* USER CODE END I2C1_MspDeInit 0 */

-    /* Peripheral clock disable */

-    __HAL_RCC_I2C1_CLK_DISABLE();

-  

-    /**I2C1 GPIO Configuration    

-    PB6     ------> I2C1_SCL

-    PB7     ------> I2C1_SDA 

-    */

-    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_6|GPIO_PIN_7);

-

-  /* USER CODE BEGIN I2C1_MspDeInit 1 */

-

-  /* USER CODE END I2C1_MspDeInit 1 */

-  }

-

-}

-

-/**

-* @brief UART MSP Initialization

-* This function configures the hardware resources used in this example

-* @param huart: UART handle pointer

-* @retval None

-*/

-void HAL_UART_MspInit(UART_HandleTypeDef* huart)

-{

-  GPIO_InitTypeDef GPIO_InitStruct = {0};

-  if(huart->Instance==USART1)

-  {

-  /* USER CODE BEGIN USART1_MspInit 0 */

-

-  /* USER CODE END USART1_MspInit 0 */

-    /* Peripheral clock enable */

-    __HAL_RCC_USART1_CLK_ENABLE();

-  

-    __HAL_RCC_GPIOA_CLK_ENABLE();

-    /**USART1 GPIO Configuration    

-    PA9     ------> USART1_TX

-    PA10     ------> USART1_RX 

-    */

-    GPIO_InitStruct.Pin = GPIO_PIN_9|GPIO_PIN_10;

-    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;

-    GPIO_InitStruct.Pull = GPIO_PULLUP;

-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;

-    GPIO_InitStruct.Alternate = GPIO_AF7_USART1;

-    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

-

-  /* USER CODE BEGIN USART1_MspInit 1 */

-

-  /* USER CODE END USART1_MspInit 1 */

-  }

-

-}

-

-/**

-* @brief UART MSP De-Initialization

-* This function freeze the hardware resources used in this example

-* @param huart: UART handle pointer

-* @retval None

-*/

-void HAL_UART_MspDeInit(UART_HandleTypeDef* huart)

-{

-  if(huart->Instance==USART1)

-  {

-  /* USER CODE BEGIN USART1_MspDeInit 0 */

-

-  /* USER CODE END USART1_MspDeInit 0 */

-    /* Peripheral clock disable */

-    __HAL_RCC_USART1_CLK_DISABLE();

-  

-    /**USART1 GPIO Configuration    

-    PA9     ------> USART1_TX

-    PA10     ------> USART1_RX 

-    */

-    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10);

-

-  /* USER CODE BEGIN USART1_MspDeInit 1 */

-

-  /* USER CODE END USART1_MspDeInit 1 */

-  }

-

-}

-

-/* USER CODE BEGIN 1 */

-

-/* USER CODE END 1 */

-

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

diff --git a/src/stm32f4xx_it.c b/src/stm32f4xx_it.c
deleted file mode 100644
index caabfb4..0000000
--- a/src/stm32f4xx_it.c
+++ /dev/null
@@ -1,203 +0,0 @@
-/* USER CODE BEGIN Header */

-/**

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

-  * @file    stm32f4xx_it.c

-  * @brief   Interrupt Service Routines.

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

-  * @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"

-#include "stm32f4xx_it.h"

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

-/* USER CODE BEGIN Includes */

-/* USER CODE END Includes */

-

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

-/* USER CODE BEGIN TD */

-

-/* USER CODE END TD */

-

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

-/* USER CODE BEGIN PD */

- 

-/* USER CODE END PD */

-

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

-/* USER CODE BEGIN PM */

-

-/* USER CODE END PM */

-

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

-/* USER CODE BEGIN PV */

-

-/* USER CODE END PV */

-

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

-/* USER CODE BEGIN PFP */

-

-/* USER CODE END PFP */

-

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

-/* USER CODE BEGIN 0 */

-

-/* USER CODE END 0 */

-

-/* External variables --------------------------------------------------------*/

-

-/* USER CODE BEGIN EV */

-

-/* USER CODE END EV */

-

-/******************************************************************************/

-/*           Cortex-M4 Processor Interruption and Exception Handlers          */ 

-/******************************************************************************/

-/**

-  * @brief This function handles Non maskable interrupt.

-  */

-void NMI_Handler(void)

-{

-  /* USER CODE BEGIN NonMaskableInt_IRQn 0 */

-

-  /* USER CODE END NonMaskableInt_IRQn 0 */

-  /* USER CODE BEGIN NonMaskableInt_IRQn 1 */

-

-  /* USER CODE END NonMaskableInt_IRQn 1 */

-}

-

-/**

-  * @brief This function handles Hard fault interrupt.

-  */

-void HardFault_Handler(void)

-{

-  /* USER CODE BEGIN HardFault_IRQn 0 */

-

-  /* USER CODE END HardFault_IRQn 0 */

-  while (1)

-  {

-    /* USER CODE BEGIN W1_HardFault_IRQn 0 */

-    /* USER CODE END W1_HardFault_IRQn 0 */

-  }

-}

-

-/**

-  * @brief This function handles Memory management fault.

-  */

-void MemManage_Handler(void)

-{

-  /* USER CODE BEGIN MemoryManagement_IRQn 0 */

-

-  /* USER CODE END MemoryManagement_IRQn 0 */

-  while (1)

-  {

-    /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */

-    /* USER CODE END W1_MemoryManagement_IRQn 0 */

-  }

-}

-

-/**

-  * @brief This function handles Pre-fetch fault, memory access fault.

-  */

-void BusFault_Handler(void)

-{

-  /* USER CODE BEGIN BusFault_IRQn 0 */

-

-  /* USER CODE END BusFault_IRQn 0 */

-  while (1)

-  {

-    /* USER CODE BEGIN W1_BusFault_IRQn 0 */

-    /* USER CODE END W1_BusFault_IRQn 0 */

-  }

-}

-

-/**

-  * @brief This function handles Undefined instruction or illegal state.

-  */

-void UsageFault_Handler(void)

-{

-  /* USER CODE BEGIN UsageFault_IRQn 0 */

-

-  /* USER CODE END UsageFault_IRQn 0 */

-  while (1)

-  {

-    /* USER CODE BEGIN W1_UsageFault_IRQn 0 */

-    /* USER CODE END W1_UsageFault_IRQn 0 */

-  }

-}

-

-/**

-  * @brief This function handles System service call via SWI instruction.

-  */

-void SVC_Handler(void)

-{

-  /* USER CODE BEGIN SVCall_IRQn 0 */

-

-  /* USER CODE END SVCall_IRQn 0 */

-  /* USER CODE BEGIN SVCall_IRQn 1 */

-

-  /* USER CODE END SVCall_IRQn 1 */

-}

-

-/**

-  * @brief This function handles Debug monitor.

-  */

-void DebugMon_Handler(void)

-{

-  /* USER CODE BEGIN DebugMonitor_IRQn 0 */

-

-  /* USER CODE END DebugMonitor_IRQn 0 */

-  /* USER CODE BEGIN DebugMonitor_IRQn 1 */

-

-  /* USER CODE END DebugMonitor_IRQn 1 */

-}

-

-/**

-  * @brief This function handles Pendable request for system service.

-  */

-void PendSV_Handler(void)

-{

-  /* USER CODE BEGIN PendSV_IRQn 0 */

-

-  /* USER CODE END PendSV_IRQn 0 */

-  /* USER CODE BEGIN PendSV_IRQn 1 */

-

-  /* USER CODE END PendSV_IRQn 1 */

-}

-

-/**

-  * @brief This function handles System tick timer.

-  */

-void SysTick_Handler(void)

-{

-  /* USER CODE BEGIN SysTick_IRQn 0 */

-

-  /* USER CODE END SysTick_IRQn 0 */

-  HAL_IncTick();

-  /* USER CODE BEGIN SysTick_IRQn 1 */

-

-  /* USER CODE END SysTick_IRQn 1 */

-}

-

-/******************************************************************************/

-/* STM32F4xx Peripheral Interrupt Handlers                                    */

-/* Add here the Interrupt Handlers for the used peripherals.                  */

-/* For the available peripheral interrupt handler names,                      */

-/* please refer to the startup file (startup_stm32f4xx.s).                    */

-/******************************************************************************/

-

-/* USER CODE BEGIN 1 */

-

-/* USER CODE END 1 */

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

diff --git a/src/stm32f4xx_it.h b/src/stm32f4xx_it.h
deleted file mode 100644
index 0f787d0..0000000
--- a/src/stm32f4xx_it.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/* USER CODE BEGIN Header */

-/**

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

-  * @file    stm32f4xx_it.h

-  * @brief   This file contains the headers of the interrupt handlers.

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

-  * @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 */

-

-/* Define to prevent recursive inclusion -------------------------------------*/

-#ifndef __STM32F4xx_IT_H

-#define __STM32F4xx_IT_H

-

-#ifdef __cplusplus

- extern "C" {

-#endif 

-

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

-/* USER CODE BEGIN Includes */

-

-/* USER CODE END Includes */

-

-/* Exported types ------------------------------------------------------------*/

-/* USER CODE BEGIN ET */

-

-/* USER CODE END ET */

-

-/* Exported constants --------------------------------------------------------*/

-/* USER CODE BEGIN EC */

-

-/* USER CODE END EC */

-

-/* Exported macro ------------------------------------------------------------*/

-/* USER CODE BEGIN EM */

-

-/* USER CODE END EM */

-

-/* Exported functions prototypes ---------------------------------------------*/

-void NMI_Handler(void);

-void HardFault_Handler(void);

-void MemManage_Handler(void);

-void BusFault_Handler(void);

-void UsageFault_Handler(void);

-void SVC_Handler(void);

-void DebugMon_Handler(void);

-void PendSV_Handler(void);

-void SysTick_Handler(void);

-/* USER CODE BEGIN EFP */

-

-/* USER CODE END EFP */

-

-#ifdef __cplusplus

-}

-#endif

-

-#endif /* __STM32F4xx_IT_H */

-

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

diff --git a/src/system_stm32f4xx.c b/src/system_stm32f4xx.c
deleted file mode 100644
index 7745026..0000000
--- a/src/system_stm32f4xx.c
+++ /dev/null
@@ -1,743 +0,0 @@
-/**

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

-  * @file    system_stm32f4xx.c

-  * @author  MCD Application Team

-  * @brief   CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.

-  *

-  *   This file provides two functions and one global variable to be called from 

-  *   user application:

-  *      - SystemInit(): This function is called at startup just after reset and 

-  *                      before branch to main program. This call is made inside

-  *                      the "startup_stm32f4xx.s" file.

-  *

-  *      - SystemCoreClock variable: Contains the core clock (HCLK), it can be used

-  *                                  by the user application to setup the SysTick 

-  *                                  timer or configure other parameters.

-  *                                     

-  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must

-  *                                 be called whenever the core clock is changed

-  *                                 during program execution.

-  *

-  *

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

-  * @attention

-  *

-  * <h2><center>&copy; COPYRIGHT 2017 STMicroelectronics</center></h2>

-  *

-  * Redistribution and use in source and binary forms, with or without modification,

-  * are permitted provided that the following conditions are met:

-  *   1. Redistributions of source code must retain the above copyright notice,

-  *      this list of conditions and the following disclaimer.

-  *   2. Redistributions in binary form must reproduce the above copyright notice,

-  *      this list of conditions and the following disclaimer in the documentation

-  *      and/or other materials provided with the distribution.

-  *   3. Neither the name of STMicroelectronics nor the names of its contributors

-  *      may be used to endorse or promote products derived from this software

-  *      without specific prior written permission.

-  *

-  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

-  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

-  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE

-  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE

-  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL

-  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR

-  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

-  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,

-  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

-  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

-  *

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

-  */

-

-/** @addtogroup CMSIS

-  * @{

-  */

-

-/** @addtogroup stm32f4xx_system

-  * @{

-  */  

-  

-/** @addtogroup STM32F4xx_System_Private_Includes

-  * @{

-  */

-

-

-#include "stm32f4xx.h"

-

-#if !defined  (HSE_VALUE) 

-  #define HSE_VALUE    ((uint32_t)25000000) /*!< Default value of the External oscillator in Hz */

-#endif /* HSE_VALUE */

-

-#if !defined  (HSI_VALUE)

-  #define HSI_VALUE    ((uint32_t)16000000) /*!< Value of the Internal oscillator in Hz*/

-#endif /* HSI_VALUE */

-

-/**

-  * @}

-  */

-

-/** @addtogroup STM32F4xx_System_Private_TypesDefinitions

-  * @{

-  */

-

-/**

-  * @}

-  */

-

-/** @addtogroup STM32F4xx_System_Private_Defines

-  * @{

-  */

-

-/************************* Miscellaneous Configuration ************************/

-/*!< Uncomment the following line if you need to use external SRAM or SDRAM as data memory  */

-#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx)\

- || defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\

- || defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx)

-/* #define DATA_IN_ExtSRAM */

-#endif /* STM32F40xxx || STM32F41xxx || STM32F42xxx || STM32F43xxx || STM32F469xx || STM32F479xx ||\

-          STM32F412Zx || STM32F412Vx */

- 

-#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\

- || defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx)

-/* #define DATA_IN_ExtSDRAM */

-#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F446xx || STM32F469xx ||\

-          STM32F479xx */

-

-/*!< Uncomment the following line if you need to relocate your vector Table in

-     Internal SRAM. */

-/* #define VECT_TAB_SRAM */

-#define VECT_TAB_OFFSET  0x00 /*!< Vector Table base offset field. 

-                                   This value must be a multiple of 0x200. */

-/******************************************************************************/

-

-/**

-  * @}

-  */

-

-/** @addtogroup STM32F4xx_System_Private_Macros

-  * @{

-  */

-

-/**

-  * @}

-  */

-

-/** @addtogroup STM32F4xx_System_Private_Variables

-  * @{

-  */

-  /* This variable is updated in three ways:

-      1) by calling CMSIS function SystemCoreClockUpdate()

-      2) by calling HAL API function HAL_RCC_GetHCLKFreq()

-      3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency 

-         Note: If you use this function to configure the system clock; then there

-               is no need to call the 2 first functions listed above, since SystemCoreClock

-               variable is updated automatically.

-  */

-uint32_t SystemCoreClock = 16000000;

-const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};

-const uint8_t APBPrescTable[8]  = {0, 0, 0, 0, 1, 2, 3, 4};

-/**

-  * @}

-  */

-

-/** @addtogroup STM32F4xx_System_Private_FunctionPrototypes

-  * @{

-  */

-

-#if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM)

-  static void SystemInit_ExtMemCtl(void); 

-#endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */

-

-/**

-  * @}

-  */

-

-/** @addtogroup STM32F4xx_System_Private_Functions

-  * @{

-  */

-

-/**

-  * @brief  Setup the microcontroller system

-  *         Initialize the FPU setting, vector table location and External memory 

-  *         configuration.

-  * @param  None

-  * @retval None

-  */

-void SystemInit(void)

-{

-  /* FPU settings ------------------------------------------------------------*/

-  #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)

-    SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2));  /* set CP10 and CP11 Full Access */

-  #endif

-

-#if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM)

-  SystemInit_ExtMemCtl(); 

-#endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */

-

-  /* Configure the Vector Table location add offset address ------------------*/

-#ifdef VECT_TAB_SRAM

-  SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */

-#else

-  SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */

-#endif

-}

-

-/**

-   * @brief  Update SystemCoreClock variable according to Clock Register Values.

-  *         The SystemCoreClock variable contains the core clock (HCLK), it can

-  *         be used by the user application to setup the SysTick timer or configure

-  *         other parameters.

-  *           

-  * @note   Each time the core clock (HCLK) changes, this function must be called

-  *         to update SystemCoreClock variable value. Otherwise, any configuration

-  *         based on this variable will be incorrect.         

-  *     

-  * @note   - The system frequency computed by this function is not the real 

-  *           frequency in the chip. It is calculated based on the predefined 

-  *           constant and the selected clock source:

-  *             

-  *           - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)

-  *                                              

-  *           - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)

-  *                          

-  *           - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) 

-  *             or HSI_VALUE(*) multiplied/divided by the PLL factors.

-  *         

-  *         (*) HSI_VALUE is a constant defined in stm32f4xx_hal_conf.h file (default value

-  *             16 MHz) but the real value may vary depending on the variations

-  *             in voltage and temperature.   

-  *    

-  *         (**) HSE_VALUE is a constant defined in stm32f4xx_hal_conf.h file (its value

-  *              depends on the application requirements), user has to ensure that HSE_VALUE

-  *              is same as the real frequency of the crystal used. Otherwise, this function

-  *              may have wrong result.

-  *                

-  *         - The result of this function could be not correct when using fractional

-  *           value for HSE crystal.

-  *     

-  * @param  None

-  * @retval None

-  */

-void SystemCoreClockUpdate(void)

-{

-  uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2;

-  

-  /* Get SYSCLK source -------------------------------------------------------*/

-  tmp = RCC->CFGR & RCC_CFGR_SWS;

-

-  switch (tmp)

-  {

-    case 0x00:  /* HSI used as system clock source */

-      SystemCoreClock = HSI_VALUE;

-      break;

-    case 0x04:  /* HSE used as system clock source */

-      SystemCoreClock = HSE_VALUE;

-      break;

-    case 0x08:  /* PLL used as system clock source */

-

-      /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N

-         SYSCLK = PLL_VCO / PLL_P

-         */    

-      pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22;

-      pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM;

-      

-      if (pllsource != 0)

-      {

-        /* HSE used as PLL clock source */

-        pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);

-      }

-      else

-      {

-        /* HSI used as PLL clock source */

-        pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);

-      }

-

-      pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;

-      SystemCoreClock = pllvco/pllp;

-      break;

-    default:

-      SystemCoreClock = HSI_VALUE;

-      break;

-  }

-  /* Compute HCLK frequency --------------------------------------------------*/

-  /* Get HCLK prescaler */

-  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];

-  /* HCLK frequency */

-  SystemCoreClock >>= tmp;

-}

-

-#if defined (DATA_IN_ExtSRAM) && defined (DATA_IN_ExtSDRAM)

-#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\

- || defined(STM32F469xx) || defined(STM32F479xx)

-/**

-  * @brief  Setup the external memory controller.

-  *         Called in startup_stm32f4xx.s before jump to main.

-  *         This function configures the external memories (SRAM/SDRAM)

-  *         This SRAM/SDRAM will be used as program data memory (including heap and stack).

-  * @param  None

-  * @retval None

-  */

-void SystemInit_ExtMemCtl(void)

-{

-  __IO uint32_t tmp = 0x00;

-

-  register uint32_t tmpreg = 0, timeout = 0xFFFF;

-  register __IO uint32_t index;

-

-  /* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface clock */

-  RCC->AHB1ENR |= 0x000001F8;

-

-  /* Delay after an RCC peripheral clock enabling */

-  tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIOCEN);

-  

-  /* Connect PDx pins to FMC Alternate function */

-  GPIOD->AFR[0]  = 0x00CCC0CC;

-  GPIOD->AFR[1]  = 0xCCCCCCCC;

-  /* Configure PDx pins in Alternate function mode */  

-  GPIOD->MODER   = 0xAAAA0A8A;

-  /* Configure PDx pins speed to 100 MHz */  

-  GPIOD->OSPEEDR = 0xFFFF0FCF;

-  /* Configure PDx pins Output type to push-pull */  

-  GPIOD->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PDx pins */ 

-  GPIOD->PUPDR   = 0x00000000;

-

-  /* Connect PEx pins to FMC Alternate function */

-  GPIOE->AFR[0]  = 0xC00CC0CC;

-  GPIOE->AFR[1]  = 0xCCCCCCCC;

-  /* Configure PEx pins in Alternate function mode */ 

-  GPIOE->MODER   = 0xAAAA828A;

-  /* Configure PEx pins speed to 100 MHz */ 

-  GPIOE->OSPEEDR = 0xFFFFC3CF;

-  /* Configure PEx pins Output type to push-pull */  

-  GPIOE->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PEx pins */ 

-  GPIOE->PUPDR   = 0x00000000;

-  

-  /* Connect PFx pins to FMC Alternate function */

-  GPIOF->AFR[0]  = 0xCCCCCCCC;

-  GPIOF->AFR[1]  = 0xCCCCCCCC;

-  /* Configure PFx pins in Alternate function mode */   

-  GPIOF->MODER   = 0xAA800AAA;

-  /* Configure PFx pins speed to 50 MHz */ 

-  GPIOF->OSPEEDR = 0xAA800AAA;

-  /* Configure PFx pins Output type to push-pull */  

-  GPIOF->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PFx pins */ 

-  GPIOF->PUPDR   = 0x00000000;

-

-  /* Connect PGx pins to FMC Alternate function */

-  GPIOG->AFR[0]  = 0xCCCCCCCC;

-  GPIOG->AFR[1]  = 0xCCCCCCCC;

-  /* Configure PGx pins in Alternate function mode */ 

-  GPIOG->MODER   = 0xAAAAAAAA;

-  /* Configure PGx pins speed to 50 MHz */ 

-  GPIOG->OSPEEDR = 0xAAAAAAAA;

-  /* Configure PGx pins Output type to push-pull */  

-  GPIOG->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PGx pins */ 

-  GPIOG->PUPDR   = 0x00000000;

-  

-  /* Connect PHx pins to FMC Alternate function */

-  GPIOH->AFR[0]  = 0x00C0CC00;

-  GPIOH->AFR[1]  = 0xCCCCCCCC;

-  /* Configure PHx pins in Alternate function mode */ 

-  GPIOH->MODER   = 0xAAAA08A0;

-  /* Configure PHx pins speed to 50 MHz */ 

-  GPIOH->OSPEEDR = 0xAAAA08A0;

-  /* Configure PHx pins Output type to push-pull */  

-  GPIOH->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PHx pins */ 

-  GPIOH->PUPDR   = 0x00000000;

-  

-  /* Connect PIx pins to FMC Alternate function */

-  GPIOI->AFR[0]  = 0xCCCCCCCC;

-  GPIOI->AFR[1]  = 0x00000CC0;

-  /* Configure PIx pins in Alternate function mode */ 

-  GPIOI->MODER   = 0x0028AAAA;

-  /* Configure PIx pins speed to 50 MHz */ 

-  GPIOI->OSPEEDR = 0x0028AAAA;

-  /* Configure PIx pins Output type to push-pull */  

-  GPIOI->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PIx pins */ 

-  GPIOI->PUPDR   = 0x00000000;

-  

-/*-- FMC Configuration -------------------------------------------------------*/

-  /* Enable the FMC interface clock */

-  RCC->AHB3ENR |= 0x00000001;

-  /* Delay after an RCC peripheral clock enabling */

-  tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN);

-

-  FMC_Bank5_6->SDCR[0] = 0x000019E4;

-  FMC_Bank5_6->SDTR[0] = 0x01115351;      

-  

-  /* SDRAM initialization sequence */

-  /* Clock enable command */

-  FMC_Bank5_6->SDCMR = 0x00000011; 

-  tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 

-  while((tmpreg != 0) && (timeout-- > 0))

-  {

-    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 

-  }

-

-  /* Delay */

-  for (index = 0; index<1000; index++);

-  

-  /* PALL command */

-  FMC_Bank5_6->SDCMR = 0x00000012;           

-  timeout = 0xFFFF;

-  while((tmpreg != 0) && (timeout-- > 0))

-  {

-    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 

-  }

-  

-  /* Auto refresh command */

-  FMC_Bank5_6->SDCMR = 0x00000073;

-  timeout = 0xFFFF;

-  while((tmpreg != 0) && (timeout-- > 0))

-  {

-    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 

-  }

- 

-  /* MRD register program */

-  FMC_Bank5_6->SDCMR = 0x00046014;

-  timeout = 0xFFFF;

-  while((tmpreg != 0) && (timeout-- > 0))

-  {

-    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 

-  } 

-  

-  /* Set refresh count */

-  tmpreg = FMC_Bank5_6->SDRTR;

-  FMC_Bank5_6->SDRTR = (tmpreg | (0x0000027C<<1));

-  

-  /* Disable write protection */

-  tmpreg = FMC_Bank5_6->SDCR[0]; 

-  FMC_Bank5_6->SDCR[0] = (tmpreg & 0xFFFFFDFF);

-

-#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)

-  /* Configure and enable Bank1_SRAM2 */

-  FMC_Bank1->BTCR[2]  = 0x00001011;

-  FMC_Bank1->BTCR[3]  = 0x00000201;

-  FMC_Bank1E->BWTR[2] = 0x0fffffff;

-#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */ 

-#if defined(STM32F469xx) || defined(STM32F479xx)

-  /* Configure and enable Bank1_SRAM2 */

-  FMC_Bank1->BTCR[2]  = 0x00001091;

-  FMC_Bank1->BTCR[3]  = 0x00110212;

-  FMC_Bank1E->BWTR[2] = 0x0fffffff;

-#endif /* STM32F469xx || STM32F479xx */

-

-  (void)(tmp); 

-}

-#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx */

-#elif defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM)

-/**

-  * @brief  Setup the external memory controller.

-  *         Called in startup_stm32f4xx.s before jump to main.

-  *         This function configures the external memories (SRAM/SDRAM)

-  *         This SRAM/SDRAM will be used as program data memory (including heap and stack).

-  * @param  None

-  * @retval None

-  */

-void SystemInit_ExtMemCtl(void)

-{

-  __IO uint32_t tmp = 0x00;

-#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\

- || defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx)

-#if defined (DATA_IN_ExtSDRAM)

-  register uint32_t tmpreg = 0, timeout = 0xFFFF;

-  register __IO uint32_t index;

-

-#if defined(STM32F446xx)

-  /* Enable GPIOA, GPIOC, GPIOD, GPIOE, GPIOF, GPIOG interface

-      clock */

-  RCC->AHB1ENR |= 0x0000007D;

-#else

-  /* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface 

-      clock */

-  RCC->AHB1ENR |= 0x000001F8;

-#endif /* STM32F446xx */  

-  /* Delay after an RCC peripheral clock enabling */

-  tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIOCEN);

-  

-#if defined(STM32F446xx)

-  /* Connect PAx pins to FMC Alternate function */

-  GPIOA->AFR[0]  |= 0xC0000000;

-  GPIOA->AFR[1]  |= 0x00000000;

-  /* Configure PDx pins in Alternate function mode */

-  GPIOA->MODER   |= 0x00008000;

-  /* Configure PDx pins speed to 50 MHz */

-  GPIOA->OSPEEDR |= 0x00008000;

-  /* Configure PDx pins Output type to push-pull */

-  GPIOA->OTYPER  |= 0x00000000;

-  /* No pull-up, pull-down for PDx pins */

-  GPIOA->PUPDR   |= 0x00000000;

-

-  /* Connect PCx pins to FMC Alternate function */

-  GPIOC->AFR[0]  |= 0x00CC0000;

-  GPIOC->AFR[1]  |= 0x00000000;

-  /* Configure PDx pins in Alternate function mode */

-  GPIOC->MODER   |= 0x00000A00;

-  /* Configure PDx pins speed to 50 MHz */

-  GPIOC->OSPEEDR |= 0x00000A00;

-  /* Configure PDx pins Output type to push-pull */

-  GPIOC->OTYPER  |= 0x00000000;

-  /* No pull-up, pull-down for PDx pins */

-  GPIOC->PUPDR   |= 0x00000000;

-#endif /* STM32F446xx */

-

-  /* Connect PDx pins to FMC Alternate function */

-  GPIOD->AFR[0]  = 0x000000CC;

-  GPIOD->AFR[1]  = 0xCC000CCC;

-  /* Configure PDx pins in Alternate function mode */  

-  GPIOD->MODER   = 0xA02A000A;

-  /* Configure PDx pins speed to 50 MHz */  

-  GPIOD->OSPEEDR = 0xA02A000A;

-  /* Configure PDx pins Output type to push-pull */  

-  GPIOD->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PDx pins */ 

-  GPIOD->PUPDR   = 0x00000000;

-

-  /* Connect PEx pins to FMC Alternate function */

-  GPIOE->AFR[0]  = 0xC00000CC;

-  GPIOE->AFR[1]  = 0xCCCCCCCC;

-  /* Configure PEx pins in Alternate function mode */ 

-  GPIOE->MODER   = 0xAAAA800A;

-  /* Configure PEx pins speed to 50 MHz */ 

-  GPIOE->OSPEEDR = 0xAAAA800A;

-  /* Configure PEx pins Output type to push-pull */  

-  GPIOE->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PEx pins */ 

-  GPIOE->PUPDR   = 0x00000000;

-

-  /* Connect PFx pins to FMC Alternate function */

-  GPIOF->AFR[0]  = 0xCCCCCCCC;

-  GPIOF->AFR[1]  = 0xCCCCCCCC;

-  /* Configure PFx pins in Alternate function mode */   

-  GPIOF->MODER   = 0xAA800AAA;

-  /* Configure PFx pins speed to 50 MHz */ 

-  GPIOF->OSPEEDR = 0xAA800AAA;

-  /* Configure PFx pins Output type to push-pull */  

-  GPIOF->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PFx pins */ 

-  GPIOF->PUPDR   = 0x00000000;

-

-  /* Connect PGx pins to FMC Alternate function */

-  GPIOG->AFR[0]  = 0xCCCCCCCC;

-  GPIOG->AFR[1]  = 0xCCCCCCCC;

-  /* Configure PGx pins in Alternate function mode */ 

-  GPIOG->MODER   = 0xAAAAAAAA;

-  /* Configure PGx pins speed to 50 MHz */ 

-  GPIOG->OSPEEDR = 0xAAAAAAAA;

-  /* Configure PGx pins Output type to push-pull */  

-  GPIOG->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PGx pins */ 

-  GPIOG->PUPDR   = 0x00000000;

-

-#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\

- || defined(STM32F469xx) || defined(STM32F479xx)  

-  /* Connect PHx pins to FMC Alternate function */

-  GPIOH->AFR[0]  = 0x00C0CC00;

-  GPIOH->AFR[1]  = 0xCCCCCCCC;

-  /* Configure PHx pins in Alternate function mode */ 

-  GPIOH->MODER   = 0xAAAA08A0;

-  /* Configure PHx pins speed to 50 MHz */ 

-  GPIOH->OSPEEDR = 0xAAAA08A0;

-  /* Configure PHx pins Output type to push-pull */  

-  GPIOH->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PHx pins */ 

-  GPIOH->PUPDR   = 0x00000000;

-  

-  /* Connect PIx pins to FMC Alternate function */

-  GPIOI->AFR[0]  = 0xCCCCCCCC;

-  GPIOI->AFR[1]  = 0x00000CC0;

-  /* Configure PIx pins in Alternate function mode */ 

-  GPIOI->MODER   = 0x0028AAAA;

-  /* Configure PIx pins speed to 50 MHz */ 

-  GPIOI->OSPEEDR = 0x0028AAAA;

-  /* Configure PIx pins Output type to push-pull */  

-  GPIOI->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PIx pins */ 

-  GPIOI->PUPDR   = 0x00000000;

-#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx */

-  

-/*-- FMC Configuration -------------------------------------------------------*/

-  /* Enable the FMC interface clock */

-  RCC->AHB3ENR |= 0x00000001;

-  /* Delay after an RCC peripheral clock enabling */

-  tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN);

-

-  /* Configure and enable SDRAM bank1 */

-#if defined(STM32F446xx)

-  FMC_Bank5_6->SDCR[0] = 0x00001954;

-#else  

-  FMC_Bank5_6->SDCR[0] = 0x000019E4;

-#endif /* STM32F446xx */

-  FMC_Bank5_6->SDTR[0] = 0x01115351;      

-  

-  /* SDRAM initialization sequence */

-  /* Clock enable command */

-  FMC_Bank5_6->SDCMR = 0x00000011; 

-  tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 

-  while((tmpreg != 0) && (timeout-- > 0))

-  {

-    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 

-  }

-

-  /* Delay */

-  for (index = 0; index<1000; index++);

-  

-  /* PALL command */

-  FMC_Bank5_6->SDCMR = 0x00000012;           

-  timeout = 0xFFFF;

-  while((tmpreg != 0) && (timeout-- > 0))

-  {

-    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 

-  }

-  

-  /* Auto refresh command */

-#if defined(STM32F446xx)

-  FMC_Bank5_6->SDCMR = 0x000000F3;

-#else  

-  FMC_Bank5_6->SDCMR = 0x00000073;

-#endif /* STM32F446xx */

-  timeout = 0xFFFF;

-  while((tmpreg != 0) && (timeout-- > 0))

-  {

-    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 

-  }

- 

-  /* MRD register program */

-#if defined(STM32F446xx)

-  FMC_Bank5_6->SDCMR = 0x00044014;

-#else  

-  FMC_Bank5_6->SDCMR = 0x00046014;

-#endif /* STM32F446xx */

-  timeout = 0xFFFF;

-  while((tmpreg != 0) && (timeout-- > 0))

-  {

-    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 

-  } 

-  

-  /* Set refresh count */

-  tmpreg = FMC_Bank5_6->SDRTR;

-#if defined(STM32F446xx)

-  FMC_Bank5_6->SDRTR = (tmpreg | (0x0000050C<<1));

-#else    

-  FMC_Bank5_6->SDRTR = (tmpreg | (0x0000027C<<1));

-#endif /* STM32F446xx */

-  

-  /* Disable write protection */

-  tmpreg = FMC_Bank5_6->SDCR[0]; 

-  FMC_Bank5_6->SDCR[0] = (tmpreg & 0xFFFFFDFF);

-#endif /* DATA_IN_ExtSDRAM */

-#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F446xx || STM32F469xx || STM32F479xx */

-

-#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx)\

- || defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\

- || defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx)

-

-#if defined(DATA_IN_ExtSRAM)

-/*-- GPIOs Configuration -----------------------------------------------------*/

-   /* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */

-  RCC->AHB1ENR   |= 0x00000078;

-  /* Delay after an RCC peripheral clock enabling */

-  tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIODEN);

-  

-  /* Connect PDx pins to FMC Alternate function */

-  GPIOD->AFR[0]  = 0x00CCC0CC;

-  GPIOD->AFR[1]  = 0xCCCCCCCC;

-  /* Configure PDx pins in Alternate function mode */  

-  GPIOD->MODER   = 0xAAAA0A8A;

-  /* Configure PDx pins speed to 100 MHz */  

-  GPIOD->OSPEEDR = 0xFFFF0FCF;

-  /* Configure PDx pins Output type to push-pull */  

-  GPIOD->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PDx pins */ 

-  GPIOD->PUPDR   = 0x00000000;

-

-  /* Connect PEx pins to FMC Alternate function */

-  GPIOE->AFR[0]  = 0xC00CC0CC;

-  GPIOE->AFR[1]  = 0xCCCCCCCC;

-  /* Configure PEx pins in Alternate function mode */ 

-  GPIOE->MODER   = 0xAAAA828A;

-  /* Configure PEx pins speed to 100 MHz */ 

-  GPIOE->OSPEEDR = 0xFFFFC3CF;

-  /* Configure PEx pins Output type to push-pull */  

-  GPIOE->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PEx pins */ 

-  GPIOE->PUPDR   = 0x00000000;

-

-  /* Connect PFx pins to FMC Alternate function */

-  GPIOF->AFR[0]  = 0x00CCCCCC;

-  GPIOF->AFR[1]  = 0xCCCC0000;

-  /* Configure PFx pins in Alternate function mode */   

-  GPIOF->MODER   = 0xAA000AAA;

-  /* Configure PFx pins speed to 100 MHz */ 

-  GPIOF->OSPEEDR = 0xFF000FFF;

-  /* Configure PFx pins Output type to push-pull */  

-  GPIOF->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PFx pins */ 

-  GPIOF->PUPDR   = 0x00000000;

-

-  /* Connect PGx pins to FMC Alternate function */

-  GPIOG->AFR[0]  = 0x00CCCCCC;

-  GPIOG->AFR[1]  = 0x000000C0;

-  /* Configure PGx pins in Alternate function mode */ 

-  GPIOG->MODER   = 0x00085AAA;

-  /* Configure PGx pins speed to 100 MHz */ 

-  GPIOG->OSPEEDR = 0x000CAFFF;

-  /* Configure PGx pins Output type to push-pull */  

-  GPIOG->OTYPER  = 0x00000000;

-  /* No pull-up, pull-down for PGx pins */ 

-  GPIOG->PUPDR   = 0x00000000;

-  

-/*-- FMC/FSMC Configuration --------------------------------------------------*/

-  /* Enable the FMC/FSMC interface clock */

-  RCC->AHB3ENR         |= 0x00000001;

-

-#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)

-  /* Delay after an RCC peripheral clock enabling */

-  tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN);

-  /* Configure and enable Bank1_SRAM2 */

-  FMC_Bank1->BTCR[2]  = 0x00001011;

-  FMC_Bank1->BTCR[3]  = 0x00000201;

-  FMC_Bank1E->BWTR[2] = 0x0fffffff;

-#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */ 

-#if defined(STM32F469xx) || defined(STM32F479xx)

-  /* Delay after an RCC peripheral clock enabling */

-  tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN);

-  /* Configure and enable Bank1_SRAM2 */

-  FMC_Bank1->BTCR[2]  = 0x00001091;

-  FMC_Bank1->BTCR[3]  = 0x00110212;

-  FMC_Bank1E->BWTR[2] = 0x0fffffff;

-#endif /* STM32F469xx || STM32F479xx */

-#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx)|| defined(STM32F417xx)\

-   || defined(STM32F412Zx) || defined(STM32F412Vx)

-  /* Delay after an RCC peripheral clock enabling */

-  tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FSMCEN);

-  /* Configure and enable Bank1_SRAM2 */

-  FSMC_Bank1->BTCR[2]  = 0x00001011;

-  FSMC_Bank1->BTCR[3]  = 0x00000201;

-  FSMC_Bank1E->BWTR[2] = 0x0FFFFFFF;

-#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F412Zx || STM32F412Vx */

-

-#endif /* DATA_IN_ExtSRAM */

-#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F427xx || STM32F437xx ||\

-          STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx || STM32F412Zx || STM32F412Vx  */ 

-  (void)(tmp); 

-}

-#endif /* DATA_IN_ExtSRAM && DATA_IN_ExtSDRAM */

-/**

-  * @}

-  */

-

-/**

-  * @}

-  */

-

-/**

-  * @}

-  */

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