pinesrcbin/pinesrcbin.c

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h>

#include <gst/gst.h>
#include <glib.h>

#include <linux/media.h>
#include <linux/videodev2.h>

#define GST_TYPE_PINESRCBIN	(gst_pinesrcbin_get_type())
#define GST_PINESRCBIN(obj) \
  (G_TYPE_CHECK_INSTANCE_CAST ((obj), GST_TYPE_PINESRCBIN, GstPineSrcBin))
#define GST_PINE64SRCBIN_CLASS(klass) \
  (G_TYPE_CHECK_CLASS_CAST ((klass), GST_TYPE_PINESRCBIN, GstPineSrcBinClass))
#define GST_IS_PINESRCBIN(obj) \
  (G_TYPE_CHECK_INSTANCE_TYPE ((obj), GST_TYPE_PINESRCBIN))
#define GST_IS_PINESRCBIN_CLASS(klass) \
  (G_TYPE_CHECK_CLASS_TYPE ((klass), GST_TYPE_PINESRCBIN))
#define GST_PINESRCBIN_GET_CLASS(obj) \
  (G_TYPE_INSTANCE_GET_CLASS ((obj), GST_TYPE_PINESRCBIN, GstPineSrcBinClass))
#define gst_pinesrcbin_parent_class parent_class
#define DEFAULT_SYNC                TRUE
enum
{
  PROP_0,
  PROP_CAPS,
  PROP_SYNC,
};

struct device_data;
typedef struct _GstPineSrcBin
{
  GstBin parent;

  GList *media_list;
  struct device_data *media_if;
  /* configuration for subclasses */
  const gchar *media_klass;     /* Audio/Video/... */
  GstElementFlags flag;         /* GST_ELEMENT_FLAG_{SINK/SOURCE} */

  /* explicit pointers to stuff used */
  GstPad *pad;
  GstCaps *filter_caps;
  gboolean sync;

  /* < private > */
  GstElement *kid;
  gboolean has_sync;
  const gchar *type_klass;      /* Source/Sink */
  const gchar *media_klass_lc, *type_klass_lc;  /* lower case versions */

  const struct media_v2_entity *v4l2entity;
  const struct media_v2_pad *v4l2pad;
  const struct media_v2_interface *v4l2interface;
  char devnode_path[260];
  int fd;
  gboolean autofocus;
  int gain_ctrl;
  int gain_max;
} GstPineSrcBin;

typedef struct _GstPineSrcBinClass
{
  GstBinClass parent_class;

  /*< private > */
  /* virtual methods for subclasses */
  void (*configure) (GstPineSrcBin * self, GstElement * kid);
  GstElement *(*create_fake_element) (GstPineSrcBin * bin);
} GstPineSrcBinClass;


G_DEFINE_TYPE (GstPineSrcBin, gst_pinesrcbin, GST_TYPE_BIN);

static int
xioctl (int fd, int request, void *arg)
{
  int r;
  do {
    r = ioctl (fd, request, arg);
  } while (r == -1 && errno == EINTR);
  return r;
}

static void
gst_pinesrcbin_clear_kid (GstPineSrcBin * self)
{
  if (self->kid) {
    gst_element_set_state (self->kid, GST_STATE_NULL);
    gst_bin_remove (GST_BIN (self), self->kid);
    self->kid = NULL;
  }
}

static GstElement *
gst_pinesrcbin_create_fake_element_default (GstPineSrcBin * self)
{
  GstElement *fake;
  gchar dummy_factory[10], dummy_name[20];

  sprintf (dummy_factory, "fake%s", self->type_klass_lc);
  sprintf (dummy_name, "fake-%s-%s", self->media_klass_lc, self->type_klass_lc);
  fake = gst_element_factory_make (dummy_factory, dummy_name);
  g_object_set (fake, "sync", self->sync, NULL);

  return fake;
}

static GstElement *
gst_pinesrcbin_create_fake_element (GstPineSrcBin * self)
{
  GstPineSrcBinClass *klass = GST_PINESRCBIN_GET_CLASS (self);
  GstElement *fake;

  if (klass->create_fake_element)
    fake = klass->create_fake_element (self);
  else
    fake = gst_pinesrcbin_create_fake_element_default (self);

  return fake;
}

static gboolean
gst_pinesrcbin_attach_ghost_pad (GstPineSrcBin * self)
{
  GstPad *target = gst_element_get_static_pad (self->kid, self->type_klass_lc);
  gboolean res = gst_ghost_pad_set_target (GST_GHOST_PAD (self->pad), target);
  gst_object_unref (target);

  return res;
}

