NXP RT1064 CSI implementation of OV5640

[mariopaja]

Hi everyone,

We have a custom board & a custom module of ov5640 from an old project and I am trying to test the tcpserversink sample.

Additionally I have tried also to implement it with mimxrt1064_evk for easier testing from others too. For this case I am using the Adafruit OV5640 Camera Breakout - 120 Degree Lens with Autofocus with the mimxrt1064_evk

I noticed that the code gets stuck at kernel/kheap.c: (void) z_pend_curr(&heap->lock, key, &heap->wait_q, timeout);

Sample: /video/tcpserversink

mimxrt1064_evk.overlay

#include <zephyr/dt-bindings/video/video-interfaces.h>

/ {
	chosen {
		zephyr,camera = &csi;
	};
};

&csi {
	pinctrl-0 = <&pinmux_csi>;
	pinctrl-names = "default";
	status = "okay";

	port {
		ep_in: endpoint {
			remote-endpoint-label = "ep_out";
            bus-width = <8>;
			hsync-active = <0>;
			vsync-active = <0>;
			pclk-sample = <1>;
		};
	};
};

csi_i2c: &lpi2c1 {
	status = "okay";
	pinctrl-0 = <&pinmux_lpi2c1>;
	pinctrl-names = "default";

	ov5640: ov5640@3c {
		compatible = "ovti,ov5640";
		reg = <0x3c>;
		status = "okay";

		port {
			ep_out: endpoint {
				remote-endpoint-label = "ep_in";
				bus-type = <VIDEO_BUS_TYPE_PARALLEL>;
			};
		};
	};
};

prj.conf

# Generic networking options
CONFIG_NETWORKING=y
CONFIG_NET_TCP=y
CONFIG_NET_IPV4=y
CONFIG_NET_SOCKETS=y
CONFIG_ZVFS_OPEN_MAX=6
CONFIG_POSIX_API=y

# Kernel options
CONFIG_MAIN_STACK_SIZE=2048
CONFIG_ENTROPY_GENERATOR=y
CONFIG_TEST_RANDOM_GENERATOR=y
CONFIG_INIT_STACKS=y

# Logging
CONFIG_NET_LOG=y
CONFIG_LOG=y
CONFIG_NET_STATISTICS=y
CONFIG_PRINTK=y

# Network buffers
CONFIG_NET_PKT_RX_COUNT=16
CONFIG_NET_PKT_TX_COUNT=64
CONFIG_NET_BUF_RX_COUNT=16
CONFIG_NET_BUF_TX_COUNT=512
CONFIG_NET_BUF_DATA_SIZE=1024

CONFIG_NET_CONTEXT_NET_PKT_POOL=y

# IP address options
CONFIG_NET_IF_UNICAST_IPV6_ADDR_COUNT=3
CONFIG_NET_IF_MCAST_IPV6_ADDR_COUNT=4
CONFIG_NET_MAX_CONTEXTS=10

# Network shell
CONFIG_NET_SHELL=y

# Network application options and configuration
CONFIG_NET_CONFIG_SETTINGS=y
CONFIG_NET_CONFIG_MY_IPV4_ADDR="10.0.0.134"

CONFIG_VIDEO=y
CONFIG_VIDEO_SW_GENERATOR=y

CONFIG_HEAP_MEM_POOL_SIZE=16384
CONFIG_THREAD_RUNTIME_STATS=y
CONFIG_SCHED_THREAD_USAGE=y
CONFIG_MAIN_STACK_SIZE=16000
CONFIG_IDLE_STACK_SIZE=2048
CONFIG_ISR_STACK_SIZE=8192

Output:

[00:00:02.972,000] <inf> phy_mc_ksz8081: PHY 0 is up
[00:00:02.972,000] <inf> phy_mc_ksz8081: PHY (0) Link speed 100 Mb, full duplex

[00:00:02.973,000] <inf> eth_nxp_enet_mac: Link is up
*** Booting Zephyr OS build v4.1.0-4810-gf432d5d510ae ***
[00:00:02.974,000] <inf> net_config: Initializing network
[00:00:02.974,000] <inf> net_config: IPv4 address: 10.0.0.134
Video device detected, format: RGBP 160x120

