v4l2 FPGA capture crash in AM5728 based board

/*
 * drivers/media/evis/evis_capture.c
 *
 * EVIS FPGA driver
 *
 * Author: Amritpreet Singh <amritpreet.singh@logic-fruit.com>
 *
 * This package is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 */

#include <linux/slab.h>
#include <linux/delay.h>
#include <linux/videodev2.h>
#include <linux/module.h>
#include <linux/v4l2-mediabus.h>
#include <linux/of.h>
#include <linux/of_graph.h>
#include <linux/platform_device.h>
#include <media/v4l2-async.h>
#include <media/v4l2-device.h>
#include <media/v4l2-common.h>
#include <media/v4l2-mediabus.h>
#include <media/v4l2-of.h>
#include <media/v4l2-ctrls.h>
#include <media/media-entity.h>
#include <linux/videodev2.h>

#include <media/v4l2-ctrls.h>
#include <media/v4l2-device.h>
#include <media/v4l2-event.h>
#include <media/v4l2-image-sizes.h>
#include <media/v4l2-subdev.h>
#include <media/v4l2-mediabus.h>
#include <media/v4l2-common.h>
#include <media/v4l2-of.h>
#include "evis_capture.h"

/* Debug functions */
static bool debug;
module_param(debug, bool, 0644);
MODULE_PARM_DESC(debug, "Debug level (0-1)");

MODULE_AUTHOR("Amritpreet Singh");
MODULE_DESCRIPTION("EVIS Capture Driver");
MODULE_LICENSE("GPL");

struct evis_capture_info {
        u32 flags;
        struct v4l2_of_endpoint endpoint;
};

/*
static struct evis_capture_info* get_pdata(struct platform_device *pdev)
{
}
*/
struct evis_format {
        u32 code;
        u32 colorspace;
        u16 fmt;
        u16 order;
};

struct evis_framesize {
        u16 width;
        u16 height;                                                                                   
};



struct evis_capture {
	struct v4l2_subdev              subdev;
	#if defined(CONFIG_MEDIA_CONTROLLER)
	 struct media_pad                pad;
	#endif
	struct v4l2_ctrl_handler        handler;
	struct evis_capture_info       *info;
	struct v4l2_rect                frame;
	struct v4l2_clk                 *clk;
	const struct evis_format	*format;

};

static int evis_s_stream(struct v4l2_subdev *sd, int enable)
{
	v4l2_dbg(1, debug, sd, "Reached evis_s_stream\n");
	return 0;
}


static int evis_enum_mbus_code(struct v4l2_subdev *sd,
				  struct v4l2_subdev_pad_config *cfg,
				  struct v4l2_subdev_mbus_code_enum *code)
{
	u32 pad = code->pad;
	u32 index = code->index;

	printk(KERN_INFO "Entered eviscapture1 evis_enum_mbus_code");
	memset(code, 0, sizeof(*code));
	code->index = index;
	code->pad = pad;

	if (index != 0) {
		printk(KERN_INFO "eviscapture1 n index in evis_enum_mbus_code");
		return -EINVAL;
	}

	code->code = MEDIA_BUS_FMT_UYVY8_2X8;//MEDIA_BUS_FMT_UYVY8_1X16;

	return 0;
}

static int evis_get_format(struct v4l2_subdev *sd,
				  struct v4l2_subdev_pad_config *cfg,
				  struct v4l2_subdev_format *fmt)
{
	struct v4l2_mbus_framefmt *mf;
	printk(KERN_INFO "Entered eviscapture1 evis_get_format");
	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
                mf = v4l2_subdev_get_try_format(sd, cfg, 0);
                fmt->format = *mf;
                return 0;
        }

        mf = &fmt->format;
        mf->width       = 720;
        mf->height      = 576;
        mf->colorspace  = V4L2_COLORSPACE_JPEG;//done for discrete BT.601//V4L2_COLORSPACE_SMPTE170M;
        mf->code        = MEDIA_BUS_FMT_UYVY8_2X8;//MEDIA_BUS_FMT_UYVY8_1X16; 
        mf->field       = V4L2_FIELD_INTERLACED;

        return 0;
}

