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 */ 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 */ 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 */ 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 */ 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 */ 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 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 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 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 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 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