|
|
|
|
@ -51,54 +51,128 @@ px4::AppState VCDevExample::appState;
@@ -51,54 +51,128 @@ 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] = { '1' }; |
|
|
|
|
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) { |
|
|
|
|
// Wait for 3 seconds
|
|
|
|
|
PX4_INFO("--- Sleeping for 4 sec\n"); |
|
|
|
|
ret = sleep(4); |
|
|
|
|
while (!g_exit) { |
|
|
|
|
// Wait for 2 seconds
|
|
|
|
|
PX4_INFO("Writer: Sleeping for 2 sec"); |
|
|
|
|
ret = sleep(2); |
|
|
|
|
if (ret < 0) { |
|
|
|
|
PX4_INFO("--- sleep failed %d %d\n", ret, errno); |
|
|
|
|
PX4_INFO("Writer: sleep failed %d %d", ret, errno); |
|
|
|
|
return ret; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
PX4_INFO("--- 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, char *buffer, size_t buflen); |
|
|
|
|
private: |
|
|
|
|
bool _is_open_for_write; |
|
|
|
|
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) |
|
|
|
|
{ |
|
|
|
|
for (size_t 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, char *buffer, size_t buflen) |
|
|
|
|
{ |
|
|
|
|
PrivData *p = (PrivData *)handlep->priv; |
|
|
|
|
ssize_t chars_read = 0; |
|
|
|
|
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 chars_read; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
VCDevExample::~VCDevExample() { |
|
|
|
|
if (_node) { |
|
|
|
|
delete _node; |
|
|
|
|
@ -119,11 +193,57 @@ static int test_pub_block(int fd, unsigned long blocked)
@@ -119,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); |
|
|
|
|
@ -131,12 +251,12 @@ int VCDevExample::main()
@@ -131,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; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
@ -153,7 +273,7 @@ int VCDevExample::main()
@@ -153,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) |
|
|
|
|
@ -162,10 +282,8 @@ int VCDevExample::main()
@@ -162,10 +282,8 @@ 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 3 seconds
|
|
|
|
|
// Start a task that will write something in 4 seconds
|
|
|
|
|
(void)px4_task_spawn_cmd("writer",
|
|
|
|
|
SCHED_DEFAULT, |
|
|
|
|
SCHED_PRIORITY_MAX - 6, |
|
|
|
|
@ -173,42 +291,28 @@ int VCDevExample::main()
@@ -173,42 +291,28 @@ int VCDevExample::main()
|
|
|
|
|
writer_main, |
|
|
|
|
(char* const*)NULL); |
|
|
|
|
|
|
|
|
|
while (!appState.exitRequested() && i<13) { |
|
|
|
|
PX4_INFO("=====================\n"); |
|
|
|
|
PX4_INFO("==== sleeping 2 sec ====\n"); |
|
|
|
|
sleep(2); |
|
|
|
|
ret = 0; |
|
|
|
|
|
|
|
|
|
fds[0].fd = fd; |
|
|
|
|
fds[0].events = POLLIN; |
|
|
|
|
fds[0].revents = 0; |
|
|
|
|
|
|
|
|
|
PX4_INFO("==== Calling Poll\n"); |
|
|
|
|
ret = px4_poll(fds, 1, 1000); |
|
|
|
|
PX4_INFO("==== Done poll\n"); |
|
|
|
|
if (ret < 0) { |
|
|
|
|
PX4_INFO("==== poll failed %d %d\n", ret, px4_errno); |
|
|
|
|
px4_close(fd); |
|
|
|
|
}
|
|
|
|
|
else if (i > 0) { |
|
|
|
|
if (ret == 0) { |
|
|
|
|
PX4_INFO("==== Nothing to read - PASS\n"); |
|
|
|
|
} |
|
|
|
|
else { |
|
|
|
|
PX4_INFO("==== poll returned %d\n", ret); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
else if (i == 0) { |
|
|
|
|
if (ret == 1) { |
|
|
|
|
PX4_INFO("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); |
|
|
|
|
} |
|
|
|
|
else { |
|
|
|
|
PX4_INFO("==== %d to read - FAIL\n", ret); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
++i; |
|
|
|
|
PX4_INFO("TEST: BLOCKING POLL ---------------"); |
|
|
|
|
if (do_poll(fd, -1, 3, 0)) { |
|
|
|
|
ret = 1; |
|
|
|
|
goto fail2; |
|
|
|
|
} |
|
|
|
|
PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); |
|
|
|
|
if(do_poll(fd, 0, 3, 0)) { |
|
|
|
|
ret = 1; |
|
|
|
|
goto fail2; |
|
|
|
|
} |
|
|
|
|
PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------"); |
|
|
|
|
if(do_poll(fd, 1000, 3, 0)) { |
|
|
|
|
ret = 1; |
|
|
|
|
goto fail2; |
|
|
|
|
} |
|
|
|
|
px4_close(fd); |
|
|
|
|
|
|
|
|
|
return 0; |
|
|
|
|
fail2: |
|
|
|
|
g_exit = true; |
|
|
|
|
px4_close(fd); |
|
|
|
|
PX4_INFO("TEST: waiting for writer to stop"); |
|
|
|
|
sleep(3); |
|
|
|
|
return ret; |
|
|
|
|
} |
|
|
|
|
|
Lorenz Meier
10 years ago