Add MediaDevice class to manage media pipelines
None of the devices are stable on the rockpi... So, in order to set
exposure and stream video and configure the pipeline, we need to
interact with the media API to discover and configure everything via
more stable names.
It was easiest to build up an in-memory representation of the
hierarchy and manipulate that than try to build up the various
operations we needed.
These operations are enough to configure the rockpi isp to stream images
and find the camera to open.
Change-Id: Ie7062fb04a21cda3619ee384db9d6c7f16009fed
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/vision/media_device.cc b/frc971/vision/media_device.cc
new file mode 100644
index 0000000..1813b6f
--- /dev/null
+++ b/frc971/vision/media_device.cc
@@ -0,0 +1,406 @@
+#include "frc971/vision/media_device.h"
+
+#include <fcntl.h>
+#include <linux/media.h>
+#include <linux/v4l2-subdev.h>
+#include <linux/videodev2.h>
+#include <sys/ioctl.h>
+
+#include <cstddef>
+#include <cstdint>
+#include <optional>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include "absl/strings/str_cat.h"
+#include "absl/strings/str_split.h"
+#include "aos/scoped/scoped_fd.h"
+#include "aos/util/file.h"
+#include "glog/logging.h"
+
+namespace frc971 {
+namespace vision {
+
+void Entity::Log() const {
+ LOG(INFO) << " { \"id\": " << id() << ",";
+ LOG(INFO) << " \"name\": \"" << name() << "\",";
+ LOG(INFO) << " \"function\": " << function() << ",";
+ LOG(INFO) << " \"interface_type\": " << interface_type() << ",";
+ LOG(INFO) << " \"major\": " << major() << ",";
+ LOG(INFO) << " \"minor\": " << minor() << ",";
+ if (has_interface_) {
+ LOG(INFO) << " \"device\": \"" << device() << "\",";
+ }
+ LOG(INFO) << " \"pads\": [";
+ for (const Pad *pad : pads_) {
+ pad->Log();
+ }
+ LOG(INFO) << " ]";
+ LOG(INFO) << " }";
+}
+
+void Entity::UpdateDevice() {
+ // There's a symlink in /sys/dev/char which gets us to the uevent file
+ // which has the DEVNAME variable set with the device name. This
+ // reliably gets us the name of the device.
+ const ::std::string contents = aos::util::ReadFileToStringOrDie(
+ absl::StrCat("/sys/dev/char/", major(), ":", minor(), "/uevent"));
+
+ // Strip it out and return it.
+ for (std::string_view line : absl::StrSplit(contents, "\n")) {
+ VLOG(1) << line;
+ if (line.size() > 8 && line.substr(0, 8) == "DEVNAME=") {
+ device_ = absl::StrCat("/dev/", line.substr(8, -1));
+ return;
+ }
+ }
+
+ LOG(FATAL) << "Failed to find DEVNAME in uevent file.";
+}
+
+std::optional<MediaDevice> MediaDevice::Initialize(int index) {
+ int fd = open(absl::StrCat("/dev/media", index).c_str(), O_RDWR);
+ std::optional<MediaDevice> result = std::nullopt;
+ if (fd >= 0) {
+ result.emplace(MediaDevice(fd));
+ }
+ return result;
+}
+
+void MediaDevice::Update() {
+ PCHECK(ioctl(fd_.get(), MEDIA_IOC_DEVICE_INFO, &device_info_) == 0);
+
+ struct media_v2_topology topology;
+ std::memset(&topology, 0, sizeof(topology));
+ PCHECK(ioctl(fd_.get(), MEDIA_IOC_G_TOPOLOGY, &topology) == 0);
+ VLOG(1) << "Got " << topology.num_entities << " entries";
+ VLOG(1) << "Got " << topology.num_interfaces << " interfaces";
+ VLOG(1) << "Got " << topology.num_pads << " pads";
+ VLOG(1) << "Got " << topology.num_links << " links";
+
+ std::vector<struct media_v2_entity> entities;
+ entities.resize(topology.num_entities);
+ topology.ptr_entities = reinterpret_cast<uint64_t>(entities.data());
+ std::vector<struct media_v2_interface> interfaces;
+ interfaces.resize(topology.num_interfaces);
+ topology.ptr_interfaces = reinterpret_cast<uint64_t>(interfaces.data());
+
+ std::vector<struct media_v2_pad> pads;
+ pads.resize(topology.num_pads);
+ topology.ptr_pads = reinterpret_cast<uint64_t>(pads.data());
+ std::vector<struct media_v2_link> links;
+ links.resize(topology.num_links);
+ topology.ptr_links = reinterpret_cast<uint64_t>(links.data());
+ PCHECK(ioctl(fd_.get(), MEDIA_IOC_G_TOPOLOGY, &topology) == 0);
+
+ entities_.reserve(entities.size());
+ for (const struct media_v2_entity &entity : entities) {
+ entities_.emplace_back();
+ entities_.back().entity_ = entity;
+ }
+
+ pads_.reserve(pads.size());
+ for (const struct media_v2_pad &pad : pads) {
+ Entity *found_entity = nullptr;
+ for (Entity &entity : entities_) {
+ if (entity.id() == pad.entity_id) {
+ found_entity = &entity;
+ break;
+ }
+ }
+ CHECK(found_entity != nullptr);
+ pads_.emplace_back();
+ pads_.back().id_ = pad.id;
+ pads_.back().flags_ = pad.flags;
+ pads_.back().entity_ = found_entity;
+
+ found_entity->pads_.emplace_back(&pads_.back());
+ pads_.back().index_ = found_entity->pads_.size() - 1u;
+ }
+
+ links_.reserve(links.size());
+
+ for (const struct media_v2_link &link : links) {
+ VLOG(1) << "Link " << link.id << " from " << link.source_id << " to "
+ << link.sink_id;
+ if ((link.flags & MEDIA_LNK_FL_LINK_TYPE) == MEDIA_LNK_FL_INTERFACE_LINK) {
+ const struct media_v2_interface *found_interface = nullptr;
+ for (const struct media_v2_interface &interface : interfaces) {
+ if (interface.id == link.source_id) {
+ found_interface = &interface;
+ break;
+ }
+ }
+ CHECK(found_interface != nullptr) << ": Failed to find interface";
+ bool found = false;
+ for (Entity &entity : entities_) {
+ if (entity.id() == link.sink_id) {
+ found = true;
+ VLOG(1) << "Added interface to " << entity.name();
+ entity.has_interface_ = true;
+ entity.interface_ = *found_interface;
+ entity.UpdateDevice();
+ break;
+ }
+ }
+ CHECK(found);
+
+ } else if ((link.flags & MEDIA_LNK_FL_LINK_TYPE) ==
+ MEDIA_LNK_FL_DATA_LINK) {
+ links_.emplace_back();
+ links_.back().flags_ = link.flags;
+ links_.back().id_ = link.id;
+
+ Pad *found_source_pad = nullptr;
+ Pad *found_sink_pad = nullptr;
+ for (Pad &pad : pads_) {
+ if (pad.id() == link.source_id) {
+ found_source_pad = &pad;
+ } else if (pad.id() == link.sink_id) {
+ found_sink_pad = &pad;
+ }
+ }
+ CHECK(found_source_pad != nullptr);
+ CHECK(found_sink_pad != nullptr);
+
+ links_.back().source_ = found_source_pad;
+ links_.back().sink_ = found_sink_pad;
+ links_.back().id_ = link.id;
+ found_source_pad->links_.push_back(&links_.back());
+ found_sink_pad->links_.push_back(&links_.back());
+ } else {
+ LOG(FATAL) << "Unknown link type " << link.flags;
+ }
+ }
+}
+
+void MediaDevice::Log() const {
+ LOG(INFO) << "{\"driver\": \"" << driver() << "\",";
+ LOG(INFO) << " \"model\": \"" << model() << "\",";
+ LOG(INFO) << " \"serial\": \"" << serial() << "\",";
+ LOG(INFO) << " \"bus_info\": \"" << bus_info() << "\",";
+ LOG(INFO) << " \"entities\": [";
+ for (const Entity &entity : entities_) {
+ entity.Log();
+ }
+ LOG(INFO) << "] }";
+}
+
+void Pad::Log() const {
+ LOG(INFO) << " {\"id\": " << id() << ",";
+ LOG(INFO) << " \"index\": " << index() << ",";
+ LOG(INFO) << " \"type\": \"" << (source() ? "source" : "sink") << "\"";
+ LOG(INFO) << " \"links\": [";
+ for (size_t i = 0; i < links_size(); ++i) {
+ LOG(INFO) << " {";
+ if (source()) {
+ LOG(INFO) << " \"sink\": \"" << links(i)->sink()->entity()->name()
+ << "\",";
+ LOG(INFO) << " \"sink_index\": \"" << links(i)->sink()->index()
+ << "\",";
+ } else {
+ LOG(INFO) << " \"source\": \""
+ << links(i)->source()->entity()->name() << "\",";
+ LOG(INFO) << " \"source_index\": \"" << links(i)->source()->index()
+ << "\",";
+ }
+ LOG(INFO) << " \"enabled\": " << links(i)->enabled() << ",";
+ LOG(INFO) << " \"immutable\": " << links(i)->immutable() << ",";
+ LOG(INFO) << " }";
+ }
+ LOG(INFO) << " ],";
+ LOG(INFO) << " }";
+}
+
+void Pad::SetSubdevCrop(uint32_t width, uint32_t height) {
+ int fd = open(entity()->device().c_str(), O_RDWR);
+ PCHECK(fd >= 0);
+
+ struct v4l2_subdev_selection selection;
+ std::memset(&selection, 0, sizeof(selection));
+ selection.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+ selection.pad = index();
+
+ PCHECK(ioctl(fd, VIDIOC_SUBDEV_G_SELECTION, &selection) == 0)
+ << ": Failed to set " << entity()->device();
+
+ selection.target = V4L2_SEL_TGT_CROP;
+ selection.r.left = 0;
+ selection.r.top = 0;
+ selection.r.width = width;
+ selection.r.height = height;
+
+ PCHECK(ioctl(fd, VIDIOC_SUBDEV_S_SELECTION, &selection) == 0);
+ LOG(INFO) << "Setting " << entity()->name() << " pad " << index()
+ << " crop to (0, 0) " << width << "x" << height;
+}
+void Pad::SetSubdevFormat(uint32_t width, uint32_t height, uint32_t code) {
+ VLOG(1) << "Opening " << entity()->device();
+ int fd = open(entity()->device().c_str(), O_RDWR);
+ PCHECK(fd >= 0);
+
+ struct v4l2_subdev_format format;
+ std::memset(&format, 0, sizeof(format));
+ format.pad = index();
+ format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+ PCHECK(ioctl(fd, VIDIOC_SUBDEV_G_FMT, &format) == 0);
+
+ VLOG(1) << format.format.width << ", " << format.format.height << ", "
+ << format.format.code << " field " << format.format.field
+ << " colorspace " << format.format.colorspace << " ycbcr_enc "
+ << format.format.ycbcr_enc << " quantization "
+ << format.format.quantization << " xfer_func "
+ << format.format.xfer_func;
+
+ format.format.width = width;
+ format.format.height = height;
+ format.format.code = code;
+ format.format.field = V4L2_FIELD_NONE;
+ format.format.colorspace = V4L2_COLORSPACE_SRGB;
+ format.format.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+ format.format.quantization = V4L2_QUANTIZATION_DEFAULT;
+ format.format.xfer_func = V4L2_XFER_FUNC_DEFAULT;
+
+ LOG(INFO) << "Setting " << entity()->name() << " pad " << index() << " format to "
+ << width << "x" << height << " code 0x" << std::hex << code;
+
+ PCHECK(ioctl(fd, VIDIOC_SUBDEV_S_FMT, &format) == 0);
+
+ PCHECK(close(fd) == 0);
+}
+
+void Entity::SetFormat(uint32_t width, uint32_t height, uint32_t code) {
+ VLOG(1) << "Opening " << device();
+ int fd = open(device().c_str(), O_RDWR);
+ PCHECK(fd >= 0);
+
+ struct v4l2_format format;
+ std::memset(&format, 0, sizeof(format));
+ format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+
+ PCHECK(ioctl(fd, VIDIOC_G_FMT, &format) == 0);
+
+ VLOG(1) << "width " << format.fmt.pix_mp.width;
+ VLOG(1) << "height " << format.fmt.pix_mp.height;
+ VLOG(1) << "pixelformat " << format.fmt.pix_mp.pixelformat;
+ VLOG(1) << "field " << format.fmt.pix_mp.field;
+ VLOG(1) << "colorspace " << format.fmt.pix_mp.colorspace;
+ VLOG(1) << " sizeimage " << format.fmt.pix_mp.plane_fmt[0].sizeimage;
+ VLOG(1) << " bytesperline " << format.fmt.pix_mp.plane_fmt[0].bytesperline;
+ VLOG(1) << "num_planes "
+ << static_cast<uint64_t>(format.fmt.pix_mp.num_planes);
+ VLOG(1) << "flags " << static_cast<uint64_t>(format.fmt.pix_mp.flags);
+ VLOG(1) << "ycbcr_enc " << static_cast<uint64_t>(format.fmt.pix_mp.ycbcr_enc);
+ VLOG(1) << "quantization "
+ << static_cast<uint64_t>(format.fmt.pix_mp.quantization);
+ VLOG(1) << "xfer_func " << static_cast<uint64_t>(format.fmt.pix_mp.xfer_func);
+
+ format.fmt.pix_mp.width = width;
+ format.fmt.pix_mp.height = height;
+ format.fmt.pix_mp.pixelformat = code;
+ format.fmt.pix_mp.field = V4L2_FIELD_NONE;
+ format.fmt.pix_mp.colorspace = V4L2_COLORSPACE_DEFAULT;
+ format.fmt.pix_mp.num_planes = 1;
+
+ // TODO(austin): This is probably V4L2_PIX_FMT_YUV422P specific... We really
+ // want to extract bytes/pixel.
+ CHECK((code == V4L2_PIX_FMT_YUV422P) || (code == V4L2_PIX_FMT_YUYV));
+ format.fmt.pix_mp.plane_fmt[0].sizeimage = width * height * 2;
+ format.fmt.pix_mp.plane_fmt[0].bytesperline = width;
+
+ format.fmt.pix_mp.flags = 0;
+ format.fmt.pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+ format.fmt.pix_mp.quantization = V4L2_QUANTIZATION_DEFAULT;
+ format.fmt.pix_mp.xfer_func = V4L2_XFER_FUNC_DEFAULT;
+
+ LOG(INFO) << "Setting " << name() << " to " << width << "x" << height
+ << " code 0x" << std::hex << code;
+ PCHECK(ioctl(fd, VIDIOC_S_FMT, &format) == 0);
+
+ PCHECK(close(fd) == 0);
+}
+
+void MediaDevice::Reset(Link *link) {
+ LOG(INFO) << "Disabling link " << link->source()->entity()->name() << " -> "
+ << link->sink()->entity()->name();
+ struct media_link_desc link_desc;
+ link_desc.source.entity = link->source()->entity()->id();
+ link_desc.source.index = link->source()->index();
+ link_desc.source.flags = 0;
+ link_desc.sink.entity = link->sink()->entity()->id();
+ link_desc.sink.index = link->sink()->index();
+ link_desc.sink.flags = 0;
+ link_desc.flags = link->flags() & (~MEDIA_LNK_FL_ENABLED);
+ PCHECK(ioctl(fd_.get(), MEDIA_IOC_SETUP_LINK, &link_desc) == 0);
+
+ link->flags_ = link_desc.flags;
+}
+
+void MediaDevice::Enable(Link *link) {
+ LOG(INFO) << "Enabling link " << link->source()->entity()->name() << " -> "
+ << link->sink()->entity()->name();
+ struct media_link_desc link_desc;
+ link_desc.source.entity = link->source()->entity()->id();
+ link_desc.source.index = link->source()->index();
+ link_desc.source.flags = 0;
+ link_desc.sink.entity = link->sink()->entity()->id();
+ link_desc.sink.index = link->sink()->index();
+ link_desc.sink.flags = 0;
+ link_desc.flags = link->flags() | MEDIA_LNK_FL_ENABLED;
+ PCHECK(ioctl(fd_.get(), MEDIA_IOC_SETUP_LINK, &link_desc) == 0);
+
+ link->flags_ = link_desc.flags;
+}
+
+Entity *MediaDevice::FindEntity(std::string_view entity_name) {
+ for (Entity &entity : entities_) {
+ if (entity.name() == entity_name) {
+ return &entity;
+ }
+ }
+ return nullptr;
+}
+
+Link *MediaDevice::FindLink(std::string_view source, int source_pad_index,
+ std::string_view sink, int sink_pad_index) {
+ Entity *source_entity = CHECK_NOTNULL(FindEntity(source));
+ Entity *sink_entity = CHECK_NOTNULL(FindEntity(sink));
+ Pad *source_pad = source_entity->pads()[source_pad_index];
+ Pad *sink_pad = sink_entity->pads()[sink_pad_index];
+ for (size_t i = 0; i < source_pad->links_size(); ++i) {
+ if (source_pad->links(i)->sink() == sink_pad) {
+ return source_pad->links(i);
+ }
+ }
+ LOG(FATAL) << "Failed to find link between " << source << " pad "
+ << source_pad_index << " and " << sink << " pad "
+ << sink_pad_index;
+}
+
+void MediaDevice::Reset() {
+ LOG(INFO) << "Resetting " << bus_info();
+
+ for (Link &link : *links()) {
+ if (!link.immutable()) {
+ Reset(&link);
+ }
+ }
+}
+
+std::optional<MediaDevice> FindMediaDevice(std::string_view device) {
+ for (int media_device_index = 0;; ++media_device_index) {
+ std::optional<MediaDevice> media_device =
+ MediaDevice::Initialize(media_device_index);
+ if (!media_device) {
+ return std::nullopt;
+ }
+ if (media_device->bus_info() == device) {
+ return media_device;
+ }
+ }
+}
+
+} // namespace vision
+} // namespace frc971