示例#1
0
	int px4_open(const char *path, int flags, ...)
	{
		PX4_DEBUG("px4_open");
		VDev *dev = VDev::getDev(path);
		int ret = 0;
		int i;
		mode_t mode;

		if (!dev && (flags & (PX4_F_WRONLY | PX4_F_CREAT)) != 0 &&
		    strncmp(path, "/obj/", 5) != 0 &&
		    strncmp(path, "/dev/", 5) != 0) {
			va_list p;
			va_start(p, flags);
			mode = va_arg(p, mode_t);
			va_end(p);

			// Create the file
			PX4_DEBUG("Creating virtual file %s", path);
			dev = VFile::createFile(path, mode);
		}

		if (dev) {

			pthread_mutex_lock(&filemutex);

			for (i = 0; i < PX4_MAX_FD; ++i) {
				if (filemap[i] == 0) {
					filemap[i] = new device::file_t(flags, dev, i);
					break;
				}
			}

			pthread_mutex_unlock(&filemutex);

			if (i < PX4_MAX_FD) {
				ret = dev->open(filemap[i]);

			} else {
				PX4_WARN("exceeded maximum number of file descriptors!");
				ret = -ENOENT;
			}

		} else {
			ret = -EINVAL;
		}

		if (ret < 0) {
			px4_errno = -ret;
			return -1;
		}

		PX4_DEBUG("px4_open fd = %d", filemap[i]->fd);
		return filemap[i]->fd;
	}
示例#2
0
int px4_ioctl(int fd, int cmd, unsigned long arg)
{
	PX4_DEBUG("px4_ioctl fd = %d", fd);
	int ret = 0;
        if (valid_fd(fd)) {
		VDev *dev = (VDev *)(filemap[fd]->vdev);
		ret = dev->ioctl(filemap[fd], cmd, arg);
	}
	else { 
                ret = -EINVAL;
        }
	if (ret < 0) {
		px4_errno = -ret;
	}
	
        return (ret == 0) ? PX4_OK : PX4_ERROR;
}
示例#3
0
ssize_t px4_write(int fd, const void *buffer, size_t buflen)
{
	int ret;
        if (valid_fd(fd)) {
		VDev *dev = (VDev *)(filemap[fd]->vdev);
		PX4_DEBUG("px4_write fd = %d", fd);
		ret = dev->write(filemap[fd], (const char *)buffer, buflen);
	}
	else { 
                ret = -EINVAL;
        }
	if (ret < 0) {
		px4_errno = -ret;
		ret = PX4_ERROR;
	}
        return ret;
}
示例#4
0
int px4_close(int fd)
{
	int ret;
	if (valid_fd(fd)) {
		VDev *dev = (VDev *)(filemap[fd]->vdev);
		PX4_DEBUG("px4_close fd = %d", fd);
		ret = dev->close(filemap[fd]);
		filemap[fd] = NULL;
	}
	else { 
                ret = -EINVAL;
        }
	if (ret < 0) {
		px4_errno = -ret;
		ret = PX4_ERROR;
	}
	return ret;
}
示例#5
0
	int px4_ioctl(int fd, int cmd, unsigned long arg)
	{
		PX4_DEBUG("px4_ioctl fd = %d", fd);
		int ret = 0;

		VDev *dev = get_vdev(fd);

		if (dev) {
			ret = dev->ioctl(filemap[fd], cmd, arg);

		} else {
			ret = -EINVAL;
		}

		if (ret < 0) {
			px4_errno = -ret;
		}

		return ret;
	}
示例#6
0
	ssize_t px4_read(int fd, void *buffer, size_t buflen)
	{
		int ret;

		VDev *dev = get_vdev(fd);

		if (dev) {
			PX4_DEBUG("px4_read fd = %d", fd);
			ret = dev->read(filemap[fd], (char *)buffer, buflen);

		} else {
			ret = -EINVAL;
		}

		if (ret < 0) {
			px4_errno = -ret;
			ret = PX4_ERROR;
		}

		return ret;
	}
示例#7
0
	int px4_close(int fd)
	{
		int ret;

		VDev *dev = get_vdev(fd);

		if (dev) {
			pthread_mutex_lock(&filemutex);
			ret = dev->close(filemap[fd]);
			filemap[fd] = nullptr;
			pthread_mutex_unlock(&filemutex);
			PX4_DEBUG("px4_close fd = %d", fd);

		} else {
			ret = -EINVAL;
		}

		if (ret < 0) {
			px4_errno = -ret;
			ret = PX4_ERROR;
		}

		return ret;
	}
示例#8
0
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
{
	sem_t sem;
	int count = 0;
	int ret;
	unsigned int i;
	struct timespec ts;

	PX4_DEBUG("Called px4_poll timeout = %d", timeout);
	sem_init(&sem, 0, 0);

	// For each fd 
	for (i=0; i<nfds; ++i)
	{
		fds[i].sem     = &sem;
		fds[i].revents = 0;
		fds[i].priv    = NULL;

		// If fd is valid
		if (valid_fd(fds[i].fd))
		{
			VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
			PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd);
			ret = dev->poll(filemap[fds[i].fd], &fds[i], true);

			if (ret < 0)
				break;
		}
	}

	if (ret >= 0)
	{
		if (timeout >= 0)
		{
			pthread_t pt;
			void *res;

			ts.tv_sec = timeout/1000;
			ts.tv_nsec = (timeout % 1000)*1000000;

			// Create a timer to unblock
			struct timerData td(sem, ts);
			int rv = pthread_create(&pt, NULL, timer_handler, (void *)&td);
			if (rv != 0) {
				count = -1;
				goto cleanup;
			}
			sem_wait(&sem);

			// Make sure timer thread is killed before sem goes
			// out of scope
			(void)pthread_cancel(pt);
			(void)pthread_join(pt, &res);
        	}
		else
		{
			sem_wait(&sem);
		}

		// For each fd 
		for (i=0; i<nfds; ++i)
		{
			// If fd is valid
			if (valid_fd(fds[i].fd))
			{
				VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
				PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd);
				ret = dev->poll(filemap[fds[i].fd], &fds[i], false);
	
				if (ret < 0)
					break;

				if (fds[i].revents)
				count += 1;
			}
		}
	}

