diff options
| -rw-r--r-- | src/config.h | 16 | ||||
| -rw-r--r-- | src/dataflow.h | 5 | ||||
| -rw-r--r-- | src/main-data.c | 373 |
3 files changed, 308 insertions, 86 deletions
diff --git a/src/config.h b/src/config.h index 059c0d3..ad035c9 100644 --- a/src/config.h +++ b/src/config.h @@ -4,3 +4,19 @@ /* Enable testing mode */ #define TESTING_ENABLE 1 + +/* Buffer size for routing */ +#define ROUTING_BUFSIZE 128 + +/** + * \def MASTER_I2C_BUS_INTERVAL + * \brief General interval value used by master in milliseconds + * + * Master uses this value to introduce delays on the I2C bus in order to reduce I2C timeout values. + * The optimum interval is usually dependent on I2C bus configuration on the hardware. Testing and debug UART + * operations will also affect this value. + * If unforeseen I2C timeout issues are encountered on master side, this value should be increased to suit the + * hardware. + * +*/ +#define MASTER_I2C_BUS_INTERVAL 150 diff --git a/src/dataflow.h b/src/dataflow.h index 700bbdc..a2217ca 100644 --- a/src/dataflow.h +++ b/src/dataflow.h @@ -14,7 +14,7 @@ * | DF_RX_DOC | 1 | Sent m2s_SOR; ready to receive s2m_DOC | Receive s2m_DOC | DF_CTS | * | DF_CTS | 2 | Received m2s_SOR; ready to send DF_CTS | Send m2s_CTS | DF_RX_DATA or DF_RX_CMD | * | DF_RX_DATA | 3 | Sent m2s_CTS; receive s2m_data | Receive s2m_data | DF_SUCCESS | - * | DF_RX_CMD | 4 | sent m2s_CTS receive s2m_command | Receive s2m_command | DF_SUCCESS | + * | DF_RX_CMD | 4 | sent m2s_CTS receive s2m_command | Receive s2m_command | DF_SUCCESS | * |------------+-----+----------------------------------------+---------------------+-------------------------| * * @@ -27,7 +27,8 @@ typedef enum dataflow_status { DF_RX_DATA = 3, DF_RX_CMD = 4, DF_SUCCESS = 5, - DF_FAIL = 6 + DF_FAIL = 6, + DF_LEN_TX = 7 } dataflow_status_t; typedef enum SOR_codes { diff --git a/src/main-data.c b/src/main-data.c index 5f45562..46b0f36 100644 --- a/src/main-data.c +++ b/src/main-data.c @@ -1,4 +1,3 @@ -/* USER CODE BEGIN Header */ /** ****************************************************************************** * @file : main.c @@ -28,6 +27,7 @@ /* 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__))) @@ -48,20 +48,29 @@ uint32_t allocated[4]={0}; uint8_t dev_sts[BUS_DEVICE_LIMIT] = {OFFLINE}; uint8_t data_idx; +_datapoint routing_buffer[ROUTING_BUFSIZE]; +/* Index information for each datapoint */ +uint8_t routing_idx_buffer[ROUTING_BUFSIZE]; +/* Pointer to tail of both data and idx buffers */ +uint32_t routing_ptr = 0; + /* Function prototypes */ void SystemClock_Config(void); static void MX_GPIO_Init(void); static void MX_I2C1_Init(void); static void MX_USART1_UART_Init(void); -bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args); hs_status_t handshake(uint32_t i2c_addr); -dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code); +dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t routing_buf_idx); +bool routing(void); bool todo_hs_or_not_todo_hs(uint8_t i2c_addr); state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status); +bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args); bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg); bool encode_datapoint_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg); bool decode_data_callback(pb_istream_t *istream, const pb_field_t *field, void **args); +bool master_encode_MDR_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg); + /** * @brief The application entry point. * @retval int @@ -97,17 +106,14 @@ int main(void) uint8_t dev_idx = GET_IDX_FROM_ADDR(I2C_ADDRESS); subs_info[dev_idx] = malloc(sizeof(subscription_info_t)); subs_info[dev_idx]->mod_idx = subs_info[dev_idx]->entity_idx = - subs_info[dev_idx]->class_idx = subs_info[dev_idx]->i2c_idx = 0; + subs_info[dev_idx]->class_idx = subs_info[dev_idx]->i2c_idx = 0; + SET_BIT_FROM_IDX(allocated, dev_idx); + + subs_info[dev_idx]->module_ids[subs_info[dev_idx]->mod_idx++] = 1; //subscribe to self + - /* These fields are from the encode_sub callback used for testing */ - for (int x=0; x<2; x++) { - subs_info[dev_idx]->module_ids[subs_info[dev_idx]->mod_idx++] - = x+10*x; - subs_info[dev_idx]->i2c_address[subs_info[dev_idx]->i2c_idx++] - = x+1; - } _MDR module_MDR = s2m_MDR_response_init_default; - + device_info[dev_idx] = malloc(sizeof(device_info_t)); device_info[dev_idx]->i2c_addr = I2C_ADDRESS; device_info[dev_idx]->device_id = dev_idx; @@ -120,99 +126,143 @@ int main(void) device_info[dev_idx]->MDR = module_MDR; /* dataflow */ - device_dataflow(0x05, 1); + /* while (1) { */ + device_dataflow(0x05, 1, 0); + HAL_Delay(MASTER_I2C_BUS_INTERVAL); + routing(); + /* HAL_Delay(500); */ + /* } */ #else /* Slave code*/ { - uint8_t SOR_buf[m2s_SOR_size] = {0}, debug_buf[128], term[]="\r\n"; - if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, m2s_SOR_size, 10000) != HAL_OK) { - sprintf((char*)debug_buf, "Failed to get SOR\r\n"); - HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); - memset(debug_buf, 0, 128); - } - else { - sprintf((char*)debug_buf, "Got SOR\r\n"); - HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); - memset(debug_buf, 0, 128); - } - m2s_SOR SOR_message; - pb_istream_t SOR_istream = pb_istream_from_buffer(SOR_buf, 2); - if (!pb_decode(&SOR_istream, m2s_SOR_fields, &SOR_message)) { - sprintf((char*)debug_buf, "SOR decode error\r\n"); - HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); - memset(debug_buf, 0, 128); - } - else { - sprintf((char*)debug_buf, "SOR decoded; code: %ld\r\n", SOR_message.SOR_code); - HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); - memset(debug_buf, 0, 128); - } - - if (SOR_message.SOR_code == 1) { - uint8_t data_buf[128]; - size_t data_enc_size; - s2m_data data; - data.datapoints.funcs.encode = encode_datapoint_callback; - pb_ostream_t data_ostream = pb_ostream_from_buffer(data_buf, sizeof(data_buf)); - if (!pb_encode(&data_ostream, s2m_data_fields, &data)) { - sprintf((char*)debug_buf, "Data encoding error\r\n"); + while (1) { + uint8_t SOR_buf[m2s_SOR_size] = {0}, debug_buf[128]; + if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, m2s_SOR_size, 10000) != HAL_OK) { + sprintf((char*)debug_buf, "Failed to get SOR\r\n"); HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); memset(debug_buf, 0, 128); - Error_Handler(); } - data_enc_size = data_ostream.bytes_written; - - s2m_DOC doc = s2m_DOC_init_zero; - uint8_t doc_buf[s2m_DOC_size]; - doc.DOC_code = 5; - doc.tx_length = data_enc_size; - pb_ostream_t doc_ostream = pb_ostream_from_buffer(doc_buf, 4); - - if (!pb_encode(&doc_ostream, s2m_DOC_fields, &doc)) { - sprintf((char*)debug_buf, "DOC encoding error\r\n"); + else { + sprintf((char*)debug_buf, "Got SOR\r\n"); HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); memset(debug_buf, 0, 128); - Error_Handler(); } - - sprintf((char*)debug_buf, "s2m_DOC encoded length: %d\r\n", doc_ostream.bytes_written); - HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); - memset(debug_buf, 0, 128); - - if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)doc_buf, 4, 10000) != HAL_OK) { - sprintf((char*)debug_buf, "DOC I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1)); + m2s_SOR SOR_message; + pb_istream_t SOR_istream = pb_istream_from_buffer(SOR_buf, 2); + if (!pb_decode(&SOR_istream, m2s_SOR_fields, &SOR_message)) { + sprintf((char*)debug_buf, "SOR decode error\r\n"); HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); memset(debug_buf, 0, 128); - Error_Handler(); } else { - sprintf((char*)debug_buf, "SENT DOC\r\n"); + sprintf((char*)debug_buf, "SOR decoded; code: %ld\r\n", SOR_message.SOR_code); HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); memset(debug_buf, 0, 128); } - uint8_t CTS_buf[8]; + if (SOR_message.SOR_code == 1) { + uint8_t data_buf[128]; + size_t data_enc_size; + s2m_data data; + data.datapoints.funcs.encode = encode_datapoint_callback; + pb_ostream_t data_ostream = pb_ostream_from_buffer(data_buf, sizeof(data_buf)); + if (!pb_encode(&data_ostream, s2m_data_fields, &data)) { + sprintf((char*)debug_buf, "Data encoding error\r\n"); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + Error_Handler(); + } + data_enc_size = data_ostream.bytes_written; - if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)CTS_buf, 2, 10000) != HAL_OK) { - sprintf((char*)debug_buf, "Failed to get CTS: %ld\r\n", HAL_I2C_GetError(&hi2c1)); + s2m_DOC doc = s2m_DOC_init_zero; + uint8_t doc_buf[s2m_DOC_size]; + doc.DOC_code = 5; + doc.tx_length = data_enc_size; + pb_ostream_t doc_ostream = pb_ostream_from_buffer(doc_buf, 4); + + if (!pb_encode(&doc_ostream, s2m_DOC_fields, &doc)) { + sprintf((char*)debug_buf, "DOC encoding error\r\n"); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + Error_Handler(); + } + + sprintf((char*)debug_buf, "s2m_DOC encoded length: %d\r\n", doc_ostream.bytes_written); HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); memset(debug_buf, 0, 128); - Error_Handler(); + + if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)doc_buf, 4, 10000) != HAL_OK) { + sprintf((char*)debug_buf, "DOC I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1)); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + Error_Handler(); + } + else { + sprintf((char*)debug_buf, "SENT DOC\r\n"); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + } + + uint8_t CTS_buf[8]; + + if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)CTS_buf, 2, 10000) != HAL_OK) { + sprintf((char*)debug_buf, "Failed to get CTS: %ld\r\n", HAL_I2C_GetError(&hi2c1)); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + Error_Handler(); + } + else { + sprintf((char*)debug_buf, "Got CTS\r\n"); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + } + + if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)data_buf, data_enc_size, 10000) != HAL_OK) { + sprintf((char*)debug_buf, "Data I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1)); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + Error_Handler(); + } + else { + sprintf((char*)debug_buf, "SENT DATA\r\n"); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + } } - else { - sprintf((char*)debug_buf, "Got CTS\r\n"); + else if (SOR_message.SOR_code == 2) { + uint8_t CTS_buf[] = {0x0, 0x1}; + uint8_t len_buf[4], *MDR_buf, *data_buf; + /* _datapoint datapoints[16]; */ + + HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000); + sprintf((char*)debug_buf, "Sent CTS\r\n"); HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); memset(debug_buf, 0, 128); - } + + HAL_I2C_Slave_Receive(&hi2c1, len_buf, 4, 1000); + + uint16_t MDR_len = len_buf[1]+(len_buf[0]<<8); + MDR_buf = malloc(MDR_len); + uint16_t data_len = len_buf[3]+(len_buf[2]<<8); + data_buf = malloc(data_len); - if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)data_buf, data_enc_size, 10000) != HAL_OK) { - sprintf((char*)debug_buf, "Data I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1)); + 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); - Error_Handler(); - } - else { - sprintf((char*)debug_buf, "SENT DATA\r\n"); + + HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000); + HAL_I2C_Slave_Receive(&hi2c1, MDR_buf, MDR_len, 10000); + HAL_I2C_Slave_Receive(&hi2c1, data_buf, data_len, 10000); + + _datapoint datapoint_message; + s2m_MDR_response MDR_message; + pb_istream_t MDR_istream = pb_istream_from_buffer(MDR_buf, MDR_len); + pb_istream_t data_istream = pb_istream_from_buffer(data_buf, data_len); + + pb_decode(&MDR_istream, s2m_MDR_response_fields, &MDR_message); + pb_decode(&data_istream, _datapoint_fields, &datapoint_message); + + sprintf((char*)debug_buf, "Got data from %ld, running version %f\r\n\tdata 0: %f\r\n", MDR_message.module_id, MDR_message.MDR_version, datapoint_message.data); HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); memset(debug_buf, 0, 128); } @@ -525,7 +575,7 @@ __DEBUG_BLOCK_END: return hs_sts; } -dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code) +dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t rbuf_data_idx) { uint8_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr); dataflow_status_t df_status = DF_IDLE; @@ -546,11 +596,12 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code) #ifdef TESTING_ENABLE uint8_t term[] = "\r\n"; #endif - + while (df_status != DF_SUCCESS && df_status != DF_FAIL) { switch (df_status) { case (DF_IDLE): { + HAL_Delay(MASTER_I2C_BUS_INTERVAL); SOR_buf = malloc(sizeof(m2s_SOR)); pb_ostream_t SOR_stream = pb_ostream_from_buffer(SOR_buf, sizeof(SOR_buf)); SOR_message.SOR_code = SOR_code; @@ -564,7 +615,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code) } else { if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)SOR_buf, - m2s_SOR_size, 10000) != HAL_OK) { + m2s_SOR_size, 1000) != HAL_OK) { df_status = DF_FAIL; #ifdef DEBUG_ENABLE goto __DF_SOR_I2C_ERROR; @@ -578,6 +629,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code) } else if (SOR_code == SLAVE_RX_DATAPOINT) { /* TODO */ + df_status = DF_LEN_TX; } else if (SOR_code == SLAVE_RX_COMMAND) { /* TODO */ @@ -649,7 +701,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code) #endif } else { - HAL_Delay(100); + HAL_Delay(MASTER_I2C_BUS_INTERVAL); if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)CTS_buf, 2, 10000) != HAL_OK) { df_status = DF_FAIL; @@ -704,18 +756,126 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code) #endif } else { + /* This could be done in the callback itself */ + for (int i = 0; i < data_idx && routing_ptr < ROUTING_BUFSIZE; i++) { + routing_idx_buffer[routing_ptr] = dev_idx; + routing_buffer[routing_ptr++] = datapoints[i]; + } df_status = DF_SUCCESS; } } break; } + case (DF_LEN_TX): + { + /* TODO error checking */ + /* Will need to package datapoint and MDR to know their lengths + Once cached, will not need to do this */ + + /* Do this after handshake to cache ================================================== */ + uint8_t MDR_buf[128], data_buf[128], CTS_buf[2]; + uint8_t src_device_idx = routing_idx_buffer[rbuf_data_idx]; + s2m_MDR_response data_src_MDR = device_info[src_device_idx]->MDR; + pb_ostream_t MDR_ostream = pb_ostream_from_buffer(MDR_buf, sizeof(MDR_buf)); + data_src_MDR.subscriptions.funcs.encode=master_encode_MDR_callback; + pb_encode(&MDR_ostream, s2m_MDR_response_fields, &data_src_MDR); + uint8_t MDR_len = MDR_ostream.bytes_written; + /* =================================================================================== */ + + _datapoint data = routing_buffer[rbuf_data_idx]; + pb_ostream_t data_ostream = pb_ostream_from_buffer(data_buf, sizeof(data_buf)); + pb_encode(&data_ostream, _datapoint_fields, &data); + uint8_t data_len = data_ostream.bytes_written; + + uint8_t data_MDR_len_buf[4] = {0, MDR_len, 0, data_len}; + + AF_error_counter = 0; + while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 10000) != HAL_OK) { + if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) { + df_status = DF_FAIL; + } + if (++AF_error_counter > 3000) { + df_status = DF_FAIL; + } + if (df_status == DF_FAIL) { + sprintf((char*)debug_buf, "Failed to get LEN CTS\r\n"); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + break; + } + } + + HAL_Delay(MASTER_I2C_BUS_INTERVAL); + if (df_status != DF_FAIL && + HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, data_MDR_len_buf, 4, 10000) == HAL_OK) { + sprintf((char*)debug_buf, "MDR len: %d data len: %d SENT\r\n", MDR_len, data_len); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + } + else { + sprintf((char*)debug_buf, "Failed to send lengths\r\n"); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + } + AF_error_counter = 0; + while (df_status != DF_FAIL && + HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 10000) != HAL_OK) { + if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) { + df_status = DF_FAIL; + } + if (++AF_error_counter > 3000) { + df_status = DF_FAIL; + } + if (df_status == DF_FAIL) { + sprintf((char*)debug_buf, "Failed to get TX CTS\r\n"); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + break; + } + } + + +#ifdef TESTING_ENABLE + uint8_t buf_title[64]; + sprintf((char*)buf_title, "MDR buffer: "); + HAL_UART_Transmit(&huart1, buf_title, sizeof(buf_title), 100); + memset(buf_title, 0, 64); + for(int x=0; x<MDR_len; x++) + sprintf((char*)debug_buf+x, "%x", MDR_buf[x]); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + HAL_UART_Transmit(&huart1, term, 2, 100); + memset(debug_buf, 0, 128); + + sprintf((char*)buf_title, "Data buffer: "); + HAL_UART_Transmit(&huart1, buf_title, sizeof(buf_title), 100); + memset(buf_title, 0, 64); + for(int x=0; x<data_len; x++) + sprintf((char*)debug_buf+x, "%x", data_buf[x]); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + HAL_UART_Transmit(&huart1, term, 2, 100); + memset(debug_buf, 0, 128); +#endif + + if (df_status != DF_FAIL && + HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, MDR_buf, MDR_len, 10000) == HAL_OK && + HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, data_buf, data_len, 10000) == HAL_OK) { + sprintf((char*)debug_buf, "Data and MDR SENT\r\n"); + HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100); + memset(debug_buf, 0, 128); + + df_status = DF_SUCCESS; + } + break; + } + case DF_SUCCESS: + case DF_FAIL: + break; } } #ifdef TESTING_ENABLE { goto __DF_TESTING_BLOCK_END; - __DF_TESTING_BLOCK_END: __asm__("nop"); } @@ -772,6 +932,43 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code) return df_status; } +bool routing(void) +{ + /* This table holds information on where to send each datapoint in the routing buffer */ + uint32_t routing_table[ROUTING_BUFSIZE][4] = {{0, 0}}; + + /* Build table with routing information */ + for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < routing_ptr; rbuf_data_idx++) { + uint8_t module_idx = routing_idx_buffer[rbuf_data_idx]; + for (uint8_t dev_idx = 0; dev_idx < BUS_DEVICE_LIMIT; dev_idx++) { + if (!(GET_BIT_FROM_IDX(allocated, dev_idx)&&1)) { // No module at this index + continue; + } + bool alloc = false; + for (uint8_t dev_sub_idx = 0; dev_sub_idx < subs_info[dev_idx]->mod_idx && !alloc; dev_sub_idx++) { + if (subs_info[dev_idx]->module_ids[dev_sub_idx] == device_info[module_idx]->MDR.module_id) { + SET_BIT_FROM_IDX(routing_table[rbuf_data_idx], module_idx); + alloc = true; + } + } + /* TODO entity ID, I2C addr and class routing, should go in the if condition above */ + } + } + + for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < routing_ptr; rbuf_data_idx++) { + for (uint8_t device_idx = 0; device_idx < BUS_DEVICE_LIMIT; device_idx++) { + if (GET_BIT_FROM_IDX(allocated, device_idx) && + GET_BIT_FROM_IDX(routing_table[rbuf_data_idx], device_idx)) { + device_dataflow(GET_ADDR_FROM_IDX(device_idx), SLAVE_RX_DATAPOINT, rbuf_data_idx); + } + } + } + + /* Reset the routing pointer, since all data in buffer should have been routed */ + routing_ptr = 0; + return true; +} + bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args) { _subscriptions subs; @@ -844,6 +1041,14 @@ state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status) return device_state; } +bool master_encode_MDR_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg) +{ + if (!pb_encode_tag_for_field(ostream, field)) { + return false; + } + return true; +} + bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg) { if(ostream!=NULL && field->tag == s2m_MDR_response_subscriptions_tag) { |