/* Most of this is taken from vgadraw.c from svgalib */
/* Mitch - m.dsouza@mrc-apu.cam.ac.uk */

/* VGAlib version 1.2 - (c) 1993 Tommy Frandsen 		   */
/*								   */
/* This library is free software; you can redistribute it and/or   */
/* modify it without any restrictions. This library is distributed */
/* in the hope that it will be useful, but without any warranty.   */

/* Multi-chipset support Copyright 1993 Harm Hanemaayer */
/* partially copyrighted (C) 1993 by Hartmut Schirmer */

#include <stdio.h>
#include <string.h>
#include "vga.h"
#define GRA_I   0x3CE   /* Graphics Controller Index */
#define SEQ_I   0x3C4   /* Sequencer Index */

#define GRA_D   0x3CF   /* Graphics Controller Data Register */
#define SEQ_D   0x3C5   /* Sequencer Data Register */



static inline void port_out( int value, int port )
{
	__asm__ volatile ("outb %0,%1"
	: : "a" ((unsigned char)value), "d" ((unsigned short)port));
}

/* used to decompose color value into bits (for fast scanline drawing) */
union bits {
    struct {
        unsigned char bit3;
        unsigned char bit2;
        unsigned char bit1;
        unsigned char bit0;
    } b;
    unsigned int i;
};

/* color decompositions */
static union bits color16[16] = {{0,0,0,0},
			         {0,0,0,1},
			      	 {0,0,1,0},
			         {0,0,1,1},
			         {0,1,0,0},
			         {0,1,0,1},
			         {0,1,1,0},
			         {0,1,1,1},
			         {1,0,0,0},
			         {1,0,0,1},
			         {1,0,1,0},
			         {1,0,1,1},
			         {1,1,0,0},
			         {1,1,0,1},
			         {1,1,1,0},
			         {1,1,1,1}};

/* display plane buffers (for fast scanline drawing) */
/* 256 bytes -> max 2048 pixel per line (2/16 colors)*/
static unsigned char plane0[256];
static unsigned char plane1[256];
static unsigned char plane2[256];
static unsigned char plane3[256];

int vga_getscanline(int line, unsigned char* colors)
{
int xdim=vga_getxdim(), ncolors=vga_getcolors();
int xbytes=vga_getmodeinfo(vga_getcurrentmode())->linewidth;

    if ((ncolors == 2) || (ncolors > 256))
	return vga_getscansegment(colors, 0, line, xbytes);
    else
	return vga_getscansegment(colors, 0, line, xdim);
} 

