diff --git a/src/ustreamer/device.c b/src/ustreamer/device.c
index 0da6e64b3..1f305a1d4 100644
--- a/src/ustreamer/device.c
+++ b/src/ustreamer/device.c
@@ -55,6 +55,7 @@ static const struct {
 };
 
 
+static void _v4l2_buffer_copy(const struct v4l2_buffer *src, struct v4l2_buffer *dest);
 static bool _device_is_buffer_valid(us_device_s *dev, const struct v4l2_buffer *buf, const uint8_t *data);
 static int _device_open_check_cap(us_device_s *dev);
 static int _device_open_dv_timings(us_device_s *dev);
@@ -81,35 +82,12 @@ static const char *_format_to_string_supported(unsigned format);
 static const char *_standard_to_string(v4l2_std_id standard);
 static const char *_io_method_to_string_supported(enum v4l2_memory io_method);
 
-static void _v4l2_buffer_init(enum v4l2_buf_type type, struct v4l2_buffer *buf);
-static void _v4l2_buffer_destroy(enum v4l2_buf_type type, struct v4l2_buffer *buf);
-static void _v4l2_buffer_copy(enum v4l2_buf_type type, struct v4l2_buffer *dst, struct v4l2_buffer *src);
 
 #define _RUN(x_next)	dev->run->x_next
 #define _D_XIOCTL(...)	us_xioctl(_RUN(fd), __VA_ARGS__)
+#define _D_IS_MPLANE	(_RUN(capture_type) == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 
 
