Inc.

include <linux/debugfs.h>

include <linux/fs.h>

include <linux/kernel.h>

include <linux/module.h>

include <linux/slab.h>

include <linux/string.h>

include <linux/uaccess.h>

include <linux/vmalloc.h>

include <linux/greybus.h>

include "gb-camera.h"

include "greybus_protocols.h"

enum gb_camera_debugs_buffer_id { GB_CAMERA_DEBUGFS_BUFFER_CAPABILITIES, GB_CAMERA_DEBUGFS_BUFFER_STREAMS, GB_CAMERA_DEBUGFS_BUFFER_CAPTURE, GB_CAMERA_DEBUGFS_BUFFER_FLUSH, GB_CAMERA_DEBUGFS_BUFFER_MAX, };

struct gb_camera_debugfs_buffer { char data[PAGE_SIZE]; size_t length; };

enum gb_camera_state { GB_CAMERA_STATE_UNCONFIGURED, GB_CAMERA_STATE_CONFIGURED, };

/**

struct gb_camera - A Greybus Camera Device
@connection: the greybus connection for camera management
@data_connection: the greybus connection for camera data
@data_cport_id: the data CPort ID on the module side
@mutex: protects the connection and state fields
@state: the current module state
@debugfs: debugfs entries for camera protocol operations testing
@module: Greybus camera module registered to HOST processor. / struct gb_camera { struct gb_bundle bundle; struct gb_connection connection; struct gb_connection data_connection; u16 data_cport_id;

struct mutex mutex; enum gb_camera_state state;

struct { struct dentry root; struct gb_camera_debugfs_buffer buffers; } debugfs;

struct gb_camera_module module; };

struct gb_camera_stream_config { unsigned int width; unsigned int height; unsigned int format; unsigned int vc; unsigned int dt[2]; unsigned int max_size; };

struct gb_camera_fmt_info { enum v4l2_mbus_pixelcode mbus_code; unsigned int gb_format; unsigned int bpp; };

/ GB format to media code map / static const struct gb_camera_fmt_info gb_fmt_info[] = { { .mbus_code = V4L2_MBUS_FMT_UYVY8_1X16, .gb_format = 0x01, .bpp = 16, }, { .mbus_code = V4L2_MBUS_FMT_NV12_1x8, .gb_format = 0x12, .bpp = 12, }, { .mbus_code = V4L2_MBUS_FMT_NV21_1x8, .gb_format = 0x13, .bpp = 12, }, { .mbus_code = V4L2_MBUS_FMT_YU12_1x8, .gb_format = 0x16, .bpp = 12, }, { .mbus_code = V4L2_MBUS_FMT_YV12_1x8, .gb_format = 0x17, .bpp = 12, }, { .mbus_code = V4L2_MBUS_FMT_JPEG_1X8, .gb_format = 0x40, .bpp = 0, }, { .mbus_code = V4L2_MBUS_FMT_GB_CAM_METADATA_1X8, .gb_format = 0x41, .bpp = 0, }, { .mbus_code = V4L2_MBUS_FMT_GB_CAM_DEBUG_DATA_1X8, .gb_format = 0x42, .bpp = 0, }, { .mbus_code = V4L2_MBUS_FMT_SBGGR10_1X10, .gb_format = 0x80, .bpp = 10, }, { .mbus_code = V4L2_MBUS_FMT_SGBRG10_1X10, .gb_format = 0x81, .bpp = 10, }, { .mbus_code = V4L2_MBUS_FMT_SGRBG10_1X10, .gb_format = 0x82, .bpp = 10, }, { .mbus_code = V4L2_MBUS_FMT_SRGGB10_1X10, .gb_format = 0x83, .bpp = 10, }, { .mbus_code = V4L2_MBUS_FMT_SBGGR12_1X12, .gb_format = 0x84, .bpp = 12, }, { .mbus_code = V4L2_MBUS_FMT_SGBRG12_1X12, .gb_format = 0x85, .bpp = 12, }, { .mbus_code = V4L2_MBUS_FMT_SGRBG12_1X12, .gb_format = 0x86, .bpp = 12, }, { .mbus_code = V4L2_MBUS_FMT_SRGGB12_1X12, .gb_format = 0x87, .bpp = 12, }, };

static const struct gb_camera_fmt_info *gb_camera_get_format_info(u16 gb_fmt) { unsigned int i;

for (i = 0; i < ARRAY_SIZE(gb_fmt_info); i++) {
    if (gb_fmt_info[i].gb_format == gb_fmt)
        return &gb_fmt_info[i];
}

return NULL;

}

define ES2_APB_CDSI0_CPORT 16

define ES2_APB_CDSI1_CPORT 17

define GB_CAMERA_MAX_SETTINGS_SIZE 8192

define gcam_dbg(gcam, format...) dev_dbg(&gcam->bundle->dev, format)

define gcam_info(gcam, format...) dev_info(&gcam->bundle->dev, format)

define gcam_err(gcam, format...) dev_err(&gcam->bundle->dev, format)

static int gb_camera_operation_sync_flags(struct gb_connection connection, int type, unsigned int flags, void request, size_t request_size, void response, size_t response_size) { struct gb_operation *operation; int ret;

operation = gb_operation_create_flags(connection, type, request_size,
                      *response_size, flags,
                      GFP_KERNEL);
if (!operation)
    return  -ENOMEM;

if (request_size)
    memcpy(operation->request->payload, request, request_size);

ret = gb_operation_request_send_sync(operation);
if (ret) {
    dev_err(&connection->hd->dev,
        "%s: synchronous operation of type 0x%02x failed: %d\n",
        connection->name, type, ret);
} else {
    *response_size = operation->response->payload_size;

    if (operation->response->payload_size)
        memcpy(response, operation->response->payload,
               operation->response->payload_size);
}

gb_operation_put(operation);

return ret;

}

static int gb_camera_get_max_pkt_size(struct gb_camera gcam, struct gb_camera_configure_streams_response resp) { unsigned int max_pkt_size = 0; unsigned int i;

for (i = 0; i < resp->num_streams; i++) {
    struct gb_camera_stream_config_response *cfg = &resp->config[i];
    const struct gb_camera_fmt_info *fmt_info;
    unsigned int pkt_size;

    fmt_info = gb_camera_get_format_info(cfg->format);
    if (!fmt_info) {
        gcam_err(gcam, "unsupported greybus image format: %d\n",
             cfg->format);
        return -EIO;
    }

    if (fmt_info->bpp == 0) {
        pkt_size = le32_to_cpu(cfg->max_pkt_size);

        if (pkt_size == 0) {
            gcam_err(gcam,
                 "Stream %u: invalid zero maximum packet size\n",
                 i);
            return -EIO;
        }
    } else {
        pkt_size = le16_to_cpu(cfg->width) * fmt_info->bpp / 8;

        if (pkt_size != le32_to_cpu(cfg->max_pkt_size)) {
            gcam_err(gcam,
                 "Stream %u: maximum packet size mismatch (%u/%u)\n",
                 i, pkt_size, cfg->max_pkt_size);
            return -EIO;
        }
    }

    max_pkt_size = max(pkt_size, max_pkt_size);
}

return max_pkt_size;

}

Validate the stream configuration response verifying padding is correctly
set and the returned number of streams is supported / static const int gb_camera_configure_streams_validate_response(struct gb_camera gcam, struct gb_camera_configure_streams_response *resp, unsigned int nstreams) { unsigned int i;

/ Validate the returned response structure / if (resp->padding[0] || resp->padding[1]) { gcam_err(gcam, "response padding != 0\n"); return -EIO; }

if (resp->num_streams > nstreams) { gcam_err(gcam, "got #streams %u > request %u\n", resp->num_streams, nstreams); return -EIO; }

for (i = 0; i < resp->num_streams; i++) { struct gb_camera_stream_config_response *cfg = &resp->config[i];
```
if (cfg->padding) {
    gcam_err(gcam, "stream #%u padding != 0\n", i);
    return -EIO;
}
```
}

return 0; }

/* -----------------------------------------------------------------------------

Hardware Configuration */

