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 "vkms_formats.h"
 11
 12/**
 13 * pixel_offset() - Get the offset of the pixel at coordinates x/y in the first plane
 14 *
 15 * @frame_info: Buffer metadata
 16 * @x: The x coordinate of the wanted pixel in the buffer
 17 * @y: The y coordinate of the wanted pixel in the buffer
 18 *
 19 * The caller must ensure that the framebuffer associated with this request uses a pixel format
 20 * where block_h == block_w == 1.
 21 * If this requirement is not fulfilled, the resulting offset can point to an other pixel or
 22 * outside of the buffer.
 23 */
 24static size_t pixel_offset(const struct vkms_frame_info *frame_info, int x, int y)
 25{
 26	return frame_info->offset + (y * frame_info->pitch)
 27				  + (x * frame_info->cpp);
 28}
 29
 30/**
 31 * packed_pixels_addr() - Get the pointer to the block containing the pixel at the given
 32 * coordinates
 33 *
 34 * @frame_info: Buffer metadata
 35 * @x: The x (width) coordinate inside the plane
 36 * @y: The y (height) coordinate inside the plane
 37 *
 38 * Takes the information stored in the frame_info, a pair of coordinates, and
 39 * returns the address of the first color channel.
 40 * This function assumes the channels are packed together, i.e. a color channel
 41 * comes immediately after another in the memory. And therefore, this function
 42 * doesn't work for YUV with chroma subsampling (e.g. YUV420 and NV21).
 43 *
 44 * The caller must ensure that the framebuffer associated with this request uses a pixel format
 45 * where block_h == block_w == 1, otherwise the returned pointer can be outside the buffer.
 46 */
 47static void *packed_pixels_addr(const struct vkms_frame_info *frame_info,
 48				int x, int y)
 49{
 50	size_t offset = pixel_offset(frame_info, x, y);
 51
 52	return (u8 *)frame_info->map[0].vaddr + offset;
 53}
 54
 55static void *get_packed_src_addr(const struct vkms_frame_info *frame_info, int y)
 56{
 57	int x_src = frame_info->src.x1 >> 16;
 58	int y_src = y - frame_info->rotated.y1 + (frame_info->src.y1 >> 16);
 59
 60	return packed_pixels_addr(frame_info, x_src, y_src);
 61}
 62
 63static int get_x_position(const struct vkms_frame_info *frame_info, int limit, int x)
 64{
 65	if (frame_info->rotation & (DRM_MODE_REFLECT_X | DRM_MODE_ROTATE_270))
 66		return limit - x - 1;
 67	return x;
 68}
 69
 70/*
 71 * The following functions take pixel data from the buffer and convert them to the format
 72 * ARGB16161616 in @out_pixel.
 73 *
 74 * They are used in the vkms_compose_row() function to handle multiple formats.
 75 */
 76
 77static void ARGB8888_to_argb_u16(u8 *src_pixels, struct pixel_argb_u16 *out_pixel)
 78{
 79	/*
 80	 * The 257 is the "conversion ratio". This number is obtained by the
 81	 * (2^16 - 1) / (2^8 - 1) division. Which, in this case, tries to get
 82	 * the best color value in a pixel format with more possibilities.
 83	 * A similar idea applies to others RGB color conversions.
 84	 */
 85	out_pixel->a = (u16)src_pixels[3] * 257;
 86	out_pixel->r = (u16)src_pixels[2] * 257;
 87	out_pixel->g = (u16)src_pixels[1] * 257;
 88	out_pixel->b = (u16)src_pixels[0] * 257;
 89}
 90
 91static void XRGB8888_to_argb_u16(u8 *src_pixels, struct pixel_argb_u16 *out_pixel)
 92{
 93	out_pixel->a = (u16)0xffff;
 94	out_pixel->r = (u16)src_pixels[2] * 257;
 95	out_pixel->g = (u16)src_pixels[1] * 257;
 96	out_pixel->b = (u16)src_pixels[0] * 257;
 97}
 98
 99static void ARGB16161616_to_argb_u16(u8 *src_pixels, struct pixel_argb_u16 *out_pixel)
