1 // SPDX-License-Identifier: GPL-2.0+
2 
3 #include <linux/kernel.h>
4 #include <linux/minmax.h>
5 #include <drm/drm_rect.h>
6 #include <drm/drm_fixed.h>
7 
8 #include "vkms_formats.h"
9 
10 static size_t pixel_offset(const struct vkms_frame_info *frame_info, int x, int y)
11 {
12 	return frame_info->offset + (y * frame_info->pitch)
13 				  + (x * frame_info->cpp);
14 }
15 
16 /*
17  * packed_pixels_addr - Get the pointer to pixel of a given pair of coordinates
18  *
19  * @frame_info: Buffer metadata
20  * @x: The x(width) coordinate of the 2D buffer
21  * @y: The y(Heigth) coordinate of the 2D buffer
22  *
23  * Takes the information stored in the frame_info, a pair of coordinates, and
24  * returns the address of the first color channel.
25  * This function assumes the channels are packed together, i.e. a color channel
26  * comes immediately after another in the memory. And therefore, this function
27  * doesn't work for YUV with chroma subsampling (e.g. YUV420 and NV21).
28  */
29 static void *packed_pixels_addr(const struct vkms_frame_info *frame_info,
30 				int x, int y)
31 {
32 	size_t offset = pixel_offset(frame_info, x, y);
33 
34 	return (u8 *)frame_info->map[0].vaddr + offset;
35 }
36 
37 static void *get_packed_src_addr(const struct vkms_frame_info *frame_info, int y)
38 {
39 	int x_src = frame_info->src.x1 >> 16;
40 	int y_src = y - frame_info->dst.y1 + (frame_info->src.y1 >> 16);
41 
42 	return packed_pixels_addr(frame_info, x_src, y_src);
43 }
44 
45 static void ARGB8888_to_argb_u16(struct line_buffer *stage_buffer,
46 				 const struct vkms_frame_info *frame_info, int y)
47 {
48 	struct pixel_argb_u16 *out_pixels = stage_buffer->pixels;
49 	u8 *src_pixels = get_packed_src_addr(frame_info, y);
50 	int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
51 			    stage_buffer->n_pixels);
52 
53 	for (size_t x = 0; x < x_limit; x++, src_pixels += 4) {
54 		/*
55 		 * The 257 is the "conversion ratio". This number is obtained by the
56 		 * (2^16 - 1) / (2^8 - 1) division. Which, in this case, tries to get
57 		 * the best color value in a pixel format with more possibilities.
58 		 * A similar idea applies to others RGB color conversions.
59 		 */
60 		out_pixels[x].a = (u16)src_pixels[3] * 257;
61 		out_pixels[x].r = (u16)src_pixels[2] * 257;
62 		out_pixels[x].g = (u16)src_pixels[1] * 257;
63 		out_pixels[x].b = (u16)src_pixels[0] * 257;
64 	}
65 }
66 
67 static void XRGB8888_to_argb_u16(struct line_buffer *stage_buffer,
68 				 const struct vkms_frame_info *frame_info, int y)
69 {
70 	struct pixel_argb_u16 *out_pixels = stage_buffer->pixels;
71 	u8 *src_pixels = get_packed_src_addr(frame_info, y);
72 	int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
73 			    stage_buffer->n_pixels);
74 
75 	for (size_t x = 0; x < x_limit; x++, src_pixels += 4) {
76 		out_pixels[x].a = (u16)0xffff;
77 		out_pixels[x].r = (u16)src_pixels[2] * 257;
78 		out_pixels[x].g = (u16)src_pixels[1] * 257;
79 		out_pixels[x].b = (u16)src_pixels[0] * 257;
80 	}
81 }
82 
83 static void ARGB16161616_to_argb_u16(struct line_buffer *stage_buffer,
84 				     const struct vkms_frame_info *frame_info,
85 				     int y)
86 {
87 	struct pixel_argb_u16 *out_pixels = stage_buffer->pixels;
88 	u16 *src_pixels = get_packed_src_addr(frame_info, y);
89 	int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
90 			    stage_buffer->n_pixels);
91 
92 	for (size_t x = 0; x < x_limit; x++, src_pixels += 4) {
93 		out_pixels[x].a = le16_to_cpu(src_pixels[3]);
94 		out_pixels[x].r = le16_to_cpu(src_pixels[2]);
95 		out_pixels[x].g = le16_to_cpu(src_pixels[1]);
96 		out_pixels[x].b = le16_to_cpu(src_pixels[0]);
97 	}
98 }
99 
100 static void XRGB16161616_to_argb_u16(struct line_buffer *stage_buffer,
101 				     const struct vkms_frame_info *frame_info,
102 				     int y)
103 {
104 	struct pixel_argb_u16 *out_pixels = stage_buffer->pixels;
105 	u16 *src_pixels = get_packed_src_addr(frame_info, y);
106 	int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
107 			    stage_buffer->n_pixels);
108 
109 	for (size_t x = 0; x < x_limit; x++, src_pixels += 4) {
110 		out_pixels[x].a = (u16)0xffff;
111 		out_pixels[x].r = le16_to_cpu(src_pixels[2]);
112 		out_pixels[x].g = le16_to_cpu(src_pixels[1]);
113 		out_pixels[x].b = le16_to_cpu(src_pixels[0]);
114 	}
115 }
116 
117 static void RGB565_to_argb_u16(struct line_buffer *stage_buffer,
118 			       const struct vkms_frame_info *frame_info, int y)
119 {
120 	struct pixel_argb_u16 *out_pixels = stage_buffer->pixels;
121 	u16 *src_pixels = get_packed_src_addr(frame_info, y);
122 	int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
123 			       stage_buffer->n_pixels);
124 
125 	s64 fp_rb_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(31));
126 	s64 fp_g_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(63));
127 
128 	for (size_t x = 0; x < x_limit; x++, src_pixels++) {
129 		u16 rgb_565 = le16_to_cpu(*src_pixels);
130 		s64 fp_r = drm_int2fixp((rgb_565 >> 11) & 0x1f);
131 		s64 fp_g = drm_int2fixp((rgb_565 >> 5) & 0x3f);
132 		s64 fp_b = drm_int2fixp(rgb_565 & 0x1f);
133 
134 		out_pixels[x].a = (u16)0xffff;
135 		out_pixels[x].r = drm_fixp2int(drm_fixp_mul(fp_r, fp_rb_ratio));
136 		out_pixels[x].g = drm_fixp2int(drm_fixp_mul(fp_g, fp_g_ratio));
137 		out_pixels[x].b = drm_fixp2int(drm_fixp_mul(fp_b, fp_rb_ratio));
138 	}
139 }
140 
141 /*
142  * The following  functions take an line of argb_u16 pixels from the
143  * src_buffer, convert them to a specific format, and store them in the
144  * destination.
145  *
146  * They are used in the `compose_active_planes` to convert and store a line
147  * from the src_buffer to the writeback buffer.
148  */
149 static void argb_u16_to_ARGB8888(struct vkms_frame_info *frame_info,
150 				 const struct line_buffer *src_buffer, int y)
151 {
152 	int x_dst = frame_info->dst.x1;
153 	u8 *dst_pixels = packed_pixels_addr(frame_info, x_dst, y);
154 	struct pixel_argb_u16 *in_pixels = src_buffer->pixels;
155 	int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
156 			    src_buffer->n_pixels);
157 
158 	for (size_t x = 0; x < x_limit; x++, dst_pixels += 4) {
159 		/*
160 		 * This sequence below is important because the format's byte order is
161 		 * in little-endian. In the case of the ARGB8888 the memory is
162 		 * organized this way:
163 		 *
164 		 * | Addr     | = blue channel
165 		 * | Addr + 1 | = green channel
166 		 * | Addr + 2 | = Red channel
167 		 * | Addr + 3 | = Alpha channel
168 		 */
169 		dst_pixels[3] = DIV_ROUND_CLOSEST(in_pixels[x].a, 257);
170 		dst_pixels[2] = DIV_ROUND_CLOSEST(in_pixels[x].r, 257);
171 		dst_pixels[1] = DIV_ROUND_CLOSEST(in_pixels[x].g, 257);
172 		dst_pixels[0] = DIV_ROUND_CLOSEST(in_pixels[x].b, 257);
173 	}
174 }
175 
176 static void argb_u16_to_XRGB8888(struct vkms_frame_info *frame_info,
177 				 const struct line_buffer *src_buffer, int y)
178 {
179 	int x_dst = frame_info->dst.x1;
180 	u8 *dst_pixels = packed_pixels_addr(frame_info, x_dst, y);
181 	struct pixel_argb_u16 *in_pixels = src_buffer->pixels;
182 	int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
183 			    src_buffer->n_pixels);
184 
185 	for (size_t x = 0; x < x_limit; x++, dst_pixels += 4) {
186 		dst_pixels[3] = 0xff;
187 		dst_pixels[2] = DIV_ROUND_CLOSEST(in_pixels[x].r, 257);
188 		dst_pixels[1] = DIV_ROUND_CLOSEST(in_pixels[x].g, 257);
189 		dst_pixels[0] = DIV_ROUND_CLOSEST(in_pixels[x].b, 257);
190 	}
191 }
192 
193 static void argb_u16_to_ARGB16161616(struct vkms_frame_info *frame_info,
194 				     const struct line_buffer *src_buffer, int y)
195 {
196 	int x_dst = frame_info->dst.x1;
197 	u16 *dst_pixels = packed_pixels_addr(frame_info, x_dst, y);
198 	struct pixel_argb_u16 *in_pixels = src_buffer->pixels;
199 	int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
200 			    src_buffer->n_pixels);
201 
202 	for (size_t x = 0; x < x_limit; x++, dst_pixels += 4) {
203 		dst_pixels[3] = cpu_to_le16(in_pixels[x].a);
204 		dst_pixels[2] = cpu_to_le16(in_pixels[x].r);
205 		dst_pixels[1] = cpu_to_le16(in_pixels[x].g);
206 		dst_pixels[0] = cpu_to_le16(in_pixels[x].b);
207 	}
208 }
209 
210 static void argb_u16_to_XRGB16161616(struct vkms_frame_info *frame_info,
211 				     const struct line_buffer *src_buffer, int y)
212 {
213 	int x_dst = frame_info->dst.x1;
214 	u16 *dst_pixels = packed_pixels_addr(frame_info, x_dst, y);
215 	struct pixel_argb_u16 *in_pixels = src_buffer->pixels;
216 	int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
217 			    src_buffer->n_pixels);
218 
219 	for (size_t x = 0; x < x_limit; x++, dst_pixels += 4) {
220 		dst_pixels[3] = 0xffff;
221 		dst_pixels[2] = cpu_to_le16(in_pixels[x].r);
222 		dst_pixels[1] = cpu_to_le16(in_pixels[x].g);
223 		dst_pixels[0] = cpu_to_le16(in_pixels[x].b);
224 	}
225 }
226 
227 static void argb_u16_to_RGB565(struct vkms_frame_info *frame_info,
228 			       const struct line_buffer *src_buffer, int y)
229 {
230 	int x_dst = frame_info->dst.x1;
231 	u16 *dst_pixels = packed_pixels_addr(frame_info, x_dst, y);
232 	struct pixel_argb_u16 *in_pixels = src_buffer->pixels;
233 	int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
234 			    src_buffer->n_pixels);
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 	for (size_t x = 0; x < x_limit; x++, dst_pixels++) {
240 		s64 fp_r = drm_int2fixp(in_pixels[x].r);
241 		s64 fp_g = drm_int2fixp(in_pixels[x].g);
242 		s64 fp_b = drm_int2fixp(in_pixels[x].b);
243 
244 		u16 r = drm_fixp2int(drm_fixp_div(fp_r, fp_rb_ratio));
245 		u16 g = drm_fixp2int(drm_fixp_div(fp_g, fp_g_ratio));
246 		u16 b = drm_fixp2int(drm_fixp_div(fp_b, fp_rb_ratio));
247 
248 		*dst_pixels = cpu_to_le16(r << 11 | g << 5 | b);
249 	}
250 }
251 
252 void *get_frame_to_line_function(u32 format)
253 {
254 	switch (format) {
255 	case DRM_FORMAT_ARGB8888:
256 		return &ARGB8888_to_argb_u16;
257 	case DRM_FORMAT_XRGB8888:
258 		return &XRGB8888_to_argb_u16;
259 	case DRM_FORMAT_ARGB16161616:
260 		return &ARGB16161616_to_argb_u16;
261 	case DRM_FORMAT_XRGB16161616:
262 		return &XRGB16161616_to_argb_u16;
263 	case DRM_FORMAT_RGB565:
264 		return &RGB565_to_argb_u16;
265 	default:
266 		return NULL;
267 	}
268 }
269 
270 void *get_line_to_frame_function(u32 format)
271 {
272 	switch (format) {
273 	case DRM_FORMAT_ARGB8888:
274 		return &argb_u16_to_ARGB8888;
275 	case DRM_FORMAT_XRGB8888:
276 		return &argb_u16_to_XRGB8888;
277 	case DRM_FORMAT_ARGB16161616:
278 		return &argb_u16_to_ARGB16161616;
279 	case DRM_FORMAT_XRGB16161616:
280 		return &argb_u16_to_XRGB16161616;
281 	case DRM_FORMAT_RGB565:
282 		return &argb_u16_to_RGB565;
283 	default:
284 		return NULL;
285 	}
286 }
287