// SPDX-License-Identifier: GPL-2.0-only
/*
 * i.MX8 ISI - Input crossbar switch
 *
 * Copyright (c) 2022 Laurent Pinchart <laurent.pinchart@ideasonboard.com>
 */

#include <linux/device.h>
#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/minmax.h>
#include <linux/regmap.h>
#include <linux/slab.h>
#include <linux/string.h>
#include <linux/types.h>

#include <media/media-entity.h>
#include <media/v4l2-subdev.h>

#include "imx8-isi-core.h"

static inline struct mxc_isi_crossbar *to_isi_crossbar(struct v4l2_subdev *sd)
{
        return container_of(sd, struct mxc_isi_crossbar, sd);
}

static int mxc_isi_crossbar_gasket_enable(struct mxc_isi_crossbar *xbar,
                                          struct v4l2_subdev_state *state,
                                          struct v4l2_subdev *remote_sd,
                                          u32 remote_pad, unsigned int port)
{
        struct mxc_isi_dev *isi = xbar->isi;
        const struct mxc_gasket_ops *gasket_ops = isi->pdata->gasket_ops;
        const struct v4l2_mbus_framefmt *fmt;
        struct v4l2_mbus_frame_desc fd;
        int ret;

        if (!gasket_ops)
                return 0;

        /*
         * Configure and enable the gasket with the frame size and CSI-2 data
         * type. For YUV422 8-bit, enable dual component mode unconditionally,
         * to match the configuration of the CSIS.
         */

        ret = v4l2_subdev_call(remote_sd, pad, get_frame_desc, remote_pad, &fd);
        if (ret) {
                dev_err(isi->dev,
                        "failed to get frame descriptor from '%s':%u: %d\n",
                        remote_sd->name, remote_pad, ret);
                return ret;
        }

        if (fd.num_entries != 1) {
                dev_err(isi->dev, "invalid frame descriptor for '%s':%u\n",
                        remote_sd->name, remote_pad);
                return -EINVAL;
        }

        fmt = v4l2_subdev_state_get_format(state, port, 0);
        if (!fmt)
                return -EINVAL;

        gasket_ops->enable(isi, &fd, fmt, port);
        return 0;
}

static void mxc_isi_crossbar_gasket_disable(struct mxc_isi_crossbar *xbar,
                                            unsigned int port)
{
        struct mxc_isi_dev *isi = xbar->isi;
        const struct mxc_gasket_ops *gasket_ops = isi->pdata->gasket_ops;

        if (!gasket_ops)
                return;

        gasket_ops->disable(isi, port);
}

/* -----------------------------------------------------------------------------
 * V4L2 subdev operations
 */

static const struct v4l2_mbus_framefmt mxc_isi_crossbar_default_format = {
        .code = MXC_ISI_DEF_MBUS_CODE_SINK,
        .width = MXC_ISI_DEF_WIDTH,
        .height = MXC_ISI_DEF_HEIGHT,
        .field = V4L2_FIELD_NONE,
        .colorspace = MXC_ISI_DEF_COLOR_SPACE,
        .ycbcr_enc = MXC_ISI_DEF_YCBCR_ENC,
        .quantization = MXC_ISI_DEF_QUANTIZATION,
        .xfer_func = MXC_ISI_DEF_XFER_FUNC,
};

static int __mxc_isi_crossbar_set_routing(struct v4l2_subdev *sd,
                                          struct v4l2_subdev_state *state,
                                          struct v4l2_subdev_krouting *routing)
{
        struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
        struct v4l2_subdev_route *route;
        int ret;

        ret = v4l2_subdev_routing_validate(sd, routing,
                                           V4L2_SUBDEV_ROUTING_NO_N_TO_1);
        if (ret)
                return ret;

        /* The memory input can be routed to the first pipeline only. */
        for_each_active_route(&state->routing, route) {
                if (route->sink_pad == xbar->num_sinks - 1 &&
                    route->source_pad != xbar->num_sinks) {
                        dev_dbg(xbar->isi->dev,
                                "invalid route from memory input (%u) to pipe %u\n",
                                route->sink_pad,
                                route->source_pad - xbar->num_sinks);
                        return -ENXIO;
                }
        }