INFO:
drivers/video/ov5640.c
The following is removed from ov5640_init():

	if (!gpio_is_ready_dt(&cfg->reset_gpio)) {
		LOG_ERR("%s: device %s is not ready", dev->name, cfg->reset_gpio.port->name);
		return -ENODEV;
	}

	if (!gpio_is_ready_dt(&cfg->powerdown_gpio)) {
		LOG_ERR("%s: device %s is not ready", dev->name, cfg->powerdown_gpio.port->name);
		return -ENODEV;
	}

	/* Power up sequence */
	if (cfg->powerdown_gpio.port != NULL) {
		ret = gpio_pin_configure_dt(&cfg->powerdown_gpio, GPIO_OUTPUT_ACTIVE);
		if (ret) {
			return ret;
		}
	}

	if (cfg->reset_gpio.port != NULL) {
		ret = gpio_pin_configure_dt(&cfg->reset_gpio, GPIO_OUTPUT_ACTIVE);
		if (ret) {
			return ret;
		}
	}

	k_sleep(K_MSEC(5));

	if (cfg->powerdown_gpio.port != NULL) {
		gpio_pin_set_dt(&cfg->powerdown_gpio, 0);
	}

	k_sleep(K_MSEC(1));

	if (cfg->reset_gpio.port != NULL) {
		gpio_pin_set_dt(&cfg->reset_gpio, 0);
	}

[DerekSnell]

Hi @mariopaja ,

There is recent support of DVP with the OV5640, see #76124. We believe this should work using the parallel interface on the RT10xx. However, I am not aware of an example for this today.

You mentioned your board does not boot. I suspect it does boot, but the execution gets stuck in the startup code initializing the drivers, and never gets to main(). Can you debug and diagnose this startup on your board and provide more details?

Best regards

[mariopaja]

@DerekSnell I found the problem out, reset_gpio & powerdown_gpio were missing. I removed them from the init on the driver and it seems like the application boots to a certain point

[mariopaja]

@ngphibang i noticed on video related PR that you have been working on this sensor, do you have an idea about this issue?

[mariopaja]

@ngphibang i noticed on video related PR that you have been working on this sensor, do you have an idea about this issue?

[ngphibang]

Hi @mariopaja ,

We only have ov5640 working on RT1170-EVK in CSI mode and don't have RT1064 with ov5640 in DVP mode. So, with your custom boards and your very little given input, it is very hard to know where is the issue. Some of my ideas for debugging are below:

• Maybe better to start with the video/capture sample (without display) than the tcpserversink sample to simplify the problem ? The capture sample focuses on the camera only.
• Maybe adding logs into the sample's main.c, the csi and the ov5640 drivers to see where is the stucked point ? Is the CSI successfully initialized ? then the ov5640 is initialized ? then the application ? at which point it stucks ...
• The init orfer priority : the CSI driver is initialized before the sensor driver to give master clock to the sensor on RT1064. With your custom boards, is the ov5640 needs clock from the CSI ? If not, maybe you need to change the init order.
• Maybe you could post the schematic of your custom boards for the camera module here to give some more information ?

BTW, @CharlesDias has made the ov5640 working on ST boards in DVP mode, maybe he can help at some point ...

[CharlesDias]

Hi @mariopaja and @ngphibang,

Please, take a look at this shield configuration:

https://docs.zephyrproject.org/latest/boards/shields/st_b_cams_omv_mb1683/doc/index.html

[mariopaja]

Hi all,

I was eventually able to implement the camera, both tcpserversink and capture samples.

In the tcpserversink the issue was on the following part:
https://github.com/zephyrproject-rtos/zephyr/blob/e22ca6b132840efb23b4a00ff7718225a0dd9f29/samples/drivers/video/tcpserversink/src/main.c#L93C1-L106C3

	if (caps.min_line_count != LINE_COUNT_HEIGHT) {
		LOG_ERR("Partial framebuffers not supported by this sample");
		return 0;
	}

	/* Alloc Buffers */
	for (i = 0; i < ARRAY_SIZE(buffers); i++) {
		buffers[i] = video_buffer_alloc(fmt.pitch * fmt.height, K_FOREVER);
		if (buffers[i] == NULL) {
			LOG_ERR("Unable to alloc video buffer");
			return 0;
		}
		buffers[i]->type = type;
	}

after changing it to:

	/* Size to allocate for each buffer */
	if (caps.min_line_count == LINE_COUNT_HEIGHT) {
		bsize = fmt.pitch * fmt.height;
	} else {
		bsize = fmt.pitch * caps.min_line_count;
	}

	/* Alloc video buffers and enqueue for capture */
	for (i = 0; i < ARRAY_SIZE(buffers); i++) {
		/*
		 * For some hardwares, such as the PxP used on i.MX RT1170 to do image rotation,
		 * buffer alignment is needed in order to achieve the best performance
		 */
		buffers[i] = video_buffer_aligned_alloc(bsize, CONFIG_VIDEO_BUFFER_POOL_ALIGN,
							K_FOREVER);
		if (buffers[i] == NULL) {
			LOG_ERR("Unable to alloc video buffer");
			return 0;
		}
		buffers[i]->type = type;
	}

