1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Greybus Camera protocol driver.
4  *
5  * Copyright 2015 Google Inc.
6  * Copyright 2015 Linaro Ltd.
7  */
8 
9 #include <linux/debugfs.h>
10 #include <linux/fs.h>
11 #include <linux/kernel.h>
12 #include <linux/module.h>
13 #include <linux/slab.h>
14 #include <linux/string.h>
15 #include <linux/uaccess.h>
16 #include <linux/vmalloc.h>
17 #include <linux/greybus.h>
18 
19 #include "gb-camera.h"
20 #include "greybus_protocols.h"
21 
22 enum gb_camera_debugs_buffer_id {
23 	GB_CAMERA_DEBUGFS_BUFFER_CAPABILITIES,
24 	GB_CAMERA_DEBUGFS_BUFFER_STREAMS,
25 	GB_CAMERA_DEBUGFS_BUFFER_CAPTURE,
26 	GB_CAMERA_DEBUGFS_BUFFER_FLUSH,
27 	GB_CAMERA_DEBUGFS_BUFFER_MAX,
28 };
29 
30 struct gb_camera_debugfs_buffer {
31 	char data[PAGE_SIZE];
32 	size_t length;
33 };
34 
35 enum gb_camera_state {
36 	GB_CAMERA_STATE_UNCONFIGURED,
37 	GB_CAMERA_STATE_CONFIGURED,
38 };
39 
40 /**
41  * struct gb_camera - A Greybus Camera Device
42  * @connection: the greybus connection for camera management
43  * @data_connection: the greybus connection for camera data
44  * @data_cport_id: the data CPort ID on the module side
45  * @mutex: protects the connection and state fields
46  * @state: the current module state
47  * @debugfs: debugfs entries for camera protocol operations testing
48  * @module: Greybus camera module registered to HOST processor.
49  */
50 struct gb_camera {
51 	struct gb_bundle *bundle;
52 	struct gb_connection *connection;
53 	struct gb_connection *data_connection;
54 	u16 data_cport_id;
55 
56 	struct mutex mutex;
57 	enum gb_camera_state state;
58 
59 	struct {
60 		struct dentry *root;
61 		struct gb_camera_debugfs_buffer *buffers;
62 	} debugfs;
63 
64 	struct gb_camera_module module;
65 };
66 
67 struct gb_camera_stream_config {
68 	unsigned int width;
69 	unsigned int height;
70 	unsigned int format;
71 	unsigned int vc;
72 	unsigned int dt[2];
73 	unsigned int max_size;
74 };
75 
76 struct gb_camera_fmt_info {
77 	enum v4l2_mbus_pixelcode mbus_code;
78 	unsigned int gb_format;
79 	unsigned int bpp;
80 };
81 
82 /* GB format to media code map */
83 static const struct gb_camera_fmt_info gb_fmt_info[] = {
84 	{
85 		.mbus_code = V4L2_MBUS_FMT_UYVY8_1X16,
86 		.gb_format = 0x01,
87 		.bpp	   = 16,
88 	},
89 	{
90 		.mbus_code = V4L2_MBUS_FMT_NV12_1x8,
91 		.gb_format = 0x12,
92 		.bpp	   = 12,
93 	},
94 	{
95 		.mbus_code = V4L2_MBUS_FMT_NV21_1x8,
96 		.gb_format = 0x13,
97 		.bpp	   = 12,
98 	},
99 	{
100 		.mbus_code = V4L2_MBUS_FMT_YU12_1x8,
101 		.gb_format = 0x16,
102 		.bpp	   = 12,
103 	},
104 	{
105 		.mbus_code = V4L2_MBUS_FMT_YV12_1x8,
106 		.gb_format = 0x17,
107 		.bpp	   = 12,
108 	},
109 	{
110 		.mbus_code = V4L2_MBUS_FMT_JPEG_1X8,
111 		.gb_format = 0x40,
112 		.bpp	   = 0,
113 	},
114 	{
115 		.mbus_code = V4L2_MBUS_FMT_GB_CAM_METADATA_1X8,
116 		.gb_format = 0x41,
117 		.bpp	   = 0,
118 	},
119 	{
120 		.mbus_code = V4L2_MBUS_FMT_GB_CAM_DEBUG_DATA_1X8,
121 		.gb_format = 0x42,
122 		.bpp	   = 0,
123 	},
124 	{
125 		.mbus_code = V4L2_MBUS_FMT_SBGGR10_1X10,
126 		.gb_format = 0x80,
127 		.bpp	   = 10,
128 	},
129 	{
130 		.mbus_code = V4L2_MBUS_FMT_SGBRG10_1X10,
131 		.gb_format = 0x81,
132 		.bpp	   = 10,
133 	},
134 	{
135 		.mbus_code = V4L2_MBUS_FMT_SGRBG10_1X10,
136 		.gb_format = 0x82,
137 		.bpp	   = 10,
138 	},
139 	{
140 		.mbus_code = V4L2_MBUS_FMT_SRGGB10_1X10,
141 		.gb_format = 0x83,
142 		.bpp	   = 10,
143 	},
144 	{
145 		.mbus_code = V4L2_MBUS_FMT_SBGGR12_1X12,
146 		.gb_format = 0x84,
147 		.bpp	   = 12,
148 	},
149 	{
150 		.mbus_code = V4L2_MBUS_FMT_SGBRG12_1X12,
151 		.gb_format = 0x85,
152 		.bpp	   = 12,
153 	},
154 	{
155 		.mbus_code = V4L2_MBUS_FMT_SGRBG12_1X12,
156 		.gb_format = 0x86,
157 		.bpp	   = 12,
158 	},
159 	{
160 		.mbus_code = V4L2_MBUS_FMT_SRGGB12_1X12,
161 		.gb_format = 0x87,
162 		.bpp	   = 12,
163 	},
164 };
165 
gb_camera_get_format_info(u16 gb_fmt)166 static const struct gb_camera_fmt_info *gb_camera_get_format_info(u16 gb_fmt)
167 {
168 	unsigned int i;
169 
170 	for (i = 0; i < ARRAY_SIZE(gb_fmt_info); i++) {
171 		if (gb_fmt_info[i].gb_format == gb_fmt)
172 			return &gb_fmt_info[i];
173 	}
174 
175 	return NULL;
176 }
177 
178 #define ES2_APB_CDSI0_CPORT		16
179 #define ES2_APB_CDSI1_CPORT		17
180 
181 #define GB_CAMERA_MAX_SETTINGS_SIZE	8192
182 
183 #define gcam_dbg(gcam, format...)	dev_dbg(&gcam->bundle->dev, format)
184 #define gcam_info(gcam, format...)	dev_info(&gcam->bundle->dev, format)
185 #define gcam_err(gcam, format...)	dev_err(&gcam->bundle->dev, format)
186 
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)187 static int gb_camera_operation_sync_flags(struct gb_connection *connection,
188 					  int type, unsigned int flags,
189 					  void *request, size_t request_size,
190 					  void *response, size_t *response_size)
191 {
192 	struct gb_operation *operation;
193 	int ret;
194 
195 	operation = gb_operation_create_flags(connection, type, request_size,
196 					      *response_size, flags,
197 					      GFP_KERNEL);
198 	if (!operation)
199 		return  -ENOMEM;
200 
201 	if (request_size)
202 		memcpy(operation->request->payload, request, request_size);
203 
204 	ret = gb_operation_request_send_sync(operation);
205 	if (ret) {
206 		dev_err(&connection->hd->dev,
207 			"%s: synchronous operation of type 0x%02x failed: %d\n",
208 			connection->name, type, ret);
209 	} else {
210 		*response_size = operation->response->payload_size;
211 
212 		if (operation->response->payload_size)
213 			memcpy(response, operation->response->payload,
214 			       operation->response->payload_size);
215 	}
216 
217 	gb_operation_put(operation);
218 
219 	return ret;
220 }
221 
gb_camera_get_max_pkt_size(struct gb_camera * gcam,struct gb_camera_configure_streams_response * resp)222 static int gb_camera_get_max_pkt_size(struct gb_camera *gcam,
223 		struct gb_camera_configure_streams_response *resp)
224 {
225 	unsigned int max_pkt_size = 0;
226 	unsigned int i;
227 
228 	for (i = 0; i < resp->num_streams; i++) {
229 		struct gb_camera_stream_config_response *cfg = &resp->config[i];
230 		const struct gb_camera_fmt_info *fmt_info;
231 		unsigned int pkt_size;
232 
233 		fmt_info = gb_camera_get_format_info(cfg->format);
234 		if (!fmt_info) {
235 			gcam_err(gcam, "unsupported greybus image format: %d\n",
236 				 cfg->format);
237 			return -EIO;
238 		}
239 
240 		if (fmt_info->bpp == 0) {
241 			pkt_size = le32_to_cpu(cfg->max_pkt_size);
242 
243 			if (pkt_size == 0) {
244 				gcam_err(gcam,
245 					 "Stream %u: invalid zero maximum packet size\n",
246 					 i);
247 				return -EIO;
248 			}
249 		} else {
250 			pkt_size = le16_to_cpu(cfg->width) * fmt_info->bpp / 8;
251 
252 			if (pkt_size != le32_to_cpu(cfg->max_pkt_size)) {
253 				gcam_err(gcam,
254 					 "Stream %u: maximum packet size mismatch (%u/%u)\n",
255 					 i, pkt_size, cfg->max_pkt_size);
256 				return -EIO;
257 			}
258 		}
259 
260 		max_pkt_size = max(pkt_size, max_pkt_size);
261 	}
262 
263 	return max_pkt_size;
264 }
265 
266 /*
267  * Validate the stream configuration response verifying padding is correctly
268  * set and the returned number of streams is supported
269  */
gb_camera_configure_streams_validate_response(struct gb_camera * gcam,struct gb_camera_configure_streams_response * resp,unsigned int nstreams)270 static const int gb_camera_configure_streams_validate_response(
271 		struct gb_camera *gcam,
272 		struct gb_camera_configure_streams_response *resp,
273 		unsigned int nstreams)
274 {
275 	unsigned int i;
276 
277 	/* Validate the returned response structure */
278 	if (resp->padding[0] || resp->padding[1]) {
279 		gcam_err(gcam, "response padding != 0\n");
280 		return -EIO;
281 	}
282 
283 	if (resp->num_streams > nstreams) {
284 		gcam_err(gcam, "got #streams %u > request %u\n",
285 			 resp->num_streams, nstreams);
286 		return -EIO;
287 	}
288 
289 	for (i = 0; i < resp->num_streams; i++) {
290 		struct gb_camera_stream_config_response *cfg = &resp->config[i];
291 
292 		if (cfg->padding) {
293 			gcam_err(gcam, "stream #%u padding != 0\n", i);
294 			return -EIO;
295 		}
296 	}
297 
298 	return 0;
299 }
300 
301 /* -----------------------------------------------------------------------------
302  * Hardware Configuration
303  */
304 
gb_camera_set_intf_power_mode(struct gb_camera * gcam,u8 intf_id,bool hs)305 static int gb_camera_set_intf_power_mode(struct gb_camera *gcam, u8 intf_id,
306 					 bool hs)
307 {
308 	struct gb_svc *svc = gcam->connection->hd->svc;
309 	int ret;
310 
311 	if (hs)
312 		ret = gb_svc_intf_set_power_mode(svc, intf_id,
313 						 GB_SVC_UNIPRO_HS_SERIES_A,
314 						 GB_SVC_UNIPRO_FAST_MODE, 2, 2,
315 						 GB_SVC_SMALL_AMPLITUDE,
316 						 GB_SVC_NO_DE_EMPHASIS,
317 						 GB_SVC_UNIPRO_FAST_MODE, 2, 2,
318 						 GB_SVC_PWRM_RXTERMINATION |
319 						 GB_SVC_PWRM_TXTERMINATION, 0,
320 						 NULL, NULL);
321 	else
322 		ret = gb_svc_intf_set_power_mode(svc, intf_id,
323 						 GB_SVC_UNIPRO_HS_SERIES_A,
324 						 GB_SVC_UNIPRO_SLOW_AUTO_MODE,
325 						 2, 1,
326 						 GB_SVC_SMALL_AMPLITUDE,
327 						 GB_SVC_NO_DE_EMPHASIS,
328 						 GB_SVC_UNIPRO_SLOW_AUTO_MODE,
329 						 2, 1,
330 						 0, 0,
331 						 NULL, NULL);
332 
333 	return ret;
334 }
335 
gb_camera_set_power_mode(struct gb_camera * gcam,bool hs)336 static int gb_camera_set_power_mode(struct gb_camera *gcam, bool hs)
337 {
338 	struct gb_interface *intf = gcam->connection->intf;
339 	struct gb_svc *svc = gcam->connection->hd->svc;
340 	int ret;
341 
342 	ret = gb_camera_set_intf_power_mode(gcam, intf->interface_id, hs);
343 	if (ret < 0) {
344 		gcam_err(gcam, "failed to set module interface to %s (%d)\n",
345 			 hs ? "HS" : "PWM", ret);
346 		return ret;
347 	}
348 
349 	ret = gb_camera_set_intf_power_mode(gcam, svc->ap_intf_id, hs);
350 	if (ret < 0) {
351 		gb_camera_set_intf_power_mode(gcam, intf->interface_id, !hs);
352 		gcam_err(gcam, "failed to set AP interface to %s (%d)\n",
353 			 hs ? "HS" : "PWM", ret);
354 		return ret;
355 	}
356 
357 	return 0;
358 }
359 
360 struct ap_csi_config_request {
361 	__u8 csi_id;
362 	__u8 flags;
363 #define GB_CAMERA_CSI_FLAG_CLOCK_CONTINUOUS 0x01
364 	__u8 num_lanes;
365 	__u8 padding;
366 	__le32 csi_clk_freq;
367 	__le32 max_pkt_size;
368 } __packed;
369 
370 /*
371  * TODO: Compute the number of lanes dynamically based on bandwidth
372  * requirements.
373  */
374 #define GB_CAMERA_CSI_NUM_DATA_LANES		4
375 
376 #define GB_CAMERA_CSI_CLK_FREQ_MAX		999000000U
377 #define GB_CAMERA_CSI_CLK_FREQ_MIN		100000000U
378 #define GB_CAMERA_CSI_CLK_FREQ_MARGIN		150000000U
379 
gb_camera_setup_data_connection(struct gb_camera * gcam,struct gb_camera_configure_streams_response * resp,struct gb_camera_csi_params * csi_params)380 static int gb_camera_setup_data_connection(struct gb_camera *gcam,
381 		struct gb_camera_configure_streams_response *resp,
382 		struct gb_camera_csi_params *csi_params)
383 {
384 	struct ap_csi_config_request csi_cfg;
385 	struct gb_connection *conn;
386 	unsigned int clk_freq;
387 	int ret;
388 
389 	/*
390 	 * Create the data connection between the camera module data CPort and
391 	 * APB CDSI1. The CDSI1 CPort ID is hardcoded by the ES2 bridge.
392 	 */
393 	conn = gb_connection_create_offloaded(gcam->bundle, gcam->data_cport_id,
394 					      GB_CONNECTION_FLAG_NO_FLOWCTRL |
395 					      GB_CONNECTION_FLAG_CDSI1);
396 	if (IS_ERR(conn))
397 		return PTR_ERR(conn);
398 
399 	gcam->data_connection = conn;
400 	gb_connection_set_data(conn, gcam);
401 
402 	ret = gb_connection_enable(conn);
403 	if (ret)
404 		goto error_conn_destroy;
405 
406 	/* Set the UniPro link to high speed mode. */
407 	ret = gb_camera_set_power_mode(gcam, true);
408 	if (ret < 0)
409 		goto error_conn_disable;
410 
411 	/*
412 	 * Configure the APB-A CSI-2 transmitter.
413 	 *
414 	 * Hardcode the number of lanes to 4 and compute the bus clock frequency
415 	 * based on the module bandwidth requirements with a safety margin.
416 	 */
417 	memset(&csi_cfg, 0, sizeof(csi_cfg));
418 	csi_cfg.csi_id = 1;
419 	csi_cfg.flags = 0;
420 	csi_cfg.num_lanes = GB_CAMERA_CSI_NUM_DATA_LANES;
421 
422 	clk_freq = resp->data_rate / 2 / GB_CAMERA_CSI_NUM_DATA_LANES;
423 	clk_freq = clamp(clk_freq + GB_CAMERA_CSI_CLK_FREQ_MARGIN,
424 			 GB_CAMERA_CSI_CLK_FREQ_MIN,
425 			 GB_CAMERA_CSI_CLK_FREQ_MAX);
426 	csi_cfg.csi_clk_freq = clk_freq;
427 
428 	ret = gb_camera_get_max_pkt_size(gcam, resp);
429 	if (ret < 0) {
430 		ret = -EIO;
431 		goto error_power;
432 	}
433 	csi_cfg.max_pkt_size = ret;
434 
435 	ret = gb_hd_output(gcam->connection->hd, &csi_cfg,
436 			   sizeof(csi_cfg),
437 			   GB_APB_REQUEST_CSI_TX_CONTROL, false);
438 	if (ret < 0) {
439 		gcam_err(gcam, "failed to start the CSI transmitter\n");
440 		goto error_power;
441 	}
442 
443 	if (csi_params) {
444 		csi_params->clk_freq = csi_cfg.csi_clk_freq;
445 		csi_params->num_lanes = csi_cfg.num_lanes;
446 	}
447 
448 	return 0;
449 
450 error_power:
451 	gb_camera_set_power_mode(gcam, false);
452 error_conn_disable:
453 	gb_connection_disable(gcam->data_connection);
454 error_conn_destroy:
455 	gb_connection_destroy(gcam->data_connection);
456 	gcam->data_connection = NULL;
457 	return ret;
458 }
459 
gb_camera_teardown_data_connection(struct gb_camera * gcam)460 static void gb_camera_teardown_data_connection(struct gb_camera *gcam)
461 {
462 	struct ap_csi_config_request csi_cfg;
463 	int ret;
464 
465 	/* Stop the APB1 CSI transmitter. */
466 	memset(&csi_cfg, 0, sizeof(csi_cfg));
467 	csi_cfg.csi_id = 1;
468 
469 	ret = gb_hd_output(gcam->connection->hd, &csi_cfg,
470 			   sizeof(csi_cfg),
471 			   GB_APB_REQUEST_CSI_TX_CONTROL, false);
472 
473 	if (ret < 0)
474 		gcam_err(gcam, "failed to stop the CSI transmitter\n");
475 
476 	/* Set the UniPro link to low speed mode. */
477 	gb_camera_set_power_mode(gcam, false);
478 
479 	/* Destroy the data connection. */
480 	gb_connection_disable(gcam->data_connection);
481 	gb_connection_destroy(gcam->data_connection);
482 	gcam->data_connection = NULL;
483 }
484 
485 /* -----------------------------------------------------------------------------
486  * Camera Protocol Operations
487  */
488 
gb_camera_capabilities(struct gb_camera * gcam,u8 * capabilities,size_t * size)489 static int gb_camera_capabilities(struct gb_camera *gcam,
490 				  u8 *capabilities, size_t *size)
491 {
492 	int ret;
493 
494 	ret = gb_pm_runtime_get_sync(gcam->bundle);
495 	if (ret)
496 		return ret;
497 
498 	mutex_lock(&gcam->mutex);
499 
500 	if (!gcam->connection) {
501 		ret = -EINVAL;
502 		goto done;
503 	}
504 
505 	ret = gb_camera_operation_sync_flags(gcam->connection,
506 					     GB_CAMERA_TYPE_CAPABILITIES,
507 					     GB_OPERATION_FLAG_SHORT_RESPONSE,
508 					     NULL, 0,
509 					     (void *)capabilities, size);
510 	if (ret)
511 		gcam_err(gcam, "failed to retrieve capabilities: %d\n", ret);
512 
513 done:
514 	mutex_unlock(&gcam->mutex);
515 
516 	gb_pm_runtime_put_autosuspend(gcam->bundle);
517 
518 	return ret;
519 }
520 
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)521 static int gb_camera_configure_streams(struct gb_camera *gcam,
522 				       unsigned int *num_streams,
523 				       unsigned int *flags,
524 				       struct gb_camera_stream_config *streams,
525 				       struct gb_camera_csi_params *csi_params)
526 {
527 	struct gb_camera_configure_streams_request *req;
528 	struct gb_camera_configure_streams_response *resp;
529 	unsigned int nstreams = *num_streams;
530 	unsigned int i;
531 	size_t req_size;
532 	size_t resp_size;
533 	int ret;
534 
535 	if (nstreams > GB_CAMERA_MAX_STREAMS)
536 		return -EINVAL;
537 
538 	req_size = sizeof(*req) + nstreams * sizeof(req->config[0]);
539 	resp_size = sizeof(*resp) + nstreams * sizeof(resp->config[0]);
540 
541 	req = kmalloc(req_size, GFP_KERNEL);
542 	resp = kmalloc(resp_size, GFP_KERNEL);
543 	if (!req || !resp) {
544 		kfree(req);
545 		kfree(resp);
546 		return -ENOMEM;
547 	}
548 
549 	req->num_streams = nstreams;
550 	req->flags = *flags;
551 	req->padding = 0;
552 
553 	for (i = 0; i < nstreams; ++i) {
554 		struct gb_camera_stream_config_request *cfg = &req->config[i];
555 
556 		cfg->width = cpu_to_le16(streams[i].width);
557 		cfg->height = cpu_to_le16(streams[i].height);
558 		cfg->format = cpu_to_le16(streams[i].format);
559 		cfg->padding = 0;
560 	}
561 
562 	mutex_lock(&gcam->mutex);
563 
564 	ret = gb_pm_runtime_get_sync(gcam->bundle);
565 	if (ret)
566 		goto done_skip_pm_put;
567 
568 	if (!gcam->connection) {
569 		ret = -EINVAL;
570 		goto done;
571 	}
572 
573 	ret = gb_camera_operation_sync_flags(gcam->connection,
574 					     GB_CAMERA_TYPE_CONFIGURE_STREAMS,
575 					     GB_OPERATION_FLAG_SHORT_RESPONSE,
576 					     req, req_size,
577 					     resp, &resp_size);
578 	if (ret < 0)
579 		goto done;
580 
581 	ret = gb_camera_configure_streams_validate_response(gcam, resp,
582 							    nstreams);
583 	if (ret < 0)
584 		goto done;
585 
586 	*flags = resp->flags;
587 	*num_streams = resp->num_streams;
588 
589 	for (i = 0; i < resp->num_streams; ++i) {
590 		struct gb_camera_stream_config_response *cfg = &resp->config[i];
591 
592 		streams[i].width = le16_to_cpu(cfg->width);
593 		streams[i].height = le16_to_cpu(cfg->height);
594 		streams[i].format = le16_to_cpu(cfg->format);
595 		streams[i].vc = cfg->virtual_channel;
596 		streams[i].dt[0] = cfg->data_type[0];
597 		streams[i].dt[1] = cfg->data_type[1];
598 		streams[i].max_size = le32_to_cpu(cfg->max_size);
599 	}
600 
601 	if ((resp->flags & GB_CAMERA_CONFIGURE_STREAMS_ADJUSTED) ||
602 	    (req->flags & GB_CAMERA_CONFIGURE_STREAMS_TEST_ONLY))
603 		goto done;
604 
605 	if (gcam->state == GB_CAMERA_STATE_CONFIGURED) {
606 		gb_camera_teardown_data_connection(gcam);
607 		gcam->state = GB_CAMERA_STATE_UNCONFIGURED;
608 
609 		/*
610 		 * When unconfiguring streams release the PM runtime reference
611 		 * that was acquired when streams were configured. The bundle
612 		 * won't be suspended until the PM runtime reference acquired at
613 		 * the beginning of this function gets released right before
614 		 * returning.
615 		 */
616 		gb_pm_runtime_put_noidle(gcam->bundle);
617 	}
618 
619 	if (resp->num_streams == 0)
620 		goto done;
621 
622 	/*
623 	 * Make sure the bundle won't be suspended until streams get
624 	 * unconfigured after the stream is configured successfully
625 	 */
626 	gb_pm_runtime_get_noresume(gcam->bundle);
627 
628 	/* Setup CSI-2 connection from APB-A to AP */
629 	ret = gb_camera_setup_data_connection(gcam, resp, csi_params);
630 	if (ret < 0) {
631 		memset(req, 0, sizeof(*req));
632 		gb_operation_sync(gcam->connection,
633 				  GB_CAMERA_TYPE_CONFIGURE_STREAMS,
634 				  req, sizeof(*req),
635 				  resp, sizeof(*resp));
636 		*flags = 0;
637 		*num_streams = 0;
638 		gb_pm_runtime_put_noidle(gcam->bundle);
639 		goto done;
640 	}
641 
642 	gcam->state = GB_CAMERA_STATE_CONFIGURED;
643 
644 done:
645 	gb_pm_runtime_put_autosuspend(gcam->bundle);
646 
647 done_skip_pm_put:
648 	mutex_unlock(&gcam->mutex);
649 	kfree(req);
650 	kfree(resp);
651 	return ret;
652 }
653 
gb_camera_capture(struct gb_camera * gcam,u32 request_id,unsigned int streams,unsigned int num_frames,size_t settings_size,const void * settings)654 static int gb_camera_capture(struct gb_camera *gcam, u32 request_id,
655 			     unsigned int streams, unsigned int num_frames,
656 			     size_t settings_size, const void *settings)
657 {
658 	struct gb_camera_capture_request *req;
659 	size_t req_size;
660 	int ret;
661 
662 	if (settings_size > GB_CAMERA_MAX_SETTINGS_SIZE)
663 		return -EINVAL;
664 
665 	req_size = sizeof(*req) + settings_size;
666 	req = kmalloc(req_size, GFP_KERNEL);
667 	if (!req)
668 		return -ENOMEM;
669 
670 	req->request_id = cpu_to_le32(request_id);
671 	req->streams = streams;
672 	req->padding = 0;
673 	req->num_frames = cpu_to_le16(num_frames);
674 	memcpy(req->settings, settings, settings_size);
675 
676 	mutex_lock(&gcam->mutex);
677 
678 	if (!gcam->connection) {
679 		ret = -EINVAL;
680 		goto done;
681 	}
682 
683 	ret = gb_operation_sync(gcam->connection, GB_CAMERA_TYPE_CAPTURE,
684 				req, req_size, NULL, 0);
685 done:
686 	mutex_unlock(&gcam->mutex);
687 
688 	kfree(req);
689 
690 	return ret;
691 }
692 
gb_camera_flush(struct gb_camera * gcam,u32 * request_id)693 static int gb_camera_flush(struct gb_camera *gcam, u32 *request_id)
694 {
695 	struct gb_camera_flush_response resp;
696 	int ret;
697 
698 	mutex_lock(&gcam->mutex);
699 
700 	if (!gcam->connection) {
701 		ret = -EINVAL;
702 		goto done;
703 	}
704 
705 	ret = gb_operation_sync(gcam->connection, GB_CAMERA_TYPE_FLUSH, NULL, 0,
706 				&resp, sizeof(resp));
707 
708 	if (ret < 0)
709 		goto done;
710 
711 	if (request_id)
712 		*request_id = le32_to_cpu(resp.request_id);
713 
714 done:
715 	mutex_unlock(&gcam->mutex);
716 
717 	return ret;
718 }
719 
gb_camera_request_handler(struct gb_operation * op)720 static int gb_camera_request_handler(struct gb_operation *op)
721 {
722 	struct gb_camera *gcam = gb_connection_get_data(op->connection);
723 	struct gb_camera_metadata_request *payload;
724 	struct gb_message *request;
725 
726 	if (op->type != GB_CAMERA_TYPE_METADATA) {
727 		gcam_err(gcam, "Unsupported unsolicited event: %u\n", op->type);
728 		return -EINVAL;
729 	}
730 
731 	request = op->request;
732 
733 	if (request->payload_size < sizeof(*payload)) {
734 		gcam_err(gcam, "Wrong event size received (%zu < %zu)\n",
735 			 request->payload_size, sizeof(*payload));
736 		return -EINVAL;
737 	}
738 
739 	payload = request->payload;
740 
741 	gcam_dbg(gcam, "received metadata for request %u, frame %u, stream %u\n",
742 		 payload->request_id, payload->frame_number, payload->stream);
743 
744 	return 0;
745 }
746 
747 /* -----------------------------------------------------------------------------
748  * Interface with HOST gmp camera.
749  */
gb_camera_mbus_to_gb(enum v4l2_mbus_pixelcode mbus_code)750 static unsigned int gb_camera_mbus_to_gb(enum v4l2_mbus_pixelcode mbus_code)
751 {
752 	unsigned int i;
753 
754 	for (i = 0; i < ARRAY_SIZE(gb_fmt_info); i++) {
755 		if (gb_fmt_info[i].mbus_code == mbus_code)
756 			return gb_fmt_info[i].gb_format;
757 	}
758 	return gb_fmt_info[0].gb_format;
759 }
760 
gb_camera_gb_to_mbus(u16 gb_fmt)761 static enum v4l2_mbus_pixelcode gb_camera_gb_to_mbus(u16 gb_fmt)
762 {
763 	unsigned int i;
764 
765 	for (i = 0; i < ARRAY_SIZE(gb_fmt_info); i++) {
766 		if (gb_fmt_info[i].gb_format == gb_fmt)
767 			return gb_fmt_info[i].mbus_code;
768 	}
769 	return gb_fmt_info[0].mbus_code;
770 }
771 
gb_camera_op_capabilities(void * priv,char * data,size_t len)772 static ssize_t gb_camera_op_capabilities(void *priv, char *data, size_t len)
773 {
774 	struct gb_camera *gcam = priv;
775 	size_t capabilities_len = len;
776 	int ret;
777 
778 	ret = gb_camera_capabilities(gcam, data, &capabilities_len);
779 	if (ret)
780 		return ret;
781 
782 	return capabilities_len;
783 }
784 
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)785 static int gb_camera_op_configure_streams(void *priv, unsigned int *nstreams,
786 		unsigned int *flags, struct gb_camera_stream *streams,
787 		struct gb_camera_csi_params *csi_params)
788 {
789 	struct gb_camera *gcam = priv;
790 	struct gb_camera_stream_config *gb_streams;
791 	unsigned int gb_flags = 0;
792 	unsigned int gb_nstreams = *nstreams;
793 	unsigned int i;
794 	int ret;
795 
796 	if (gb_nstreams > GB_CAMERA_MAX_STREAMS)
797 		return -EINVAL;
798 
799 	gb_streams = kcalloc(gb_nstreams, sizeof(*gb_streams), GFP_KERNEL);
800 	if (!gb_streams)
801 		return -ENOMEM;
802 
803 	for (i = 0; i < gb_nstreams; i++) {
804 		gb_streams[i].width = streams[i].width;
805 		gb_streams[i].height = streams[i].height;
806 		gb_streams[i].format =
807 			gb_camera_mbus_to_gb(streams[i].pixel_code);
808 	}
809 
810 	if (*flags & GB_CAMERA_IN_FLAG_TEST)
811 		gb_flags |= GB_CAMERA_CONFIGURE_STREAMS_TEST_ONLY;
812 
813 	ret = gb_camera_configure_streams(gcam, &gb_nstreams,
814 					  &gb_flags, gb_streams, csi_params);
815 	if (ret < 0)
816 		goto done;
817 	if (gb_nstreams > *nstreams) {
818 		ret = -EINVAL;
819 		goto done;
820 	}
821 
822 	*flags = 0;
823 	if (gb_flags & GB_CAMERA_CONFIGURE_STREAMS_ADJUSTED)
824 		*flags |= GB_CAMERA_OUT_FLAG_ADJUSTED;
825 
826 	for (i = 0; i < gb_nstreams; i++) {
827 		streams[i].width = gb_streams[i].width;
828 		streams[i].height = gb_streams[i].height;
829 		streams[i].vc = gb_streams[i].vc;
830 		streams[i].dt[0] = gb_streams[i].dt[0];
831 		streams[i].dt[1] = gb_streams[i].dt[1];
832 		streams[i].max_size = gb_streams[i].max_size;
833 		streams[i].pixel_code =
834 			gb_camera_gb_to_mbus(gb_streams[i].format);
835 	}
836 	*nstreams = gb_nstreams;
837 
838 done:
839 	kfree(gb_streams);
840 	return ret;
841 }
842 
gb_camera_op_capture(void * priv,u32 request_id,unsigned int streams,unsigned int num_frames,size_t settings_size,const void * settings)843 static int gb_camera_op_capture(void *priv, u32 request_id,
844 				unsigned int streams, unsigned int num_frames,
845 				size_t settings_size, const void *settings)
846 {
847 	struct gb_camera *gcam = priv;
848 
849 	return gb_camera_capture(gcam, request_id, streams, num_frames,
850 				 settings_size, settings);
851 }
852 
gb_camera_op_flush(void * priv,u32 * request_id)853 static int gb_camera_op_flush(void *priv, u32 *request_id)
854 {
855 	struct gb_camera *gcam = priv;
856 
857 	return gb_camera_flush(gcam, request_id);
858 }
859 
860 static const struct gb_camera_ops gb_cam_ops = {
861 	.capabilities = gb_camera_op_capabilities,
862 	.configure_streams = gb_camera_op_configure_streams,
863 	.capture = gb_camera_op_capture,
864 	.flush = gb_camera_op_flush,
865 };
866 
867 /* -----------------------------------------------------------------------------
868  * DebugFS
869  */
870 
gb_camera_debugfs_capabilities(struct gb_camera * gcam,char * buf,size_t len)871 static ssize_t gb_camera_debugfs_capabilities(struct gb_camera *gcam,
872 					      char *buf, size_t len)
873 {
874 	struct gb_camera_debugfs_buffer *buffer =
875 		&gcam->debugfs.buffers[GB_CAMERA_DEBUGFS_BUFFER_CAPABILITIES];
876 	size_t size = 1024;
877 	unsigned int i;
878 	u8 *caps;
879 	int ret;
880 
881 	caps = kmalloc(size, GFP_KERNEL);
882 	if (!caps)
883 		return -ENOMEM;
884 
885 	ret = gb_camera_capabilities(gcam, caps, &size);
886 	if (ret < 0)
887 		goto done;
888 
889 	/*
890 	 * hex_dump_to_buffer() doesn't return the number of bytes dumped prior
891 	 * to v4.0, we need our own implementation :-(
892 	 */
893 	buffer->length = 0;
894 
895 	for (i = 0; i < size; i += 16) {
896 		unsigned int nbytes = min_t(unsigned int, size - i, 16);
897 
898 		buffer->length += sprintf(buffer->data + buffer->length,
899 					  "%*ph\n", nbytes, caps + i);
900 	}
901 
902 done:
903 	kfree(caps);
904 	return ret;
905 }
906 
gb_camera_debugfs_configure_streams(struct gb_camera * gcam,char * buf,size_t len)907 static ssize_t gb_camera_debugfs_configure_streams(struct gb_camera *gcam,
908 						   char *buf, size_t len)
909 {
910 	struct gb_camera_debugfs_buffer *buffer =
911 		&gcam->debugfs.buffers[GB_CAMERA_DEBUGFS_BUFFER_STREAMS];
912 	struct gb_camera_stream_config *streams;
913 	unsigned int nstreams;
914 	unsigned int flags;
915 	unsigned int i;
916 	char *token;
917 	int ret;
918 
919 	/* Retrieve number of streams to configure */
920 	token = strsep(&buf, ";");
921 	if (!token)
922 		return -EINVAL;
923 
924 	ret = kstrtouint(token, 10, &nstreams);
925 	if (ret < 0)
926 		return ret;
927 
928 	if (nstreams > GB_CAMERA_MAX_STREAMS)
929 		return -EINVAL;
930 
931 	token = strsep(&buf, ";");
932 	if (!token)
933 		return -EINVAL;
934 
935 	ret = kstrtouint(token, 10, &flags);
936 	if (ret < 0)
937 		return ret;
938 
939 	/* For each stream to configure parse width, height and format */
940 	streams = kcalloc(nstreams, sizeof(*streams), GFP_KERNEL);
941 	if (!streams)
942 		return -ENOMEM;
943 
944 	for (i = 0; i < nstreams; ++i) {
945 		struct gb_camera_stream_config *stream = &streams[i];
946 
947 		/* width */
948 		token = strsep(&buf, ";");
949 		if (!token) {
950 			ret = -EINVAL;
951 			goto done;
952 		}
953 		ret = kstrtouint(token, 10, &stream->width);
954 		if (ret < 0)
955 			goto done;
956 
957 		/* height */
958 		token = strsep(&buf, ";");
959 		if (!token)
960 			goto done;
961 
962 		ret = kstrtouint(token, 10, &stream->height);
963 		if (ret < 0)
964 			goto done;
965 
966 		/* Image format code */
967 		token = strsep(&buf, ";");
968 		if (!token)
969 			goto done;
970 
971 		ret = kstrtouint(token, 16, &stream->format);
972 		if (ret < 0)
973 			goto done;
974 	}
975 
976 	ret = gb_camera_configure_streams(gcam, &nstreams, &flags, streams,
977 					  NULL);
978 	if (ret < 0)
979 		goto done;
980 
981 	buffer->length = sprintf(buffer->data, "%u;%u;", nstreams, flags);
982 
983 	for (i = 0; i < nstreams; ++i) {
984 		struct gb_camera_stream_config *stream = &streams[i];
985 
986 		buffer->length += sprintf(buffer->data + buffer->length,
987 					  "%u;%u;%u;%u;%u;%u;%u;",
988 					  stream->width, stream->height,
989 					  stream->format, stream->vc,
990 					  stream->dt[0], stream->dt[1],
991 					  stream->max_size);
992 	}
993 
994 	ret = len;
995 
996 done:
997 	kfree(streams);
998 	return ret;
999 };
1000 
gb_camera_debugfs_capture(struct gb_camera * gcam,char * buf,size_t len)1001 static ssize_t gb_camera_debugfs_capture(struct gb_camera *gcam,
1002 					 char *buf, size_t len)
1003 {
1004 	unsigned int request_id;
1005 	unsigned int streams_mask;
1006 	unsigned int num_frames;
1007 	char *token;
1008 	int ret;
1009 
1010 	/* Request id */
1011 	token = strsep(&buf, ";");
1012 	if (!token)
1013 		return -EINVAL;
1014 	ret = kstrtouint(token, 10, &request_id);
1015 	if (ret < 0)
1016 		return ret;
1017 
1018 	/* Stream mask */
1019 	token = strsep(&buf, ";");
1020 	if (!token)
1021 		return -EINVAL;
1022 	ret = kstrtouint(token, 16, &streams_mask);
1023 	if (ret < 0)
1024 		return ret;
1025 
1026 	/* number of frames */
1027 	token = strsep(&buf, ";");
1028 	if (!token)
1029 		return -EINVAL;
1030 	ret = kstrtouint(token, 10, &num_frames);
1031 	if (ret < 0)
1032 		return ret;
1033 
1034 	ret = gb_camera_capture(gcam, request_id, streams_mask, num_frames, 0,
1035 				NULL);
1036 	if (ret < 0)
1037 		return ret;
1038 
1039 	return len;
1040 }
1041 
gb_camera_debugfs_flush(struct gb_camera * gcam,char * buf,size_t len)1042 static ssize_t gb_camera_debugfs_flush(struct gb_camera *gcam,
1043 				       char *buf, size_t len)
1044 {
1045 	struct gb_camera_debugfs_buffer *buffer =
1046 		&gcam->debugfs.buffers[GB_CAMERA_DEBUGFS_BUFFER_FLUSH];
1047 	unsigned int req_id;
1048 	int ret;
1049 
1050 	ret = gb_camera_flush(gcam, &req_id);
1051 	if (ret < 0)
1052 		return ret;
1053 
1054 	buffer->length = sprintf(buffer->data, "%u", req_id);
1055 
1056 	return len;
1057 }
1058 
1059 struct gb_camera_debugfs_entry {
1060 	const char *name;
1061 	unsigned int mask;
1062 	unsigned int buffer;
1063 	ssize_t (*execute)(struct gb_camera *gcam, char *buf, size_t len);
1064 };
1065 
1066 static const struct gb_camera_debugfs_entry gb_camera_debugfs_entries[] = {
1067 	{
1068 		.name = "capabilities",
1069 		.mask = S_IFREG | 0444,
1070 		.buffer = GB_CAMERA_DEBUGFS_BUFFER_CAPABILITIES,
1071 		.execute = gb_camera_debugfs_capabilities,
1072 	}, {
1073 		.name = "configure_streams",
1074 		.mask = S_IFREG | 0666,
1075 		.buffer = GB_CAMERA_DEBUGFS_BUFFER_STREAMS,
1076 		.execute = gb_camera_debugfs_configure_streams,
1077 	}, {
1078 		.name = "capture",
1079 		.mask = S_IFREG | 0666,
1080 		.buffer = GB_CAMERA_DEBUGFS_BUFFER_CAPTURE,
1081 		.execute = gb_camera_debugfs_capture,
1082 	}, {
1083 		.name = "flush",
1084 		.mask = S_IFREG | 0666,
1085 		.buffer = GB_CAMERA_DEBUGFS_BUFFER_FLUSH,
1086 		.execute = gb_camera_debugfs_flush,
1087 	},
1088 };
1089 
gb_camera_debugfs_read(struct file * file,char __user * buf,size_t len,loff_t * offset)1090 static ssize_t gb_camera_debugfs_read(struct file *file, char __user *buf,
1091 				      size_t len, loff_t *offset)
1092 {
1093 	const struct gb_camera_debugfs_entry *op = file->private_data;
1094 	struct gb_camera *gcam = file_inode(file)->i_private;
1095 	struct gb_camera_debugfs_buffer *buffer;
1096 	ssize_t ret;
1097 
1098 	/* For read-only entries the operation is triggered by a read. */
1099 	if (!(op->mask & 0222)) {
1100 		ret = op->execute(gcam, NULL, 0);
1101 		if (ret < 0)
1102 			return ret;
1103 	}
1104 
1105 	buffer = &gcam->debugfs.buffers[op->buffer];
1106 
1107 	return simple_read_from_buffer(buf, len, offset, buffer->data,
1108 				       buffer->length);
1109 }
1110 
gb_camera_debugfs_write(struct file * file,const char __user * buf,size_t len,loff_t * offset)1111 static ssize_t gb_camera_debugfs_write(struct file *file,
1112 				       const char __user *buf, size_t len,
1113 				       loff_t *offset)
1114 {
1115 	const struct gb_camera_debugfs_entry *op = file->private_data;
1116 	struct gb_camera *gcam = file_inode(file)->i_private;
1117 	ssize_t ret;
1118 	char *kbuf;
1119 
1120 	if (len > 1024)
1121 		return -EINVAL;
1122 
1123 	kbuf = memdup_user_nul(buf, len);
1124 	if (IS_ERR(kbuf))
1125 		return PTR_ERR(kbuf);
1126 
1127 	ret = op->execute(gcam, kbuf, len);
1128 
1129 done:
1130 	kfree(kbuf);
1131 	return ret;
1132 }
1133 
gb_camera_debugfs_open(struct inode * inode,struct file * file)1134 static int gb_camera_debugfs_open(struct inode *inode, struct file *file)
1135 {
1136 	unsigned int i;
1137 
1138 	for (i = 0; i < ARRAY_SIZE(gb_camera_debugfs_entries); ++i) {
1139 		const struct gb_camera_debugfs_entry *entry =
1140 			&gb_camera_debugfs_entries[i];
1141 
1142 		if (!strcmp(file->f_path.dentry->d_iname, entry->name)) {
1143 			file->private_data = (void *)entry;
1144 			break;
1145 		}
1146 	}
1147 
1148 	return 0;
1149 }
1150 
1151 static const struct file_operations gb_camera_debugfs_ops = {
1152 	.open = gb_camera_debugfs_open,
1153 	.read = gb_camera_debugfs_read,
1154 	.write = gb_camera_debugfs_write,
1155 };
1156 
gb_camera_debugfs_init(struct gb_camera * gcam)1157 static int gb_camera_debugfs_init(struct gb_camera *gcam)
1158 {
1159 	struct gb_connection *connection = gcam->connection;
1160 	char dirname[27];
1161 	unsigned int i;
1162 
1163 	/*
1164 	 * Create root debugfs entry and a file entry for each camera operation.
1165 	 */
1166 	snprintf(dirname, 27, "camera-%u.%u", connection->intf->interface_id,
1167 		 gcam->bundle->id);
1168 
1169 	gcam->debugfs.root = debugfs_create_dir(dirname, gb_debugfs_get());
1170 
1171 	gcam->debugfs.buffers =
1172 		vmalloc(array_size(GB_CAMERA_DEBUGFS_BUFFER_MAX,
1173 				   sizeof(*gcam->debugfs.buffers)));
1174 	if (!gcam->debugfs.buffers)
1175 		return -ENOMEM;
1176 
1177 	for (i = 0; i < ARRAY_SIZE(gb_camera_debugfs_entries); ++i) {
1178 		const struct gb_camera_debugfs_entry *entry =
1179 			&gb_camera_debugfs_entries[i];
1180 
1181 		gcam->debugfs.buffers[i].length = 0;
1182 
1183 		debugfs_create_file(entry->name, entry->mask,
1184 				    gcam->debugfs.root, gcam,
1185 				    &gb_camera_debugfs_ops);
1186 	}
1187 
1188 	return 0;
1189 }
1190 
gb_camera_debugfs_cleanup(struct gb_camera * gcam)1191 static void gb_camera_debugfs_cleanup(struct gb_camera *gcam)
1192 {
1193 	debugfs_remove_recursive(gcam->debugfs.root);
1194 
1195 	vfree(gcam->debugfs.buffers);
1196 }
1197 
1198 /* -----------------------------------------------------------------------------
1199  * Init & Cleanup
1200  */
1201 
gb_camera_cleanup(struct gb_camera * gcam)1202 static void gb_camera_cleanup(struct gb_camera *gcam)
1203 {
1204 	gb_camera_debugfs_cleanup(gcam);
1205 
1206 	mutex_lock(&gcam->mutex);
1207 	if (gcam->data_connection) {
1208 		gb_connection_disable(gcam->data_connection);
1209 		gb_connection_destroy(gcam->data_connection);
1210 		gcam->data_connection = NULL;
1211 	}
1212 
1213 	if (gcam->connection) {
1214 		gb_connection_disable(gcam->connection);
1215 		gb_connection_destroy(gcam->connection);
1216 		gcam->connection = NULL;
1217 	}
1218 	mutex_unlock(&gcam->mutex);
1219 }
1220 
gb_camera_release_module(struct kref * ref)1221 static void gb_camera_release_module(struct kref *ref)
1222 {
1223 	struct gb_camera_module *cam_mod =
1224 		container_of(ref, struct gb_camera_module, refcount);
1225 	kfree(cam_mod->priv);
1226 }
1227 
gb_camera_probe(struct gb_bundle * bundle,const struct greybus_bundle_id * id)1228 static int gb_camera_probe(struct gb_bundle *bundle,
1229 			   const struct greybus_bundle_id *id)
1230 {
1231 	struct gb_connection *conn;
1232 	struct gb_camera *gcam;
1233 	u16 mgmt_cport_id = 0;
1234 	u16 data_cport_id = 0;
1235 	unsigned int i;
1236 	int ret;
1237 
1238 	/*
1239 	 * The camera bundle must contain exactly two CPorts, one for the
1240 	 * camera management protocol and one for the camera data protocol.
1241 	 */
1242 	if (bundle->num_cports != 2)
1243 		return -ENODEV;
1244 
1245 	for (i = 0; i < bundle->num_cports; ++i) {
1246 		struct greybus_descriptor_cport *desc = &bundle->cport_desc[i];
1247 
1248 		switch (desc->protocol_id) {
1249 		case GREYBUS_PROTOCOL_CAMERA_MGMT:
1250 			mgmt_cport_id = le16_to_cpu(desc->id);
1251 			break;
1252 		case GREYBUS_PROTOCOL_CAMERA_DATA:
1253 			data_cport_id = le16_to_cpu(desc->id);
1254 			break;
1255 		default:
1256 			return -ENODEV;
1257 		}
1258 	}
1259 
1260 	if (!mgmt_cport_id || !data_cport_id)
1261 		return -ENODEV;
1262 
1263 	gcam = kzalloc(sizeof(*gcam), GFP_KERNEL);
1264 	if (!gcam)
1265 		return -ENOMEM;
1266 
1267 	mutex_init(&gcam->mutex);
1268 
1269 	gcam->bundle = bundle;
1270 	gcam->state = GB_CAMERA_STATE_UNCONFIGURED;
1271 	gcam->data_cport_id = data_cport_id;
1272 
1273 	conn = gb_connection_create(bundle, mgmt_cport_id,
1274 				    gb_camera_request_handler);
1275 	if (IS_ERR(conn)) {
1276 		ret = PTR_ERR(conn);
1277 		goto error;
1278 	}
1279 
1280 	gcam->connection = conn;
1281 	gb_connection_set_data(conn, gcam);
1282 
1283 	ret = gb_connection_enable(conn);
1284 	if (ret)
1285 		goto error;
1286 
1287 	ret = gb_camera_debugfs_init(gcam);
1288 	if (ret < 0)
1289 		goto error;
1290 
1291 	gcam->module.priv = gcam;
1292 	gcam->module.ops = &gb_cam_ops;
1293 	gcam->module.interface_id = gcam->connection->intf->interface_id;
1294 	gcam->module.release = gb_camera_release_module;
1295 	ret = gb_camera_register(&gcam->module);
1296 	if (ret < 0)
1297 		goto error;
1298 
1299 	greybus_set_drvdata(bundle, gcam);
1300 
1301 	gb_pm_runtime_put_autosuspend(gcam->bundle);
1302 
1303 	return 0;
1304 
1305 error:
1306 	gb_camera_cleanup(gcam);
1307 	kfree(gcam);
1308 	return ret;
1309 }
1310 
gb_camera_disconnect(struct gb_bundle * bundle)1311 static void gb_camera_disconnect(struct gb_bundle *bundle)
1312 {
1313 	struct gb_camera *gcam = greybus_get_drvdata(bundle);
1314 	int ret;
1315 
1316 	ret = gb_pm_runtime_get_sync(bundle);
1317 	if (ret)
1318 		gb_pm_runtime_get_noresume(bundle);
1319 
1320 	gb_camera_cleanup(gcam);
1321 	gb_camera_unregister(&gcam->module);
1322 }
1323 
1324 static const struct greybus_bundle_id gb_camera_id_table[] = {
1325 	{ GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_CAMERA) },
1326 	{ },
1327 };
1328 
1329 #ifdef CONFIG_PM
gb_camera_suspend(struct device * dev)1330 static int gb_camera_suspend(struct device *dev)
1331 {
1332 	struct gb_bundle *bundle = to_gb_bundle(dev);
1333 	struct gb_camera *gcam = greybus_get_drvdata(bundle);
1334 
1335 	if (gcam->data_connection)
1336 		gb_connection_disable(gcam->data_connection);
1337 
1338 	gb_connection_disable(gcam->connection);
1339 
1340 	return 0;
1341 }
1342 
gb_camera_resume(struct device * dev)1343 static int gb_camera_resume(struct device *dev)
1344 {
1345 	struct gb_bundle *bundle = to_gb_bundle(dev);
1346 	struct gb_camera *gcam = greybus_get_drvdata(bundle);
1347 	int ret;
1348 
1349 	ret = gb_connection_enable(gcam->connection);
1350 	if (ret) {
1351 		gcam_err(gcam, "failed to enable connection: %d\n", ret);
1352 		return ret;
1353 	}
1354 
1355 	if (gcam->data_connection) {
1356 		ret = gb_connection_enable(gcam->data_connection);
1357 		if (ret) {
1358 			gcam_err(gcam,
1359 				 "failed to enable data connection: %d\n", ret);
1360 			return ret;
1361 		}
1362 	}
1363 
1364 	return 0;
1365 }
1366 #endif
1367 
1368 static const struct dev_pm_ops gb_camera_pm_ops = {
1369 	SET_RUNTIME_PM_OPS(gb_camera_suspend, gb_camera_resume, NULL)
1370 };
1371 
1372 static struct greybus_driver gb_camera_driver = {
1373 	.name		= "camera",
1374 	.probe		= gb_camera_probe,
1375 	.disconnect	= gb_camera_disconnect,
1376 	.id_table	= gb_camera_id_table,
1377 	.driver.pm	= &gb_camera_pm_ops,
1378 };
1379 
1380 module_greybus_driver(gb_camera_driver);
1381 
1382 MODULE_LICENSE("GPL v2");
1383