Hello guys! I just started to learn about V4L2 and I got an issue, I wanted to use some low-api (istead of gstreamer). But my code, which is edited example (not much) is really slow. Without any calculations, writing to file is CPU usage on 40% on 2.6GHz!! When I tried cheese (capture program), it has only 14% cpu usage but cheese is showing image from cam, my app not. So I can't see why my program is so slow.

Code:
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <linux/videodev2.h>
#include <libv4l2.h>
#define CLEAR(x) memset(&(x), 0, sizeof(x))

struct buffer {
        void   *start;
        size_t length;
};

static void xioctl(int fh, int request, void *arg)
{
        int r;

        do {
                r = v4l2_ioctl(fh, request, arg);
        } while (r == -1 && ((errno == EINTR) || (errno == EAGAIN)));

        if (r == -1) {
                fprintf(stderr, "error %d, %s\n", errno, strerror(errno));
                exit(EXIT_FAILURE);
        }
}

typedef struct {
	unsigned char *r;
	unsigned char *g;
	unsigned char *b;
} pixel;

pixel getPixel(int x, int y, int* width, unsigned char* buf){
	int pos = (y + x * (*width)) * 3;
	pixel pix;
	pix.r = &buf[pos    ];
	pix.g = &buf[pos + 1];
	pix.b = &buf[pos + 2];
	return pix;
}

void setPixel(int x, int y, int* width, unsigned char* buf, pixel color){
	int pos = (y + x * *width) * 3;

	buf[pos    ] = *color.r;
	buf[pos + 1] = *color.g;
	buf[pos + 2] = *color.b;
}
int main(int argc, char **argv)
{
        struct v4l2_format              fmt;
        struct v4l2_buffer              buf;
        struct v4l2_requestbuffers      req;
        enum v4l2_buf_type              type;
        fd_set                          fds;
        struct timeval                  tv;
        int                             r, fd = -1;
        unsigned int                    i, n_buffers;
        char                            *dev_name = "/dev/video0";
        char                            out_name[256];
        FILE                            *fout;
        struct buffer                   *buffers;

        fd = v4l2_open(dev_name, O_RDWR | O_NONBLOCK, 0);
        if (fd < 0) {
                perror("Cannot open device");
                exit(EXIT_FAILURE);
        }

		int width = 640;
		int height = 480;

        CLEAR(fmt);
        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        fmt.fmt.pix.width       = width;
        fmt.fmt.pix.height      = height;
        fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
        fmt.fmt.pix.field       = V4L2_FIELD_NONE;
        xioctl(fd, VIDIOC_S_FMT, &fmt);
        if (fmt.fmt.pix.pixelformat != V4L2_PIX_FMT_RGB24) {
                printf("Libv4l didn't accept RGB24 format. Can't proceed.\n");
                exit(EXIT_FAILURE);
        }
        printf("Driver is sending image at %dx%d\n",
                        fmt.fmt.pix.width, fmt.fmt.pix.height);

        CLEAR(req);
        req.count = 2;
        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        req.memory = V4L2_MEMORY_MMAP;
        xioctl(fd, VIDIOC_REQBUFS, &req);

        buffers = calloc(req.count, sizeof(*buffers));
        for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
                CLEAR(buf);

                buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                buf.memory      = V4L2_MEMORY_MMAP;
                buf.index       = n_buffers;

                xioctl(fd, VIDIOC_QUERYBUF, &buf);

                buffers[n_buffers].length = buf.length;
                buffers[n_buffers].start = v4l2_mmap(NULL, buf.length,
                              PROT_READ | PROT_WRITE, MAP_SHARED,
                              fd, buf.m.offset);

                if (MAP_FAILED == buffers[n_buffers].start) {
                        perror("mmap");
                        exit(EXIT_FAILURE);
                }
        }

        for (i = 0; i < n_buffers; ++i) {
                CLEAR(buf);
                buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                buf.memory = V4L2_MEMORY_MMAP;			
                buf.index = i;
				
                xioctl(fd, VIDIOC_QBUF, &buf);
        }