static int gb_camera_set_intf_power_mode(struct gb_camera gcam, u8 intf_id, bool hs) { struct gb_svc svc = gcam->connection->hd->svc; int ret;

if (hs)
    ret = gb_svc_intf_set_power_mode(svc, intf_id,
                     GB_SVC_UNIPRO_HS_SERIES_A,
                     GB_SVC_UNIPRO_FAST_MODE, 2, 2,
                     GB_SVC_SMALL_AMPLITUDE,
                     GB_SVC_NO_DE_EMPHASIS,
                     GB_SVC_UNIPRO_FAST_MODE, 2, 2,
                     GB_SVC_PWRM_RXTERMINATION |
                     GB_SVC_PWRM_TXTERMINATION, 0,
                     NULL, NULL);
else
    ret = gb_svc_intf_set_power_mode(svc, intf_id,
                     GB_SVC_UNIPRO_HS_SERIES_A,
                     GB_SVC_UNIPRO_SLOW_AUTO_MODE,
                     2, 1,
                     GB_SVC_SMALL_AMPLITUDE,
                     GB_SVC_NO_DE_EMPHASIS,
                     GB_SVC_UNIPRO_SLOW_AUTO_MODE,
                     2, 1,
                     0, 0,
                     NULL, NULL);

return ret;

}

static int gb_camera_set_power_mode(struct gb_camera gcam, bool hs) { struct gb_interface intf = gcam->connection->intf; struct gb_svc *svc = gcam->connection->hd->svc; int ret;

ret = gb_camera_set_intf_power_mode(gcam, intf->interface_id, hs);
if (ret < 0) {
    gcam_err(gcam, "failed to set module interface to %s (%d)\n",
         hs ? "HS" : "PWM", ret);
    return ret;
}

ret = gb_camera_set_intf_power_mode(gcam, svc->ap_intf_id, hs);
if (ret < 0) {
    gb_camera_set_intf_power_mode(gcam, intf->interface_id, !hs);
    gcam_err(gcam, "failed to set AP interface to %s (%d)\n",
         hs ? "HS" : "PWM", ret);
    return ret;
}

return 0;

}

