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
28 bitcpy(struct fb_info *p, unsigned long *dst, int dst_idx,
29 		const unsigned long *src, int 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 >> right, *dst, first);
90 			} else if (src_idx+n <= bits) {
91 				/* Single source word */
92 				*dst = comp(*src << left, *dst, first);
93 			} else {
94 				/* 2 source words */
95 				d0 = *src++;
96 				d1 = *src;
97 				*dst = comp(d0 << left | d1 >> right, *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 >> right, *dst, first);
113 				dst++;
114 				n -= bits - dst_idx;
115 			} else {
116 				/* 2 source words */
117 				d1 = *src++;
118 				*dst = comp(d0 << left | *dst >> right, *dst, first);
119 				d0 = d1;
120 				dst++;
121 				n -= bits - dst_idx;
122 			}
123 
124 			/* Main chunk */
125 			m = n % bits;
126 			n /= bits;
127 			while (n >= 4) {
128 				d1 = *src++;
129 				*dst++ = d0 << left | d1 >> right;
130 				d0 = d1;
131 				d1 = *src++;
132 				*dst++ = d0 << left | d1 >> right;
133 				d0 = d1;
134 				d1 = *src++;
135 				*dst++ = d0 << left | d1 >> right;
136 				d0 = d1;
137 				d1 = *src++;
138 				*dst++ = d0 << left | d1 >> right;
139 				d0 = d1;
140 				n -= 4;
141 			}
142 			while (n--) {
143 				d1 = *src++;
144 				*dst++ = d0 << left | d1 >> right;
145 				d0 = d1;
146 			}
147 
148 			/* Trailing bits */
149 			if (last) {
150 				if (m <= right) {
151 					/* Single source word */
152 					*dst = comp(d0 << left, *dst, last);
153 				} else {
154 					/* 2 source words */
155  					d1 = *src;
156 					*dst = comp(d0 << left | d1 >> right,
157 						    *dst, last);
158 				}
159 			}
160 		}
161 	}
162 }
163 
164     /*
165      *  Generic bitwise copy algorithm, operating backward
166      */
167 
168 static void
169 bitcpy_rev(struct fb_info *p, unsigned long *dst, int dst_idx,
170 		const unsigned long *src, int src_idx, int bits, unsigned n)
171 {
172 	unsigned long first, last;
173 	int shift;
174 
175 	dst += (n-1)/bits;
176 	src += (n-1)/bits;
177 	if ((n-1) % bits) {
178 		dst_idx += (n-1) % bits;
179 		dst += dst_idx >> (ffs(bits) - 1);
180 		dst_idx &= bits - 1;
181 		src_idx += (n-1) % bits;
182 		src += src_idx >> (ffs(bits) - 1);
183 		src_idx &= bits - 1;
184 	}
185 
186 	shift = dst_idx-src_idx;
187 
188 	first = FB_SHIFT_LOW(p, ~0UL, bits - 1 - dst_idx);
189 	last = ~(FB_SHIFT_LOW(p, ~0UL, bits - 1 - ((dst_idx-n) % bits)));
190 
191 	if (!shift) {
192 		/* Same alignment for source and dest */
193 		if ((unsigned long)dst_idx+1 >= n) {
194 			/* Single word */
195 			if (last)
196 				first &= last;
197 			*dst = comp(*src, *dst, first);
198 		} else {
199 			/* Multiple destination words */
200 
201 			/* Leading bits */
202 			if (first != ~0UL) {
203 				*dst = comp(*src, *dst, first);
204 				dst--;
205 				src--;
206 				n -= dst_idx+1;
207 			}
208 
209 			/* Main chunk */
210 			n /= bits;
211 			while (n >= 8) {
212 				*dst-- = *src--;
213 				*dst-- = *src--;
214 				*dst-- = *src--;
215 				*dst-- = *src--;
216 				*dst-- = *src--;
217 				*dst-- = *src--;
218 				*dst-- = *src--;
219 				*dst-- = *src--;
220 				n -= 8;
221 			}
222 			while (n--)
223 				*dst-- = *src--;
224 			/* Trailing bits */
225 			if (last)
226 				*dst = comp(*src, *dst, last);
227 		}
228 	} else {
229 		/* Different alignment for source and dest */
230 
231 		int const left = -shift & (bits-1);
232 		int const right = shift & (bits-1);
233 
234 		if ((unsigned long)dst_idx+1 >= n) {
235 			/* Single destination word */
236 			if (last)
237 				first &= last;
238 			if (shift < 0) {
239 				/* Single source word */
240 				*dst = comp(*src << left, *dst, first);
241 			} else if (1+(unsigned long)src_idx >= n) {
242 				/* Single source word */
243 				*dst = comp(*src >> right, *dst, first);
244 			} else {
245 				/* 2 source words */
246 				*dst = comp(*src >> right | *(src-1) << left,
247 					    *dst, first);
248 			}
249 		} else {
250 			/* Multiple destination words */
251 			/** We must always remember the last value read,
252 			    because in case SRC and DST overlap bitwise (e.g.
253 			    when moving just one pixel in 1bpp), we always
254 			    collect one full long for DST and that might
255 			    overlap with the current long from SRC. We store
256 			    this value in 'd0'. */
257 			unsigned long d0, d1;
258 			int m;
259 
260 			d0 = *src--;
261 			/* Leading bits */
262 			if (shift < 0) {
263 				/* Single source word */
264 				*dst = comp(d0 << left, *dst, first);
265 			} else {
266 				/* 2 source words */
267 				d1 = *src--;
268 				*dst = comp(d0 >> right | d1 << left, *dst,
269 					    first);
270 				d0 = d1;
271 			}
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 >> right | d1 << left;
281 				d0 = d1;
282 				d1 = *src--;
283 				*dst-- = d0 >> right | d1 << left;
284 				d0 = d1;
285 				d1 = *src--;
286 				*dst-- = d0 >> right | d1 << left;
287 				d0 = d1;
288 				d1 = *src--;
289 				*dst-- = d0 >> right | d1 << left;
290 				d0 = d1;
291 				n -= 4;
292 			}
293 			while (n--) {
294 				d1 = *src--;
295 				*dst-- = d0 >> right | d1 << left;
296 				d0 = d1;
297 			}
298 
299 			/* Trailing bits */
300 			if (last) {
301 				if (m <= left) {
302 					/* Single source word */
303 					*dst = comp(d0 >> right, *dst, last);
304 				} else {
305 					/* 2 source words */
306 					d1 = *src;
307 					*dst = comp(d0 >> right | d1 << left,
308 						    *dst, last);
309 				}
310 			}
311 		}
312 	}
313 }
314 
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 *dst = NULL, *src = NULL;
321 	int bits = BITS_PER_LONG, bytes = bits >> 3;
322 	int 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 	dst = src = (unsigned long *)((unsigned long)p->screen_base &
338 				      ~(bytes-1));
339 	dst_idx = src_idx = 8*((unsigned long)p->screen_base & (bytes-1));
340 	/* add offset of source and target area */
341 	dst_idx += dy*bits_per_line + dx*p->var.bits_per_pixel;
342 	src_idx += sy*bits_per_line + sx*p->var.bits_per_pixel;
343 
344 	if (p->fbops->fb_sync)
345 		p->fbops->fb_sync(p);
346 
347 	if (rev_copy) {
348 		while (height--) {
349 			dst_idx -= bits_per_line;
350 			src_idx -= bits_per_line;
351 			dst += dst_idx >> (ffs(bits) - 1);
352 			dst_idx &= (bytes - 1);
353 			src += src_idx >> (ffs(bits) - 1);
354 			src_idx &= (bytes - 1);
355 			bitcpy_rev(p, dst, dst_idx, src, src_idx, bits,
356 				width*p->var.bits_per_pixel);
357 		}
358 	} else {
359 		while (height--) {
360 			dst += dst_idx >> (ffs(bits) - 1);
361 			dst_idx &= (bytes - 1);
362 			src += src_idx >> (ffs(bits) - 1);
363 			src_idx &= (bytes - 1);
364 			bitcpy(p, dst, dst_idx, src, src_idx, bits,
365 				width*p->var.bits_per_pixel);
366 			dst_idx += bits_per_line;
367 			src_idx += bits_per_line;
368 		}
369 	}
370 }
371 
372 EXPORT_SYMBOL(sys_copyarea);
373 
374 MODULE_AUTHOR("Antonino Daplas <adaplas@pol.net>");
375 MODULE_DESCRIPTION("Generic copyarea (sys-to-sys)");
376 MODULE_LICENSE("GPL");
377 
378