/* 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 */
/* HH: Added 4bpp support, and use bytesperpixel. */

#include <stdio.h>
#include "vga.h"
#include "libvga.h"


static inline void read_write(char *p)
{
  ++(*p); 
}

int vga_drawpixel(int x, int y)
{
    unsigned long offset;

    if (MODEX) {
    		/* select plane */ 
		port_out(0x02, SEQ_I); 
    		port_out(1 << (x & 3), SEQ_D);
	        /* write color to pixel */
		GM[y*CI.xbytes + (x>>2)] = COL;  
	        return 0;
    }

    switch (__svgalib_cur_info.bytesperpixel) {
      case 0 :	/* Must be 2 or 16 color mode. */
	    /* set up video page */
	    offset = y*CI.xbytes + (x>>3);
            vga_setpage(offset>>16);
	    offset &= 0xffff;

	    /* select bit */
	    port_out(8, GRA_I);
	    port_out(0x80 >> (x & 7), GRA_D);   

            /* read into latch and write dummy back */
	    read_write(GM+offset);
            break;
      case 1 :
	    offset = y*CI.xbytes+x; 

	    /* select segment */
	    vga_setpage(offset >> 16);

            /* write color to pixel */
	    GM[offset & 0xFFFF] = COL;  
            break;
        case 2 :
            offset = y * CI.xbytes + x * 2;
            vga_setpage(offset >> 16);
            *(unsigned short *)(GM + (offset & 0xffff)) = COL;
            break;
        case 3 :
        	offset = y * CI.xbytes + x * 3;
        	vga_setpage(offset >> 16);
        	switch (offset & 0xffff) {
        		case 0xfffe :
        			*(unsigned short *)(GM+0xfffe) = COL;
        			vga_setpage((offset >> 16) + 1);
        			GM[0] = COL >> 16;
        			break; 
        		case 0xffff :
        			GM[0xffff] = COL;
        			vga_setpage((offset >> 16) + 1);
        			*(unsigned short *)(GM) = COL>>8;
        			break;
        		default :
				offset &= 0xffff;
        			*(unsigned short *)(GM+offset) = COL;
        			GM[offset+2] = COL>>16;
        			break;
        	}
        	break;
        case 4 :
        	offset = y * __svgalib_cur_info.xbytes + x * 4;
        	vga_setpage(offset >> 16);
		*(unsigned *)(GM + (offset & 0xffff)) =
			(MODEFLAGS & RGB_MISORDERED) ? (COL << 8) : COL;
        	break;
    }

    return 0;
}

int vga_getpixel(int x, int y)
{
    unsigned long offset;
    unsigned char mask;
    int pix;

    if (MODEX) {
    		/* select plane */ 
		port_out(0x02, SEQ_I); 
    		port_out(1 << (x & 3), SEQ_D);
	        return __svgalib_graph_mem[y*CI.xbytes + (x>>2)];  
    }

    switch (__svgalib_cur_info.bytesperpixel) {
      case 0 :	/* Must be 2 or 16 color mode. */
	    /* set up video page */
	    offset = y*CI.xbytes + (x>>3);
            vga_setpage(offset>>16);
	    offset &= 0xffff;

	    /* select bit */
	    port_out(8, GRA_I);
	    port_out(0x80 >> (x & 7), GRA_D);   
	    mask = 0x80 >> (x & 7);
	    pix = 0;
	    port_out(4, GRA_I);
	    port_out(0, GRA_D);		/* Select plane 0. */
	    if ((volatile)__svgalib_graph_mem[offset] & mask)
	    	pix |= 0x01;
	    port_out(4, GRA_I);
	    port_out(1, GRA_D);		/* Select plane 1. */
	    if ((volatile)__svgalib_graph_mem[offset] & mask)
	    	pix |= 0x02;
	    port_out(4, GRA_I);
	    port_out(2, GRA_D);		/* Select plane 2. */
	    if ((volatile)__svgalib_graph_mem[offset] & mask)
	    	pix |= 0x04;
	    port_out(4, GRA_I);
	    port_out(3, GRA_D);		/* Select plane 3. */
	    if ((volatile)__svgalib_graph_mem[offset] & mask)
	    	pix |= 0x08;
	    return pix;
      case 1 :
	    offset = y*CI.xbytes+x; 

	    /* select segment */
	    vga_setpage(offset >> 16);

            return __svgalib_graph_mem[offset & 0xFFFF];
        case 2 :
            offset = y * CI.xbytes + x * 2;
            vga_setpage(offset >> 16);
            return *(unsigned short *)(GM + (offset & 0xffff));
        case 3 :
        	offset = y * CI.xbytes + x * 3;
        	vga_setpage(offset >> 16);
        	switch (offset & 0xffff) {
        		case 0xfffe :
        			pix = *(unsigned short *)(GM+0xfffe);
        			vga_setpage((offset >> 16) + 1);
        			return pix + (__svgalib_graph_mem[0] << 16);
        		case 0xffff :
        			pix = __svgalib_graph_mem[0xffff];
        			vga_setpage((offset >> 16) + 1);
        			return pix + (*(unsigned short *)
        				__svgalib_graph_mem << 8);
        		default :
				offset &= 0xffff;
				return *(unsigned short *)(__svgalib_graph_mem
					+ offset) + (__svgalib_graph_mem[
					offset + 2] << 16);
        	}
        	break;
        case 4 :
        	offset = y * __svgalib_cur_info.xbytes + x * 4;
        	vga_setpage(offset >> 16);
        	return *(unsigned *)(__svgalib_graph_mem + (offset & 0xffff));
    }

    return 0;
}