struct ap_csi_config_request { __u8 csi_id; __u8 flags;

define GB_CAMERA_CSI_FLAG_CLOCK_CONTINUOUS 0x01

__u8 num_lanes;
__u8 padding;
__le32 csi_clk_freq;
__le32 max_pkt_size;

} __packed;

TODO: Compute the number of lanes dynamically based on bandwidth
requirements. */
define GB_CAMERA_CSI_NUM_DATA_LANES 4

define GB_CAMERA_CSI_CLK_FREQ_MAX 999000000U

define GB_CAMERA_CSI_CLK_FREQ_MIN 100000000U

define GB_CAMERA_CSI_CLK_FREQ_MARGIN 150000000U

static int gb_camera_setup_data_connection(struct gb_camera gcam, struct gb_camera_configure_streams_response resp, struct gb_camera_csi_params csi_params) { struct ap_csi_config_request csi_cfg; struct gb_connection conn; unsigned int clk_freq; int ret;

/*
 * Create the data connection between the camera module data CPort and
 * APB CDSI1. The CDSI1 CPort ID is hardcoded by the ES2 bridge.
 */
conn = gb_connection_create_offloaded(gcam->bundle, gcam->data_cport_id,
                      GB_CONNECTION_FLAG_NO_FLOWCTRL |
                      GB_CONNECTION_FLAG_CDSI1);
if (IS_ERR(conn))
    return PTR_ERR(conn);

gcam->data_connection = conn;
gb_connection_set_data(conn, gcam);

ret = gb_connection_enable(conn);
if (ret)
    goto error_conn_destroy;

/* Set the UniPro link to high speed mode. */
ret = gb_camera_set_power_mode(gcam, true);
if (ret < 0)
    goto error_conn_disable;

/*
 * Configure the APB-A CSI-2 transmitter.
 *
 * Hardcode the number of lanes to 4 and compute the bus clock frequency
 * based on the module bandwidth requirements with a safety margin.
 */
memset(&csi_cfg, 0, sizeof(csi_cfg));
csi_cfg.csi_id = 1;
csi_cfg.flags = 0;
csi_cfg.num_lanes = GB_CAMERA_CSI_NUM_DATA_LANES;

clk_freq = resp->data_rate / 2 / GB_CAMERA_CSI_NUM_DATA_LANES;
clk_freq = clamp(clk_freq + GB_CAMERA_CSI_CLK_FREQ_MARGIN,
         GB_CAMERA_CSI_CLK_FREQ_MIN,
         GB_CAMERA_CSI_CLK_FREQ_MAX);
csi_cfg.csi_clk_freq = clk_freq;

ret = gb_camera_get_max_pkt_size(gcam, resp);
if (ret < 0) {
    ret = -EIO;
    goto error_power;
}
csi_cfg.max_pkt_size = ret;

ret = gb_hd_output(gcam->connection->hd, &csi_cfg,
           sizeof(csi_cfg),
           GB_APB_REQUEST_CSI_TX_CONTROL, false);
if (ret < 0) {
    gcam_err(gcam, "failed to start the CSI transmitter\n");
    goto error_power;
}

if (csi_params) {
    csi_params->clk_freq = csi_cfg.csi_clk_freq;
    csi_params->num_lanes = csi_cfg.num_lanes;
}

return 0;

error_power: gb_camera_set_power_mode(gcam, false); error_conn_disable: gb_connection_disable(gcam->data_connection); error_conn_destroy: gb_connection_destroy(gcam->data_connection); gcam->data_connection = NULL; return ret; }

static void gb_camera_teardown_data_connection(struct gb_camera *gcam) { struct ap_csi_config_request csi_cfg; int ret;

/* Stop the APB1 CSI transmitter. */
memset(&csi_cfg, 0, sizeof(csi_cfg));
csi_cfg.csi_id = 1;

ret = gb_hd_output(gcam->connection->hd, &csi_cfg,
           sizeof(csi_cfg),
           GB_APB_REQUEST_CSI_TX_CONTROL, false);

if (ret < 0)
    gcam_err(gcam, "failed to stop the CSI transmitter\n");

/* Set the UniPro link to low speed mode. */
gb_camera_set_power_mode(gcam, false);

/* Destroy the data connection. */
gb_connection_disable(gcam->data_connection);
gb_connection_destroy(gcam->data_connection);
gcam->data_connection = NULL;

}

/* -----------------------------------------------------------------------------

Camera Protocol Operations */

static int gb_camera_capabilities(struct gb_camera gcam, u8 capabilities, size_t *size) { int ret;

ret = gb_pm_runtime_get_sync(gcam->bundle);
if (ret)
    return ret;

mutex_lock(&gcam->mutex);

if (!gcam->connection) {
    ret = -EINVAL;
    goto done;
}

ret = gb_camera_operation_sync_flags(gcam->connection,
                     GB_CAMERA_TYPE_CAPABILITIES,
                     GB_OPERATION_FLAG_SHORT_RESPONSE,
                     NULL, 0,
                     (void *)capabilities, size);
if (ret)
    gcam_err(gcam, "failed to retrieve capabilities: %d\n", ret);

done: mutex_unlock(&gcam->mutex);

gb_pm_runtime_put_autosuspend(gcam->bundle);

return ret;

}

