/* * TODO Do conditional includes based on which target we are building for. * Actually, target-specific includes should go in the port folder * This should be specified in the master config file * TODO This file should be moved to the posix port folder * */ /* FreeRTOS includes. */ /* #include "FreeRTOS_POSIX.h" */ /* System headers. */ #include #include #include /* Library includes */ #include #include /* (Ideally) platform-agnostic project includes. */ #include "master_posix.h" #include "main.h" #include "devices.h" #include "config.h" #include "dataflow.h" #include "handshake.pb.h" #include "data.pb.h" #include "stream.h" #include "stream_stdio.h" #include "port.h" #include "handshake.h" /* FreeRTOS+POSIX. should go in the port folder */ /* #include "FreeRTOS_POSIX/pthread.h" */ /* #include "FreeRTOS_POSIX/mqueue.h" */ /* #include "FreeRTOS_POSIX/time.h" */ /* #include "FreeRTOS_POSIX/fcntl.h" */ /* #include "FreeRTOS_POSIX/errno.h" */ /* Constants. */ #define LINE_BREAK "\r\n" #define device_MDR s2m_MDR_response #define GET_IDX_FROM_ADDR(i2c_addr) (i2c_addr>>1)-1 #define GET_ADDR_FROM_IDX(idx) (idx+1)<<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 STREAM_INIT(PERIPH, vptr, sptr) \ sptr.read=&read_##PERIPH; \ sptr.write=&write_##PERIPH; \ sptr.props=vptr; #define BUS_DEVICE_LIMIT 2 hs_func_t hs_jumptable[NUM_HS_STATES] = { HS_STATE_TABLE(EXPAND_AS_JUMPTABLE) }; df_func_t df_jumptable[NUM_DF_STATES] = { DF_STATE_TABLE(EXPAND_AS_JUMPTABLE) }; p_stream_t device_streams[2]; device_info_t *device_info[BUS_DEVICE_LIMIT] = {NULL}; subscription_info_t* subs_info[BUS_DEVICE_LIMIT]; uint32_t allocated[4]={0}; uint8_t dev_sts[BUS_DEVICE_LIMIT] = {OFFLINE}; uint8_t data_idx; 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 *MDR_rbuf[BUS_DEVICE_LIMIT]; /*< Buffer to store encoded MDR */ uint8_t *MDR_len_buf[BUS_DEVICE_LIMIT]; /*< Buffer to store encoded MDR lengths */ 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 */ p_stream_t stream; static void *handshake_func(void * pvArgs); static void *dataflow_func(void *pvArgs); static void *routing_func(void *pvArgs); bool routing(void); bool cmd_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 Main function; * * See the top of this file for detailed description. */ void vStartPOSIXMaster(void *pvParams) { pthread_t handshake_thread, dataflow_thread, routing_thread; /* Stream initializations should be handled by the devicetree library once that's set up */ STREAM_INIT(STDIO, NULL, device_streams[0]); STREAM_INIT(STDIO, NULL, device_streams[1]); /* This is the global stream that is being used by handhsake and dataflow */ STREAM_INIT(STDIO, NULL, stream); /* pthread_create(&handshake_thread, NULL, handshake_func, NULL); */ pthread_create(&dataflow_thread, NULL, dataflow_func, NULL); /* pthread_create(&routing_thread, NULL, routing_func, NULL); */ /* This function will be defined for the port * Each port will have a configuration file that includes details about the bus, * either in a custom format or in a devicetree format (preferred) * */ /* Add device-specific stream/thread declerations here, if needed */ /* ... */ /* This task was created with the native xTaskCreate() API function, so must not run off the end of its implementing thread. */ /* vTaskDelete(NULL); */ } static void *handshake_func(void * pvArgs) { printf("Handshake thread started %s", LINE_BREAK); /* for (;;) { */ for (int dev_idx = 0; dev_idx < BUS_DEVICE_LIMIT-1; dev_idx++) { /* * With the new state machine structure, what is the correct condition * for performing a handshake? * * Answer: Handshake attempts will occur based on user-configured processor devices. * This ties in with the (new) grander philosophy at work here: a processor will have * two types of configurations of devices, viz. ones that are present in the * devicetree at boot time and others that will be hot-plugged during runtime. The * user will configure certain peripherals (in a TBD manner) to enable * hot-pluggability of peripherals on those devices. For example, a UART device could * be configured hot-pluggable, which would result in handshake being attempted at the * device every now and then. * This gets interesting when devices are configured as * both hot-pluggable and with statically configured peripherals at the device. For * example, an I2C device can have one or more peripherals configured on the bus in * the devicetree, but can also be configured for hot-pluggability. It is obvious that * not all devices can support dual configuration.. some mechanism will be needed to * detect device configuration errors at compile time. * */ /* if (todo_hs_or_not_todo_hs(GET_ADDR_FROM_IDX(dev_idx))) { */ int hs_state = HS_STATE_0; void **args; args = malloc(sizeof(uint8_t*)*2); uint32_t dev_idx = 0; /*< Do something with this, not relevant anymore */ while (hs_state != HS_STATE_FAIL && hs_state != HS_STATE_SUCCESS) hs_state = hs_jumptable[hs_state](stream, args); if (hs_state == HS_STATE_SUCCESS) { uint8_t MDR_len = ((uint8_t*)args[0])[0]; /* Store the encoded MDR buffer */ MDR_rbuf[dev_idx] = malloc(sizeof(uint8_t)*MDR_len); memcpy(MDR_rbuf[dev_idx], args[1], MDR_len); free(args[1]); MDR_len_buf[dev_idx] = &MDR_len; /* Attempt to decode the protobuf, and add devices */ s2m_MDR_response MDR_res_message = s2m_MDR_response_init_default; 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_rbuf[dev_idx], MDR_len); if (!pb_decode(&MDR_res_stream, s2m_MDR_response_fields, &MDR_res_message)) { printf("decode fail\n"); hs_state = HS_STATE_FAIL; } else { printf("decode done\n"); device_info[dev_idx] = malloc(sizeof(device_info_t)); device_info[dev_idx]->i2c_addr = 0x0; device_info[dev_idx]->device_id = dev_idx; device_info[dev_idx]->MDR = MDR_res_message; hs_state = HS_REGISTERED; } /* } */ /* TODO This is slightly redundant now, so fix this */ /* dev_sts[dev_idx] = get_state_from_hs_status(GET_ADDR_FROM_IDX(dev_idx), hs_state); */ } /* } */ } return NULL; } static void *dataflow_func(void *pvArgs) { printf("Dataflow thread started %s", LINE_BREAK); void **args; args = malloc(sizeof(uint8_t*)*11); int x; for (x = 0; x < 11; x++) args[x] = malloc(sizeof(uint8_t*)); uint8_t rbuf_idx = 0; args[2] = &data_rbuf; args[3] = &data_routing_ptr; args[4] = &data_src_idx_rbuf; args[5] = &data_len_buf; args[6] = &cmd_routing_buf; args[7] = &cmd_routing_ptr; args[8] = &cmd_src_idx_rbuf; args[9] = &cmd_dst_idx_rbuf; args[10] = &MDR_rbuf; args[11] = &rbuf_idx; uint8_t SOR_code = SLAVE_TX; args[0] = &SOR_code; int df_state = DF_IDLE; while (df_state != DF_STATE_FAIL && df_state != DF_STATE_SUCCESS) df_state = df_jumptable[df_state](stream, args); printf("Dataflow thread ended %s", LINE_BREAK); return NULL; } static void *routing_func(void *pvArgs) { printf("Routing thread started %s", LINE_BREAK); for (;;) { routing(); cmd_routing(); } return NULL; } 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 < 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 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) { SET_BIT_FROM_IDX(routing_table[rbuf_data_idx], dev_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 < 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)) { /* TODO modify this to work with state machine */ /* 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 */ data_routing_ptr = 0; return true; } 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; } 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 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 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; }