static void
gst_pinesrcbin_reset (GstPineSrcBin * self)
{
  gst_pinesrcbin_clear_kid (self);

  /* placeholder element */
  self->kid = gst_pinesrcbin_create_fake_element (self);
  gst_bin_add (GST_BIN (self), self->kid);

  gst_pinesrcbin_attach_ghost_pad (self);
}

static GstStaticCaps raw_video_caps = GST_STATIC_CAPS ("video/x-raw");
static void
gst_pinesrcbin_constructed (GObject * object)
{
  GstPineSrcBin *self = GST_PINESRCBIN (object);
  if (G_OBJECT_CLASS (parent_class)->constructed)
    G_OBJECT_CLASS (parent_class)->constructed (object);
  self->type_klass = (self->flag == GST_ELEMENT_FLAG_SINK) ? "Sink" : "Source";
  self->filter_caps = gst_static_caps_get (&raw_video_caps);
  self->type_klass_lc = (self->flag == GST_ELEMENT_FLAG_SINK) ? "sink" : "src";
  self->media_klass_lc = "video";
  self->pad = gst_ghost_pad_new_no_target (self->type_klass_lc,
      (self->flag == GST_ELEMENT_FLAG_SINK) ? GST_PAD_SINK : GST_PAD_SRC);
  gst_element_add_pad (GST_ELEMENT (self), self->pad);
  gst_pinesrcbin_reset (self);
  GST_OBJECT_FLAG_SET (self, self->flag);
  gst_bin_set_suppressed_flags (GST_BIN (self),
      GST_ELEMENT_FLAG_SOURCE | GST_ELEMENT_FLAG_SINK);
}

static void
gst_pinesrcbin_dispose (GObject * object)
{
  GstPineSrcBin *self = GST_PINESRCBIN (object);

  gst_pinesrcbin_clear_kid (self);

  if (self->filter_caps)
    gst_caps_unref (self->filter_caps);
  self->filter_caps = NULL;

  G_OBJECT_CLASS (parent_class)->dispose ((GObject *) self);
}

static void
gst_pinesrcbin_set_property (GObject * object, guint prop_id,
    const GValue * value, GParamSpec * pspec)
{
  GstPineSrcBin *self = GST_PINESRCBIN (object);

  switch (prop_id) {
    case PROP_CAPS:
      if (self->filter_caps)
        gst_caps_unref (self->filter_caps);
      self->filter_caps = gst_caps_copy (gst_value_get_caps (value));
      break;
    case PROP_SYNC:
      self->sync = g_value_get_boolean (value);
      if (self->kid && self->has_sync)
        g_object_set_property (G_OBJECT (self->kid), pspec->name, value);
      break;
    default:
      G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
      break;
  }
}

static void
gst_pinesrcbin_get_property (GObject * object, guint prop_id,
    GValue * value, GParamSpec * pspec)
{
  GstPineSrcBin *self = GST_PINESRCBIN (object);

  switch (prop_id) {
    case PROP_CAPS:
      gst_value_set_caps (value, self->filter_caps);
      break;
    case PROP_SYNC:
      g_value_set_boolean (value, self->sync);
      break;
    default:
      G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
      break;
  }
}

static gboolean
gst_pinesrcbin_detect (GstPineSrcBin * self)
{
  GstElement *kid;
  GstPineSrcBinClass *klass = GST_PINESRCBIN_GET_CLASS (self);

  gst_pinesrcbin_clear_kid (self);

  kid = gst_element_factory_make ("v4l2src", NULL);
#if 0
  /* find element */
  GST_DEBUG_OBJECT (self, "Creating new kid");
  if (!(kid = gst_pinesrcbin_find_best (self)))
#endif
    if (!kid)
      goto no_sink;

  self->has_sync =
      g_object_class_find_property (G_OBJECT_GET_CLASS (kid), "sync") != NULL;
  if (self->has_sync)
    g_object_set (G_OBJECT (kid), "sync", self->sync, NULL);
  g_object_set (G_OBJECT (kid), "device", self->devnode_path, NULL);
  if (klass->configure) {
    klass->configure (self, kid);
  }

  self->kid = kid;

  gst_bin_add (GST_BIN (self), kid);

  /* Ensure the child is brought up to the right state to match the parent. */
  if (GST_STATE (self->kid) < GST_STATE (self))
    gst_element_set_state (self->kid, GST_STATE (self));

  /* attach ghost pad */
  GST_DEBUG_OBJECT (self, "Re-assigning ghostpad");
  if (!gst_pinesrcbin_attach_ghost_pad (self))
    goto target_failed;

  GST_DEBUG_OBJECT (self, "done changing auto %s %s", self->media_klass_lc,
      self->type_klass_lc);

  return TRUE;

  /* ERRORS */
no_sink:
  {
    GST_ELEMENT_ERROR (self, LIBRARY, INIT, (NULL),
        ("Failed to find a supported sink"));
    return FALSE;
  }
target_failed:
  {
    GST_ELEMENT_ERROR (self, LIBRARY, INIT, (NULL),
        ("Failed to set target pad"));
    return FALSE;
  }
}


