Home | History | Annotate | Line # | Download | only in hpc
bicons.c revision 1.4 
      1 /*	$NetBSD: bicons.c,v 1.4 2001/11/13 12:47:56 lukem 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.4 2001/11/13 12:47:56 lukem 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 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 void 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 void
    139 bicons_init(struct consdev *cndev)
    140 {
    141 	biconscninit(cndev);
    142 	biconscnprobe(cndev);
    143 }
    144 
    145 void
    146 biconscninit(struct consdev *cndev)
    147 {
    148 	int fb_index = -1;
    149 
    150 	for (fb_index = 0; fb_index < FB_TABLE_SIZE; fb_index++)
    151 		if (fb_table[fb_index].type == bootinfo->fb_type)
    152 			break;
    153 
    154 	if (FB_TABLE_SIZE <= fb_index || fb_index == -1) {
    155 		/*
    156 		 *  Unknown frame buffer type, but what can I do ?
    157 		 */
    158 		fb_index = 0;
    159 	}
    160 
    161 	fb_vram = (u_int8_t *)bootinfo->fb_addr;
    162 	fb_line_bytes = bootinfo->fb_line_bytes;
    163 	bicons_xpixel = bootinfo->fb_width;
    164 	bicons_ypixel = bootinfo->fb_height;
    165 
    166 	fb_put_oxel = fb_table[fb_index].func;
    167 	fb_clear_byte = fb_table[fb_index].clear_byte;
    168 	fb_oxel_bytes = fb_table[fb_index].oxel_bytes;
    169 
    170 	bicons_width = bicons_xpixel / (8 * FONT_WIDTH);
    171 	bicons_height = bicons_ypixel / FONT_HEIGHT;
    172 	clear(0, bicons_ypixel);
    173 
    174 	curs_x = 0;
    175 	curs_y = 0;
    176 
    177 	bicons_puts("builtin console type = ");
    178 	bicons_puts(fb_table[fb_index].name);
    179 	bicons_puts("\n");
    180 }
    181 
    182 void
    183 biconscnprobe(struct consdev *cndev)
    184 {
    185 	int maj;
    186 
    187 	/* locate the major number */
    188 	for (maj = 0; maj < nchrdev; maj++)
    189 		if (cdevsw[maj].d_open == biconsdevopen)
    190 			break;
    191 
    192 	cndev->cn_dev = makedev(maj, 0);
    193 	cndev->cn_pri = bicons_priority;
    194 }
    195 
    196 void
    197 bicons_set_priority(int priority)
    198 {
    199 	bicons_priority = priority;
    200 }
    201 
    202 int
    203 biconscngetc(dev_t dev)
    204 {
    205 	printf("no input method. reboot me.\n");
    206 	while (1)
    207 		;
    208 	/* NOTREACHED */
    209 }
    210 
    211 void
    212 biconscnputc(dev_t dev, int c)
    213 {
    214 	int line_feed = 0;
    215 
    216 	switch (c) {
    217 	case 0x08: /* back space */
    218 		if (--curs_x < 0) {
    219 			curs_x = 0;
    220 		}
    221 		/* erase character ar cursor position */
    222 		draw_char(curs_x, curs_y, ' ');
    223 		break;
    224 	case '\r':
    225 		curs_x = 0;
    226 		break;
    227 	case '\n':
    228 		curs_x = 0;
    229 		line_feed = 1;
    230 		break;
    231 	default:
    232 		draw_char(curs_x, curs_y, c);
    233 		if (bicons_width <= ++curs_x) {
    234 			curs_x = 0;
    235 			line_feed = 1;
    236 		}
    237 	}
    238 
    239 	if (line_feed) {
    240 		if (bicons_height <= ++curs_y) {
    241 			/* scroll up */
    242 			scroll(FONT_HEIGHT, (bicons_height - 1) * FONT_HEIGHT,
    243 			       - FONT_HEIGHT);
    244 			clear((bicons_height - 1) * FONT_HEIGHT, FONT_HEIGHT);
    245 			curs_y--;
    246 		}
    247 	}
    248 }
    249 
    250 void
    251 bicons_puts(char *s)
    252 {
    253 	while (*s)
    254 		biconscnputc(NULL, *s++);
    255 }
    256 
    257 
    258 void
    259 bicons_putn(const char *s, int n)
    260 {
    261 	while (0 < n--)
    262 		biconscnputc(NULL, *s++);
    263 }
    264 
    265 void
    266 #ifdef __STDC__
    267 bicons_printf(const char *fmt, ...)
    268 #else
    269 bicons_printf(fmt, va_alist)
    270 	char *fmt;
    271 	va_dcl
    272 #endif
    273 {
    274 	va_list ap;
    275 	char buf[0x100];
    276 
    277 	va_start(ap, fmt);
    278 	vsnprintf(buf, sizeof(buf), fmt, ap);
    279 	va_end(ap);
    280 	bicons_puts(buf);
    281 }
    282 
    283 static void
    284 draw_char(int x, int y, int c)
    285 {
    286 	int i;
    287 	u_int8_t *p;
    288 
    289 	if (!fb_vram)
    290 		return;
    291 
    292 	p = &fb_vram[(y * FONT_HEIGHT * fb_line_bytes) +
    293 		    x * FONT_WIDTH * fb_oxel_bytes];
    294 	for (i = 0; i < FONT_HEIGHT; i++) {
    295 		(*fb_put_oxel)(p, font_clR8x8_data
    296 			       [FONT_WIDTH * (FONT_HEIGHT * c + i)], 0xff);
    297 		p += (fb_line_bytes);
    298 	}
    299 }
    300 
    301 static void
    302 clear(int y, int height)
    303 {
    304 	u_int8_t *p;
    305 
    306 	if (!fb_vram)
    307 		return;
    308 
    309 	p = &fb_vram[y * fb_line_bytes];
    310 
    311 	while (0 < height--) {
    312 		memset(p, fb_clear_byte,
    313 		       bicons_width * fb_oxel_bytes * FONT_WIDTH);
    314 		p += fb_line_bytes;
    315 	}
    316 }
    317 
    318 static void
    319 scroll(int y, int height, int d)
    320 {
    321 	u_int8_t *from, *to;
    322 
    323 	if (!fb_vram)
    324 		return;
    325 
    326 	if (d < 0) {
    327 		from = &fb_vram[y * fb_line_bytes];
    328 		to = from + d * fb_line_bytes;
    329 		while (0 < height--) {
    330 			memcpy(to, from, bicons_width * fb_oxel_bytes);
    331 			from += fb_line_bytes;
    332 			to += fb_line_bytes;
    333 		}
    334 	} else {
    335 		from = &fb_vram[(y + height - 1) * fb_line_bytes];
    336 		to = from + d * fb_line_bytes;
    337 		while (0 < height--) {
    338 			memcpy(to, from, bicons_xpixel * fb_oxel_bytes / 8);
    339 			from -= fb_line_bytes;
    340 			to -= fb_line_bytes;
    341 		}
    342 	}
    343 }
    344 
    345 /*=============================================================================
    346  *
    347  *	D2_M2L_3
    348  *
    349  */
    350 static void
    351 put_oxel_D2_M2L_3(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    352 {
    353 #if 1
    354 	u_int16_t *addr = (u_int16_t *)xaddr;
    355 	static u_int16_t map0[] = {
    356 		0x0000, 0x0300, 0x0c00, 0x0f00, 0x3000, 0x3300, 0x3c00, 0x3f00,
    357 		0xc000, 0xc300, 0xcc00, 0xcf00, 0xf000, 0xf300, 0xfc00, 0xff00,
    358 	};
    359 	static u_int16_t map1[] = {
    360 		0x0000, 0x0003, 0x000c, 0x000f, 0x0030, 0x0033, 0x003c, 0x003f,
    361 		0x00c0, 0x00c3, 0x00cc, 0x00cf, 0x00f0, 0x00f3, 0x00fc, 0x00ff,
    362 	};
    363 	*addr = (map1[data >> 4] | map0[data & 0x0f]);
    364 #else
    365 	static u_int8_t map[] = {
    366 		0x00, 0x03, 0x0c, 0x0f, 0x30, 0x33, 0x3c, 0x3f,
    367 		0xc0, 0xc3, 0xcc, 0xcf, 0xf0, 0xf3, 0xfc, 0xff,
    368 	};
    369 	u_int8_t *addr = xaddr;
    370 
    371 	*addr++ = (map[(data >> 4) & 0x0f] & map[(mask >> 4) & 0x0f]) |
    372 		(*addr & ~map[(mask >> 4) & 0x0f]);
    373 	*addr   = (map[(data >> 0) & 0x0f] & map[(mask >> 0) & 0x0f]) |
    374 		(*addr & ~map[(mask >> 0) & 0x0f]);
    375 #endif
    376 }
    377 
    378 /*=============================================================================
    379  *
    380  *	D2_M2L_3x2
    381  *
    382  */
    383 static void
    384 put_oxel_D2_M2L_3x2(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    385 {
    386 	register u_int8_t odd = (data & 0xaa);
    387 	register u_int8_t even = (data & 0x55);
    388 
    389 	*xaddr = (odd | (even << 1)) | ((odd >> 1) & even);
    390 }
    391 
    392 /*=============================================================================
    393  *
    394  *	D2_M2L_0
    395  *
    396  */
    397 static void
    398 put_oxel_D2_M2L_0(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    399 {
    400 #if 1
    401 	u_int16_t *addr = (u_int16_t *)xaddr;
    402 	static u_int16_t map0[] = {
    403 		0xff00, 0xfc00, 0xf300, 0xf000, 0xcf00, 0xcc00, 0xc300, 0xc000,
    404 		0x3f00, 0x3c00, 0x3300, 0x3000, 0x0f00, 0x0c00, 0x0300, 0x0000,
    405 	};
    406 	static u_int16_t map1[] = {
    407 		0x00ff, 0x00fc, 0x00f3, 0x00f0, 0x00cf, 0x00cc, 0x00c3, 0x00c0,
    408 		0x003f, 0x003c, 0x0033, 0x0030, 0x000f, 0x000c, 0x0003, 0x0000,
    409 	};
    410 	*addr = (map1[data >> 4] | map0[data & 0x0f]);
    411 #else
    412 	static u_int8_t map[] = {
    413 		0x00, 0x03, 0x0c, 0x0f, 0x30, 0x33, 0x3c, 0x3f,
    414 		0xc0, 0xc3, 0xcc, 0xcf, 0xf0, 0xf3, 0xfc, 0xff,
    415 	};
    416 	u_int8_t *addr = xaddr;
    417 
    418 	*addr++ = (~(map[(data >> 4) & 0x0f] & map[(mask >> 4) & 0x0f])) |
    419 		(*addr & ~map[(mask >> 4) & 0x0f]);
    420 	*addr   = (~(map[(data >> 0) & 0x0f] & map[(mask >> 0) & 0x0f])) |
    421 		(*addr & ~map[(mask >> 0) & 0x0f]);
    422 #endif
    423 }
    424 
    425 /*=============================================================================
    426  *
    427  *	D2_M2L_0x2
    428  *
    429  */
    430 static void
    431 put_oxel_D2_M2L_0x2(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    432 {
    433 	register u_int8_t odd = (data & 0xaa);
    434 	register u_int8_t even = (data & 0x55);
    435 
    436 	*xaddr = ~((odd | (even << 1)) | ((odd >> 1) & even));
    437 }
    438 
    439 /*=============================================================================
    440  *
    441  *	D4_M2L_F
    442  *
    443  */
    444 static void
    445 put_oxel_D4_M2L_F(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    446 {
    447 	u_int32_t *addr = (u_int32_t *)xaddr;
    448 	static u_int32_t map[] = {
    449 		0x0000, 0x0f00, 0xf000, 0xff00, 0x000f, 0x0f0f, 0xf00f, 0xff0f,
    450 		0x00f0, 0x0ff0, 0xf0f0, 0xfff0, 0x00ff, 0x0fff, 0xf0ff, 0xffff,
    451 	};
    452 	*addr = (map[data >> 4] | (map[data & 0x0f] << 16));
    453 }
    454 
    455 /*=============================================================================
    456  *
    457  *	D4_M2L_Fx2
    458  *
    459  */
    460 static void
    461 put_oxel_D4_M2L_Fx2(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    462 {
    463 	u_int16_t *addr = (u_int16_t *)xaddr;
    464 	static u_int16_t map[] = {
    465 		0x00, 0x08, 0x08, 0x0f, 0x80, 0x88, 0x88, 0x8f,
    466 		0x80, 0x88, 0x88, 0x8f, 0xf0, 0xf8, 0xf8, 0xff,
    467 	};
    468 
    469 	*addr = (map[data >> 4] | (map[data & 0x0f] << 8));
    470 }
    471 
    472 /*=============================================================================
    473  *
    474  *	D4_M2L_0
    475  *
    476  */
    477 static void
    478 put_oxel_D4_M2L_0(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    479 {
    480 	u_int32_t *addr = (u_int32_t *)xaddr;
    481 	static u_int32_t map[] = {
    482 		0xffff, 0xf0ff, 0x0fff, 0x00ff, 0xfff0, 0xf0f0, 0x0ff0, 0x00f0,
    483 		0xff0f, 0xf00f, 0x0f0f, 0x000f, 0xff00, 0xf000, 0x0f00, 0x0000,
    484 	};
    485 	*addr = (map[data >> 4] | (map[data & 0x0f] << 16));
    486 }
    487 
    488 /*=============================================================================
    489  *
    490  *	D4_M2L_0x2
    491  *
    492  */
    493 static void
    494 put_oxel_D4_M2L_0x2(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    495 {
    496 	u_int16_t *addr = (u_int16_t *)xaddr;
    497 	static u_int16_t map[] = {
    498 		0xff, 0xf8, 0xf8, 0xf0, 0x8f, 0x88, 0x88, 0x80,
    499 		0x8f, 0x88, 0x88, 0x80, 0x0f, 0x08, 0x08, 0x00,
    500 	};
    501 
    502 	*addr = (map[data >> 4] | (map[data & 0x0f] << 8));
    503 }
    504 
    505 /*=============================================================================
    506  *
    507  *	D8_00
    508  *
    509  */
    510 static void
    511 put_oxel_D8_00(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    512 {
    513 	int i;
    514 	u_int8_t *addr = xaddr;
    515 
    516 	for (i = 0; i < 8; i++) {
    517 		if (mask & 0x80) {
    518 			*addr = (data & 0x80) ? 0x00 : 0xFF;
    519 		}
    520 		addr++;
    521 		data <<= 1;
    522 		mask <<= 1;
    523 	}
    524 }
    525 
    526 /*=============================================================================
    527  *
    528  *	D8_FF
    529  *
    530  */
    531 static void
    532 put_oxel_D8_FF(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    533 {
    534 	int i;
    535 	u_int8_t *addr = xaddr;
    536 
    537 	for (i = 0; i < 8; i++) {
    538 		if (mask & 0x80) {
    539 			*addr = (data & 0x80) ? 0xFF : 0x00;
    540 		}
    541 		addr++;
    542 		data <<= 1;
    543 		mask <<= 1;
    544 	}
    545 }
    546 
    547 /*=============================================================================
    548  *
    549  *	D16_0000
    550  *
    551  */
    552 static void
    553 put_oxel_D16_0000(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    554 {
    555 	int i;
    556 	u_int16_t *addr = (u_int16_t *)xaddr;
    557 
    558 	for (i = 0; i < 8; i++) {
    559 		if (mask & 0x80) {
    560 			*addr = (data & 0x80) ? 0x0000 : 0xFFFF;
    561 		}
    562 		addr++;
    563 		data <<= 1;
    564 		mask <<= 1;
    565 	}
    566 }
    567 
    568 /*=============================================================================
    569  *
    570  *	D16_FFFF
    571  *
    572  */
    573 static void
    574 put_oxel_D16_FFFF(u_int8_t *xaddr, u_int8_t data, u_int8_t mask)
    575 {
    576 	int i;
    577 	u_int16_t *addr = (u_int16_t *)xaddr;
    578 
    579 	for (i = 0; i < 8; i++) {
    580 		if (mask & 0x80) {
    581 			*addr = (data & 0x80) ? 0xFFFF : 0x0000;
    582 		}
    583 		addr++;
    584 		data <<= 1;
    585 		mask <<= 1;
    586 	}
    587 }
    588