Skip to content

Commit 00b5fc2

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 00b5fc2

9 files changed

+406
-1
lines changed

‎camera.c

Lines changed: 272 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,272 @@
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+
static int camera_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+
static int camera_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+
static int camera_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+
static int camera_query_buffer(const int fd, uint8_t **buf_ref, struct v4l2_buffer *buf_info_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+
return 0;
119+
}
120+
121+
static int camera_capture_frame(const int fd, struct v4l2_buffer *buf_info)
122+
{
123+
if (ioctl(fd, VIDIOC_QBUF, buf_info))
124+
{
125+
fprintf(stderr, "Couldn't queue buffer: %s (%d)\n", strerror(errno), errno);
126+
return -1;
127+
}
128+
enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
129+
if (ioctl(fd, VIDIOC_STREAMON, &type) < 0)
130+
{
131+
fprintf(stderr, "Couldn't start streaming: %s (%d)\n", strerror(errno), errno);
132+
return -1;
133+
}
134+
if (ioctl(fd, VIDIOC_DQBUF, buf_info) < 0)
135+
{
136+
fprintf(stderr, "Couldn't dequeue buffer: %s (%d)\n", strerror(errno), errno);
137+
return -1;
138+
}
139+
if (ioctl(fd, VIDIOC_STREAMOFF, &type) < 0)
140+
{
141+
fprintf(stderr, "Couldn't stop streaming: %s (%d)\n", strerror(errno), errno);
142+
return -1;
143+
}
144+
145+
return 0;
146+
}
147+
148+
static int camera_yuyv_to_jpeg(uint8_t **jpeg_ref, unsigned long *size_ref, const uint8_t *yuyv, const int width, const int height)
149+
{
150+
struct jpeg_compress_struct cinfo;
151+
struct jpeg_error_mgr jerr;
152+
153+
JSAMPROW row_pointer[1];
154+
155+
cinfo.err = jpeg_std_error(&jerr);
156+
jpeg_create_compress(&cinfo);
157+
158+
jpeg_mem_dest(&cinfo, jpeg_ref, size_ref);
159+
160+
cinfo.image_width = width;
161+
cinfo.image_height = height;
162+
cinfo.input_components = 3;
163+
cinfo.in_color_space = JCS_YCbCr;
164+
165+
jpeg_set_defaults(&cinfo);
166+
jpeg_set_quality(&cinfo, 100, TRUE);
167+
168+
jpeg_start_compress(&cinfo, TRUE);
169+
170+
uint8_t* row_buffer = (uint8_t*)malloc(width * 3 * sizeof(uint8_t));
171+
if (!row_buffer)
172+
{
173+
fprintf(stderr, "Memory allocation failed\n");
174+
jpeg_destroy_compress(&cinfo);
175+
return -1;
176+
}
177+
178+
while (cinfo.next_scanline < height)
179+
{
180+
const uint32_t offset = cinfo.next_scanline * width * 2;
181+
for (uint32_t i = 0, j = 0; i < width * 2; i += 4, j += 6)
182+
{
183+
row_buffer[j + 0] = yuyv[offset + i + 0];
184+
row_buffer[j + 1] = yuyv[offset + i + 1];
185+
row_buffer[j + 2] = yuyv[offset + i + 3];
186+
row_buffer[j + 3] = yuyv[offset + i + 2];
187+
row_buffer[j + 4] = yuyv[offset + i + 1];
188+
row_buffer[j + 5] = yuyv[offset + i + 3];
189+
}
190+
row_pointer[0] = row_buffer;
191+
jpeg_write_scanlines(&cinfo, row_pointer, 1);
192+
}
193+
194+
jpeg_finish_compress(&cinfo);
195+
jpeg_destroy_compress(&cinfo);
196+
197+
free(row_buffer);
198+
199+
return 0;
200+
}
201+
202+
int camera_capture_jpeg(uint8_t **buffer_ref, unsigned long *size_ref, const char *video_device)
203+
{
204+
int fd = -1;
205+
uint8_t *raw_buffer = NULL;
206+
207+
int ret = 0;
208+
char *driver_str = NULL;
209+
210+
int width, height = 0;
211+
uint32_t format = 0;
212+
213+
struct v4l2_buffer buf_info = {0};
214+
215+
uint8_t *jpeg = NULL;
216+
217+
ret = camera_init_device(video_device, &fd, &driver_str);
218+
if (ret < 0)
219+
{
220+
goto cleanup;
221+
}
222+
223+
ret = camera_configure_format(fd, &width, &height, &format);
224+
if (ret < 0)
225+
{
226+
goto cleanup;
227+
}
228+
229+
fprintf(stderr, "Driver: %s, Resolution: %dx%d, Format: %c%c%c%c\n", driver_str, width, height,
230+
(char)((format >> 0) & 0xFF), (char)((format >> 8) & 0xFF), (char)((format >> 16) & 0xFF), (char)((format >> 24) & 0xFF));
231+
free(driver_str);
232+
233+
ret = camera_request_buffer(fd);
234+
if (ret < 0)
235+
{
236+
goto cleanup;
237+
}
238+
239+
ret = camera_query_buffer(fd, &raw_buffer, &buf_info);
240+
if (ret < 0)
241+
{
242+
goto cleanup;
243+
}
244+
245+
camera_capture_frame(fd, &buf_info);
246+
247+
switch (format)
248+
{
249+
case V4L2_PIX_FMT_YUYV:
250+
camera_yuyv_to_jpeg(&jpeg, size_ref, raw_buffer, width, height);
251+
break;
252+
253+
default:
254+
fprintf(stderr, "Unsupported format\n");
255+
ret = -1;
256+
goto cleanup;
257+
}
258+
259+
*buffer_ref = jpeg;
260+
261+
cleanup:
262+
if (fd >= 0)
263+
{
264+
close(fd);
265+
}
266+
if (raw_buffer != NULL)
267+
{
268+
munmap(raw_buffer, buf_info.length);
269+
}
270+
271+
return ret;
272+
}

