path: root/camera/V4LCameraAdapter/V4LM2M.cpp

/*
 * Copyright (C) Texas Instruments - http://www.ti.com/
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/**
* @file V4LM2M.cpp
*
* This file implements the V4L M2M color conversion / scaling using VPE h/w block
*
*/
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>
#include <errno.h>

#include <linux/videodev2.h>
#include <linux/v4l2-controls.h>

#include <sys/mman.h>
#include <sys/ioctl.h>
#include <utils/Log.h>
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <time.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/select.h>
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <time.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <dirent.h>
#include <ui/GraphicBuffer.h>
#include "CameraHal.h"

#define pexit(str) { \
	ALOGE(str); \
	return -EINVAL; \
}

#define V4L2_CID_TRANS_NUM_BUFS         (V4L2_CID_PRIVATE_BASE)
#define DEVICE "/dev/videoxx"
#define DEVICE_PATH "/dev/"
#define DEVICE_NAME "videoxx"


/**<	File descriptor for device			*/
static int	fd   = -1;

static char    *video_dev;

static int streaming = 0;
static int  field = V4L2_FIELD_TOP;
static int init_queued_count = 0;
static enum v4l2_colorspace    src_colorspace, dst_colorspace;
static int src_coplanar = 0, dst_coplanar = 0;
static int srcSize    = 0, dstSize   = 0, srcSize_uv = 0, dstSize_uv = 0;
static int src_numbuf = 6;
static int dst_numbuf = 6;
static int startup_threshold = 2;
/*
 * Convert a format name string into fourcc and calculate the
 * size of one frame and set flags
 */
int describeFormat (
	const char	*format,
	int	width,
	int	height,
	int	*size,
	int	*fourcc,
	int	*coplanar,
	enum v4l2_colorspace *clrspc)
{
	*size   = -1;
	*fourcc = -1;
	if (strcmp (format, "rgb24") == 0) {
		*fourcc = V4L2_PIX_FMT_RGB24;
		*size = height * width * 3;
		*coplanar = 0;
		*clrspc = V4L2_COLORSPACE_SRGB;

	} else if (strcmp (format, "bgr24") == 0) {
		*fourcc = V4L2_PIX_FMT_BGR24;
		*size = height * width * 3;
		*coplanar = 0;
		*clrspc = V4L2_COLORSPACE_SRGB;

	} else if (strcmp (format, "argb32") == 0) {
		*fourcc = V4L2_PIX_FMT_RGB32;
		*size = height * width * 4;
		*coplanar = 0;
		*clrspc = V4L2_COLORSPACE_SRGB;

	} else if (strcmp (format, "abgr32") == 0) {
		*fourcc = V4L2_PIX_FMT_BGR32;
		*size = height * width * 4;
		*coplanar = 0;
		*clrspc = V4L2_COLORSPACE_SRGB;

	} else if (strcmp (format, "yuv444") == 0) {
		*fourcc = V4L2_PIX_FMT_YUV444;
		*size = height * width * 3;
		*coplanar = 0;
		*clrspc = V4L2_COLORSPACE_SMPTE170M;

	} else if (strcmp (format, "yvyu") == 0) {
		*fourcc = V4L2_PIX_FMT_YVYU;
		*size = height * width * 2;
		*coplanar = 0;
		*clrspc = V4L2_COLORSPACE_SMPTE170M;

	} else if (strcmp (format, "yuyv") == 0) {
		*fourcc = V4L2_PIX_FMT_YUYV;
		*size = height * width * 2;
		*coplanar = 0;
		*clrspc = V4L2_COLORSPACE_SMPTE170M;

	} else if (strcmp (format, "uyvy") == 0) {
		*fourcc = V4L2_PIX_FMT_UYVY;
		*size = height * width * 2;
		*coplanar = 0;
		*clrspc = V4L2_COLORSPACE_SMPTE170M;

	} else if (strcmp (format, "vyuy") == 0) {
		*fourcc = V4L2_PIX_FMT_VYUY;
		*size = height * width * 2;
		*coplanar = 0;
		*clrspc = V4L2_COLORSPACE_SMPTE170M;

	} else if (strcmp (format, "nv16") == 0) {
		*fourcc = V4L2_PIX_FMT_NV16;
		*size = height * width * 2;
		*coplanar = 0;
		*clrspc = V4L2_COLORSPACE_SMPTE170M;

	} else if (strcmp (format, "nv61") == 0) {
		*fourcc = V4L2_PIX_FMT_NV61;
		*size = height * width * 2;
		*coplanar = 0;
		*clrspc = V4L2_COLORSPACE_SMPTE170M;

	} else if (strcmp (format, "nv12") == 0) {
		*fourcc = V4L2_PIX_FMT_NV12;
		*size = height * width * 1.5;
		*coplanar = 1;
		*clrspc = V4L2_COLORSPACE_SMPTE170M;

	} else if (strcmp (format, "nv21") == 0) {
		*fourcc = V4L2_PIX_FMT_NV21;
		*size = height * width * 1.5;
		*coplanar = 1;
		*clrspc = V4L2_COLORSPACE_SMPTE170M;

	} else {
		return 0;

	}