everything worked as intended

[ngphibang]

In the tcpserversink the issue was on the following part:
...
after changing it to:

I believe your change does not actually change the logic as the CSI is working on full frame mode so its
caps.min_line_count already set to LINE_COUNT_HEIGHT:

https://github.com/zephyrproject-rtos/zephyr/blob/main/drivers/video/video_mcux_csi.c#L327

I believe there are other changes that made it work, not the above change.

[mariopaja]

Maybe there was something else that I didnt notice but the main difference is

from (where the appliation got stuck before):
buffers[i] = video_buffer_alloc(fmt.pitch * fmt.height, K_FOREVER);
to:
buffers[i] = video_buffer_aligned_alloc(bsize, CONFIG_VIDEO_BUFFER_POOL_ALIGN, K_FOREVER);

I wrote the application based on capture sample adn then added the tcpserversink logic

The full application is:

#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/linker/sections.h>
#include <errno.h>
#include <stdio.h>

#include <zephyr/logging/log.h>

#include <zephyr/drivers/display.h>
#include <zephyr/drivers/video.h>
#include <zephyr/drivers/video-controls.h>

#include <zephyr/net/net_if.h>
#include <zephyr/net/net_core.h>
#include <zephyr/net/net_context.h>
#include <zephyr/net/net_mgmt.h>

#include <zephyr/net/socket.h>

LOG_MODULE_REGISTER(main, 4);

static ssize_t sendall(int sock, const void *buf, size_t len)
{
	while (len) {
		ssize_t out_len = send(sock, buf, len, 0);

		if (out_len < 0) {
			return out_len;
		}
		buf = (const char *)buf + out_len;
		len -= out_len;
	}

	return 0;
}

#if !DT_HAS_CHOSEN(zephyr_camera)
#error No camera chosen in devicetree. Missing "--shield" or "--snippet video-sw-generator" flag?
#endif

#if DT_HAS_CHOSEN(zephyr_display)
static inline int display_setup(const struct device *const display_dev, const uint32_t pixfmt)
{
	struct display_capabilities capabilities;
	int ret = 0;

	LOG_INF("Display device: %s", display_dev->name);

	display_get_capabilities(display_dev, &capabilities);

	LOG_INF("- Capabilities:");
	LOG_INF("  x_resolution = %u, y_resolution = %u, supported_pixel_formats = %u"
		"  current_pixel_format = %u, current_orientation = %u",
		capabilities.x_resolution, capabilities.y_resolution,
		capabilities.supported_pixel_formats, capabilities.current_pixel_format,
		capabilities.current_orientation);

	/* Set display pixel format to match the one in use by the camera */
	switch (pixfmt) {
	case VIDEO_PIX_FMT_RGB565:
		if (capabilities.current_pixel_format != PIXEL_FORMAT_BGR_565) {
			ret = display_set_pixel_format(display_dev, PIXEL_FORMAT_BGR_565);
		}
		break;
	case VIDEO_PIX_FMT_XRGB32:
		if (capabilities.current_pixel_format != PIXEL_FORMAT_ARGB_8888) {
			ret = display_set_pixel_format(display_dev, PIXEL_FORMAT_ARGB_8888);
		}
		break;
	default:
		return -ENOTSUP;
	}

	if (ret) {
		LOG_ERR("Unable to set display format");
		return ret;
	}

	/* Turn off blanking if driver supports it */
	ret = display_blanking_off(display_dev);
	if (ret == -ENOSYS) {
		LOG_DBG("Display blanking off not available");
		ret = 0;
	}

	return ret;
}

static inline void video_display_frame(const struct device *const display_dev,
				       const struct video_buffer *const vbuf,
				       const struct video_format fmt)
{
	struct display_buffer_descriptor buf_desc = {
		.buf_size = vbuf->bytesused,
		.width = fmt.width,
		.pitch = buf_desc.width,
		.height = vbuf->bytesused / fmt.pitch,
	};

	display_write(display_dev, 0, vbuf->line_offset, &buf_desc, vbuf->buffer);
}
#endif