static int gb_camera_configure_streams(struct gb_camera gcam, unsigned int num_streams, unsigned int flags, struct gb_camera_stream_config streams, struct gb_camera_csi_params csi_params) { struct gb_camera_configure_streams_request req; struct gb_camera_configure_streams_response resp; unsigned int nstreams = num_streams; unsigned int i; size_t req_size; size_t resp_size; int ret;

if (nstreams > GB_CAMERA_MAX_STREAMS)
    return -EINVAL;

req_size = sizeof(*req) + nstreams * sizeof(req->config[0]);
resp_size = sizeof(*resp) + nstreams * sizeof(resp->config[0]);

req = kmalloc(req_size, GFP_KERNEL);
resp = kmalloc(resp_size, GFP_KERNEL);
if (!req || !resp) {
    kfree(req);
    kfree(resp);
    return -ENOMEM;
}

req->num_streams = nstreams;
req->flags = *flags;
req->padding = 0;

for (i = 0; i < nstreams; ++i) {
    struct gb_camera_stream_config_request *cfg = &req->config[i];

    cfg->width = cpu_to_le16(streams[i].width);
    cfg->height = cpu_to_le16(streams[i].height);
    cfg->format = cpu_to_le16(streams[i].format);
    cfg->padding = 0;
}

mutex_lock(&gcam->mutex);

ret = gb_pm_runtime_get_sync(gcam->bundle);
if (ret)
    goto done_skip_pm_put;

if (!gcam->connection) {
    ret = -EINVAL;
    goto done;
}

ret = gb_camera_operation_sync_flags(gcam->connection,
                     GB_CAMERA_TYPE_CONFIGURE_STREAMS,
                     GB_OPERATION_FLAG_SHORT_RESPONSE,
                     req, req_size,
                     resp, &resp_size);
if (ret < 0)
    goto done;

ret = gb_camera_configure_streams_validate_response(gcam, resp,
                            nstreams);
if (ret < 0)
    goto done;

*flags = resp->flags;
*num_streams = resp->num_streams;

for (i = 0; i < resp->num_streams; ++i) {
    struct gb_camera_stream_config_response *cfg = &resp->config[i];

    streams[i].width = le16_to_cpu(cfg->width);
    streams[i].height = le16_to_cpu(cfg->height);
    streams[i].format = le16_to_cpu(cfg->format);
    streams[i].vc = cfg->virtual_channel;
    streams[i].dt[0] = cfg->data_type[0];
    streams[i].dt[1] = cfg->data_type[1];
    streams[i].max_size = le32_to_cpu(cfg->max_size);
}

if ((resp->flags & GB_CAMERA_CONFIGURE_STREAMS_ADJUSTED) ||
    (req->flags & GB_CAMERA_CONFIGURE_STREAMS_TEST_ONLY))
    goto done;

if (gcam->state == GB_CAMERA_STATE_CONFIGURED) {
    gb_camera_teardown_data_connection(gcam);
    gcam->state = GB_CAMERA_STATE_UNCONFIGURED;

    /*
     * When unconfiguring streams release the PM runtime reference
     * that was acquired when streams were configured. The bundle
     * won't be suspended until the PM runtime reference acquired at
     * the beginning of this function gets released right before
     * returning.
     */
    gb_pm_runtime_put_noidle(gcam->bundle);
}

if (resp->num_streams == 0)
    goto done;

/*
 * Make sure the bundle won't be suspended until streams get
 * unconfigured after the stream is configured successfully
 */
gb_pm_runtime_get_noresume(gcam->bundle);

/* Setup CSI-2 connection from APB-A to AP */
ret = gb_camera_setup_data_connection(gcam, resp, csi_params);
if (ret < 0) {
    memset(req, 0, sizeof(*req));
    gb_operation_sync(gcam->connection,
              GB_CAMERA_TYPE_CONFIGURE_STREAMS,
              req, sizeof(*req),
              resp, sizeof(*resp));
    *flags = 0;
    *num_streams = 0;
    gb_pm_runtime_put_noidle(gcam->bundle);
    goto done;
}

gcam->state = GB_CAMERA_STATE_CONFIGURED;

done: gb_pm_runtime_put_autosuspend(gcam->bundle);

done_skip_pm_put: mutex_unlock(&gcam->mutex); kfree(req); kfree(resp); return ret; }

static int gb_camera_capture(struct gb_camera gcam, u32 request_id, unsigned int streams, unsigned int num_frames, size_t settings_size, const void settings) { struct gb_camera_capture_request *req; size_t req_size; int ret;

if (settings_size > GB_CAMERA_MAX_SETTINGS_SIZE)
    return -EINVAL;

req_size = sizeof(*req) + settings_size;
req = kmalloc(req_size, GFP_KERNEL);
if (!req)
    return -ENOMEM;

req->request_id = cpu_to_le32(request_id);
req->streams = streams;
req->padding = 0;
req->num_frames = cpu_to_le16(num_frames);
memcpy(req->settings, settings, settings_size);

mutex_lock(&gcam->mutex);

if (!gcam->connection) {
    ret = -EINVAL;
    goto done;
}

ret = gb_operation_sync(gcam->connection, GB_CAMERA_TYPE_CAPTURE,
            req, req_size, NULL, 0);

done: mutex_unlock(&gcam->mutex);

kfree(req);

return ret;

}

