copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/aos/atom_code/camera/Buffers.cpp b/aos/atom_code/camera/Buffers.cpp
new file mode 100644
index 0000000..22b7337
--- /dev/null
+++ b/aos/atom_code/camera/Buffers.cpp
@@ -0,0 +1,160 @@
+#include "Buffers.h"
+#include "V4L2.h"
+
+#include <sys/mman.h>
+
+namespace aos {
+namespace camera {
+
+// Represents an actual v4l2 buffer.
+struct Buffers::Buffer {
+ void *start;
+ size_t length; // for munmap
+};
+const std::string Buffers::kFDServerName("/tmp/aos_fd_server");
+const std::string Buffers::kQueueName("CameraBufferQueue");
+const aos_type_sig Buffers::kSignature{sizeof(Message), 971, 1};
+
+int Buffers::CreateSocket(int (*bind_connect)(int, const sockaddr *, socklen_t)) {
+ union af_unix_sockaddr {
+ sockaddr_un un;
+ sockaddr addr;
+ } addr;
+ const int r = socket(AF_UNIX, SOCK_STREAM, 0);
+ if (r == -1) {
+ LOG(ERROR, "socket(AF_UNIX, SOCK_STREAM, 0) failed with %d: %s\n",
+ errno, strerror(errno));
+ return -1;
+ }
+ addr.un.sun_family = AF_UNIX;
+ memset(addr.un.sun_path, 0, sizeof(addr.un.sun_path));
+ strcpy(addr.un.sun_path, kFDServerName.c_str());
+ if (bind_connect(r, &addr.addr, sizeof(addr.un)) == -1) {
+ LOG(ERROR, "bind_connect(=%p)(%d, %p, %zd) failed with %d: %s\n",
+ bind_connect, r, &addr.addr, sizeof(addr.un), errno, strerror(errno));
+ close(r); // what are we going to do about an error?
+ return -1;
+ }
+ return r;
+}
+
+void Buffers::MMap() {
+ buffers_ = new Buffer[kNumBuffers];
+ v4l2_buffer buf;
+ for (unsigned int n = 0; n < kNumBuffers; ++n) {
+ memset(&buf, 0, sizeof(buf));
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = n;
+ if (xioctl(fd_, VIDIOC_QUERYBUF, &buf) == -1) {
+ LOG(FATAL, "ioctl VIDIOC_QUERYBUF(%d, %p) failed with %d: %s\n",
+ fd_, &buf, errno, strerror(errno));
+ }
+ buffers_[n].length = buf.length;
+ buffers_[n].start = mmap(NULL, buf.length,
+ PROT_READ | PROT_WRITE, MAP_SHARED,
+ fd_, buf.m.offset);
+ if (buffers_[n].start == MAP_FAILED) {
+ LOG(FATAL, "mmap(NULL, %zd, PROT_READ | PROT_WRITE, MAP_SHARED, %d, %jd)"
+ " failed with %d: %s\n", buf.length, fd_, static_cast<intmax_t>(buf.m.offset),
+ errno, strerror(errno));
+ }
+ }
+}
+
+void Buffers::Release() {
+ if (message_ != NULL) {
+ aos_queue_free_msg(queue_, message_);
+ message_ = NULL;
+ }
+}
+const void *Buffers::GetNext(bool block,
+ uint32_t *bytesused, timeval *timestamp, uint32_t *sequence) {
+ Release();
+
+ // TODO(brians) make sure the camera reader process hasn't died
+ do {
+ if (block) {
+ message_ = static_cast<const Message *>(aos_queue_read_msg(queue_, PEEK | BLOCK));
+ } else {
+ static int index = 0;
+ message_ = static_cast<const Message *>(aos_queue_read_msg_index(queue_, BLOCK,
+ &index));
+ }
+ } while (block && message_ == NULL);
+ if (message_ != NULL) {
+ if (bytesused != NULL) memcpy(bytesused, &message_->bytesused, sizeof(*bytesused));
+ if (timestamp != NULL) memcpy(timestamp, &message_->timestamp, sizeof(*timestamp));
+ if (sequence != NULL) memcpy(sequence, &message_->sequence, sizeof(*sequence));
+ return buffers_[message_->index].start;
+ } else {
+ return NULL;
+ }
+}
+
+int Buffers::FetchFD() {
+ int myfds[Buffers::kNumFDs]; // where to retrieve the fds into
+ char buf[CMSG_SPACE(sizeof(myfds))]; // ancillary data buffer
+
+ iovec data;
+ memset(&data, 0, sizeof(data));
+ char dummy;
+ data.iov_base = &dummy;
+ data.iov_len = sizeof(dummy);
+ msghdr msg;
+ memset(&msg, 0, sizeof(msg));
+ msg.msg_iov = &data;
+ msg.msg_iovlen = 1;
+ msg.msg_control = buf;
+ msg.msg_controllen = sizeof(buf);
+
+ switch (recvmsg(server_, &msg, 0)) {
+ case 0: // "the peer has performed an orderly shutdown"
+ LOG(FATAL, "the fd server shut down (connected on %d)\n", server_);
+ case -1:
+ LOG(FATAL, "recvmsg(server_(=%d), %p, 0) failed with %d: %s\n",
+ server_, &msg, errno, strerror(errno));
+ }
+ const cmsghdr *const cmsg = CMSG_FIRSTHDR(&msg);
+ if (cmsg == NULL) {
+ LOG(FATAL, "no headers in message\n");
+ }
+ if (cmsg->cmsg_len != CMSG_LEN(sizeof(myfds))) {
+ LOG(FATAL, "got wrong size. got %d but expected %zd\n",
+ cmsg->cmsg_len, CMSG_LEN(sizeof(myfds)));
+ }
+ if (cmsg->cmsg_level != SOL_SOCKET) {
+ LOG(FATAL, "cmsg_level=%d. expected SOL_SOCKET(=%d)\n", cmsg->cmsg_level, SOL_SOCKET);
+ }
+ if (cmsg->cmsg_type != SCM_RIGHTS) {
+ LOG(FATAL, "cmsg_type=%d. expected SCM_RIGHTS(=%d)\n", cmsg->cmsg_type, SCM_RIGHTS);
+ }
+ memcpy(myfds, CMSG_DATA(cmsg), sizeof(myfds));
+
+ return myfds[0];
+}
+Buffers::Buffers() : server_(CreateSocket(connect)), fd_(FetchFD()), message_(NULL) {
+ MMap();
+ queue_ = aos_fetch_queue(kQueueName.c_str(), &kSignature);
+}
+
+Buffers::~Buffers() {
+ Release();
+
+ for (unsigned i = 0; i < kNumBuffers; ++i) {
+ if (munmap(buffers_[i].start, buffers_[i].length) == -1) {
+ LOG(WARNING, "munmap(%p, %zd) for destruction failed with %d: %s\n",
+ buffers_[i].start, buffers_[i].length, errno, strerror(errno));
+ }
+ }
+ delete[] buffers_;
+
+ if (close(fd_) == -1) {
+ LOG(WARNING, "close(%d) for destruction failed with %d: %s\n",
+ fd_, errno, strerror(errno));
+ }
+}
+
+} // namespace camera
+} // namespace aos
+