static struct net_mgmt_event_callback mgmt_cb;

static void start_dhcpv4_client(struct net_if *iface, void *user_data)
{
	ARG_UNUSED(user_data);

	LOG_INF("Start on %s: index=%d", net_if_get_device(iface)->name,
		net_if_get_by_iface(iface));
	net_dhcpv4_start(iface);
}

static void handler(struct net_mgmt_event_callback *cb, uint64_t mgmt_event, struct net_if *iface)
{
	int i = 0;

	if (mgmt_event != NET_EVENT_IPV4_ADDR_ADD) {
		return;
	}

	for (i = 0; i < NET_IF_MAX_IPV4_ADDR; i++) {
		char buf[NET_IPV4_ADDR_LEN];

		if (iface->config.ip.ipv4->unicast[i].ipv4.addr_type != NET_ADDR_DHCP) {
			continue;
		}

		LOG_INF("Address[%d]: %s", net_if_get_by_iface(iface),
			net_addr_ntop(AF_INET,
				      &iface->config.ip.ipv4->unicast[i].ipv4.address.in_addr, buf,
				      sizeof(buf)));
		LOG_INF("Subnet[%d]: %s", net_if_get_by_iface(iface),
			net_addr_ntop(AF_INET, &iface->config.ip.ipv4->unicast[i].netmask, buf,
				      sizeof(buf)));
		LOG_INF("Router[%d]: %s", net_if_get_by_iface(iface),
			net_addr_ntop(AF_INET, &iface->config.ip.ipv4->gw, buf, sizeof(buf)));
		LOG_INF("Lease time[%d]: %u seconds", net_if_get_by_iface(iface),
			iface->config.dhcpv4.lease_time);
	}
}

int main(void)
{

	net_mgmt_init_event_callback(&mgmt_cb, handler, NET_EVENT_IPV4_ADDR_ADD);
	net_mgmt_add_event_callback(&mgmt_cb);
	net_if_foreach(start_dhcpv4_client, NULL);
	struct net_if *iface = net_if_get_default();
	/* Wait DHCPv4 IP */
	while (iface->config.dhcpv4.state != NET_DHCPV4_BOUND) {
		k_msleep(500);
	}

	struct video_buffer *buffers[CONFIG_VIDEO_BUFFER_POOL_NUM_MAX];
	struct video_buffer *vbuf = &(struct video_buffer){};
	const struct device *video_dev;
	struct video_format fmt;
	struct video_caps caps;
	struct video_frmival frmival;
	struct video_frmival_enum fie;
	enum video_buf_type type = VIDEO_BUF_TYPE_OUTPUT;
#if (CONFIG_VIDEO_SOURCE_CROP_WIDTH && CONFIG_VIDEO_SOURCE_CROP_HEIGHT) ||                         \
	CONFIG_VIDEO_FRAME_HEIGHT || CONFIG_VIDEO_FRAME_WIDTH
	struct video_selection sel = {
		.type = VIDEO_BUF_TYPE_OUTPUT,
	};
#endif
	unsigned int frame = 0;
	size_t bsize;
	int i = 0;
	int err;

	/* When the video shell is enabled, do not run the capture loop */
	if (IS_ENABLED(CONFIG_VIDEO_SHELL)) {
		LOG_INF("Letting the user control the device with the video shell");
		return 0;
	}

	video_dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_camera));
	if (!device_is_ready(video_dev)) {
		LOG_ERR("%s: video device is not ready", video_dev->name);
		return 0;
	}

	LOG_INF("Video device: %s", video_dev->name);

	/* Get capabilities */
	caps.type = type;
	if (video_get_caps(video_dev, &caps)) {
		LOG_ERR("Unable to retrieve video capabilities");
		return 0;
	}

	LOG_INF("- Capabilities:");
	while (caps.format_caps[i].pixelformat) {
		const struct video_format_cap *fcap = &caps.format_caps[i];
		/* fourcc to string */
		LOG_INF("  %s width [%u; %u; %u] height [%u; %u; %u]",
			VIDEO_FOURCC_TO_STR(fcap->pixelformat), fcap->width_min, fcap->width_max,
			fcap->width_step, fcap->height_min, fcap->height_max, fcap->height_step);
		i++;
	}

	/* Get default/native format */
	fmt.type = type;
	if (video_get_format(video_dev, &fmt)) {
		LOG_ERR("Unable to retrieve video format");
		return 0;
	}

	/* Set the crop setting if necessary */