static int gb_camera_flush(struct gb_camera gcam, u32 request_id) { struct gb_camera_flush_response resp; int ret;

mutex_lock(&gcam->mutex);

if (!gcam->connection) {
    ret = -EINVAL;
    goto done;
}

ret = gb_operation_sync(gcam->connection, GB_CAMERA_TYPE_FLUSH, NULL, 0,
            &resp, sizeof(resp));

if (ret < 0)
    goto done;

if (request_id)
    *request_id = le32_to_cpu(resp.request_id);

done: mutex_unlock(&gcam->mutex);

return ret;

}

static int gb_camera_request_handler(struct gb_operation op) { struct gb_camera gcam = gb_connection_get_data(op->connection); struct gb_camera_metadata_request payload; struct gb_message request;

if (op->type != GB_CAMERA_TYPE_METADATA) {
    gcam_err(gcam, "Unsupported unsolicited event: %u\n", op->type);
    return -EINVAL;
}

request = op->request;

if (request->payload_size < sizeof(*payload)) {
    gcam_err(gcam, "Wrong event size received (%zu < %zu)\n",
         request->payload_size, sizeof(*payload));
    return -EINVAL;
}

payload = request->payload;

gcam_dbg(gcam, "received metadata for request %u, frame %u, stream %u\n",
     payload->request_id, payload->frame_number, payload->stream);

return 0;

}

/* -----------------------------------------------------------------------------

Interface with HOST gmp camera. */ static unsigned int gb_camera_mbus_to_gb(enum v4l2_mbus_pixelcode mbus_code) { unsigned int i;

for (i = 0; i < ARRAY_SIZE(gb_fmt_info); i++) { if (gb_fmt_info[i].mbus_code == mbus_code) return gb_fmt_info[i].gb_format; } return gb_fmt_info[0].gb_format; }

static enum v4l2_mbus_pixelcode gb_camera_gb_to_mbus(u16 gb_fmt) { unsigned int i;

for (i = 0; i < ARRAY_SIZE(gb_fmt_info); i++) {
    if (gb_fmt_info[i].gb_format == gb_fmt)
        return gb_fmt_info[i].mbus_code;
}
return gb_fmt_info[0].mbus_code;

}

static ssize_t gb_camera_op_capabilities(void priv, char data, size_t len) { struct gb_camera *gcam = priv; size_t capabilities_len = len; int ret;

ret = gb_camera_capabilities(gcam, data, &capabilities_len);
if (ret)
    return ret;

return capabilities_len;

}

static int gb_camera_op_configure_streams(void priv, unsigned int nstreams, unsigned int flags, struct gb_camera_stream streams, struct gb_camera_csi_params csi_params) { struct gb_camera gcam = priv; struct gb_camera_stream_config gb_streams; unsigned int gb_flags = 0; unsigned int gb_nstreams = nstreams; unsigned int i; int ret;

if (gb_nstreams > GB_CAMERA_MAX_STREAMS)
    return -EINVAL;

gb_streams = kcalloc(gb_nstreams, sizeof(*gb_streams), GFP_KERNEL);
if (!gb_streams)
    return -ENOMEM;

for (i = 0; i < gb_nstreams; i++) {
    gb_streams[i].width = streams[i].width;
    gb_streams[i].height = streams[i].height;
    gb_streams[i].format =
        gb_camera_mbus_to_gb(streams[i].pixel_code);
}

if (*flags & GB_CAMERA_IN_FLAG_TEST)
    gb_flags |= GB_CAMERA_CONFIGURE_STREAMS_TEST_ONLY;

ret = gb_camera_configure_streams(gcam, &gb_nstreams,
                  &gb_flags, gb_streams, csi_params);
if (ret < 0)
    goto done;
if (gb_nstreams > *nstreams) {
    ret = -EINVAL;
    goto done;
}

*flags = 0;
if (gb_flags & GB_CAMERA_CONFIGURE_STREAMS_ADJUSTED)
    *flags |= GB_CAMERA_OUT_FLAG_ADJUSTED;

for (i = 0; i < gb_nstreams; i++) {
    streams[i].width = gb_streams[i].width;
    streams[i].height = gb_streams[i].height;
    streams[i].vc = gb_streams[i].vc;
    streams[i].dt[0] = gb_streams[i].dt[0];
    streams[i].dt[1] = gb_streams[i].dt[1];
    streams[i].max_size = gb_streams[i].max_size;
    streams[i].pixel_code =
        gb_camera_gb_to_mbus(gb_streams[i].format);
}
*nstreams = gb_nstreams;

done: kfree(gb_streams); return ret; }

static int gb_camera_op_capture(void priv, u32 request_id, unsigned int streams, unsigned int num_frames, size_t settings_size, const void settings) { struct gb_camera *gcam = priv;

return gb_camera_capture(gcam, request_id, streams, num_frames,
             settings_size, settings);

}

static int gb_camera_op_flush(void priv, u32 request_id) { struct gb_camera *gcam = priv;

return gb_camera_flush(gcam, request_id);

}