	return 1;
}

/*
 * Set format of one of the CAPTURE or OUTPUT stream (passed as type)
 * Request to allocate memory mapped buffers, map them and store
 * the mapped addresses of y and uv buffers into the array
 */
int allocBuffers(
	int	type,
	int	width,
	int	height,
	int	coplanar,
	enum v4l2_colorspace clrspc,
	int	*sizeimage_y,
	int	*sizeimage_uv,
	int	fourcc,
	int	*numbuf,
	int	interlace)
{
	struct v4l2_format		fmt;
	struct v4l2_requestbuffers	reqbuf;
	struct v4l2_buffer		buffer;
	struct v4l2_plane		buf_planes[2];
	int				i = 0;
	int				ret = -1;

	bzero(&fmt,sizeof(fmt));
	fmt.type		= type;
	fmt.fmt.pix_mp.width	= width;
	fmt.fmt.pix_mp.height	= height;
	fmt.fmt.pix_mp.pixelformat = fourcc;
	fmt.fmt.pix_mp.colorspace = clrspc;

	if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE && interlace)
		fmt.fmt.pix_mp.field = V4L2_FIELD_ALTERNATE;
	else
		fmt.fmt.pix_mp.field = V4L2_FIELD_ANY;

	/* Set format and update the sizes of y and uv planes
	 * If the returned value is changed, it means driver corrected the size
	 */
	ret = ioctl(fd,VIDIOC_S_FMT,&fmt);
	if(ret < 0) {
		pexit("Cant set color format\n");
	} else {
		*sizeimage_y = fmt.fmt.pix_mp.plane_fmt[0].sizeimage;
		*sizeimage_uv = fmt.fmt.pix_mp.plane_fmt[1].sizeimage;
	}

	bzero(&reqbuf,sizeof(reqbuf));
	reqbuf.count	= *numbuf;
	reqbuf.type	= type;
	reqbuf.memory	= V4L2_MEMORY_DMABUF;

	ret = ioctl (fd, VIDIOC_REQBUFS, &reqbuf);
	if(ret < 0) {
		pexit("Cant request buffers\n");
	} else {
		*numbuf = reqbuf.count;
	}

	for(i = 0; i < *numbuf; i++) {

		memset(&buffer, 0, sizeof(buffer));
	        buffer.type	= type;
	        buffer.memory	= V4L2_MEMORY_DMABUF;
	        buffer.index	= i;
		buffer.m.planes	= buf_planes;
		buffer.length	= coplanar ? 2 : 1;

	        ret = ioctl (fd, VIDIOC_QUERYBUF, &buffer);
		if (ret < 0)
			pexit("Cant query buffers\n");

		ALOGV("query buf, plane 0 = %d, plane 1 = %d\n", buffer.m.planes[0].length,
			buffer.m.planes[1].length);
	}

	return 1;
}


/* Queue one buffer (identified by index) */
int queue(
	int	type,
	int	index,
	int	field,
    void* buf_handle,
	int	size_y,
	int size_uv)
{
	struct v4l2_buffer	buffer;
	struct v4l2_plane	buf_planes[2];
	int			ret = -1;
    int num_planes = 0;

    if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
        buf_planes[0].bytesused = size_y;
        buf_planes[1].bytesused = size_uv;
        buf_planes[0].length = size_y + size_uv;
        buf_planes[1].length = size_y + size_uv;
        buf_planes[0].data_offset = 0;
		buf_planes[1].data_offset = size_y;
        buf_planes[0].m.fd = ((buffer_handle_t)(buf_handle))->data[0];
        buf_planes[1].m.fd = ((buffer_handle_t)(buf_handle))->data[0];
        num_planes = 2;
    } else if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
        buf_planes[0].length = buf_planes[0].bytesused = ((Ti::Camera::CameraBuffer*)(buf_handle))->actual_size;
        buf_planes[0].data_offset = 0;
        buf_planes[0].m.fd = ((Ti::Camera::CameraBuffer*)(buf_handle))->dma_buf_fd;
        num_planes = 1;
    }

	memset(&buffer,0,sizeof(buffer));
	buffer.type	= type;
	buffer.memory	= V4L2_MEMORY_DMABUF;
	buffer.index	= index;
	buffer.m.planes	= buf_planes;
	buffer.field    = field;
	buffer.length	= 2;

	ret = ioctl (fd, VIDIOC_QBUF, &buffer);
	if(-1 == ret) {
		pexit("Failed to queue\n");
	}
	return ret;
}