int vga_getscansegment(unsigned char* colors, int x, int y, int length)
{
int xdim=vga_getxdim(), ncolors=vga_getcolors();
char *GM=vga_getgraphmem();
int CM=vga_getcurrentmode();
int xbytes=vga_getmodeinfo(CM)->linewidth;

    /* both length and x must divide with 8 */

    if (vga_getmodeinfo(CM)->flags & IS_MODEX) goto modeX;
    switch (ncolors) {
        case 16:
            {
       		int i, j, k, first, last, page, l1, l2;
                int offset;
		union bits bytes;
                unsigned char* address;

                k = 0;
#if 0
                for(i = 0; i < length; i += 8) {
                    bytes.i = 0;
                    first = i;
                    last  = i+8;
                    for(j = first; j < last; j++)
                       bytes.i = (bytes.i<<1) | color16[colors[j]].i;
		    plane0[k]   = bytes.b.bit0;
		    plane1[k]   = bytes.b.bit1;
		    plane2[k]   = bytes.b.bit2;
		    plane3[k++] = bytes.b.bit3;
                }
#endif
                offset = (y*xdim + x)/8;
		vga_setpage((page = offset>>16));
		l1 = 0x10000 - (offset &= 0xffff);
		length /= 8;
		if (l1 > length) l1 = length;
		l2 = length - l1; 
		
                address = GM + offset;

#if 1

#if 0
		/* disable Set/Reset Register */
	    	port_out(0x01, GRA_I ); 
    		port_out(0x00, GRA_D ); 

		/* write to all bits */
	        port_out(0x08, GRA_I ); 
    		port_out(0xFF, GRA_D );   

		/* select map mask register */
	    	port_out(0x02, SEQ_I ); 
#endif
		port_out(0x04, GRA_I); 

                /* read plane 0 */
    		port_out(0x00, GRA_D ); 
	        bcopy(address, plane0, l1);

                /* read plane 1 */
    		port_out(0x01, GRA_D ); 
	        bcopy(address, plane1, l1);

                /* read plane 2 */
    		port_out(0x02, GRA_D ); 
	        bcopy(address, plane2, l1);

                /* read plane 3 */
    		port_out(0x03, GRA_D ); 
	        bcopy(address, plane3, l1);

		if (l2 > 0) {
		    vga_setpage(page+1);

                    /* read plane 0 */
    		    port_out(0x00, GRA_D ); 
	            bcopy( GM, &plane0[l1], l2);

                    /* read plane 1 */
    		    port_out(0x01, GRA_D ); 
	            bcopy( GM, &plane1[l1], l2);

                    /* read plane 2 */
    		    port_out(0x02, GRA_D ); 
	            bcopy( GM, &plane2[l1], l2);

                    /* read plane 3 */
    		    port_out(0x03, GRA_D ); 
	            bcopy( GM, &plane3[l1], l2);
		}

                /* restore map mask register */
    		port_out(0x0F, SEQ_D ); 
  
		/* enable Set/Reset Register */
	        port_out(0x01, GRA_I );
                port_out(0x0F, GRA_D );

                for(k = i = 0; i < length*8; i += 8) {
                    bytes.i = 0;
                    first = i;
                    last  = i+8;
		    bytes.b.bit0 = plane0[k];
		    bytes.b.bit1 = plane1[k];
		    bytes.b.bit2 = plane2[k];
		    bytes.b.bit3 = plane3[k];

                    for(j = first; j < last; j++) {
			union bits b;

			b = (bytes.i>>(j-first));
			colors[i+7-(j-first)]=((b.b.bit3&1)<<3) |
					  ((b.b.bit2&1)<<2) |
					  ((b.b.bit1&1)<<1) |
					  ((b.b.bit0&1));
		    }
		    k++;
                }
#endif
#if 0
fprintf(stderr,"In %s got l1 = %d, length = %d, x=%d, y=%d\n",__FILE__,l1, length, x, y);
		for (i = 0; i < 4; i++) {
        		/* save plane i */
			port_out(0x04, GRA_I); 
			port_out(i, GRA_D);
            		bcopy(address, colors, l1);
#endif		}

	    }

            break;
        case 2 :
	    {
		/* disable Set/Reset Register */
	    	port_out(0x01, GRA_I ); 
	        port_out(0x00, GRA_D );

		/* read to all bits */
	        port_out(0x08, GRA_I ); 
    		port_out(0xFF, GRA_D );   

		/* read to all planes */
	    	port_out(0x02, SEQ_I ); 
    		port_out(0x0F, SEQ_D ); 

	    	bcopy( GM + (y*xdim + x)/8, colors, length);

                /* restore map mask register */
    		port_out(0x0F, SEQ_D ); 
  
		/* enable Set/Reset Register */
	    	port_out(0x01, GRA_I ); 
    		port_out(0x0F, GRA_D );   
	      }
            break;
	case 256:
            {
	      switch (CM) {
		  case G320x200x256: /* linear addressing - easy and fast */
		                     bcopy(GM+y*xdim+x,colors,length);
				     return 0;
	          case G320x240x256:
	          case G320x400x256:
	          case G360x480x256:
modeX:
				     {
				       int first, last, offset, pixel, plane;

				       /* select map mask register */ 
				       port_out(0x04, GRA_I); 

				       for(plane = 0; plane < 4; plane++) {
					 /* select plane */
					 port_out(plane, GRA_D);   

					 pixel = plane;
					 first = (y*xdim + x)/4;
					 last  = (y*xdim + x + length)/4;
					 for(offset = first; offset < last; offset++) {
					   colors[pixel] = GM[offset];  
					   pixel += 4;
					 }
				       }
				     }
				     return 0;
	      }
	      {
		unsigned long offset;
		int segment, free;

  SegmentedCopy:
		offset  = y*xbytes+x;
		segment = offset >> 16;
		free	= ((segment+1)<<16)-offset;
		offset  &= 0xFFFF;

		if (free < length) {
			vga_setpage(segment);
			bcopy(GM + offset, colors, free);
   		    	vga_setpage(segment + 1);
			bcopy(GM, colors+free, length-free);
		} else {
			vga_setpage(segment);
			bcopy(GM + offset, colors, length);
		}
	      }
	    }  
            break;
        case 32768:
	case 65536: x *= 2;
	            goto SegmentedCopy;
	case 1<<24: x*=3;
	            goto SegmentedCopy;
    }

    return 0;
}

