前段時間寫了一個測試 camera 的應用程序，很簡單的實現了在平臺上拍照和錄像的功能，bmp文件頭暫時只支持固定分辨率 640 x 480：
#include <stdio.h>
#include <errno.h>
#include <stdint.h>
#include <signal.h>
#include <string.h>
#include <stdlib.h>
#include <getopt.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <linux/fb.h>
#include <linux/videodev2.h>
#define CLIP_MIN (-278)
#define CLIP_MAX (535)
static volatile int clip_init_done = 0;
static uint8_t clip[CLIP_MAX - CLIP_MIN + 1];
uint8_t *init_clip()
{
int i;
if(clip_init_done)
return &clip[-CLIP_MIN];
for (i = CLIP_MIN; i <= CLIP_MAX; ++i)
clip[i - CLIP_MIN] = (i < 0) ? 0 : (i > 255) ? 255 : (uint8_t)i;
clip_init_done = 1;
return &clip[-CLIP_MIN];
}
/*
* Convert YUV planner to RGB565
*
* For YUV420: y_stride = width, yu_stride = width / 2
* For YUV422: y_stride = width, yu_stride = width
* src_y src_u src_v: start address of Y U V data block in source frame.
* dst: start address of results frame.
*/
static void yuv_planar_2_rgb(size_t width, size_t height,
size_t y_stride, size_t uv_stride,
const uint8_t *src_y, const uint8_t *src_u,
const uint8_t *src_v, void *dst)
{
size_t x, y;
uint8_t *adj_clip = init_clip();
uint32_t *dst_ptr = (uint32_t *)dst;
int y1, y2, u, v, u_b, u_g, v_g, v_r, tmp1, b1, g1, r1, tmp2, b2, g2, r2;
uint32_t rgb1, rgb2;
for (y = 0; y < height; ++y) {
for (x = 0; x < width; x += 2) {
y1 = (int)src_y[x] - 16;
y2 = (int)src_y[x + 1] - 16;
u = (int)src_u[x / 2] - 128;
v = (int)src_v[x / 2] - 128;
u_b = u * 517;
u_g = -u * 100;
v_g = -v * 208;
v_r = v * 409;
tmp1 = y1 * 298;
b1 = (tmp1 + u_b) / 256;
g1 = (tmp1 + v_g + u_g) / 256;
r1 = (tmp1 + v_r) / 256;
tmp2 = y2 * 298;
b2 = (tmp2 + u_b) / 256;
g2 = (tmp2 + v_g + u_g) / 256;
r2 = (tmp2 + v_r) / 256;
rgb1 = ((adj_clip[r1] >> 3) << 11)
| ((adj_clip[g1] >> 2) << 5)
| (adj_clip[b1] >> 3);
rgb2 = ((adj_clip[r2] >> 3) << 11)
| ((adj_clip[g2] >> 2) << 5)
| (adj_clip[b2] >> 3);
dst_ptr[x / 2] = (rgb2 << 16) | rgb1;
}
src_y += y_stride;
if (y & 1) {
src_u += uv_stride;
src_v += uv_stride;
}
dst_ptr += width / 2;
}
}
/*
* Convert RGB565 to RGB888
*
* src: start address of RGB565 data block in source frame.
* dst: start address of results frame.
*/
static void rgb565_2_rgb888(uint16_t *src, uint8_t *dst, int width, int height)
{
int i;
for(i = 0; i < width * height; i++, src++, dst += 3)
{
/* 順序: b、g、r, 每個分量都是 8 bit 對齊 */
dst[0] = (uint8_t)(((*src & 0x001f) << 3) | ((*src & 0x001f) >> 2));
dst[1] = (uint8_t)((((*src & 0x07e0) >> 5) << 2) | (((*src & 0x07e0) >> 5) >> 4));
dst[2] = (uint8_t)(((*src >> 11) << 3) | ((*src >> 11) >> 2));
}
}
/* used for calculate FPS */
static struct timeval start, end;
static unsigned long long g_tick, g_precision = 1000000;
static inline void clock_start()
{
gettimeofday(&start, NULL);
return;
}
/* for calculate fps */
static inline void clock_end()
{
struct timeval val;
gettimeofday(&end, NULL);
timersub(&end, &start, &val);
g_tick = val.tv_sec * 1000000 + val.tv_usec;
return;
}
/* bmp file's head */
char bmp_head[] = {0x42, 0x4d, 0x36, 0x10, 0x0e, 0x00, 0x00, 0x00,
0x00, 0x00, 0x36, 0x00, 0x00, 0x00, 0x28, 0x00,
0x00, 0x00, 0x80, 0x02, 0x00, 0x00, 0xe0, 0x01,
0x00, 0x00, 0x01, 0x00, 0x18, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc4, 0x0e,
0x00, 0x00, 0xc4, 0x0e, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
struct cam_buffer {
void *start;
void *vstart;
unsigned long length;
};
static struct cam_info {
int num; /* pic statistics */
int count; /* take pic number, -1 means not stop */
int dev_fd; /* camera dev file descriptor */
int yuv_fd; /* camera yuv file descriptor */
int rgb_fd; /* camera rgb file descriptor */
unsigned long index; /* buffer index */
unsigned long nbuffer; /* total buffer number */
unsigned long bufsize; /* buffer size */
unsigned long stop_stream; /* flag indicate to stop work */
struct cam_buffer *buffers; /* mmaped buffer info */
struct v4l2_format format; /* pic format */
struct v4l2_requestbuffers req;
} cam_info;
static void open_yuv_file()
{
cam_info.yuv_fd = open("/sdcard/cam_test.yuv", O_CREAT | O_TRUNC | O_RDWR, 0777);
if (cam_info.yuv_fd < 0) {
printf("open /sdcard/cam_test.yuv failed!\n");
}
}
static void open_rgb_file()
{
cam_info.rgb_fd = open("/sdcard/cam_test.bmp", O_CREAT | O_TRUNC | O_RDWR, 0777);
if (cam_info.rgb_fd < 0) {
printf("open /sdcard/cam_test.bmp failed!\n");
}
}
static int start_capturing()
{
int ret;
unsigned long i;
struct v4l2_buffer buf;
enum v4l2_buf_type type;
for (i = 0; i < cam_info.nbuffer; ++i) {
memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
ret = ioctl(cam_info.dev_fd, VIDIOC_QBUF, &buf);
if (ret < 0) {
printf("ioctl VIDIOC_QBUF failed, %d\n", errno);
return -1;
}
}
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
ret = ioctl(cam_info.dev_fd, VIDIOC_STREAMON, &type);
if (ret < 0) {
printf("ioctl VIDIOC_STREAMON failed!, %d\n", errno);
return -1;
}
return 0;
}
static int write_frame()
{
int ret;
struct v4l2_buffer buf;
memset(&buf, 0, sizeof(struct v4l2_buffer));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
ret = ioctl(cam_info.dev_fd, VIDIOC_DQBUF, &buf);
if (ret < 0) {
if(errno == EPIPE) {
printf("Streaming stop, we need restart stream\n");
start_capturing();
return 0;
} else {
printf("ioctl VIDIOC_DQBUF failed!, %d\n", errno);
return -1;
}
}
if (cam_info.nbuffer < buf.index) {
printf("Kernel return wrong index %d, totoal %lu\n", buf.index, cam_info.nbuffer);
return -1;
}
ret = write(cam_info.yuv_fd, cam_info.buffers[buf.index].vstart, cam_info.format.fmt.pix.sizeimage);
if(ret < cam_info.format.fmt.pix.sizeimage) {
printf("write failed, ret = %d\n", ret);
return ret;
}
ret = ioctl(cam_info.dev_fd, VIDIOC_QBUF, &buf);
if (ret < 0) {
printf("ioctl VIDIOC_QBUF failed!, %d\n", errno);
return -1;
}
cam_info.num++;
return 0;
}
static int stop_capturing()
{
int ret;
enum v4l2_buf_type type;
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
ret = ioctl(cam_info.dev_fd, VIDIOC_STREAMOFF, &type);
if (ret < 0) {
printf("ioctl VIDIOC_STREAMOFF failed!, %d\n", errno);
return -1;
}
return 0;
}
static int start_preview()
{
int ret;
fd_set fds;
struct timeval tv;
/* Timeout. */
tv.tv_sec = 5;
tv.tv_usec = 0;
open_yuv_file();
clock_start();
while (!cam_info.stop_stream && (cam_info.count < 0 || cam_info.num < cam_info.count)) {
FD_ZERO(&fds);
FD_SET(cam_info.dev_fd, &fds);
ret = select(cam_info.dev_fd + 1, &fds, NULL, NULL, &tv);
if (ret < 0) {
printf("select() failed, %d\n", errno);
break;
}
if (ret == 0)
printf("select() timeout\n");
ret = write_frame();
if (ret < 0)
break;
}
clock_end();
printf("\nget %d frame, fps: %f\n", cam_info.num, (cam_info.num * 1.0 * g_precision) / g_tick);
cam_info.num = 0;
close(cam_info.yuv_fd);
stop_capturing();
printf("preview done, streaming off\n");
return ret;
}
static int init_device()
{
int i, ret;
struct v4l2_buffer buf;
struct v4l2_capability cap;
/* capabilities related */
memset(&cap, 0, sizeof(cap));
ret = ioctl(cam_info.dev_fd, VIDIOC_QUERYCAP, &cap);
if (ret < 0) {
printf("ioctl VIDIOC_QUERYCAP failed! ret = %d\n", ret);
return -1;
}
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
printf("Opening a no video capture device\n");
return -1;
}
if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
printf("device does not support streaming i/o\n");
return -1;
}
/* set format */
cam_info.format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
cam_info.format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P;
ret = ioctl(cam_info.dev_fd, VIDIOC_S_FMT, &cam_info.format);
if (ret < 0) {
printf("ioctl VIDIOC_S_FMT failed! ret = %d\n", ret);
return -1;
}
/* request buffer */
cam_info.req.count = cam_info.nbuffer;
cam_info.req.memory = V4L2_MEMORY_MMAP;
cam_info.req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
ret = ioctl(cam_info.dev_fd, VIDIOC_REQBUFS, &cam_info.req);
if (ret < 0) {
printf("ioctl VIDIOC_REQBUFS failed! ret = %d\n", ret);
return -1;
}
cam_info.buffers = malloc(cam_info.req.count * sizeof(struct cam_buffer));
if (!cam_info.buffers) {
printf("malloc() failed!\n");
return -1;
}
/* query buffer */
for (i = 0; i < cam_info.req.count; i++) {
memset(&buf, 0, sizeof(buf));
buf.index = i;
buf.memory = V4L2_MEMORY_MMAP;
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
ret = ioctl(cam_info.dev_fd, VIDIOC_QUERYBUF, &buf);
if (ret < 0) {
printf("ioctl VIDIOC_QUERYBUF failed! ret = %d\n", ret);
free(cam_info.buffers);
return -1;
}
cam_info.buffers[i].length = buf.length;
cam_info.buffers[i].vstart = mmap (NULL, buf.length,
PROT_READ | PROT_WRITE, MAP_SHARED,
cam_info.dev_fd, buf.m.offset);
printf("mmap virt %p, phy %p, size %lu\n",
(void *)cam_info.buffers[i].vstart,
(void *)buf.reserved,
(unsigned long)buf.length);
if (MAP_FAILED == cam_info.buffers[i].vstart) {
printf("mmap() failed!\n");
free(cam_info.buffers);
return -1;
}
}
return 0;
}
/* read frame */
static int take_photo()
{
int i, ret, len, rgb565_len, rgb888_len;
char *buffer, *rgb565_buffer, *rgb888_buffer;
len = cam_info.format.fmt.pix.sizeimage;
rgb565_len = cam_info.format.fmt.pix.sizeimage;
rgb888_len = cam_info.format.fmt.pix.sizeimage * 3 / 2;
buffer = malloc(len);
rgb565_buffer = malloc(rgb565_len);
rgb888_buffer = malloc(rgb888_len);
open_yuv_file();
clock_start();
for(i = 0; i < cam_info.req.count; i++) {
ret = read(cam_info.dev_fd, buffer, len);
if(ret < len) {
printf("read failed, ret = %d, len = %d\n", ret, len);
goto exit;
}
ret = write(cam_info.yuv_fd, buffer, len);
if(ret < len) {
printf("write failed, ret = %d\n", ret);
goto exit;
}
}
clock_end();
close(cam_info.yuv_fd);
printf("get %d frame, fps: %f\n", i, (i * 1.0 * g_precision) / g_tick);
open_rgb_file();
ret = write(cam_info.rgb_fd, bmp_head, sizeof(bmp_head));
if(ret < sizeof(bmp_head)) {
printf("write bmp_head failed, ret = %d\n", ret);
}
yuv_planar_2_rgb(cam_info.format.fmt.pix.width, cam_info.format.fmt.pix.height,
cam_info.format.fmt.pix.width, cam_info.format.fmt.pix.width,
(const uint8_t *)buffer,
(const uint8_t *)(buffer + rgb565_len / 2),
(const uint8_t *)(buffer + 3 * rgb565_len / 4),
rgb565_buffer);
rgb565_2_rgb888(rgb565_buffer, rgb888_buffer, cam_info.format.fmt.pix.width, cam_info.format.fmt.pix.height);
ret = write(cam_info.rgb_fd, rgb888_buffer, rgb888_len);
if(ret < rgb888_len) {
printf("write rgb_buffer failed, ret = %d\n", ret);
goto exit;
}
close(cam_info.rgb_fd);
free(buffer);
free(rgb565_buffer);
free(rgb888_buffer);
return 0;
exit:
free(buffer);
free(rgb565_buffer);
free(rgb888_buffer);
return ret;
}
static int take_video()
{
int ret;
ret = start_capturing();
if(ret < 0)
return ret;
return start_preview();
}
static void show_usage()
{
printf("cam_test -d <dev> -c <mode> -w <width> -h <height> -n <count>\n");
printf("Options:\n");
printf(" -d: device num. 0 - back, 1 - front.\n");
printf(" -c: capture mode. 0 - video, 1 - photo.\n");
printf(" -w: specify frame width.\n");
printf(" -h: specify frame height.\n");
printf(" -n: specify the count of frames.\n");
}
int main(int argc, char *argv[])
{
int c, cam_dev, cam_ctl, pix_width, pix_height, pix_count;
if (argc < 2) {
show_usage();
return -1;
}
while ((c = getopt(argc, argv, "d:c:n:h:w:")) != -1) {
switch(c) {
case 'd':
cam_dev = atoi(optarg);
break;
case 'c':
cam_ctl = atoi(optarg);
break;
case 'n':
pix_count = atoi(optarg);
break;
case 'h':
pix_height = atoi(optarg);
break;
case 'w':
pix_width = atoi(optarg);
break;
default:
show_usage();
return -1;
}
}
printf("pix_width = %d, pix_height = %d, pix_count = %d\n", pix_width, pix_height, pix_count);
if(cam_dev)
cam_info.dev_fd = open("/dev/video1", O_RDWR);
else
cam_info.dev_fd = open("/dev/video0", O_RDWR);
cam_info.format.fmt.pix.width = pix_width;
cam_info.format.fmt.pix.height = pix_height;
cam_info.count = pix_count;
cam_info.nbuffer = 10;
cam_info.nbuffer = cam_info.nbuffer < pix_count ? cam_info.nbuffer : pix_count;
init_device();
if(cam_ctl)
take_photo();
else
take_video();
close(cam_info.dev_fd);
return 0;
}
