datamanClient: add DatamanCache class

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<uint8_t>(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<dm_item_t>(_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;
+	}
 }

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;
 };

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);