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->fmt[i] = (media_format_h *) calloc(dev_fmt_list[i].size(), sizeof(media_format_h));
 414                 if (!vs_handle->fmt[i]) {
 415                         return VISION_SOURCE_ERROR_OUT_OF_MEMORY;
 416                 }
 417
 418                 for (size_t j = 0; j < dev_fmt_list[i].size(); j++) {
 419                         media_format_h fmt;
 420                         media_format_create(&fmt);
 421                         media_format_set_video_mime(fmt, dev_fmt_list[i][j].type);
 422                         media_format_set_video_width(fmt, dev_fmt_list[i][j].width);
 423                         media_format_set_video_height(fmt, dev_fmt_list[i][j].height);
 424                         media_format_set_video_frame_rate(fmt, dev_fmt_list[i][j].fps);
 425                         vs_handle->fmt[i][j] = fmt;
 426                 }
 427         }
 428
 429         return ret;
 430 }
 431
 432 static int __vision_source_v4l2_stream(int device_fd, int type, bool onoff)
 433 {
 434         if (v4l2_ioctl(device_fd, onoff ? VIDIOC_STREAMON : VIDIOC_STREAMOFF, &type) < 0) {
 435                 LOGE("stream %d failed. [t:%d] errno %d", onoff, type, errno);
 436                 return VISION_SOURCE_ERROR_INTERNAL;
 437         }
 438
 439         LOGD("stream %d (1:on, 0:off) done [t:%d]", onoff, type);
 440
 441         return VISION_SOURCE_ERROR_NONE;
 442 }
 443
 444 static int __vision_source_v4l2_reqbufs(int device_fd, int type, int memory, uint32_t count, uint32_t *result_count)
 445 {
 446         struct v4l2_requestbuffers v4l2_reqbuf;
 447
 448         memset(&v4l2_reqbuf, 0x0, sizeof(struct v4l2_requestbuffers));
 449
 450         v4l2_reqbuf.type = type;
 451         v4l2_reqbuf.memory = memory;
 452         v4l2_reqbuf.count = count;
 453
 454         if (v4l2_ioctl(device_fd, VIDIOC_REQBUFS, &v4l2_reqbuf) < 0) {
 455                 LOGE("REQBUFS[count %d] failed. errno %d", count, errno);
 456                 return VISION_SOURCE_ERROR_INTERNAL;
 457         }
 458
 459         if (v4l2_reqbuf.count != count) {
 460                 LOGE("different count [req:%d, result:%d]", count, v4l2_reqbuf.count);
 461                 return VISION_SOURCE_ERROR_INTERNAL;
 462         }
 463
 464         *result_count = v4l2_reqbuf.count;
 465
 466         return VISION_SOURCE_ERROR_NONE;
 467 }
 468
 469 static int __vision_source_stop_stream(vision_source_v4l2_s *handle)
 470 {
 471         LOGD("__vision_source_stop_stream");
 472
 473         /* stream off */
 474         int ret = __vision_source_v4l2_stream(handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, false);
 475
 476         /* munmap */
 477         for (size_t i = 0; i < handle->mmap_addr.size(); i++) {
 478                 v4l2_munmap(handle->mmap_addr[i], handle->v4l2_buf[i].length);
 479         }
 480         handle->v4l2_buf.clear();
 481         handle->mmap_addr.clear();
 482
 483         /* reqbufs 0 */
 484         uint32_t buffer_count;
 485         ret = __vision_source_v4l2_reqbufs(handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_MEMORY_MMAP, 0,
 486                                                                            &buffer_count);
 487
 488         LOGD("reqbufs 0 : 0x%x", ret);
 489
 490         return ret;
 491 }
 492
 493 static int __vision_source_set_stream(vision_source_v4l2_s *handle)
 494 {
 495         fmt_info &fmt = handle->fmt_list[handle->device_index][handle->fmt_index];
 496         struct v4l2_format v4l2_fmt;
 497
 498         unsigned int fourcc = 0;
 499
 500         int ret = __vision_source_get_fourcc_plane_num(fmt.type, &fourcc);
 501         if (ret != VISION_SOURCE_ERROR_NONE)
 502                 return ret;
 503
 504         memset(&v4l2_fmt, 0x0, sizeof(struct v4l2_format));
 505
 506         v4l2_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 507         v4l2_fmt.fmt.pix.width = fmt.width;
 508         v4l2_fmt.fmt.pix.height = fmt.height;
 509         v4l2_fmt.fmt.pix.pixelformat = fourcc;
 510
 511         if (v4l2_ioctl(handle->device_fd, VIDIOC_S_FMT, &v4l2_fmt) < 0) {
 512                 LOGE("S_FMT failed. errno %d", errno);
 513                 return VISION_SOURCE_ERROR_INTERNAL;
 514         }
 515
 516         struct v4l2_streamparm v4l2_parm;
 517         /* G_PARM */
 518         memset(&v4l2_parm, 0x0, sizeof(struct v4l2_streamparm));
 519
 520         v4l2_parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 521         if (v4l2_ioctl(handle->device_fd, VIDIOC_G_PARM, &v4l2_parm) < 0) {
 522                 LOGE("G_PARM failed. errno %d", errno);
 523                 return VISION_SOURCE_ERROR_INTERNAL;
 524         }
 525
 526         /* S_PARM to set fps */
 527         v4l2_parm.parm.capture.timeperframe.numerator = 1;
 528         v4l2_parm.parm.capture.timeperframe.denominator = fmt.fps;
 529
 530         if (v4l2_ioctl(handle->device_fd, VIDIOC_S_PARM, &v4l2_parm) < 0) {
 531                 LOGE("S_PARM failed. errno %d", errno);
 532                 return VISION_SOURCE_ERROR_INTERNAL;
 533         }
 534
 535         return VISION_SOURCE_ERROR_NONE;
 536 }
 537
 538 static int __vision_source_v4l2_dqbuf(int device_fd, int type, int memory, int *index, unsigned int *used_size)
 539 {
 540         struct v4l2_buffer v4l2_buf;
 541         memset(&v4l2_buf, 0x0, sizeof(struct v4l2_buffer));
 542
 543         v4l2_buf.type = type;
 544         v4l2_buf.memory = memory;
 545
 546         int ret = v4l2_ioctl(device_fd, VIDIOC_DQBUF, &v4l2_buf);
 547         if (ret < 0) {
 548                 ret = errno;
 549                 LOGE("dqbuf failed. [t: %d, m: %d] errno %d", type, memory, ret);
 550                 if (ret == EIO)
 551                         LOGE("EIO is internal hw error");
 552
 553                 return VISION_SOURCE_ERROR_INTERNAL;
 554         }
 555
 556         *index = v4l2_buf.index;
 557         *used_size = v4l2_buf.bytesused;
 558
 559         return ret;
 560 }
 561
 562 static int __vision_source_v4l2_wait_frame(int device_fd, int wait_time)
 563 {
 564         fd_set fds;
 565         FD_ZERO(&fds);
 566         FD_SET(device_fd, &fds);
 567
 568         struct timeval timeout;
 569         memset(&timeout, 0x0, sizeof(struct timeval));
 570
 571         timeout.tv_sec = wait_time;
 572         timeout.tv_usec = 0;
 573
 574         /*LOGD("select : %d sec", wait_time);*/
 575
 576         int ret = select(device_fd + 1, &fds, NULL, NULL, &timeout);
 577         if (ret == -1) {
 578                 ret = errno;
 579                 if (ret == EINTR) {
 580                         LOGD("select error : EINTR");
 581                         return VISION_SOURCE_ERROR_NONE;
 582                 }
 583                 LOGE("select failed. errno %d", ret);
 584                 return VISION_SOURCE_ERROR_INTERNAL;
 585         }
 586         if (ret == 0) {
 587                 LOGE("select timeout.");
 588                 return VISION_SOURCE_ERROR_INTERNAL;
 589         }
 590
 591         return VISION_SOURCE_ERROR_NONE;
 592 }
 593
 594 static int __vision_source_mmap(vision_source_v4l2_s *handle)
 595 {
 596         /* query buffer, mmap and qbuf */
 597         for (int i = 0; i < BUFFER_MAX; i++) {
 598                 struct v4l2_buffer v4l2_buf;
 599                 memset(&v4l2_buf, 0x0, sizeof(struct v4l2_buffer));
 600
 601                 v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 602                 v4l2_buf.memory = V4L2_MEMORY_MMAP;
 603                 v4l2_buf.index = i;
 604
 605                 if (v4l2_ioctl(handle->device_fd, VIDIOC_QUERYBUF, &v4l2_buf) < 0) {
 606                         LOGE("[%d] query buf failed. errno %d", i, errno);
 607                         return VISION_SOURCE_ERROR_INTERNAL;
 608                 }
 609
 610                 handle->v4l2_buf.push_back(v4l2_buf);
 611
 612                 void *data =
 613                                 v4l2_mmap(0, v4l2_buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, handle->device_fd, v4l2_buf.m.offset);
 614
 615                 if (data == MAP_FAILED) {
 616                         LOGE("[%d] mmap failed (errno %d)", i, errno);
 617                         return VISION_SOURCE_ERROR_INTERNAL;
 618                 }
 619
 620                 handle->mmap_addr.push_back(data);
 621
 622                 if (__vision_source_v4l2_qbuf(handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_MEMORY_MMAP, i) !=
 623                         VISION_SOURCE_ERROR_NONE) {
 624                         LOGE("[%d] qbuf failed (errno %d)", i, errno);
 625                         return VISION_SOURCE_ERROR_INTERNAL;
 626                 }
 627         }
 628
 629         /* stream on */
 630         return __vision_source_v4l2_stream(handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, true);
 631 }
 632
 633 static int __vision_source_start_stream(vision_source_v4l2_s *handle)
 634 {
 635         uint32_t buffer_count = 0;
 636         /* request buffer */
 637         int ret = __vision_source_v4l2_reqbufs(handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_MEMORY_MMAP, BUFFER_MAX,
 638                                                                                    &buffer_count);
 639         if (ret != VISION_SOURCE_ERROR_NONE) {
 640                 return ret;
 641         }
 642
 643         LOGD("buffer count : request %d -> result %d", BUFFER_MAX, buffer_count);
 644
 645         ret = __vision_source_mmap(handle);
 646         if (ret != VISION_SOURCE_ERROR_NONE)
 647                 __vision_source_v4l2_reqbufs(handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_MEMORY_MMAP, 0,
 648                                                                          &buffer_count);
 649
 650         return ret;
 651 }
 652
 653 int vision_source_init(vision_source_h *handle)
 654 {
 655         vision_source_v4l2_s *v4l2_handle = new vision_source_v4l2_s;
 656
 657         int ret = __vision_source_list_devices(v4l2_handle);
 658         if (ret != VISION_SOURCE_ERROR_NONE) {
 659                 free(v4l2_handle);
 660                 LOGE("get device info failed");
 661                 return ret;
 662         }
 663
 664         v4l2_handle->status = VISION_SOURCE_STATUS_INITIALIZED;
 665         *handle = v4l2_handle;
 666
 667         return VISION_SOURCE_ERROR_NONE;
 668 }
 669
 670 static void __vision_source_release_handle(vision_source_v4l2_s *handle)
 671 {
 672         for (size_t i = 0; i < handle->fmt_list.size(); i++) {
 673                 for (size_t j = 0; j < handle->fmt_list[i].size(); j++) {
 674                         media_format_unref(handle->fmt[i][j]);
 675                 }
 676                 free(handle->fmt[i]);
 677         }
 678         free(handle->fmt);
 679         free(handle->dev_info);
 680
 681         LOGD("vision_source_v4l2_s %p destroy", handle);
 682
 683         return;
 684 }
 685
 686 int vision_source_exit(vision_source_h handle)
 687 {
 688         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 689
 690         if (v4l2_handle->dev_info)
 691                 __vision_source_release_handle(v4l2_handle);
 692         delete v4l2_handle;
 693
 694         return VISION_SOURCE_ERROR_NONE;
 695 }
 696
 697 int vision_source_list_devices(vision_source_h handle, const vision_source_device_info_s **dev_list, int *dev_count)
 698 {
 699         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 700
 701         *dev_list = v4l2_handle->dev_info;
 702         *dev_count = v4l2_handle->dev_name.size();
 703
 704         return VISION_SOURCE_ERROR_NONE;
 705 }
 706
 707 int vision_source_list_device_caps(vision_source_h handle, int dev_index, const media_format_h **fmt_list,
 708                                                                    int *fmt_count)
 709 {
 710         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 711
 712         if (dev_index < 0 || (size_t) dev_index >= v4l2_handle->fmt_list.size())
 713                 return VISION_SOURCE_ERROR_INVALID_PARAMETER;
 714
 715         *fmt_list = v4l2_handle->fmt[dev_index];
 716         *fmt_count = v4l2_handle->fmt_list[dev_index].size();
 717
 718         return VISION_SOURCE_ERROR_NONE;
 719 }
 720
 721 int vision_source_open_device(vision_source_h handle, int dev_index)
 722 {
 723         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 724
 725         if (v4l2_handle->status != VISION_SOURCE_STATUS_INITIALIZED) {
 726                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 727         }
 728
 729         if (dev_index < 0 || (size_t) dev_index >= v4l2_handle->fmt_list.size())
 730                 return VISION_SOURCE_ERROR_INVALID_PARAMETER;
 731
 732         int device_fd = VISION_SOURCE_INITIAL_FD;
 733 #ifdef HAVE_LIBV4L2
 734         int libv4l2_fd = VISION_SOURCE_INITIAL_FD;
 735 #endif /* HAVE_LIBV4L2 */
 736
 737         char *node_path = v4l2_handle->dev_info[dev_index].name;
 738
 739         device_fd = open(node_path, O_RDWR);
 740         if (device_fd < 0) {
 741                 LOGE("open [%s] failed [errno %d]", node_path, errno);
 742                 return VISION_SOURCE_ERROR_INTERNAL;
 743         }
 744
 745 #ifdef HAVE_LIBV4L2
 746         libv4l2_fd = v4l2_fd_open(device_fd, V4L2_ENABLE_ENUM_FMT_EMULATION);
 747
 748         LOGI("device_fd[%d], libv4l2_fd[%d]", device_fd, libv4l2_fd);
 749
 750         if (libv4l2_fd != VISION_SOURCE_INITIAL_FD)
 751                 device_fd = libv4l2_fd;
 752 #endif /* HAVE_LIBV4L2 */
 753
 754         v4l2_handle->device_index = dev_index;
 755         v4l2_handle->device_fd = device_fd;
 756         v4l2_handle->status = VISION_SOURCE_STATUS_OPENDED;
 757
 758         LOGD("[%d] device[%s] opened [fd %d]", dev_index, node_path, device_fd);
 759
 760         return VISION_SOURCE_ERROR_NONE;
 761 }
 762
 763 int vision_source_close_device(vision_source_h handle)
 764 {
 765         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 766
 767         if (v4l2_handle->status != VISION_SOURCE_STATUS_OPENDED) {
 768                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 769         }
 770
 771         if (v4l2_handle->device_fd < 0) {
 772                 LOGE("invalid fd %d", v4l2_handle->device_fd);
 773                 return VISION_SOURCE_ERROR_INTERNAL;
 774         }
 775
 776         LOGD("close fd %d", v4l2_handle->device_fd);
 777
 778         v4l2_close(v4l2_handle->device_fd);
 779
 780         v4l2_handle->status = VISION_SOURCE_STATUS_INITIALIZED;
 781         LOGD("device [%d] closed", v4l2_handle->device_index);
 782         v4l2_handle->device_fd = VISION_SOURCE_INITIAL_FD;
 783         v4l2_handle->device_index = VISION_SOURCE_INITIAL_FD;
 784
 785         return VISION_SOURCE_ERROR_NONE;
 786 }
 787
 788 int vision_source_set_stream_format(vision_source_h handle, media_format_h fmt)
 789 {
 790         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 791
 792         if (!v4l2_handle->fmt || v4l2_handle->status != VISION_SOURCE_STATUS_OPENDED) {
 793                 LOGE("Invalid state");
 794                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 795         }
 796
 797         media_format_mimetype_e mimetype;
 798         int width, height, fps;
 799         int ret = media_format_get_video_info(fmt, &mimetype, &width, &height, nullptr, nullptr);
 800         if (ret != MEDIA_FORMAT_ERROR_NONE) {
 801                 LOGE("media_format_get_video_info failed");
 802                 return VISION_SOURCE_ERROR_INTERNAL;
 803         }
 804         ret = media_format_get_video_frame_rate(fmt, &fps);
 805         if (ret != MEDIA_FORMAT_ERROR_NONE) {
 806                 LOGE("media_format_get_video_info failed");
 807                 return VISION_SOURCE_ERROR_INTERNAL;
 808         }
 809
 810         LOGI("Try set format width: %d, height: %d, fps: %d", width, height, fps);
 811
 812         fmt_info request { mimetype, (uint32_t) width, (uint32_t) height, (uint32_t) fps };
 813         vector<fmt_info> &vec = v4l2_handle->fmt_list[v4l2_handle->device_index];
 814         const auto pos = find(vec.begin(), vec.end(), request);
 815         if (pos == vec.end()) {
 816                 LOGE("Not supported format");
 817                 return VISION_SOURCE_ERROR_INVALID_PARAMETER;
 818         }
 819
 820         auto index = distance(vec.begin(), pos);
 821         if (v4l2_handle->fmt_index == index) {
 822                 LOGD("no need to restart preview stream");
 823                 return VISION_SOURCE_ERROR_NONE;
 824         }
 825
 826         v4l2_handle->fmt_index = index;
 827
 828         ret = __vision_source_set_stream(v4l2_handle);
 829         if (ret != VISION_SOURCE_ERROR_NONE) {
 830                 LOGE("failed to set stream");
 831                 return ret;
 832         }
 833
 834         return VISION_SOURCE_ERROR_NONE;
 835 }
 836
 837 static void __vision_source_pkt_dispose_cb(media_packet_h packet, void *user_data)
 838 {
 839         pkt_dispose_cb_data *data = (pkt_dispose_cb_data *) user_data;
 840
 841         int ret = __vision_source_v4l2_qbuf(data->fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_MEMORY_MMAP, data->index);
 842         if (ret != VISION_SOURCE_ERROR_NONE)
 843                 LOGE("qbuf failed");
 844 }
 845
 846 static void __fetch_buffer_and_callback(vision_source_v4l2_s *v4l2_handle)
 847 {
 848         LOGD("__fetch_buffer_and_callback");
 849
 850         int index;
 851         unsigned int byte_size;
 852         int ret;
 853
 854         for (size_t i = 0; i < BUFFER_MAX; i++) {
 855                 v4l2_handle->pkt_data[i].fd = v4l2_handle->device_fd;
 856                 v4l2_handle->pkt_data[i].index = i;
 857         }
 858
 859         while (v4l2_handle->buffer_thread_run) {
 860                 LOGD("__fetch_buffer_and_callback: waiting for buffer");
 861                 ret = __vision_source_v4l2_wait_frame(v4l2_handle->device_fd, 5);
 862                 if (ret != VISION_SOURCE_ERROR_NONE) {
 863                         LOGE("frame wait failed");
 864                         break;
 865                 }
 866
 867                 if (v4l2_handle->buffer_thread_run == false) {
 868                         LOGD("stop buffer handler thread");
 869                         break;
 870                 }
 871                 ret = __vision_source_v4l2_dqbuf(v4l2_handle->device_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_MEMORY_MMAP, &index,
 872                                                                                  &byte_size);
 873                 if (ret != VISION_SOURCE_ERROR_NONE) {
 874                         LOGE("dqbuf failed");
 875                         break;
 876                 }
 877
 878                 media_format_h fmt = v4l2_handle->fmt[v4l2_handle->device_index][v4l2_handle->fmt_index];
 879
 880                 media_packet_h pkt;
 881                 ret = media_packet_new_from_external_memory(fmt, v4l2_handle->mmap_addr[index], byte_size,
 882                                                                                                         __vision_source_pkt_dispose_cb, &v4l2_handle->pkt_data[index],
 883                                                                                                         &pkt);
 884                 if (ret != MEDIA_PACKET_ERROR_NONE) {
 885                         LOGE("media_packet_new_from_external_memory failed");
 886                         continue;
 887                 }
 888                 if (v4l2_handle->stream_callback) {
 889                         v4l2_handle->stream_callback(pkt, v4l2_handle->stream_user_data);
 890                 }
 891
 892                 media_packet_unref(pkt);
 893                 sched_yield();
 894         }
 895
 896         return;
 897 }
 898
 899 int vision_source_start_stream(vision_source_h handle)
 900 {
 901         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 902
 903         if (v4l2_handle->status != VISION_SOURCE_STATUS_OPENDED) {
 904                 LOGE("status is not VISION_SOURCE_STATUS_OPENDED");
 905                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 906         }
 907         if (v4l2_handle->fmt_index < 0) {
 908                 LOGE("format is not set");
 909                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 910         }
 911
 912         int ret = __vision_source_start_stream(v4l2_handle);
 913         if (ret != VISION_SOURCE_ERROR_NONE) {
 914                 LOGE("__vision_source_start_stream failed[0x%x]", ret);
 915                 return ret;
 916         }
 917
 918         v4l2_handle->buffer_thread_run = true;
 919         v4l2_handle->buffer_thread = thread(__fetch_buffer_and_callback, v4l2_handle);
 920
 921         v4l2_handle->status = VISION_SOURCE_STATUS_STARTED;
 922         LOGD("start preview done");
 923
 924         return VISION_SOURCE_ERROR_NONE;
 925 }
 926
 927 /*
 928  * TODO: buffer_thread has some problem, what happend to buffer?
 929  */
 930 int vision_source_stop_stream(vision_source_h handle)
 931 {
 932         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 933
 934         if (v4l2_handle->status != VISION_SOURCE_STATUS_STARTED)
 935                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 936
 937         v4l2_handle->buffer_thread_run = false;
 938         v4l2_handle->buffer_thread.join();
 939         LOGI("buffer thread stopped and joined");
 940
 941         int ret = __vision_source_stop_stream(v4l2_handle);
 942         if (ret != VISION_SOURCE_ERROR_NONE)
 943                 LOGE("__vision_source_stop_stream failed, but buffer thread will be stopped");
 944
 945         v4l2_handle->fmt_index = VISION_SOURCE_INITIAL_FD;
 946         v4l2_handle->stream_callback = nullptr;
 947         v4l2_handle->stream_user_data = nullptr;
 948         v4l2_handle->status = VISION_SOURCE_STATUS_OPENDED;
 949         LOGD("stop preview done [0x%x]", ret);
 950
 951         return ret;
 952 }
 953
 954 /* TODO: run-time callback change*/
 955 int vision_source_set_stream_cb(vision_source_h handle, stream_cb callback, void *user_data)
 956 {
 957         vision_source_v4l2_s *v4l2_handle = (vision_source_v4l2_s *) handle;
 958
 959         if (v4l2_handle->status != VISION_SOURCE_STATUS_OPENDED)
 960                 return VISION_SOURCE_ERROR_INVALID_OPERATION;
 961
 962         v4l2_handle->stream_callback = callback;
 963         v4l2_handle->stream_user_data = user_data;
 964         return VISION_SOURCE_ERROR_NONE;
 965 }
 966
 967 #ifdef USE_DL
 968 void attach_backend(vision_source_func_s *funcp)
 969 {
 970         funcp->init = vision_source_v4l2_init;
 971         funcp->exit = vision_source_v4l2_exit;
 972         funcp->list_devices = vision_source_v4l2_list_devices;
 973         funcp->list_device_caps = vision_source_v4l2_list_device_caps;
 974         funcp->open_device = vision_source_v4l2_open_device;
 975         funcp->close_device = vision_source_v4l2_close_device;
 976         funcp->set_stream_format = vision_source_v4l2_set_stream_format;
 977         funcp->start_stream = vision_source_v4l2_start_stream;
 978         funcp->stop_stream = vision_source_v4l2_stop_stream;
 979         funcp->set_stream_cb = vision_source_v4l2_set_stream_cb;
 980 }
 981 #endif