posix: unit test for poll and vdev

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp

@@ -51,73 +51,126 @@ px4::AppState VCDevExample::appState;
 
 using namespace device;
 
-#define TESTDEV "/dev/vdevex"
+#define TESTDEV "/dev/vdevtest"
+
+static bool g_exit = false;
 
 static int writer_main(int argc, char *argv[])
 {
 	char buf[1];
 
-	int fd = px4_open(TESTDEV, PX4_F_RDONLY);
+	int fd = px4_open(TESTDEV, PX4_F_WRONLY);
 	if (fd < 0) {
-		PX4_INFO("--- Open failed %d %d", fd, px4_errno);
+		PX4_INFO("Writer: Open failed %d %d", fd, px4_errno);
 		return -px4_errno;
 	}
 
 	int ret;
 	int i=0;
-	while (i<3) {
+	while (!g_exit) {
-		// Wait for 4 seconds
+		// Wait for 2 seconds
-		PX4_INFO("Writer: Sleeping for 4 sec\n");
+		PX4_INFO("Writer: Sleeping for 2 sec");
-		ret = sleep(4);
+		ret = sleep(2);
 		if (ret < 0) {
-			PX4_INFO("Writer: sleep failed %d %d\n", ret, errno);
+			PX4_INFO("Writer: sleep failed %d %d", ret, errno);
 			return ret;
 		}
 
-		PX4_INFO("Writer: writing to fd\n");
 		buf[0] = 'A'+(char)(i % 26);
+		PX4_INFO("Writer: writing char '%c'", buf[0]);
 		ret = px4_write(fd, buf, 1);
 	++i;
 	}
 	px4_close(fd);
+	PX4_INFO("Writer: stopped");
 	return ret;
 }
 
+class PrivData {
+public:
+	PrivData() : _read_offset(0) {}
+	~PrivData() {}
+	
+	size_t _read_offset;
+};
+	
 class VCDevNode : public VDev {
 public:
-	VCDevNode() : VDev("vcdevtest", TESTDEV) {};
+	VCDevNode() : 
+		VDev("vcdevtest", TESTDEV), 
+		_is_open_for_write(false),
+		_write_offset(0) {};
 
 	~VCDevNode() {}
 
+	virtual int open(device::file_t *handlep);
+	virtual int close(device::file_t *handlep);
 	virtual ssize_t write(device::file_t *handlep, const char *buffer, size_t buflen);
-	virtual ssize_t read(device::file_t *handlep, const char *buffer, size_t buflen);
+	virtual ssize_t read(device::file_t *handlep, char *buffer, size_t buflen);
 private:
-	uint32_t _read_offset;
+	bool _is_open_for_write;
-	uint32_t _write_offset;
+	size_t _write_offset;
 	char     _buf[1000];
 };
 
+int VCDevNode::open(device::file_t *handlep)
+{
+	// Only allow one writer
+	if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) {
+		errno = EBUSY;
+		return -1;
+	}
+	int ret = VDev::open(handlep);
+	if (ret != 0) {
+		return ret;
+	}
+	handlep->priv = new PrivData;
+
+	if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) {
+		_is_open_for_write = true;
+	}
+	
+	return 0;
+}
+
+int VCDevNode::close(device::file_t *handlep)
+{
+	delete (PrivData *)handlep->priv;
+	handlep->priv = nullptr;
+	VDev::close(handlep);
+
+	// Enable a new writer of the device is re-opened for write
+	if ((handlep->flags & PX4_F_WRONLY) && _is_open_for_write) {
+		_is_open_for_write = false;
+	}
+	return 0;
+}
+
 ssize_t VCDevNode::write(device::file_t *handlep, const char *buffer, size_t buflen)
 {
-	// ignore what was written, but let pollers know something was written
+	for (size_t i=0; i<buflen && _write_offset<1000; i++) {
-	poll_notify(POLLIN);
-
-	for (int i=0; i<buflen && _write_offset<1000; i++) {
 		_buf[_write_offset] = buffer[i];
 		_write_offset++;
 	}
 
+	// ignore what was written, but let pollers know something was written
+	poll_notify(POLLIN);
+
 	return buflen;
 }
 
-ssize_t VCDevNode::read(device::file_t *handlep, const char *buffer, size_t buflen)
+ssize_t VCDevNode::read(device::file_t *handlep, char *buffer, size_t buflen)
 {
-	for (int i=0; i<buflen && _read_offset<_write_offset; i++) {
+	PrivData *p = (PrivData *)handlep->priv;
-		buffer[i] = _buf[_read_offset];
+	ssize_t chars_read = 0;
-		_read_offset++;
+	PX4_INFO("read %zu write %zu", p->_read_offset, _write_offset);
+	for (size_t i=0; i<buflen && (p->_read_offset < _write_offset); i++) {
+		buffer[i] = _buf[p->_read_offset];
+		p->_read_offset++;
+		chars_read++;
 	}
 	
-	return buflen;
+	return chars_read;
 }
 
 VCDevExample::~VCDevExample() {
@@ -140,11 +193,57 @@ static int test_pub_block(int fd, unsigned long blocked)
 		PX4_INFO("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno);
 		return -px4_errno;
 	}
-	PX4_INFO("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL");
+	PX4_INFO("pub_blocked = %d %s", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL");
 
 	return 0;
 }
 
+int VCDevExample::do_poll(int fd, int timeout, int iterations, int delayms_after_poll)
+{
+	int pollret, readret;
+	int loop_count = 0;
+	char readbuf[10];
+	px4_pollfd_struct_t fds[1];
+
+	fds[0].fd = fd;
+	fds[0].events = POLLIN;
+	fds[0].revents = 0;
+
+	bool mustblock = (timeout < 0);
+
+	// Test indefinte blocking poll
+	while ((!appState.exitRequested()) && (loop_count < iterations)) {
+		pollret = px4_poll(fds, 1, timeout);
+		if (pollret < 0) {
+			PX4_ERR("Reader: px4_poll failed %d %d FAIL", pollret, px4_errno);
+			goto fail;
+		} 
+		PX4_INFO("Reader: px4_poll returned %d", pollret);
+		if (pollret) {
+			readret = px4_read(fd, readbuf, 10);
+			if (readret != 1) {
+				if (mustblock) {
+					PX4_ERR("Reader:     read failed %d FAIL", readret);
+					goto fail;
+				}
+				else {
+					PX4_INFO("Reader:     read failed %d FAIL", readret);
+				}
+			}
+			else {
+				readbuf[readret] = '\0';
+				PX4_INFO("Reader: px4_poll     returned %d, read '%s' PASS", pollret, readbuf);
+			}
+		}
+		if (delayms_after_poll) {
+			usleep(delayms_after_poll*1000);
+		}
+		loop_count++;
+	}
+	return 0;
+fail:
+	return 1;
+}
 int VCDevExample::main()
 {
 	appState.setRunning(true);
@@ -152,12 +251,12 @@ int VCDevExample::main()
 	_node = new VCDevNode();
 
 	if (_node == 0) {
-		PX4_INFO("Failed to allocate VCDevNode\n");
+		PX4_INFO("Failed to allocate VCDevNode");
 		return -ENOMEM;
 	}
 
 	if (_node->init() != PX4_OK) {
-		PX4_INFO("Failed to init VCDevNode\n");
+		PX4_INFO("Failed to init VCDevNode");
 		return 1;
 	}
 
@@ -174,7 +273,7 @@ int VCDevExample::main()
 		PX4_INFO("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno);
 		return -px4_errno;
 	}
-	PX4_INFO("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL");
+	PX4_INFO("priv data = %p %s", p, p == (void *)_node ? "PASS" : "FAIL");
 
 	ret = test_pub_block(fd, 1);
 	if (ret < 0)
@@ -183,8 +282,6 @@ int VCDevExample::main()
 	if (ret < 0)
 		return ret;
 
-	int i=0;
-	px4_pollfd_struct_t fds[1];
 
 	// Start a task that will write something in 4 seconds
 	(void)px4_task_spawn_cmd("writer", 
@@ -194,71 +291,28 @@ int VCDevExample::main()
 				       writer_main,
 				       (char* const*)NULL);
 
-	while (!appState.exitRequested() && i<13) {
+	ret = 0;
-		PX4_INFO("=====================\n");
-		PX4_INFO("Reader: sleeping 2 sec\n");
-		sleep(2);
 
-		fds[0].fd = fd;
+	PX4_INFO("TEST: BLOCKING POLL ---------------");
-		fds[0].events = POLLIN;
+	if (do_poll(fd, -1, 3, 0)) {
-		fds[0].revents = 0;
+		ret = 1;
-
+		goto fail2;
-		PX4_INFO("Reader: Calling px4_poll with 1 sec wait\n");
-		ret = px4_poll(fds, 1, 1000);
-		PX4_INFO("Reader: Done px4_poll\n");
-		if (ret < 0) {
-			PX4_INFO("Reader: px4_poll failed %d %d\n", ret, px4_errno);
-			px4_close(fd);
-		} 
-		else if (i > 0) {
-			if (ret == 0) {
-				PX4_INFO("Reader: Nothing to read - PASS\n");
-			}
-			else {
-				PX4_INFO("Reader: poll returned %d\n", ret);
-			}
-		}
-		else if (i == 0) {
-			if (ret == 1) {
-				PX4_INFO("Reader: %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL");
-			}
-			else {
-				PX4_INFO("Reader: %d to read - FAIL\n", ret);
-			}
-		
-		}
-		++i;
 	}
-	i=0;
+	PX4_INFO("TEST: ZERO TIMEOUT POLL -----------");
-	while (!appState.exitRequested() && i<13) {
+	if(do_poll(fd, 0, 3, 0)) {
-		PX4_INFO("=====================\n");
+		ret = 1;
-		PX4_INFO("Reader:  sleeping 2 sec ====\n");
+		goto fail2;
-		sleep(2);
-
-		fds[0].fd = fd;
-		fds[0].events = POLLIN;
-		fds[0].revents = 0;
-
-		PX4_INFO("Reader: Calling px4_poll with 0 timeout\n");
-		ret = px4_poll(fds, 1, 0);
-		PX4_INFO("Reader: Done px4_poll\n");
-		if (ret < 0) {
-			PX4_INFO("Reader: px4_poll failed %d %d\n", ret, px4_errno);
-			px4_close(fd);
-		} 
-		if (ret == 0) {
-			PX4_INFO("Reader: Nothing to read - PASS\n");
-		}
-		if (ret == 1) {
-			PX4_INFO("Reader: %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL");
-		}
-		else {
-			PX4_INFO("Reader: %d to read - FAIL\n", ret);
-		}
-		
-		++i;
 	}
+	PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------");
+	if(do_poll(fd, 1000, 3, 0)) {
+		ret = 1;
+		goto fail2;
+	}
+
+fail2:
+	g_exit = true;
 	px4_close(fd);
-
+	PX4_INFO("TEST: waiting for writer to stop");
-	return 0;
+	sleep(3);
+	return ret;
 }

src/platforms/posix/tests/vcdev_test/vcdevtest_example.h

@@ -54,5 +54,7 @@ public:
 	static px4::AppState appState; /* track requests to terminate app */
 
 private:
+	int do_poll(int fd, int timeout, int iterations, int delayms_after_poll);
+
 	VCDevNode *_node;
 };