From 1e49eb419d5e58dc3e0d59231359666ef9f25988 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Thu, 4 May 2023 15:25:02 +0200 Subject: [PATCH] datamanClient: add DatamanCache class --- src/lib/dataman_client/DatamanClient.cpp | 194 ++++++++++++++++++++++- src/lib/dataman_client/DatamanClient.hpp | 57 ++++--- src/systemcmds/tests/test_dataman.cpp | 168 ++++++++++++++++++++ 3 files changed, 400 insertions(+), 19 deletions(-) diff --git a/src/lib/dataman_client/DatamanClient.cpp b/src/lib/dataman_client/DatamanClient.cpp index 6d0f9809ca..9c0713c8bc 100644 --- a/src/lib/dataman_client/DatamanClient.cpp +++ b/src/lib/dataman_client/DatamanClient.cpp @@ -569,6 +569,198 @@ void DatamanClient::abortCurrentOperation() _state = State::Idle; } -DatamanCache::DatamanCache(int num_items) +DatamanCache::DatamanCache(const char *cache_miss_perf_counter_name, uint32_t num_items) + : _cache_miss_perf(perf_alloc(PC_COUNT, cache_miss_perf_counter_name)) { + _items = new Item[num_items] {}; + + if (_items != nullptr) { + _num_items = num_items; + + } else { + PX4_ERR("alloc failed"); + } +} + +DatamanCache::~DatamanCache() +{ + delete[] _items; + perf_free(_cache_miss_perf); +} + +void DatamanCache::resize(uint32_t num_items) +{ + Item *new_items = new Item[num_items] {}; + + if (new_items != nullptr) { + uint32_t num_min = num_items < _num_items ? num_items : _num_items; + + for (uint32_t i = 0; i < num_min; ++i) { + new_items[i] = _items[i]; + } + + delete[] _items; + _items = new_items; + _num_items = num_items; + + } else { + PX4_ERR("alloc failed"); + } +} + +bool DatamanCache::load(dm_item_t item, uint32_t index) +{ + if (!_items) { + return false; + } + + bool success = false; + + if (_item_counter < _num_items) { + + _items[_load_index].cache_state = State::RequestPrepared; + _items[_load_index].response.item = item; + _items[_load_index].response.index = index; + + _load_index = (_load_index + 1) % _num_items; + + ++_item_counter; + + success = true; + } + + return success; +} + +bool DatamanCache::loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + if (!_items) { + return false; + } + + bool success = false; + bool item_found = false; + + for (uint32_t i = 0; i < _num_items; ++i) { + if ((_items[i].response.item == item) && + (_items[i].response.index == index)) { + item_found = true; + + if (_items[i].cache_state == State::ResponseReceived) { + memcpy(buffer, _items[i].response.data, length); + success = true; + break; + } + } + } + + if (!success && (timeout > 0)) { + perf_count(_cache_miss_perf); + success = _client.readSync(item, index, buffer, length, timeout); + + // Cache the item if not found already (it could be in the process of being loaded) + if (success && !item_found && _item_counter < _num_items) { + _items[_load_index].cache_state = State::ResponseReceived; + _items[_load_index].response.item = item; + _items[_load_index].response.index = index; + memcpy(_items[_load_index].response.data, buffer, length); + + _load_index = (_load_index + 1) % _num_items; + + ++_item_counter; // Still increase the counter here + } + } + + return success; +} + +void DatamanCache::update() +{ + if (_item_counter > 0) { + + _client.update(); + + bool success = false; + bool response_success = false; + + switch (_items[_update_index].cache_state) { + case State::Idle: + break; + + case State::ResponseReceived: + // Skip it + changeUpdateIndex(); + break; + + case State::RequestPrepared: + + success = _client.readAsync(static_cast(_items[_update_index].response.item), + _items[_update_index].response.index, + _items[_update_index].response.data, g_per_item_size[_items[_update_index].response.item]); + + if (success) { + _items[_update_index].cache_state = State::RequestSent; + + } else { + _items[_update_index].cache_state = State::Error; + } + + break; + + case State::RequestSent: + + if (_client.lastOperationCompleted(response_success)) { + + if (response_success) { + + _items[_update_index].cache_state = State::ResponseReceived; + changeUpdateIndex(); + + } else { + _items[_update_index].cache_state = State::Error; + } + } + + break; + + case State::Error: + // Handled below + break; + } + + if (_items[_update_index].cache_state == State::Error) { + PX4_ERR("Caching: item %" PRIu8 ", index %" PRIu32", status %" PRIu8, + _items[_update_index].response.item, _items[_update_index].response.index, + _items[_update_index].response.status); + + changeUpdateIndex(); + } + + } +} + +void DatamanCache::invalidate() +{ + for (uint32_t i = 0; i < _num_items; ++i) { + _items[i].cache_state = State::Idle; + } + + _update_index = 0; + _item_counter = 0; + _load_index = 0; + _client.abortCurrentOperation(); +} + +inline void DatamanCache::changeUpdateIndex() +{ + _update_index = (_update_index + 1) % _num_items; + + if (_item_counter > 0) { + --_item_counter; + } } diff --git a/src/lib/dataman_client/DatamanClient.hpp b/src/lib/dataman_client/DatamanClient.hpp index 3e17764094..8d28097b10 100644 --- a/src/lib/dataman_client/DatamanClient.hpp +++ b/src/lib/dataman_client/DatamanClient.hpp @@ -80,12 +80,6 @@ private: ResponseReceived }; - enum class Status { - Idle, - RequestSent, - ResponseReceived - }; - struct Request { hrt_abstime timestamp; dm_function_t request_type; @@ -117,27 +111,54 @@ private: class DatamanCache { public: - DatamanCache(int num_items); - ~DatamanCache() = default; + DatamanCache(const char *cache_miss_perf_counter_name, uint32_t num_items); + ~DatamanCache(); - void resize(int num_items); + void resize(uint32_t num_items); void invalidate(); - bool load(dm_item_t item, unsigned index); + bool load(dm_item_t item, uint32_t index); - bool loadWait(dm_item_t item, unsigned index, uint8_t *buffer, hrt_abstime timeout); + bool loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 0); void update(); + /** + * @brief Function providing info if there items to be loaded to dataman cache. + * + * @return true if there are items to be processed. + */ + bool isLoading() {return (_item_counter > 0);} + DatamanClient &client() { return _client; } -private: - struct Item { - dataman_response_s data; - //State cache_state; - }; - Item *_items{nullptr}; + int size() const { return _num_items; } - DatamanClient _client; +private: + + enum class State { + Idle, + RequestPrepared, + RequestSent, + ResponseReceived, + Error + }; + + struct Item { + dataman_response_s response; + State cache_state; + }; + + inline void changeUpdateIndex(); + + Item *_items{nullptr}; + uint32_t _load_index{0}; ///< index for tracking last index used by load function + uint32_t _update_index{0}; ///< index for tracking last index used by update function + uint32_t _item_counter{0}; ///< number of items to process with update function + uint32_t _num_items{0}; + + DatamanClient _client{}; + + perf_counter_t _cache_miss_perf; }; diff --git a/src/systemcmds/tests/test_dataman.cpp b/src/systemcmds/tests/test_dataman.cpp index 56a1a8910d..5c73ab391b 100644 --- a/src/systemcmds/tests/test_dataman.cpp +++ b/src/systemcmds/tests/test_dataman.cpp @@ -96,6 +96,9 @@ private: bool testAsyncWriteReadAllItemsMaxSize(); bool testAsyncClearAll(); + //Cache + bool testCache(); + //The last test will reset the items so that FMU can boot without Dataman errors. bool testResetItems(); @@ -108,6 +111,9 @@ private: DatamanClient _dataman_client_thread2{}; DatamanClient _dataman_client_thread3{}; + uint32_t _cache_size = 10; + DatamanCache _dataman_cache{_cache_size}; + static void *testAsyncNoLocksThread(void *arg); static void *testAsyncWithLocksThread(void *arg); @@ -1339,6 +1345,166 @@ DatamanTest::testAsyncClearAll() return true; } +//Cache +bool +DatamanTest::testCache() +{ + bool success = false; + dm_item_t item = DM_KEY_WAYPOINTS_OFFBOARD_0; + uint32_t uniq_number = 13; // Use this to make sure stored data is from this test + + for (uint32_t index = 0; index < 15; ++index) { + uint8_t value = index + uniq_number; + memset(_buffer_write, value, sizeof(_buffer_write)); + success = _dataman_cache.client().writeSync(item, index, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + } + + // Write one extra for loadWait with timeout + uint32_t extra_index = 100; + _buffer_write[0] = 123; + success = _dataman_cache.client().writeSync(item, extra_index, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + + // Load cache + for (uint32_t index = 0; index < _cache_size; ++index) { + _dataman_cache.load(item, index); + } + + hrt_abstime start_time = hrt_absolute_time(); + + // loop represents the task, we collect the data + while (hrt_elapsed_time(&start_time) < 500_ms) { + + px4_usleep(1_ms); + _dataman_cache.update(); + } + + // check cached data + for (uint32_t index = 0; index < _cache_size; ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + PX4_ERR("Failed loadWait at index %" PRIu32, index); + return false; + } + + for (uint32_t i = 0; i < sizeof(_buffer_read); ++i) { + if (_buffer_read[i] != value) { + PX4_ERR("Wrong data recived %" PRIu8" , expected %" PRIu8, _buffer_read[i], value); + return false; + } + } + } + + // expected to fail without timeout set + success = _dataman_cache.loadWait(item, extra_index, _buffer_read, sizeof(_buffer_read)); + + if (success) { + PX4_ERR("loadWait unexpectedly succeeded"); + return false; + } + + // expected to success with timeout set + success = _dataman_cache.loadWait(item, extra_index, _buffer_read, sizeof(_buffer_read), 100_ms); + + if (!success) { + PX4_ERR("loadWait failed"); + return false; + } + + uint32_t old_cache_size = _cache_size; + _cache_size = 5; + _dataman_cache.resize(_cache_size); + + // check cached data after resize (reduced) + for (uint32_t index = 0; index < _cache_size; ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + PX4_ERR("Failed loadWait at index %" PRIu32, index); + return false; + } + + for (uint32_t i = 0; i < sizeof(_buffer_read); ++i) { + if (_buffer_read[i] != value) { + PX4_ERR("Wrong data recived %" PRIu8" , expected %" PRIu8, _buffer_read[i], value); + return false; + } + } + } + + for (uint32_t index = _cache_size; index < old_cache_size; ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + if (success) { + PX4_ERR("loadWait unexpectedly succeeded at index %" PRIu32, index); + return false; + } + } + + _cache_size = 15; + _dataman_cache.resize(_cache_size); + + // Load cache + for (uint32_t index = 0; index < _cache_size; ++index) { + _dataman_cache.load(item, index); + } + + start_time = hrt_absolute_time(); + + // loop represents the task, we collect the data + while (hrt_elapsed_time(&start_time) < 500_ms) { + + px4_usleep(1_ms); + _dataman_cache.update(); + } + + // check cached data + for (uint32_t index = 0; index < _cache_size; ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + PX4_ERR("Failed loadWait at index %" PRIu32, index); + return false; + } + + for (uint32_t i = 0; i < sizeof(_buffer_read); ++i) { + if (_buffer_read[i] != value) { + PX4_ERR("Wrong data recived %" PRIu8" , expected %" PRIu8, _buffer_read[i], value); + return false; + } + } + } + + // invalidate and check cached data + _dataman_cache.invalidate(); + + for (uint32_t index = 0; index < _cache_size; ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + // expected to fail + if (success) { + PX4_ERR("loadWait unexpectedly succeeded at index %" PRIu32, index); + return false; + } + + } + + return true; +} + bool DatamanTest::testResetItems() { @@ -1410,6 +1576,8 @@ bool DatamanTest::run_tests() ut_run_test(testAsyncWriteReadAllItemsMaxSize); ut_run_test(testAsyncClearAll); + ut_run_test(testCache); + ut_run_test(testResetItems); return (_tests_failed == 0);