#if CONFIG_VIDEO_SOURCE_CROP_WIDTH && CONFIG_VIDEO_SOURCE_CROP_HEIGHT
	sel.target = VIDEO_SEL_TGT_CROP;
	sel.rect.left = CONFIG_VIDEO_SOURCE_CROP_LEFT;
	sel.rect.top = CONFIG_VIDEO_SOURCE_CROP_TOP;
	sel.rect.width = CONFIG_VIDEO_SOURCE_CROP_WIDTH;
	sel.rect.height = CONFIG_VIDEO_SOURCE_CROP_HEIGHT;
	if (video_set_selection(video_dev, &sel)) {
		LOG_ERR("Unable to set selection crop");
		return 0;
	}
	LOG_INF("Selection crop set to (%u,%u)/%ux%u", sel.rect.left, sel.rect.top, sel.rect.width,
		sel.rect.height);
#endif

#if CONFIG_VIDEO_FRAME_HEIGHT || CONFIG_VIDEO_FRAME_WIDTH
#if CONFIG_VIDEO_FRAME_HEIGHT
	fmt.height = CONFIG_VIDEO_FRAME_HEIGHT;
#endif

#if CONFIG_VIDEO_FRAME_WIDTH
	fmt.width = CONFIG_VIDEO_FRAME_WIDTH;
#endif

	/*
	 * Check (if possible) if targeted size is same as crop
	 * and if compose is necessary
	 */
	sel.target = VIDEO_SEL_TGT_CROP;
	err = video_get_selection(video_dev, &sel);
	if (err < 0 && err != -ENOSYS) {
		LOG_ERR("Unable to get selection crop");
		return 0;
	}

	if (err == 0 && (sel.rect.width != fmt.width || sel.rect.height != fmt.height)) {
		sel.target = VIDEO_SEL_TGT_COMPOSE;
		sel.rect.left = 0;
		sel.rect.top = 0;
		sel.rect.width = fmt.width;
		sel.rect.height = fmt.height;
		err = video_set_selection(video_dev, &sel);
		if (err < 0 && err != -ENOSYS) {
			LOG_ERR("Unable to set selection compose");
			return 0;
		}
	}
#endif

	if (strcmp(CONFIG_VIDEO_PIXEL_FORMAT, "")) {
		fmt.pixelformat = VIDEO_FOURCC_FROM_STR(CONFIG_VIDEO_PIXEL_FORMAT);
	}

	LOG_INF("- Video format: %s %ux%u", VIDEO_FOURCC_TO_STR(fmt.pixelformat), fmt.width,
		fmt.height);

	if (video_set_format(video_dev, &fmt)) {
		LOG_ERR("Unable to set format");
		return 0;
	}

	if (!video_get_frmival(video_dev, &frmival)) {
		LOG_INF("- Default frame rate : %f fps",
			1.0 * frmival.denominator / frmival.numerator);
	}

	LOG_INF("- Supported frame intervals for the default format:");
	memset(&fie, 0, sizeof(fie));
	fie.format = &fmt;
	while (video_enum_frmival(video_dev, &fie) == 0) {
		if (fie.type == VIDEO_FRMIVAL_TYPE_DISCRETE) {
			LOG_INF("   %u/%u", fie.discrete.numerator, fie.discrete.denominator);
		} else {
			LOG_INF("   [min = %u/%u; max = %u/%u; step = %u/%u]",
				fie.stepwise.min.numerator, fie.stepwise.min.denominator,
				fie.stepwise.max.numerator, fie.stepwise.max.denominator,
				fie.stepwise.step.numerator, fie.stepwise.step.denominator);
		}
		fie.index++;
	}

	/* Get supported controls */
	LOG_INF("- Supported controls:");
	const struct device *last_dev = NULL;
	struct video_ctrl_query cq = {.dev = video_dev, .id = VIDEO_CTRL_FLAG_NEXT_CTRL};

	while (!video_query_ctrl(&cq)) {
		if (cq.dev != last_dev) {
			last_dev = cq.dev;
			LOG_INF("\t\tdevice: %s", cq.dev->name);
		}
		video_print_ctrl(&cq);
		cq.id |= VIDEO_CTRL_FLAG_NEXT_CTRL;
	}

	/* Set controls */
	struct video_control ctrl = {.id = VIDEO_CID_HFLIP, .val = 1};
	int tp_set_ret = -ENOTSUP;

	if (IS_ENABLED(CONFIG_VIDEO_CTRL_HFLIP)) {
		video_set_ctrl(video_dev, &ctrl);
	}

	if (IS_ENABLED(CONFIG_VIDEO_CTRL_VFLIP)) {
		ctrl.id = VIDEO_CID_VFLIP;
		video_set_ctrl(video_dev, &ctrl);
	}

	if (IS_ENABLED(CONFIG_TEST)) {
		ctrl.id = VIDEO_CID_TEST_PATTERN;
		tp_set_ret = video_set_ctrl(video_dev, &ctrl);
	}