static GstStateChangeReturn
gst_pinesrcbin_change_state (GstElement * element, GstStateChange transition)
{
  GstStateChangeReturn ret = GST_STATE_CHANGE_SUCCESS;
  GstPineSrcBin *sink = GST_PINESRCBIN (element);

  switch (transition) {
    case GST_STATE_CHANGE_NULL_TO_READY:
      if (!gst_pinesrcbin_detect (sink))
        return GST_STATE_CHANGE_FAILURE;
      break;
    default:
      break;
  }

  ret = GST_ELEMENT_CLASS (parent_class)->change_state (element, transition);
  if (ret == GST_STATE_CHANGE_FAILURE)
    return ret;

  switch (transition) {
    case GST_STATE_CHANGE_READY_TO_NULL:
      gst_pinesrcbin_reset (sink);
      break;
    default:
      break;
  }

  return ret;
}

static void
gst_pinesrcbin_class_init (GstPineSrcBinClass * klass)
{
  GObjectClass *gobject_class;
  GstElementClass *eklass;

  gobject_class = G_OBJECT_CLASS (klass);
  eklass = GST_ELEMENT_CLASS (klass);

  gobject_class->constructed = gst_pinesrcbin_constructed;
  gobject_class->dispose = gst_pinesrcbin_dispose;
  gobject_class->set_property = gst_pinesrcbin_set_property;
  gobject_class->get_property = gst_pinesrcbin_get_property;

  eklass->change_state = GST_DEBUG_FUNCPTR (gst_pinesrcbin_change_state);
}

GST_DEBUG_CATEGORY (pinesrcbin_debug);

struct device_data
{
  char path[261];
  int fd;
  struct media_v2_topology topology;
  struct media_device_info info;

  struct media_v2_entity *entities;
  size_t num_entities;
  struct media_v2_interface *interfaces;
  size_t num_interfaces;
  struct media_v2_pad *pads;
  size_t num_pads;
  struct media_v2_link *links;
  size_t num_links;
};
static const struct media_v2_link *
gst_pinesrcbin_get_link_from_entity (struct device_data *dev, uint32_t id)
{
  int i;
  for (i = 0; i < dev->num_links; i++)
    if (dev->links[i].sink_id == id)
      return &dev->links[i];
  return NULL;
}

static const struct media_v2_interface *
gst_pinesrcbin_get_interface_by_id (struct device_data *dev, uint32_t id)
{
  int i;
  for (i = 0; i < dev->num_interfaces; i++)
    if (dev->interfaces[i].id == id)
      return &dev->interfaces[i];
  return NULL;
}

static const struct media_v2_interface *
gst_pinesrcbin_get_entity_interface (struct device_data *dev, uint32_t id)
{
  const struct media_v2_link *link =
      gst_pinesrcbin_get_link_from_entity (dev, id);
  g_print ("entity id: %d\n", id);
  if (!link)
    return NULL;
  g_print ("link id: %d %d\n", link->sink_id, link->source_id);
  return gst_pinesrcbin_get_interface_by_id (dev, link->source_id);
}

static const struct media_v2_pad *
gst_pinesrcbin_get_pad_from_entity (struct device_data *dev, uint32_t id)
{
  int i;
  for (i = 0; i < dev->num_pads; i++) {
    if (dev->pads[i].entity_id == id)
      return &dev->pads[i];
  }
  return NULL;
}

static const struct media_v2_entity *
gst_pinesrcbin_find_media_entity (struct device_data *dev,
    const char *driver_name)
{
  int length = strlen (driver_name);
  int i;
  for (i = 0; i < dev->num_entities; i++)
    g_print ("%s: entity: %s:%s\n", dev->path, dev->entities[i].name,
        driver_name);
  for (i = 0; i < dev->num_entities; i++) {
    if (!strncmp (dev->entities[i].name, driver_name, length))
      return &dev->entities[i];
  }
  return NULL;
}

