Skip to content

Commit 6d73d06

Browse files
RayyanAnsariRayyanAnsari
committed
cdba: add image capture support
Add support for capturing an image from the server and saving it on the client with a new command, ^A i. Signed-off-by: Rayyan Ansari <[email protected]>
1 parent 2f3e661 commit 6d73d06

File tree

9 files changed

+399
-6
lines changed

9 files changed

+399
-6
lines changed

‎camera.c‎

Lines changed: 298 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,298 @@
1+
/*
2+
* Copyright (c) 2024, Linaro Ltd.
3+
* All rights reserved.
4+
*
5+
* SPDX-License-Identifier: BSD-3-Clause
6+
*/
7+
#include <errno.h>
8+
#include <fcntl.h>
9+
#include <stdint.h>
10+
#include <stdio.h>
11+
#include <stdlib.h>
12+
#include <string.h>
13+
#include <sys/ioctl.h>
14+
#include <sys/mman.h>
15+
#include <unistd.h>
16+
17+
#include <linux/videodev2.h>
18+
19+
#include <jpeglib.h>
20+
21+
#include "camera.h"
22+
#include "device.h"
23+
24+
int init_device(const char *dev_name, int *fd_ref, char **driver_ref)
25+
{
26+
*fd_ref = open(dev_name, O_RDWR);
27+
if (*fd_ref < 0)
28+
{
29+
fprintf(stderr, "Couldn't open device: %s (%d)\n", strerror(errno), errno);
30+
return -1;
31+
}
32+
33+
struct v4l2_capability cap;
34+
35+
if (ioctl(*fd_ref, VIDIOC_QUERYCAP, &cap) < 0)
36+
{
37+
fprintf(stderr, "Couldn't query device capabilities: %s (%d)\n", strerror(errno), errno);
38+
return -1;
39+
}
40+
41+
*driver_ref = strdup((char *)cap.driver);
42+
43+
return 0;
44+
}
45+
46+
int configure_format(const int fd, int *width_ref, int *height_ref, uint32_t *format_ref)
47+
{
48+
struct v4l2_format fmt = {0};
49+
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
50+
51+
if (ioctl(fd, VIDIOC_G_FMT, &fmt) < 0)
52+
{
53+
fprintf(stderr, "Couldn't get format: %s (%d)\n", strerror(errno), errno);
54+
return -1;
55+
}
56+
57+
struct v4l2_frmsizeenum frame_size = {0};
58+
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
59+
frame_size.pixel_format = fmt.fmt.pix.pixelformat;
60+
61+
if (ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frame_size) < 0)
62+
{
63+
fprintf(stderr, "Couldn't get frame size: %s (%d)\n", strerror(errno), errno);
64+
return -1;
65+
}
66+
67+
fmt.fmt.pix.width = frame_size.discrete.width;
68+
fmt.fmt.pix.height = frame_size.discrete.height;
69+
70+
if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0)
71+
{
72+
fprintf(stderr, "Couldn't set format: %s (%d)\n", strerror(errno), errno);
73+
return -1;
74+
}
75+
76+
*width_ref = fmt.fmt.pix.width;
77+
*height_ref = fmt.fmt.pix.height;
78+
*format_ref = fmt.fmt.pix.pixelformat;
79+
80+
return 0;
81+
}
82+
83+
int request_buffer(const int fd)
84+
{
85+
struct v4l2_requestbuffers reqbuf = {0};
86+
reqbuf.count = 1;
87+
reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
88+
reqbuf.memory = V4L2_MEMORY_MMAP;
89+
90+
if (ioctl(fd, VIDIOC_REQBUFS, &reqbuf) < 0)
91+
{
92+
fprintf(stderr, "Couldn't request buffer: %s (%d)\n", strerror(errno), errno);
93+
return -1;
94+
}
95+
96+
return 0;
97+
}
98+
99+
int query_buffer(const int fd, uint8_t **buf_ref, struct v4l2_buffer *buf_info_ref, int *size_ref)
100+
{
101+
buf_info_ref->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
102+
buf_info_ref->memory = V4L2_MEMORY_MMAP;
103+
buf_info_ref->index = 0;
104+
105+
if (ioctl(fd, VIDIOC_QUERYBUF, buf_info_ref) < 0)
106+
{
107+
fprintf(stderr, "Couldn't query buffer: %s (%d)\n", strerror(errno), errno);
108+
return -1;
109+
}
110+
111+
*buf_ref = mmap(NULL, buf_info_ref->length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf_info_ref->m.offset);
112+
if (buf_ref == MAP_FAILED)
113+
{
114+
fprintf(stderr, "Couldn't map buffer: %s (%d)\n", strerror(errno), errno);
115+
return -1;
116+
}
117+
118+
*size_ref = buf_info_ref->length;
119+
120+
return 0;
121+
}
122+
123+
int capture_frame(const int fd, struct v4l2_buffer *buf_info)
124+
{
125+
if (ioctl(fd, VIDIOC_QBUF, buf_info))
126+
{
127+
fprintf(stderr, "Couldn't queue buffer: %s (%d)\n", strerror(errno), errno);
128+
return -1;
129+
}
130+
enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
131+
if (ioctl(fd, VIDIOC_STREAMON, &type) < 0)
132+
{
133+
fprintf(stderr, "Couldn't start streaming: %s (%d)\n", strerror(errno), errno);
134+
return -1;
135+
}
136+
if (ioctl(fd, VIDIOC_DQBUF, buf_info) < 0)
137+
{
138+
fprintf(stderr, "Couldn't dequeue buffer: %s (%d)\n", strerror(errno), errno);
139+
return -1;
140+
}
141+
if (ioctl(fd, VIDIOC_STREAMOFF, &type) < 0)
142+
{
143+
fprintf(stderr, "Couldn't stop streaming: %s (%d)\n", strerror(errno), errno);
144+
return -1;
145+
}
146+
147+
return 0;
148+
}
149+
150+
void yuyv_to_rgb(uint8_t *rgb_ref, const uint8_t *yuyv, const int width, const int height)
151+
{
152+
int frame_size = width * height * 2;
153+
154+
for (int i = 0, j = 0; i < frame_size; i += 4, j += 6)
155+
{
156+
int Y0 = yuyv[i];
157+
int U = yuyv[i + 1] - 128;
158+
int Y1 = yuyv[i + 2];
159+
int V = yuyv[i + 3] - 128;
160+
161+
int R1 = Y0 + 1.140 * V;
162+
int G1 = Y0 - 0.395 * U - 0.581 * V;
163+
int B1 = Y0 + 2.032 * U;
164+
165+
int R2 = Y1 + 1.140 * V;
166+
int G2 = Y1 - 0.395 * U - 0.581 * V;
167+
int B2 = Y1 + 2.032 * U;
168+
169+
rgb_ref[j + 0] = (uint8_t)(R1 < 0 ? 0 : (R1 > 255 ? 255 : R1));
170+
rgb_ref[j + 1] = (uint8_t)(G1 < 0 ? 0 : (G1 > 255 ? 255 : G1));
171+
rgb_ref[j + 2] = (uint8_t)(B1 < 0 ? 0 : (B1 > 255 ? 255 : B1));
172+
rgb_ref[j + 3] = (uint8_t)(R2 < 0 ? 0 : (R2 > 255 ? 255 : R2));
173+
rgb_ref[j + 4] = (uint8_t)(G2 < 0 ? 0 : (G2 > 255 ? 255 : G2));
174+
rgb_ref[j + 5] = (uint8_t)(B2 < 0 ? 0 : (B2 > 255 ? 255 : B2));
175+
}
176+
}
177+
178+
void rgb_to_jpeg(uint8_t **jpeg_ref, unsigned long *size_ref, const uint8_t *rgb, const int width, const int height)
179+
{
180+
struct jpeg_compress_struct cinfo;
181+
struct jpeg_error_mgr jerr;
182+
183+
JSAMPROW row_pointer[1];
184+
int row_stride;
185+
186+
cinfo.err = jpeg_std_error(&jerr);
187+
jpeg_create_compress(&cinfo);
188+
189+
jpeg_mem_dest(&cinfo, jpeg_ref, size_ref);
190+
191+
cinfo.image_width = width;
192+
cinfo.image_height = height;
193+
cinfo.input_components = 3;
194+
cinfo.in_color_space = JCS_RGB;
195+
196+
jpeg_set_defaults(&cinfo);
197+
jpeg_set_quality(&cinfo, 75, TRUE);
198+
199+
jpeg_start_compress(&cinfo, TRUE);
200+
201+
row_stride = width * 3;
202+
203+
while (cinfo.next_scanline < cinfo.image_height)
204+
{
205+
row_pointer[0] = (JSAMPROW)&rgb[cinfo.next_scanline * row_stride];
206+
jpeg_write_scanlines(&cinfo, row_pointer, 1);
207+
}
208+
209+
jpeg_finish_compress(&cinfo);
210+
jpeg_destroy_compress(&cinfo);
211+
}
212+
213+
int camera_capture_jpeg(uint8_t **buffer_ref, unsigned long *size_ref, const char *video_device)
214+
{
215+
int fd = -1;
216+
uint8_t *raw_buffer = NULL;
217+
int raw_size = 0;
218+
219+
void cleanup()
220+
{
221+
if (fd >= 0)
222+
{
223+
close(fd);
224+
}
225+
if (raw_buffer != NULL)
226+
{
227+
munmap(raw_buffer, raw_size);
228+
}
229+
}
230+
231+
int ret = 0;
232+
char *driver_str;
233+
234+
fprintf(stderr, "Opening device: %s\n", video_device);
235+
ret = init_device(video_device, &fd, &driver_str);
236+
if (ret < 0)
237+
{
238+
cleanup();
239+
return -1;
240+
}
241+
242+
int width, height;
243+
uint32_t format;
244+
ret = configure_format(fd, &width, &height, &format);
245+
if (ret < 0)
246+
{
247+
cleanup();
248+
return -1;
249+
}
250+
251+
fprintf(stderr, "Driver: %s, Resolution: %dx%d, Format: %c%c%c%c\n", driver_str, width, height,
252+
(char)((format >> 0) & 0xFF), (char)((format >> 8) & 0xFF), (char)((format >> 16) & 0xFF), (char)((format >> 24) & 0xFF));
253+
free(driver_str);
254+
255+
ret = request_buffer(fd);
256+
if (ret < 0)
257+
{
258+
cleanup();
259+
return -1;
260+
}
261+
262+
struct v4l2_buffer buf_info = {0};
263+
264+
ret = query_buffer(fd, &raw_buffer, &buf_info, &raw_size);
265+
if (ret < 0)
266+
{
267+
cleanup();
268+
return -1;
269+
}
270+
271+
capture_frame(fd, &buf_info);
272+
273+
uint8_t *jpeg = NULL;
274+
switch (format)
275+
{
276+
case V4L2_PIX_FMT_YUYV:
277+
{
278+
uint8_t *rgb = (uint8_t *)malloc(width * height * 3);
279+
280+
yuyv_to_rgb(rgb, raw_buffer, width, height);
281+
rgb_to_jpeg(&jpeg, size_ref, rgb, width, height);
282+
283+
free(rgb);
284+
break;
285+
}
286+
287+
default:
288+
fprintf(stderr, "Unsupported format\n");
289+
cleanup();
290+
return -1;
291+
}
292+
293+
*buffer_ref = jpeg;
294+
295+
cleanup();
296+
297+
return 0;
298+
}