#if DT_HAS_CHOSEN(zephyr_display)
	const struct device *const display_dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_display));

	if (!device_is_ready(display_dev)) {
		LOG_ERR("%s: display device not ready.", display_dev->name);
		return 0;
	}

	err = display_setup(display_dev, fmt.pixelformat);
	if (err) {
		LOG_ERR("Unable to set up display");
		return err;
	}
#endif

	/* Size to allocate for each buffer */
	if (caps.min_line_count == LINE_COUNT_HEIGHT) {
		bsize = fmt.pitch * fmt.height;
		LOG_DBG("bsize = fmt.pitch * fmt.height");
	} else {
		bsize = fmt.pitch * caps.min_line_count;
		LOG_DBG("bsize = fmt.pitch * caps.min_line_count");
	}

	/* Alloc video buffers and enqueue for capture */
	for (i = 0; i < ARRAY_SIZE(buffers); i++) {
		/*
		 * For some hardwares, such as the PxP used on i.MX RT1170 to do image rotation,
		 * buffer alignment is needed in order to achieve the best performance
		 */
		buffers[i] = video_buffer_aligned_alloc(bsize, CONFIG_VIDEO_BUFFER_POOL_ALIGN,
							K_FOREVER);
		if (buffers[i] == NULL) {
			LOG_ERR("Unable to alloc video buffer");
			return 0;
		}
		buffers[i]->type = type;
		// video_enqueue(video_dev, buffers[i]);
	}

	struct sockaddr_in addr, client_addr;
	socklen_t client_addr_len = sizeof(client_addr);
	int ret, sock, client;

#define MY_PORT          5000
#define MAX_CLIENT_QUEUE 1

	/* Prepare Network */
	(void)memset(&addr, 0, sizeof(addr));
	addr.sin_family = AF_INET;
	addr.sin_port = htons(MY_PORT);
	addr.sin_addr.s_addr = htonl(INADDR_ANY);

	sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (sock < 0) {
		LOG_ERR("Failed to create TCP socket: %d", errno);
		return 0;
	}

	ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr));
	if (ret < 0) {
		LOG_ERR("Failed to bind TCP socket: %d", errno);
		close(sock);
		return 0;
	}

	ret = listen(sock, MAX_CLIENT_QUEUE);
	if (ret < 0) {
		LOG_ERR("Failed to listen on TCP socket: %d", errno);
		close(sock);
		return 0;
	}

	do {
		LOG_DBG("TCP: Waiting for client...");
		client = accept(sock, (struct sockaddr *)&client_addr, &client_addr_len);
		if (client < 0) {
			LOG_ERR("Failed to accept: %d", errno);
		}
		LOG_DBG("TCP: Accepted client %d", client);

		for (i = 0; i < ARRAY_SIZE(buffers); i++) {
			video_enqueue(video_dev, buffers[i]);
		}

		/* Start video capture */
		if (video_stream_start(video_dev, type)) {
			LOG_ERR("Unable to start video");
			return 0;
		}

		LOG_DBG("TCP: Stream started");

		i = 0;
		vbuf->type = type;

		do {
			ret = video_dequeue(video_dev, &vbuf, K_FOREVER);
			if (ret) {
				LOG_ERR("Unable to dequeue video buf");
				return 0;
			}

			LOG_DBG("\rSending frame %d", i++);

			/* Send video buffer to TCP client */
			ret = sendall(client, vbuf->buffer, vbuf->bytesused);
			if (ret && ret != -EAGAIN) {
				/* client disconnected */
				printk("\nTCP: Client disconnected %d\n", ret);
				close(client);
			}

			(void)video_enqueue(video_dev, vbuf);

		} while (!ret);

		/* stop capture */
		if (video_stream_stop(video_dev, type)) {
			LOG_ERR("Unable to stop video");
			return 0;
		}

		/* Flush remaining buffers */
		do {
			ret = video_dequeue(video_dev, &vbuf, K_NO_WAIT);
		} while (!ret);

	} while (true);

}