        return v4l2_subdev_set_routing_with_fmt(sd, state, routing,
                                                &mxc_isi_crossbar_default_format);
}

static struct v4l2_subdev *
mxc_isi_crossbar_xlate_streams(struct mxc_isi_crossbar *xbar,
                               struct v4l2_subdev_state *state,
                               u32 source_pad, u64 source_streams,
                               u32 *__sink_pad, u64 *__sink_streams,
                               u32 *remote_pad)
{
        struct v4l2_subdev_route *route;
        struct v4l2_subdev *sd;
        struct media_pad *pad;
        u64 sink_streams = 0;
        int sink_pad = -1;

        /*
         * Translate the source pad and streams to the sink side. The routing
         * validation forbids stream merging, so all matching entries in the
         * routing table are guaranteed to have the same sink pad.
         *
         * TODO: This is likely worth a helper function, it could perhaps be
         * supported by v4l2_subdev_state_xlate_streams() with pad1 set to -1.
         */
        for_each_active_route(&state->routing, route) {
                if (route->source_pad != source_pad ||
                    !(source_streams & BIT(route->source_stream)))
                        continue;

                sink_streams |= BIT(route->sink_stream);
                sink_pad = route->sink_pad;
        }

        if (sink_pad < 0) {
                dev_dbg(xbar->isi->dev,
                        "no stream connected to pipeline %u\n",
                        source_pad - xbar->num_sinks);
                return ERR_PTR(-EPIPE);
        }

        pad = media_pad_remote_pad_first(&xbar->pads[sink_pad]);
        sd = media_entity_to_v4l2_subdev(pad->entity);
        if (!sd) {
                dev_dbg(xbar->isi->dev,
                        "no entity connected to crossbar input %u\n",
                        sink_pad);
                return ERR_PTR(-EPIPE);
        }

        *__sink_pad = sink_pad;
        *__sink_streams = sink_streams;
        *remote_pad = pad->index;

        return sd;
}

static int mxc_isi_crossbar_init_state(struct v4l2_subdev *sd,
                                       struct v4l2_subdev_state *state)
{
        struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
        struct v4l2_subdev_krouting routing = { };
        struct v4l2_subdev_route *routes;
        unsigned int i;
        int ret;

        /*
         * Create a 1:1 mapping between pixel link inputs and outputs to
         * pipelines by default.
         */
        routing.num_routes = min(xbar->num_sinks - 1, xbar->num_sources);
        routes = kzalloc_objs(*routes, routing.num_routes);
        if (!routes)
                return -ENOMEM;

        for (i = 0; i < routing.num_routes; ++i) {
                struct v4l2_subdev_route *route = &routes[i];

                route->sink_pad = i;
                route->source_pad = i + xbar->num_sinks;
                route->flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE;
        }

        routing.routes = routes;

        ret = __mxc_isi_crossbar_set_routing(sd, state, &routing);

        kfree(routes);

        return ret;
}

static int mxc_isi_crossbar_enum_mbus_code(struct v4l2_subdev *sd,
                                           struct v4l2_subdev_state *state,
                                           struct v4l2_subdev_mbus_code_enum *code)
{
        struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
        const struct mxc_isi_bus_format_info *info;

        if (code->pad >= xbar->num_sinks) {
                const struct v4l2_mbus_framefmt *format;

                /*
                 * The media bus code on source pads is identical to the
                 * connected sink pad.
                 */
                if (code->index > 0)
                        return -EINVAL;

                format = v4l2_subdev_state_get_opposite_stream_format(state,
                                                                      code->pad,
                                                                      code->stream);
                if (!format)
                        return -EINVAL;

                code->code = format->code;

                return 0;
        }

        info = mxc_isi_bus_format_by_index(code->index, MXC_ISI_PIPE_PAD_SINK);
        if (!info)
                return -EINVAL;

        code->code = info->mbus_code;

        return 0;
}

static int mxc_isi_crossbar_set_fmt(struct v4l2_subdev *sd,
                                    struct v4l2_subdev_state *state,
                                    struct v4l2_subdev_format *fmt)
{
        struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
        struct v4l2_mbus_framefmt *sink_fmt;
        struct v4l2_subdev_route *route;

