1 /*
2  *  Generic Bit Block Transfer for frame buffers located in system RAM with
3  *  packed pixels of any depth.
4  *
5  *  Based almost entirely from cfbcopyarea.c (which is based almost entirely
6  *  on Geert Uytterhoeven's copyarea routine)
7  *
8  *      Copyright (C)  2007 Antonino Daplas <adaplas@pol.net>
9  *
10  *  This file is subject to the terms and conditions of the GNU General Public
11  *  License.  See the file COPYING in the main directory of this archive for
12  *  more details.
13  *
14  */
15 #include <linux/module.h>
16 #include <linux/kernel.h>
17 #include <linux/string.h>
18 #include <linux/fb.h>
19 #include <asm/types.h>
20 #include <asm/io.h>
21 #include "fb_draw.h"
22 
23     /*
24      *  Generic bitwise copy algorithm
25      */
26 
27 static void
bitcpy(struct fb_info * p,unsigned long * dst,unsigned dst_idx,const unsigned long * src,unsigned src_idx,int bits,unsigned n)28 bitcpy(struct fb_info *p, unsigned long *dst, unsigned dst_idx,
29 	const unsigned long *src, unsigned src_idx, int bits, unsigned n)
30 {
31 	unsigned long first, last;
32 	int const shift = dst_idx-src_idx;
33 	int left, right;
34 
35 	first = FB_SHIFT_HIGH(p, ~0UL, dst_idx);
36 	last = ~(FB_SHIFT_HIGH(p, ~0UL, (dst_idx+n) % bits));
37 
38 	if (!shift) {
39 		/* Same alignment for source and dest */
40 		if (dst_idx+n <= bits) {
41 			/* Single word */
42 			if (last)
43 				first &= last;
44 			*dst = comp(*src, *dst, first);
45 		} else {
46 			/* Multiple destination words */
47 			/* Leading bits */
48  			if (first != ~0UL) {
49 				*dst = comp(*src, *dst, first);
50 				dst++;
51 				src++;
52 				n -= bits - dst_idx;
53 			}
54 
55 			/* Main chunk */
56 			n /= bits;
57 			while (n >= 8) {
58 				*dst++ = *src++;
59 				*dst++ = *src++;
60 				*dst++ = *src++;
61 				*dst++ = *src++;
62 				*dst++ = *src++;
63 				*dst++ = *src++;
64 				*dst++ = *src++;
65 				*dst++ = *src++;
66 				n -= 8;
67 			}
68 			while (n--)
69 				*dst++ = *src++;
70 
71 			/* Trailing bits */
72 			if (last)
73 				*dst = comp(*src, *dst, last);
74 		}
75 	} else {
76 		unsigned long d0, d1;
77 		int m;
78 
79 		/* Different alignment for source and dest */
80 		right = shift & (bits - 1);
81 		left = -shift & (bits - 1);
82 
83 		if (dst_idx+n <= bits) {
84 			/* Single destination word */
85 			if (last)
86 				first &= last;
87 			if (shift > 0) {
88 				/* Single source word */
89 				*dst = comp(*src << left, *dst, first);
90 			} else if (src_idx+n <= bits) {
91 				/* Single source word */
92 				*dst = comp(*src >> right, *dst, first);
93 			} else {
94 				/* 2 source words */
95 				d0 = *src++;
96 				d1 = *src;
97 				*dst = comp(d0 >> right | d1 << left, *dst,
98 					    first);
99 			}
100 		} else {
101 			/* Multiple destination words */
102 			/** We must always remember the last value read,
103 			    because in case SRC and DST overlap bitwise (e.g.
104 			    when moving just one pixel in 1bpp), we always
105 			    collect one full long for DST and that might
106 			    overlap with the current long from SRC. We store
107 			    this value in 'd0'. */
108 			d0 = *src++;
109 			/* Leading bits */
110 			if (shift > 0) {
111 				/* Single source word */
112 				*dst = comp(d0 << left, *dst, first);
113 				dst++;
114 				n -= bits - dst_idx;
115 			} else {
116 				/* 2 source words */
117 				d1 = *src++;
118 				*dst = comp(d0 >> right | d1 << left, *dst,
119 					    first);
120 				d0 = d1;
121 				dst++;
122 				n -= bits - dst_idx;
123 			}
124 
125 			/* Main chunk */
126 			m = n % bits;
127 			n /= bits;
128 			while (n >= 4) {
129 				d1 = *src++;
130 				*dst++ = d0 >> right | d1 << left;
131 				d0 = d1;
132 				d1 = *src++;
133 				*dst++ = d0 >> right | d1 << left;
134 				d0 = d1;
135 				d1 = *src++;
136 				*dst++ = d0 >> right | d1 << left;
137 				d0 = d1;
138 				d1 = *src++;
139 				*dst++ = d0 >> right | d1 << left;
140 				d0 = d1;
141 				n -= 4;
142 			}
143 			while (n--) {
144 				d1 = *src++;
145 				*dst++ = d0 >> right | d1 << left;
146 				d0 = d1;
147 			}
148 
149 			/* Trailing bits */
150 			if (m) {
151 				if (m <= bits - right) {
152 					/* Single source word */
153 					d0 >>= right;
154 				} else {
155 					/* 2 source words */
156  					d1 = *src;
157 					d0 = d0 >> right | d1 << left;
158 				}
159 				*dst = comp(d0, *dst, last);
160 			}
161 		}
162 	}
163 }
164 
165     /*
166      *  Generic bitwise copy algorithm, operating backward
167      */
168 
169 static void
bitcpy_rev(struct fb_info * p,unsigned long * dst,unsigned dst_idx,const unsigned long * src,unsigned src_idx,unsigned bits,unsigned n)170 bitcpy_rev(struct fb_info *p, unsigned long *dst, unsigned dst_idx,
171 	   const unsigned long *src, unsigned src_idx, unsigned bits,
172 	   unsigned n)
173 {
174 	unsigned long first, last;
175 	int shift;
176 
177 	dst += (dst_idx + n - 1) / bits;
178 	src += (src_idx + n - 1) / bits;
179 	dst_idx = (dst_idx + n - 1) % bits;
180 	src_idx = (src_idx + n - 1) % bits;
181 
182 	shift = dst_idx-src_idx;
183 
184 	first = ~FB_SHIFT_HIGH(p, ~0UL, (dst_idx + 1) % bits);
185 	last = FB_SHIFT_HIGH(p, ~0UL, (bits + dst_idx + 1 - n) % bits);
186 
187 	if (!shift) {
188 		/* Same alignment for source and dest */
189 		if ((unsigned long)dst_idx+1 >= n) {
190 			/* Single word */
191 			if (first)
192 				last &= first;
193 			*dst = comp(*src, *dst, last);
194 		} else {
195 			/* Multiple destination words */
196 
197 			/* Leading bits */
198 			if (first) {
199 				*dst = comp(*src, *dst, first);
200 				dst--;
201 				src--;
202 				n -= dst_idx+1;
203 			}
204 
205 			/* Main chunk */
206 			n /= bits;
207 			while (n >= 8) {
208 				*dst-- = *src--;
209 				*dst-- = *src--;
210 				*dst-- = *src--;
211 				*dst-- = *src--;
212 				*dst-- = *src--;
213 				*dst-- = *src--;
214 				*dst-- = *src--;
215 				*dst-- = *src--;
216 				n -= 8;
217 			}
218 			while (n--)
219 				*dst-- = *src--;
220 			/* Trailing bits */
221 			if (last != -1UL)
222 				*dst = comp(*src, *dst, last);
223 		}
224 	} else {
225 		/* Different alignment for source and dest */
226 
227 		int const left = shift & (bits-1);
228 		int const right = -shift & (bits-1);
229 
230 		if ((unsigned long)dst_idx+1 >= n) {
231 			/* Single destination word */
232 			if (first)
233 				last &= first;
234 			if (shift < 0) {
235 				/* Single source word */
236 				*dst = comp(*src >> right, *dst, last);
237 			} else if (1+(unsigned long)src_idx >= n) {
238 				/* Single source word */
239 				*dst = comp(*src << left, *dst, last);
240 			} else {
241 				/* 2 source words */
242 				*dst = comp(*src << left | *(src-1) >> right,
243 					    *dst, last);
244 			}
245 		} else {
246 			/* Multiple destination words */
247 			/** We must always remember the last value read,
248 			    because in case SRC and DST overlap bitwise (e.g.
249 			    when moving just one pixel in 1bpp), we always
250 			    collect one full long for DST and that might
251 			    overlap with the current long from SRC. We store
252 			    this value in 'd0'. */
253 			unsigned long d0, d1;
254 			int m;
255 
256 			d0 = *src--;
257 			/* Leading bits */
258 			if (shift < 0) {
259 				/* Single source word */
260 				d1 = d0;
261 				d0 >>= right;
262 			} else {
263 				/* 2 source words */
264 				d1 = *src--;
265 				d0 = d0 << left | d1 >> right;
266 			}
267 			if (!first)
268 				*dst = d0;
269 			else
270 				*dst = comp(d0, *dst, first);
271 			d0 = d1;
272 			dst--;
273 			n -= dst_idx+1;
274 
275 			/* Main chunk */
276 			m = n % bits;
277 			n /= bits;
278 			while (n >= 4) {
279 				d1 = *src--;
280 				*dst-- = d0 << left | d1 >> right;
281 				d0 = d1;
282 				d1 = *src--;
283 				*dst-- = d0 << left | d1 >> right;
284 				d0 = d1;
285 				d1 = *src--;
286 				*dst-- = d0 << left | d1 >> right;
287 				d0 = d1;
288 				d1 = *src--;
289 				*dst-- = d0 << left | d1 >> right;
290 				d0 = d1;
291 				n -= 4;
292 			}
293 			while (n--) {
294 				d1 = *src--;
295 				*dst-- = d0 << left | d1 >> right;
296 				d0 = d1;
297 			}
298 
299 			/* Trailing bits */
300 			if (m) {
301 				if (m <= bits - left) {
302 					/* Single source word */
303 					d0 <<= left;
304 				} else {
305 					/* 2 source words */
306 					d1 = *src;
307 					d0 = d0 << left | d1 >> right;
308 				}
309 				*dst = comp(d0, *dst, last);
310 			}
311 		}
312 	}
313 }
314 
sys_copyarea(struct fb_info * p,const struct fb_copyarea * area)315 void sys_copyarea(struct fb_info *p, const struct fb_copyarea *area)
316 {
317 	u32 dx = area->dx, dy = area->dy, sx = area->sx, sy = area->sy;
318 	u32 height = area->height, width = area->width;
319 	unsigned long const bits_per_line = p->fix.line_length*8u;
320 	unsigned long *base = NULL;
321 	int bits = BITS_PER_LONG, bytes = bits >> 3;
322 	unsigned dst_idx = 0, src_idx = 0, rev_copy = 0;
323 
324 	if (p->state != FBINFO_STATE_RUNNING)
325 		return;
326 
327 	/* if the beginning of the target area might overlap with the end of
328 	the source area, be have to copy the area reverse. */
329 	if ((dy == sy && dx > sx) || (dy > sy)) {
330 		dy += height;
331 		sy += height;
332 		rev_copy = 1;
333 	}
334 
335 	/* split the base of the framebuffer into a long-aligned address and
336 	   the index of the first bit */
337 	base = (unsigned long *)((unsigned long)p->screen_base & ~(bytes-1));
338 	dst_idx = src_idx = 8*((unsigned long)p->screen_base & (bytes-1));
339 	/* add offset of source and target area */
340 	dst_idx += dy*bits_per_line + dx*p->var.bits_per_pixel;
341 	src_idx += sy*bits_per_line + sx*p->var.bits_per_pixel;
342 
343 	if (p->fbops->fb_sync)
344 		p->fbops->fb_sync(p);
345 
346 	if (rev_copy) {
347 		while (height--) {
348 			dst_idx -= bits_per_line;
349 			src_idx -= bits_per_line;
350 			bitcpy_rev(p, base + (dst_idx / bits), dst_idx % bits,
351 				base + (src_idx / bits), src_idx % bits, bits,
352 				width*p->var.bits_per_pixel);
353 		}
354 	} else {
355 		while (height--) {
356 			bitcpy(p, base + (dst_idx / bits), dst_idx % bits,
357 				base + (src_idx / bits), src_idx % bits, bits,
358 				width*p->var.bits_per_pixel);
359 			dst_idx += bits_per_line;
360 			src_idx += bits_per_line;
361 		}
362 	}
363 }
364 
365 EXPORT_SYMBOL(sys_copyarea);
366 
367 MODULE_AUTHOR("Antonino Daplas <adaplas@pol.net>");
368 MODULE_DESCRIPTION("Generic copyarea (sys-to-sys)");
369 MODULE_LICENSE("GPL");
370 
371