cleanup:
	sem_destroy(&sem);

	return count;
}
示例#9
0
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
{
	sem_t sem;
	int count = 0;
	int ret;
	unsigned int i;

	PX4_DEBUG("Called px4_poll timeout = %d", timeout);
	sem_init(&sem, 0, 0);

	// For each fd 
	for (i=0; i<nfds; ++i)
	{
		fds[i].sem     = &sem;
		fds[i].revents = 0;
		fds[i].priv    = NULL;

		// If fd is valid
		if (valid_fd(fds[i].fd))
		{
			VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
			PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd);
			ret = dev->poll(filemap[fds[i].fd], &fds[i], true);

			if (ret < 0)
				break;
		}
	}

	if (ret >= 0)
	{
		if (timeout >= 0)
		{
			// Use a work queue task
			work_s _hpwork;

			hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000*timeout);
			sem_wait(&sem);

			// Make sure timer thread is killed before sem goes
			// out of scope
			hrt_work_cancel(&_hpwork);
        	}
		else
		{
			sem_wait(&sem);
		}

		// For each fd 
		for (i=0; i<nfds; ++i)
		{
			// If fd is valid
			if (valid_fd(fds[i].fd))
			{
				VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
				PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd);
				ret = dev->poll(filemap[fds[i].fd], &fds[i], false);
	
				if (ret < 0)
					break;

				if (fds[i].revents)
				count += 1;
			}
		}
	}

	sem_destroy(&sem);

	return count;
}
示例#10
0
	int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
	{
		if (nfds == 0) {
			PX4_WARN("px4_poll with no fds");
			return -1;
		}

		px4_sem_t sem;
		int count = 0;
		int ret = -1;
		unsigned int i;

		const unsigned NAMELEN = 32;
		char thread_name[NAMELEN] = {};
		int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN);

		if (nret || thread_name[0] == 0) {
			PX4_WARN("failed getting thread name");
		}

		PX4_DEBUG("Called px4_poll timeout = %d", timeout);
		px4_sem_init(&sem, 0, 0);

		// Go through all fds and check them for a pollable state
		bool fd_pollable = false;

		for (i = 0; i < nfds; ++i) {
			fds[i].sem     = &sem;
			fds[i].revents = 0;
			fds[i].priv    = NULL;

			VDev *dev = get_vdev(fds[i].fd);

			// If fd is valid
			if (dev) {
				PX4_DEBUG("%s: px4_poll: VDev->poll(setup) %d", thread_name, fds[i].fd);
				ret = dev->poll(filemap[fds[i].fd], &fds[i], true);

				if (ret < 0) {
					PX4_WARN("%s: px4_poll() error: %s",
						 thread_name, strerror(errno));
					break;
				}

				if (ret >= 0) {
					fd_pollable = true;
				}
			}
		}

		// If any FD can be polled, lock the semaphore and
		// check for new data
		if (fd_pollable) {
			if (timeout > 0) {

				// Get the current time
				struct timespec ts;
				px4_clock_gettime(CLOCK_REALTIME, &ts);

				// Calculate an absolute time in the future
				const unsigned billion = (1000 * 1000 * 1000);
				unsigned tdiff = timeout;
				uint64_t nsecs = ts.tv_nsec + (tdiff * 1000 * 1000);
				ts.tv_sec += nsecs / billion;
				nsecs -= (nsecs / billion) * billion;
				ts.tv_nsec = nsecs;

				// Execute a blocking wait for that time in the future
				errno = 0;
				ret = px4_sem_timedwait(&sem, &ts);
#ifndef __PX4_DARWIN
				ret = errno;
#endif

				// Ensure ret is negative on failure
				if (ret > 0) {
					ret = -ret;
				}

				if (ret && ret != -ETIMEDOUT) {
					PX4_WARN("%s: px4_poll() sem error", thread_name);
				}

			} else if (timeout < 0) {
				px4_sem_wait(&sem);
			}

			// We have waited now (or not, depending on timeout),
			// go through all fds and count how many have data
			for (i = 0; i < nfds; ++i) {

				VDev *dev = get_vdev(fds[i].fd);

				// If fd is valid
				if (dev) {
					PX4_DEBUG("%s: px4_poll: VDev->poll(teardown) %d", thread_name, fds[i].fd);
					ret = dev->poll(filemap[fds[i].fd], &fds[i], false);

					if (ret < 0) {
						PX4_WARN("%s: px4_poll() 2nd poll fail", thread_name);
						break;
					}

					if (fds[i].revents) {
						count += 1;
					}
				}
			}
		}

		px4_sem_destroy(&sem);

		// Return the positive count if present,
		// return the negative error number if failed
		return (count) ? count : ret;
	}