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