1 /* SPDX-License-Identifier: GPL-2.0-only
2 *
3 * Generic bit area copy and twister engine for packed pixel framebuffers
4 *
5 * Rewritten by:
6 * Copyright (C) 2025 Zsolt Kajtar (soci@c64.rulez.org)
7 *
8 * Based on previous work of:
9 * Copyright (C) 1999-2005 James Simmons <jsimmons@www.infradead.org>
10 * Anton Vorontsov <avorontsov@ru.mvista.com>
11 * Pavel Pisa <pisa@cmp.felk.cvut.cz>
12 * Antonino Daplas <adaplas@hotpop.com>
13 * Geert Uytterhoeven
14 * and others
15 *
16 * NOTES:
17 *
18 * Handles native and foreign byte order on both endians, standard and
19 * reverse pixel order in a byte (<8 BPP), word length of 32/64 bits,
20 * bits per pixel from 1 to the word length. Handles line lengths at byte
21 * granularity while maintaining aligned accesses.
22 *
23 * Optimized routines for word aligned copying and byte aligned copying
24 * on reverse pixel framebuffers.
25 */
26 #include "fb_draw.h"
27
28 /* used when no reversing is necessary */
fb_no_reverse(unsigned long val,struct fb_reverse reverse)29 static inline unsigned long fb_no_reverse(unsigned long val, struct fb_reverse reverse)
30 {
31 return val;
32 }
33
34 /* modifies the masked area in a word */
fb_copy_offset_masked(unsigned long mask,int offset,const struct fb_address * dst,const struct fb_address * src)35 static inline void fb_copy_offset_masked(unsigned long mask, int offset,
36 const struct fb_address *dst,
37 const struct fb_address *src)
38 {
39 fb_modify_offset(fb_read_offset(offset, src), mask, offset, dst);
40 }
41
42 /* copies the whole word */
fb_copy_offset(int offset,const struct fb_address * dst,const struct fb_address * src)43 static inline void fb_copy_offset(int offset, const struct fb_address *dst,
44 const struct fb_address *src)
45 {
46 fb_write_offset(fb_read_offset(offset, src), offset, dst);
47 }
48
49 /* forward aligned copy */
fb_copy_aligned_fwd(const struct fb_address * dst,const struct fb_address * src,int end,struct fb_reverse reverse)50 static inline void fb_copy_aligned_fwd(const struct fb_address *dst,
51 const struct fb_address *src,
52 int end, struct fb_reverse reverse)
53 {
54 unsigned long first, last;
55
56 first = fb_pixel_mask(dst->bits, reverse);
57 last = ~fb_pixel_mask(end & (BITS_PER_LONG-1), reverse);
58
59 /* Same alignment for source and dest */
60 if (end <= BITS_PER_LONG) {
61 /* Single word */
62 last = last ? (last & first) : first;
63
64 /* Trailing bits */
65 if (last == ~0UL)
66 fb_copy_offset(0, dst, src);
67 else
68 fb_copy_offset_masked(last, 0, dst, src);
69 } else {
70 /* Multiple destination words */
71 int offset = first != ~0UL;
72
73 /* Leading bits */
74 if (offset)
75 fb_copy_offset_masked(first, 0, dst, src);
76
77 /* Main chunk */
78 end /= BITS_PER_LONG;
79 while (offset + 4 <= end) {
80 fb_copy_offset(offset + 0, dst, src);
81 fb_copy_offset(offset + 1, dst, src);
82 fb_copy_offset(offset + 2, dst, src);
83 fb_copy_offset(offset + 3, dst, src);
84 offset += 4;
85 }
86 while (offset < end)
87 fb_copy_offset(offset++, dst, src);
88
89 /* Trailing bits */
90 if (last)
91 fb_copy_offset_masked(last, offset, dst, src);
92 }
93 }
94
95 /* reverse aligned copy */
fb_copy_aligned_rev(const struct fb_address * dst,const struct fb_address * src,int end,struct fb_reverse reverse)96 static inline void fb_copy_aligned_rev(const struct fb_address *dst,
97 const struct fb_address *src,
98 int end, struct fb_reverse reverse)
99 {
100 unsigned long first, last;
101
102 first = fb_pixel_mask(dst->bits, reverse);
103 last = ~fb_pixel_mask(end & (BITS_PER_LONG-1), reverse);
104
105 if (end <= BITS_PER_LONG) {
106 /* Single word */
107 if (last)
108 first &= last;
109 if (first == ~0UL)
110 fb_copy_offset(0, dst, src);
111 else
112 fb_copy_offset_masked(first, 0, dst, src);
113 } else {
114 /* Multiple destination words */
115 int offset = first != ~0UL;
116
117 /* Trailing bits */
118 end /= BITS_PER_LONG;
119
120 if (last)
121 fb_copy_offset_masked(last, end, dst, src);
122
123 /* Main chunk */
124 while (end >= offset + 4) {
125 fb_copy_offset(end - 1, dst, src);
126 fb_copy_offset(end - 2, dst, src);
127 fb_copy_offset(end - 3, dst, src);
128 fb_copy_offset(end - 4, dst, src);
129 end -= 4;
130 }
131 while (end > offset)
132 fb_copy_offset(--end, dst, src);
133
134 /* Leading bits */
135 if (offset)
136 fb_copy_offset_masked(first, 0, dst, src);
137 }
138 }
139
fb_copy_aligned(struct fb_address * dst,struct fb_address * src,int width,u32 height,unsigned int bits_per_line,struct fb_reverse reverse,bool rev_copy)140 static inline void fb_copy_aligned(struct fb_address *dst, struct fb_address *src,
141 int width, u32 height, unsigned int bits_per_line,
142 struct fb_reverse reverse, bool rev_copy)
143 {
144 if (rev_copy)
145 while (height--) {
146 fb_copy_aligned_rev(dst, src, width + dst->bits, reverse);
147 fb_address_backward(dst, bits_per_line);
148 fb_address_backward(src, bits_per_line);
149 }
150 else
151 while (height--) {
152 fb_copy_aligned_fwd(dst, src, width + dst->bits, reverse);
153 fb_address_forward(dst, bits_per_line);
154 fb_address_forward(src, bits_per_line);
155 }
156 }
157
fb_copy_fwd(const struct fb_address * dst,const struct fb_address * src,int width,unsigned long (* reorder)(unsigned long val,struct fb_reverse reverse),struct fb_reverse reverse)158 static __always_inline void fb_copy_fwd(const struct fb_address *dst,
159 const struct fb_address *src, int width,
160 unsigned long (*reorder)(unsigned long val,
161 struct fb_reverse reverse),
162 struct fb_reverse reverse)
163 {
164 unsigned long first, last;
165 unsigned long d0, d1;
166 int end = dst->bits + width;
167 int shift, left, right;
168
169 first = fb_pixel_mask(dst->bits, reverse);
170 last = ~fb_pixel_mask(end & (BITS_PER_LONG-1), reverse);
171
172 shift = dst->bits - src->bits;
173 right = shift & (BITS_PER_LONG - 1);
174 left = -shift & (BITS_PER_LONG - 1);
175
176 if (end <= BITS_PER_LONG) {
177 /* Single destination word */
178 last = last ? (last & first) : first;
179 if (shift < 0) {
180 d0 = fb_left(reorder(fb_read_offset(-1, src), reverse), left);
181 if (src->bits + width > BITS_PER_LONG)
182 d0 |= fb_right(reorder(fb_read_offset(0, src), reverse), right);
183
184 if (last == ~0UL)
185 fb_write_offset(reorder(d0, reverse), 0, dst);
186 else
187 fb_modify_offset(reorder(d0, reverse), last, 0, dst);
188 } else {
189 d0 = fb_right(reorder(fb_read_offset(0, src), reverse), right);
190 fb_modify_offset(reorder(d0, reverse), last, 0, dst);
191 }
192 } else {
193 /* Multiple destination words */
194 int offset = first != ~0UL;
195
196 /* Leading bits */
197 if (shift < 0)
198 d0 = reorder(fb_read_offset(-1, src), reverse);
199 else
200 d0 = 0;
201
202 /* 2 source words */
203 if (offset) {
204 d1 = reorder(fb_read_offset(0, src), reverse);
205 d0 = fb_left(d0, left) | fb_right(d1, right);
206 fb_modify_offset(reorder(d0, reverse), first, 0, dst);
207 d0 = d1;
208 }
209
210 /* Main chunk */
211 end /= BITS_PER_LONG;
212 if (reorder == fb_no_reverse)
213 while (offset + 4 <= end) {
214 d1 = fb_read_offset(offset + 0, src);
215 d0 = fb_left(d0, left) | fb_right(d1, right);
216 fb_write_offset(d0, offset + 0, dst);
217 d0 = d1;
218 d1 = fb_read_offset(offset + 1, src);
219 d0 = fb_left(d0, left) | fb_right(d1, right);
220 fb_write_offset(d0, offset + 1, dst);
221 d0 = d1;
222 d1 = fb_read_offset(offset + 2, src);
223 d0 = fb_left(d0, left) | fb_right(d1, right);
224 fb_write_offset(d0, offset + 2, dst);
225 d0 = d1;
226 d1 = fb_read_offset(offset + 3, src);
227 d0 = fb_left(d0, left) | fb_right(d1, right);
228 fb_write_offset(d0, offset + 3, dst);
229 d0 = d1;
230 offset += 4;
231 }
232
233 while (offset < end) {
234 d1 = reorder(fb_read_offset(offset, src), reverse);
235 d0 = fb_left(d0, left) | fb_right(d1, right);
236 fb_write_offset(reorder(d0, reverse), offset, dst);
237 d0 = d1;
238 offset++;
239 }
240
241 /* Trailing bits */
242 if (last) {
243 d0 = fb_left(d0, left);
244 if (src->bits + width
245 > offset * BITS_PER_LONG + ((shift < 0) ? BITS_PER_LONG : 0))
246 d0 |= fb_right(reorder(fb_read_offset(offset, src), reverse),
247 right);
248 fb_modify_offset(reorder(d0, reverse), last, offset, dst);
249 }
250 }
251 }
252
fb_copy_rev(const struct fb_address * dst,const struct fb_address * src,int end,unsigned long (* reorder)(unsigned long val,struct fb_reverse reverse),struct fb_reverse reverse)253 static __always_inline void fb_copy_rev(const struct fb_address *dst,
254 const struct fb_address *src, int end,
255 unsigned long (*reorder)(unsigned long val,
256 struct fb_reverse reverse),
257 struct fb_reverse reverse)
258 {
259 unsigned long first, last;
260 unsigned long d0, d1;
261 int shift, left, right;
262
263 first = fb_pixel_mask(dst->bits, reverse);
264 last = ~fb_pixel_mask(end & (BITS_PER_LONG-1), reverse);
265
266 shift = dst->bits - src->bits;
267 right = shift & (BITS_PER_LONG-1);
268 left = -shift & (BITS_PER_LONG-1);
269
270 if (end <= BITS_PER_LONG) {
271 /* Single destination word */
272 if (last)
273 first &= last;
274
275 if (shift > 0) {
276 d0 = fb_right(reorder(fb_read_offset(1, src), reverse), right);
277 if (src->bits > left)
278 d0 |= fb_left(reorder(fb_read_offset(0, src), reverse), left);
279 fb_modify_offset(reorder(d0, reverse), first, 0, dst);
280 } else {
281 d0 = fb_left(reorder(fb_read_offset(0, src), reverse), left);
282 if (src->bits + end - dst->bits > BITS_PER_LONG)
283 d0 |= fb_right(reorder(fb_read_offset(1, src), reverse), right);
284 if (first == ~0UL)
285 fb_write_offset(reorder(d0, reverse), 0, dst);
286 else
287 fb_modify_offset(reorder(d0, reverse), first, 0, dst);
288 }
289 } else {
290 /* Multiple destination words */
291 int offset = first != ~0UL;
292
293 end /= BITS_PER_LONG;
294
295 /* 2 source words */
296 if (fb_right(~0UL, right) & last)
297 d0 = fb_right(reorder(fb_read_offset(end + 1, src), reverse), right);
298 else
299 d0 = 0;
300
301 /* Trailing bits */
302 d1 = reorder(fb_read_offset(end, src), reverse);
303 if (last)
304 fb_modify_offset(reorder(fb_left(d1, left) | d0, reverse),
305 last, end, dst);
306 d0 = d1;
307
308 /* Main chunk */
309 if (reorder == fb_no_reverse)
310 while (end >= offset + 4) {
311 d1 = fb_read_offset(end - 1, src);
312 d0 = fb_left(d1, left) | fb_right(d0, right);
313 fb_write_offset(d0, end - 1, dst);
314 d0 = d1;
315 d1 = fb_read_offset(end - 2, src);
316 d0 = fb_left(d1, left) | fb_right(d0, right);
317 fb_write_offset(d0, end - 2, dst);
318 d0 = d1;
319 d1 = fb_read_offset(end - 3, src);
320 d0 = fb_left(d1, left) | fb_right(d0, right);
321 fb_write_offset(d0, end - 3, dst);
322 d0 = d1;
323 d1 = fb_read_offset(end - 4, src);
324 d0 = fb_left(d1, left) | fb_right(d0, right);
325 fb_write_offset(d0, end - 4, dst);
326 d0 = d1;
327 end -= 4;
328 }
329
330 while (end > offset) {
331 end--;
332 d1 = reorder(fb_read_offset(end, src), reverse);
333 d0 = fb_left(d1, left) | fb_right(d0, right);
334 fb_write_offset(reorder(d0, reverse), end, dst);
335 d0 = d1;
336 }
337
338 /* Leading bits */
339 if (offset) {
340 d0 = fb_right(d0, right);
341 if (src->bits > left)
342 d0 |= fb_left(reorder(fb_read_offset(0, src), reverse), left);
343 fb_modify_offset(reorder(d0, reverse), first, 0, dst);
344 }
345 }
346 }
347
fb_copy(struct fb_address * dst,struct fb_address * src,int width,u32 height,unsigned int bits_per_line,unsigned long (* reorder)(unsigned long val,struct fb_reverse reverse),struct fb_reverse reverse,bool rev_copy)348 static __always_inline void fb_copy(struct fb_address *dst, struct fb_address *src,
349 int width, u32 height, unsigned int bits_per_line,
350 unsigned long (*reorder)(unsigned long val,
351 struct fb_reverse reverse),
352 struct fb_reverse reverse, bool rev_copy)
353 {
354 if (rev_copy)
355 while (height--) {
356 int move = src->bits < dst->bits ? -1 : 0;
357
358 fb_address_move_long(src, move);
359 fb_copy_rev(dst, src, width + dst->bits, reorder, reverse);
360 fb_address_backward(dst, bits_per_line);
361 fb_address_backward(src, bits_per_line);
362 fb_address_move_long(src, -move);
363 }
364 else
365 while (height--) {
366 int move = src->bits > dst->bits ? 1 : 0;
367
368 fb_address_move_long(src, move);
369 fb_copy_fwd(dst, src, width, reorder, reverse);
370 fb_address_forward(dst, bits_per_line);
371 fb_address_forward(src, bits_per_line);
372 fb_address_move_long(src, -move);
373 }
374 }
375
fb_copyarea(struct fb_info * p,const struct fb_copyarea * area)376 static inline void fb_copyarea(struct fb_info *p, const struct fb_copyarea *area)
377 {
378 int bpp = p->var.bits_per_pixel;
379 u32 dy = area->dy;
380 u32 sy = area->sy;
381 u32 height = area->height;
382 int width = area->width * bpp;
383 unsigned int bits_per_line = BYTES_TO_BITS(p->fix.line_length);
384 struct fb_reverse reverse = fb_reverse_init(p);
385 struct fb_address dst = fb_address_init(p);
386 struct fb_address src = dst;
387 bool rev_copy = (dy > sy) || (dy == sy && area->dx > area->sx);
388
389 if (rev_copy) {
390 dy += height - 1;
391 sy += height - 1;
392 }
393 fb_address_forward(&dst, dy*bits_per_line + area->dx*bpp);
394 fb_address_forward(&src, sy*bits_per_line + area->sx*bpp);
395
396 if (src.bits == dst.bits)
397 fb_copy_aligned(&dst, &src, width, height, bits_per_line, reverse, rev_copy);
398 else if (!reverse.byte && (!reverse.pixel ||
399 !((src.bits ^ dst.bits) & (BITS_PER_BYTE-1)))) {
400 fb_copy(&dst, &src, width, height, bits_per_line,
401 fb_no_reverse, reverse, rev_copy);
402 } else
403 fb_copy(&dst, &src, width, height, bits_per_line,
404 fb_reverse_long, reverse, rev_copy);
405 }
406