        if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
            media_pad_is_streaming(&xbar->pads[fmt->pad]))
                return -EBUSY;

        /*
         * The source pad format is always identical to the sink pad format and
         * can't be modified.
         */
        if (fmt->pad >= xbar->num_sinks)
                return v4l2_subdev_get_fmt(sd, state, fmt);

        /* Validate the requested format. */
        if (!mxc_isi_bus_format_by_code(fmt->format.code, MXC_ISI_PIPE_PAD_SINK))
                fmt->format.code = MXC_ISI_DEF_MBUS_CODE_SINK;

        fmt->format.width = clamp_t(unsigned int, fmt->format.width,
                                    MXC_ISI_MIN_WIDTH, MXC_ISI_MAX_WIDTH_CHAINED);
        fmt->format.height = clamp_t(unsigned int, fmt->format.height,
                                     MXC_ISI_MIN_HEIGHT, MXC_ISI_MAX_HEIGHT);
        fmt->format.field = V4L2_FIELD_NONE;

        /*
         * Set the format on the sink stream and propagate it to the source
         * streams.
         */
        sink_fmt = v4l2_subdev_state_get_format(state, fmt->pad, fmt->stream);
        if (!sink_fmt)
                return -EINVAL;

        *sink_fmt = fmt->format;

        /* TODO: A format propagation helper would be useful. */
        for_each_active_route(&state->routing, route) {
                struct v4l2_mbus_framefmt *source_fmt;

                if (route->sink_pad != fmt->pad ||
                    route->sink_stream != fmt->stream)
                        continue;

                source_fmt = v4l2_subdev_state_get_format(state,
                                                          route->source_pad,
                                                          route->source_stream);
                if (!source_fmt)
                        return -EINVAL;

                *source_fmt = fmt->format;
        }

        return 0;
}

static int mxc_isi_crossbar_set_routing(struct v4l2_subdev *sd,
                                        struct v4l2_subdev_state *state,
                                        enum v4l2_subdev_format_whence which,
                                        struct v4l2_subdev_krouting *routing)
{
        if (which == V4L2_SUBDEV_FORMAT_ACTIVE &&
            media_entity_is_streaming(&sd->entity))
                return -EBUSY;

        return __mxc_isi_crossbar_set_routing(sd, state, routing);
}

static int mxc_isi_crossbar_enable_streams(struct v4l2_subdev *sd,
                                           struct v4l2_subdev_state *state,
                                           u32 pad, u64 streams_mask)
{
        struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
        struct v4l2_subdev *remote_sd;
        struct mxc_isi_input *input;
        u64 sink_streams;
        u32 sink_pad;
        u32 remote_pad;
        int ret;

        remote_sd = mxc_isi_crossbar_xlate_streams(xbar, state, pad, streams_mask,
                                                   &sink_pad, &sink_streams,
                                                   &remote_pad);
        if (IS_ERR(remote_sd))
                return PTR_ERR(remote_sd);

        input = &xbar->inputs[sink_pad];

        /*
         * TODO: Track per-stream enable counts to support multiplexed
         * streams.
         */
        if (!input->enable_count) {
                ret = mxc_isi_crossbar_gasket_enable(xbar, state, remote_sd,
                                                     remote_pad, sink_pad);
                if (ret)
                        return ret;

                ret = v4l2_subdev_enable_streams(remote_sd, remote_pad,
                                                 sink_streams);
                if (ret) {
                        dev_err(xbar->isi->dev,
                                "failed to enable streams 0x%llx on '%s':%u: %d\n",
                                sink_streams, remote_sd->name, remote_pad, ret);
                        mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
                        return ret;
                }
        }

        input->enable_count++;

        return 0;
}

static int mxc_isi_crossbar_disable_streams(struct v4l2_subdev *sd,
                                            struct v4l2_subdev_state *state,
                                            u32 pad, u64 streams_mask)
{
        struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
        struct v4l2_subdev *remote_sd;
        struct mxc_isi_input *input;
        u64 sink_streams;
        u32 sink_pad;
        u32 remote_pad;
        int ret = 0;

        remote_sd = mxc_isi_crossbar_xlate_streams(xbar, state, pad, streams_mask,
                                                   &sink_pad, &sink_streams,
                                                   &remote_pad);
        if (IS_ERR(remote_sd))
                return PTR_ERR(remote_sd);

        input = &xbar->inputs[sink_pad];

        input->enable_count--;

        if (!input->enable_count) {
                ret = v4l2_subdev_disable_streams(remote_sd, remote_pad,
                                                  sink_streams);
                if (ret)
                        dev_err(xbar->isi->dev,
                                "failed to disable streams 0x%llx on '%s':%u: %d\n",
                                sink_streams, remote_sd->name, remote_pad, ret);

                mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
        }

        return ret;
}