static const struct gb_camera_ops gb_cam_ops = { .capabilities = gb_camera_op_capabilities, .configure_streams = gb_camera_op_configure_streams, .capture = gb_camera_op_capture, .flush = gb_camera_op_flush, };

/* -----------------------------------------------------------------------------

DebugFS */

static ssize_t gb_camera_debugfs_capabilities(struct gb_camera gcam, char buf, size_t len) { struct gb_camera_debugfs_buffer buffer = &gcam->debugfs.buffers[GB_CAMERA_DEBUGFS_BUFFER_CAPABILITIES]; size_t size = 1024; unsigned int i; u8 caps; int ret;

caps = kmalloc(size, GFP_KERNEL);
if (!caps)
    return -ENOMEM;

ret = gb_camera_capabilities(gcam, caps, &size);
if (ret < 0)
    goto done;

/*
 * hex_dump_to_buffer() doesn't return the number of bytes dumped prior
 * to v4.0, we need our own implementation :-(
 */
buffer->length = 0;

for (i = 0; i < size; i += 16) {
    unsigned int nbytes = min_t(unsigned int, size - i, 16);

    buffer->length += sprintf(buffer->data + buffer->length,
                  "%*ph\n", nbytes, caps + i);
}

done: kfree(caps); return ret; }

static ssize_t gb_camera_debugfs_configure_streams(struct gb_camera gcam, char buf, size_t len) { struct gb_camera_debugfs_buffer buffer = &gcam->debugfs.buffers[GB_CAMERA_DEBUGFS_BUFFER_STREAMS]; struct gb_camera_stream_config streams; unsigned int nstreams; unsigned int flags; unsigned int i; char *token; int ret;

/* Retrieve number of streams to configure */
token = strsep(&buf, ";");
if (!token)
    return -EINVAL;

ret = kstrtouint(token, 10, &nstreams);
if (ret < 0)
    return ret;

if (nstreams > GB_CAMERA_MAX_STREAMS)
    return -EINVAL;

token = strsep(&buf, ";");
if (!token)
    return -EINVAL;

ret = kstrtouint(token, 10, &flags);
if (ret < 0)
    return ret;

/* For each stream to configure parse width, height and format */
streams = kcalloc(nstreams, sizeof(*streams), GFP_KERNEL);
if (!streams)
    return -ENOMEM;

for (i = 0; i < nstreams; ++i) {
    struct gb_camera_stream_config *stream = &streams[i];

    /* width */
    token = strsep(&buf, ";");
    if (!token) {
        ret = -EINVAL;
        goto done;
    }
    ret = kstrtouint(token, 10, &stream->width);
    if (ret < 0)
        goto done;

    /* height */
    token = strsep(&buf, ";");
    if (!token)
        goto done;

    ret = kstrtouint(token, 10, &stream->height);
    if (ret < 0)
        goto done;

    /* Image format code */
    token = strsep(&buf, ";");
    if (!token)
        goto done;

    ret = kstrtouint(token, 16, &stream->format);
    if (ret < 0)
        goto done;
}

ret = gb_camera_configure_streams(gcam, &nstreams, &flags, streams,
                  NULL);
if (ret < 0)
    goto done;

buffer->length = sprintf(buffer->data, "%u;%u;", nstreams, flags);

for (i = 0; i < nstreams; ++i) {
    struct gb_camera_stream_config *stream = &streams[i];

    buffer->length += sprintf(buffer->data + buffer->length,
                  "%u;%u;%u;%u;%u;%u;%u;",
                  stream->width, stream->height,
                  stream->format, stream->vc,
                  stream->dt[0], stream->dt[1],
                  stream->max_size);
}

ret = len;

done: kfree(streams); return ret; };

static ssize_t gb_camera_debugfs_capture(struct gb_camera gcam, char buf, size_t len) { unsigned int request_id; unsigned int streams_mask; unsigned int num_frames; char *token; int ret;

/* Request id */
token = strsep(&buf, ";");
if (!token)
    return -EINVAL;
ret = kstrtouint(token, 10, &request_id);
if (ret < 0)
    return ret;

/* Stream mask */
token = strsep(&buf, ";");
if (!token)
    return -EINVAL;
ret = kstrtouint(token, 16, &streams_mask);
if (ret < 0)
    return ret;

/* number of frames */
token = strsep(&buf, ";");
if (!token)
    return -EINVAL;
ret = kstrtouint(token, 10, &num_frames);
if (ret < 0)
    return ret;

ret = gb_camera_capture(gcam, request_id, streams_mask, num_frames, 0,
            NULL);
if (ret < 0)
    return ret;

return len;

}

static ssize_t gb_camera_debugfs_flush(struct gb_camera gcam, char buf, size_t len) { struct gb_camera_debugfs_buffer *buffer = &gcam->debugfs.buffers[GB_CAMERA_DEBUGFS_BUFFER_FLUSH]; unsigned int req_id; int ret;

ret = gb_camera_flush(gcam, &req_id);
if (ret < 0)
    return ret;

buffer->length = sprintf(buffer->data, "%u", req_id);

return len;

}

struct gb_camera_debugfs_entry { const char name; unsigned int mask; unsigned int buffer; ssize_t (execute)(struct gb_camera gcam, char buf, size_t len); };