-void _v4l2_buffer_init(enum v4l2_buf_type type, struct v4l2_buffer *buf) {
-	if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-		US_CALLOC(buf->m.planes, VIDEO_MAX_PLANES);
-	}
-}
-
-void _v4l2_buffer_destroy(enum v4l2_buf_type type, struct v4l2_buffer *buf) {
-	if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-		free(buf->m.planes);
-	}
-}
-
-void _v4l2_buffer_copy(enum v4l2_buf_type type, struct v4l2_buffer *dst, struct v4l2_buffer *src) {
-	struct v4l2_plane *planes = dst->m.planes;
-	memcpy(dst, src, sizeof(*src));
-	if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-		memcpy(planes, src->m.planes, sizeof(*src->m.planes) * VIDEO_MAX_PLANES);
-		dst->m.planes = planes;
-	}
-}
-
 us_device_s *us_device_init(void) {
 	us_device_runtime_s *run;
 	US_CALLOC(run, 1);
@@ -203,7 +181,7 @@ void us_device_close(us_device_s *dev) {
 	if (_RUN(hw_bufs) != NULL) {
 		US_LOG_DEBUG("Releasing device buffers ...");
 		for (unsigned index = 0; index < _RUN(n_bufs); ++index) {
-#			define HW(x_next) (_RUN(hw_bufs)[index].x_next)
+#			define HW(x_next) _RUN(hw_bufs)[index].x_next
 
 			if (HW(dma_fd) >= 0) {
 				close(HW(dma_fd));
@@ -220,7 +198,10 @@ void us_device_close(us_device_s *dev) {
 				US_DELETE(HW(raw.data), free);
 			}
 
-			_v4l2_buffer_destroy(dev->capture_type, &HW(buf));
+			if (_D_IS_MPLANE) {
+				free(HW(buf.m.planes));
+			}
+
 #			undef HW
 		}
 		_RUN(n_bufs) = 0;
@@ -243,8 +224,8 @@ int us_device_export_to_dma(us_device_s *dev) {
 #	define DMA_FD		_RUN(hw_bufs[index].dma_fd)
 
 	for (unsigned index = 0; index < _RUN(n_bufs); ++index) {
-		struct v4l2_exportbuffer exp = {};
-		exp.type = dev->capture_type;
+		struct v4l2_exportbuffer exp = {0};
+		exp.type = _RUN(capture_type);
 		exp.index = index;
 
 		US_LOG_DEBUG("Exporting device buffer=%u to DMA ...", index);
@@ -271,7 +252,7 @@ int us_device_export_to_dma(us_device_s *dev) {
 
 int us_device_switch_capturing(us_device_s *dev, bool enable) {
 	if (enable != _RUN(capturing)) {
-		enum v4l2_buf_type type = dev->capture_type;
+		enum v4l2_buf_type type = _RUN(capture_type);
 
 		US_LOG_DEBUG("%s device capturing ...", (enable ? "Starting" : "Stopping"));
 		if (_D_XIOCTL((enable ? VIDIOC_STREAMON : VIDIOC_STREAMOFF), &type) < 0) {
@@ -336,8 +317,13 @@ int us_device_select(us_device_s *dev, bool *has_read, bool *has_write, bool *ha
 int us_device_grab_buffer(us_device_s *dev, us_hw_buffer_s **hw) {
 	*hw = NULL;
 
-	struct v4l2_buffer buf = {};
-	struct v4l2_plane planes[VIDEO_MAX_PLANES] = {};
+	struct v4l2_buffer buf = {0};
+	struct v4l2_plane buf_planes[VIDEO_MAX_PLANES] = {0};
+	if (_D_IS_MPLANE) {
+		// Just for _v4l2_buffer_copy(), buf.length is not needed here
+		buf.m.planes = buf_planes;
+	}
+
 	bool buf_got = false;
 	unsigned skipped = 0;
 	bool broken = false;
@@ -345,12 +331,13 @@ int us_device_grab_buffer(us_device_s *dev, us_hw_buffer_s **hw) {
 	US_LOG_DEBUG("Grabbing device buffer ...");
 
 	do {
-		struct v4l2_buffer new = {};
-		new.type = dev->capture_type;
+		struct v4l2_buffer new = {0};
+		struct v4l2_plane new_planes[VIDEO_MAX_PLANES] = {0};
+		new.type = _RUN(capture_type);
 		new.memory = dev->io_method;
-		if (dev->capture_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		if (_D_IS_MPLANE) {
 			new.length = VIDEO_MAX_PLANES;
-			new.m.planes = planes;
+			new.m.planes = new_planes;
 		}
 		
 		const bool new_got = (_D_XIOCTL(VIDIOC_DQBUF, &new) >= 0);
@@ -370,8 +357,9 @@ int us_device_grab_buffer(us_device_s *dev, us_hw_buffer_s **hw) {
 			}
 			GRABBED(new) = true;
 
-			if (dev->capture_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) 
+			if (_D_IS_MPLANE) {
 				new.bytesused = new.m.planes[0].bytesused;
+			}
 
 			broken = !_device_is_buffer_valid(dev, &new, FRAME_DATA(new));
 			if (broken) {
@@ -397,8 +385,7 @@ int us_device_grab_buffer(us_device_s *dev, us_hw_buffer_s **hw) {
 #			undef GRABBED
 #			undef FRAME_DATA
 
-			memcpy(&buf, &new, sizeof(struct v4l2_buffer));
-
+			_v4l2_buffer_copy(&new, &buf);
 			buf_got = true;
 
 		} else {
@@ -414,7 +401,7 @@ int us_device_grab_buffer(us_device_s *dev, us_hw_buffer_s **hw) {
 		}
 	} while (true);
 
-#	define HW(x_next) (_RUN(hw_bufs)[buf.index].x_next)
+#	define HW(x_next) _RUN(hw_bufs)[buf.index].x_next
 	HW(raw.dma_fd) = HW(dma_fd);
 	HW(raw.used) = buf.bytesused;
 	HW(raw.width) = _RUN(width);
@@ -422,8 +409,8 @@ int us_device_grab_buffer(us_device_s *dev, us_hw_buffer_s **hw) {
 	HW(raw.format) = _RUN(format);
 	HW(raw.stride) = _RUN(stride);
 	HW(raw.online) = true;
-	_v4l2_buffer_copy(dev->capture_type, &HW(buf), &buf);
-	HW(raw.grab_ts)= (long double)((buf.timestamp.tv_sec * (uint64_t)1000) + (buf.timestamp.tv_usec / 1000)) / 1000;
+	_v4l2_buffer_copy(&buf, &HW(buf));
+	HW(raw.grab_ts) = (long double)((buf.timestamp.tv_sec * (uint64_t)1000) + (buf.timestamp.tv_usec / 1000)) / 1000;
 	US_LOG_DEBUG("Grabbed new frame: buffer=%u, bytesused=%u, grab_ts=%.3Lf, latency=%.3Lf, skipped=%u",
 		buf.index, buf.bytesused, HW(raw.grab_ts), us_get_now_monotonic() - HW(raw.grab_ts), skipped);
 #	undef HW
@@ -463,6 +450,16 @@ int us_device_consume_event(us_device_s *dev) {
 	return 0;
 }
 
+static void _v4l2_buffer_copy(const struct v4l2_buffer *src, struct v4l2_buffer *dest) {
+	struct v4l2_plane *dest_planes = dest->m.planes;
+	memcpy(dest, src, sizeof(struct v4l2_buffer));
+	if (src->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		assert(dest_planes);
+		dest->m.planes = dest_planes;
+		memcpy(dest->m.planes, src->m.planes, sizeof(struct v4l2_plane) * VIDEO_MAX_PLANES);
+	}
+}
+
 bool _device_is_buffer_valid(us_device_s *dev, const struct v4l2_buffer *buf, const uint8_t *data) {
 	// Workaround for broken, corrupted frames:
 	// Under low light conditions corrupted frames may get captured.
@@ -503,7 +500,7 @@ bool _device_is_buffer_valid(us_device_s *dev, const struct v4l2_buffer *buf, co
 }
 
 static int _device_open_check_cap(us_device_s *dev) {
-	struct v4l2_capability cap = {};
+	struct v4l2_capability cap = {0};
 
 	US_LOG_DEBUG("Querying device capabilities ...");
 	if (_D_XIOCTL(VIDIOC_QUERYCAP, &cap) < 0) {
@@ -512,9 +509,11 @@ static int _device_open_check_cap(us_device_s *dev) {
 	}
 
 	if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) {
-		dev->capture_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		_RUN(capture_type) = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		US_LOG_INFO("Using capture type: single-planar");
 	} else if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
-		dev->capture_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+		_RUN(capture_type) = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+		US_LOG_INFO("Using capture type: multi-planar");
 	} else {
 		US_LOG_ERROR("Video capture is not supported by device");
 		return -1;
@@ -525,7 +524,7 @@ static int _device_open_check_cap(us_device_s *dev) {
 		return -1;
 	}
 
-	if (dev->capture_type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+	if (!_D_IS_MPLANE) {
 		int input = dev->input; // Needs a pointer to int for ioctl()
 		US_LOG_INFO("Using input channel: %d", input);
 		if (_D_XIOCTL(VIDIOC_S_INPUT, &input) < 0) {
@@ -555,7 +554,7 @@ static int _device_open_dv_timings(us_device_s *dev) {
 			return -1;
 		}
 
-		struct v4l2_event_subscription sub = {};
+		struct v4l2_event_subscription sub = {0};
 		sub.type = V4L2_EVENT_SOURCE_CHANGE;
 
 		US_LOG_DEBUG("Subscribing to DV-timings events ...")
@@ -568,7 +567,7 @@ static int _device_open_dv_timings(us_device_s *dev) {
 }
 
 static int _device_apply_dv_timings(us_device_s *dev) {
-	struct v4l2_dv_timings dv = {};
+	struct v4l2_dv_timings dv = {0};
 
 	US_LOG_DEBUG("Calling us_xioctl(VIDIOC_QUERY_DV_TIMINGS) ...");
 	if (_D_XIOCTL(VIDIOC_QUERY_DV_TIMINGS, &dv) == 0) {
@@ -609,12 +608,12 @@ static int _device_apply_dv_timings(us_device_s *dev) {
 	return 0;
 }
 
-static int _device_open_format(us_device_s *dev, bool first) {
+static int _device_open_format(us_device_s *dev, bool first) { // FIXME
 	const unsigned stride = us_align_size(_RUN(width), 32) << 1;
 
-	struct v4l2_format fmt = {};
-	fmt.type = dev->capture_type;
-	if (dev->capture_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) { 
+	struct v4l2_format fmt = {0};
+	fmt.type = _RUN(capture_type);
+	if (_D_IS_MPLANE) {
 		fmt.fmt.pix_mp.width = _RUN(width);
 		fmt.fmt.pix_mp.height = _RUN(height);
 		fmt.fmt.pix_mp.pixelformat = dev->format;
@@ -637,8 +636,13 @@ static int _device_open_format(us_device_s *dev, bool first) {
 		return -1;
 	}
 
-#	define FMT(x_next) ( fmt.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ? fmt.fmt.pix_mp.x_next : fmt.fmt.pix.x_next )
-#	define FMTS(x_next) ( fmt.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ? fmt.fmt.pix_mp.plane_fmt[0].x_next : fmt.fmt.pix.x_next )
+	if (fmt.type != _RUN(capture_type)) {
+		US_LOG_ERROR("Capture format mismatch, please report to the developer");
+		return -1;
+	}
+
+#	define FMT(x_next)	(_D_IS_MPLANE ? fmt.fmt.pix_mp.x_next : fmt.fmt.pix.x_next)
+#	define FMTS(x_next)	(_D_IS_MPLANE ? fmt.fmt.pix_mp.plane_fmt[0].x_next : fmt.fmt.pix.x_next)
 
 	// Check resolution
 	bool retry = false;
@@ -652,7 +656,7 @@ static int _device_open_format(us_device_s *dev, bool first) {
 	if (first && retry) {
 		return _device_open_format(dev, false);
 	}
-	US_LOG_INFO("Using resolution: %ux%u, size:%u", _RUN(width), _RUN(height), _RUN(width)*_RUN(height)*3);
+	US_LOG_INFO("Using resolution: %ux%u", _RUN(width), _RUN(height));
 
 	// Check format
 	if (FMT(pixelformat) != dev->format) {
@@ -687,8 +691,8 @@ static int _device_open_format(us_device_s *dev, bool first) {
 static void _device_open_hw_fps(us_device_s *dev) {
 	_RUN(hw_fps) = 0;
 
-	struct v4l2_streamparm setfps = {};
-	setfps.type = dev->capture_type;
+	struct v4l2_streamparm setfps = {0};
+	setfps.type = _RUN(capture_type);
 
 	US_LOG_DEBUG("Querying HW FPS ...");
 	if (_D_XIOCTL(VIDIOC_G_PARM, &setfps) < 0) {
@@ -708,7 +712,7 @@ static void _device_open_hw_fps(us_device_s *dev) {
 #	define SETFPS_TPF(x_next) setfps.parm.capture.timeperframe.x_next
 
 	US_MEMSET_ZERO(setfps);
-	setfps.type = dev->capture_type;
+	setfps.type = _RUN(capture_type);
 	SETFPS_TPF(numerator) = 1;
 	SETFPS_TPF(denominator) = (dev->desired_fps == 0 ? 255 : dev->desired_fps);
 
@@ -741,7 +745,7 @@ static void _device_open_jpeg_quality(us_device_s *dev) {
 	unsigned quality = 0;
 
 	if (us_is_jpeg(_RUN(format))) {
-		struct v4l2_jpegcompression comp = {};
+		struct v4l2_jpegcompression comp = {0};
 
 		if (_D_XIOCTL(VIDIOC_G_JPEGCOMP, &comp) < 0) {
 			US_LOG_ERROR("Device doesn't support setting of HW encoding quality parameters");
@@ -769,9 +773,9 @@ static int _device_open_io_method(us_device_s *dev) {
 }
 
 static int _device_open_io_method_mmap(us_device_s *dev) {
-	struct v4l2_requestbuffers req = {};
+	struct v4l2_requestbuffers req = {0};
 	req.count = dev->n_bufs;
-	req.type = dev->capture_type;
+	req.type = _RUN(capture_type);
 	req.memory = V4L2_MEMORY_MMAP;
 
 	US_LOG_DEBUG("Requesting %u device buffers for MMAP ...", req.count);
@@ -791,12 +795,12 @@ static int _device_open_io_method_mmap(us_device_s *dev) {
 
 	US_CALLOC(_RUN(hw_bufs), req.count);
 	for (_RUN(n_bufs) = 0; _RUN(n_bufs) < req.count; ++_RUN(n_bufs)) {
-		struct v4l2_buffer buf = {};
-		struct v4l2_plane planes[VIDEO_MAX_PLANES];
-		buf.type = dev->capture_type;
+		struct v4l2_buffer buf = {0};
+		struct v4l2_plane planes[VIDEO_MAX_PLANES] = {0};
+		buf.type = _RUN(capture_type);
 		buf.memory = V4L2_MEMORY_MMAP;
 		buf.index = _RUN(n_bufs);
-		if (dev->capture_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		if (_D_IS_MPLANE) {
 			buf.m.planes = planes;
 			buf.length = VIDEO_MAX_PLANES;
 		}
@@ -807,34 +811,42 @@ static int _device_open_io_method_mmap(us_device_s *dev) {
 			return -1;
 		}
 
-#		define HW(x_next) (_RUN(hw_bufs)[_RUN(n_bufs)].x_next)
+#		define HW(x_next) _RUN(hw_bufs)[_RUN(n_bufs)].x_next
 
 		HW(dma_fd) = -1;
 
+		const size_t buf_size = (_D_IS_MPLANE ? buf.m.planes[0].length : buf.length);
+		const off_t buf_offset = (_D_IS_MPLANE ? buf.m.planes[0].m.mem_offset : buf.m.offset);
+
 		US_LOG_DEBUG("Mapping device buffer=%u ...", _RUN(n_bufs));
 		if ((HW(raw.data) = mmap(
 			NULL,
-			(buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ? buf.m.planes[0].length : buf.length),
+			buf_size,
 			PROT_READ | PROT_WRITE,
 			MAP_SHARED,
 			_RUN(fd),
-			(buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ? buf.m.planes[0].m.mem_offset : buf.m.offset)
+			buf_offset
 		)) == MAP_FAILED) {
 			US_LOG_PERROR("Can't map device buffer=%u", _RUN(n_bufs));
 			return -1;
 		}
 		assert(HW(raw.data) != NULL);
-		HW(raw.allocated) = (buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ? buf.m.planes[0].length : buf.length);
-		_v4l2_buffer_init(buf.type, &HW(buf));
+
+		HW(raw.allocated) = buf_size;
+
+		if (_D_IS_MPLANE) {
+			US_CALLOC(HW(buf.m.planes), VIDEO_MAX_PLANES);
+		}
+
 #		undef HW
 	}
 	return 0;
 }
 
 static int _device_open_io_method_userptr(us_device_s *dev) {
-	struct v4l2_requestbuffers req = {};
+	struct v4l2_requestbuffers req = {0};
 	req.count = dev->n_bufs;
-	req.type = dev->capture_type;
+	req.type = _RUN(capture_type);
 	req.memory = V4L2_MEMORY_USERPTR;
 
 	US_LOG_DEBUG("Requesting %u device buffers for USERPTR ...", req.count);
@@ -858,11 +870,13 @@ static int _device_open_io_method_userptr(us_device_s *dev) {
 	const unsigned buf_size = us_align_size(_RUN(raw_size), page_size);
 
 	for (_RUN(n_bufs) = 0; _RUN(n_bufs) < req.count; ++_RUN(n_bufs)) {
-#       define HW(x_next) (_RUN(hw_bufs)[_RUN(n_bufs)].x_next)
+#       define HW(x_next) _RUN(hw_bufs)[_RUN(n_bufs)].x_next
 		assert((HW(raw.data) = aligned_alloc(page_size, buf_size)) != NULL);
 		memset(HW(raw.data), 0, buf_size);
 		HW(raw.allocated) = buf_size;
-		_v4l2_buffer_init(dev->capture_type, &HW(buf));
+		if (_D_IS_MPLANE) {
+			US_CALLOC(HW(buf.m.planes), VIDEO_MAX_PLANES);
+		}
 #		undef HW
 	}
 	return 0;
@@ -870,18 +884,18 @@ static int _device_open_io_method_userptr(us_device_s *dev) {
 
 static int _device_open_queue_buffers(us_device_s *dev) {
 	for (unsigned index = 0; index < _RUN(n_bufs); ++index) {
-		struct v4l2_buffer buf = {};
-		struct v4l2_plane planes[VIDEO_MAX_PLANES];
-		buf.type = dev->capture_type;
+		struct v4l2_buffer buf = {0};
+		struct v4l2_plane planes[VIDEO_MAX_PLANES] = {0};
+		buf.type = _RUN(capture_type);
 		buf.memory = dev->io_method;
 		buf.index = index;
-		if (dev->capture_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		if (_D_IS_MPLANE) {
 			buf.m.planes = planes;
 			buf.length = 1;
 		}
 		
 		if (dev->io_method == V4L2_MEMORY_USERPTR) {
-			// i am not sure, may be this is incorrect for mplane device, 
+			// I am not sure, may be this is incorrect for mplane device, 
 			// but i don't have one which supports V4L2_MEMORY_USERPTR
 			buf.m.userptr = (unsigned long)_RUN(hw_bufs)[index].raw.data;
 			buf.length = _RUN(hw_bufs)[index].raw.allocated;
@@ -997,7 +1011,7 @@ static void _device_set_control(
 		return;
 	}
 
-	struct v4l2_control ctl = {};
+	struct v4l2_control ctl = {0};
 	ctl.id = cid;
 	ctl.value = value;
 
diff --git a/src/ustreamer/device.h b/src/ustreamer/device.h
index 5992ccee6..bb2d18f13 100644
--- a/src/ustreamer/device.h
+++ b/src/ustreamer/device.h
@@ -75,18 +75,19 @@ typedef struct {
 } us_hw_buffer_s;
 
 typedef struct {
-	int				fd;
-	unsigned		width;
-	unsigned		height;
-	unsigned		format;
-	unsigned		stride;
-	unsigned		hw_fps;
-	unsigned		jpeg_quality;
-	size_t			raw_size;
-	unsigned		n_bufs;
-	us_hw_buffer_s	*hw_bufs;
-	bool			capturing;
-	bool			persistent_timeout_reported;
+	int					fd;
+	unsigned			width;
+	unsigned			height;
+	unsigned			format;
+	unsigned			stride;
+	unsigned			hw_fps;
+	unsigned			jpeg_quality;
+	size_t				raw_size;
+	unsigned			n_bufs;
+	us_hw_buffer_s		*hw_bufs;
+	enum v4l2_buf_type	capture_type;
+	bool				capturing;
+	bool				persistent_timeout_reported;
 } us_device_runtime_s;
 
 typedef enum {
@@ -126,16 +127,13 @@ typedef struct {
 	unsigned			jpeg_quality;
 	v4l2_std_id			standard;
 	enum v4l2_memory	io_method;
-	enum v4l2_buf_type  capture_type;
 	bool				dv_timings;
 	unsigned			n_bufs;
 	unsigned			desired_fps;
 	size_t				min_frame_size;
 	bool				persistent;
 	unsigned			timeout;
-
 	us_controls_s 		ctl;
-
 	us_device_runtime_s *run;
 } us_device_s;
 
diff --git a/src/ustreamer/encoders/cpu/encoder.c b/src/ustreamer/encoders/cpu/encoder.c
index 41a064d00..b1f0a6f44 100644
--- a/src/ustreamer/encoders/cpu/encoder.c
+++ b/src/ustreamer/encoders/cpu/encoder.c
@@ -79,8 +79,8 @@ void us_cpu_encoder_compress(const us_frame_s *src, us_frame_s *dest, unsigned q
 		WRITE_SCANLINES(V4L2_PIX_FMT_YUYV, _jpeg_write_scanlines_yuyv);
 		WRITE_SCANLINES(V4L2_PIX_FMT_UYVY, _jpeg_write_scanlines_uyvy);
 		WRITE_SCANLINES(V4L2_PIX_FMT_RGB565, _jpeg_write_scanlines_rgb565);
-		WRITE_SCANLINES(V4L2_PIX_FMT_BGR24, _jpeg_write_scanlines_bgr24);
 		WRITE_SCANLINES(V4L2_PIX_FMT_RGB24, _jpeg_write_scanlines_rgb24);
+		WRITE_SCANLINES(V4L2_PIX_FMT_BGR24, _jpeg_write_scanlines_bgr24);
 		default: assert(0 && "Unsupported input format for CPU encoder");
 	}
 
@@ -222,6 +222,18 @@ static void _jpeg_write_scanlines_rgb565(struct jpeg_compress_struct *jpeg, cons
 	free(line_buf);
 }
 
+static void _jpeg_write_scanlines_rgb24(struct jpeg_compress_struct *jpeg, const us_frame_s *frame) {
+	const unsigned padding = us_frame_get_padding(frame);
+	uint8_t *data = frame->data;
+
+	while (jpeg->next_scanline < frame->height) {
+		JSAMPROW scanlines[1] = {data};
+		jpeg_write_scanlines(jpeg, scanlines, 1);
+
+		data += (frame->width * 3) + padding;
+	}
+}
+
 static void _jpeg_write_scanlines_bgr24(struct jpeg_compress_struct *jpeg, const us_frame_s *frame) {
 	uint8_t *line_buf;
 	US_CALLOC(line_buf, frame->width * 3);
@@ -233,9 +245,9 @@ static void _jpeg_write_scanlines_bgr24(struct jpeg_compress_struct *jpeg, const
 		uint8_t *ptr = line_buf;
 
 		// swap B and R values
-		for (unsigned x = 0; x < frame->width * 3; x+=3) {
-			*(ptr++) = data[x+2];
-			*(ptr++) = data[x+1];
+		for (unsigned x = 0; x < frame->width * 3; x += 3) {
+			*(ptr++) = data[x + 2];
+			*(ptr++) = data[x + 1];
 			*(ptr++) = data[x];
 		}
 		
@@ -248,18 +260,6 @@ static void _jpeg_write_scanlines_bgr24(struct jpeg_compress_struct *jpeg, const
 	free(line_buf);
 }
 
-static void _jpeg_write_scanlines_rgb24(struct jpeg_compress_struct *jpeg, const us_frame_s *frame) {
-	const unsigned padding = us_frame_get_padding(frame);
-	uint8_t *data = frame->data;
-
-	while (jpeg->next_scanline < frame->height) {
-		JSAMPROW scanlines[1] = {data};
-		jpeg_write_scanlines(jpeg, scanlines, 1);
-
-		data += (frame->width * 3) + padding;
-	}
-}
-
 #define JPEG_OUTPUT_BUFFER_SIZE ((size_t)4096)
 
 static void _jpeg_init_destination(j_compress_ptr jpeg) {