static int evis_set_format(struct v4l2_subdev *sd,
				  struct v4l2_subdev_pad_config *cfg,
				  struct v4l2_subdev_format *fmt)
{
	printk(KERN_INFO "Entered eviscapture1 evis_set_format");
        v4l2_dbg(1, debug, sd, "Reached set format");
	return 0;
}

static int
evis_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
{
	printk(KERN_INFO "Entered eviscapture1 evis_mbus_fmt");
        if (f == NULL) {
		printk(KERN_INFO "eviscapture1 f is NULL");
                return -EINVAL;
	}
        f->code = MEDIA_BUS_FMT_UYVY8_2X8;/*MEDIA_BUS_FMT_UYVY8_1X16;*/
        f->width = 720;
        f->height = 576;
        f->field = V4L2_FIELD_INTERLACED;
        f->colorspace = V4L2_COLORSPACE_JPEG;//done for discrete BT.601//V4L2_COLORSPACE_SMPTE170M;
        v4l2_dbg(1, debug, sd, "Reached mbus_fmt");
        return 0;
}

static const struct v4l2_subdev_core_ops evis_core_ops = {
	.log_status = v4l2_ctrl_subdev_log_status, 
        .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
        .unsubscribe_event = v4l2_event_subdev_unsubscribe,
};

static int
evis_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
                                        u32 *code)
{
	printk(KERN_INFO "Entered eviscapture1 evis_enum_mbus_fmt");
        if (index) {
		printk(KERN_INFO "eviscapture1 evis_enum_mbus_fmt index not found");
                return -EINVAL;
	}

        *code = MEDIA_BUS_FMT_UYVY8_2X8;//MEDIA_BUS_FMT_UYVY8_1X16;
        return 0;
}

static const struct v4l2_subdev_video_ops evis_video_ops = {
	.enum_mbus_fmt = evis_enum_mbus_fmt,
	.g_mbus_fmt = evis_mbus_fmt,
	.try_mbus_fmt = evis_mbus_fmt,
	.s_mbus_fmt = evis_mbus_fmt,
	.s_stream = evis_s_stream,
};

static int evis_enum_frame_sizes(struct v4l2_subdev *sd,
                                    struct v4l2_subdev_pad_config *cfg,
                                    struct v4l2_subdev_frame_size_enum *fse)
{
	printk(KERN_INFO "Entered eviscapture1 evis_enum_frame_sizes");
	fse->min_width = 720;
	fse->max_width = 720;
	fse->max_height = 576;
	fse->min_height = 576;
        return 0;
}
static const struct v4l2_subdev_pad_ops evis_pad_ops = {
	.enum_mbus_code = evis_enum_mbus_code,
	.enum_frame_size = evis_enum_frame_sizes,
	.get_fmt = evis_get_format,
	.set_fmt = evis_set_format,
};

static const struct v4l2_subdev_ops evis_ops = {
	.core = &evis_core_ops,
	.video = &evis_video_ops,
	.pad = &evis_pad_ops,
};

static int evis_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
{
        struct v4l2_mbus_framefmt *mf;

        mf = v4l2_subdev_get_try_format(sd, fh->pad, 0);
        return 0;
}

static const struct v4l2_subdev_internal_ops evis_subdev_internal_ops = {
        .open = evis_open,
};

static int evis_s_ctrl(struct v4l2_ctrl *ctrl)
{
	printk(KERN_INFO "Entered evis_s_ctrl");
	return 0;
}

//struct v4l2_subdev *g_sd;
static const struct v4l2_ctrl_ops evis_ctrl_ops = {
	.s_ctrl = evis_s_ctrl,
};

static int initialize_controls(struct evis_capture *capture)
{
	const struct v4l2_ctrl_ops *ops = &evis_ctrl_ops;
	struct v4l2_subdev *sd = &capture->subdev;
	struct v4l2_ctrl_handler *hdl = &capture->handler;
	int ret;
	
	printk(KERN_INFO "%s\n", __func__);

	ret = v4l2_ctrl_handler_init(hdl, 2);
	if (ret < 0) {
		printk(KERN_INFO "v4l2_ctrl_handler_init failed\n");
		return ret;
	}
	sd->ctrl_handler = hdl;
	if (capture->handler.error)
	                return capture->handler.error;

	return 0;
}

