diff --git a/zero/meta-csp/conf/layer.conf b/zero/meta-csp/conf/layer.conf
index cd86548..9b6cfa4 100644
--- a/zero/meta-csp/conf/layer.conf
+++ b/zero/meta-csp/conf/layer.conf
@@ -14,7 +14,7 @@ BBFILE_PRIORITY_csp = "6"
 LAYERDEPENDS_csp = "libcsp"
 LAYERSERIES_COMPAT_csp = "kirkstone"
 
-IMAGE_INSTALL:append = " csp-handler cspd csp-server-client"
+IMAGE_INSTALL:append = " csp-handler cspd csp-server-client v4l-utils"
 
 ENABLE_UART = "1"
 ENABLE_I2C = "1"
diff --git a/zero/meta-csp/recipes-cspd/csp-handler/files/camera.c b/zero/meta-csp/recipes-cspd/csp-handler/files/camera.c
index 79f4df8..cf56ea4 100644
--- a/zero/meta-csp/recipes-cspd/csp-handler/files/camera.c
+++ b/zero/meta-csp/recipes-cspd/csp-handler/files/camera.c
@@ -10,11 +10,140 @@
 #include <sys/stat.h>
 #include <dirent.h>
 #include <systemd/sd-journal.h>
+#include <linux/videodev2.h>
+#include <sys/stat.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <unistd.h>
 #include "cspd.h"
 #include "utils.h"
 
-#define CAM_FRAME_PATH   "/storageA/photo"
-#define CAM_FRAME_PREFIX "frame"
+#define CAMERA_WIDTH  1920
+#define CAMERA_HEIGHT 1080
+
+#define CAM_FRAME_PATH      "/storageA/photo"
+#define CAM_FRAME_PREFIX    "frame"
+#define CAM_FRAME_EXTENSION "sgrbg10"
+#define CAM_PATH            "/dev/video0"
+#define MAX_FILENAME_LENGTH 256
+
+static void get_next_filename(char *filename, size_t size, const char *extension)
+{
+	unsigned max_num = 0;
+	struct dirent *entry;
+	DIR *dp = opendir(CAM_FRAME_PATH);
+	if (!dp) {
+		perror("opendir");
+		return;
+	}
+
+	while ((entry = readdir(dp)) != NULL) {
+		unsigned num;
+		if (sscanf(entry->d_name, CAM_FRAME_PREFIX "-%u.%*s", &num) == 1) {
+			if (num > max_num) {
+				max_num = num;
+			}
+		}
+	}
+	closedir(dp);
+
+	snprintf(filename, size, "%s/" CAM_FRAME_PREFIX "-%03u.%s", CAM_FRAME_PATH, max_num + 1,
+		 extension);
+}
+
+static void save_raw_file(uint8_t *buffer, int length)
+{
+	char filename[MAX_FILENAME_LENGTH];
+	get_next_filename(filename, sizeof(filename), CAM_FRAME_EXTENSION);
+	FILE *fp = fopen(filename, "wb");
+	if (fp) {
+		fwrite(buffer, 1, length, fp);
+		fclose(fp);
+	}
+}
+
+static int capture_raw_frame(const char *camera_dev)
+{
+	int fd = open(camera_dev, O_RDWR);
+	if (fd < 0) {
+		perror("open");
+		return EXIT_FAILURE;
+	}
+
+	struct v4l2_format fmt;
+	memset(&fmt, 0, sizeof(fmt));
+	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	fmt.fmt.pix.width = CAMERA_WIDTH;
+	fmt.fmt.pix.height = CAMERA_HEIGHT;
+	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SGRBG10;
+	fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
+
+	if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
+		perror("VIDIOC_S_FMT");
+		close(fd);
+		return EXIT_FAILURE;
+	}
+
+	struct v4l2_buffer buf;
+	struct v4l2_requestbuffers req;
+	uint8_t *buffer;
+
+	memset(&req, 0, sizeof(req));
+	req.count = 1;
+	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	req.memory = V4L2_MEMORY_MMAP;
+
+	if (ioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
+		perror("VIDIOC_REQBUFS");
+		exit(EXIT_FAILURE);
+	}
+
+	memset(&buf, 0, sizeof(buf));
+	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	buf.memory = V4L2_MEMORY_MMAP;
+	buf.index = 0;
+
+	if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
+		perror("VIDIOC_QUERYBUF");
+		exit(EXIT_FAILURE);
+	}
+
+	buffer = (uint8_t *)mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd,
+				 buf.m.offset);
+	if (buffer == MAP_FAILED) {
+		perror("mmap");
+		exit(EXIT_FAILURE);
+	}
+
+	if (ioctl(fd, VIDIOC_STREAMON, &buf.type) < 0) {
+		perror("VIDIOC_STREAMON");
+		exit(EXIT_FAILURE);
+	}
+
+	if (ioctl(fd, VIDIOC_QBUF, &buf) < 0) {
+		perror("VIDIOC_QBUF");
+		exit(EXIT_FAILURE);
+	}
+
+	if (ioctl(fd, VIDIOC_DQBUF, &buf) < 0) {
+		perror("VIDIOC_DQBUF");
+		exit(EXIT_FAILURE);
+	}
+
+	save_raw_file(buffer, buf.bytesused);
+
+	munmap(buffer, buf.length);
+	ioctl(fd, VIDIOC_STREAMOFF, &buf.type);
+
+	close(fd);
+	return EXIT_SUCCESS;
+}
 
 static int init_photo_dir(void)
 {
@@ -48,22 +177,7 @@ static int init_photo_dir(void)
 
 static int capture_frame(void)
 {
-	static uint16_t seq = 1;
-	char fname[128];
-	char cam_cmd[256];
-	int ret;
-
-	snprintf(fname, sizeof(fname), "%s/frame-%03d.bin", CAM_FRAME_PATH, seq);
-	snprintf(cam_cmd, sizeof(cam_cmd), "cam -c 1 --capture=1 --file=%s &",
-		 fname);
-
-	ret = system(cam_cmd);
-
-	if (ret == 0) {
-		seq++;
-	}
-
-	return ret;
+	return capture_raw_frame(CAM_PATH);
 }
 
 static int get_frame_file_count(uint16_t *count)
diff --git a/zero/meta-csp/recipes-cspd/systemd/files/cspd.service b/zero/meta-csp/recipes-cspd/systemd/files/cspd.service
index 93088ef..76a81ce 100644
--- a/zero/meta-csp/recipes-cspd/systemd/files/cspd.service
+++ b/zero/meta-csp/recipes-cspd/systemd/files/cspd.service
@@ -2,7 +2,7 @@
 Description=CSP Routing and execute Camera
 
 [Service]
-ExecStart=/usr/bin/cspd
+ExecStart=/usr/bin/libcamerify /usr/bin/cspd
 
 [Install]
 WantedBy=multi-user.target