static struct device_data *
gst_pinesrcbin_find_media_device (GstPineSrcBin * self, const char *driver_name)
{
  int length = strlen (driver_name);
  GList *l = self->media_list;
  while (l) {
    struct device_data *dev = l->data;
    const struct media_device_info *info = &dev->info;
    g_print ("device %s:%s:%s\n", dev->path, info->driver, driver_name);
    if (!strncmp (info->driver, driver_name, length))
      return dev;
    l = l->next;
  }
  return NULL;
}

static gboolean
get_device_path (struct media_v2_intf_devnode devnode, char *path, int length)
{
  char uevent_path[256];
  snprintf (uevent_path, 256, "/sys/dev/char/%d:%d/uevent", devnode.major,
      devnode.minor);

  FILE *f = fopen (uevent_path, "r");
  if (!f) {
    return FALSE;
  }

  char line[512];
  while (fgets (line, 512, f)) {
    if (strncmp (line, "DEVNAME=", 8) == 0) {
      // Drop newline
      int length = strlen (line);
      if (line[length - 1] == '\n')
        line[length - 1] = '\0';

      snprintf (path, length, "/dev/%s", line + 8);
      fclose (f);
      return TRUE;
    }
  }

  fclose (f);

  return FALSE;
}

static gboolean
query_autofocus (int fd)
{
  struct v4l2_query_ext_ctrl ctrl = { };
  ctrl.id = V4L2_CID_FOCUS_AUTO;
  int rc = xioctl (fd, VIDIOC_QUERY_EXT_CTRL, &ctrl);
  if (rc < 0)
    return FALSE;
  return TRUE;
}

static void
setup_gain (int fd, int *gain_ctrl, int *gain_max)
{
  struct v4l2_query_ext_ctrl ctrl = { };
  ctrl.id = V4L2_CID_GAIN;
  int rc = xioctl (fd, VIDIOC_QUERY_EXT_CTRL, &ctrl);
  if (rc >= 0) {
    *gain_ctrl = ctrl.id;
    *gain_max = ctrl.maximum;
    return;
  }
  ctrl.id = V4L2_CID_ANALOGUE_GAIN;
  rc = xioctl (fd, VIDIOC_QUERY_EXT_CTRL, &ctrl);
  if (rc >= 0) {
    *gain_ctrl = ctrl.id;
    *gain_max = ctrl.maximum;
    return;
  }
}

static gboolean
camera_control (int fd, uint32_t id, int request, uint32_t * value)
{
  struct v4l2_ext_control ctrl = { };
  ctrl.id = id;
  ctrl.value = *value;

  struct v4l2_ext_controls ctrls = {
    .ctrl_class = 0,
    .which = V4L2_CTRL_WHICH_CUR_VAL,
    .count = 1,
    .controls = &ctrl,
  };
  if (xioctl (fd, request, &ctrls) == -1) {
    return TRUE;
  }

  *value = ctrl.value;
  return FALSE;
}

static inline const struct media_v2_pad *
get_device_pad (const struct device_data *dev, uint32_t id)
{
  int i;
  for (i = 0; i < dev->num_pads; i++) {
    if (dev->pads[i].id == id) {
      return &dev->pads[i];
    }
  }
  return NULL;
}

/* TODO - get media device pad and video device pad and set up link */
static gboolean
setup_link (const struct device_data *dev, uint32_t source_pad_id,
    uint32_t sink_pad_id, gboolean enabled)
{
  const struct media_v2_pad *source_pad = get_device_pad (dev, source_pad_id);
  g_return_val_if_fail (source_pad, FALSE);

  const struct media_v2_pad *sink_pad = get_device_pad (dev, sink_pad_id);
  g_return_val_if_fail (sink_pad, FALSE);

  struct media_link_desc link = { };
  link.flags = enabled ? MEDIA_LNK_FL_ENABLED : 0;
  link.source.entity = source_pad->entity_id;
  link.source.index = 0;
  link.sink.entity = sink_pad->entity_id;
  link.sink.index = 0;
  if (xioctl (dev->fd, MEDIA_IOC_SETUP_LINK, &link) == -1) {
    g_printerr ("MEDIA_IOC_SETUP_LINK: %d %s", errno, strerror (errno));
    return FALSE;
  }

  return TRUE;
}

