1 // SPDX-License-Identifier: GPL-2.0+
2
3 #include <linux/kernel.h>
4 #include <linux/minmax.h>
5
6 #include <drm/drm_blend.h>
7 #include <drm/drm_rect.h>
8 #include <drm/drm_fixed.h>
9
10 #include <kunit/visibility.h>
11
12 #include "vkms_formats.h"
13
14 /**
15 * packed_pixels_offset() - Get the offset of the block containing the pixel at coordinates x/y
16 *
17 * @frame_info: Buffer metadata
18 * @x: The x coordinate of the wanted pixel in the buffer
19 * @y: The y coordinate of the wanted pixel in the buffer
20 * @plane_index: The index of the plane to use
21 * @offset: The returned offset inside the buffer of the block
22 * @rem_x: The returned X coordinate of the requested pixel in the block
23 * @rem_y: The returned Y coordinate of the requested pixel in the block
24 *
25 * As some pixel formats store multiple pixels in a block (DRM_FORMAT_R* for example), some
26 * pixels are not individually addressable. This function return 3 values: the offset of the
27 * whole block, and the coordinate of the requested pixel inside this block.
28 * For example, if the format is DRM_FORMAT_R1 and the requested coordinate is 13,5, the offset
29 * will point to the byte 5*pitches + 13/8 (second byte of the 5th line), and the rem_x/rem_y
30 * coordinates will be (13 % 8, 5 % 1) = (5, 0)
31 *
32 * With this function, the caller just have to extract the correct pixel from the block.
33 */
packed_pixels_offset(const struct vkms_frame_info * frame_info,int x,int y,int plane_index,int * offset,int * rem_x,int * rem_y)34 static void packed_pixels_offset(const struct vkms_frame_info *frame_info, int x, int y,
35 int plane_index, int *offset, int *rem_x, int *rem_y)
36 {
37 struct drm_framebuffer *fb = frame_info->fb;
38 const struct drm_format_info *format = frame_info->fb->format;
39 /* Directly using x and y to multiply pitches and format->ccp is not sufficient because
40 * in some formats a block can represent multiple pixels.
41 *
42 * Dividing x and y by the block size allows to extract the correct offset of the block
43 * containing the pixel.
44 */
45
46 int block_x = x / drm_format_info_block_width(format, plane_index);
47 int block_y = y / drm_format_info_block_height(format, plane_index);
48 int block_pitch = fb->pitches[plane_index] * drm_format_info_block_height(format,
49 plane_index);
50 *rem_x = x % drm_format_info_block_width(format, plane_index);
51 *rem_y = y % drm_format_info_block_height(format, plane_index);
52 *offset = fb->offsets[plane_index] +
53 block_y * block_pitch +
54 block_x * format->char_per_block[plane_index];
55 }
56
57 /**
58 * packed_pixels_addr() - Get the pointer to the block containing the pixel at the given
59 * coordinates
60 *
61 * @frame_info: Buffer metadata
62 * @x: The x (width) coordinate inside the plane
63 * @y: The y (height) coordinate inside the plane
64 * @plane_index: The index of the plane
65 * @addr: The returned pointer
66 * @rem_x: The returned X coordinate of the requested pixel in the block
67 * @rem_y: The returned Y coordinate of the requested pixel in the block
68 *
69 * Takes the information stored in the frame_info, a pair of coordinates, and returns the address
70 * of the block containing this pixel and the pixel position inside this block.
71 *
72 * See @packed_pixels_offset for details about rem_x/rem_y behavior.
73 */
packed_pixels_addr(const struct vkms_frame_info * frame_info,int x,int y,int plane_index,u8 ** addr,int * rem_x,int * rem_y)74 static void packed_pixels_addr(const struct vkms_frame_info *frame_info,
75 int x, int y, int plane_index, u8 **addr, int *rem_x,
76 int *rem_y)
77 {
78 int offset;
79
80 packed_pixels_offset(frame_info, x, y, plane_index, &offset, rem_x, rem_y);
81 *addr = (u8 *)frame_info->map[0].vaddr + offset;
82 }
83
84 /**
85 * get_block_step_bytes() - Common helper to compute the correct step value between each pixel block
86 * to read in a certain direction.
87 *
88 * @fb: Framebuffer to iter on
89 * @direction: Direction of the reading
90 * @plane_index: Plane to get the step from
91 *
92 * As the returned count is the number of bytes between two consecutive blocks in a direction,
93 * the caller may have to read multiple pixels before using the next one (for example, to read from
94 * left to right in a DRM_FORMAT_R1 plane, each block contains 8 pixels, so the step must be used
95 * only every 8 pixels).
96 */
get_block_step_bytes(struct drm_framebuffer * fb,enum pixel_read_direction direction,int plane_index)97 static int get_block_step_bytes(struct drm_framebuffer *fb, enum pixel_read_direction direction,
98 int plane_index)
99 {
100 switch (direction) {
101 case READ_LEFT_TO_RIGHT:
102 return fb->format->char_per_block[plane_index];
103 case READ_RIGHT_TO_LEFT:
104 return -fb->format->char_per_block[plane_index];
105 case READ_TOP_TO_BOTTOM:
106 return (int)fb->pitches[plane_index] * drm_format_info_block_width(fb->format,
107 plane_index);
108 case READ_BOTTOM_TO_TOP:
109 return -(int)fb->pitches[plane_index] * drm_format_info_block_width(fb->format,
110 plane_index);
111 }
112
113 return 0;
114 }
115
116 /**
117 * packed_pixels_addr_1x1() - Get the pointer to the block containing the pixel at the given
118 * coordinates
119 *
120 * @frame_info: Buffer metadata
121 * @x: The x (width) coordinate inside the plane
122 * @y: The y (height) coordinate inside the plane
123 * @plane_index: The index of the plane
124 * @addr: The returned pointer
125 *
126 * This function can only be used with format where block_h == block_w == 1.
127 */
packed_pixels_addr_1x1(const struct vkms_frame_info * frame_info,int x,int y,int plane_index,u8 ** addr)128 static void packed_pixels_addr_1x1(const struct vkms_frame_info *frame_info,
129 int x, int y, int plane_index, u8 **addr)
130 {
131 int offset, rem_x, rem_y;
132
133 WARN_ONCE(drm_format_info_block_width(frame_info->fb->format,
134 plane_index) != 1,
135 "%s() only support formats with block_w == 1", __func__);
136 WARN_ONCE(drm_format_info_block_height(frame_info->fb->format,
137 plane_index) != 1,
138 "%s() only support formats with block_h == 1", __func__);
139
140 packed_pixels_offset(frame_info, x, y, plane_index, &offset, &rem_x,
141 &rem_y);
142 *addr = (u8 *)frame_info->map[0].vaddr + offset;
143 }
144
145 /**
146 * get_subsampling() - Get the subsampling divisor value on a specific direction
147 *
148 * @format: format to extarct the subsampling from
149 * @direction: direction of the subsampling requested
150 */
get_subsampling(const struct drm_format_info * format,enum pixel_read_direction direction)151 static int get_subsampling(const struct drm_format_info *format,
152 enum pixel_read_direction direction)
153 {
154 switch (direction) {
155 case READ_BOTTOM_TO_TOP:
156 case READ_TOP_TO_BOTTOM:
157 return format->vsub;
158 case READ_RIGHT_TO_LEFT:
159 case READ_LEFT_TO_RIGHT:
160 return format->hsub;
161 }
162 WARN_ONCE(true, "Invalid direction for pixel reading: %d\n", direction);
163 return 1;
164 }
165
166 /**
167 * get_subsampling_offset() - An offset for keeping the chroma siting consistent regardless of
168 * x_start and y_start values
169 *
170 * @direction: direction of the reading to properly compute this offset
171 * @x_start: x coordinate of the starting point of the readed line
172 * @y_start: y coordinate of the starting point of the readed line
173 */
get_subsampling_offset(enum pixel_read_direction direction,int x_start,int y_start)174 static int get_subsampling_offset(enum pixel_read_direction direction, int x_start, int y_start)
175 {
176 switch (direction) {
177 case READ_BOTTOM_TO_TOP:
178 return -y_start - 1;
179 case READ_TOP_TO_BOTTOM:
180 return y_start;
181 case READ_RIGHT_TO_LEFT:
182 return -x_start - 1;
183 case READ_LEFT_TO_RIGHT:
184 return x_start;
185 }
186 WARN_ONCE(true, "Invalid direction for pixel reading: %d\n", direction);
187 return 0;
188 }
189
190 /*
191 * The following functions take pixel data (a, r, g, b, pixel, ...) and convert them to
192 * &struct pixel_argb_u16
193 *
194 * They are used in the `read_line`s functions to avoid duplicate work for some pixel formats.
195 */
196
argb_u16_from_u8888(u8 a,u8 r,u8 g,u8 b)197 static struct pixel_argb_u16 argb_u16_from_u8888(u8 a, u8 r, u8 g, u8 b)
198 {
199 struct pixel_argb_u16 out_pixel;
200 /*
201 * The 257 is the "conversion ratio". This number is obtained by the
202 * (2^16 - 1) / (2^8 - 1) division. Which, in this case, tries to get
203 * the best color value in a pixel format with more possibilities.
204 * A similar idea applies to others RGB color conversions.
205 */
206 out_pixel.a = (u16)a * 257;
207 out_pixel.r = (u16)r * 257;
208 out_pixel.g = (u16)g * 257;
209 out_pixel.b = (u16)b * 257;
210
211 return out_pixel;
212 }
213
argb_u16_from_u16161616(u16 a,u16 r,u16 g,u16 b)214 static struct pixel_argb_u16 argb_u16_from_u16161616(u16 a, u16 r, u16 g, u16 b)
215 {
216 struct pixel_argb_u16 out_pixel;
217
218 out_pixel.a = a;
219 out_pixel.r = r;
220 out_pixel.g = g;
221 out_pixel.b = b;
222
223 return out_pixel;
224 }
225
argb_u16_from_le16161616(__le16 a,__le16 r,__le16 g,__le16 b)226 static struct pixel_argb_u16 argb_u16_from_le16161616(__le16 a, __le16 r, __le16 g, __le16 b)
227 {
228 return argb_u16_from_u16161616(le16_to_cpu(a), le16_to_cpu(r), le16_to_cpu(g),
229 le16_to_cpu(b));
230 }
231
argb_u16_from_RGB565(const __le16 * pixel)232 static struct pixel_argb_u16 argb_u16_from_RGB565(const __le16 *pixel)
233 {
234 struct pixel_argb_u16 out_pixel;
235
236 s64 fp_rb_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(31));
237 s64 fp_g_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(63));
238
239 u16 rgb_565 = le16_to_cpu(*pixel);
240 s64 fp_r = drm_int2fixp((rgb_565 >> 11) & 0x1f);
241 s64 fp_g = drm_int2fixp((rgb_565 >> 5) & 0x3f);
242 s64 fp_b = drm_int2fixp(rgb_565 & 0x1f);
243
244 out_pixel.a = (u16)0xffff;
245 out_pixel.r = drm_fixp2int_round(drm_fixp_mul(fp_r, fp_rb_ratio));
246 out_pixel.g = drm_fixp2int_round(drm_fixp_mul(fp_g, fp_g_ratio));
247 out_pixel.b = drm_fixp2int_round(drm_fixp_mul(fp_b, fp_rb_ratio));
248
249 return out_pixel;
250 }
251
argb_u16_from_gray8(u8 gray)252 static struct pixel_argb_u16 argb_u16_from_gray8(u8 gray)
253 {
254 return argb_u16_from_u8888(255, gray, gray, gray);
255 }
256
argb_u16_from_grayu16(u16 gray)257 static struct pixel_argb_u16 argb_u16_from_grayu16(u16 gray)
258 {
259 return argb_u16_from_u16161616(0xFFFF, gray, gray, gray);
260 }
261
argb_u16_from_BGR565(const __le16 * pixel)262 static struct pixel_argb_u16 argb_u16_from_BGR565(const __le16 *pixel)
263 {
264 struct pixel_argb_u16 out_pixel;
265
266 out_pixel = argb_u16_from_RGB565(pixel);
267 swap(out_pixel.r, out_pixel.b);
268
269 return out_pixel;
270 }
271
272 VISIBLE_IF_KUNIT
argb_u16_from_yuv161616(const struct conversion_matrix * matrix,u16 y,u16 channel_1,u16 channel_2)273 struct pixel_argb_u16 argb_u16_from_yuv161616(const struct conversion_matrix *matrix,
274 u16 y, u16 channel_1, u16 channel_2)
275 {
276 u16 r, g, b;
277 s64 fp_y, fp_channel_1, fp_channel_2;
278 s64 fp_r, fp_g, fp_b;
279
280 fp_y = drm_int2fixp((int)y - matrix->y_offset * 257);
281 fp_channel_1 = drm_int2fixp((int)channel_1 - 128 * 257);
282 fp_channel_2 = drm_int2fixp((int)channel_2 - 128 * 257);
283
284 fp_r = drm_fixp_mul(matrix->matrix[0][0], fp_y) +
285 drm_fixp_mul(matrix->matrix[0][1], fp_channel_1) +
286 drm_fixp_mul(matrix->matrix[0][2], fp_channel_2);
287 fp_g = drm_fixp_mul(matrix->matrix[1][0], fp_y) +
288 drm_fixp_mul(matrix->matrix[1][1], fp_channel_1) +
289 drm_fixp_mul(matrix->matrix[1][2], fp_channel_2);
290 fp_b = drm_fixp_mul(matrix->matrix[2][0], fp_y) +
291 drm_fixp_mul(matrix->matrix[2][1], fp_channel_1) +
292 drm_fixp_mul(matrix->matrix[2][2], fp_channel_2);
293
294 fp_r = drm_fixp2int_round(fp_r);
295 fp_g = drm_fixp2int_round(fp_g);
296 fp_b = drm_fixp2int_round(fp_b);
297
298 r = clamp(fp_r, 0, 0xffff);
299 g = clamp(fp_g, 0, 0xffff);
300 b = clamp(fp_b, 0, 0xffff);
301
302 return argb_u16_from_u16161616(0xffff, r, g, b);
303 }
304 EXPORT_SYMBOL_IF_KUNIT(argb_u16_from_yuv161616);
305
306 /**
307 * READ_LINE() - Generic generator for a read_line function which can be used for format with one
308 * plane and a block_h == block_w == 1.
309 *
310 * @function_name: Function name to generate
311 * @pixel_name: Temporary pixel name used in the @__VA_ARGS__ parameters
312 * @pixel_type: Used to specify the type you want to cast the pixel pointer
313 * @callback: Callback to call for each pixels. This fonction should take @__VA_ARGS__ as parameter
314 * and return a pixel_argb_u16
315 * __VA_ARGS__: Argument to pass inside the callback. You can use @pixel_name to access current
316 * pixel.
317 */
318 #define READ_LINE(function_name, pixel_name, pixel_type, callback, ...) \
319 static void function_name(const struct vkms_plane_state *plane, int x_start, \
320 int y_start, enum pixel_read_direction direction, int count, \
321 struct pixel_argb_u16 out_pixel[]) \
322 { \
323 struct pixel_argb_u16 *end = out_pixel + count; \
324 int step = get_block_step_bytes(plane->frame_info->fb, direction, 0); \
325 u8 *src_pixels; \
326 \
327 packed_pixels_addr_1x1(plane->frame_info, x_start, y_start, 0, &src_pixels); \
328 \
329 while (out_pixel < end) { \
330 pixel_type *(pixel_name) = (pixel_type *)src_pixels; \
331 *out_pixel = (callback)(__VA_ARGS__); \
332 out_pixel += 1; \
333 src_pixels += step; \
334 } \
335 }
336
337 /**
338 * READ_LINE_ARGB8888() - Generic generator for ARGB8888 formats.
339 * The pixel type used is u8, so pixel_name[0]..pixel_name[n] are the n components of the pixel.
340 *
341 * @function_name: Function name to generate
342 * @pixel_name: temporary pixel to use in @a, @r, @g and @b parameters
343 * @a: alpha value
344 * @r: red value
345 * @g: green value
346 * @b: blue value
347 */
348 #define READ_LINE_ARGB8888(function_name, pixel_name, a, r, g, b) \
349 READ_LINE(function_name, pixel_name, u8, argb_u16_from_u8888, a, r, g, b)
350 /**
351 * READ_LINE_le16161616() - Generic generator for ARGB16161616 formats.
352 * The pixel type used is u16, so pixel_name[0]..pixel_name[n] are the n components of the pixel.
353 *
354 * @function_name: Function name to generate
355 * @pixel_name: temporary pixel to use in @a, @r, @g and @b parameters
356 * @a: alpha value
357 * @r: red value
358 * @g: green value
359 * @b: blue value
360 */
361 #define READ_LINE_le16161616(function_name, pixel_name, a, r, g, b) \
362 READ_LINE(function_name, pixel_name, __le16, argb_u16_from_le16161616, a, r, g, b)
363
364 /*
365 * The following functions are read_line function for each pixel format supported by VKMS.
366 *
367 * They read a line starting at the point @x_start,@y_start following the @direction. The result
368 * is stored in @out_pixel and in a 64 bits format, see struct pixel_argb_u16.
369 *
370 * These functions are very repetitive, but the innermost pixel loops must be kept inside these
371 * functions for performance reasons. Some benchmarking was done in [1] where having the innermost
372 * loop factored out of these functions showed a slowdown by a factor of three.
373 *
374 * [1]: https://lore.kernel.org/dri-devel/d258c8dc-78e9-4509-9037-a98f7f33b3a3@riseup.net/
375 */
376
Rx_read_line(const struct vkms_plane_state * plane,int x_start,int y_start,enum pixel_read_direction direction,int count,struct pixel_argb_u16 out_pixel[])377 static void Rx_read_line(const struct vkms_plane_state *plane, int x_start,
378 int y_start, enum pixel_read_direction direction, int count,
379 struct pixel_argb_u16 out_pixel[])
380 {
381 struct pixel_argb_u16 *end = out_pixel + count;
382 int bits_per_pixel = drm_format_info_bpp(plane->frame_info->fb->format, 0);
383 u8 *src_pixels;
384 int rem_x, rem_y;
385
386 WARN_ONCE(drm_format_info_block_height(plane->frame_info->fb->format, 0) != 1,
387 "%s() only support formats with block_h == 1", __func__);
388
389 packed_pixels_addr(plane->frame_info, x_start, y_start, 0, &src_pixels, &rem_x, &rem_y);
390 int bit_offset = (8 - bits_per_pixel) - rem_x * bits_per_pixel;
391 int step = get_block_step_bytes(plane->frame_info->fb, direction, 0);
392 int mask = (0x1 << bits_per_pixel) - 1;
393 int lum_per_level = 0xFFFF / mask;
394
395 if (direction == READ_LEFT_TO_RIGHT || direction == READ_RIGHT_TO_LEFT) {
396 int restart_bit_offset;
397 int step_bit_offset;
398
399 if (direction == READ_LEFT_TO_RIGHT) {
400 restart_bit_offset = 8 - bits_per_pixel;
401 step_bit_offset = -bits_per_pixel;
402 } else {
403 restart_bit_offset = 0;
404 step_bit_offset = bits_per_pixel;
405 }
406
407 while (out_pixel < end) {
408 u8 val = ((*src_pixels) >> bit_offset) & mask;
409
410 *out_pixel = argb_u16_from_grayu16((int)val * lum_per_level);
411
412 bit_offset += step_bit_offset;
413 if (bit_offset < 0 || 8 <= bit_offset) {
414 bit_offset = restart_bit_offset;
415 src_pixels += step;
416 }
417 out_pixel += 1;
418 }
419 } else if (direction == READ_TOP_TO_BOTTOM || direction == READ_BOTTOM_TO_TOP) {
420 while (out_pixel < end) {
421 u8 val = (*src_pixels >> bit_offset) & mask;
422 *out_pixel = argb_u16_from_grayu16((int)val * lum_per_level);
423 src_pixels += step;
424 out_pixel += 1;
425 }
426 }
427 }
428
R1_read_line(const struct vkms_plane_state * plane,int x_start,int y_start,enum pixel_read_direction direction,int count,struct pixel_argb_u16 out_pixel[])429 static void R1_read_line(const struct vkms_plane_state *plane, int x_start,
430 int y_start, enum pixel_read_direction direction, int count,
431 struct pixel_argb_u16 out_pixel[])
432 {
433 Rx_read_line(plane, x_start, y_start, direction, count, out_pixel);
434 }
435
R2_read_line(const struct vkms_plane_state * plane,int x_start,int y_start,enum pixel_read_direction direction,int count,struct pixel_argb_u16 out_pixel[])436 static void R2_read_line(const struct vkms_plane_state *plane, int x_start,
437 int y_start, enum pixel_read_direction direction, int count,
438 struct pixel_argb_u16 out_pixel[])
439 {
440 Rx_read_line(plane, x_start, y_start, direction, count, out_pixel);
441 }
442
R4_read_line(const struct vkms_plane_state * plane,int x_start,int y_start,enum pixel_read_direction direction,int count,struct pixel_argb_u16 out_pixel[])443 static void R4_read_line(const struct vkms_plane_state *plane, int x_start,
444 int y_start, enum pixel_read_direction direction, int count,
445 struct pixel_argb_u16 out_pixel[])
446 {
447 Rx_read_line(plane, x_start, y_start, direction, count, out_pixel);
448 }
449
450
451 READ_LINE_ARGB8888(XRGB8888_read_line, px, 0xFF, px[2], px[1], px[0])
452 READ_LINE_ARGB8888(XBGR8888_read_line, px, 0xFF, px[0], px[1], px[2])
453
READ_LINE_ARGB8888(ARGB8888_read_line,px,px[3],px[2],px[1],px[0])454 READ_LINE_ARGB8888(ARGB8888_read_line, px, px[3], px[2], px[1], px[0])
455 READ_LINE_ARGB8888(ABGR8888_read_line, px, px[3], px[0], px[1], px[2])
456 READ_LINE_ARGB8888(RGBA8888_read_line, px, px[0], px[3], px[2], px[1])
457 READ_LINE_ARGB8888(BGRA8888_read_line, px, px[0], px[1], px[2], px[3])
458
459 READ_LINE_ARGB8888(RGB888_read_line, px, 0xFF, px[2], px[1], px[0])
460 READ_LINE_ARGB8888(BGR888_read_line, px, 0xFF, px[0], px[1], px[2])
461
462 READ_LINE_le16161616(ARGB16161616_read_line, px, px[3], px[2], px[1], px[0])
463 READ_LINE_le16161616(ABGR16161616_read_line, px, px[3], px[0], px[1], px[2])
464 READ_LINE_le16161616(XRGB16161616_read_line, px, cpu_to_le16(0xFFFF), px[2], px[1], px[0])
465 READ_LINE_le16161616(XBGR16161616_read_line, px, cpu_to_le16(0xFFFF), px[0], px[1], px[2])
466
467 READ_LINE(RGB565_read_line, px, __le16, argb_u16_from_RGB565, px)
468 READ_LINE(BGR565_read_line, px, __le16, argb_u16_from_BGR565, px)
469
470 READ_LINE(R8_read_line, px, u8, argb_u16_from_gray8, *px)
471
472 /*
473 * This callback can be used for YUV formats where U and V values are
474 * stored in the same plane (often called semi-planar formats). It will
475 * correctly handle subsampling as described in the drm_format_info of the plane.
476 *
477 * The conversion matrix stored in the @plane is used to:
478 * - Apply the correct color range and encoding
479 * - Convert YUV and YVU with the same function (a column swap is needed when setting up
480 * plane->conversion_matrix)
481 */
482
483 /**
484 * READ_LINE_YUV_SEMIPLANAR() - Generic generator for a read_line function which can be used for yuv
485 * formats with two planes and block_w == block_h == 1.
486 *
487 * @function_name: Function name to generate
488 * @pixel_1_name: temporary pixel name for the first plane used in the @__VA_ARGS__ parameters
489 * @pixel_2_name: temporary pixel name for the second plane used in the @__VA_ARGS__ parameters
490 * @pixel_1_type: Used to specify the type you want to cast the pixel pointer on the plane 1
491 * @pixel_2_type: Used to specify the type you want to cast the pixel pointer on the plane 2
492 * @callback: Callback to call for each pixels. This function should take
493 * (struct conversion_matrix*, @__VA_ARGS__) as parameter and return a pixel_argb_u16
494 * __VA_ARGS__: Argument to pass inside the callback. You can use @pixel_1_name and @pixel_2_name
495 * to access current pixel values
496 */
497 #define READ_LINE_YUV_SEMIPLANAR(function_name, pixel_1_name, pixel_2_name, pixel_1_type, \
498 pixel_2_type, callback, ...) \
499 static void function_name(const struct vkms_plane_state *plane, int x_start, \
500 int y_start, enum pixel_read_direction direction, int count, \
501 struct pixel_argb_u16 out_pixel[]) \
502 { \
503 u8 *plane_1; \
504 u8 *plane_2; \
505 \
506 packed_pixels_addr_1x1(plane->frame_info, x_start, y_start, 0, \
507 &plane_1); \
508 packed_pixels_addr_1x1(plane->frame_info, \
509 x_start / plane->frame_info->fb->format->hsub, \
510 y_start / plane->frame_info->fb->format->vsub, 1, \
511 &plane_2); \
512 int step_1 = get_block_step_bytes(plane->frame_info->fb, direction, 0); \
513 int step_2 = get_block_step_bytes(plane->frame_info->fb, direction, 1); \
514 int subsampling = get_subsampling(plane->frame_info->fb->format, direction); \
515 int subsampling_offset = get_subsampling_offset(direction, x_start, y_start); \
516 const struct conversion_matrix *conversion_matrix = &plane->conversion_matrix; \
517 \
518 for (int i = 0; i < count; i++) { \
519 pixel_1_type *(pixel_1_name) = (pixel_1_type *)plane_1; \
520 pixel_2_type *(pixel_2_name) = (pixel_2_type *)plane_2; \
521 *out_pixel = (callback)(conversion_matrix, __VA_ARGS__); \
522 out_pixel += 1; \
523 plane_1 += step_1; \
524 if ((i + subsampling_offset + 1) % subsampling == 0) \
525 plane_2 += step_2; \
526 } \
527 }
528
529 READ_LINE_YUV_SEMIPLANAR(YUV888_semiplanar_read_line, y, uv, u8, u8, argb_u16_from_yuv161616,
530 y[0] * 257, uv[0] * 257, uv[1] * 257)
531 READ_LINE_YUV_SEMIPLANAR(YUV161616_semiplanar_read_line, y, uv, u16, u16, argb_u16_from_yuv161616,
532 y[0], uv[0], uv[1])
533 /*
534 * This callback can be used for YUV format where each color component is
535 * stored in a different plane (often called planar formats). It will
536 * correctly handle subsampling as described in the drm_format_info of the plane.
537 *
538 * The conversion matrix stored in the @plane is used to:
539 * - Apply the correct color range and encoding
540 * - Convert YUV and YVU with the same function (a column swap is needed when setting up
541 * plane->conversion_matrix)
542 */
543 static void planar_yuv_read_line(const struct vkms_plane_state *plane, int x_start,
544 int y_start, enum pixel_read_direction direction, int count,
545 struct pixel_argb_u16 out_pixel[])
546 {
547 u8 *y_plane;
548 u8 *channel_1_plane;
549 u8 *channel_2_plane;
550
551 packed_pixels_addr_1x1(plane->frame_info, x_start, y_start, 0,
552 &y_plane);
553 packed_pixels_addr_1x1(plane->frame_info,
554 x_start / plane->frame_info->fb->format->hsub,
555 y_start / plane->frame_info->fb->format->vsub, 1,
556 &channel_1_plane);
557 packed_pixels_addr_1x1(plane->frame_info,
558 x_start / plane->frame_info->fb->format->hsub,
559 y_start / plane->frame_info->fb->format->vsub, 2,
560 &channel_2_plane);
561 int step_y = get_block_step_bytes(plane->frame_info->fb, direction, 0);
562 int step_channel_1 = get_block_step_bytes(plane->frame_info->fb, direction, 1);
563 int step_channel_2 = get_block_step_bytes(plane->frame_info->fb, direction, 2);
564 int subsampling = get_subsampling(plane->frame_info->fb->format, direction);
565 int subsampling_offset = get_subsampling_offset(direction, x_start, y_start);
566 const struct conversion_matrix *conversion_matrix = &plane->conversion_matrix;
567
568 for (int i = 0; i < count; i++) {
569 *out_pixel = argb_u16_from_yuv161616(conversion_matrix,
570 *y_plane * 257, *channel_1_plane * 257,
571 *channel_2_plane * 257);
572 out_pixel += 1;
573 y_plane += step_y;
574 if ((i + subsampling_offset + 1) % subsampling == 0) {
575 channel_1_plane += step_channel_1;
576 channel_2_plane += step_channel_2;
577 }
578 }
579 }
580
581 /*
582 * The following functions take one &struct pixel_argb_u16 and convert it to a specific format.
583 * The result is stored in @out_pixel.
584 *
585 * They are used in vkms_writeback_row() to convert and store a pixel from the src_buffer to
586 * the writeback buffer.
587 */
argb_u16_to_ARGB8888(u8 * out_pixel,const struct pixel_argb_u16 * in_pixel)588 static void argb_u16_to_ARGB8888(u8 *out_pixel, const struct pixel_argb_u16 *in_pixel)
589 {
590 /*
591 * This sequence below is important because the format's byte order is
592 * in little-endian. In the case of the ARGB8888 the memory is
593 * organized this way:
594 *
595 * | Addr | = blue channel
596 * | Addr + 1 | = green channel
597 * | Addr + 2 | = Red channel
598 * | Addr + 3 | = Alpha channel
599 */
600 out_pixel[3] = DIV_ROUND_CLOSEST(in_pixel->a, 257);
601 out_pixel[2] = DIV_ROUND_CLOSEST(in_pixel->r, 257);
602 out_pixel[1] = DIV_ROUND_CLOSEST(in_pixel->g, 257);
603 out_pixel[0] = DIV_ROUND_CLOSEST(in_pixel->b, 257);
604 }
605
argb_u16_to_XRGB8888(u8 * out_pixel,const struct pixel_argb_u16 * in_pixel)606 static void argb_u16_to_XRGB8888(u8 *out_pixel, const struct pixel_argb_u16 *in_pixel)
607 {
608 out_pixel[3] = 0xff;
609 out_pixel[2] = DIV_ROUND_CLOSEST(in_pixel->r, 257);
610 out_pixel[1] = DIV_ROUND_CLOSEST(in_pixel->g, 257);
611 out_pixel[0] = DIV_ROUND_CLOSEST(in_pixel->b, 257);
612 }
613
argb_u16_to_ABGR8888(u8 * out_pixel,const struct pixel_argb_u16 * in_pixel)614 static void argb_u16_to_ABGR8888(u8 *out_pixel, const struct pixel_argb_u16 *in_pixel)
615 {
616 out_pixel[3] = DIV_ROUND_CLOSEST(in_pixel->a, 257);
617 out_pixel[2] = DIV_ROUND_CLOSEST(in_pixel->b, 257);
618 out_pixel[1] = DIV_ROUND_CLOSEST(in_pixel->g, 257);
619 out_pixel[0] = DIV_ROUND_CLOSEST(in_pixel->r, 257);
620 }
621
argb_u16_to_ARGB16161616(u8 * out_pixel,const struct pixel_argb_u16 * in_pixel)622 static void argb_u16_to_ARGB16161616(u8 *out_pixel, const struct pixel_argb_u16 *in_pixel)
623 {
624 __le16 *pixel = (__le16 *)out_pixel;
625
626 pixel[3] = cpu_to_le16(in_pixel->a);
627 pixel[2] = cpu_to_le16(in_pixel->r);
628 pixel[1] = cpu_to_le16(in_pixel->g);
629 pixel[0] = cpu_to_le16(in_pixel->b);
630 }
631
argb_u16_to_XRGB16161616(u8 * out_pixel,const struct pixel_argb_u16 * in_pixel)632 static void argb_u16_to_XRGB16161616(u8 *out_pixel, const struct pixel_argb_u16 *in_pixel)
633 {
634 __le16 *pixel = (__le16 *)out_pixel;
635
636 pixel[3] = cpu_to_le16(0xffff);
637 pixel[2] = cpu_to_le16(in_pixel->r);
638 pixel[1] = cpu_to_le16(in_pixel->g);
639 pixel[0] = cpu_to_le16(in_pixel->b);
640 }
641
argb_u16_to_RGB565(u8 * out_pixel,const struct pixel_argb_u16 * in_pixel)642 static void argb_u16_to_RGB565(u8 *out_pixel, const struct pixel_argb_u16 *in_pixel)
643 {
644 __le16 *pixel = (__le16 *)out_pixel;
645
646 s64 fp_rb_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(31));
647 s64 fp_g_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(63));
648
649 s64 fp_r = drm_int2fixp(in_pixel->r);
650 s64 fp_g = drm_int2fixp(in_pixel->g);
651 s64 fp_b = drm_int2fixp(in_pixel->b);
652
653 u16 r = drm_fixp2int(drm_fixp_div(fp_r, fp_rb_ratio));
654 u16 g = drm_fixp2int(drm_fixp_div(fp_g, fp_g_ratio));
655 u16 b = drm_fixp2int(drm_fixp_div(fp_b, fp_rb_ratio));
656
657 *pixel = cpu_to_le16(r << 11 | g << 5 | b);
658 }
659
660 /**
661 * vkms_writeback_row() - Generic loop for all supported writeback format. It is executed just
662 * after the blending to write a line in the writeback buffer.
663 *
664 * @wb: Job where to insert the final image
665 * @src_buffer: Line to write
666 * @y: Row to write in the writeback buffer
667 */
vkms_writeback_row(struct vkms_writeback_job * wb,const struct line_buffer * src_buffer,int y)668 void vkms_writeback_row(struct vkms_writeback_job *wb,
669 const struct line_buffer *src_buffer, int y)
670 {
671 struct vkms_frame_info *frame_info = &wb->wb_frame_info;
672 int x_dst = frame_info->dst.x1;
673 u8 *dst_pixels;
674 int rem_x, rem_y;
675
676 packed_pixels_addr(frame_info, x_dst, y, 0, &dst_pixels, &rem_x, &rem_y);
677 struct pixel_argb_u16 *in_pixels = src_buffer->pixels;
678 int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst), src_buffer->n_pixels);
679
680 for (size_t x = 0; x < x_limit; x++, dst_pixels += frame_info->fb->format->cpp[0])
681 wb->pixel_write(dst_pixels, &in_pixels[x]);
682 }
683
684 /**
685 * get_pixel_read_line_function() - Retrieve the correct read_line function for a specific
686 * format. The returned pointer is NULL for unsupported pixel formats. The caller must ensure that
687 * the pointer is valid before using it in a vkms_plane_state.
688 *
689 * @format: DRM_FORMAT_* value for which to obtain a conversion function (see [drm_fourcc.h])
690 */
get_pixel_read_line_function(u32 format)691 pixel_read_line_t get_pixel_read_line_function(u32 format)
692 {
693 switch (format) {
694 case DRM_FORMAT_ARGB8888:
695 return &ARGB8888_read_line;
696 case DRM_FORMAT_ABGR8888:
697 return &ABGR8888_read_line;
698 case DRM_FORMAT_BGRA8888:
699 return &BGRA8888_read_line;
700 case DRM_FORMAT_RGBA8888:
701 return &RGBA8888_read_line;
702 case DRM_FORMAT_XRGB8888:
703 return &XRGB8888_read_line;
704 case DRM_FORMAT_XBGR8888:
705 return &XBGR8888_read_line;
706 case DRM_FORMAT_RGB888:
707 return &RGB888_read_line;
708 case DRM_FORMAT_BGR888:
709 return &BGR888_read_line;
710 case DRM_FORMAT_ARGB16161616:
711 return &ARGB16161616_read_line;
712 case DRM_FORMAT_ABGR16161616:
713 return &ABGR16161616_read_line;
714 case DRM_FORMAT_XRGB16161616:
715 return &XRGB16161616_read_line;
716 case DRM_FORMAT_XBGR16161616:
717 return &XBGR16161616_read_line;
718 case DRM_FORMAT_RGB565:
719 return &RGB565_read_line;
720 case DRM_FORMAT_BGR565:
721 return &BGR565_read_line;
722 case DRM_FORMAT_NV12:
723 case DRM_FORMAT_NV16:
724 case DRM_FORMAT_NV24:
725 case DRM_FORMAT_NV21:
726 case DRM_FORMAT_NV61:
727 case DRM_FORMAT_NV42:
728 return &YUV888_semiplanar_read_line;
729 case DRM_FORMAT_P010:
730 case DRM_FORMAT_P012:
731 case DRM_FORMAT_P016:
732 return &YUV161616_semiplanar_read_line;
733 case DRM_FORMAT_YUV420:
734 case DRM_FORMAT_YUV422:
735 case DRM_FORMAT_YUV444:
736 case DRM_FORMAT_YVU420:
737 case DRM_FORMAT_YVU422:
738 case DRM_FORMAT_YVU444:
739 return &planar_yuv_read_line;
740 case DRM_FORMAT_R1:
741 return &R1_read_line;
742 case DRM_FORMAT_R2:
743 return &R2_read_line;
744 case DRM_FORMAT_R4:
745 return &R4_read_line;
746 case DRM_FORMAT_R8:
747 return &R8_read_line;
748 default:
749 /*
750 * This is a bug in vkms_plane_atomic_check(). All the supported
751 * format must:
752 * - Be listed in vkms_formats in vkms_plane.c
753 * - Have a pixel_read callback defined here
754 */
755 pr_err("Pixel format %p4cc is not supported by VKMS planes. This is a kernel bug, atomic check must forbid this configuration.\n",
756 &format);
757 BUG();
758 }
759 }
760
761 /*
762 * Those matrices were generated using the colour python framework
763 *
764 * Below are the function calls used to generate each matrix, go to
765 * https://colour.readthedocs.io/en/develop/generated/colour.matrix_YCbCr.html
766 * for more info:
767 *
768 * numpy.around(colour.matrix_YCbCr(K=colour.WEIGHTS_YCBCR["ITU-R BT.601"],
769 * is_legal = False,
770 * bits = 8) * 2**32).astype(int)
771 */
772 static const struct conversion_matrix no_operation = {
773 .matrix = {
774 { 4294967296, 0, 0, },
775 { 0, 4294967296, 0, },
776 { 0, 0, 4294967296, },
777 },
778 .y_offset = 0,
779 };
780
781 static const struct conversion_matrix yuv_bt601_full = {
782 .matrix = {
783 { 4294967296, 0, 6021544149 },
784 { 4294967296, -1478054095, -3067191994 },
785 { 4294967296, 7610682049, 0 },
786 },
787 .y_offset = 0,
788 };
789
790 /*
791 * numpy.around(colour.matrix_YCbCr(K=colour.WEIGHTS_YCBCR["ITU-R BT.601"],
792 * is_legal = True,
793 * bits = 8) * 2**32).astype(int)
794 */
795 static const struct conversion_matrix yuv_bt601_limited = {
796 .matrix = {
797 { 5020601039, 0, 6881764740 },
798 { 5020601039, -1689204679, -3505362278 },
799 { 5020601039, 8697922339, 0 },
800 },
801 .y_offset = 16,
802 };
803
804 /*
805 * numpy.around(colour.matrix_YCbCr(K=colour.WEIGHTS_YCBCR["ITU-R BT.709"],
806 * is_legal = False,
807 * bits = 8) * 2**32).astype(int)
808 */
809 static const struct conversion_matrix yuv_bt709_full = {
810 .matrix = {
811 { 4294967296, 0, 6763714498 },
812 { 4294967296, -804551626, -2010578443 },
813 { 4294967296, 7969741314, 0 },
814 },
815 .y_offset = 0,
816 };
817
818 /*
819 * numpy.around(colour.matrix_YCbCr(K=colour.WEIGHTS_YCBCR["ITU-R BT.709"],
820 * is_legal = True,
821 * bits = 8) * 2**32).astype(int)
822 */
823 static const struct conversion_matrix yuv_bt709_limited = {
824 .matrix = {
825 { 5020601039, 0, 7729959424 },
826 { 5020601039, -919487572, -2297803934 },
827 { 5020601039, 9108275786, 0 },
828 },
829 .y_offset = 16,
830 };
831
832 /*
833 * numpy.around(colour.matrix_YCbCr(K=colour.WEIGHTS_YCBCR["ITU-R BT.2020"],
834 * is_legal = False,
835 * bits = 8) * 2**32).astype(int)
836 */
837 static const struct conversion_matrix yuv_bt2020_full = {
838 .matrix = {
839 { 4294967296, 0, 6333358775 },
840 { 4294967296, -706750298, -2453942994 },
841 { 4294967296, 8080551471, 0 },
842 },
843 .y_offset = 0,
844 };
845
846 /*
847 * numpy.around(colour.matrix_YCbCr(K=colour.WEIGHTS_YCBCR["ITU-R BT.2020"],
848 * is_legal = True,
849 * bits = 8) * 2**32).astype(int)
850 */
851 static const struct conversion_matrix yuv_bt2020_limited = {
852 .matrix = {
853 { 5020601039, 0, 7238124312 },
854 { 5020601039, -807714626, -2804506279 },
855 { 5020601039, 9234915964, 0 },
856 },
857 .y_offset = 16,
858 };
859
860 /**
861 * swap_uv_columns() - Swap u and v column of a given matrix
862 *
863 * @matrix: Matrix in which column are swapped
864 */
swap_uv_columns(struct conversion_matrix * matrix)865 static void swap_uv_columns(struct conversion_matrix *matrix)
866 {
867 swap(matrix->matrix[0][2], matrix->matrix[0][1]);
868 swap(matrix->matrix[1][2], matrix->matrix[1][1]);
869 swap(matrix->matrix[2][2], matrix->matrix[2][1]);
870 }
871
872 /**
873 * get_conversion_matrix_to_argb_u16() - Retrieve the correct yuv to rgb conversion matrix for a
874 * given encoding and range.
875 *
876 * @format: DRM_FORMAT_* value for which to obtain a conversion function (see [drm_fourcc.h])
877 * @encoding: DRM_COLOR_* value for which to obtain a conversion matrix
878 * @range: DRM_COLOR_*_RANGE value for which to obtain a conversion matrix
879 * @matrix: Pointer to store the value into
880 */
get_conversion_matrix_to_argb_u16(u32 format,enum drm_color_encoding encoding,enum drm_color_range range,struct conversion_matrix * matrix)881 void get_conversion_matrix_to_argb_u16(u32 format,
882 enum drm_color_encoding encoding,
883 enum drm_color_range range,
884 struct conversion_matrix *matrix)
885 {
886 const struct conversion_matrix *matrix_to_copy;
887 bool limited_range;
888
889 switch (range) {
890 case DRM_COLOR_YCBCR_LIMITED_RANGE:
891 limited_range = true;
892 break;
893 case DRM_COLOR_YCBCR_FULL_RANGE:
894 limited_range = false;
895 break;
896 case DRM_COLOR_RANGE_MAX:
897 limited_range = false;
898 WARN_ONCE(true, "The requested range is not supported.");
899 break;
900 }
901
902 switch (encoding) {
903 case DRM_COLOR_YCBCR_BT601:
904 matrix_to_copy = limited_range ? &yuv_bt601_limited :
905 &yuv_bt601_full;
906 break;
907 case DRM_COLOR_YCBCR_BT709:
908 matrix_to_copy = limited_range ? &yuv_bt709_limited :
909 &yuv_bt709_full;
910 break;
911 case DRM_COLOR_YCBCR_BT2020:
912 matrix_to_copy = limited_range ? &yuv_bt2020_limited :
913 &yuv_bt2020_full;
914 break;
915 case DRM_COLOR_ENCODING_MAX:
916 matrix_to_copy = &no_operation;
917 WARN_ONCE(true, "The requested encoding is not supported.");
918 break;
919 }
920
921 memcpy(matrix, matrix_to_copy, sizeof(*matrix_to_copy));
922
923 switch (format) {
924 case DRM_FORMAT_YVU420:
925 case DRM_FORMAT_YVU422:
926 case DRM_FORMAT_YVU444:
927 case DRM_FORMAT_NV21:
928 case DRM_FORMAT_NV61:
929 case DRM_FORMAT_NV42:
930 swap_uv_columns(matrix);
931 break;
932 default:
933 break;
934 }
935 }
936 EXPORT_SYMBOL(get_conversion_matrix_to_argb_u16);
937
938 /**
939 * get_pixel_write_function() - Retrieve the correct write_pixel function for a specific format.
940 * The returned pointer is NULL for unsupported pixel formats. The caller must ensure that the
941 * pointer is valid before using it in a vkms_writeback_job.
942 *
943 * @format: DRM_FORMAT_* value for which to obtain a conversion function (see [drm_fourcc.h])
944 */
get_pixel_write_function(u32 format)945 pixel_write_t get_pixel_write_function(u32 format)
946 {
947 switch (format) {
948 case DRM_FORMAT_ARGB8888:
949 return &argb_u16_to_ARGB8888;
950 case DRM_FORMAT_XRGB8888:
951 return &argb_u16_to_XRGB8888;
952 case DRM_FORMAT_ABGR8888:
953 return &argb_u16_to_ABGR8888;
954 case DRM_FORMAT_ARGB16161616:
955 return &argb_u16_to_ARGB16161616;
956 case DRM_FORMAT_XRGB16161616:
957 return &argb_u16_to_XRGB16161616;
958 case DRM_FORMAT_RGB565:
959 return &argb_u16_to_RGB565;
960 default:
961 /*
962 * This is a bug in vkms_writeback_atomic_check. All the supported
963 * format must:
964 * - Be listed in vkms_wb_formats in vkms_writeback.c
965 * - Have a pixel_write callback defined here
966 */
967 pr_err("Pixel format %p4cc is not supported by VKMS writeback. This is a kernel bug, atomic check must forbid this configuration.\n",
968 &format);
969 BUG();
970 }
971 }
972