static struct evis_capture_info * get_pdata(struct platform_device *pdev)
{
	struct device_node *endpoint;
	struct v4l2_of_endpoint *v4l2_endpoint;
	struct evis_capture_info *pdata;

	printk(KERN_INFO "_get_pdata invoked\n");

	if (!IS_ENABLED(CONFIG_OF) || !pdev->dev.of_node)
                return pdev->dev.platform_data;
	printk(KERN_INFO "_get_pdata: DT Node found\n");

	endpoint = of_graph_get_next_endpoint(pdev->dev.of_node, NULL);
        if (!endpoint)
                return NULL;
	printk(KERN_INFO "_get_pdata: endpoint found\n");

	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
        if (!pdata)
                goto done;

	v4l2_endpoint = &pdata->endpoint;
        v4l2_of_parse_endpoint(endpoint, v4l2_endpoint);

done:
        of_node_put(endpoint);
        return pdata;
}

static int evis_capture_probe(struct platform_device *pdev)
{
	struct v4l2_subdev *sd;
	struct evis_capture_info *info = get_pdata(pdev);
	struct evis_capture *capture;
	int ret;

	printk(KERN_INFO "Entered evis capture1 probe\n");
	capture = devm_kzalloc(&pdev->dev, sizeof(*capture), GFP_KERNEL);
	if (!capture) {
		printk(KERN_INFO "evis capture1 kzalloc failed\n");
		return -ENOMEM;
	}
	capture->info = info;
	/* Register with V4L2 layer as slave device */
	sd = &capture->subdev;
	v4l2_subdev_init(sd, &evis_ops);
	printk(KERN_INFO "evis capture1 subdev initialized\n");
	strlcpy(sd->name, "evis_capture_driver1", sizeof(sd->name));
	sd->internal_ops = &evis_subdev_internal_ops;
	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
		V4L2_SUBDEV_FL_HAS_EVENTS;
#if defined(CONFIG_MEDIA_CONTROLLER)
	printk(KERN_INFO "evis capture1 entered media controller\n");
	capture->pad.flags = MEDIA_PAD_FL_SOURCE;
	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_DECODER;
	ret = media_entity_init(&sd->entity, 1, &capture->pad, 0);
	if (ret < 0) {
		printk(KERN_INFO "EVIS capture driver failed to register media entity!!\n");
		return ret;
	}
#endif
	printk(KERN_INFO "evis capture1 exited media controller\n");
	ret = initialize_controls(capture);
	if (ret < 0)
		goto err_me;
	printk(KERN_INFO "evis capture1 controls initialized\n");
	printk(KERN_INFO "evis capture1 %s\n", pdev->dev.of_node->name);

//	ret = v4l2_async_register_subdev(&capture->subdev);
	ret = v4l2_async_register_subdev(sd);
        if (ret) {
		printk(KERN_INFO "evis capture1 v4l2_async_register_subdev failed\n");
                goto err_ctrls;
	} else
		printk(KERN_INFO "evis capture1 v4l2_async_register_subdev passed\n");

        dev_info(&pdev->dev, "%s EVIS Capture driver registered !!\n", sd->name);
        return 0;
err_ctrls:
	v4l2_ctrl_handler_free(sd->ctrl_handler);
err_me:
#if defined(CONFIG_MEDIA_CONTROLLER)
	media_entity_cleanup(&sd->entity);
#endif
	return ret;
}

static int evis_capture_remove(struct platform_device *pdev)
{
//	v4l2_async_unregister_subdev(g_sd);
//#if defined(CONFIG_MEDIA_CONTROLLER)
//	media_entity_cleanup(&g_sd->entity);
//#endif
	return 0;
}

#if IS_ENABLED(CONFIG_OF)
static const struct of_device_id evis_capture_of_match[] = {
	{ .compatible = "EVIS,evis_capture_driver1", },
	{ /* sentinel */ },
};
MODULE_DEVICE_TABLE(of, evis_capture_of_match);
#endif

static struct platform_driver evis_capture_driver = {
	.driver = {
		.of_match_table = of_match_ptr(evis_capture_of_match),
		.owner = THIS_MODULE,
		.name = "evis_capture_driver1",
	},
	.probe = evis_capture_probe,
	.remove = evis_capture_remove,
};

module_platform_driver(evis_capture_driver);