diff --git a/engine/IO/inc/_luos_phy.h b/engine/IO/inc/_luos_phy.h index 71566a0dd..11e5cf6d5 100644 --- a/engine/IO/inc/_luos_phy.h +++ b/engine/IO/inc/_luos_phy.h @@ -27,6 +27,8 @@ void Phy_FiltersInit(void); void Phy_AddLocalServices(uint16_t service_id, uint16_t service_number); bool Phy_FilterType(uint16_t type_id); void Phy_IndexSet(uint8_t *index, uint16_t id); +void Phy_NodeIndexRm(uint16_t id); +void Phy_ServiceIndexRm(uint16_t id); void Phy_ResetAllNeeded(void); #endif /* _PRIVATE_LUOS_PHY_H_ */ diff --git a/engine/IO/src/luos_io.c b/engine/IO/src/luos_io.c index 4fb1df856..766eed8c4 100644 --- a/engine/IO/src/luos_io.c +++ b/engine/IO/src/luos_io.c @@ -476,12 +476,12 @@ error_return_t LuosIO_ConsumeMsg(const msg_t *input) switch (input->header.size) { case 2: - // generate local ID + // Generate local ID RoutingTB_Erase(); memcpy(&base_id, &input->data[0], sizeof(uint16_t)); Service_GenerateId(base_id); case 0: - // send back a local routing table + // Send back a local routing table output_msg.header.cmd = RTB; output_msg.header.target_mode = NODEIDACK; output_msg.header.target = input->header.source; @@ -582,10 +582,23 @@ error_return_t LuosIO_ConsumeMsg(const msg_t *input) case DEADTARGET: if (dead_target->node_id != 0) { + // Get all services of this node and remove them from the indexes + search_result_t result; + RTFilter_Node(RTFilter_Reset(&result), dead_target->node_id); + for (size_t i = 0; i < result.result_nbr; i++) + { + Phy_ServiceIndexRm(result.result_table[i]->id); + } + // remove the node from the indexes + Phy_NodeIndexRm(dead_target->node_id); + // remove the node services from the routing table RoutingTB_RemoveNode(dead_target->node_id); } if (dead_target->service_id != 0) { + // Remove the service from the indexes + Phy_ServiceIndexRm(dead_target->service_id); + // Remove the service from the routing table RoutingTB_RemoveService(dead_target->service_id); } // This assert information could be usefull for services, do not remove it. diff --git a/engine/IO/src/luos_phy.c b/engine/IO/src/luos_phy.c index 080d7b12f..21c3d33e1 100644 --- a/engine/IO/src/luos_phy.c +++ b/engine/IO/src/luos_phy.c @@ -100,6 +100,7 @@ static int Phy_GetPhyId(luos_phy_t *phy_ptr); static bool Phy_IndexFilter(uint8_t *index, uint16_t id); static bool Phy_Need(luos_phy_t *phy_ptr, header_t *header); static phy_target_t Phy_ComputeTargets(luos_phy_t *phy_ptr, header_t *header); +static void Phy_IndexRm(uint8_t *index, uint16_t id); /******************************************************************************* * Variables @@ -527,7 +528,7 @@ bool Phy_Need(luos_phy_t *phy_ptr, header_t *header) // To avoid to spend precious computing time, instead of checking all the phy we will only check if this concern only the receiving phy. // We need to keep this message only if the message target is not only for the phy_ptr, except if it is Luos because Luos can do localhost. - // If this phy is Luos phy, we need to keep all the messages + // If this phy is Luos phy, we need to keep all the messages no matter what. if (Phy_GetPhyId(phy_ptr) == 0) { return true; @@ -543,19 +544,27 @@ bool Phy_Need(luos_phy_t *phy_ptr, header_t *header) break; case SERVICEIDACK: case SERVICEID: - // If the target is not the phy_ptr, we need to keep this message - return !Phy_IndexFilter(phy_ptr->services, header->target); + // If the target is not the phy_ptr, and the source service is known, we need to keep this message + return (!Phy_IndexFilter(phy_ptr->services, header->target)) && (Phy_IndexFilter(phy_ptr->services, header->source)); break; case NODEIDACK: case NODEID: if (header->target == 0) { - return Node_DoWeWaitId() || (phy_ctx.PhyExeptSourceDone == false); // or we are waiting for child branches ((phy_ctx.topology_running == true) && (header->source == 1)) + return Node_DoWeWaitId() || (phy_ctx.PhyExeptSourceDone == false); } else { - // If the target is not for the receiving phy, we need to keep this message - return (!Phy_IndexFilter(phy_ptr->nodes, header->target) && (Node_Get()->node_id != 0)); + if (Luos_IsDetected()) + { + // If the target is not for the receiving phy, and the source service is known, we need to keep this message + return (!Phy_IndexFilter(phy_ptr->nodes, header->target) && (Node_Get()->node_id != 0) && (Phy_IndexFilter(phy_ptr->services, header->source))); + } + else + { + // If the target is not for the receiving phy, we need to keep this message + return (!Phy_IndexFilter(phy_ptr->nodes, header->target) && (Node_Get()->node_id != 0)); + } } break; default: @@ -1199,8 +1208,8 @@ void Phy_AddLocalServices(uint16_t service_id, uint16_t service_number) /****************************************************************************** * @brief check if the given id value concern this phy index - * @param index Pointer to the index of the node - * @param id id of the service concerned by this message + * @param index Pointer to the index of the node or service + * @param id id of the node or service concerned by this message * @return phy concerned by this message ******************************************************************************/ inline bool Phy_IndexFilter(uint8_t *index, uint16_t id) @@ -1212,8 +1221,8 @@ inline bool Phy_IndexFilter(uint8_t *index, uint16_t id) /****************************************************************************** * @brief Set a given id value in the index - * @param index Pointer to the index of the node - * @param id id of the service concerned by this message + * @param index Pointer to the index of the node or service + * @param id id of the node or service concerned by this message * @return phy concerned by this message ******************************************************************************/ inline void Phy_IndexSet(uint8_t *index, uint16_t id) @@ -1223,6 +1232,49 @@ inline void Phy_IndexSet(uint8_t *index, uint16_t id) index[bit_index / 8] |= 1 << (bit_index % 8); } +/****************************************************************************** + * @brief Remove a given id value in the index + * @param index Pointer to the index of the node or service + * @param id id of the service concerned by this message + * @return phy concerned by this message + ******************************************************************************/ +inline void Phy_IndexRm(uint8_t *index, uint16_t id) +{ + LUOS_ASSERT((index != NULL) && (id <= 0x0FFF) && (id != 0)); + uint8_t bit_index = id - 1; // Because 1 represent bit index 0. + index[bit_index / 8] &= ~(1 << (bit_index % 8)); +} + +/****************************************************************************** + * @brief Remove a given service id value in the index of all phys + * @param id id of the service concerned by this message + * @return phy concerned by this message + ******************************************************************************/ +inline void Phy_ServiceIndexRm(uint16_t id) +{ + LUOS_ASSERT((id <= 0x0FFF) && (id != 0)); + // for all phy + for (int i = 0; i < phy_ctx.phy_nb; i++) + { + Phy_IndexRm(phy_ctx.phy[i].services, id); + } +} + +/****************************************************************************** + * @brief Remove a given node id value in the index of all phys + * @param id id of the node concerned by this message + * @return phy concerned by this message + ******************************************************************************/ +inline void Phy_NodeIndexRm(uint16_t id) +{ + LUOS_ASSERT((id <= 0x0FFF) && (id != 0)); + // for all phy + for (int i = 0; i < phy_ctx.phy_nb; i++) + { + Phy_IndexRm(phy_ctx.phy[i].nodes, id); + } +} + /****************************************************************************** * @brief Parse all services type to find if target exists * @param type_id of message diff --git a/engine/core/src/routing_table.c b/engine/core/src/routing_table.c index 035fedeb8..1eb006cb9 100644 --- a/engine/core/src/routing_table.c +++ b/engine/core/src/routing_table.c @@ -1030,12 +1030,12 @@ search_result_t *RTFilter_Node(search_result_t *result, uint16_t node_id) uint8_t entry_nbr = 0; // Check result pointer LUOS_ASSERT(result != 0); - // if we the result is not initialized return 0 + // If the result is not initialized return 0 if (RTFilter_InitCheck(result) == FAILED) { result->result_nbr = 0; } - // search all the entries of the research table + // Search all the entries of the research table while (entry_nbr < result->result_nbr) { // find a service with the wanted node_id diff --git a/test/tests_io/test_phy/main.c b/test/tests_io/test_phy/main.c index 52300eab7..dcc10e075 100644 --- a/test/tests_io/test_phy/main.c +++ b/test/tests_io/test_phy/main.c @@ -2,6 +2,7 @@ #include "../src/luos_phy.c" #include "../src/msg_alloc.c" #include "../src/luos_io.c" +#include "../../core/src/node.c" #include luos_phy_t *robus_phy; @@ -696,7 +697,7 @@ void unittest_phy_ComputeHeader() END_TRY; } - NEW_TEST_CASE("Check ComputeHeader for a non timestamped message needed by Luos"); + NEW_TEST_CASE("Check ComputeHeader for a non timestamped message from Luos"); { TRY { @@ -723,7 +724,7 @@ void unittest_phy_ComputeHeader() luos_phy->rx_alloc_job = false; luos_phy->rx_size = 0; luos_phy->rx_phy_filter = 0x00; - Phy_ComputeHeader(luos_phy); + Phy_ComputeHeader(luos_phy); // Here, because we use luos_phy, the message is accepted no matter what. TEST_ASSERT_EQUAL(msg.header.size + sizeof(header_t), luos_phy->rx_size); TEST_ASSERT_EQUAL(true, luos_phy->rx_keep); TEST_ASSERT_EQUAL(true, luos_phy->rx_ack); @@ -736,6 +737,264 @@ void unittest_phy_ComputeHeader() END_TRY; } + NEW_TEST_CASE("Check ComputeHeader for a non timestamped message needed by Luos"); + { + TRY + { + phy_test_reset(); + // Create a fake service with id 1 + Phy_AddLocalServices(1, 1); + + msg_t msg; + msg.header.config = BASE_PROTOCOL; + msg.header.target_mode = SERVICEIDACK; + msg.header.target = 1; + msg.header.cmd = IO_STATE; + msg.header.size = 1; + msg.header.source = 2; + msg.data[0] = 0xAE; + + // Configure the source in indexes + Phy_IndexSet(robus_phy->services, 2); + // Save message information in the Luos phy struct + robus_phy->rx_buffer_base = (uint8_t *)&msg; + robus_phy->rx_data = luos_phy->rx_buffer_base; + // For now let just consider that we received the header allowing us to compute the message things based on it. + robus_phy->received_data = sizeof(header_t); + robus_phy->rx_keep = true; // Tell phy that we want to keep this message + robus_phy->rx_ack = false; + robus_phy->rx_alloc_job = false; + robus_phy->rx_size = 0; + robus_phy->rx_phy_filter = 0x00; + Phy_ComputeHeader(robus_phy); // Here, because we use luos_phy, the message is accepted no matter what. + TEST_ASSERT_EQUAL(msg.header.size + sizeof(header_t), robus_phy->rx_size); + TEST_ASSERT_EQUAL(true, robus_phy->rx_keep); + TEST_ASSERT_EQUAL(true, robus_phy->rx_ack); + TEST_ASSERT_EQUAL(true, robus_phy->rx_alloc_job); + } + CATCH + { + TEST_ASSERT_TRUE(false); + } + END_TRY; + } + + NEW_TEST_CASE("Check ComputeHeader failure for a non referenced service source message needed by Luos"); + { + TRY + { + phy_test_reset(); + // Create a fake service with id 1 + Phy_AddLocalServices(1, 1); + + msg_t msg; + msg.header.config = BASE_PROTOCOL; + msg.header.target_mode = SERVICEIDACK; + msg.header.target = 1; + msg.header.cmd = IO_STATE; + msg.header.size = 1; + msg.header.source = 3; + msg.data[0] = 0xAE; + + // Configure the source in indexes + Phy_IndexSet(robus_phy->services, 2); + // Save message information in the Luos phy struct + robus_phy->rx_buffer_base = (uint8_t *)&msg; + robus_phy->rx_data = luos_phy->rx_buffer_base; + // For now let just consider that we received the header allowing us to compute the message things based on it. + robus_phy->received_data = sizeof(header_t); + robus_phy->rx_keep = true; // Tell phy that we want to keep this message + robus_phy->rx_ack = false; + robus_phy->rx_alloc_job = false; + robus_phy->rx_size = 0; + robus_phy->rx_phy_filter = 0x00; + Phy_ComputeHeader(robus_phy); + TEST_ASSERT_EQUAL(msg.header.size + sizeof(header_t), robus_phy->rx_size); + TEST_ASSERT_EQUAL(false, robus_phy->rx_keep); + TEST_ASSERT_EQUAL(false, robus_phy->rx_ack); + TEST_ASSERT_EQUAL(false, robus_phy->rx_alloc_job); + } + CATCH + { + TEST_ASSERT_TRUE(false); + } + END_TRY; + } + + NEW_TEST_CASE("Check ComputeHeader failure for a non referenced service source node message needed by Luos during the detection"); + { + TRY + { + phy_test_reset(); + // Create a fake service with id 1 + Phy_AddLocalServices(1, 1); + + msg_t msg; + msg.header.config = BASE_PROTOCOL; + msg.header.target_mode = NODEIDACK; + msg.header.target = 2; + msg.header.cmd = IO_STATE; + msg.header.size = 1; + msg.header.source = 2; + msg.data[0] = 0xAE; + Node_Get()->node_id = 0; + + // Configure the source in indexes + Phy_IndexSet(robus_phy->services, 2); + // Save message information in the Luos phy struct + robus_phy->rx_buffer_base = (uint8_t *)&msg; + robus_phy->rx_data = luos_phy->rx_buffer_base; + // For now let just consider that we received the header allowing us to compute the message things based on it. + robus_phy->received_data = sizeof(header_t); + robus_phy->rx_keep = true; // Tell phy that we want to keep this message + robus_phy->rx_ack = false; + robus_phy->rx_alloc_job = false; + robus_phy->rx_size = 0; + robus_phy->rx_phy_filter = 0x00; + Phy_ComputeHeader(robus_phy); // Here, because we don't have node_id yet, the message is rejected no matter what. + TEST_ASSERT_EQUAL(msg.header.size + sizeof(header_t), robus_phy->rx_size); + TEST_ASSERT_EQUAL(false, robus_phy->rx_keep); + TEST_ASSERT_EQUAL(false, robus_phy->rx_ack); + TEST_ASSERT_EQUAL(false, robus_phy->rx_alloc_job); + } + CATCH + { + TEST_ASSERT_TRUE(false); + } + END_TRY; + } + + NEW_TEST_CASE("Check ComputeHeader failure for a non referenced service source node message needed by Luos before detection"); + { + TRY + { + phy_test_reset(); + // Create a fake service with id 1 + Phy_AddLocalServices(1, 1); + + msg_t msg; + msg.header.config = BASE_PROTOCOL; + msg.header.target_mode = NODEIDACK; + msg.header.target = 2; + msg.header.cmd = IO_STATE; + msg.header.size = 1; + msg.header.source = 3; + msg.data[0] = 0xAE; + Node_Get()->node_id = 2; + + // Configure the source in indexes + Phy_IndexSet(robus_phy->services, 2); + // Save message information in the Luos phy struct + robus_phy->rx_buffer_base = (uint8_t *)&msg; + robus_phy->rx_data = luos_phy->rx_buffer_base; + // For now let just consider that we received the header allowing us to compute the message things based on it. + robus_phy->received_data = sizeof(header_t); + robus_phy->rx_keep = true; // Tell phy that we want to keep this message + robus_phy->rx_ack = false; + robus_phy->rx_alloc_job = false; + robus_phy->rx_size = 0; + robus_phy->rx_phy_filter = 0x00; + Phy_ComputeHeader(robus_phy); // Here, the source is not a referenced service, we have a node ID, but detection is not done, so the message is accepted. + TEST_ASSERT_EQUAL(msg.header.size + sizeof(header_t), robus_phy->rx_size); + TEST_ASSERT_EQUAL(true, robus_phy->rx_keep); + TEST_ASSERT_EQUAL(true, robus_phy->rx_ack); + TEST_ASSERT_EQUAL(true, robus_phy->rx_alloc_job); + } + CATCH + { + TEST_ASSERT_TRUE(false); + } + END_TRY; + } + + NEW_TEST_CASE("Check ComputeHeader failure for a non referenced service source node message needed by Luos"); + { + TRY + { + phy_test_reset(); + // Create a fake service with id 1 + Phy_AddLocalServices(1, 1); + + msg_t msg; + msg.header.config = BASE_PROTOCOL; + msg.header.target_mode = NODEIDACK; + msg.header.target = 2; + msg.header.cmd = IO_STATE; + msg.header.size = 1; + msg.header.source = 3; + msg.data[0] = 0xAE; + Node_Get()->node_id = 2; + node_ctx.state = DETECTION_OK; + + // Configure the source in indexes + Phy_IndexSet(robus_phy->services, 2); + // Save message information in the Luos phy struct + robus_phy->rx_buffer_base = (uint8_t *)&msg; + robus_phy->rx_data = luos_phy->rx_buffer_base; + // For now let just consider that we received the header allowing us to compute the message things based on it. + robus_phy->received_data = sizeof(header_t); + robus_phy->rx_keep = true; // Tell phy that we want to keep this message + robus_phy->rx_ack = false; + robus_phy->rx_alloc_job = false; + robus_phy->rx_size = 0; + robus_phy->rx_phy_filter = 0x00; + Phy_ComputeHeader(robus_phy); // Here, the source is not a referenced service, we have a node ID, and detection is done, so the message is rejected. + TEST_ASSERT_EQUAL(msg.header.size + sizeof(header_t), robus_phy->rx_size); + TEST_ASSERT_EQUAL(false, robus_phy->rx_keep); + TEST_ASSERT_EQUAL(false, robus_phy->rx_ack); + TEST_ASSERT_EQUAL(false, robus_phy->rx_alloc_job); + } + CATCH + { + TEST_ASSERT_TRUE(false); + } + END_TRY; + } + + NEW_TEST_CASE("Check ComputeHeader failure for a referenced service source node message needed by Luos"); + { + TRY + { + phy_test_reset(); + // Create a fake service with id 1 + Phy_AddLocalServices(1, 1); + + msg_t msg; + msg.header.config = BASE_PROTOCOL; + msg.header.target_mode = NODEIDACK; + msg.header.target = 2; + msg.header.cmd = IO_STATE; + msg.header.size = 1; + msg.header.source = 2; + msg.data[0] = 0xAE; + Node_Get()->node_id = 2; + node_ctx.state = DETECTION_OK; + + // Configure the source in indexes + Phy_IndexSet(robus_phy->services, 2); + // Save message information in the Luos phy struct + robus_phy->rx_buffer_base = (uint8_t *)&msg; + robus_phy->rx_data = luos_phy->rx_buffer_base; + // For now let just consider that we received the header allowing us to compute the message things based on it. + robus_phy->received_data = sizeof(header_t); + robus_phy->rx_keep = true; // Tell phy that we want to keep this message + robus_phy->rx_ack = false; + robus_phy->rx_alloc_job = false; + robus_phy->rx_size = 0; + robus_phy->rx_phy_filter = 0x00; + Phy_ComputeHeader(robus_phy); // Here, the source service is referenced, we have a node Id, and detection is done. The message is accepted. + TEST_ASSERT_EQUAL(msg.header.size + sizeof(header_t), robus_phy->rx_size); + TEST_ASSERT_EQUAL(true, robus_phy->rx_keep); + TEST_ASSERT_EQUAL(true, robus_phy->rx_ack); + TEST_ASSERT_EQUAL(true, robus_phy->rx_alloc_job); + } + CATCH + { + TEST_ASSERT_TRUE(false); + } + END_TRY; + } + NEW_TEST_CASE("Check ComputeHeader for a timestamped message needed by Luos"); { TRY @@ -750,6 +1009,7 @@ void unittest_phy_ComputeHeader() msg.header.target = 1; msg.header.cmd = IO_STATE; msg.header.size = 1; + msg.header.source = 2; msg.data[0] = 0xAE; // Save message information in the Luos phy struct @@ -1028,6 +1288,7 @@ void unittest_phy_ValidMsg() msg.header.config = TIMESTAMP_PROTOCOL; msg.header.target_mode = SERVICEIDACK; msg.header.target = 2; + msg.header.source = 1; msg.header.cmd = IO_STATE; msg.header.size = 600; msg.data[0] = 0xAE; @@ -1043,6 +1304,7 @@ void unittest_phy_ValidMsg() luos_phy->rx_size = MAX_DATA_MSG_SIZE + sizeof(header_t) + sizeof(time_luos_t); luos_phy->rx_phy_filter = 0x00; luos_phy->rx_timestamp = 10; + luos_phy->services[0] = 0x01; // Configure service 1 as accessible from Luos robus_phy->services[0] = 0x02; // Configure this service as accessible from Robus TEST_ASSERT_EQUAL(0, phy_ctx.io_job_nb);