static void
gst_pinesrcbin_init (GstPineSrcBin * self)
{
  self->media_list = NULL;
  struct dirent *dir;
  g_print ("starting device scan\n");
  DIR *d = opendir ("/dev");
  while ((dir = readdir (d)) != NULL) {
    if (strncmp (dir->d_name, "media", 5) == 0) {
      char path[261];
      int fd;
      snprintf (path, 261, "/dev/%s", dir->d_name);
      fd = open (path, O_RDWR);
      g_print ("opened device: %s: fd=%d\n", path, fd);
      if (fd >= 0) {
        int rc = -1;
        struct device_data *dev = g_new0 (struct device_data, 1);
        if (!dev)
          goto out;
        dev->fd = fd;
        strncpy (dev->path, path, sizeof (dev->path));
        g_print ("topology x1\n");
        rc = xioctl (dev->fd, MEDIA_IOC_G_TOPOLOGY, &dev->topology);
        if (rc < 0) {
          g_printerr ("topology x1 error %d %d %s\n", rc,
              errno, strerror (errno));
          goto out;
        }
        dev->entities =
            calloc (dev->topology.num_entities,
            sizeof (struct media_v2_entity));
        dev->num_entities = dev->topology.num_entities;
        dev->interfaces =
            calloc (dev->topology.num_interfaces,
            sizeof (struct media_v2_interface));
        dev->num_interfaces = dev->topology.num_interfaces;
        dev->pads =
            calloc (dev->topology.num_pads, sizeof (struct media_v2_pad));
        dev->num_pads = dev->topology.num_pads;
        dev->links =
            calloc (dev->topology.num_links, sizeof (struct media_v2_link));
        dev->num_links = dev->topology.num_links;
        dev->topology.ptr_entities = (uint64_t) dev->entities;
        dev->topology.ptr_interfaces = (uint64_t) dev->interfaces;
        dev->topology.ptr_pads = (uint64_t) dev->pads;
        dev->topology.ptr_links = (uint64_t) dev->links;
        g_print ("topology x2\n");
        rc = xioctl (dev->fd, MEDIA_IOC_G_TOPOLOGY, &dev->topology);
        if (rc < 0)
          goto out;

        g_print ("device info\n");
        rc = xioctl (dev->fd, MEDIA_IOC_DEVICE_INFO, &dev->info);
        if (rc < 0)
          goto out;
        self->media_list = g_list_append (self->media_list, dev);
        g_print ("added device %s %s\n", dev->path, dev->info.driver);
      out:
        if (rc < 0 && dev)
          g_free (dev);
      }
    }
  }
  self->media_if = gst_pinesrcbin_find_media_device (self, "sun6i-csi");
  if (!self->media_if)
    g_printerr ("can't find device sun6i-csi\n");
  self->v4l2entity =
      gst_pinesrcbin_find_media_entity (self->media_if, "sun6i-csi");
  if (!self->v4l2entity)
    g_printerr ("can't find entity sun6i-csi\n");
  /* This is pad for host controller */
  self->v4l2pad =
      gst_pinesrcbin_get_pad_from_entity (self->media_if, self->v4l2entity->id);
  if (!self->v4l2pad)
    g_printerr ("can't find interface pad sun6i-csi\n");
  self->v4l2interface =
      gst_pinesrcbin_get_entity_interface (self->media_if,
      self->v4l2entity->id);
  if (!self->v4l2interface)
    g_printerr ("can't find interface sun6i-csi\n");
  if (!get_device_path (self->v4l2interface->devnode,
          self->devnode_path, sizeof (self->devnode_path)))
    g_printerr ("Failed to get devnode path\n");
  g_print ("devnode: %s\n", self->devnode_path);
  /* TODO: source pad? sink pad? */
//  setup_link(self->media_if, ,self->v4l2interface->pad_id, false);
  self->fd = open (self->devnode_path, O_RDWR);
  if (self->fd < 0)
    g_printerr ("failed to open %s\n", self->devnode_path);
  self->autofocus = FALSE;
  if (query_autofocus (self->fd)) {
    uint32_t enable = 1U;
    g_print ("autofocus possible\n");
    self->autofocus = TRUE;
    camera_control (self->fd, V4L2_CID_FOCUS_AUTO, VIDIOC_S_EXT_CTRLS, &enable);
  }
  setup_gain (self->fd, &self->gain_ctrl, &self->gain_max);
  self->sync = DEFAULT_SYNC;
}

static gboolean
plugin_init (GstPlugin * plugin)
{
  GST_DEBUG_CATEGORY_INIT (pinesrcbin_debug, "pinesrcbin", 0,
      "pinephone v4lsrc camera wrapper");
  return gst_element_register (plugin, "pinesrcbin",
      GST_RANK_NONE, GST_TYPE_PINESRCBIN);
}

GST_PLUGIN_DEFINE (GST_VERSION_MAJOR, GST_VERSION_MINOR,
    pinesrcbin,
    "pinephone v4lsrc camera wrapper",
    plugin_init, "0.0", GST_LICENSE, "pinesrcbin", "omp")