‎camera.h‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
/*
2+
* Copyright (c) 2024, Linaro Ltd.
3+
* All rights reserved.
4+
*
5+
* SPDX-License-Identifier: BSD-3-Clause
6+
*/
7+
#ifndef __CAMERA_H__
8+
#define __CAMERA_H__
9+
10+
#include <stdint.h>
11+
12+
int camera_capture_jpeg(uint8_t **buffer_ref, unsigned long *size_ref, const char *video_device);
13+
14+
#endif // __CAMERA_H__

‎cdba-server.c‎

Lines changed: 27 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
#include <unistd.h>
1515
#include <syslog.h>
1616

17+
#include "camera.h"
1718
#include "cdba-server.h"
1819
#include "circ_buf.h"
1920
#include "device.h"
@@ -99,6 +100,28 @@ static void msg_fastboot_continue(void)
99100
cdba_send(MSG_FASTBOOT_CONTINUE);
100101
}
101102

103+
static void capture_image(struct device *device)
104+
{
105+
uint8_t *jpeg = NULL;
106+
size_t size = 0;
107+
108+
if (!device->video_device)
109+
{
110+
fprintf(stderr, "video_device not specified!\n");
111+
return;
112+
}
113+
114+
int ret = camera_capture_jpeg(&jpeg, &size, device->video_device);
115+
if (ret < 0)
116+
{
117+
fprintf(stderr, "Failed to capture image\n");
118+
return;
119+
}
120+
121+
cdba_send_buf(MSG_CAPTURE_IMAGE, size, jpeg);
122+
free(jpeg);
123+
}
124+
102125
void cdba_send_buf(int type, size_t len, const void *buf)
103126
{
104127
struct msg msg = {
@@ -185,8 +208,11 @@ static int handle_stdin(int fd, void *buf)
185208
case MSG_FASTBOOT_CONTINUE:
186209
msg_fastboot_continue();
187210
break;
211+
case MSG_CAPTURE_IMAGE:
212+
capture_image(selected_device);
213+
break;
188214
default:
189-
fprintf(stderr, "unk %d len %d\n", msg->type, msg->len);
215+
fprintf(stderr, "unk %d len %u\n", msg->type, msg->len);
190216
exit(1);
191217
}
192218

0 commit comments

Comments
 (0)