‎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: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,10 @@
1414
#include <unistd.h>
1515
#include <syslog.h>
1616

17+
#ifndef CDBA_CAMERA_DISABLED
18+
#include "camera.h"
19+
#endif // CDBA_CAMERA_DISABLED
20+
1721
#include "cdba-server.h"
1822
#include "circ_buf.h"
1923
#include "device.h"
@@ -99,6 +103,50 @@ static void msg_fastboot_continue(void)
99103
cdba_send(MSG_FASTBOOT_CONTINUE);
100104
}
101105

106+
#ifdef CDBA_CAMERA_DISABLED
107+
static void capture_image(struct device *device)
108+
{
109+
fprintf(stderr, "Camera support is not built into the server\n");
110+
}
111+
#else
112+
static void capture_image(struct device *device)
113+
{
114+
uint8_t *jpeg = NULL;
115+
size_t size = 0;
116+
117+
if (!device->video_device)
118+
{
119+
fprintf(stderr, "video_device not specified!\n");
120+
return;
121+
}
122+
123+
int ret = camera_capture_jpeg(&jpeg, &size, device->video_device);
124+
if (ret < 0)
125+
{
126+
fprintf(stderr, "Failed to capture image\n");
127+
return;
128+
}
129+
130+
const size_t chunk_size = 1024;
131+
size_t remaining_size = size;
132+
const uint8_t *ptr = jpeg;
133+
134+
while (remaining_size > 0) {
135+
size_t send_size = remaining_size > chunk_size ? chunk_size : remaining_size;
136+
137+
cdba_send_buf(MSG_CAPTURE_IMAGE, send_size, ptr);
138+
139+
ptr += send_size;
140+
remaining_size -= send_size;
141+
}
142+
143+
// Empty message to indicate end of image
144+
cdba_send_buf(MSG_CAPTURE_IMAGE, 0, NULL);
145+
146+
free(jpeg);
147+
}
148+
#endif // CDBA_CAMERA_DISABLED
149+
102150
void cdba_send_buf(int type, size_t len, const void *buf)
103151
{
104152
struct msg msg = {
@@ -185,6 +233,9 @@ static int handle_stdin(int fd, void *buf)
185233
case MSG_FASTBOOT_CONTINUE:
186234
msg_fastboot_continue();
187235
break;
236+
case MSG_CAPTURE_IMAGE:
237+
capture_image(selected_device);
238+
break;
188239
default:
189240
fprintf(stderr, "unk %d len %d\n", msg->type, msg->len);
190241
exit(1);

0 commit comments

Comments
 (0)