2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/tmtcpacket/packetmatcher/PacketMatchTree.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
|
2022-07-19 18:13:25 +02:00
|
|
|
#include "fsfw/storagemanager/LocalPool.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/tmtcpacket/packetmatcher/ApidMatcher.h"
|
2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/tmtcpacket/packetmatcher/ServiceMatcher.h"
|
|
|
|
#include "fsfw/tmtcpacket/packetmatcher/SubserviceMatcher.h"
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2020-10-15 13:45:47 +02:00
|
|
|
// This should be configurable..
|
|
|
|
const LocalPool::LocalPoolConfig PacketMatchTree::poolConfig = {
|
2022-02-02 10:29:30 +01:00
|
|
|
{10, sizeof(ServiceMatcher)},
|
|
|
|
{20, sizeof(SubServiceMatcher)},
|
|
|
|
{2, sizeof(ApidMatcher)},
|
|
|
|
{40, sizeof(PacketMatchTree::Node)}};
|
2020-10-15 13:45:47 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
PacketMatchTree::PacketMatchTree(Node* root)
|
2022-07-19 18:13:25 +02:00
|
|
|
: MatchTree<PusTmIF*>(root, 2),
|
2022-02-02 10:29:30 +01:00
|
|
|
factoryBackend(0, poolConfig, false, true),
|
|
|
|
factory(&factoryBackend) {}
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
PacketMatchTree::PacketMatchTree(iterator root)
|
2022-07-19 18:13:25 +02:00
|
|
|
: MatchTree<PusTmIF*>(root.element, 2),
|
2022-02-02 10:29:30 +01:00
|
|
|
factoryBackend(0, poolConfig, false, true),
|
|
|
|
factory(&factoryBackend) {}
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
PacketMatchTree::PacketMatchTree()
|
2022-07-19 18:13:25 +02:00
|
|
|
: MatchTree<PusTmIF*>(nullptr, 2),
|
2022-02-02 10:29:30 +01:00
|
|
|
factoryBackend(0, poolConfig, false, true),
|
|
|
|
factory(&factoryBackend) {}
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-07-19 18:13:25 +02:00
|
|
|
PacketMatchTree::~PacketMatchTree() = default;
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t PacketMatchTree::addMatch(uint16_t apid, uint8_t type, uint8_t subtype) {
|
|
|
|
// We assume adding APID is always requested.
|
2022-07-20 22:21:15 +02:00
|
|
|
mintm::MinimalPusTm data{};
|
2022-07-20 11:43:16 +02:00
|
|
|
data.secHeader.service = type;
|
|
|
|
data.secHeader.subservice = subtype;
|
2022-07-19 18:13:25 +02:00
|
|
|
PusTmMinimal testPacket((uint8_t*)&data);
|
2022-07-18 10:20:26 +02:00
|
|
|
testPacket.setApid(apid);
|
2022-07-19 18:13:25 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
iterator lastTest;
|
|
|
|
iterator rollback;
|
|
|
|
ReturnValue_t result =
|
2022-07-19 18:13:25 +02:00
|
|
|
findOrInsertMatch<PusTmIF*, ApidMatcher>(this->begin(), &testPacket, &lastTest);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (result == NEW_NODE_CREATED) {
|
|
|
|
rollback = lastTest;
|
|
|
|
} else if (result != RETURN_OK) {
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
if (type == 0) {
|
|
|
|
// Check if lastTest has no children, otherwise, delete them,
|
|
|
|
// as a more general check is requested.
|
2022-07-19 18:13:25 +02:00
|
|
|
if (lastTest.left() != PacketMatchTree::end()) {
|
2022-02-02 10:29:30 +01:00
|
|
|
removeElementAndAllChildren(lastTest.left());
|
2021-06-14 15:14:57 +02:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
return RETURN_OK;
|
|
|
|
}
|
|
|
|
// Type insertion required.
|
2022-07-19 18:13:25 +02:00
|
|
|
result = findOrInsertMatch<PusTmIF*, ServiceMatcher>(lastTest.left(), &testPacket, &lastTest);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (result == NEW_NODE_CREATED) {
|
2022-07-19 18:13:25 +02:00
|
|
|
if (rollback == PacketMatchTree::end()) {
|
2022-02-02 10:29:30 +01:00
|
|
|
rollback = lastTest;
|
|
|
|
}
|
|
|
|
} else if (result != RETURN_OK) {
|
2022-07-19 18:13:25 +02:00
|
|
|
if (rollback != PacketMatchTree::end()) {
|
2022-02-02 10:29:30 +01:00
|
|
|
removeElementAndAllChildren(rollback);
|
|
|
|
}
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
if (subtype == 0) {
|
2022-07-19 18:13:25 +02:00
|
|
|
if (lastTest.left() != PacketMatchTree::end()) {
|
2022-02-02 10:29:30 +01:00
|
|
|
// See above
|
|
|
|
removeElementAndAllChildren(lastTest.left());
|
2021-06-14 15:14:57 +02:00
|
|
|
}
|
|
|
|
return RETURN_OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
// Subtype insertion required.
|
2022-07-19 18:13:25 +02:00
|
|
|
result = findOrInsertMatch<PusTmIF*, SubServiceMatcher>(lastTest.left(), &testPacket, &lastTest);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (result == NEW_NODE_CREATED) {
|
|
|
|
return RETURN_OK;
|
|
|
|
} else if (result != RETURN_OK) {
|
2022-07-19 18:13:25 +02:00
|
|
|
if (rollback != PacketMatchTree::end()) {
|
2022-02-02 10:29:30 +01:00
|
|
|
removeElementAndAllChildren(rollback);
|
|
|
|
}
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
return RETURN_OK;
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
template <typename VALUE_T, typename INSERTION_T>
|
2018-07-12 16:29:32 +02:00
|
|
|
ReturnValue_t PacketMatchTree::findOrInsertMatch(iterator startAt, VALUE_T test,
|
2022-02-02 10:29:30 +01:00
|
|
|
iterator* lastTest) {
|
|
|
|
bool attachToBranch = AND;
|
|
|
|
iterator iter = startAt;
|
2022-07-19 18:13:25 +02:00
|
|
|
while (iter != PacketMatchTree::end()) {
|
2022-02-02 10:29:30 +01:00
|
|
|
bool isMatch = iter->match(test);
|
|
|
|
attachToBranch = OR;
|
|
|
|
*lastTest = iter;
|
|
|
|
if (isMatch) {
|
|
|
|
return RETURN_OK;
|
|
|
|
} else {
|
|
|
|
// Go down OR branch.
|
|
|
|
iter = iter.right();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
// Only reached if nothing was found.
|
|
|
|
SerializeableMatcherIF<VALUE_T>* newContent = factory.generate<INSERTION_T>(test);
|
2022-07-19 18:13:25 +02:00
|
|
|
if (newContent == nullptr) {
|
2022-02-02 10:29:30 +01:00
|
|
|
return FULL;
|
|
|
|
}
|
|
|
|
Node* newNode = factory.generate<Node>(newContent);
|
2022-07-19 18:13:25 +02:00
|
|
|
if (newNode == nullptr) {
|
2022-02-02 10:29:30 +01:00
|
|
|
// Need to make sure partially generated content is deleted, otherwise, that's a leak.
|
|
|
|
factory.destroy<INSERTION_T>(static_cast<INSERTION_T*>(newContent));
|
|
|
|
return FULL;
|
|
|
|
}
|
|
|
|
*lastTest = insert(attachToBranch, *lastTest, newNode);
|
|
|
|
if (*lastTest == end()) {
|
|
|
|
// This actaully never fails, so creating a dedicated returncode seems an overshoot.
|
|
|
|
return RETURN_FAILED;
|
|
|
|
}
|
|
|
|
return NEW_NODE_CREATED;
|
2018-07-12 16:29:32 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t PacketMatchTree::removeMatch(uint16_t apid, uint8_t type, uint8_t subtype) {
|
2022-07-20 22:21:15 +02:00
|
|
|
mintm::MinimalPusTm data{};
|
2022-07-20 11:43:16 +02:00
|
|
|
data.secHeader.service = type;
|
|
|
|
data.secHeader.subservice = subtype;
|
2022-07-19 18:13:25 +02:00
|
|
|
PusTmMinimal testPacket((uint8_t*)&data);
|
2022-07-18 10:20:26 +02:00
|
|
|
testPacket.setApid(apid);
|
2022-02-02 10:29:30 +01:00
|
|
|
iterator foundElement = findMatch(begin(), &testPacket);
|
2022-07-20 11:43:16 +02:00
|
|
|
if (foundElement == PacketMatchTree::end()) {
|
2022-02-02 10:29:30 +01:00
|
|
|
return NO_MATCH;
|
|
|
|
}
|
|
|
|
if (type == 0) {
|
|
|
|
if (foundElement.left() == end()) {
|
|
|
|
return removeElementAndReconnectChildren(foundElement);
|
|
|
|
} else {
|
|
|
|
return TOO_GENERAL_REQUEST;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
// Go down AND branch. Will abort if empty.
|
|
|
|
foundElement = findMatch(foundElement.left(), &testPacket);
|
|
|
|
if (foundElement == this->end()) {
|
|
|
|
return NO_MATCH;
|
|
|
|
}
|
|
|
|
if (subtype == 0) {
|
|
|
|
if (foundElement.left() == end()) {
|
|
|
|
return removeElementAndReconnectChildren(foundElement);
|
|
|
|
} else {
|
|
|
|
return TOO_GENERAL_REQUEST;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
// Again, go down AND branch.
|
|
|
|
foundElement = findMatch(foundElement.left(), &testPacket);
|
|
|
|
if (foundElement == end()) {
|
|
|
|
return NO_MATCH;
|
|
|
|
}
|
|
|
|
return removeElementAndReconnectChildren(foundElement);
|
2018-07-12 16:29:32 +02:00
|
|
|
}
|
|
|
|
|
2022-07-19 18:13:25 +02:00
|
|
|
PacketMatchTree::iterator PacketMatchTree::findMatch(iterator startAt, PusTmIF* test) {
|
2022-02-02 10:29:30 +01:00
|
|
|
iterator iter = startAt;
|
|
|
|
while (iter != end()) {
|
|
|
|
bool isMatch = iter->match(test);
|
|
|
|
if (isMatch) {
|
|
|
|
break;
|
|
|
|
} else {
|
|
|
|
iter = iter.right(); // next OR element
|
2021-06-14 15:14:57 +02:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return iter;
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t PacketMatchTree::initialize() { return factoryBackend.initialize(); }
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t PacketMatchTree::changeMatch(bool addToMatch, uint16_t apid, uint8_t type,
|
|
|
|
uint8_t subtype) {
|
|
|
|
if (addToMatch) {
|
|
|
|
return addMatch(apid, type, subtype);
|
|
|
|
} else {
|
|
|
|
return removeMatch(apid, type, subtype);
|
|
|
|
}
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t PacketMatchTree::cleanUpElement(iterator position) {
|
2022-02-02 10:29:30 +01:00
|
|
|
factory.destroy(position.element->value);
|
|
|
|
// Go on anyway, there's nothing we can do.
|
|
|
|
// SHOULDDO: Throw event, or write debug message?
|
|
|
|
return factory.destroy(position.element);
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|