//////////////////////////
		struct v4l2_streamparm parm;
			int ret;

			memset(&parm, 0, sizeof parm);
			parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

			ret = ioctl(fd, VIDIOC_G_PARM, &parm);
			if (ret < 0) {
				printf("Unable to get frame rate: %d.\n", errno);
				return ret;
			}

			printf("Current frame rate: %u/%u\n",
				parm.parm.capture.timeperframe.numerator,
				parm.parm.capture.timeperframe.denominator);

			parm.parm.capture.timeperframe.numerator = 1;
			parm.parm.capture.timeperframe.denominator = 60;

			ret = ioctl(fd, VIDIOC_S_PARM, &parm);
			if (ret < 0) {
				printf("Unable to set frame rate: %d.\n", errno);
				return ret;
			}

			ret = ioctl(fd, VIDIOC_G_PARM, &parm);
			if (ret < 0) {
				printf("Unable to get frame rate: %d.\n", errno);
				return ret;
			}

			printf("Frame rate set: %u/%u\n",
				parm.parm.capture.timeperframe.numerator,
				parm.parm.capture.timeperframe.denominator);
////////////////////
        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		
        xioctl(fd, VIDIOC_STREAMON, &type);

		int frame = 0;
		long int lastsecond = time(NULL);
        for (i = 0; i < 600; i++) {				
				
				frame++;
				long int second  = time(NULL);

				if(second > lastsecond){
					printf("Fps: %d \n", frame);
					frame = 0;
					lastsecond = second;
				}
				
                do {
                        FD_ZERO(&fds);
                        FD_SET(fd, &fds);

                        r = select(fd + 1, &fds, NULL, NULL, NULL);
                } while ((r == -1 && (errno = EINTR)));
                if (r == -1) {
                        perror("select");
                        return errno;
                }

                CLEAR(buf);
                buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                buf.memory = V4L2_MEMORY_MMAP;

                xioctl(fd, VIDIOC_DQBUF, &buf);
				/*
                sprintf(out_name, "out.ppm", i);
				
                fout = fopen(out_name, "w");
                if (!fout) {
                        perror("Cannot open image");
                        exit(EXIT_FAILURE);
                }
                fprintf(fout, "P6\n%d %d 255\n",
                        fmt.fmt.pix.width, fmt.fmt.pix.height);
				
				unsigned char* buffer = buffers[buf.index].start;
				int x,y;

				unsigned char white = 254;

				
				for(x = 0; x < height; x++){
					for(y = 0; y < width-100; y++){//BUG!!!!!!!!

						pixel pix = getPixel( x, y, &width, buffer);
						
						

						if(*pix.r > 128 && *pix.g > 128 && *pix.b > 128){

							*pix.r = white;
							*pix.g = white;
							*pix.b = white;

						}

						int shift = 0;
						
						pixel pixr = getPixel(x, y + shift > width ? width - 1 : y + shift, &width, buffer);
						pixel pixl = getPixel(x, y - shift < 0 ? 0 : y - shift, &width, buffer);

						pixel pixb = getPixel( x + shift > width ? width - 1 : x + shift, y, &width, buffer);
						pixel pixu = getPixel( x - shift < 0 ? 0 : x - shift, y, &width, buffer);

						*pix.r = (*pix.r + *pixr.r + *pixl.r + *pixb.r + *pixu.r) / 5;
						*pix.g = (*pix.g + *pixr.g + *pixl.g + *pixb.g + *pixu.g) / 5;
						*pix.b = (*pix.b + *pixr.b + *pixl.b + *pixb.b + *pixu.b) / 5;

					}
				}
				

                fwrite(buffers[buf.index].start, buf.bytesused, 1, fout);

                fclose(fout);*/

                xioctl(fd, VIDIOC_QBUF, &buf);
        }
		

        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        xioctl(fd, VIDIOC_STREAMOFF, &type);
        for (i = 0; i < n_buffers; ++i)
                v4l2_munmap(buffers[i].start, buffers[i].length);
        v4l2_close(fd);

        return 0;
}
Thank you for help