static const struct v4l2_subdev_pad_ops mxc_isi_crossbar_subdev_pad_ops = {
        .enum_mbus_code = mxc_isi_crossbar_enum_mbus_code,
        .get_fmt = v4l2_subdev_get_fmt,
        .set_fmt = mxc_isi_crossbar_set_fmt,
        .set_routing = mxc_isi_crossbar_set_routing,
        .enable_streams = mxc_isi_crossbar_enable_streams,
        .disable_streams = mxc_isi_crossbar_disable_streams,
};

static const struct v4l2_subdev_ops mxc_isi_crossbar_subdev_ops = {
        .pad = &mxc_isi_crossbar_subdev_pad_ops,
};

static const struct v4l2_subdev_internal_ops mxc_isi_crossbar_internal_ops = {
        .init_state = mxc_isi_crossbar_init_state,
};

static const struct media_entity_operations mxc_isi_cross_entity_ops = {
        .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
        .link_validate  = v4l2_subdev_link_validate,
        .has_pad_interdep = v4l2_subdev_has_pad_interdep,
};

/* -----------------------------------------------------------------------------
 * Init & cleanup
 */

int mxc_isi_crossbar_init(struct mxc_isi_dev *isi)
{
        struct mxc_isi_crossbar *xbar = &isi->crossbar;
        struct v4l2_subdev *sd = &xbar->sd;
        unsigned int num_pads;
        unsigned int i;
        int ret;

        xbar->isi = isi;

        v4l2_subdev_init(sd, &mxc_isi_crossbar_subdev_ops);
        sd->internal_ops = &mxc_isi_crossbar_internal_ops;
        sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
        strscpy(sd->name, "crossbar", sizeof(sd->name));
        sd->dev = isi->dev;

        sd->entity.function = MEDIA_ENT_F_VID_MUX;
        sd->entity.ops = &mxc_isi_cross_entity_ops;

        /*
         * The subdev has one sink and one source per port, plus one sink for
         * the memory input.
         */
        xbar->num_sinks = isi->pdata->num_ports + 1;
        xbar->num_sources = isi->pdata->num_channels;
        num_pads = xbar->num_sinks + xbar->num_sources;

        xbar->pads = kzalloc_objs(*xbar->pads, num_pads);
        if (!xbar->pads)
                return -ENOMEM;

        xbar->inputs = kzalloc_objs(*xbar->inputs, xbar->num_sinks);
        if (!xbar->inputs) {
                ret = -ENOMEM;
                goto err_free;
        }

        for (i = 0; i < xbar->num_sinks; ++i)
                xbar->pads[i].flags = MEDIA_PAD_FL_SINK
                                    | MEDIA_PAD_FL_MUST_CONNECT;
        for (i = 0; i < xbar->num_sources; ++i)
                xbar->pads[i + xbar->num_sinks].flags = MEDIA_PAD_FL_SOURCE;

        ret = media_entity_pads_init(&sd->entity, num_pads, xbar->pads);
        if (ret)
                goto err_free;

        ret = v4l2_subdev_init_finalize(sd);
        if (ret < 0)
                goto err_entity;

        return 0;

err_entity:
        media_entity_cleanup(&sd->entity);
err_free:
        kfree(xbar->pads);
        kfree(xbar->inputs);

        return ret;
}

void mxc_isi_crossbar_cleanup(struct mxc_isi_crossbar *xbar)
{
        media_entity_cleanup(&xbar->sd.entity);
        kfree(xbar->pads);
        kfree(xbar->inputs);
}

int mxc_isi_crossbar_register(struct mxc_isi_crossbar *xbar)
{
        return v4l2_device_register_subdev(&xbar->isi->v4l2_dev, &xbar->sd);
}

void mxc_isi_crossbar_unregister(struct mxc_isi_crossbar *xbar)
{
}