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
pixel_offset(const struct vkms_frame_info * frame_info,int x,int y)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 */
packed_pixels_addr(const struct vkms_frame_info * frame_info,int x,int y)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
get_packed_src_addr(const struct vkms_frame_info * frame_info,int y)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
ARGB8888_to_argb_u16(u8 * src_pixels,struct pixel_argb_u16 * out_pixel)45 static void ARGB8888_to_argb_u16(u8 *src_pixels, struct pixel_argb_u16 *out_pixel)
46 {
47 /*
48 * The 257 is the "conversion ratio". This number is obtained by the
49 * (2^16 - 1) / (2^8 - 1) division. Which, in this case, tries to get
50 * the best color value in a pixel format with more possibilities.
51 * A similar idea applies to others RGB color conversions.
52 */
53 out_pixel->a = (u16)src_pixels[3] * 257;
54 out_pixel->r = (u16)src_pixels[2] * 257;
55 out_pixel->g = (u16)src_pixels[1] * 257;
56 out_pixel->b = (u16)src_pixels[0] * 257;
57 }
58
XRGB8888_to_argb_u16(u8 * src_pixels,struct pixel_argb_u16 * out_pixel)59 static void XRGB8888_to_argb_u16(u8 *src_pixels, struct pixel_argb_u16 *out_pixel)
60 {
61 out_pixel->a = (u16)0xffff;
62 out_pixel->r = (u16)src_pixels[2] * 257;
63 out_pixel->g = (u16)src_pixels[1] * 257;
64 out_pixel->b = (u16)src_pixels[0] * 257;
65 }
66
ARGB16161616_to_argb_u16(u8 * src_pixels,struct pixel_argb_u16 * out_pixel)67 static void ARGB16161616_to_argb_u16(u8 *src_pixels, struct pixel_argb_u16 *out_pixel)
68 {
69 u16 *pixels = (u16 *)src_pixels;
70
71 out_pixel->a = le16_to_cpu(pixels[3]);
72 out_pixel->r = le16_to_cpu(pixels[2]);
73 out_pixel->g = le16_to_cpu(pixels[1]);
74 out_pixel->b = le16_to_cpu(pixels[0]);
75 }
76
XRGB16161616_to_argb_u16(u8 * src_pixels,struct pixel_argb_u16 * out_pixel)77 static void XRGB16161616_to_argb_u16(u8 *src_pixels, struct pixel_argb_u16 *out_pixel)
78 {
79 u16 *pixels = (u16 *)src_pixels;
80
81 out_pixel->a = (u16)0xffff;
82 out_pixel->r = le16_to_cpu(pixels[2]);
83 out_pixel->g = le16_to_cpu(pixels[1]);
84 out_pixel->b = le16_to_cpu(pixels[0]);
85 }
86
RGB565_to_argb_u16(u8 * src_pixels,struct pixel_argb_u16 * out_pixel)87 static void RGB565_to_argb_u16(u8 *src_pixels, struct pixel_argb_u16 *out_pixel)
88 {
89 u16 *pixels = (u16 *)src_pixels;
90
91 s64 fp_rb_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(31));
92 s64 fp_g_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(63));
93
94 u16 rgb_565 = le16_to_cpu(*pixels);
95 s64 fp_r = drm_int2fixp((rgb_565 >> 11) & 0x1f);
96 s64 fp_g = drm_int2fixp((rgb_565 >> 5) & 0x3f);
97 s64 fp_b = drm_int2fixp(rgb_565 & 0x1f);
98
99 out_pixel->a = (u16)0xffff;
100 out_pixel->r = drm_fixp2int_round(drm_fixp_mul(fp_r, fp_rb_ratio));
101 out_pixel->g = drm_fixp2int_round(drm_fixp_mul(fp_g, fp_g_ratio));
102 out_pixel->b = drm_fixp2int_round(drm_fixp_mul(fp_b, fp_rb_ratio));
103 }
104
vkms_compose_row(struct line_buffer * stage_buffer,struct vkms_plane_state * plane,int y)105 void vkms_compose_row(struct line_buffer *stage_buffer, struct vkms_plane_state *plane, int y)
106 {
107 struct pixel_argb_u16 *out_pixels = stage_buffer->pixels;
108 struct vkms_frame_info *frame_info = plane->frame_info;
109 u8 *src_pixels = get_packed_src_addr(frame_info, y);
110 int limit = min_t(size_t, drm_rect_width(&frame_info->dst), stage_buffer->n_pixels);
111
112 for (size_t x = 0; x < limit; x++, src_pixels += frame_info->cpp)
113 plane->pixel_read(src_pixels, &out_pixels[x]);
114 }
115
116 /*
117 * The following functions take an line of argb_u16 pixels from the
118 * src_buffer, convert them to a specific format, and store them in the
119 * destination.
120 *
121 * They are used in the `compose_active_planes` to convert and store a line
122 * from the src_buffer to the writeback buffer.
123 */
argb_u16_to_ARGB8888(struct vkms_frame_info * frame_info,const struct line_buffer * src_buffer,int y)124 static void argb_u16_to_ARGB8888(struct vkms_frame_info *frame_info,
125 const struct line_buffer *src_buffer, int y)
126 {
127 int x_dst = frame_info->dst.x1;
128 u8 *dst_pixels = packed_pixels_addr(frame_info, x_dst, y);
129 struct pixel_argb_u16 *in_pixels = src_buffer->pixels;
130 int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
131 src_buffer->n_pixels);
132
133 for (size_t x = 0; x < x_limit; x++, dst_pixels += 4) {
134 /*
135 * This sequence below is important because the format's byte order is
136 * in little-endian. In the case of the ARGB8888 the memory is
137 * organized this way:
138 *
139 * | Addr | = blue channel
140 * | Addr + 1 | = green channel
141 * | Addr + 2 | = Red channel
142 * | Addr + 3 | = Alpha channel
143 */
144 dst_pixels[3] = DIV_ROUND_CLOSEST(in_pixels[x].a, 257);
145 dst_pixels[2] = DIV_ROUND_CLOSEST(in_pixels[x].r, 257);
146 dst_pixels[1] = DIV_ROUND_CLOSEST(in_pixels[x].g, 257);
147 dst_pixels[0] = DIV_ROUND_CLOSEST(in_pixels[x].b, 257);
148 }
149 }
150
argb_u16_to_XRGB8888(struct vkms_frame_info * frame_info,const struct line_buffer * src_buffer,int y)151 static void argb_u16_to_XRGB8888(struct vkms_frame_info *frame_info,
152 const struct line_buffer *src_buffer, int y)
153 {
154 int x_dst = frame_info->dst.x1;
155 u8 *dst_pixels = packed_pixels_addr(frame_info, x_dst, y);
156 struct pixel_argb_u16 *in_pixels = src_buffer->pixels;
157 int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
158 src_buffer->n_pixels);
159
160 for (size_t x = 0; x < x_limit; x++, dst_pixels += 4) {
161 dst_pixels[3] = 0xff;
162 dst_pixels[2] = DIV_ROUND_CLOSEST(in_pixels[x].r, 257);
163 dst_pixels[1] = DIV_ROUND_CLOSEST(in_pixels[x].g, 257);
164 dst_pixels[0] = DIV_ROUND_CLOSEST(in_pixels[x].b, 257);
165 }
166 }
167
argb_u16_to_ARGB16161616(struct vkms_frame_info * frame_info,const struct line_buffer * src_buffer,int y)168 static void argb_u16_to_ARGB16161616(struct vkms_frame_info *frame_info,
169 const struct line_buffer *src_buffer, int y)
170 {
171 int x_dst = frame_info->dst.x1;
172 u16 *dst_pixels = packed_pixels_addr(frame_info, x_dst, y);
173 struct pixel_argb_u16 *in_pixels = src_buffer->pixels;
174 int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
175 src_buffer->n_pixels);
176
177 for (size_t x = 0; x < x_limit; x++, dst_pixels += 4) {
178 dst_pixels[3] = cpu_to_le16(in_pixels[x].a);
179 dst_pixels[2] = cpu_to_le16(in_pixels[x].r);
180 dst_pixels[1] = cpu_to_le16(in_pixels[x].g);
181 dst_pixels[0] = cpu_to_le16(in_pixels[x].b);
182 }
183 }
184
argb_u16_to_XRGB16161616(struct vkms_frame_info * frame_info,const struct line_buffer * src_buffer,int y)185 static void argb_u16_to_XRGB16161616(struct vkms_frame_info *frame_info,
186 const struct line_buffer *src_buffer, int y)
187 {
188 int x_dst = frame_info->dst.x1;
189 u16 *dst_pixels = packed_pixels_addr(frame_info, x_dst, y);
190 struct pixel_argb_u16 *in_pixels = src_buffer->pixels;
191 int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
192 src_buffer->n_pixels);
193
194 for (size_t x = 0; x < x_limit; x++, dst_pixels += 4) {
195 dst_pixels[3] = 0xffff;
196 dst_pixels[2] = cpu_to_le16(in_pixels[x].r);
197 dst_pixels[1] = cpu_to_le16(in_pixels[x].g);
198 dst_pixels[0] = cpu_to_le16(in_pixels[x].b);
199 }
200 }
201
argb_u16_to_RGB565(struct vkms_frame_info * frame_info,const struct line_buffer * src_buffer,int y)202 static void argb_u16_to_RGB565(struct vkms_frame_info *frame_info,
203 const struct line_buffer *src_buffer, int y)
204 {
205 int x_dst = frame_info->dst.x1;
206 u16 *dst_pixels = packed_pixels_addr(frame_info, x_dst, y);
207 struct pixel_argb_u16 *in_pixels = src_buffer->pixels;
208 int x_limit = min_t(size_t, drm_rect_width(&frame_info->dst),
209 src_buffer->n_pixels);
210
211 s64 fp_rb_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(31));
212 s64 fp_g_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(63));
213
214 for (size_t x = 0; x < x_limit; x++, dst_pixels++) {
215 s64 fp_r = drm_int2fixp(in_pixels[x].r);
216 s64 fp_g = drm_int2fixp(in_pixels[x].g);
217 s64 fp_b = drm_int2fixp(in_pixels[x].b);
218
219 u16 r = drm_fixp2int_round(drm_fixp_div(fp_r, fp_rb_ratio));
220 u16 g = drm_fixp2int_round(drm_fixp_div(fp_g, fp_g_ratio));
221 u16 b = drm_fixp2int_round(drm_fixp_div(fp_b, fp_rb_ratio));
222
223 *dst_pixels = cpu_to_le16(r << 11 | g << 5 | b);
224 }
225 }
226
get_pixel_conversion_function(u32 format)227 void *get_pixel_conversion_function(u32 format)
228 {
229 switch (format) {
230 case DRM_FORMAT_ARGB8888:
231 return &ARGB8888_to_argb_u16;
232 case DRM_FORMAT_XRGB8888:
233 return &XRGB8888_to_argb_u16;
234 case DRM_FORMAT_ARGB16161616:
235 return &ARGB16161616_to_argb_u16;
236 case DRM_FORMAT_XRGB16161616:
237 return &XRGB16161616_to_argb_u16;
238 case DRM_FORMAT_RGB565:
239 return &RGB565_to_argb_u16;
240 default:
241 return NULL;
242 }
243 }
244
get_line_to_frame_function(u32 format)245 void *get_line_to_frame_function(u32 format)
246 {
247 switch (format) {
248 case DRM_FORMAT_ARGB8888:
249 return &argb_u16_to_ARGB8888;
250 case DRM_FORMAT_XRGB8888:
251 return &argb_u16_to_XRGB8888;
252 case DRM_FORMAT_ARGB16161616:
253 return &argb_u16_to_ARGB16161616;
254 case DRM_FORMAT_XRGB16161616:
255 return &argb_u16_to_XRGB16161616;
256 case DRM_FORMAT_RGB565:
257 return &argb_u16_to_RGB565;
258 default:
259 return NULL;
260 }
261 }
262