src/vision_source_v4l2.cpp

   1 /*
   2  * vision_source_v4l2.c
   3  *
   4  * Copyright (c) 2022 - 2023 Samsung Electronics Co., Ltd. All rights reserved.
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the "License");
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  * http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an "AS IS" BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  *
  18  * Most of code is from hal_camera_v4l2
  19  */
  20
  21 #include <algorithm>
  22 #include <atomic>
  23 #include <cstdint>
  24 #include <string>
  25 #include <thread>
  26 #include <vector>
  27
  28 #include <dlog.h>
  29 #include <fcntl.h>
  30 #include <glob.h>
  31 #include <linux/videodev2.h>
  32 #include <media_packet.h>
  33 #include <stdlib.h>
  34 #include <sys/mman.h>
  35 #include <vision_source_interface.h>
  36
  37 #ifdef LOG_TAG
  38 #undef LOG_TAG
  39 #endif
  40
  41 #define LOG_TAG "VISION_SOURCE_V4L2"
  42 #ifdef HAVE_LIBV4L2
  43 #include <libv4l2.h>
  44 #else
  45 #include <sys/ioctl.h>
  46 #include <unistd.h>
  47
  48 #define v4l2_fd_open(fd, flags) (fd)
  49 #define v4l2_close close
  50 #define v4l2_dup dup
  51 #define v4l2_ioctl ioctl
  52 #define v4l2_read read
  53 #define v4l2_mmap mmap
  54 #define v4l2_munmap munmap
  55 #endif /* ENABLE_LIBV4L2 */
  56
  57 #define VISION_SOURCE_CHECK_CONDITION(condition, error, msg)   \
  58         do {                                                       \
  59                 if (!(condition)) {                                    \
  60                         LOGE("[%s] %s(0x%08x)", __FUNCTION__, msg, error); \
  61                         return error;                                      \
  62                 }                                                      \
  63         } while (0)
  64
  65 #define VISION_SOURCE_NULL_ARG_CHECK(arg)                                             \
  66         VISION_SOURCE_CHECK_CONDITION(arg != NULL, VISION_SOURCE_ERROR_INVALID_PARAMETER, \
  67                                                                   "VISION_SOURCE_ERROR_INVALID_PARAMETER")
  68
  69 #define VISION_SOURCE_INITIAL_FD -1
  70 #define BUFFER_MAX 4
  71
  72 #define DEVICE_NODE_PATH_PREFIX "/dev/video"
  73 #define FOURCC_FORMAT "%c%c%c%c"
  74 #define FOURCC_CONVERT(fourcc) fourcc & 0xff, (fourcc >> 8) & 0xff, (fourcc >> 16) & 0xff, (fourcc >> 24) & 0xff
  75
  76 #ifdef USE_DL
  77 #define vision_source_init vision_source_v4l2_init
  78 #define vision_source_exit vision_source_v4l2_exit
  79 #define vision_source_list_devices vision_source_v4l2_list_devices
  80 #define vision_source_list_device_caps vision_source_v4l2_list_device_caps
  81 #define vision_source_open_device vision_source_v4l2_open_device
  82 #define vision_source_close_device vision_source_v4l2_close_device
  83 #define vision_source_set_stream_format vision_source_v4l2_set_stream_format
  84 #define vision_source_start_stream vision_source_v4l2_start_stream
  85 #define vision_source_stop_stream vision_source_v4l2_stop_stream
  86 #define vision_source_set_stream_cb vision_source_v4l2_set_stream_cb
  87 #else
  88 #include <vision_source.h>
  89 #endif
  90
  91 using namespace std;
  92
  93 struct fmt_info {
  94         media_format_mimetype_e type;
  95         uint32_t width;
  96         uint32_t height;
  97         uint32_t fps;
  98
  99         bool operator==(const fmt_info &rhs)
 100         {
 101                 return (type == rhs.type) && (width == rhs.width) && (height == rhs.height) && (fps == rhs.fps);
 102         }
 103 };
 104
 105 struct pkt_dispose_cb_data {
 106         int fd;
 107         int index;
 108 };
 109
 110 struct vision_source_v4l2_s {
 111         /* device */
 112         vision_source_device_info_s *dev_info {};
 113         media_format_h **fmt {};
 114         vector<string> dev_name {};
 115         vector<vector<fmt_info> > fmt_list {};
 116
 117         /* current user setting */
 118         int device_index = VISION_SOURCE_INITIAL_FD;
 119         int device_fd = VISION_SOURCE_INITIAL_FD;
 120         int fmt_index = VISION_SOURCE_INITIAL_FD;
 121         vision_source_status_e status;
 122
 123         /* buffer */
 124         vector<v4l2_buffer> v4l2_buf {};
 125         vector<void *> mmap_addr {};
 126         struct pkt_dispose_cb_data pkt_data[BUFFER_MAX];
 127
 128         /* thead */
 129         thread buffer_thread;
 130         atomic_bool buffer_thread_run {};
 131         stream_cb stream_callback {};
 132         void *stream_user_data {};
 133 };
 134
 135 static int __vision_source_get_format(uint32_t fourcc, int *pixel_format)
 136 {
 137         switch (fourcc) {
 138         case V4L2_PIX_FMT_RGB24:
 139                 *pixel_format = MEDIA_FORMAT_RGB888;
 140                 break;
 141         case V4L2_PIX_FMT_YUYV:
 142                 *pixel_format = MEDIA_FORMAT_YUYV;
 143                 break;
 144         case V4L2_PIX_FMT_NV12:
 145                 *pixel_format = MEDIA_FORMAT_NV12;
 146                 break;
 147         case V4L2_PIX_FMT_NV21:
 148                 *pixel_format = MEDIA_FORMAT_NV21;
 149                 break;
 150         default:
 151                 LOGW("unknown fourcc " FOURCC_FORMAT, FOURCC_CONVERT(fourcc));
 152                 return VISION_SOURCE_ERROR_NOT_SUPPORTED_FORMAT;
 153         }
 154
 155         LOGD("fourcc " FOURCC_FORMAT " -> %d", FOURCC_CONVERT(fourcc), *pixel_format);
 156
 157         return VISION_SOURCE_ERROR_NONE;
 158 }
 159
 160 static int __vision_source_get_fourcc_plane_num(media_format_mimetype_e pixel_format, uint32_t *fourcc)
 161 {
 162         switch (pixel_format) {
 163         case MEDIA_FORMAT_RGB888:
 164                 *fourcc = V4L2_PIX_FMT_RGB24;
 165                 break;
 166         case MEDIA_FORMAT_YUYV:
 167                 *fourcc = V4L2_PIX_FMT_YUYV;
 168                 break;
 169         case MEDIA_FORMAT_NV12:
 170                 *fourcc = V4L2_PIX_FMT_NV12;
 171                 break;
 172         case MEDIA_FORMAT_NV21:
 173                 *fourcc = V4L2_PIX_FMT_NV21;
 174                 break;
 175         default:
 176                 LOGE("unknown format %d", pixel_format);
 177                 return VISION_SOURCE_ERROR_INTERNAL;
 178         }
 179
 180         LOGD("format %d -> fourcc " FOURCC_FORMAT, pixel_format, FOURCC_CONVERT(*fourcc));
 181
 182         return VISION_SOURCE_ERROR_NONE;
 183 }
 184
 185 static vector<uint32_t> __vision_source_get_fps_list(int device_fd, uint32_t pixel_format, int width, int height)
 186 {
 187         vector<uint32_t> fps_list;
 188         struct v4l2_frmivalenum ival;
 189
 190         ival.index = 0;
 191         ival.pixel_format = pixel_format;
 192         ival.width = width;
 193         ival.height = height;
 194
 195         if (v4l2_ioctl(device_fd, VIDIOC_ENUM_FRAMEINTERVALS, &ival) < 0) {
 196                 LOGE("VIDIOC_ENUM_FRAMEINTERVALS failed[%d]", errno);
 197                 return fps_list;
 198         }
 199
 200         if (ival.type != V4L2_FRMIVAL_TYPE_DISCRETE) {
 201                 LOGE("NOT V4L2_FRMIVAL_TYPE_DISCRETE -> [%u]", ival.type);
 202                 return fps_list;
 203         }
 204
 205         do {
 206                 LOGI("\t\t\t\tFramerate[%u/%u]", ival.discrete.denominator, ival.discrete.numerator);
 207                 fps_list.push_back(ival.discrete.denominator);
 208                 ival.index++;
 209         } while (v4l2_ioctl(device_fd, VIDIOC_ENUM_FRAMEINTERVALS, &ival) >= 0);
 210
 211         return fps_list;
 212 }
 213
 214 static int __vision_source_v4l2_qbuf(int device_fd, int type, int memory, int index)
 215 {
 216         struct v4l2_buffer v4l2_buf;
 217
 218         memset(&v4l2_buf, 0x0, sizeof(struct v4l2_buffer));
 219
 220         v4l2_buf.index = index;
 221         v4l2_buf.type = type;
 222         v4l2_buf.memory = memory;
 223
 224         if (v4l2_ioctl(device_fd, VIDIOC_QBUF, &v4l2_buf) < 0) {
 225                 LOGE("qbuf failed.  [i: %d, t: %d, m: %d] errno %d", index, type, memory, errno);
 226                 return VISION_SOURCE_ERROR_INTERNAL;
 227         }
 228
 229         // LOGD("QBUF done [i: %d, t: %d, m: %d]", index, type, memory);
 230
 231         return VISION_SOURCE_ERROR_NONE;
 232 }
 233
 234 static int __vision_source_get_valid_fmt(int device_fd, vector<fmt_info> &found_fmt)
 235 {
 236         int pixel_format = 0;
 237         int pixel_index = 0;
 238         int pixel_count = 0;
 239
 240         while (1) {
 241                 struct v4l2_fmtdesc v4l2_format;
 242                 memset(&v4l2_format, 0x0, sizeof(struct v4l2_fmtdesc));
 243
 244                 v4l2_format.index = pixel_index;
 245                 v4l2_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 246
 247                 if (v4l2_ioctl(device_fd, VIDIOC_ENUM_FMT, &v4l2_format) < 0) {
 248                         LOGW("\tformat : end of enumeration");
 249                         break;
 250                 }
 251
 252                 LOGD("\tformat[%d] " FOURCC_FORMAT " (emulated:%d)", pixel_index, FOURCC_CONVERT(v4l2_format.pixelformat),
 253                          ((v4l2_format.flags & V4L2_FMT_FLAG_EMULATED) ? 1 : 0));
 254
 255                 if (__vision_source_get_format(v4l2_format.pixelformat, &pixel_format) != VISION_SOURCE_ERROR_NONE) {
 256                         pixel_index++;
 257                         continue;
 258                 }
 259
 260                 int resolution_index = 0;
 261                 int resolution_count = 0;
 262
 263                 while (1) {
 264                         struct v4l2_frmsizeenum v4l2_frame;
 265                         memset(&v4l2_frame, 0x0, sizeof(struct v4l2_frmsizeenum));
 266
 267                         v4l2_frame.index = resolution_index;
 268                         v4l2_frame.pixel_format = v4l2_format.pixelformat;
 269
 270                         if (v4l2_ioctl(device_fd, VIDIOC_ENUM_FRAMESIZES, &v4l2_frame) < 0) {
 271                                 LOGW("\t\tframe : end of enumeration ");
 272                                 break;
 273                         }
 274
 275                         // TODO : support other type
 276                         if (v4l2_frame.type != V4L2_FRMSIZE_TYPE_DISCRETE) {
 277                                 LOGW("\t\tframe type %d needs to support", v4l2_frame.type);
 278                                 return VISION_SOURCE_ERROR_NONE;
 279                         }
 280
 281                         uint32_t width = v4l2_frame.discrete.width;
 282                         uint32_t height = v4l2_frame.discrete.height;
 283
 284                         LOGD("\t\tsize[%d] %ux%u", resolution_index, width, height);
 285
 286                         auto fps_list = __vision_source_get_fps_list(device_fd, v4l2_frame.pixel_format, width, height);
 287                         if (fps_list.empty()) {
 288                                 return VISION_SOURCE_ERROR_NOT_SUPPORTED_FORMAT;
 289                         }
 290                         resolution_count++;
 291                         resolution_index++;
 292
 293                         for (auto fps : fps_list) {
 294                                 fmt_info fmt { (media_format_mimetype_e) pixel_format, width, height, fps };
 295                                 found_fmt.push_back(fmt);
 296                         }
 297                 }
 298
 299                 LOGD("\t\tresolution count [%d]", resolution_count);
 300                 pixel_index++;
 301                 pixel_count++;
 302         }
 303
 304         LOGD("\tformat count [%d]", pixel_count);
 305
 306         return VISION_SOURCE_ERROR_NONE;
 307 }
 308
 309 static int __dev_info_from_path(const char *path, vector<fmt_info> &fmt_list)
 310 {
 311         LOGD("check device [%s]", path);
 312         int ret = VISION_SOURCE_ERROR_NONE;
 313
 314         int device_fd = open(path, O_RDWR);
 315         if (device_fd < 0) {
 316                 LOGE("open failed [%s] errno %d", path, errno);
 317                 return ret;
 318         }
 319
 320 #ifdef HAVE_LIBV4L2
 321         int libv4l2_fd = v4l2_fd_open(device_fd, V4L2_ENABLE_ENUM_FMT_EMULATION);
 322
 323         LOGI("device_fd[%d], libv4l2_fd[%d]", device_fd, libv4l2_fd);
 324
 325         if (libv4l2_fd > 0)
 326                 device_fd = libv4l2_fd;
 327 #endif /* HAVE_LIBV4L2 */
 328
 329         struct v4l2_capability v4l2_cap;
 330         memset(&v4l2_cap, 0x0, sizeof(struct v4l2_capability));
 331
 332         if (v4l2_ioctl(device_fd, VIDIOC_QUERYCAP, &v4l2_cap) < 0) {
 333                 LOGE("querycap failed. errno %d", errno);
 334                 v4l2_close(device_fd);
 335                 return ret;
 336         }
 337
 338         unsigned int dev_caps;
 339
 340         if (v4l2_cap.capabilities & V4L2_CAP_DEVICE_CAPS)
 341                 dev_caps = v4l2_cap.device_caps;
 342         else
 343                 dev_caps = v4l2_cap.capabilities;
 344
 345         if (!(dev_caps & V4L2_CAP_VIDEO_CAPTURE) || (dev_caps & V4L2_CAP_VIDEO_OUTPUT)) {
 346                 LOGW("[%s] is not a capture device 0x%x", path, dev_caps);
 347                 v4l2_close(device_fd);
 348                 return ret;
 349         }
 350
 351         ret = __vision_source_get_valid_fmt(device_fd, fmt_list);
 352
 353         v4l2_close(device_fd);
 354         return ret;
 355 }
 356
 357 static int __vision_source_list_devices(vision_source_v4l2_s *vs_handle)
 358 {
 359         glob_t glob_buf;
 360         memset(&glob_buf, 0x0, sizeof(glob_t));
 361         int ret = glob(DEVICE_NODE_PATH_PREFIX "*", 0, 0, &glob_buf);
 362         if (ret != 0) {
 363                 switch (ret) {
 364                 case GLOB_NOSPACE:
 365                         LOGE("out of memory");
 366                         return VISION_SOURCE_ERROR_OUT_OF_MEMORY;
 367                 case GLOB_ABORTED:
 368                         LOGE("read error");
 369                         return VISION_SOURCE_ERROR_INTERNAL;
 370                 case GLOB_NOMATCH:
 371                         LOGE("match not found");
 372                         return VISION_SOURCE_ERROR_NO_DATA;
 373                 }
 374         }
 375
 376         LOGD("device node count : %zu", glob_buf.gl_pathc);
 377
 378         vector<string> &dev_name = vs_handle->dev_name;
 379         vector<vector<fmt_info> > &dev_fmt_list = vs_handle->fmt_list;
 380
 381         for (size_t i = 0; i < glob_buf.gl_pathc; i++) {
 382                 vector<fmt_info> fmt_list;
 383                 ret = __dev_info_from_path(glob_buf.gl_pathv[i], fmt_list);
 384                 if (ret != VISION_SOURCE_ERROR_NONE) {
 385                         break;
 386                 }
 387
 388                 if (!fmt_list.empty()) {
 389                         dev_name.push_back(glob_buf.gl_pathv[i]);
 390                         dev_fmt_list.push_back(fmt_list);
 391                 }
 392         }
 393         globfree(&glob_buf);
 394
 395         if (dev_name.empty())
 396                 return ret;
 397
 398         size_t dev_count = dev_name.size();
 399
 400         vs_handle->dev_info = (vision_source_device_info_s *) calloc(dev_count, sizeof(vision_source_device_info_s));
 401         if (!vs_handle->dev_info) {
 402                 return VISION_SOURCE_ERROR_OUT_OF_MEMORY;
 403         }
 404
 405         vs_handle->fmt = (media_format_h **) calloc(dev_count, sizeof(media_format_h *));
 406         if (!vs_handle->fmt) {
 407                 return VISION_SOURCE_ERROR_OUT_OF_MEMORY;
 408         }
 409
 410         for (size_t i = 0; i < dev_count; i++) {
 411                 vs_handle->dev_info[i].index = i;
 412                 strncpy(vs_handle->dev_info[i].name, dev_name[i].c_str(), DEVICE_NAME_LENGTH_MAX);
 413                 vs_handle->dev_info[i].name[DEVICE_NAME_LENGTH_MAX - 1] = '\0';
 414                 vs_handle->fmt[i] = (media_format_h *) calloc(dev_fmt_list[i].size(), sizeof(media_format_h));
 415                 if (!vs_handle->fmt[i]) {
 416                         return VISION_SOURCE_ERROR_OUT_OF_MEMORY;
 417                 }
 418
 419                 for (size_t j = 0; j < dev_fmt_list[i].size(); j++) {
 420                         media_format_h fmt;
 421                         media_format_create(&fmt);
 422                         media_format_set_video_mime(fmt, dev_fmt_list[i][j].type);
 423                         media_format_set_video_width(fmt, dev_fmt_list[i][j].width);
 424                         media_format_set_video_height(fmt, dev_fmt_list[i][j].height);
 425                         media_format_set_video_frame_rate(fmt, dev_fmt_list[i][j].fps);
 426                         vs_handle->fmt[i][j] = fmt;
 427                 }
 428         }
 429
 430         return ret;
 431 }
 432
 433 static int __vision_source_v4l2_stream(int device_fd, int type, bool onoff)
 434 {
 435         if (v4l2_ioctl(device_fd, onoff ? VIDIOC_STREAMON : VIDIOC_STREAMOFF, &type) < 0) {
 436                 LOGE("stream %d failed. [t:%d] errno %d", onoff, type, errno);
 437                 return VISION_SOURCE_ERROR_INTERNAL;
 438         }
 439
 440         LOGD("stream %d (1:on, 0:off) done [t:%d]", onoff, type);
 441
 442         return VISION_SOURCE_ERROR_NONE;
 443 }
 444
 445 static int __vision_source_v4l2_reqbufs(int device_fd, int type, int memory, uint32_t count, uint32_t *result_count)
 446 {
 447         struct v4l2_requestbuffers v4l2_reqbuf;
 448
 449         memset(&v4l2_reqbuf, 0x0, sizeof(struct v4l2_requestbuffers));
 450
 451         v4l2_reqbuf.type = type;
 452         v4l2_reqbuf.memory = memory;
 453         v4l2_reqbuf.count = count;
 454
 455         if (v4l2_ioctl(device_fd, VIDIOC_REQBUFS, &v4l2_reqbuf) < 0) {
 456                 LOGE("REQBUFS[count %d] failed. errno %d", count, errno);
 457                 return VISION_SOURCE_ERROR_INTERNAL;
 458         }
 459
 460         if (v4l2_reqbuf.count != count) {
 461                 LOGE("different count [req:%d, result:%d]", count, v4l2_reqbuf.count);
 462                 return VISION_SOURCE_ERROR_INTERNAL;
 463         }
 464
 465         *result_count = v4l2_reqbuf.count;
 466
 467         return VISION_SOURCE_ERROR_NONE;
 468 }
 469
 470 static int __vision_source_stop_stream(vision_source_v4l2_s *handle)
 471 {
 472         LOGD("__vision_source_stop_stream");
 473
 474         /* stream off */
 475         int ret = __vision_source_v4l2_stream(handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, false);
 476
 477         /* munmap */
 478         for (size_t i = 0; i < handle->mmap_addr.size(); i++) {
 479                 v4l2_munmap(handle->mmap_addr[i], handle->v4l2_buf[i].length);
 480         }
 481         handle->v4l2_buf.clear();
 482         handle->mmap_addr.clear();
 483
 484         /* reqbufs 0 */
 485         uint32_t buffer_count;
 486         ret = __vision_source_v4l2_reqbufs(handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_MEMORY_MMAP, 0,
 487                                                                            &buffer_count);
 488
 489         LOGD("reqbufs 0 : 0x%x", ret);
 490
 491         return ret;
 492 }
 493
 494 static int __vision_source_set_stream(vision_source_v4l2_s *handle)
 495 {
 496         fmt_info &fmt = handle->fmt_list[handle->device_index][handle->fmt_index];
 497         struct v4l2_format v4l2_fmt;
 498
 499         unsigned int fourcc = 0;
 500
 501         int ret = __vision_source_get_fourcc_plane_num(fmt.type, &fourcc);
 502         if (ret != VISION_SOURCE_ERROR_NONE)
 503                 return ret;
 504
 505         memset(&v4l2_fmt, 0x0, sizeof(struct v4l2_format));
 506
 507         v4l2_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 508         v4l2_fmt.fmt.pix.width = fmt.width;
 509         v4l2_fmt.fmt.pix.height = fmt.height;
 510         v4l2_fmt.fmt.pix.pixelformat = fourcc;
 511
 512         if (v4l2_ioctl(handle->device_fd, VIDIOC_S_FMT, &v4l2_fmt) < 0) {
 513                 LOGE("S_FMT failed. errno %d", errno);
 514                 return VISION_SOURCE_ERROR_INTERNAL;
 515         }
 516
 517         struct v4l2_streamparm v4l2_parm;
 518         /* G_PARM */
 519         memset(&v4l2_parm, 0x0, sizeof(struct v4l2_streamparm));
 520
 521         v4l2_parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 522         if (v4l2_ioctl(handle->device_fd, VIDIOC_G_PARM, &v4l2_parm) < 0) {
 523                 LOGE("G_PARM failed. errno %d", errno);
 524                 return VISION_SOURCE_ERROR_INTERNAL;
 525         }
 526
 527         /* S_PARM to set fps */
 528         v4l2_parm.parm.capture.timeperframe.numerator = 1;
 529         v4l2_parm.parm.capture.timeperframe.denominator = fmt.fps;
 530
 531         if (v4l2_ioctl(handle->device_fd, VIDIOC_S_PARM, &v4l2_parm) < 0) {
 532                 LOGE("S_PARM failed. errno %d", errno);
 533                 return VISION_SOURCE_ERROR_INTERNAL;
 534         }
 535
 536         return VISION_SOURCE_ERROR_NONE;
 537 }
 538
 539 static int __vision_source_v4l2_dqbuf(int device_fd, int type, int memory, int *index, unsigned int *used_size)
 540 {
 541         struct v4l2_buffer v4l2_buf;
 542         memset(&v4l2_buf, 0x0, sizeof(struct v4l2_buffer));
 543
 544         v4l2_buf.type = type;
 545         v4l2_buf.memory = memory;
 546
 547         int ret = v4l2_ioctl(device_fd, VIDIOC_DQBUF, &v4l2_buf);
 548         if (ret < 0) {
 549                 ret = errno;
 550                 LOGE("dqbuf failed. [t: %d, m: %d] errno %d", type, memory, ret);
 551                 if (ret == EIO)
 552                         LOGE("EIO is internal hw error");
 553
 554                 return VISION_SOURCE_ERROR_INTERNAL;
 555         }
 556
 557         *index = v4l2_buf.index;
 558         *used_size = v4l2_buf.bytesused;
 559
 560         return ret;
 561 }
 562
 563 static int __vision_source_v4l2_wait_frame(int device_fd, int wait_time)
 564 {
 565         fd_set fds;
 566         FD_ZERO(&fds);
 567         FD_SET(device_fd, &fds);
 568
 569         struct timeval timeout;
 570         memset(&timeout, 0x0, sizeof(struct timeval));
 571
 572         timeout.tv_sec = wait_time;
 573         timeout.tv_usec = 0;
 574
 575         /*LOGD("select : %d sec", wait_time);*/
 576
 577         int ret = select(device_fd + 1, &fds, NULL, NULL, &timeout);
 578         if (ret == -1) {
 579                 ret = errno;
 580                 if (ret == EINTR) {
 581                         LOGD("select error : EINTR");
 582                         return VISION_SOURCE_ERROR_NONE;
 583                 }
 584                 LOGE("select failed. errno %d", ret);
 585                 return VISION_SOURCE_ERROR_INTERNAL;
 586         }
 587         if (ret == 0) {
 588                 LOGE("select timeout.");
 589                 return VISION_SOURCE_ERROR_INTERNAL;
 590         }
 591
 592         return VISION_SOURCE_ERROR_NONE;
 593 }
 594
 595 static int __vision_source_mmap(vision_source_v4l2_s *handle)
 596 {
 597         /* query buffer, mmap and qbuf */
 598         for (int i = 0; i < BUFFER_MAX; i++) {
 599                 struct v4l2_buffer v4l2_buf;
 600                 memset(&v4l2_buf, 0x0, sizeof(struct v4l2_buffer));
 601
 602                 v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 603                 v4l2_buf.memory = V4L2_MEMORY_MMAP;
 604                 v4l2_buf.index = i;
 605
 606                 if (v4l2_ioctl(handle->device_fd, VIDIOC_QUERYBUF, &v4l2_buf) < 0) {
 607                         LOGE("[%d] query buf failed. errno %d", i, errno);
 608                         return VISION_SOURCE_ERROR_INTERNAL;
 609                 }
 610
 611                 handle->v4l2_buf.push_back(v4l2_buf);
 612
 613                 void *data =
 614                                 v4l2_mmap(0, v4l2_buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, handle->device_fd, v4l2_buf.m.offset);
 615
 616                 if (data == MAP_FAILED) {
 617                         LOGE("[%d] mmap failed (errno %d)", i, errno);
 618                         return VISION_SOURCE_ERROR_INTERNAL;
 619                 }
 620
 621                 handle->mmap_addr.push_back(data);
 622
 623                 if (__vision_source_v4l2_qbuf(handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_MEMORY_MMAP, i) !=
 624                         VISION_SOURCE_ERROR_NONE) {
 625                         LOGE("[%d] qbuf failed (errno %d)", i, errno);
 626                         return VISION_SOURCE_ERROR_INTERNAL;
 627                 }
 628         }
 629
 630         /* stream on */
 631         return __vision_source_v4l2_stream(handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, true);
 632 }
 633
 634 static int __vision_source_start_stream(vision_source_v4l2_s *handle)
 635 {
 636         uint32_t buffer_count = 0;
 637         /* request buffer */
 638         int ret = __vision_source_v4l2_reqbufs(handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_MEMORY_MMAP, BUFFER_MAX,
 639                                                                                    &buffer_count);
 640         if (ret != VISION_SOURCE_ERROR_NONE) {
 641                 return ret;
 642         }
 643
 644         LOGD("buffer count : request %d -> result %d", BUFFER_MAX, buffer_count);
 645
 646         ret = __vision_source_mmap(handle);
 647         if (ret != VISION_SOURCE_ERROR_NONE)
 648                 __vision_source_v4l2_reqbufs(handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_MEMORY_MMAP, 0,
 649                                                                          &buffer_count);
 650
 651         return ret;
 652 }
 653
 654 int vision_source_init(vision_source_h *handle)
 655 {
 656         vision_source_v4l2_s *v4l2_handle = new vision_source_v4l2_s;
 657
 658         int ret = __vision_source_list_devices(v4l2_handle);
 659         if (ret != VISION_SOURCE_ERROR_NONE) {
 660                 delete v4l2_handle;
 661                 LOGE("get device info failed");
 662                 return ret;
 663         }
 664
 665         v4l2_handle->status = VISION_SOURCE_STATUS_INITIALIZED;
 666         *handle = v4l2_handle;
 667
 668         return VISION_SOURCE_ERROR_NONE;
 669 }
 670
 671 static void __vision_source_release_handle(vision_source_v4l2_s *handle)
 672 {
 673         for (size_t i = 0; i < handle->fmt_list.size(); i++) {
 674                 for (size_t j = 0; j < handle->fmt_list[i].size(); j++) {
 675                         media_format_unref(handle->fmt[i][j]);
 676                 }
 677                 free(handle->fmt[i]);
 678         }
 679         free(handle->fmt);
 680         free(handle->dev_info);
 681
 682         LOGD("vision_source_v4l2_s %p destroy", handle);
 683
 684         return;
 685 }
 686
 687 int vision_source_exit(vision_source_h handle)
 688 {
 689         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 690
 691         if (v4l2_handle->dev_info)
 692                 __vision_source_release_handle(v4l2_handle);
 693         delete v4l2_handle;
 694
 695         return VISION_SOURCE_ERROR_NONE;
 696 }
 697
 698 int vision_source_list_devices(vision_source_h handle, const vision_source_device_info_s **dev_list, int *dev_count)
 699 {
 700         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 701
 702         *dev_list = v4l2_handle->dev_info;
 703         *dev_count = v4l2_handle->dev_name.size();
 704
 705         return VISION_SOURCE_ERROR_NONE;
 706 }
 707
 708 int vision_source_list_device_caps(vision_source_h handle, int dev_index, const media_format_h **fmt_list,
 709                                                                    int *fmt_count)
 710 {
 711         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 712
 713         if (dev_index < 0 || (size_t) dev_index >= v4l2_handle->fmt_list.size())
 714                 return VISION_SOURCE_ERROR_INVALID_PARAMETER;
 715
 716         *fmt_list = v4l2_handle->fmt[dev_index];
 717         *fmt_count = v4l2_handle->fmt_list[dev_index].size();
 718
 719         return VISION_SOURCE_ERROR_NONE;
 720 }
 721
 722 int vision_source_open_device(vision_source_h handle, int dev_index)
 723 {
 724         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 725
 726         if (v4l2_handle->status != VISION_SOURCE_STATUS_INITIALIZED) {
 727                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 728         }
 729
 730         if (dev_index < 0 || (size_t) dev_index >= v4l2_handle->fmt_list.size())
 731                 return VISION_SOURCE_ERROR_INVALID_PARAMETER;
 732
 733         int device_fd = VISION_SOURCE_INITIAL_FD;
 734 #ifdef HAVE_LIBV4L2
 735         int libv4l2_fd = VISION_SOURCE_INITIAL_FD;
 736 #endif /* HAVE_LIBV4L2 */
 737
 738         char *node_path = v4l2_handle->dev_info[dev_index].name;
 739
 740         device_fd = open(node_path, O_RDWR);
 741         if (device_fd < 0) {
 742                 LOGE("open [%s] failed [errno %d]", node_path, errno);
 743                 return VISION_SOURCE_ERROR_INTERNAL;
 744         }
 745
 746 #ifdef HAVE_LIBV4L2
 747         libv4l2_fd = v4l2_fd_open(device_fd, V4L2_ENABLE_ENUM_FMT_EMULATION);
 748
 749         LOGI("device_fd[%d], libv4l2_fd[%d]", device_fd, libv4l2_fd);
 750
 751         if (libv4l2_fd != VISION_SOURCE_INITIAL_FD)
 752                 device_fd = libv4l2_fd;
 753 #endif /* HAVE_LIBV4L2 */
 754
 755         v4l2_handle->device_index = dev_index;
 756         v4l2_handle->device_fd = device_fd;
 757         v4l2_handle->status = VISION_SOURCE_STATUS_OPENDED;
 758
 759         LOGD("[%d] device[%s] opened [fd %d]", dev_index, node_path, device_fd);
 760
 761         return VISION_SOURCE_ERROR_NONE;
 762 }
 763
 764 int vision_source_close_device(vision_source_h handle)
 765 {
 766         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 767
 768         if (v4l2_handle->status != VISION_SOURCE_STATUS_OPENDED) {
 769                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 770         }
 771
 772         if (v4l2_handle->device_fd < 0) {
 773                 LOGE("invalid fd %d", v4l2_handle->device_fd);
 774                 return VISION_SOURCE_ERROR_INTERNAL;
 775         }
 776
 777         LOGD("close fd %d", v4l2_handle->device_fd);
 778
 779         v4l2_close(v4l2_handle->device_fd);
 780
 781         v4l2_handle->status = VISION_SOURCE_STATUS_INITIALIZED;
 782         LOGD("device [%d] closed", v4l2_handle->device_index);
 783         v4l2_handle->device_fd = VISION_SOURCE_INITIAL_FD;
 784         v4l2_handle->device_index = VISION_SOURCE_INITIAL_FD;
 785
 786         return VISION_SOURCE_ERROR_NONE;
 787 }
 788
 789 int vision_source_set_stream_format(vision_source_h handle, media_format_h fmt)
 790 {
 791         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 792
 793         if (!v4l2_handle->fmt || v4l2_handle->status != VISION_SOURCE_STATUS_OPENDED) {
 794                 LOGE("Invalid state");
 795                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 796         }
 797
 798         media_format_mimetype_e mimetype;
 799         int width, height, fps;
 800         int ret = media_format_get_video_info(fmt, &mimetype, &width, &height, nullptr, nullptr);
 801         if (ret != MEDIA_FORMAT_ERROR_NONE) {
 802                 LOGE("media_format_get_video_info failed");
 803                 return VISION_SOURCE_ERROR_INTERNAL;
 804         }
 805         ret = media_format_get_video_frame_rate(fmt, &fps);
 806         if (ret != MEDIA_FORMAT_ERROR_NONE) {
 807                 LOGE("media_format_get_video_info failed");
 808                 return VISION_SOURCE_ERROR_INTERNAL;
 809         }
 810
 811         LOGI("Try set format width: %d, height: %d, fps: %d", width, height, fps);
 812
 813         fmt_info request { mimetype, (uint32_t) width, (uint32_t) height, (uint32_t) fps };
 814         vector<fmt_info> &vec = v4l2_handle->fmt_list[v4l2_handle->device_index];
 815         const auto pos = find(vec.begin(), vec.end(), request);
 816         if (pos == vec.end()) {
 817                 LOGE("Not supported format");
 818                 return VISION_SOURCE_ERROR_INVALID_PARAMETER;
 819         }
 820
 821         auto index = distance(vec.begin(), pos);
 822         if (v4l2_handle->fmt_index == index) {
 823                 LOGD("no need to restart preview stream");
 824                 return VISION_SOURCE_ERROR_NONE;
 825         }
 826
 827         v4l2_handle->fmt_index = index;
 828
 829         ret = __vision_source_set_stream(v4l2_handle);
 830         if (ret != VISION_SOURCE_ERROR_NONE) {
 831                 LOGE("failed to set stream");
 832                 return ret;
 833         }
 834
 835         return VISION_SOURCE_ERROR_NONE;
 836 }
 837
 838 static void __vision_source_pkt_dispose_cb(media_packet_h packet, void *user_data)
 839 {
 840         pkt_dispose_cb_data *data = (pkt_dispose_cb_data *) user_data;
 841
 842         int ret = __vision_source_v4l2_qbuf(data->fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_MEMORY_MMAP, data->index);
 843         if (ret != VISION_SOURCE_ERROR_NONE)
 844                 LOGE("qbuf failed");
 845 }
 846
 847 static void __fetch_buffer_and_callback(vision_source_v4l2_s *v4l2_handle)
 848 {
 849         LOGD("__fetch_buffer_and_callback");
 850
 851         int index;
 852         unsigned int byte_size;
 853         int ret;
 854
 855         for (size_t i = 0; i < BUFFER_MAX; i++) {
 856                 v4l2_handle->pkt_data[i].fd = v4l2_handle->device_fd;
 857                 v4l2_handle->pkt_data[i].index = i;
 858         }
 859
 860         while (v4l2_handle->buffer_thread_run) {
 861                 LOGD("__fetch_buffer_and_callback: waiting for buffer");
 862                 ret = __vision_source_v4l2_wait_frame(v4l2_handle->device_fd, 5);
 863                 if (ret != VISION_SOURCE_ERROR_NONE) {
 864                         LOGE("frame wait failed");
 865                         break;
 866                 }
 867
 868                 if (v4l2_handle->buffer_thread_run == false) {
 869                         LOGD("stop buffer handler thread");
 870                         break;
 871                 }
 872                 ret = __vision_source_v4l2_dqbuf(v4l2_handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_MEMORY_MMAP, &index,
 873                                                                                  &byte_size);
 874                 if (ret != VISION_SOURCE_ERROR_NONE) {
 875                         LOGE("dqbuf failed");
 876                         break;
 877                 }
 878
 879                 media_format_h fmt = v4l2_handle->fmt[v4l2_handle->device_index][v4l2_handle->fmt_index];
 880
 881                 media_packet_h pkt;
 882                 ret = media_packet_new_from_external_memory(fmt, v4l2_handle->mmap_addr[index], byte_size,
 883                                                                                                         __vision_source_pkt_dispose_cb, &v4l2_handle->pkt_data[index],
 884                                                                                                         &pkt);
 885                 if (ret != MEDIA_PACKET_ERROR_NONE) {
 886                         LOGE("media_packet_new_from_external_memory failed");
 887                         continue;
 888                 }
 889                 if (v4l2_handle->stream_callback) {
 890                         v4l2_handle->stream_callback(pkt, v4l2_handle->stream_user_data);
 891                 }
 892
 893                 media_packet_unref(pkt);
 894                 sched_yield();
 895         }
 896
 897         return;
 898 }
 899
 900 int vision_source_start_stream(vision_source_h handle)
 901 {
 902         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 903
 904         if (v4l2_handle->status != VISION_SOURCE_STATUS_OPENDED) {
 905                 LOGE("status is not VISION_SOURCE_STATUS_OPENDED");
 906                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 907         }
 908         if (v4l2_handle->fmt_index < 0) {
 909                 LOGE("format is not set");
 910                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 911         }
 912
 913         int ret = __vision_source_start_stream(v4l2_handle);
 914         if (ret != VISION_SOURCE_ERROR_NONE) {
 915                 LOGE("__vision_source_start_stream failed[0x%x]", ret);
 916                 return ret;
 917         }
 918
 919         v4l2_handle->buffer_thread_run = true;
 920         v4l2_handle->buffer_thread = thread(__fetch_buffer_and_callback, v4l2_handle);
 921
 922         v4l2_handle->status = VISION_SOURCE_STATUS_STARTED;
 923         LOGD("start preview done");
 924
 925         return VISION_SOURCE_ERROR_NONE;
 926 }
 927
 928 /*
 929  * TODO: buffer_thread has some problem, what happend to buffer?
 930  */
 931 int vision_source_stop_stream(vision_source_h handle)
 932 {
 933         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 934
 935         if (v4l2_handle->status != VISION_SOURCE_STATUS_STARTED)
 936                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 937
 938         v4l2_handle->buffer_thread_run = false;
 939         v4l2_handle->buffer_thread.join();
 940         LOGI("buffer thread stopped and joined");
 941
 942         int ret = __vision_source_stop_stream(v4l2_handle);
 943         if (ret != VISION_SOURCE_ERROR_NONE)
 944                 LOGE("__vision_source_stop_stream failed, but buffer thread will be stopped");
 945
 946         v4l2_handle->fmt_index = VISION_SOURCE_INITIAL_FD;
 947         v4l2_handle->stream_callback = nullptr;
 948         v4l2_handle->stream_user_data = nullptr;
 949         v4l2_handle->status = VISION_SOURCE_STATUS_OPENDED;
 950         LOGD("stop preview done [0x%x]", ret);
 951
 952         return ret;
 953 }
 954
 955 /* TODO: run-time callback change*/
 956 int vision_source_set_stream_cb(vision_source_h handle, stream_cb callback, void *user_data)
 957 {
 958         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 959
 960         if (v4l2_handle->status != VISION_SOURCE_STATUS_OPENDED)
 961                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 962
 963         v4l2_handle->stream_callback = callback;
 964         v4l2_handle->stream_user_data = user_data;
 965         return VISION_SOURCE_ERROR_NONE;
 966 }
 967
 968 #ifdef USE_DL
 969 void attach_backend(vision_source_func_s *funcp)
 970 {
 971         funcp->init = vision_source_v4l2_init;
 972         funcp->exit = vision_source_v4l2_exit;
 973         funcp->list_devices = vision_source_v4l2_list_devices;
 974         funcp->list_device_caps = vision_source_v4l2_list_device_caps;
 975         funcp->open_device = vision_source_v4l2_open_device;
 976         funcp->close_device = vision_source_v4l2_close_device;
 977         funcp->set_stream_format = vision_source_v4l2_set_stream_format;
 978         funcp->start_stream = vision_source_v4l2_start_stream;
 979         funcp->stop_stream = vision_source_v4l2_stop_stream;
 980         funcp->set_stream_cb = vision_source_v4l2_set_stream_cb;
 981 }
 982 #endif