100{
101	__le16 *pixels = (__force __le16 *)src_pixels;
102
103	out_pixel->a = le16_to_cpu(pixels[3]);
104	out_pixel->r = le16_to_cpu(pixels[2]);
105	out_pixel->g = le16_to_cpu(pixels[1]);
106	out_pixel->b = le16_to_cpu(pixels[0]);
107}
108
109static void XRGB16161616_to_argb_u16(u8 *src_pixels, struct pixel_argb_u16 *out_pixel)
110{
111	__le16 *pixels = (__force __le16 *)src_pixels;
112
113	out_pixel->a = (u16)0xffff;
114	out_pixel->r = le16_to_cpu(pixels[2]);
115	out_pixel->g = le16_to_cpu(pixels[1]);
116	out_pixel->b = le16_to_cpu(pixels[0]);
117}
118
119static void RGB565_to_argb_u16(u8 *src_pixels, struct pixel_argb_u16 *out_pixel)
120{
121	__le16 *pixels = (__force __le16 *)src_pixels;
122
123	s64 fp_rb_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(31));
124	s64 fp_g_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(63));
125
126	u16 rgb_565 = le16_to_cpu(*pixels);
127	s64 fp_r = drm_int2fixp((rgb_565 >> 11) & 0x1f);
128	s64 fp_g = drm_int2fixp((rgb_565 >> 5) & 0x3f);
129	s64 fp_b = drm_int2fixp(rgb_565 & 0x1f);
130
131	out_pixel->a = (u16)0xffff;
132	out_pixel->r = drm_fixp2int_round(drm_fixp_mul(fp_r, fp_rb_ratio));
133	out_pixel->g = drm_fixp2int_round(drm_fixp_mul(fp_g, fp_g_ratio));
134	out_pixel->b = drm_fixp2int_round(drm_fixp_mul(fp_b, fp_rb_ratio));
135}
136
137/**
138 * vkms_compose_row - compose a single row of a plane
139 * @stage_buffer: output line with the composed pixels
140 * @plane: state of the plane that is being composed
141 * @y: y coordinate of the row
142 *
143 * This function composes a single row of a plane. It gets the source pixels
144 * through the y coordinate (see get_packed_src_addr()) and goes linearly
145 * through the source pixel, reading the pixels and converting it to
146 * ARGB16161616 (see the pixel_read() callback). For rotate-90 and rotate-270,
147 * the source pixels are not traversed linearly. The source pixels are queried
148 * on each iteration in order to traverse the pixels vertically.
149 */
150void vkms_compose_row(struct line_buffer *stage_buffer, struct vkms_plane_state *plane, int y)
151{
152	struct pixel_argb_u16 *out_pixels = stage_buffer->pixels;
153	struct vkms_frame_info *frame_info = plane->frame_info;
154	u8 *src_pixels = get_packed_src_addr(frame_info, y);
155	int limit = min_t(size_t, drm_rect_width(&frame_info->dst), stage_buffer->n_pixels);
156
157	for (size_t x = 0; x < limit; x++, src_pixels += frame_info->cpp) {
158		int x_pos = get_x_position(frame_info, limit, x);
159
160		if (drm_rotation_90_or_270(frame_info->rotation))
161			src_pixels = get_packed_src_addr(frame_info, x + frame_info->rotated.y1)
162				+ frame_info->cpp * y;
163
164		plane->pixel_read(src_pixels, &out_pixels[x_pos]);
165	}
166}
167
168/*
169 * The following functions take one &struct pixel_argb_u16 and convert it to a specific format.
170 * The result is stored in @dst_pixels.
171 *
172 * They are used in vkms_writeback_row() to convert and store a pixel from the src_buffer to
173 * the writeback buffer.
174 */
175static void argb_u16_to_ARGB8888(u8 *dst_pixels, struct pixel_argb_u16 *in_pixel)
176{
177	/*
178	 * This sequence below is important because the format's byte order is
179	 * in little-endian. In the case of the ARGB8888 the memory is
180	 * organized this way:
181	 *
182	 * | Addr     | = blue channel
183	 * | Addr + 1 | = green channel
184	 * | Addr + 2 | = Red channel
185	 * | Addr + 3 | = Alpha channel
186	 */
187	dst_pixels[3] = DIV_ROUND_CLOSEST(in_pixel->a, 257);
188	dst_pixels[2] = DIV_ROUND_CLOSEST(in_pixel->r, 257);
189	dst_pixels[1] = DIV_ROUND_CLOSEST(in_pixel->g, 257);
190	dst_pixels[0] = DIV_ROUND_CLOSEST(in_pixel->b, 257);
191}
192
193static void argb_u16_to_XRGB8888(u8 *dst_pixels, struct pixel_argb_u16 *in_pixel)
194{
195	dst_pixels[3] = 0xff;
196	dst_pixels[2] = DIV_ROUND_CLOSEST(in_pixel->r, 257);
197	dst_pixels[1] = DIV_ROUND_CLOSEST(in_pixel->g, 257);
198	dst_pixels[0] = DIV_ROUND_CLOSEST(in_pixel->b, 257);
199}
200
201static void argb_u16_to_ARGB16161616(u8 *dst_pixels, struct pixel_argb_u16 *in_pixel)
202{
203	__le16 *pixels = (__force __le16 *)dst_pixels;
204
205	pixels[3] = cpu_to_le16(in_pixel->a);
206	pixels[2] = cpu_to_le16(in_pixel->r);
207	pixels[1] = cpu_to_le16(in_pixel->g);
208	pixels[0] = cpu_to_le16(in_pixel->b);
209}
210
211static void argb_u16_to_XRGB16161616(u8 *dst_pixels, struct pixel_argb_u16 *in_pixel)
212{
213	__le16 *pixels = (__force __le16 *)dst_pixels;
214
215	pixels[3] = cpu_to_le16(0xffff);
216	pixels[2] = cpu_to_le16(in_pixel->r);
217	pixels[1] = cpu_to_le16(in_pixel->g);
218	pixels[0] = cpu_to_le16(in_pixel->b);
219}
220
221static void argb_u16_to_RGB565(u8 *dst_pixels, struct pixel_argb_u16 *in_pixel)
222{
223	__le16 *pixels = (__force __le16 *)dst_pixels;
224
225	s64 fp_rb_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(31));
226	s64 fp_g_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(63));
227
228	s64 fp_r = drm_int2fixp(in_pixel->r);
229	s64 fp_g = drm_int2fixp(in_pixel->g);
230	s64 fp_b = drm_int2fixp(in_pixel->b);
231
232	u16 r = drm_fixp2int(drm_fixp_div(fp_r, fp_rb_ratio));
233	u16 g = drm_fixp2int(drm_fixp_div(fp_g, fp_g_ratio));
234	u16 b = drm_fixp2int(drm_fixp_div(fp_b, fp_rb_ratio));
235
236	*pixels = cpu_to_le16(r << 11 | g << 5 | b);
237}
238
239/**
240 * vkms_writeback_row() - Generic loop for all supported writeback format. It is executed just
241 * after the blending to write a line in the writeback buffer.
242 *
243 * @wb: Job where to insert the final image
244 * @src_buffer: Line to write
245 * @y: Row to write in the writeback buffer
246 */
247void vkms_writeback_row(struct vkms_writeback_job *wb,
248			const struct line_buffer *src_buffer, int y)
249{
250	struct vkms_frame_info *frame_info = &wb->wb_frame_info;
251	int x_dst = frame_info->dst.x1;
252	u8 *dst_pixels = packed_pixels_addr(frame_info, x_dst, y);
253	struct pixel_argb_u16 *in_pixels = src_buffer->pixels;
254	int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst), src_buffer->n_pixels);
255
256	for (size_t x = 0; x < x_limit; x++, dst_pixels += frame_info->cpp)
257		wb->pixel_write(dst_pixels, &in_pixels[x]);
258}
259
260/**
261 * get_pixel_conversion_function() - Retrieve the correct read_pixel function for a specific
262 * format. The returned pointer is NULL for unsupported pixel formats. The caller must ensure that
263 * the pointer is valid before using it in a vkms_plane_state.
264 *
265 * @format: DRM_FORMAT_* value for which to obtain a conversion function (see [drm_fourcc.h])
266 */
267void *get_pixel_conversion_function(u32 format)
268{
269	switch (format) {
270	case DRM_FORMAT_ARGB8888:
271		return &ARGB8888_to_argb_u16;
272	case DRM_FORMAT_XRGB8888:
273		return &XRGB8888_to_argb_u16;
274	case DRM_FORMAT_ARGB16161616:
275		return &ARGB16161616_to_argb_u16;
276	case DRM_FORMAT_XRGB16161616:
277		return &XRGB16161616_to_argb_u16;
278	case DRM_FORMAT_RGB565:
279		return &RGB565_to_argb_u16;
280	default:
281		return NULL;
282	}
283}
284
285/**
286 * get_pixel_write_function() - Retrieve the correct write_pixel function for a specific format.
287 * The returned pointer is NULL for unsupported pixel formats. The caller must ensure that the
288 * pointer is valid before using it in a vkms_writeback_job.
289 *
290 * @format: DRM_FORMAT_* value for which to obtain a conversion function (see [drm_fourcc.h])
291 */
292void *get_pixel_write_function(u32 format)
293{
294	switch (format) {
295	case DRM_FORMAT_ARGB8888:
296		return &argb_u16_to_ARGB8888;
297	case DRM_FORMAT_XRGB8888:
298		return &argb_u16_to_XRGB8888;
299	case DRM_FORMAT_ARGB16161616:
300		return &argb_u16_to_ARGB16161616;
301	case DRM_FORMAT_XRGB16161616:
302		return &argb_u16_to_XRGB16161616;
303	case DRM_FORMAT_RGB565:
304		return &argb_u16_to_RGB565;
305	default:
306		return NULL;
307	}
308}