diff --git a/camera.c b/camera.c new file mode 100644 index 0000000..022321d --- /dev/null +++ b/camera.c @@ -0,0 +1,272 @@ +/* + * Copyright (c) 2024, Linaro Ltd. + * All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "camera.h" +#include "device.h" + +static int camera_init_device(const char *dev_name, int *fd_ref, char **driver_ref) +{ + *fd_ref = open(dev_name, O_RDWR); + if (*fd_ref < 0) + { + fprintf(stderr, "Couldn't open device: %s (%d)\n", strerror(errno), errno); + return -1; + } + + struct v4l2_capability cap; + + if (ioctl(*fd_ref, VIDIOC_QUERYCAP, &cap) < 0) + { + fprintf(stderr, "Couldn't query device capabilities: %s (%d)\n", strerror(errno), errno); + return -1; + } + + *driver_ref = strdup((char *)cap.driver); + + return 0; +} + +static int camera_configure_format(const int fd, int *width_ref, int *height_ref, uint32_t *format_ref) +{ + struct v4l2_format fmt = {0}; + fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + + if (ioctl(fd, VIDIOC_G_FMT, &fmt) < 0) + { + fprintf(stderr, "Couldn't get format: %s (%d)\n", strerror(errno), errno); + return -1; + } + + struct v4l2_frmsizeenum frame_size = {0}; + fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV; + frame_size.pixel_format = fmt.fmt.pix.pixelformat; + + if (ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frame_size) < 0) + { + fprintf(stderr, "Couldn't get frame size: %s (%d)\n", strerror(errno), errno); + return -1; + } + + fmt.fmt.pix.width = frame_size.discrete.width; + fmt.fmt.pix.height = frame_size.discrete.height; + + if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) + { + fprintf(stderr, "Couldn't set format: %s (%d)\n", strerror(errno), errno); + return -1; + } + + *width_ref = fmt.fmt.pix.width; + *height_ref = fmt.fmt.pix.height; + *format_ref = fmt.fmt.pix.pixelformat; + + return 0; +} + +static int camera_request_buffer(const int fd) +{ + struct v4l2_requestbuffers reqbuf = {0}; + reqbuf.count = 1; + reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + reqbuf.memory = V4L2_MEMORY_MMAP; + + if (ioctl(fd, VIDIOC_REQBUFS, &reqbuf) < 0) + { + fprintf(stderr, "Couldn't request buffer: %s (%d)\n", strerror(errno), errno); + return -1; + } + + return 0; +} + +static int camera_query_buffer(const int fd, uint8_t **buf_ref, struct v4l2_buffer *buf_info_ref) +{ + buf_info_ref->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf_info_ref->memory = V4L2_MEMORY_MMAP; + buf_info_ref->index = 0; + + if (ioctl(fd, VIDIOC_QUERYBUF, buf_info_ref) < 0) + { + fprintf(stderr, "Couldn't query buffer: %s (%d)\n", strerror(errno), errno); + return -1; + } + + *buf_ref = mmap(NULL, buf_info_ref->length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf_info_ref->m.offset); + if (buf_ref == MAP_FAILED) + { + fprintf(stderr, "Couldn't map buffer: %s (%d)\n", strerror(errno), errno); + return -1; + } + + return 0; +} + +static int camera_capture_frame(const int fd, struct v4l2_buffer *buf_info) +{ + if (ioctl(fd, VIDIOC_QBUF, buf_info)) + { + fprintf(stderr, "Couldn't queue buffer: %s (%d)\n", strerror(errno), errno); + return -1; + } + enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (ioctl(fd, VIDIOC_STREAMON, &type) < 0) + { + fprintf(stderr, "Couldn't start streaming: %s (%d)\n", strerror(errno), errno); + return -1; + } + if (ioctl(fd, VIDIOC_DQBUF, buf_info) < 0) + { + fprintf(stderr, "Couldn't dequeue buffer: %s (%d)\n", strerror(errno), errno); + return -1; + } + if (ioctl(fd, VIDIOC_STREAMOFF, &type) < 0) + { + fprintf(stderr, "Couldn't stop streaming: %s (%d)\n", strerror(errno), errno); + return -1; + } + + return 0; +} + +static int camera_yuyv_to_jpeg(uint8_t **jpeg_ref, unsigned long *size_ref, const uint8_t *yuyv, const int width, const int height) +{ + struct jpeg_compress_struct cinfo; + struct jpeg_error_mgr jerr; + + JSAMPROW row_pointer[1]; + + cinfo.err = jpeg_std_error(&jerr); + jpeg_create_compress(&cinfo); + + jpeg_mem_dest(&cinfo, jpeg_ref, size_ref); + + cinfo.image_width = width; + cinfo.image_height = height; + cinfo.input_components = 3; + cinfo.in_color_space = JCS_YCbCr; + + jpeg_set_defaults(&cinfo); + jpeg_set_quality(&cinfo, 100, TRUE); + + jpeg_start_compress(&cinfo, TRUE); + + uint8_t* row_buffer = (uint8_t*)malloc(width * 3 * sizeof(uint8_t)); + if (!row_buffer) + { + fprintf(stderr, "Memory allocation failed\n"); + jpeg_destroy_compress(&cinfo); + return -1; + } + + while (cinfo.next_scanline < height) + { + const uint32_t offset = cinfo.next_scanline * width * 2; + for (uint32_t i = 0, j = 0; i < width * 2; i += 4, j += 6) + { + row_buffer[j + 0] = yuyv[offset + i + 0]; + row_buffer[j + 1] = yuyv[offset + i + 1]; + row_buffer[j + 2] = yuyv[offset + i + 3]; + row_buffer[j + 3] = yuyv[offset + i + 2]; + row_buffer[j + 4] = yuyv[offset + i + 1]; + row_buffer[j + 5] = yuyv[offset + i + 3]; + } + row_pointer[0] = row_buffer; + jpeg_write_scanlines(&cinfo, row_pointer, 1); + } + + jpeg_finish_compress(&cinfo); + jpeg_destroy_compress(&cinfo); + + free(row_buffer); + + return 0; +} + +int camera_capture_jpeg(uint8_t **buffer_ref, unsigned long *size_ref, const char *video_device) +{ + int fd = -1; + uint8_t *raw_buffer = NULL; + + int ret = 0; + char *driver_str = NULL; + + int width, height = 0; + uint32_t format = 0; + + struct v4l2_buffer buf_info = {0}; + + uint8_t *jpeg = NULL; + + ret = camera_init_device(video_device, &fd, &driver_str); + if (ret < 0) + { + goto cleanup; + } + + ret = camera_configure_format(fd, &width, &height, &format); + if (ret < 0) + { + goto cleanup; + } + + fprintf(stderr, "Driver: %s, Resolution: %dx%d, Format: %c%c%c%c\n", driver_str, width, height, + (char)((format >> 0) & 0xFF), (char)((format >> 8) & 0xFF), (char)((format >> 16) & 0xFF), (char)((format >> 24) & 0xFF)); + free(driver_str); + + ret = camera_request_buffer(fd); + if (ret < 0) + { + goto cleanup; + } + + ret = camera_query_buffer(fd, &raw_buffer, &buf_info); + if (ret < 0) + { + goto cleanup; + } + + camera_capture_frame(fd, &buf_info); + + switch (format) + { + case V4L2_PIX_FMT_YUYV: + camera_yuyv_to_jpeg(&jpeg, size_ref, raw_buffer, width, height); + break; + + default: + fprintf(stderr, "Unsupported format\n"); + ret = -1; + goto cleanup; + } + + *buffer_ref = jpeg; + +cleanup: + if (fd >= 0) + { + close(fd); + } + if (raw_buffer != NULL) + { + munmap(raw_buffer, buf_info.length); + } + + return ret; +} diff --git a/camera.h b/camera.h new file mode 100644 index 0000000..99a2b61 --- /dev/null +++ b/camera.h @@ -0,0 +1,14 @@ +/* + * Copyright (c) 2024, Linaro Ltd. + * All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ +#ifndef __CAMERA_H__ +#define __CAMERA_H__ + +#include + +int camera_capture_jpeg(uint8_t **buffer_ref, unsigned long *size_ref, const char *video_device); + +#endif // __CAMERA_H__ diff --git a/cdba-server.c b/cdba-server.c index c463f19..908b591 100644 --- a/cdba-server.c +++ b/cdba-server.c @@ -14,6 +14,10 @@ #include #include +#ifndef CDBA_CAMERA_DISABLED +#include "camera.h" +#endif // CDBA_CAMERA_DISABLED + #include "cdba-server.h" #include "circ_buf.h" #include "device.h" @@ -99,6 +103,45 @@ static void msg_fastboot_continue(void) cdba_send(MSG_FASTBOOT_CONTINUE); } +#ifndef CDBA_CAMERA_DISABLED +static void capture_image(struct device *device) +{ + uint8_t *jpeg = NULL; + size_t size = 0; + + if (!device->video_device) + { + fprintf(stderr, "video_device not specified!\n"); + return; + } + + int ret = camera_capture_jpeg(&jpeg, &size, device->video_device); + if (ret < 0) + { + fprintf(stderr, "Failed to capture image\n"); + return; + } + + const size_t chunk_size = 1024; + size_t remaining_size = size; + const uint8_t *ptr = jpeg; + + while (remaining_size > 0) { + size_t send_size = remaining_size > chunk_size ? chunk_size : remaining_size; + + cdba_send_buf(MSG_CAPTURE_IMAGE, send_size, ptr); + + ptr += send_size; + remaining_size -= send_size; + } + + // Empty message to indicate end of image + cdba_send_buf(MSG_CAPTURE_IMAGE, 0, NULL); + + free(jpeg); +} +#endif // CDBA_CAMERA_DISABLED + void cdba_send_buf(int type, size_t len, const void *buf) { struct msg msg = { @@ -184,6 +227,14 @@ static int handle_stdin(int fd, void *buf) break; case MSG_FASTBOOT_CONTINUE: msg_fastboot_continue(); + break; + case MSG_CAPTURE_IMAGE: + #ifdef CDBA_CAMERA_DISABLED + fprintf(stderr, "Camera support is not built into the server\n"); + #else + capture_image(selected_device); + #endif + break; default: fprintf(stderr, "unk %d len %d\n", msg->type, msg->len); diff --git a/cdba.c b/cdba.c index 1809423..9b45e09 100644 --- a/cdba.c +++ b/cdba.c @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -184,6 +185,9 @@ static int tty_callback(int *ssh_fds) case 'B': cdba_send(ssh_fds[0], MSG_SEND_BREAK); break; + case 'i': + cdba_send(ssh_fds[0], MSG_CAPTURE_IMAGE); + break; } special = false; @@ -481,6 +485,45 @@ static void handle_console(const void *data, size_t len) write(STDOUT_FILENO, data, len); } +static void handle_image_capture(void *data, size_t len) +{ + static int fd = -1; + const char *filename = "capture.jpg"; + int ret; + + if (len == 0) + { + // End of image + if (fd >= 0) + { + close(fd); + fd = -1; + + fprintf(stderr, "Saved captured image to %s\n", filename); + } + return; + } + + if (fd < 0) { + // First chunk of image + fd = open(filename, O_CREAT | O_WRONLY | O_TRUNC, 0666); + if (fd < 0) { + err(1, "Failed to open %s", filename); + } + } + + while (len > 0) + { + ret = write(fd, data, len); + if (ret < 0) { + err(1, "Failed to write to %s", filename); + } + + data = (uint8_t *)data + ret; + len -= ret; + } +} + static bool auto_power_on; static int handle_message(struct circ_buf *buf) @@ -557,6 +600,9 @@ static int handle_message(struct circ_buf *buf) case MSG_FASTBOOT_CONTINUE: // printf("======================================== MSG_FASTBOOT_CONTINUE\n"); break; + case MSG_CAPTURE_IMAGE: + handle_image_capture(msg->data, msg->len); + break; default: fprintf(stderr, "unk %d len %d\n", msg->type, msg->len); return -1; diff --git a/cdba.h b/cdba.h index 0264c40..63ae7cb 100644 --- a/cdba.h +++ b/cdba.h @@ -31,6 +31,7 @@ enum { MSG_LIST_DEVICES, MSG_BOARD_INFO, MSG_FASTBOOT_CONTINUE, + MSG_CAPTURE_IMAGE, }; #endif diff --git a/device.h b/device.h index 75a780a..30fa3fc 100644 --- a/device.h +++ b/device.h @@ -42,6 +42,9 @@ struct device { bool tickle_mmc; bool usb_always_on; bool power_always_on; + + char *video_device; + struct fastboot *fastboot; unsigned int fastboot_key_timeout; int state; diff --git a/device_parser.c b/device_parser.c index f5c2aa7..f938195 100644 --- a/device_parser.c +++ b/device_parser.c @@ -178,6 +178,8 @@ static void parse_board(struct device_parser *dp) dev->status_cmd = strdup(value); } else if (!strcmp(key, "power_always_on")) { dev->power_always_on = !strcmp(value, "true"); + } else if (!strcmp(key, "video_device")) { + dev->video_device = strdup(value); } else { fprintf(stderr, "device parser: unknown key \"%s\"\n", key); exit(1); diff --git a/meson.build b/meson.build index c9e22ee..6af8ab8 100644 --- a/meson.build +++ b/meson.build @@ -102,6 +102,19 @@ foreach d: cdbalib_deps endforeach if build_server + camera_opt = get_option('camera') + jpeg_dep = dependency('libjpeg', required: camera_opt) + + build_camera = true + server_cflags = [] + if not jpeg_dep.found() + build_camera = false + server_cflags += '-DCDBA_CAMERA_DISABLED' + else + cdbalib_srcs += ['camera.c'] + cdbalib_deps += jpeg_dep + endif + libcdba = static_library('cdba', cdbalib_srcs + drivers_srcs, dependencies : cdbalib_deps, @@ -110,7 +123,8 @@ if build_server executable('cdba-server', server_srcs, link_with : libcdba, - install : true) + install : true, + c_args : server_cflags) executable('cdba-power', ['cdba-power.c'], link_with : libcdba, diff --git a/meson_options.txt b/meson_options.txt index 9c4d8bc..7a2af0c 100644 --- a/meson_options.txt +++ b/meson_options.txt @@ -1 +1,3 @@ option('server', type: 'feature', description: 'Controls whether the CDBA server is built. By default it will be built if all dependencies are present.') + +option('camera', type: 'feature', description: 'Controls whether camera support is built into the server. By default it will be built if libjpeg is present.')