/*
 * Dequeue one buffer
 * Index of dequeue buffer is returned by ioctl
 */
int dequeue(int	type, struct v4l2_buffer *buf, struct v4l2_plane *buf_planes)
{
	int			ret = -1;

	memset(buf, 0, sizeof(*buf));
	buf->type	= type;
	buf->memory	= V4L2_MEMORY_DMABUF;
	buf->m.planes	= buf_planes;
	buf->length	= 2;
	ret = ioctl (fd, VIDIOC_DQBUF, buf);
	if(-1 == ret) {
		pexit("Failed to dequeue\n");
	}
	return ret;
}

/* Start processing */
int streamON (
	int	type)
{
	int	ret = -1;
	ret = ioctl (fd, VIDIOC_STREAMON, &type);
	if(-1 == ret) {
		pexit("Cant Stream on\n");
	}

	return ret;
}

/* Stop processing */
int streamOFF (
	int	type)
{
	int	ret = -1;
	ret = ioctl (fd, VIDIOC_STREAMOFF, &type);
	if(-1 == ret) {
		pexit("Cant Stream on\n");
	}

	return ret;
}

//scan for video devices
void detectVideoDevice(char** video_device_list, int& num_device) {
    char dir_path[20];
    char* filename;
    char** dev_list = video_device_list;
    DIR *d;
    struct dirent *dir;
    int index = 0;

    strcpy(dir_path, DEVICE_PATH);
    d = opendir(dir_path);
    if(d) {
        //read each entry in the /dev/ and find if there is videox entry.
        while ((dir = readdir(d)) != NULL) {
            filename = dir->d_name;
            if (strncmp(filename, DEVICE_NAME, 5) == 0) {
                strcpy(dev_list[index],DEVICE_PATH);
                strncat(dev_list[index],filename,sizeof(DEVICE_NAME));
                index++;
            }
       } //end of while()
       closedir(d);
       num_device = index;

       for(int i=0; i<index; i++){
           ALOGV("Video device list::dev_list[%d]= %s",i,dev_list[i]);
       }
    }
}

static char device[15];
char *detectM2MDevice()
{
    char device_list[5][15];
    char* video_device_list[5];
    int num_v4l_devices = 0;
	int tempHandle = NULL;
	int ret;
	struct v4l2_capability cap;

	memset((void*)&cap, 0, sizeof(v4l2_capability));

    for (int i = 0; i < 5; i++) {
        video_device_list[i] = device_list[i];
    }
    //look for the connected video devices
    detectVideoDevice(video_device_list, num_v4l_devices);

    for (int i = 0; i < num_v4l_devices; i++) {
        ALOGV("Opening device[%d] = %s..",i, video_device_list[i]);
        if ((tempHandle = open(video_device_list[i], O_RDWR)) == -1) {
            ALOGE("Error while opening handle to V4L2 Camera(%s): %s",video_device_list[i], strerror(errno));
            continue;
        }

        ret = ioctl (tempHandle, VIDIOC_QUERYCAP, &cap);
        if (ret < 0) {
            ALOGE("Error when querying the capabilities of the V4L Camera");
            close(tempHandle);
            continue;
        }

        //check for video capture devices
        if (!((cap.capabilities & V4L2_CAP_VIDEO_M2M_MPLANE) && (cap.capabilities & V4L2_CAP_STREAMING))) {
            ALOGE("Error while adapter initialization: video capture not supported.");
            close(tempHandle);
            continue;
        }

        strcpy(device, video_device_list[i]);
		ALOGE("Found VPE M2M device %s!!!", device);
		break;
	}

	return device;
}

int setupM2MDevice()
{
	video_dev = detectM2MDevice();

    ALOGV("setupM2MDevice");

    return 0;
}

int closeM2MDevice()
{
    ALOGV("closeM2MDevice");

    return 0;
}

int startM2MDevice()
{
    ALOGV("startM2MDevice");

	fd = open(video_dev,O_RDWR);
	if(fd < 0) {
		pexit("Can't open device");
	}

    /* Just a NOP for now */
    return 0;
}

