1 /*
2 Copyright (c) 2011-2012,2015, The Linux Foundation. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without
5 modification, are permitted provided that the following conditions are
6 met:
7     * Redistributions of source code must retain the above copyright
8       notice, this list of conditions and the following disclaimer.
9     * Redistributions in binary form must reproduce the above
10       copyright notice, this list of conditions and the following
11       disclaimer in the documentation and/or other materials provided
12       with the distribution.
13     * Neither the name of The Linux Foundation nor the names of its
14       contributors may be used to endorse or promote products derived
15       from this software without specific prior written permission.
16 
17 THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
18 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
19 MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
20 ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
21 BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
24 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
25 WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
26 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
27 IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 */
29 
30 #include <pthread.h>
31 #include "mm_camera_dbg.h"
32 #include <errno.h>
33 #include <sys/ioctl.h>
34 #include <sys/types.h>
35 #include <sys/stat.h>
36 #include <fcntl.h>
37 #include <poll.h>
38 #include <time.h>
39 
40 #include "mm_camera_interface2.h"
41 #include "mm_camera.h"
42 
43 static void mm_camera_stream_util_set_state(mm_camera_stream_t *stream,
44                          mm_camera_stream_state_type_t state);
45 
mm_camera_stream_init_q(mm_camera_frame_queue_t * q)46 int mm_camera_stream_init_q(mm_camera_frame_queue_t *q)
47 {
48     pthread_mutex_init(&q->mutex, NULL);
49     return MM_CAMERA_OK;
50 }
mm_camera_stream_deinit_q(mm_camera_frame_queue_t * q)51 int mm_camera_stream_deinit_q(mm_camera_frame_queue_t *q)
52 {
53     pthread_mutex_destroy(&q->mutex);
54     return MM_CAMERA_OK;
55 }
56 
mm_camera_stream_frame_get_q_cnt(mm_camera_frame_queue_t * q)57 int mm_camera_stream_frame_get_q_cnt(mm_camera_frame_queue_t *q)
58 {
59     int cnt;
60     pthread_mutex_lock(&q->mutex);
61     cnt = q->cnt;
62     pthread_mutex_unlock(&q->mutex);
63     return cnt;
64 }
65 
mm_camera_stream_frame_deq_no_lock(mm_camera_frame_queue_t * q)66 mm_camera_frame_t *mm_camera_stream_frame_deq_no_lock(mm_camera_frame_queue_t *q)
67 {
68     mm_camera_frame_t *tmp;
69 
70     tmp = q->head;
71 
72     if(tmp == NULL) goto end;
73     if(q->head == q->tail) {
74         q->head = NULL;
75         q->tail = NULL;
76     } else {
77         q->head = tmp->next;
78     }
79     tmp->next = NULL;
80     q->cnt--;
81 end:
82     return tmp;
83 }
84 
mm_camera_stream_frame_enq_no_lock(mm_camera_frame_queue_t * q,mm_camera_frame_t * node)85 void mm_camera_stream_frame_enq_no_lock(mm_camera_frame_queue_t *q, mm_camera_frame_t *node)
86 {
87     node->next = NULL;
88     if(q->head == NULL) {
89         q->head = node;
90         q->tail = node;
91     } else {
92         q->tail->next = node;
93         q->tail = node;
94     }
95     q->cnt++;
96 }
97 
mm_camera_stream_frame_deq(mm_camera_frame_queue_t * q)98 mm_camera_frame_t *mm_camera_stream_frame_deq(mm_camera_frame_queue_t *q)
99 {
100     mm_camera_frame_t *tmp;
101 
102     pthread_mutex_lock(&q->mutex);
103     tmp = q->head;
104 
105     if(tmp == NULL) goto end;
106     if(q->head == q->tail) {
107         q->head = NULL;
108         q->tail = NULL;
109     } else {
110         q->head = tmp->next;
111     }
112     tmp->next = NULL;
113     q->cnt--;
114 end:
115     pthread_mutex_unlock(&q->mutex);
116     return tmp;
117 }
118 
mm_camera_stream_frame_enq(mm_camera_frame_queue_t * q,mm_camera_frame_t * node)119 void mm_camera_stream_frame_enq(mm_camera_frame_queue_t *q, mm_camera_frame_t *node)
120 {
121     pthread_mutex_lock(&q->mutex);
122     node->next = NULL;
123     if(q->head == NULL) {
124         q->head = node;
125         q->tail = node;
126     } else {
127         q->tail->next = node;
128         q->tail = node;
129     }
130     q->cnt++;
131     pthread_mutex_unlock(&q->mutex);
132 }
133 
mm_stream_frame_flash_q(mm_camera_frame_queue_t * q)134 void mm_stream_frame_flash_q(mm_camera_frame_queue_t *q)
135 {
136     pthread_mutex_lock(&q->mutex);
137     q->cnt = 0;
138     q->match_cnt = 0;
139     q->head = NULL;
140     q->tail = NULL;
141     pthread_mutex_unlock(&q->mutex);
142 }
143 
mm_camera_stream_frame_refill_q(mm_camera_frame_queue_t * q,mm_camera_frame_t * node,int num)144 void mm_camera_stream_frame_refill_q(mm_camera_frame_queue_t *q, mm_camera_frame_t *node, int num)
145 {
146     int i;
147 
148     mm_stream_frame_flash_q(q);
149     for(i = 0; i < num; i++)
150         mm_camera_stream_frame_enq(q, &node[i]);
151     CDBG("%s: q=0x%x, num = %d, q->cnt=%d\n",
152              __func__,(uint32_t)q,num, mm_camera_stream_frame_get_q_cnt(q));
153 }
154 
mm_camera_stream_deinit_frame(mm_camera_stream_frame_t * frame)155 void mm_camera_stream_deinit_frame(mm_camera_stream_frame_t *frame)
156 {
157     pthread_mutex_destroy(&frame->mutex);
158     mm_camera_stream_deinit_q(&frame->readyq);
159     memset(frame, 0, sizeof(mm_camera_stream_frame_t));
160 }
161 
mm_camera_stream_init_frame(mm_camera_stream_frame_t * frame)162 void mm_camera_stream_init_frame(mm_camera_stream_frame_t *frame)
163 {
164     memset(frame, 0, sizeof(mm_camera_stream_frame_t));
165     pthread_mutex_init(&frame->mutex, NULL);
166     mm_camera_stream_init_q(&frame->readyq);
167 }
168 
mm_camera_stream_release(mm_camera_stream_t * stream)169 void mm_camera_stream_release(mm_camera_stream_t *stream)
170 {
171     mm_camera_stream_deinit_frame(&stream->frame);
172     if(stream->fd > 0) close(stream->fd);
173     memset(stream, 0, sizeof(*stream));
174     //stream->fd = -1;
175     mm_camera_stream_util_set_state(stream, MM_CAMERA_STREAM_STATE_NOTUSED);
176 }
177 
mm_camera_stream_is_active(mm_camera_stream_t * stream)178 int mm_camera_stream_is_active(mm_camera_stream_t *stream)
179 {
180     return (stream->state == MM_CAMERA_STREAM_STATE_ACTIVE)? TRUE : FALSE;
181 }
182 
mm_camera_stream_util_set_state(mm_camera_stream_t * stream,mm_camera_stream_state_type_t state)183 static void mm_camera_stream_util_set_state(mm_camera_stream_t *stream,
184                                  mm_camera_stream_state_type_t state)
185 {
186     CDBG("%s:stream fd=%d, stream type=%d, cur_state=%d,new_state=%d\n",
187       __func__, stream->fd, stream->stream_type, stream->state, state);
188     stream->state = state;
189 }
190 
mm_camera_read_msm_frame(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream)191 int mm_camera_read_msm_frame(mm_camera_obj_t * my_obj,
192                         mm_camera_stream_t *stream)
193 {
194     int idx = -1, rc = MM_CAMERA_OK;
195     uint32_t i = 0;
196     struct v4l2_buffer vb;
197     struct v4l2_plane planes[VIDEO_MAX_PLANES];
198 
199     memset(&vb,  0,  sizeof(vb));
200     vb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
201     vb.memory = V4L2_MEMORY_USERPTR;
202     vb.m.planes = &planes[0];
203     vb.length = stream->fmt.fmt.pix_mp.num_planes;
204 
205     CDBG("%s: VIDIOC_DQBUF ioctl call\n", __func__);
206     rc = ioctl(stream->fd, VIDIOC_DQBUF, &vb);
207     if (rc < 0)
208         return idx;
209     idx = vb.index;
210     for(i = 0; i < vb.length; i++) {
211         CDBG("%s plane %d addr offset: %d data offset:%d\n",
212              __func__, i, vb.m.planes[i].reserved[0],
213              vb.m.planes[i].data_offset);
214         stream->frame.frame[idx].planes[i].reserved[0] =
215             vb.m.planes[i].reserved[0];
216         stream->frame.frame[idx].planes[i].data_offset =
217             vb.m.planes[i].data_offset;
218     }
219 
220     stream->frame.frame[idx].frame.frame_id = vb.sequence;
221     stream->frame.frame[idx].frame.ts.tv_sec  = vb.timestamp.tv_sec;
222     stream->frame.frame[idx].frame.ts.tv_nsec = vb.timestamp.tv_usec * 1000;
223 
224     CDBG("%s:type=%d,frame idx=%d, frame_id %d\n", __func__,
225       stream->stream_type, idx, vb.sequence);
226 
227     return idx;
228 }
229 
mm_camera_stream_util_proc_get_crop(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream,mm_camera_rect_t * val)230 static int mm_camera_stream_util_proc_get_crop(mm_camera_obj_t *my_obj,
231                             mm_camera_stream_t *stream,
232                             mm_camera_rect_t *val)
233 {
234   struct v4l2_crop crop;
235   int rc = MM_CAMERA_OK;
236   memset(&crop, 0, sizeof(crop));
237   crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
238   rc = ioctl(stream->fd, VIDIOC_G_CROP, &crop);
239   if (rc < 0)
240       return rc;
241   val->left = crop.c.left;
242   val->top = crop.c.top;
243   val->width = crop.c.width;
244   val->height = crop.c.height;
245   return rc;
246 }
247 
mm_camera_util_s_ctrl(int32_t fd,uint32_t id,int32_t value)248 int32_t mm_camera_util_s_ctrl( int32_t fd,  uint32_t id, int32_t value)
249 {
250     int rc = MM_CAMERA_OK;
251     struct v4l2_control control;
252 
253     memset(&control, 0, sizeof(control));
254     control.id = id;
255     control.value = value;
256     rc = ioctl (fd, VIDIOC_S_CTRL, &control);
257 
258     if(rc) {
259         CDBG("%s: fd=%d, S_CTRL, id=0x%x, value = 0x%x, rc = %ld\n",
260                  __func__, fd, id, (uint32_t)value, rc);
261         rc = MM_CAMERA_E_GENERAL;
262     }
263     return rc;
264 }
265 
mm_camera_util_private_s_ctrl(int32_t fd,uint32_t id,int32_t value)266 int32_t mm_camera_util_private_s_ctrl(int32_t fd,  uint32_t id, int32_t value)
267 {
268     int rc = MM_CAMERA_OK;
269     struct msm_camera_v4l2_ioctl_t v4l2_ioctl;
270 
271     memset(&v4l2_ioctl, 0, sizeof(v4l2_ioctl));
272     v4l2_ioctl.id = id;
273     v4l2_ioctl.ioctl_ptr = value;
274     rc = ioctl (fd, MSM_CAM_V4L2_IOCTL_PRIVATE_S_CTRL, &v4l2_ioctl);
275 
276     if(rc) {
277         CDBG_ERROR("%s: fd=%d, S_CTRL, id=0x%x, value = 0x%x, rc = %ld\n",
278                  __func__, fd, id, (uint32_t)value, rc);
279         rc = MM_CAMERA_E_GENERAL;
280     }
281     return rc;
282 }
283 
mm_camera_util_g_ctrl(int32_t fd,uint32_t id,int32_t * value)284 int32_t mm_camera_util_g_ctrl( int32_t fd, uint32_t id, int32_t *value)
285 {
286     int rc = MM_CAMERA_OK;
287   struct v4l2_control control;
288 
289     memset(&control, 0, sizeof(control));
290     control.id = id;
291     control.value = (int32_t)value;
292     rc = ioctl (fd, VIDIOC_G_CTRL, &control);
293     if(rc) {
294         CDBG("%s: fd=%d, G_CTRL, id=0x%x, rc = %d\n", __func__, fd, id, rc);
295         rc = MM_CAMERA_E_GENERAL;
296     }
297     *value = control.value;
298     return rc;
299 }
300 
mm_camera_util_get_v4l2_fmt(cam_format_t fmt,uint8_t * num_planes)301 static uint32_t mm_camera_util_get_v4l2_fmt(cam_format_t fmt,
302                                             uint8_t *num_planes)
303 {
304     uint32_t val;
305     switch(fmt) {
306     case CAMERA_YUV_420_NV12:
307         val = V4L2_PIX_FMT_NV12;
308         *num_planes = 2;
309         break;
310     case CAMERA_YUV_420_NV21:
311         val = V4L2_PIX_FMT_NV21;
312         *num_planes = 2;
313         break;
314     case CAMERA_BAYER_SBGGR10:
315         val= V4L2_PIX_FMT_SBGGR10;
316         *num_planes = 1;
317         break;
318     case CAMERA_YUV_422_NV61:
319         val= V4L2_PIX_FMT_NV61;
320         *num_planes = 2;
321         break;
322     case CAMERA_YUV_422_YUYV:
323         val= V4L2_PIX_FMT_YUYV;
324         *num_planes = 1;
325         break;
326     case CAMERA_YUV_420_YV12:
327         val= V4L2_PIX_FMT_NV12;
328         *num_planes = 3;
329          break;
330     default:
331         val = 0;
332         *num_planes = 0;
333         break;
334     }
335     return val;
336 }
337 
mm_camera_stream_util_set_ext_mode(mm_camera_stream_t * stream)338 static int mm_camera_stream_util_set_ext_mode(mm_camera_stream_t *stream)
339 {
340     int rc = 0;
341     struct v4l2_streamparm s_parm;
342     s_parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
343         switch(stream->stream_type) {
344         case MM_CAMERA_STREAM_PREVIEW:
345             s_parm.parm.capture.extendedmode = MSM_V4L2_EXT_CAPTURE_MODE_PREVIEW;
346             break;
347         case MM_CAMERA_STREAM_SNAPSHOT:
348             s_parm.parm.capture.extendedmode = MSM_V4L2_EXT_CAPTURE_MODE_MAIN;
349             break;
350         case MM_CAMERA_STREAM_THUMBNAIL:
351             s_parm.parm.capture.extendedmode = MSM_V4L2_EXT_CAPTURE_MODE_THUMBNAIL;
352             break;
353         case MM_CAMERA_STREAM_VIDEO:
354             s_parm.parm.capture.extendedmode = MSM_V4L2_EXT_CAPTURE_MODE_VIDEO;
355             break;
356         case MM_CAMERA_STREAM_RAW:
357                 s_parm.parm.capture.extendedmode = MSM_V4L2_EXT_CAPTURE_MODE_MAIN; //MSM_V4L2_EXT_CAPTURE_MODE_RAW;
358                 break;
359         case MM_CAMERA_STREAM_VIDEO_MAIN:
360         default:
361             return 0;
362         }
363 
364     rc = ioctl(stream->fd, VIDIOC_S_PARM, &s_parm);
365         CDBG("%s:stream fd=%d,type=%d,rc=%d,extended_mode=%d\n",
366                  __func__, stream->fd, stream->stream_type, rc,
367                  s_parm.parm.capture.extendedmode);
368     return rc;
369 }
370 
mm_camera_util_set_op_mode(int fd,int opmode)371 static int mm_camera_util_set_op_mode(int fd, int opmode)
372 {
373     int rc = 0;
374     struct v4l2_control s_ctrl;
375     s_ctrl.id = MSM_V4L2_PID_CAM_MODE;
376     s_ctrl.value = opmode;
377 
378         rc = ioctl(fd, VIDIOC_S_CTRL, &s_ctrl);
379     if (rc < 0)
380         CDBG("%s: VIDIOC_S_CTRL failed, rc=%d\n",
381                          __func__, rc);
382     return rc;
383 }
384 
mm_camera_stream_qbuf(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream,int idx)385 int mm_camera_stream_qbuf(mm_camera_obj_t * my_obj, mm_camera_stream_t *stream,
386   int idx)
387 {
388   int32_t i, rc = MM_CAMERA_OK;
389   int *ret;
390   struct v4l2_buffer buffer;
391 
392   memset(&buffer, 0, sizeof(buffer));
393   buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
394   buffer.memory = V4L2_MEMORY_USERPTR;
395   buffer.index = idx;
396   buffer.m.planes = &(stream->frame.frame[idx].planes[0]);
397   buffer.length = stream->frame.frame[idx].num_planes;
398 
399   CDBG("%s Ref : PREVIEW=%d VIDEO=%d SNAPSHOT=%d THUMB=%d ", __func__,
400     MM_CAMERA_STREAM_PREVIEW, MM_CAMERA_STREAM_VIDEO,
401     MM_CAMERA_STREAM_SNAPSHOT, MM_CAMERA_STREAM_THUMBNAIL);
402   CDBG("%s:fd=%d,type=%d,frame idx=%d,num planes %d\n", __func__,
403     stream->fd, stream->stream_type, idx, buffer.length);
404 
405   rc = ioctl(stream->fd, VIDIOC_QBUF, &buffer);
406   if (rc < 0) {
407       CDBG_ERROR("%s: VIDIOC_QBUF error = %d, stream type=%d, errno = %d\n", __func__, rc, stream->stream_type, errno);
408       return rc;
409   }
410   CDBG("%s: X idx: %d, stream_type:%d", __func__, idx, stream->stream_type);
411   return rc;
412 }
413 
414 /* This function let kernel know amount of buffers will be registered */
mm_camera_stream_util_request_buf(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream,int8_t buf_num)415 static int mm_camera_stream_util_request_buf(mm_camera_obj_t * my_obj,
416                       mm_camera_stream_t *stream,
417                       int8_t buf_num)
418 {
419     int32_t rc = MM_CAMERA_OK;
420     struct v4l2_requestbuffers bufreq;
421 
422     if(buf_num > MM_CAMERA_MAX_NUM_FRAMES) {
423         rc = -MM_CAMERA_E_GENERAL;
424         CDBG("%s: buf num %d > max limit %d\n",
425                  __func__, buf_num, MM_CAMERA_MAX_NUM_FRAMES);
426         goto end;
427     }
428 
429     memset(&bufreq, 0, sizeof(bufreq));
430     bufreq.count = buf_num;
431     bufreq.type  = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
432     bufreq.memory = V4L2_MEMORY_USERPTR;
433     rc = ioctl(stream->fd, VIDIOC_REQBUFS, &bufreq);
434     if (rc < 0) {
435       CDBG("%s: fd=%d, ioctl VIDIOC_REQBUFS failed: rc=%d\n",
436         __func__, stream->fd, rc);
437       goto end;
438     }
439     ALOGV("%s: stream fd=%d, ioctl VIDIOC_REQBUFS: memtype = %d, num_frames = %d, rc=%d\n",
440         __func__, stream->fd, bufreq.memory, bufreq.count, rc);
441 
442 end:
443     return rc;
444 }
445 
mm_camera_stream_util_reg_buf(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream,mm_camera_buf_def_t * vbuf)446 static int mm_camera_stream_util_reg_buf(mm_camera_obj_t * my_obj,
447                       mm_camera_stream_t *stream,
448                       mm_camera_buf_def_t *vbuf)
449 {
450     int32_t i, rc = MM_CAMERA_OK, j;
451     int *ret;
452     struct v4l2_requestbuffers bufreq;
453     int image_type;
454     uint8_t num_planes;
455     uint32_t planes[VIDEO_MAX_PLANES];
456 
457     if(vbuf->num > MM_CAMERA_MAX_NUM_FRAMES) {
458         rc = -MM_CAMERA_E_GENERAL;
459         CDBG_ERROR("%s: buf num %d > max limit %d\n",
460                  __func__, vbuf->num, MM_CAMERA_MAX_NUM_FRAMES);
461         goto end;
462     }
463     switch(stream->stream_type) {
464     case MM_CAMERA_STREAM_PREVIEW:
465       image_type = OUTPUT_TYPE_P;
466       break;
467     case MM_CAMERA_STREAM_SNAPSHOT:
468     case MM_CAMERA_STREAM_RAW:
469       image_type = OUTPUT_TYPE_S;
470       break;
471     case MM_CAMERA_STREAM_THUMBNAIL:
472       image_type = OUTPUT_TYPE_T;
473       break;
474     case MM_CAMERA_STREAM_VIDEO:
475     default:
476       image_type = OUTPUT_TYPE_V;
477       break;
478     }
479     stream->frame.frame_len = mm_camera_get_msm_frame_len(stream->cam_fmt,
480                               my_obj->current_mode,
481                               stream->fmt.fmt.pix.width,
482                               stream->fmt.fmt.pix.height,
483                               image_type, &num_planes, planes);
484     if(stream->frame.frame_len == 0) {
485         CDBG_ERROR("%s:incorrect frame size = %d\n", __func__, stream->frame.frame_len);
486         rc = -1;
487         goto end;
488     }
489     stream->frame.num_frame = vbuf->num;
490     bufreq.count = stream->frame.num_frame;
491     bufreq.type  = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
492     bufreq.memory = V4L2_MEMORY_USERPTR;
493     CDBG("%s: calling VIDIOC_REQBUFS - fd=%d, num_buf=%d, type=%d, memory=%d\n",
494              __func__,stream->fd, bufreq.count, bufreq.type, bufreq.memory);
495     rc = ioctl(stream->fd, VIDIOC_REQBUFS, &bufreq);
496     if (rc < 0) {
497       CDBG_ERROR("%s: fd=%d, ioctl VIDIOC_REQBUFS failed: rc=%d\n",
498         __func__, stream->fd, rc);
499       goto end;
500     }
501     CDBG("%s: stream fd=%d, ioctl VIDIOC_REQBUFS: memtype = %d,"
502       "num_frames = %d, rc=%d\n", __func__, stream->fd, bufreq.memory,
503       bufreq.count, rc);
504 
505     for(i = 0; i < vbuf->num; i++){
506         vbuf->buf.mp[i].idx = i; /* remember the index to stream frame if first time qbuf */
507         memcpy(&stream->frame.frame[i].frame, &(vbuf->buf.mp[i].frame),
508                      sizeof(vbuf->buf.mp[i].frame));
509         stream->frame.frame[i].idx = i;
510         stream->frame.frame[i].num_planes = vbuf->buf.mp[i].num_planes;
511         for(j = 0; j < vbuf->buf.mp[i].num_planes; j++) {
512             stream->frame.frame[i].planes[j] = vbuf->buf.mp[i].planes[j];
513         }
514 
515         if(vbuf->buf.mp[i].frame_offset) {
516             stream->frame.frame_offset[i] = vbuf->buf.mp[i].frame_offset;
517         } else {
518             stream->frame.frame_offset[i] = 0;
519         }
520 
521         if (!vbuf->no_enqueue_flag[i]) {
522             rc = mm_camera_stream_qbuf(my_obj, stream, stream->frame.frame[i].idx);
523             if (rc < 0) {
524                 CDBG_ERROR("%s: VIDIOC_QBUF rc = %d\n", __func__, rc);
525                 goto end;
526             }
527             stream->frame.ref_count[i] = 0;
528         } else {
529             stream->frame.ref_count[i] = 1;
530         }
531         stream->frame.reg_flag[i] = vbuf->no_enqueue_flag[i];
532         CDBG("%s: stream_fd = %d, frame_fd = %d, frame ID = %d, offset = %d, reg_flag = %d, ref_cnt = %d\n",
533           __func__, stream->fd, stream->frame.frame[i].frame.fd,
534           i, stream->frame.frame_offset[i], stream->frame.reg_flag[i], stream->frame.ref_count[i]);
535     }
536     stream->frame.qbuf = 1;
537 end:
538     return rc;
539 }
mm_camera_stream_util_unreg_buf(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream)540 static int mm_camera_stream_util_unreg_buf(mm_camera_obj_t * my_obj,
541                           mm_camera_stream_t *stream)
542 {
543     struct v4l2_requestbuffers bufreq;
544     int32_t i, rc = MM_CAMERA_OK;
545 
546     bufreq.count = 0;
547     bufreq.type  = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
548     bufreq.memory = V4L2_MEMORY_USERPTR;
549     rc = ioctl(stream->fd, VIDIOC_REQBUFS, &bufreq);
550     if (rc < 0) {
551         CDBG_ERROR("%s: fd=%d, VIDIOC_REQBUFS failed, rc=%d\n",
552               __func__, stream->fd, rc);
553         return rc;
554     }
555     mm_stream_frame_flash_q(&stream->frame.readyq);
556     memset(stream->frame.ref_count,0,(stream->frame.num_frame * sizeof(int8_t)));
557     stream->frame.qbuf = 0;
558     CDBG("%s:fd=%d,type=%d,rc=%d\n", __func__, stream->fd,
559       stream->stream_type, rc);
560     return rc;
561 }
562 
mm_camera_stream_fsm_notused(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream,mm_camera_state_evt_type_t evt,void * val)563 static int32_t mm_camera_stream_fsm_notused(mm_camera_obj_t * my_obj,
564                          mm_camera_stream_t *stream,
565                          mm_camera_state_evt_type_t evt, void *val)
566 {
567     int32_t rc = 0;
568     char dev_name[MM_CAMERA_DEV_NAME_LEN];
569 
570     switch(evt) {
571     case MM_CAMERA_STATE_EVT_ACQUIRE:
572         snprintf(dev_name, sizeof(dev_name), "/dev/%s", mm_camera_util_get_dev_name(my_obj));
573         CDBG("%s: open dev '%s', stream type = %d\n",
574                  __func__, dev_name, *((mm_camera_stream_type_t *)val));
575         stream->fd = open(dev_name, O_RDWR | O_NONBLOCK);
576         if(stream->fd <= 0){
577             CDBG("%s: open dev returned %d\n", __func__, stream->fd);
578             return -1;
579         }
580         stream->stream_type = *((mm_camera_stream_type_t *)val);
581         rc = mm_camera_stream_util_set_ext_mode(stream);
582         CDBG("%s: fd=%d, stream type=%d, mm_camera_stream_util_set_ext_mode() err=%d\n",
583                  __func__, stream->fd, stream->stream_type, rc);
584         if(rc == MM_CAMERA_OK) {
585             mm_camera_stream_init_frame(&stream->frame);
586             mm_camera_stream_util_set_state(stream, MM_CAMERA_STREAM_STATE_ACQUIRED);
587         } else if(stream->fd > 0) {
588             close(stream->fd);
589             stream->fd = 0;
590         }
591         break;
592     default:
593         CDBG_ERROR("%s: Invalid evt=%d, stream_state=%d", __func__, evt,
594           stream->state);
595         return -1;
596     }
597     return rc;
598 }
599 
mm_camera_stream_util_proc_fmt(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream,mm_camera_image_fmt_t * fmt)600 static int32_t mm_camera_stream_util_proc_fmt(mm_camera_obj_t *my_obj,
601     mm_camera_stream_t *stream,
602     mm_camera_image_fmt_t *fmt)
603 {
604     int32_t rc = MM_CAMERA_OK;
605 
606     if(fmt->dim.width == 0 || fmt->dim.height == 0) {
607         rc = -MM_CAMERA_E_INVALID_INPUT;
608         CDBG("%s:invalid input[w=%d,h=%d,fmt=%d]\n",
609                  __func__, fmt->dim.width, fmt->dim.height, fmt->fmt);
610         goto end;
611     }
612     CDBG("%s: dw=%d,dh=%d,vw=%d,vh=%d,pw=%d,ph=%d,tw=%d,th=%d,raw_w=%d,raw_h=%d,fmt=%d\n",
613        __func__,
614        my_obj->dim.display_width,my_obj->dim.display_height,
615        my_obj->dim.video_width,my_obj->dim.video_height,
616        my_obj->dim.picture_width,my_obj->dim.picture_height,
617        my_obj->dim.ui_thumbnail_width,my_obj->dim.ui_thumbnail_height,
618        my_obj->dim.raw_picture_width,my_obj->dim.raw_picture_height,fmt->fmt);
619     stream->cam_fmt = fmt->fmt;
620     stream->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
621     stream->fmt.fmt.pix_mp.width = fmt->dim.width;
622     stream->fmt.fmt.pix_mp.height= fmt->dim.height;
623     stream->fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;
624     stream->fmt.fmt.pix_mp.pixelformat =
625             mm_camera_util_get_v4l2_fmt(stream->cam_fmt,
626                                       &(stream->fmt.fmt.pix_mp.num_planes));
627     rc = ioctl(stream->fd, VIDIOC_S_FMT, &stream->fmt);
628     if (rc < 0) {
629         CDBG("%s: ioctl VIDIOC_S_FMT failed: rc=%d\n", __func__, rc);
630         rc = -MM_CAMERA_E_GENERAL;
631     }
632 end:
633     CDBG("%s:fd=%d,type=%d,rc=%d\n",
634              __func__, stream->fd, stream->stream_type, rc);
635     return rc;
636 }
mm_camera_stream_fsm_acquired(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream,mm_camera_state_evt_type_t evt,void * val)637 static int32_t mm_camera_stream_fsm_acquired(mm_camera_obj_t * my_obj,
638                   mm_camera_stream_t *stream,
639                   mm_camera_state_evt_type_t evt, void *val)
640 {
641     int32_t rc = 0;
642 
643     switch(evt) {
644     case MM_CAMERA_STATE_EVT_SET_FMT:
645         rc = mm_camera_stream_util_proc_fmt(my_obj,stream,
646                     (mm_camera_image_fmt_t *)val);
647         if(!rc) mm_camera_stream_util_set_state(stream, MM_CAMERA_STREAM_STATE_CFG);
648         break;
649     case MM_CAMERA_STATE_EVT_RELEASE:
650         mm_camera_stream_release(stream);
651         break;
652     case MM_CAMERA_STATE_EVT_GET_CROP:
653       rc = mm_camera_stream_util_proc_get_crop(my_obj,stream, val);
654       break;
655     default:
656         CDBG_ERROR("%s: Invalid evt=%d, stream_state=%d", __func__, evt,
657           stream->state);
658         return -1;
659     }
660     return rc;
661 }
mm_camera_stream_fsm_cfg(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream,mm_camera_state_evt_type_t evt,void * val)662 static int32_t mm_camera_stream_fsm_cfg(mm_camera_obj_t * my_obj,
663                          mm_camera_stream_t *stream,
664                          mm_camera_state_evt_type_t evt, void *val)
665 {
666     int32_t rc = 0;
667     switch(evt) {
668     case MM_CAMERA_STATE_EVT_RELEASE:
669         mm_camera_stream_release(stream);
670         break;
671     case MM_CAMERA_STATE_EVT_SET_FMT:
672         rc = mm_camera_stream_util_proc_fmt(my_obj,stream,
673                     (mm_camera_image_fmt_t *)val);
674         break;
675     case MM_CAMERA_STATE_EVT_REG_BUF:
676         rc = mm_camera_stream_util_reg_buf(my_obj, stream, (mm_camera_buf_def_t *)val);
677         if(!rc) mm_camera_stream_util_set_state(stream, MM_CAMERA_STREAM_STATE_REG);
678         break;
679     case MM_CAMERA_STATE_EVT_GET_CROP:
680       rc = mm_camera_stream_util_proc_get_crop(my_obj,stream, val);
681       break;
682     case MM_CAMERA_STATE_EVT_REQUEST_BUF:
683         rc = mm_camera_stream_util_request_buf(my_obj, stream, ((mm_camera_buf_def_t *)val)->num);
684         break;
685     default:
686         CDBG_ERROR("%s: Invalid evt=%d, stream_state=%d", __func__, evt,
687           stream->state);
688         return -1;
689     }
690     return rc;
691 }
692 
mm_camera_stream_util_buf_done(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream,mm_camera_notify_frame_t * frame)693 int32_t mm_camera_stream_util_buf_done(mm_camera_obj_t * my_obj,
694                     mm_camera_stream_t *stream,
695                     mm_camera_notify_frame_t *frame)
696 {
697     int32_t rc = MM_CAMERA_OK;
698     pthread_mutex_lock(&stream->frame.mutex);
699 
700     if(stream->frame.ref_count[frame->idx] == 0) {
701         rc = mm_camera_stream_qbuf(my_obj, stream, frame->idx);
702         CDBG_ERROR("%s: Error Trying to free second time?(idx=%d) count=%d, stream type=%d\n",
703                    __func__, frame->idx, stream->frame.ref_count[frame->idx], stream->stream_type);
704         rc = -1;
705     }else{
706         stream->frame.ref_count[frame->idx]--;
707         if(0 == stream->frame.ref_count[frame->idx]) {
708             CDBG("<DEBUG> : Buf done for buffer:%p:%d",stream,frame->idx);
709             rc = mm_camera_stream_qbuf(my_obj, stream, frame->idx);
710             if(rc < 0)
711                 CDBG_ERROR("%s: mm_camera_stream_qbuf(idx=%d) err=%d\n",
712                      __func__, frame->idx, rc);
713         }else{
714             CDBG("<DEBUG> : Still ref count pending count :%d",stream->frame.ref_count[frame->idx]);
715             CDBG("<DEBUG> : for buffer:%p:%d, stream type=%d",stream,frame->idx, stream->stream_type);
716         }
717     }
718 
719 #if 0
720     stream->frame.ref_count[frame->idx]--;
721     if(stream->frame.ref_count[frame->idx] == 0) {
722         CDBG("%s: Queue the buffer (idx=%d) count=%d frame id = %d\n",
723                  __func__, frame->idx, stream->frame.ref_count[frame->idx],
724                  frame->frame->frame_id);
725         rc = mm_camera_stream_qbuf(my_obj, stream, frame->idx);
726         if(rc < 0)
727           CDBG_ERROR("%s: mm_camera_stream_qbuf(idx=%d) err=%d\n", __func__,
728             frame->idx, rc);
729     } else if(stream->frame.ref_count[frame->idx] == 1) {
730         ALOGE("<DEBUG> : Buf done for buffer:%p:%d",stream,frame->idx);
731         rc = mm_camera_stream_qbuf(my_obj, stream, frame->idx);
732         if(rc < 0)
733             CDBG("%s: mm_camera_stream_qbuf(idx=%d) err=%d\n",
734                  __func__, frame->idx, rc);
735     } else {
736         CDBG_ERROR("%s: Error Trying to free second time?(idx=%d) count=%d\n",
737           __func__, frame->idx, stream->frame.ref_count[frame->idx]);
738         rc = -1;
739     }
740 #endif
741     pthread_mutex_unlock(&stream->frame.mutex);
742     return rc;
743 }
744 
mm_camera_stream_fsm_reg(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream,mm_camera_state_evt_type_t evt,void * val)745 static int32_t mm_camera_stream_fsm_reg(mm_camera_obj_t * my_obj,
746                              mm_camera_stream_t *stream,
747                              mm_camera_state_evt_type_t evt, void *val)
748 {
749     int32_t rc = 0;
750     switch(evt) {
751     case MM_CAMERA_STATE_EVT_GET_CROP:
752       rc = mm_camera_stream_util_proc_get_crop(my_obj,stream, val);
753       break;
754     case MM_CAMERA_STATE_EVT_QBUF:
755         break;
756     case MM_CAMERA_STATE_EVT_RELEASE:
757         mm_camera_stream_release(stream);
758         break;
759     case MM_CAMERA_STATE_EVT_UNREG_BUF:
760         rc = mm_camera_stream_util_unreg_buf(my_obj, stream);
761         if(!rc)
762             mm_camera_stream_util_set_state(stream, MM_CAMERA_STREAM_STATE_CFG);
763         break;
764     case MM_CAMERA_STATE_EVT_STREAM_ON:
765         {
766             enum v4l2_buf_type buf_type;
767             int i = 0;
768             mm_camera_frame_t *frame;
769             if(stream->frame.qbuf == 0) {
770                 CDBG("%s: queueing buffers during stream on", __func__);
771                 for(i = 0; i < stream->frame.num_frame; i++) {
772                     if (!stream->frame.reg_flag[i]) {
773                         rc = mm_camera_stream_qbuf(my_obj, stream,
774                                  stream->frame.frame[i].idx);
775                         if (rc < 0) {
776                             CDBG_ERROR("%s: ioctl VIDIOC_QBUF error=%d, stream->type=%d\n",
777                                __func__, rc, stream->stream_type);
778                             return rc;
779                         }
780                     }
781                     stream->frame.ref_count[i] = 0;
782                 }
783                 stream->frame.qbuf = 1;
784             }
785             buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
786             CDBG("%s: STREAMON,fd=%d,stream_type=%d\n",
787                      __func__, stream->fd, stream->stream_type);
788             rc = ioctl(stream->fd, VIDIOC_STREAMON, &buf_type);
789             if (rc < 0) {
790                     CDBG_ERROR("%s: ioctl VIDIOC_STREAMON failed: rc=%d\n",
791                         __func__, rc);
792             }
793             else
794                 mm_camera_stream_util_set_state(stream, MM_CAMERA_STREAM_STATE_ACTIVE);
795         }
796         break;
797     default:
798         CDBG_ERROR("%s: Invalid evt=%d, stream_state=%d", __func__, evt,
799           stream->state);
800         return -1;
801     }
802     return rc;
803 }
mm_camera_stream_fsm_active(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream,mm_camera_state_evt_type_t evt,void * val)804 static int32_t mm_camera_stream_fsm_active(mm_camera_obj_t * my_obj,
805                mm_camera_stream_t *stream,
806                mm_camera_state_evt_type_t evt, void *val)
807 {
808     int32_t rc = 0;
809     switch(evt) {
810     case MM_CAMERA_STATE_EVT_GET_CROP:
811       rc = mm_camera_stream_util_proc_get_crop(my_obj,stream, val);
812       break;
813     case MM_CAMERA_STATE_EVT_QBUF:
814         rc = mm_camera_stream_util_buf_done(my_obj, stream,
815           (mm_camera_notify_frame_t *)val);
816         break;
817     case MM_CAMERA_STATE_EVT_RELEASE:
818         mm_camera_stream_release(stream);
819         break;
820     case MM_CAMERA_STATE_EVT_STREAM_OFF:
821         {
822             enum v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
823             CDBG("%s: STREAMOFF,fd=%d,type=%d\n",
824                 __func__, stream->fd, stream->stream_type);
825             rc = ioctl(stream->fd, VIDIOC_STREAMOFF, &buf_type);
826             if (rc < 0) {
827                     CDBG_ERROR("%s: STREAMOFF failed: %s\n",
828                         __func__, strerror(errno));
829             }
830             else {
831                 stream->frame.qbuf = 0;
832                 mm_camera_stream_util_set_state(stream, MM_CAMERA_STREAM_STATE_REG);
833             }
834         }
835         break;
836    default:
837         CDBG_ERROR("%s: Invalid evt=%d, stream_state=%d", __func__, evt,
838           stream->state);
839         return -1;
840     }
841     return rc;
842 }
843 
844 typedef int32_t (*mm_camera_stream_fsm_fn_t) (mm_camera_obj_t * my_obj,
845                     mm_camera_stream_t *stream,
846                     mm_camera_state_evt_type_t evt, void *val);
847 
848 static mm_camera_stream_fsm_fn_t mm_camera_stream_fsm_fn[MM_CAMERA_STREAM_STATE_MAX] = {
849     mm_camera_stream_fsm_notused,
850     mm_camera_stream_fsm_acquired,
851     mm_camera_stream_fsm_cfg,
852     mm_camera_stream_fsm_reg,
853     mm_camera_stream_fsm_active
854 };
mm_camera_stream_fsm_fn_vtbl(mm_camera_obj_t * my_obj,mm_camera_stream_t * stream,mm_camera_state_evt_type_t evt,void * val)855 int32_t mm_camera_stream_fsm_fn_vtbl (mm_camera_obj_t * my_obj,
856                    mm_camera_stream_t *stream,
857                    mm_camera_state_evt_type_t evt, void *val)
858 {
859     CDBG("%s: stream fd=%d, type = %d, state=%d, evt\n",
860                  __func__, stream->fd, stream->stream_type, stream->state, evt);
861     return mm_camera_stream_fsm_fn[stream->state] (my_obj, stream, evt, val);
862 }
863 
864