1 /*
2
3 # RGB and YUV crop routines
4
5 # (C) 2008 Hans de Goede <hdegoede@redhat.com>
6
7 # This program is free software; you can redistribute it and/or modify
8 # it under the terms of the GNU Lesser General Public License as published by
9 # the Free Software Foundation; either version 2.1 of the License, or
10 # (at your option) any later version.
11 #
12 # This program is distributed in the hope that it will be useful,
13 # but WITHOUT ANY WARRANTY; without even the implied warranty of
14 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 # GNU Lesser General Public License for more details.
16 #
17 # You should have received a copy of the GNU Lesser General Public License
18 # along with this program; if not, write to the Free Software
19 # Foundation, Inc., 51 Franklin Street, Suite 500, Boston, MA 02110-1335 USA
20
21 */
22
23 #include <string.h>
24 #include "libv4lconvert-priv.h"
25
26
v4lconvert_reduceandcrop_rgbbgr24(unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt,const struct v4l2_format * dest_fmt)27 static void v4lconvert_reduceandcrop_rgbbgr24(
28 unsigned char *src, unsigned char *dest,
29 const struct v4l2_format *src_fmt, const struct v4l2_format *dest_fmt)
30 {
31 int x, y;
32 int startx = src_fmt->fmt.pix.width / 2 - dest_fmt->fmt.pix.width;
33 int starty = src_fmt->fmt.pix.height / 2 - dest_fmt->fmt.pix.height;
34
35 src += starty * src_fmt->fmt.pix.bytesperline + 3 * startx;
36
37 for (y = 0; y < dest_fmt->fmt.pix.height; y++) {
38 unsigned char *mysrc = src;
39 for (x = 0; x < dest_fmt->fmt.pix.width; x++) {
40 *(dest++) = *(mysrc++);
41 *(dest++) = *(mysrc++);
42 *(dest++) = *(mysrc++);
43 mysrc += 3; /* skip one pixel */
44 }
45 src += 2 * src_fmt->fmt.pix.bytesperline; /* skip one line */
46 }
47 }
48
v4lconvert_crop_rgbbgr24(unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt,const struct v4l2_format * dest_fmt)49 static void v4lconvert_crop_rgbbgr24(unsigned char *src, unsigned char *dest,
50 const struct v4l2_format *src_fmt, const struct v4l2_format *dest_fmt)
51 {
52 int x;
53 int startx = (src_fmt->fmt.pix.width - dest_fmt->fmt.pix.width) / 2;
54 int starty = (src_fmt->fmt.pix.height - dest_fmt->fmt.pix.height) / 2;
55
56 src += starty * src_fmt->fmt.pix.bytesperline + 3 * startx;
57
58 for (x = 0; x < dest_fmt->fmt.pix.height; x++) {
59 memcpy(dest, src, dest_fmt->fmt.pix.width * 3);
60 src += src_fmt->fmt.pix.bytesperline;
61 dest += dest_fmt->fmt.pix.bytesperline;
62 }
63 }
64
v4lconvert_reduceandcrop_yuv420(unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt,const struct v4l2_format * dest_fmt)65 static void v4lconvert_reduceandcrop_yuv420(
66 unsigned char *src, unsigned char *dest,
67 const struct v4l2_format *src_fmt, const struct v4l2_format *dest_fmt)
68 {
69 int x, y;
70 int dest_height_half = dest_fmt->fmt.pix.height / 2;
71 int dest_width_half = dest_fmt->fmt.pix.width / 2;
72 int startx = (src_fmt->fmt.pix.width / 2 - dest_fmt->fmt.pix.width) & ~1;
73 int starty = (src_fmt->fmt.pix.height / 2 - dest_fmt->fmt.pix.height) & ~1;
74 unsigned char *mysrc, *mysrc2;
75
76 /* Y */
77 mysrc = src + starty * src_fmt->fmt.pix.bytesperline + startx;
78 for (y = 0; y < dest_fmt->fmt.pix.height; y++) {
79 mysrc2 = mysrc;
80 for (x = 0; x < dest_fmt->fmt.pix.width; x++) {
81 *(dest++) = *mysrc2;
82 mysrc2 += 2; /* skip one pixel */
83 }
84 mysrc += 2 * src_fmt->fmt.pix.bytesperline; /* skip one line */
85 }
86
87 /* U */
88 mysrc = src + src_fmt->fmt.pix.height * src_fmt->fmt.pix.bytesperline +
89 (starty / 2) * src_fmt->fmt.pix.bytesperline / 2 + startx / 2;
90 for (y = 0; y < dest_height_half; y++) {
91 mysrc2 = mysrc;
92 for (x = 0; x < dest_width_half; x++) {
93 *(dest++) = *mysrc2;
94 mysrc2 += 2; /* skip one pixel */
95 }
96 mysrc += src_fmt->fmt.pix.bytesperline ; /* skip one line */
97 }
98
99 /* V */
100 mysrc = src + src_fmt->fmt.pix.height * src_fmt->fmt.pix.bytesperline * 5 / 4
101 + (starty / 2) * src_fmt->fmt.pix.bytesperline / 2 + startx / 2;
102 for (y = 0; y < dest_height_half; y++) {
103 mysrc2 = mysrc;
104 for (x = 0; x < dest_width_half; x++) {
105 *(dest++) = *mysrc2;
106 mysrc2 += 2; /* skip one pixel */
107 }
108 mysrc += src_fmt->fmt.pix.bytesperline ; /* skip one line */
109 }
110 }
111
v4lconvert_crop_yuv420(unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt,const struct v4l2_format * dest_fmt)112 static void v4lconvert_crop_yuv420(unsigned char *src, unsigned char *dest,
113 const struct v4l2_format *src_fmt, const struct v4l2_format *dest_fmt)
114 {
115 int x;
116 int startx = ((src_fmt->fmt.pix.width - dest_fmt->fmt.pix.width) / 2) & ~1;
117 int starty = ((src_fmt->fmt.pix.height - dest_fmt->fmt.pix.height) / 2) & ~1;
118 unsigned char *mysrc = src + starty * src_fmt->fmt.pix.bytesperline + startx;
119
120 /* Y */
121 for (x = 0; x < dest_fmt->fmt.pix.height; x++) {
122 memcpy(dest, mysrc, dest_fmt->fmt.pix.width);
123 mysrc += src_fmt->fmt.pix.bytesperline;
124 dest += dest_fmt->fmt.pix.bytesperline;
125 }
126
127 /* U */
128 mysrc = src + src_fmt->fmt.pix.height * src_fmt->fmt.pix.bytesperline +
129 (starty / 2) * src_fmt->fmt.pix.bytesperline / 2 + startx / 2;
130 for (x = 0; x < dest_fmt->fmt.pix.height / 2; x++) {
131 memcpy(dest, mysrc, dest_fmt->fmt.pix.width / 2);
132 mysrc += src_fmt->fmt.pix.bytesperline / 2;
133 dest += dest_fmt->fmt.pix.bytesperline / 2;
134 }
135
136 /* V */
137 mysrc = src + src_fmt->fmt.pix.height * src_fmt->fmt.pix.bytesperline * 5 / 4
138 + (starty / 2) * src_fmt->fmt.pix.bytesperline / 2 + startx / 2;
139 for (x = 0; x < dest_fmt->fmt.pix.height / 2; x++) {
140 memcpy(dest, mysrc, dest_fmt->fmt.pix.width / 2);
141 mysrc += src_fmt->fmt.pix.bytesperline / 2;
142 dest += dest_fmt->fmt.pix.bytesperline / 2;
143 }
144 }
145
146 /* Ok, so this is not really cropping, but more the reverse, whatever */
v4lconvert_add_border_rgbbgr24(unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt,const struct v4l2_format * dest_fmt)147 static void v4lconvert_add_border_rgbbgr24(
148 unsigned char *src, unsigned char *dest,
149 const struct v4l2_format *src_fmt, const struct v4l2_format *dest_fmt)
150 {
151 int y;
152 int borderx = (dest_fmt->fmt.pix.width - src_fmt->fmt.pix.width) / 2;
153 int bordery = (dest_fmt->fmt.pix.height - src_fmt->fmt.pix.height) / 2;
154
155 for (y = 0; y < bordery; y++) {
156 memset(dest, 0, dest_fmt->fmt.pix.width * 3);
157 dest += dest_fmt->fmt.pix.bytesperline;
158 }
159
160 for (y = 0; y < src_fmt->fmt.pix.height; y++) {
161 memset(dest, 0, borderx * 3);
162 dest += borderx * 3;
163
164 memcpy(dest, src, src_fmt->fmt.pix.width * 3);
165 src += src_fmt->fmt.pix.bytesperline;
166 dest += src_fmt->fmt.pix.width * 3;
167
168 memset(dest, 0, borderx * 3);
169 dest += dest_fmt->fmt.pix.bytesperline -
170 (borderx + src_fmt->fmt.pix.width) * 3;
171 }
172
173 for (y = 0; y < bordery; y++) {
174 memset(dest, 0, dest_fmt->fmt.pix.width * 3);
175 dest += dest_fmt->fmt.pix.bytesperline;
176 }
177 }
178
v4lconvert_add_border_yuv420(unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt,const struct v4l2_format * dest_fmt)179 static void v4lconvert_add_border_yuv420(
180 unsigned char *src, unsigned char *dest,
181 const struct v4l2_format *src_fmt, const struct v4l2_format *dest_fmt)
182 {
183 int y;
184 int borderx = ((dest_fmt->fmt.pix.width - src_fmt->fmt.pix.width) / 2) & ~1;
185 int bordery = ((dest_fmt->fmt.pix.height - src_fmt->fmt.pix.height) / 2) & ~1;
186
187 /* Y */
188 for (y = 0; y < bordery; y++) {
189 memset(dest, 16, dest_fmt->fmt.pix.width);
190 dest += dest_fmt->fmt.pix.bytesperline;
191 }
192
193 for (y = 0; y < src_fmt->fmt.pix.height; y++) {
194 memset(dest, 16, borderx);
195 dest += borderx;
196
197 memcpy(dest, src, src_fmt->fmt.pix.width);
198 src += src_fmt->fmt.pix.bytesperline;
199 dest += src_fmt->fmt.pix.width;
200
201 memset(dest, 16, borderx);
202 dest += dest_fmt->fmt.pix.bytesperline -
203 (borderx + src_fmt->fmt.pix.width);
204 }
205
206 for (y = 0; y < bordery; y++) {
207 memset(dest, 16, dest_fmt->fmt.pix.width);
208 dest += dest_fmt->fmt.pix.bytesperline;
209 }
210
211 /* U */
212 for (y = 0; y < bordery / 2; y++) {
213 memset(dest, 128, dest_fmt->fmt.pix.width / 2);
214 dest += dest_fmt->fmt.pix.bytesperline / 2;
215 }
216
217 for (y = 0; y < src_fmt->fmt.pix.height / 2; y++) {
218 memset(dest, 128, borderx / 2);
219 dest += borderx / 2;
220
221 memcpy(dest, src, src_fmt->fmt.pix.width / 2);
222 src += src_fmt->fmt.pix.bytesperline / 2;
223 dest += src_fmt->fmt.pix.width / 2;
224
225 memset(dest, 128, borderx / 2);
226 dest += (dest_fmt->fmt.pix.bytesperline -
227 (borderx + src_fmt->fmt.pix.width)) / 2;
228 }
229
230 for (y = 0; y < bordery / 2; y++) {
231 memset(dest, 128, dest_fmt->fmt.pix.width / 2);
232 dest += dest_fmt->fmt.pix.bytesperline / 2;
233 }
234
235 /* V */
236 for (y = 0; y < bordery / 2; y++) {
237 memset(dest, 128, dest_fmt->fmt.pix.width / 2);
238 dest += dest_fmt->fmt.pix.bytesperline / 2;
239 }
240
241 for (y = 0; y < src_fmt->fmt.pix.height / 2; y++) {
242 memset(dest, 128, borderx / 2);
243 dest += borderx / 2;
244
245 memcpy(dest, src, src_fmt->fmt.pix.width / 2);
246 src += src_fmt->fmt.pix.bytesperline / 2;
247 dest += src_fmt->fmt.pix.width / 2;
248
249 memset(dest, 128, borderx / 2);
250 dest += (dest_fmt->fmt.pix.bytesperline -
251 (borderx + src_fmt->fmt.pix.width)) / 2;
252 }
253
254 for (y = 0; y < bordery / 2; y++) {
255 memset(dest, 128, dest_fmt->fmt.pix.width / 2);
256 dest += dest_fmt->fmt.pix.bytesperline / 2;
257 }
258 }
259
v4lconvert_crop(unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt,const struct v4l2_format * dest_fmt)260 void v4lconvert_crop(unsigned char *src, unsigned char *dest,
261 const struct v4l2_format *src_fmt, const struct v4l2_format *dest_fmt)
262 {
263 switch (dest_fmt->fmt.pix.pixelformat) {
264 case V4L2_PIX_FMT_RGB24:
265 case V4L2_PIX_FMT_BGR24:
266 if (src_fmt->fmt.pix.width <= dest_fmt->fmt.pix.width &&
267 src_fmt->fmt.pix.height <= dest_fmt->fmt.pix.height)
268 v4lconvert_add_border_rgbbgr24(src, dest, src_fmt, dest_fmt);
269 else if (src_fmt->fmt.pix.width >= 2 * dest_fmt->fmt.pix.width &&
270 src_fmt->fmt.pix.height >= 2 * dest_fmt->fmt.pix.height)
271 v4lconvert_reduceandcrop_rgbbgr24(src, dest, src_fmt, dest_fmt);
272 else
273 v4lconvert_crop_rgbbgr24(src, dest, src_fmt, dest_fmt);
274 break;
275
276 case V4L2_PIX_FMT_YUV420:
277 case V4L2_PIX_FMT_YVU420:
278 if (src_fmt->fmt.pix.width <= dest_fmt->fmt.pix.width &&
279 src_fmt->fmt.pix.height <= dest_fmt->fmt.pix.height)
280 v4lconvert_add_border_yuv420(src, dest, src_fmt, dest_fmt);
281 else if (src_fmt->fmt.pix.width >= 2 * dest_fmt->fmt.pix.width &&
282 src_fmt->fmt.pix.height >= 2 * dest_fmt->fmt.pix.height)
283 v4lconvert_reduceandcrop_yuv420(src, dest, src_fmt, dest_fmt);
284 else
285 v4lconvert_crop_yuv420(src, dest, src_fmt, dest_fmt);
286 break;
287 }
288 }
289