int stopM2MDevice()
{
    if (!streaming)
        return 0;

    ALOGV("stopM2MDevice");

	/* Driver cleanup */
	streamOFF (V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
	streamOFF (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);

	close(fd);

    streaming = 0;
    init_queued_count = 0;
    field = V4L2_FIELD_TOP;

    return 0;
}

int processFrame(
	void *srcHandle,
	int srcWidth,
	int srcHeight,
	int srcStride,
	const char *srcFmt,
	void *dstHandle,
	int dstWidth,
	int dstHeight,
	int dstStride,
	const char *dstFmt,
	bool dei,
	int translen, int index, int *in_index, int *out_index)
{
	int	i, ret = 0;

	int	srcFourcc  = 0, dstFourcc = 0;
	int	num_frames = 1;
	int	interlace = 0;
	struct	v4l2_control ctrl;
    num_frames = 1;

	interlace = dei;
	translen = translen;

    if (interlace)
        srcHeight /=2;

    if (!streaming && init_queued_count==0) {
        describeFormat (srcFmt, srcWidth, srcHeight, &srcSize, &srcFourcc, &src_coplanar, &src_colorspace);
        describeFormat (dstFmt, dstWidth, dstHeight, &dstSize, &dstFourcc, &dst_coplanar, &dst_colorspace);

        /* Number of buffers to process in one transaction - translen */
        memset(&ctrl, 0, sizeof(ctrl));
        ctrl.id = V4L2_CID_TRANS_NUM_BUFS;
        ctrl.value = translen;
        ret = ioctl(fd, VIDIOC_S_CTRL, &ctrl);
        if (ret < 0)
            pexit("Can't set translen control");

        /* Allocate buffers for CAPTURE and OUTPUT stream */
        ret = allocBuffers (V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, srcWidth,
            srcHeight, src_coplanar, src_colorspace, &srcSize, &srcSize_uv,
            srcFourcc, &src_numbuf, interlace);
        if(ret < 0) {
            pexit("Cant Allocate buffers for OUTPUT device\n");
        }

        ret = allocBuffers (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, dstWidth,
            dstHeight, dst_coplanar, dst_colorspace, &dstSize, &dstSize_uv,
            dstFourcc, &dst_numbuf, interlace);
        if(ret < 0) {
            pexit("Cant Allocate buffers for CAPTURE device\n");
        }

        ALOGI ("Start Streaming Input  = %d x %d , %d(%s)\nOutput = %d x %d , %d(%s)\n",
            srcWidth, srcHeight, srcFourcc, srcFmt,
            dstWidth, dstHeight, dstFourcc, dstFmt);

    }

	ALOGI ("Input  = %d x %d , %d(%s)\nOutput = %d x %d , %d(%s)\n",
		srcWidth, srcHeight, srcFourcc, srcFmt,
		dstWidth, dstHeight, dstFourcc, dstFmt);

    if (!streaming && init_queued_count != startup_threshold) {

        queue(V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, index, V4L2_FIELD_NONE, dstHandle, dstSize, dstSize_uv);


	/*************************************
		Driver is ready Now
	*************************************/

		queue(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, index, field, srcHandle, srcSize, srcSize_uv);
		if (field == V4L2_FIELD_TOP)
			field = V4L2_FIELD_BOTTOM;
		else
			field = V4L2_FIELD_TOP;
        init_queued_count++;
        return -1;
	}

    if(!streaming) {
        /*************************************
                 Data is ready Now
            *************************************/
        queue(V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, index, V4L2_FIELD_NONE, dstHandle, dstSize, dstSize_uv);
        queue(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, index, field, srcHandle, srcSize, srcSize_uv);

        streamON (V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
        streamON (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
    } else {
            queue(V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, index, V4L2_FIELD_NONE, dstHandle, dstSize, dstSize_uv);

        /*************************************
            Driver is ready Now
        *************************************/
            queue(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, index, field, srcHandle, srcSize, srcSize_uv);
            if (field == V4L2_FIELD_TOP)
                field = V4L2_FIELD_BOTTOM;
            else
                field = V4L2_FIELD_TOP;
    }

	while (num_frames) {
		struct v4l2_buffer buf;
		struct v4l2_plane buf_planes[2];

		/* Wait for and dequeue one buffer from OUTPUT
		 * to write data for next interlaced field */
		dequeue(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, &buf, buf_planes);
		ALOGV("dequeued source buffer with index %d\n", buf.index);
        *in_index = buf.index;

		/* Dequeue progressive frame from CAPTURE stream
		 * write to the file and queue one empty buffer */
		dequeue(V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, &buf, buf_planes);
	    ALOGV("dequeued dest buffer with index %d\n", buf.index);

        *out_index = buf.index;
		num_frames--;

		ALOGV("frames left %d\n", num_frames);
	}

    streaming = 1;
	return 0;
}