Home | History | Annotate | Line # | Download | only in hpc
bicons.c revision 1.5 
      1 /*	$NetBSD: bicons.c,v 1.5 2002/01/02 12:57:49 uch Exp $	*/
      2 
      3 /*-
      4  * Copyright (c) 1999-2001
      5  *         Shin Takemura and PocketBSD Project. All rights reserved.
      6  *
      7  * Redistribution and use in source and binary forms, with or without
      8  * modification, are permitted provided that the following conditions
      9  * are met:
     10  * 1. Redistributions of source code must retain the above copyright
     11  *    notice, this list of conditions and the following disclaimer.
     12  * 2. Redistributions in binary form must reproduce the above copyright
     13  *    notice, this list of conditions and the following disclaimer in the
     14  *    documentation and/or other materials provided with the distribution.
     15  * 3. All advertising materials mentioning features or use of this software
     16  *    must display the following acknowledgement:
     17  *	This product includes software developed by the PocketBSD project
     18  *	and its contributors.
     19  * 4. Neither the name of the project nor the names of its contributors
     20  *    may be used to endorse or promote products derived from this software
     21  *    without specific prior written permission.
     22  *
     23  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
     24  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
     25  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
     26  * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
     27  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
     28  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
     29  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
     30  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
     31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
     32  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
     33  * SUCH DAMAGE.
     34  *
     35  */
     36 
     37 #include <sys/cdefs.h>
     38 __KERNEL_RCSID(0, "$NetBSD: bicons.c,v 1.5 2002/01/02 12:57:49 uch Exp $");
     39 
     40 #define HALF_FONT
     41 
     42 #include <sys/param.h>
     43 #include <sys/device.h>
     44 #include <sys/systm.h>
     45 #include <sys/conf.h>
     46 #include <dev/cons.h>
     47 
     48 #include <machine/bootinfo.h>
     49 #include <machine/bus.h>
     50 #include <machine/platid.h>
     51 #include <machine/stdarg.h>
     52 
     53 #include <dev/hpc/biconsvar.h>
     54 #include <dev/hpc/bicons.h>
     55 extern u_int8_t font_clR8x8_data[];
     56 #define FONT_HEIGHT	8
     57 #define FONT_WIDTH	1
     58 
     59 static void put_oxel_D2_M2L_3(u_int8_t *, u_int8_t, u_int8_t);
     60 static void put_oxel_D2_M2L_3x2(u_int8_t *, u_int8_t, u_int8_t);
     61 static void put_oxel_D2_M2L_0(u_int8_t *, u_int8_t, u_int8_t);
     62 static void put_oxel_D2_M2L_0x2(u_int8_t *, u_int8_t, u_int8_t);
     63 static void put_oxel_D4_M2L_F(u_int8_t *, u_int8_t, u_int8_t);
     64 static void put_oxel_D4_M2L_Fx2(u_int8_t *, u_int8_t, u_int8_t);
     65 static void put_oxel_D4_M2L_0(u_int8_t *, u_int8_t, u_int8_t);
     66 static void put_oxel_D4_M2L_0x2(u_int8_t *, u_int8_t, u_int8_t);
     67 static void put_oxel_D8_00(u_int8_t *, u_int8_t, u_int8_t);
     68 static void put_oxel_D8_FF(u_int8_t *, u_int8_t, u_int8_t);
     69 static void put_oxel_D16_0000(u_int8_t *, u_int8_t, u_int8_t);
     70 static void put_oxel_D16_FFFF(u_int8_t *, u_int8_t, u_int8_t);
     71 
     72 static const struct {
     73 	int type;
     74 	char *name;
     75 	void (*func)(u_int8_t *, u_int8_t, u_int8_t);
     76 	u_int8_t clear_byte;
     77 	int16_t oxel_bytes;
     78 } fb_table[] = {
     79 	{ BIFB_D2_M2L_3,	BIFBN_D2_M2L_3,
     80 	  put_oxel_D2_M2L_3,	0,	2	},
     81 	{ BIFB_D2_M2L_3x2,	BIFBN_D2_M2L_3x2,
     82 	  put_oxel_D2_M2L_3x2,	0,	1	},
     83 	{ BIFB_D2_M2L_0,	BIFBN_D2_M2L_0,
     84 	  put_oxel_D2_M2L_0,	0xff,	2	},
     85 	{ BIFB_D2_M2L_0x2,	BIFBN_D2_M2L_0x2,
     86 	  put_oxel_D2_M2L_0x2,	0xff,	1	},
     87 	{ BIFB_D4_M2L_F,	BIFBN_D4_M2L_F,
     88 	  put_oxel_D4_M2L_F,	0x00,	4	},
     89 	{ BIFB_D4_M2L_Fx2,	BIFBN_D4_M2L_Fx2,
     90 	  put_oxel_D4_M2L_Fx2,	0x00,	2	},
     91 	{ BIFB_D4_M2L_0,	BIFBN_D4_M2L_0,
     92 	  put_oxel_D4_M2L_0,	0xff,	4	},
     93 	{ BIFB_D4_M2L_0x2,	BIFBN_D4_M2L_0x2,
     94 	  put_oxel_D4_M2L_0x2,	0xff,	2	},
     95 	{ BIFB_D8_00,		BIFBN_D8_00,
     96 	  put_oxel_D8_00,	0xff,	8	},
     97 	{ BIFB_D8_FF,		BIFBN_D8_FF,
     98 	  put_oxel_D8_FF,	0x00,	8	},
     99 	{ BIFB_D16_0000,	BIFBN_D16_0000,
    100 	  put_oxel_D16_0000,	0xff,	16	},
    101 	{ BIFB_D16_FFFF,	BIFBN_D16_FFFF,
    102 	  put_oxel_D16_FFFF,	0x00,	16	},
    103 };
    104 #define FB_TABLE_SIZE (sizeof(fb_table) / sizeof(*fb_table))
    105 
    106 static u_int8_t	*fb_vram;
    107 static int16_t	fb_line_bytes;
    108 static u_int8_t	fb_clear_byte;
    109 int16_t	bicons_ypixel;
    110 int16_t	bicons_xpixel;
    111 #ifdef HALF_FONT
    112 static int16_t	fb_oxel_bytes	= 1;
    113 int16_t	bicons_width	= 80;
    114 void	(*fb_put_oxel)(u_int8_t *, u_int8_t, u_int8_t) = put_oxel_D2_M2L_3x2;
    115 #else /* HALF_FONT */
    116 static int16_t	fb_oxel_bytes	= 2;
    117 int16_t	bicons_width	= 40;
    118 void	(*fb_put_oxel)(u_int8_t *, u_int8_t, u_int8_t) = put_oxel_D2_M2L_3;
    119 #endif /* HALF_FONT */
    120 int16_t bicons_height;
    121 static int16_t curs_x;
    122 static int16_t curs_y;
    123 
    124 cdev_decl(biconsdev);
    125 
    126 static int bicons_priority;
    127 int biconscninit(struct consdev *);
    128 void biconscnprobe(struct consdev *);
    129 void biconscnputc(dev_t, int);
    130 int biconscngetc(dev_t);	/* harmless place holder */
    131 
    132 static void draw_char(int, int, int);
    133 static void clear(int, int);
    134 static void scroll(int, int, int);
    135 static void bicons_puts(char *);
    136 static void bicons_printf(const char *, ...) __attribute__((__unused__));
    137 
    138 int
    139 bicons_init(struct consdev *cndev)
    140 {
    141 
    142 	if (biconscninit(cndev) != 0)
    143 		return (1);
    144 
    145 	biconscnprobe(cndev);
    146 
    147 	return (0);	/* success */
    148 }
    149 
    150 int
    151 biconscninit(struct consdev *cndev)
    152 {
    153 	int fb_index = -1;
    154 
    155 	if (bootinfo->fb_addr == 0) {
    156 		/* Bootinfo don't have frame buffer address */
    157 		return (1);
    158 	}
    159 
    160 	for (fb_index = 0; fb_index < FB_TABLE_SIZE; fb_index++)
    161 		if (fb_table[fb_index].type == bootinfo->fb_type)
    162 			break;
    163 
    164 	if (FB_TABLE_SIZE <= fb_index || fb_index == -1) {
    165 		/* Unknown frame buffer type, don't enable bicons. */
    166 		return (1);
    167 	}
    168 
    169 	fb_vram = (u_int8_t *)bootinfo->fb_addr;
    170 	fb_line_bytes = bootinfo->fb_line_bytes;
    171 	bicons_xpixel = bootinfo->fb_width;
    172 	bicons_ypixel = bootinfo->fb_height;
    173 
    174 	fb_put_oxel = fb_table[fb_index].func;
    175 	fb_clear_byte = fb_table[fb_index].clear_byte;
    176 	fb_oxel_bytes = fb_table[fb_index].oxel_bytes;
    177 
    178 	bicons_width = bicons_xpixel / (8 * FONT_WIDTH);
    179 	bicons_height = bicons_ypixel / FONT_HEIGHT;
    180 	clear(0, bicons_ypixel);
    181 
    182 	curs_x = 0;
    183 	curs_y = 0;
    184 
    185 	bicons_puts("builtin console type = ");
    186 	bicons_puts(fb_table[fb_index].name);
    187 	bicons_puts("\n");
    188 
    189 	return (0);
    190 }
    191 
    192 void
    193 biconscnprobe(struct consdev *cndev)
    194 {
    195 	int maj;
    196 
    197 	/* locate the major number */
    198 	for (maj = 0; maj < nchrdev; maj++)
    199 		if (cdevsw[maj].d_open == biconsdevopen)
    200 			break;
    201 
    202 	cndev->cn_dev = makedev(maj, 0);
    203 	cndev->cn_pri = bicons_priority;
    204 }
    205 
    206 void
    207 bicons_set_priority(int priority)
    208 {
    209 	bicons_priority = priority;
    210 }
    211 
    212 int
    213 biconscngetc(dev_t dev)
    214 {
    215 	printf("no input method. reboot me.\n");
    216 	while (1)
    217 		;
    218 	/* NOTREACHED */
    219 }
    220 
    221 void
    222 biconscnputc(dev_t dev, int c)
    223 {
    224 	int line_feed = 0;
    225 
    226 	switch (c) {
    227 	case 0x08: /* back space */
    228 		if (--curs_x < 0) {
    229 			curs_x = 0;
    230 		}
    231 		/* erase character ar cursor position */
    232 		draw_char(curs_x, curs_y, ' ');
    233 		break;
    234 	case '\r':
    235 		curs_x = 0;
    236 		break;
    237 	case '\n':
    238 		curs_x = 0;
    239 		line_feed = 1;
    240 		break;
    241 	default:
    242 		draw_char(curs_x, curs_y, c);
    243 		if (bicons_width <= ++curs_x) {
    244 			curs_x = 0;
    245 			line_feed = 1;
    246 		}
    247 	}
    248 
    249 	if (line_feed) {
    250 		if (bicons_height <= ++curs_y) {
    251 			/* scroll up */
    252 			scroll(FONT_HEIGHT, (bicons_height - 1) * FONT_HEIGHT,
    253 			       - FONT_HEIGHT);
    254 			clear((bicons_height - 1) * FONT_HEIGHT, FONT_HEIGHT);
    255 			curs_y--;
    256 		}
    257 	}
    258 }
    259 
    260 void
    261 bicons_puts(char *s)
    262 {
    263 	while (*s)
    264 		biconscnputc(NULL, *s++);
    265 }
    266 
    267 
    268 void
    269 bicons_putn(const char *s, int n)
    270 {
    271 	while (0 < n--)
    272 		biconscnputc(NULL, *s++);
    273 }
    274 
    275 void
    276 #ifdef __STDC__
    277 bicons_printf(const char *fmt, ...)
    278 #else
    279 bicons_printf(fmt, va_alist)
    280 	char *fmt;
    281 	va_dcl
    282 #endif
    283 {
    284 	va_list ap;
    285 	char buf[0x100];
    286 
    287 	va_start(ap, fmt);
    288 	vsnprintf(buf, sizeof(buf), fmt, ap);
    289 	va_end(ap);
    290 	bicons_puts(buf);
    291 }
    292 
    293 static void
    294 draw_char(int x, int y, int c)
    295 {
    296 	int i;
    297 	u_int8_t *p;
    298 
    299 	if (!fb_vram)
    300 		return;
    301 
    302 	p = &fb_vram[(y * FONT_HEIGHT * fb_line_bytes) +
    303 		    x * FONT_WIDTH * fb_oxel_bytes];
    304 	for (i = 0; i < FONT_HEIGHT; i++) {
    305 		(*fb_put_oxel)(p, font_clR8x8_data
    306 			       [FONT_WIDTH * (FONT_HEIGHT * c + i)], 0xff);
    307 		p += (fb_line_bytes);
    308 	}
    309 }
    310 
    311 static void
    312 clear(int y, int height)
    313 {
    314 	u_int8_t *p;
    315 
    316 	if (!fb_vram)
    317 		return;
    318 
    319 	p = &fb_vram[y * fb_line_bytes];
    320 
    321 	while (0 < height--) {
    322 		memset(p, fb_clear_byte,
    323 		       bicons_width * fb_oxel_bytes * FONT_WIDTH);
    324 		p += fb_line_bytes;
    325 	}
    326 }
    327 
    328 static void
    329 scroll(int y, int height, int d)
    330 {
    331 	u_int8_t *from, *to;
    332 
    333 	if (!fb_vram)
    334 		return;
    335 
    336 	if (d < 0) {
    337 		from = &fb_vram[y * fb_line_bytes];
    338 		to = from + d * fb_line_bytes;
    339 		while (0 < height--) {
    340 			memcpy(to, from, bicons_width * fb_oxel_bytes);
    341 			from += fb_line_bytes;
    342 			to += fb_line_bytes;
    343 		}
    344 	} else {
    345 		from = &fb_vram[(y + height - 1) * fb_line_bytes];
    346 		to = from + d * fb_line_bytes;
    347 		while (0 < height--) {
    348 			memcpy(to, from, bicons_xpixel * fb_oxel_bytes / 8);
    349 			from -= fb_line_bytes;
    350 			to -= fb_line_bytes;
    351 		}
    352 	}
    353 }
    354 
    355 /*=============================================================================
    356  *
    357  *	D2_M2L_3
    358  *
    359  */
    360 static void
    361 put_oxel_D2_M2L_3(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    362 {
    363 #if 1
    364 	u_int16_t *addr = (u_int16_t *)xaddr;
    365 	static u_int16_t map0[] = {
    366 		0x0000, 0x0300, 0x0c00, 0x0f00, 0x3000, 0x3300, 0x3c00, 0x3f00,
    367 		0xc000, 0xc300, 0xcc00, 0xcf00, 0xf000, 0xf300, 0xfc00, 0xff00,
    368 	};
    369 	static u_int16_t map1[] = {
    370 		0x0000, 0x0003, 0x000c, 0x000f, 0x0030, 0x0033, 0x003c, 0x003f,
    371 		0x00c0, 0x00c3, 0x00cc, 0x00cf, 0x00f0, 0x00f3, 0x00fc, 0x00ff,
    372 	};
    373 	*addr = (map1[data >> 4] | map0[data & 0x0f]);
    374 #else
    375 	static u_int8_t map[] = {
    376 		0x00, 0x03, 0x0c, 0x0f, 0x30, 0x33, 0x3c, 0x3f,
    377 		0xc0, 0xc3, 0xcc, 0xcf, 0xf0, 0xf3, 0xfc, 0xff,
    378 	};
    379 	u_int8_t *addr = xaddr;
    380 
    381 	*addr++ = (map[(data >> 4) & 0x0f] & map[(mask >> 4) & 0x0f]) |
    382 		(*addr & ~map[(mask >> 4) & 0x0f]);
    383 	*addr   = (map[(data >> 0) & 0x0f] & map[(mask >> 0) & 0x0f]) |
    384 		(*addr & ~map[(mask >> 0) & 0x0f]);
    385 #endif
    386 }
    387 
    388 /*=============================================================================
    389  *
    390  *	D2_M2L_3x2
    391  *
    392  */
    393 static void
    394 put_oxel_D2_M2L_3x2(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    395 {
    396 	register u_int8_t odd = (data & 0xaa);
    397 	register u_int8_t even = (data & 0x55);
    398 
    399 	*xaddr = (odd | (even << 1)) | ((odd >> 1) & even);
    400 }
    401 
    402 /*=============================================================================
    403  *
    404  *	D2_M2L_0
    405  *
    406  */
    407 static void
    408 put_oxel_D2_M2L_0(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    409 {
    410 #if 1
    411 	u_int16_t *addr = (u_int16_t *)xaddr;
    412 	static u_int16_t map0[] = {
    413 		0xff00, 0xfc00, 0xf300, 0xf000, 0xcf00, 0xcc00, 0xc300, 0xc000,
    414 		0x3f00, 0x3c00, 0x3300, 0x3000, 0x0f00, 0x0c00, 0x0300, 0x0000,
    415 	};
    416 	static u_int16_t map1[] = {
    417 		0x00ff, 0x00fc, 0x00f3, 0x00f0, 0x00cf, 0x00cc, 0x00c3, 0x00c0,
    418 		0x003f, 0x003c, 0x0033, 0x0030, 0x000f, 0x000c, 0x0003, 0x0000,
    419 	};
    420 	*addr = (map1[data >> 4] | map0[data & 0x0f]);
    421 #else
    422 	static u_int8_t map[] = {
    423 		0x00, 0x03, 0x0c, 0x0f, 0x30, 0x33, 0x3c, 0x3f,
    424 		0xc0, 0xc3, 0xcc, 0xcf, 0xf0, 0xf3, 0xfc, 0xff,
    425 	};
    426 	u_int8_t *addr = xaddr;
    427 
    428 	*addr++ = (~(map[(data >> 4) & 0x0f] & map[(mask >> 4) & 0x0f])) |
    429 		(*addr & ~map[(mask >> 4) & 0x0f]);
    430 	*addr   = (~(map[(data >> 0) & 0x0f] & map[(mask >> 0) & 0x0f])) |
    431 		(*addr & ~map[(mask >> 0) & 0x0f]);
    432 #endif
    433 }
    434 
    435 /*=============================================================================
    436  *
    437  *	D2_M2L_0x2
    438  *
    439  */
    440 static void
    441 put_oxel_D2_M2L_0x2(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    442 {
    443 	register u_int8_t odd = (data & 0xaa);
    444 	register u_int8_t even = (data & 0x55);
    445 
    446 	*xaddr = ~((odd | (even << 1)) | ((odd >> 1) & even));
    447 }
    448 
    449 /*=============================================================================
    450  *
    451  *	D4_M2L_F
    452  *
    453  */
    454 static void
    455 put_oxel_D4_M2L_F(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    456 {
    457 	u_int32_t *addr = (u_int32_t *)xaddr;
    458 	static u_int32_t map[] = {
    459 		0x0000, 0x0f00, 0xf000, 0xff00, 0x000f, 0x0f0f, 0xf00f, 0xff0f,
    460 		0x00f0, 0x0ff0, 0xf0f0, 0xfff0, 0x00ff, 0x0fff, 0xf0ff, 0xffff,
    461 	};
    462 	*addr = (map[data >> 4] | (map[data & 0x0f] << 16));
    463 }
    464 
    465 /*=============================================================================
    466  *
    467  *	D4_M2L_Fx2
    468  *
    469  */
    470 static void
    471 put_oxel_D4_M2L_Fx2(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    472 {
    473 	u_int16_t *addr = (u_int16_t *)xaddr;
    474 	static u_int16_t map[] = {
    475 		0x00, 0x08, 0x08, 0x0f, 0x80, 0x88, 0x88, 0x8f,
    476 		0x80, 0x88, 0x88, 0x8f, 0xf0, 0xf8, 0xf8, 0xff,
    477 	};
    478 
    479 	*addr = (map[data >> 4] | (map[data & 0x0f] << 8));
    480 }
    481 
    482 /*=============================================================================
    483  *
    484  *	D4_M2L_0
    485  *
    486  */
    487 static void
    488 put_oxel_D4_M2L_0(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    489 {
    490 	u_int32_t *addr = (u_int32_t *)xaddr;
    491 	static u_int32_t map[] = {
    492 		0xffff, 0xf0ff, 0x0fff, 0x00ff, 0xfff0, 0xf0f0, 0x0ff0, 0x00f0,
    493 		0xff0f, 0xf00f, 0x0f0f, 0x000f, 0xff00, 0xf000, 0x0f00, 0x0000,
    494 	};
    495 	*addr = (map[data >> 4] | (map[data & 0x0f] << 16));
    496 }
    497 
    498 /*=============================================================================
    499  *
    500  *	D4_M2L_0x2
    501  *
    502  */
    503 static void
    504 put_oxel_D4_M2L_0x2(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    505 {
    506 	u_int16_t *addr = (u_int16_t *)xaddr;
    507 	static u_int16_t map[] = {
    508 		0xff, 0xf8, 0xf8, 0xf0, 0x8f, 0x88, 0x88, 0x80,
    509 		0x8f, 0x88, 0x88, 0x80, 0x0f, 0x08, 0x08, 0x00,
    510 	};
    511 
    512 	*addr = (map[data >> 4] | (map[data & 0x0f] << 8));
    513 }
    514 
    515 /*=============================================================================
    516  *
    517  *	D8_00
    518  *
    519  */
    520 static void
    521 put_oxel_D8_00(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    522 {
    523 	int i;
    524 	u_int8_t *addr = xaddr;
    525 
    526 	for (i = 0; i < 8; i++) {
    527 		if (mask & 0x80) {
    528 			*addr = (data & 0x80) ? 0x00 : 0xFF;
    529 		}
    530 		addr++;
    531 		data <<= 1;
    532 		mask <<= 1;
    533 	}
    534 }
    535 
    536 /*=============================================================================
    537  *
    538  *	D8_FF
    539  *
    540  */
    541 static void
    542 put_oxel_D8_FF(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    543 {
    544 	int i;
    545 	u_int8_t *addr = xaddr;
    546 
    547 	for (i = 0; i < 8; i++) {
    548 		if (mask & 0x80) {
    549 			*addr = (data & 0x80) ? 0xFF : 0x00;
    550 		}
    551 		addr++;
    552 		data <<= 1;
    553 		mask <<= 1;
    554 	}
    555 }
    556 
    557 /*=============================================================================
    558  *
    559  *	D16_0000
    560  *
    561  */
    562 static void
    563 put_oxel_D16_0000(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    564 {
    565 	int i;
    566 	u_int16_t *addr = (u_int16_t *)xaddr;
    567 
    568 	for (i = 0; i < 8; i++) {
    569 		if (mask & 0x80) {
    570 			*addr = (data & 0x80) ? 0x0000 : 0xFFFF;
    571 		}
    572 		addr++;
    573 		data <<= 1;
    574 		mask <<= 1;
    575 	}
    576 }
    577 
    578 /*=============================================================================
    579  *
    580  *	D16_FFFF
    581  *
    582  */
    583 static void
    584 put_oxel_D16_FFFF(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    585 {
    586 	int i;
    587 	u_int16_t *addr = (u_int16_t *)xaddr;
    588 
    589 	for (i = 0; i < 8; i++) {
    590 		if (mask & 0x80) {
    591 			*addr = (data & 0x80) ? 0xFFFF : 0x0000;
    592 		}
    593 		addr++;
    594 		data <<= 1;
    595 		mask <<= 1;
    596 	}
    597 }
    598