fb_copyarea.h

  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 */
 29static 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 */
 35static 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 */
 43static 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 */
 50static 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 */
 96static 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
140static 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
158static __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
253static __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
348static __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
376static 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}