static const struct gb_camera_debugfs_entry gb_camera_debugfs_entries[] = { { .name = "capabilities", .mask = S_IFREG | 0444, .buffer = GB_CAMERA_DEBUGFS_BUFFER_CAPABILITIES, .execute = gb_camera_debugfs_capabilities, }, { .name = "configure_streams", .mask = S_IFREG | 0666, .buffer = GB_CAMERA_DEBUGFS_BUFFER_STREAMS, .execute = gb_camera_debugfs_configure_streams, }, { .name = "capture", .mask = S_IFREG | 0666, .buffer = GB_CAMERA_DEBUGFS_BUFFER_CAPTURE, .execute = gb_camera_debugfs_capture, }, { .name = "flush", .mask = S_IFREG | 0666, .buffer = GB_CAMERA_DEBUGFS_BUFFER_FLUSH, .execute = gb_camera_debugfs_flush, }, };

static ssize_t gb_camera_debugfs_read(struct file file, char __user buf, size_t len, loff_t offset) { const struct gb_camera_debugfs_entry op = file->private_data; struct gb_camera gcam = file_inode(file)->i_private; struct gb_camera_debugfs_buffer buffer; ssize_t ret;

/* For read-only entries the operation is triggered by a read. */
if (!(op->mask & 0222)) {
    ret = op->execute(gcam, NULL, 0);
    if (ret < 0)
        return ret;
}

buffer = &gcam->debugfs.buffers[op->buffer];

return simple_read_from_buffer(buf, len, offset, buffer->data,
                   buffer->length);

}

static ssize_t gb_camera_debugfs_write(struct file file, const char __user buf, size_t len, loff_t offset) { const struct gb_camera_debugfs_entry op = file->private_data; struct gb_camera gcam = file_inode(file)->i_private; ssize_t ret; char kbuf;

if (len > 1024)
    return -EINVAL;

kbuf = memdup_user_nul(buf, len);
if (IS_ERR(kbuf))
    return PTR_ERR(kbuf);

ret = op->execute(gcam, kbuf, len);

done: kfree(kbuf); return ret; }

static int gb_camera_debugfs_open(struct inode inode, struct file file) { unsigned int i;

for (i = 0; i < ARRAY_SIZE(gb_camera_debugfs_entries); ++i) {
    const struct gb_camera_debugfs_entry *entry =
        &gb_camera_debugfs_entries[i];

    if (!strcmp(file->f_path.dentry->d_iname, entry->name)) {
        file->private_data = (void *)entry;
        break;
    }
}

return 0;

}

static const struct file_operations gb_camera_debugfs_ops = { .open = gb_camera_debugfs_open, .read = gb_camera_debugfs_read, .write = gb_camera_debugfs_write, };

static int gb_camera_debugfs_init(struct gb_camera gcam) { struct gb_connection connection = gcam->connection; char dirname[27]; unsigned int i;

/*
 * Create root debugfs entry and a file entry for each camera operation.
 */
snprintf(dirname, 27, "camera-%u.%u", connection->intf->interface_id,
     gcam->bundle->id);

gcam->debugfs.root = debugfs_create_dir(dirname, gb_debugfs_get());

gcam->debugfs.buffers =
    vmalloc(array_size(GB_CAMERA_DEBUGFS_BUFFER_MAX,
               sizeof(*gcam->debugfs.buffers)));
if (!gcam->debugfs.buffers)
    return -ENOMEM;

for (i = 0; i < ARRAY_SIZE(gb_camera_debugfs_entries); ++i) {
    const struct gb_camera_debugfs_entry *entry =
        &gb_camera_debugfs_entries[i];

    gcam->debugfs.buffers[i].length = 0;

    debugfs_create_file(entry->name, entry->mask,
                gcam->debugfs.root, gcam,
                &gb_camera_debugfs_ops);
}

return 0;

}

static void gb_camera_debugfs_cleanup(struct gb_camera *gcam) { debugfs_remove_recursive(gcam->debugfs.root);

vfree(gcam->debugfs.buffers);

}

/* -----------------------------------------------------------------------------

Init & Cleanup */

static void gb_camera_cleanup(struct gb_camera *gcam) { gb_camera_debugfs_cleanup(gcam);

mutex_lock(&gcam->mutex);
if (gcam->data_connection) {
    gb_connection_disable(gcam->data_connection);
    gb_connection_destroy(gcam->data_connection);
    gcam->data_connection = NULL;
}

if (gcam->connection) {
    gb_connection_disable(gcam->connection);
    gb_connection_destroy(gcam->connection);
    gcam->connection = NULL;
}
mutex_unlock(&gcam->mutex);

}

static void gb_camera_release_module(struct kref ref) { struct gb_camera_module cam_mod = container_of(ref, struct gb_camera_module, refcount); kfree(cam_mod->priv); }

