/*
 *  Generic fillrect for frame buffers with packed pixels of any depth. 
 *
 *      Copyright (C)  2000 James Simmons (jsimmons@linux-fbdev.org) 
 *
 *  This file is subject to the terms and conditions of the GNU General Public
 *  License.  See the file COPYING in the main directory of this archive for
 *  more details.
 *
 * NOTES:
 *
 *  The code for depths like 24 that don't have integer number of pixels per 
 *  long is broken and needs to be fixed. For now I turned these types of 
 *  mode off.
 *
 *  Also need to add code to deal with cards endians that are different than
 *  the native cpu endians. I also need to deal with MSB position in the word.
 *
 */
#include <linux/string.h>
#include <linux/fb.h>
#include <asm/types.h>
#include <video/fbcon.h>

#if BITS_PER_LONG == 32
#define FB_WRITEL(x,y) fb_writel(x,y)
#define FB_READL(x)    fb_readl(x)
#else
#define FB_WRITEL(x,y) fb_writeq(x,y)
#define FB_READL(x)    fb_readq(x)
#endif

#if defined (__BIG_ENDIAN)
#define SHIFT_HIGH(val, bits)  ((val) >> (bits))
#define SHIFT_LOW(val, bits)   ((val) << (bits))
#else
#define SHIFT_HIGH(val, bits)  ((val) << (bits))
#define SHIFT_LOW(val, bits)   ((val) >> (bits))
#endif

void cfb_fillrect(struct fb_info *p, struct fb_fillrect *rect)
{
	unsigned long start_index, pitch_index;
	unsigned long height, fg, bitstart, shift, color;
	unsigned long bpp = p->var.bits_per_pixel;
	unsigned long null_bits = BITS_PER_LONG - bpp;
	unsigned long n, x2, y2, linesize = p->fix.line_length;
	unsigned long bpl = sizeof(unsigned long);
	unsigned long *dst = NULL;
	char *dst1, *dst2;

	if (!rect->width || !rect->height)
		return;

	/* We could use hardware clipping but on many cards you get around
	 * hardware clipping by writing to framebuffer directly. */
	x2 = rect->dx + rect->width;
	y2 = rect->dy + rect->height;
	x2 = x2 < p->var.xres_virtual ? x2 : p->var.xres_virtual;
	y2 = y2 < p->var.yres_virtual ? y2 : p->var.yres_virtual;
	rect->width = x2 - rect->dx;
	height = y2 - rect->dy;

	if (p->fix.visual == FB_VISUAL_TRUECOLOR ||
	    p->fix.visual == FB_VISUAL_DIRECTCOLOR )
		fg = ((u32 *) (p->pseudo_palette))[rect->color];
	else
		fg = rect->color;

	bitstart = (((rect->dy * linesize) * 8) + 
		    rect->dx * bpp);
	
	start_index = bitstart & (BITS_PER_LONG - 1);

	/* line_length not a multiple of an unsigned long? */
	pitch_index = (linesize & (bpl - 1)) * 8;

	bitstart /= 8;
	bitstart &= ~(bpl - 1);
	dst1 = dst2 = p->screen_base + bitstart;

	switch (rect->rop) {
	case ROP_COPY:
		do {
			dst = (unsigned long *) dst1;
			shift = 0;
			color = 0;
			n = rect->width;

			/* 
			 * read leading bits
			 */
			if (start_index) {
				unsigned long start_mask = ~(SHIFT_HIGH(~0UL, start_index));
				
				color = FB_READL(dst) & start_mask;
				shift = start_index;
			}

			while (n--) {
				color |= SHIFT_HIGH(fg, shift);
				if (shift >= null_bits) {
					FB_WRITEL(color, dst++);
					if (shift == null_bits)
						color = 0;
					else
						color = SHIFT_LOW(color, BITS_PER_LONG - shift);
				}
				shift += bpp;
				shift &= (BITS_PER_LONG - 1);
			}
			
			/* 
			 * write trailing bits
			 */
			if (shift) {
				unsigned long end_mask = SHIFT_HIGH(~0UL, shift);

				FB_WRITEL((FB_READL(dst) & end_mask) | color, dst);
			}
			
			if (!pitch_index) {
				dst1 += linesize;
			}
			else {
				dst2 += linesize;
				dst1 = dst2;
				(unsigned long) dst1 &= ~(bpl - 1);
				start_index += pitch_index;
				start_index &= BITS_PER_LONG - 1;
			}

		} while (--height);
		break;
	case ROP_XOR:
		do {
			dst = (unsigned long *) dst1;
			shift = start_index;
			color = 0;
			n = rect->width;

			while (n--) {
				color |= SHIFT_HIGH(fg, shift);
				if (shift >= null_bits) {
					FB_WRITEL(FB_READL(dst) ^ color, dst);
					dst++;
					if (shift == null_bits)
						color = 0;
					else
						color = SHIFT_LOW(color, BITS_PER_LONG - shift);
				}
				shift += bpp;
				shift &= (BITS_PER_LONG - 1);
			}
			if (shift) 
				FB_WRITEL(FB_READL(dst) ^ color, dst);

			if (!pitch_index) {
				dst1 += linesize;
			}
			else {
				dst2 += linesize;
				dst1 = dst2;
				(unsigned long) dst1 &= ~(bpl - 1);
				start_index += pitch_index;
				start_index &= BITS_PER_LONG - 1;
			}
		} while (--height);
		break;
	}
	
	return;
}