static int gb_camera_probe(struct gb_bundle bundle, const struct greybus_bundle_id id) { struct gb_connection conn; struct gb_camera gcam; u16 mgmt_cport_id = 0; u16 data_cport_id = 0; unsigned int i; int ret;

/*
 * The camera bundle must contain exactly two CPorts, one for the
 * camera management protocol and one for the camera data protocol.
 */
if (bundle->num_cports != 2)
    return -ENODEV;

for (i = 0; i < bundle->num_cports; ++i) {
    struct greybus_descriptor_cport *desc = &bundle->cport_desc[i];

    switch (desc->protocol_id) {
    case GREYBUS_PROTOCOL_CAMERA_MGMT:
        mgmt_cport_id = le16_to_cpu(desc->id);
        break;
    case GREYBUS_PROTOCOL_CAMERA_DATA:
        data_cport_id = le16_to_cpu(desc->id);
        break;
    default:
        return -ENODEV;
    }
}

if (!mgmt_cport_id || !data_cport_id)
    return -ENODEV;

gcam = kzalloc(sizeof(*gcam), GFP_KERNEL);
if (!gcam)
    return -ENOMEM;

mutex_init(&gcam->mutex);

gcam->bundle = bundle;
gcam->state = GB_CAMERA_STATE_UNCONFIGURED;
gcam->data_cport_id = data_cport_id;

conn = gb_connection_create(bundle, mgmt_cport_id,
                gb_camera_request_handler);
if (IS_ERR(conn)) {
    ret = PTR_ERR(conn);
    goto error;
}

gcam->connection = conn;
gb_connection_set_data(conn, gcam);

ret = gb_connection_enable(conn);
if (ret)
    goto error;

ret = gb_camera_debugfs_init(gcam);
if (ret < 0)
    goto error;

gcam->module.priv = gcam;
gcam->module.ops = &gb_cam_ops;
gcam->module.interface_id = gcam->connection->intf->interface_id;
gcam->module.release = gb_camera_release_module;
ret = gb_camera_register(&gcam->module);
if (ret < 0)
    goto error;

greybus_set_drvdata(bundle, gcam);

gb_pm_runtime_put_autosuspend(gcam->bundle);

return 0;

error: gb_camera_cleanup(gcam); kfree(gcam); return ret; }

static void gb_camera_disconnect(struct gb_bundle bundle) { struct gb_camera gcam = greybus_get_drvdata(bundle); int ret;

ret = gb_pm_runtime_get_sync(bundle);
if (ret)
    gb_pm_runtime_get_noresume(bundle);

gb_camera_cleanup(gcam);
gb_camera_unregister(&gcam->module);

}

static const struct greybus_bundle_id gb_camera_id_table[] = { { GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_CAMERA) }, { }, };

ifdef CONFIG_PM

static int gb_camera_suspend(struct device dev) { struct gb_bundle bundle = to_gb_bundle(dev); struct gb_camera *gcam = greybus_get_drvdata(bundle);

if (gcam->data_connection)
    gb_connection_disable(gcam->data_connection);

gb_connection_disable(gcam->connection);

return 0;

}

static int gb_camera_resume(struct device dev) { struct gb_bundle bundle = to_gb_bundle(dev); struct gb_camera *gcam = greybus_get_drvdata(bundle); int ret;

ret = gb_connection_enable(gcam->connection);
if (ret) {
    gcam_err(gcam, "failed to enable connection: %d\n", ret);
    return ret;
}

if (gcam->data_connection) {
    ret = gb_connection_enable(gcam->data_connection);
    if (ret) {
        gcam_err(gcam,
             "failed to enable data connection: %d\n", ret);
        return ret;
    }
}

return 0;

}

endif

static const struct dev_pm_ops gb_camera_pm_ops = { SET_RUNTIME_PM_OPS(gb_camera_suspend, gb_camera_resume, NULL) };

static struct greybus_driver gb_camera_driver = { .name = "camera", .probe = gb_camera_probe, .disconnect = gb_camera_disconnect, .id_table = gb_camera_id_table, .driver.pm = &gb_camera_pm_ops, };

module_greybus_driver(gb_camera_driver);

MODULE_LICENSE("GPL v2");

MiRO92 / uYou-for-YouTube

YouTube #456

include <linux/debugfs.h>

include <linux/fs.h>

include <linux/kernel.h>

include <linux/module.h>

include <linux/slab.h>

include <linux/string.h>

include <linux/uaccess.h>

include <linux/vmalloc.h>

include <linux/greybus.h>

include "gb-camera.h"

include "greybus_protocols.h"

define ES2_APB_CDSI0_CPORT 16

define ES2_APB_CDSI1_CPORT 17

define GB_CAMERA_MAX_SETTINGS_SIZE 8192

define gcam_dbg(gcam, format...) dev_dbg(&gcam->bundle->dev, format)

define gcam_info(gcam, format...) dev_info(&gcam->bundle->dev, format)

define gcam_err(gcam, format...) dev_err(&gcam->bundle->dev, format)

define GB_CAMERA_CSI_FLAG_CLOCK_CONTINUOUS 0x01

define GB_CAMERA_CSI_NUM_DATA_LANES 4

define GB_CAMERA_CSI_CLK_FREQ_MAX 999000000U

define GB_CAMERA_CSI_CLK_FREQ_MIN 100000000U

define GB_CAMERA_CSI_CLK_FREQ_MARGIN 150000000U

ifdef CONFIG_PM

endif