xref: /illumos-gate/usr/src/cmd/bhyve/vga.c (revision 4c87aefe)
1 /*-
2  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
3  *
4  * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
5  * 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  *
16  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
20  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
22  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
25  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
26  * SUCH DAMAGE.
27  */
28 
29 /*
30  * Copyright 2018 Joyent, Inc.
31  */
32 
33 #include <sys/cdefs.h>
34 __FBSDID("$FreeBSD$");
35 
36 #include <sys/param.h>
37 
38 #include <assert.h>
39 #include <pthread.h>
40 #include <stdbool.h>
41 #include <stdio.h>
42 #include <stdlib.h>
43 #include <string.h>
44 
45 #include <machine/vmm.h>
46 
47 #include "bhyvegc.h"
48 #include "console.h"
49 #include "inout.h"
50 #include "mem.h"
51 #include "vga.h"
52 
53 #define	KB	(1024UL)
54 #define	MB	(1024 * 1024UL)
55 
56 struct vga_softc {
57 	struct mem_range	mr;
58 
59 	struct bhyvegc		*gc;
60 	int			gc_width;
61 	int			gc_height;
62 	struct bhyvegc_image	*gc_image;
63 
64 	uint8_t			*vga_ram;
65 
66 	/*
67 	 * General registers
68 	 */
69 	uint8_t			vga_misc;
70 	uint8_t			vga_sts1;
71 
72 	/*
73 	 * Sequencer
74 	 */
75 	struct {
76 		int		seq_index;
77 		uint8_t		seq_reset;
78 		uint8_t		seq_clock_mode;
79 		int		seq_cm_dots;
80 		uint8_t		seq_map_mask;
81 		uint8_t		seq_cmap_sel;
82 		int		seq_cmap_pri_off;
83 		int		seq_cmap_sec_off;
84 		uint8_t		seq_mm;
85 	} vga_seq;
86 
87 	/*
88 	 * CRT Controller
89 	 */
90 	struct {
91 		int		crtc_index;
92 		uint8_t		crtc_mode_ctrl;
93 		uint8_t		crtc_horiz_total;
94 		uint8_t		crtc_horiz_disp_end;
95 		uint8_t		crtc_start_horiz_blank;
96 		uint8_t		crtc_end_horiz_blank;
97 		uint8_t		crtc_start_horiz_retrace;
98 		uint8_t		crtc_end_horiz_retrace;
99 		uint8_t		crtc_vert_total;
100 		uint8_t		crtc_overflow;
101 		uint8_t		crtc_present_row_scan;
102 		uint8_t		crtc_max_scan_line;
103 		uint8_t		crtc_cursor_start;
104 		uint8_t		crtc_cursor_on;
105 		uint8_t		crtc_cursor_end;
106 		uint8_t		crtc_start_addr_high;
107 		uint8_t		crtc_start_addr_low;
108 		uint16_t	crtc_start_addr;
109 		uint8_t		crtc_cursor_loc_low;
110 		uint8_t		crtc_cursor_loc_high;
111 		uint16_t	crtc_cursor_loc;
112 		uint8_t		crtc_vert_retrace_start;
113 		uint8_t		crtc_vert_retrace_end;
114 		uint8_t		crtc_vert_disp_end;
115 		uint8_t		crtc_offset;
116 		uint8_t		crtc_underline_loc;
117 		uint8_t		crtc_start_vert_blank;
118 		uint8_t		crtc_end_vert_blank;
119 		uint8_t		crtc_line_compare;
120 	} vga_crtc;
121 
122 	/*
123 	 * Graphics Controller
124 	 */
125 	struct {
126 		int		gc_index;
127 		uint8_t		gc_set_reset;
128 		uint8_t		gc_enb_set_reset;
129 		uint8_t		gc_color_compare;
130 		uint8_t		gc_rotate;
131 		uint8_t		gc_op;
132 		uint8_t		gc_read_map_sel;
133 		uint8_t		gc_mode;
134 		bool		gc_mode_c4;		/* chain 4 */
135 		bool		gc_mode_oe;		/* odd/even */
136 		uint8_t		gc_mode_rm;		/* read mode */
137 		uint8_t		gc_mode_wm;		/* write mode */
138 		uint8_t		gc_misc;
139 		uint8_t		gc_misc_gm;		/* graphics mode */
140 		uint8_t		gc_misc_mm;		/* memory map */
141 		uint8_t		gc_color_dont_care;
142 		uint8_t		gc_bit_mask;
143 		uint8_t		gc_latch0;
144 		uint8_t		gc_latch1;
145 		uint8_t		gc_latch2;
146 		uint8_t		gc_latch3;
147 	} vga_gc;
148 
149 	/*
150 	 * Attribute Controller
151 	 */
152 	struct {
153 		int		atc_flipflop;
154 		int		atc_index;
155 		uint8_t		atc_palette[16];
156 		uint8_t		atc_mode;
157 		uint8_t		atc_overscan_color;
158 		uint8_t		atc_color_plane_enb;
159 		uint8_t		atc_horiz_pixel_panning;
160 		uint8_t		atc_color_select;
161 		uint8_t		atc_color_select_45;
162 		uint8_t		atc_color_select_67;
163 	} vga_atc;
164 
165 	/*
166 	 * DAC
167 	 */
168 	struct {
169 		uint8_t		dac_state;
170 		uint8_t		dac_rd_index;
171 		uint8_t		dac_rd_subindex;
172 		uint8_t		dac_wr_index;
173 		uint8_t		dac_wr_subindex;
174 		uint8_t		dac_palette[3 * 256];
175 		uint32_t	dac_palette_rgb[256];
176 	} vga_dac;
177 };
178 
179 static bool
vga_in_reset(struct vga_softc * sc)180 vga_in_reset(struct vga_softc *sc)
181 {
182 	return (((sc->vga_seq.seq_clock_mode & SEQ_CM_SO) != 0) ||
183 	    ((sc->vga_seq.seq_reset & SEQ_RESET_ASYNC) == 0) ||
184 	    ((sc->vga_seq.seq_reset & SEQ_RESET_SYNC) == 0) ||
185 	    ((sc->vga_crtc.crtc_mode_ctrl & CRTC_MC_TE) == 0));
186 }
187 
188 static void
vga_check_size(struct bhyvegc * gc,struct vga_softc * sc)189 vga_check_size(struct bhyvegc *gc, struct vga_softc *sc)
190 {
191 	int old_width, old_height;
192 
193 	if (vga_in_reset(sc))
194 		return;
195 
196 	//old_width = sc->gc_width;
197 	//old_height = sc->gc_height;
198 	old_width = sc->gc_image->width;
199 	old_height = sc->gc_image->height;
200 
201 	/*
202 	 * Horizontal Display End: For text modes this is the number
203 	 * of characters.  For graphics modes this is the number of
204 	 * pixels per scanlines divided by the number of pixels per
205 	 * character clock.
206 	 */
207 	sc->gc_width = (sc->vga_crtc.crtc_horiz_disp_end + 1) *
208 	    sc->vga_seq.seq_cm_dots;
209 
210 	sc->gc_height = (sc->vga_crtc.crtc_vert_disp_end |
211 	    (((sc->vga_crtc.crtc_overflow & CRTC_OF_VDE8) >> CRTC_OF_VDE8_SHIFT) << 8) |
212 	    (((sc->vga_crtc.crtc_overflow & CRTC_OF_VDE9) >> CRTC_OF_VDE9_SHIFT) << 9)) + 1;
213 
214 	if (old_width != sc->gc_width || old_height != sc->gc_height)
215 		bhyvegc_resize(gc, sc->gc_width, sc->gc_height);
216 }
217 
218 static uint32_t
vga_get_pixel(struct vga_softc * sc,int x,int y)219 vga_get_pixel(struct vga_softc *sc, int x, int y)
220 {
221 	int offset;
222 	int bit;
223 	uint8_t data;
224 	uint8_t idx;
225 
226 	offset = (y * sc->gc_width / 8) + (x / 8);
227 	bit = 7 - (x % 8);
228 
229 	data = (((sc->vga_ram[offset + 0 * 64*KB] >> bit) & 0x1) << 0) |
230 		(((sc->vga_ram[offset + 1 * 64*KB] >> bit) & 0x1) << 1) |
231 		(((sc->vga_ram[offset + 2 * 64*KB] >> bit) & 0x1) << 2) |
232 		(((sc->vga_ram[offset + 3 * 64*KB] >> bit) & 0x1) << 3);
233 
234 	data &= sc->vga_atc.atc_color_plane_enb;
235 
236 	if (sc->vga_atc.atc_mode & ATC_MC_IPS) {
237 		idx = sc->vga_atc.atc_palette[data] & 0x0f;
238 		idx |= sc->vga_atc.atc_color_select_45;
239 	} else {
240 		idx = sc->vga_atc.atc_palette[data];
241 	}
242 	idx |= sc->vga_atc.atc_color_select_67;
243 
244 	return (sc->vga_dac.dac_palette_rgb[idx]);
245 }
246 
247 static void
vga_render_graphics(struct vga_softc * sc)248 vga_render_graphics(struct vga_softc *sc)
249 {
250 	int x, y;
251 
252 	for (y = 0; y < sc->gc_height; y++) {
253 		for (x = 0; x < sc->gc_width; x++) {
254 			int offset;
255 
256 			offset = y * sc->gc_width + x;
257 			sc->gc_image->data[offset] = vga_get_pixel(sc, x, y);
258 		}
259 	}
260 }
261 
262 static uint32_t
vga_get_text_pixel(struct vga_softc * sc,int x,int y)263 vga_get_text_pixel(struct vga_softc *sc, int x, int y)
264 {
265 	int dots, offset, bit, font_offset;
266 	uint8_t ch, attr, font;
267 	uint8_t idx;
268 
269 	dots = sc->vga_seq.seq_cm_dots;
270 
271 	offset = 2 * sc->vga_crtc.crtc_start_addr;
272 	offset += (y / 16 * sc->gc_width / dots) * 2 + (x / dots) * 2;
273 
274 	bit = 7 - (x % dots > 7 ? 7 : x % dots);
275 
276 	ch = sc->vga_ram[offset + 0 * 64*KB];
277 	attr = sc->vga_ram[offset + 1 * 64*KB];
278 
279 	if (sc->vga_crtc.crtc_cursor_on &&
280 	    (offset == (sc->vga_crtc.crtc_cursor_loc * 2)) &&
281 	    ((y % 16) >= (sc->vga_crtc.crtc_cursor_start & CRTC_CS_CS)) &&
282 	    ((y % 16) <= (sc->vga_crtc.crtc_cursor_end & CRTC_CE_CE))) {
283 		idx = sc->vga_atc.atc_palette[attr & 0xf];
284 		return (sc->vga_dac.dac_palette_rgb[idx]);
285 	}
286 
287 	if ((sc->vga_seq.seq_mm & SEQ_MM_EM) &&
288 	    sc->vga_seq.seq_cmap_pri_off != sc->vga_seq.seq_cmap_sec_off) {
289 		if (attr & 0x8)
290 			font_offset = sc->vga_seq.seq_cmap_pri_off +
291 				(ch << 5) + y % 16;
292 		else
293 			font_offset = sc->vga_seq.seq_cmap_sec_off +
294 				(ch << 5) + y % 16;
295 		attr &= ~0x8;
296 	} else {
297 		font_offset = (ch << 5) + y % 16;
298 	}
299 
300 	font = sc->vga_ram[font_offset + 2 * 64*KB];
301 
302 	if (font & (1 << bit))
303 		idx = sc->vga_atc.atc_palette[attr & 0xf];
304 	else
305 		idx = sc->vga_atc.atc_palette[attr >> 4];
306 
307 	return (sc->vga_dac.dac_palette_rgb[idx]);
308 }
309 
310 static void
vga_render_text(struct vga_softc * sc)311 vga_render_text(struct vga_softc *sc)
312 {
313 	int x, y;
314 
315 	for (y = 0; y < sc->gc_height; y++) {
316 		for (x = 0; x < sc->gc_width; x++) {
317 			int offset;
318 
319 			offset = y * sc->gc_width + x;
320 			sc->gc_image->data[offset] = vga_get_text_pixel(sc, x, y);
321 		}
322 	}
323 }
324 
325 void
vga_render(struct bhyvegc * gc,void * arg)326 vga_render(struct bhyvegc *gc, void *arg)
327 {
328 	struct vga_softc *sc = arg;
329 
330 	vga_check_size(gc, sc);
331 
332 	if (vga_in_reset(sc)) {
333 		memset(sc->gc_image->data, 0,
334 		    sc->gc_image->width * sc->gc_image->height *
335 		     sizeof (uint32_t));
336 		return;
337 	}
338 
339 	if (sc->vga_gc.gc_misc_gm && (sc->vga_atc.atc_mode & ATC_MC_GA))
340 		vga_render_graphics(sc);
341 	else
342 		vga_render_text(sc);
343 }
344 
345 static uint64_t
vga_mem_rd_handler(struct vmctx * ctx,uint64_t addr,void * arg1)346 vga_mem_rd_handler(struct vmctx *ctx, uint64_t addr, void *arg1)
347 {
348 	struct vga_softc *sc = arg1;
349 	uint8_t map_sel;
350 	int offset;
351 
352 	offset = addr;
353 	switch (sc->vga_gc.gc_misc_mm) {
354 	case 0x0:
355 		/*
356 		 * extended mode: base 0xa0000 size 128k
357 		 */
358 		offset -=0xa0000;
359 		offset &= (128 * KB - 1);
360 		break;
361 	case 0x1:
362 		/*
363 		 * EGA/VGA mode: base 0xa0000 size 64k
364 		 */
365 		offset -=0xa0000;
366 		offset &= (64 * KB - 1);
367 		break;
368 	case 0x2:
369 		/*
370 		 * monochrome text mode: base 0xb0000 size 32kb
371 		 */
372 #ifdef __FreeBSD__
373 		assert(0);
374 #else
375 		abort();
376 #endif
377 	case 0x3:
378 		/*
379 		 * color text mode and CGA: base 0xb8000 size 32kb
380 		 */
381 		offset -=0xb8000;
382 		offset &= (32 * KB - 1);
383 		break;
384 	}
385 
386 	/* Fill latches. */
387 	sc->vga_gc.gc_latch0 = sc->vga_ram[offset + 0*64*KB];
388 	sc->vga_gc.gc_latch1 = sc->vga_ram[offset + 1*64*KB];
389 	sc->vga_gc.gc_latch2 = sc->vga_ram[offset + 2*64*KB];
390 	sc->vga_gc.gc_latch3 = sc->vga_ram[offset + 3*64*KB];
391 
392 	if (sc->vga_gc.gc_mode_rm) {
393 		/* read mode 1 */
394 		assert(0);
395 	}
396 
397 	map_sel = sc->vga_gc.gc_read_map_sel;
398 	if (sc->vga_gc.gc_mode_oe) {
399 		map_sel |= (offset & 1);
400 		offset &= ~1;
401 	}
402 
403 	/* read mode 0: return the byte from the selected plane. */
404 	offset += map_sel * 64*KB;
405 
406 	return (sc->vga_ram[offset]);
407 }
408 
409 static void
vga_mem_wr_handler(struct vmctx * ctx,uint64_t addr,uint8_t val,void * arg1)410 vga_mem_wr_handler(struct vmctx *ctx, uint64_t addr, uint8_t val, void *arg1)
411 {
412 	struct vga_softc *sc = arg1;
413 	uint8_t c0, c1, c2, c3;
414 	uint8_t m0, m1, m2, m3;
415 	uint8_t set_reset;
416 	uint8_t enb_set_reset;
417 	uint8_t	mask;
418 	int offset;
419 
420 	offset = addr;
421 	switch (sc->vga_gc.gc_misc_mm) {
422 	case 0x0:
423 		/*
424 		 * extended mode: base 0xa0000 size 128kb
425 		 */
426 		offset -=0xa0000;
427 		offset &= (128 * KB - 1);
428 		break;
429 	case 0x1:
430 		/*
431 		 * EGA/VGA mode: base 0xa0000 size 64kb
432 		 */
433 		offset -=0xa0000;
434 		offset &= (64 * KB - 1);
435 		break;
436 	case 0x2:
437 		/*
438 		 * monochrome text mode: base 0xb0000 size 32kb
439 		 */
440 #ifdef __FreeBSD__
441 		assert(0);
442 #else
443 		abort();
444 #endif
445 	case 0x3:
446 		/*
447 		 * color text mode and CGA: base 0xb8000 size 32kb
448 		 */
449 		offset -=0xb8000;
450 		offset &= (32 * KB - 1);
451 		break;
452 	}
453 
454 	set_reset = sc->vga_gc.gc_set_reset;
455 	enb_set_reset = sc->vga_gc.gc_enb_set_reset;
456 
457 	c0 = sc->vga_gc.gc_latch0;
458 	c1 = sc->vga_gc.gc_latch1;
459 	c2 = sc->vga_gc.gc_latch2;
460 	c3 = sc->vga_gc.gc_latch3;
461 
462 	switch (sc->vga_gc.gc_mode_wm) {
463 	case 0:
464 		/* write mode 0 */
465 		mask = sc->vga_gc.gc_bit_mask;
466 
467 		val = (val >> sc->vga_gc.gc_rotate) |
468 		    (val << (8 - sc->vga_gc.gc_rotate));
469 
470 		switch (sc->vga_gc.gc_op) {
471 		case 0x00:		/* replace */
472 			m0 = (set_reset & 1) ? mask : 0x00;
473 			m1 = (set_reset & 2) ? mask : 0x00;
474 			m2 = (set_reset & 4) ? mask : 0x00;
475 			m3 = (set_reset & 8) ? mask : 0x00;
476 
477 			c0 = (enb_set_reset & 1) ? (c0 & ~mask) : (val & mask);
478 			c1 = (enb_set_reset & 2) ? (c1 & ~mask) : (val & mask);
479 			c2 = (enb_set_reset & 4) ? (c2 & ~mask) : (val & mask);
480 			c3 = (enb_set_reset & 8) ? (c3 & ~mask) : (val & mask);
481 
482 			c0 |= m0;
483 			c1 |= m1;
484 			c2 |= m2;
485 			c3 |= m3;
486 			break;
487 		case 0x08:		/* AND */
488 			m0 = set_reset & 1 ? 0xff : ~mask;
489 			m1 = set_reset & 2 ? 0xff : ~mask;
490 			m2 = set_reset & 4 ? 0xff : ~mask;
491 			m3 = set_reset & 8 ? 0xff : ~mask;
492 
493 			c0 = enb_set_reset & 1 ? c0 & m0 : val & m0;
494 			c1 = enb_set_reset & 2 ? c1 & m1 : val & m1;
495 			c2 = enb_set_reset & 4 ? c2 & m2 : val & m2;
496 			c3 = enb_set_reset & 8 ? c3 & m3 : val & m3;
497 			break;
498 		case 0x10:		/* OR */
499 			m0 = set_reset & 1 ? mask : 0x00;
500 			m1 = set_reset & 2 ? mask : 0x00;
501 			m2 = set_reset & 4 ? mask : 0x00;
502 			m3 = set_reset & 8 ? mask : 0x00;
503 
504 			c0 = enb_set_reset & 1 ? c0 | m0 : val | m0;
505 			c1 = enb_set_reset & 2 ? c1 | m1 : val | m1;
506 			c2 = enb_set_reset & 4 ? c2 | m2 : val | m2;
507 			c3 = enb_set_reset & 8 ? c3 | m3 : val | m3;
508 			break;
509 		case 0x18:		/* XOR */
510 			m0 = set_reset & 1 ? mask : 0x00;
511 			m1 = set_reset & 2 ? mask : 0x00;
512 			m2 = set_reset & 4 ? mask : 0x00;
513 			m3 = set_reset & 8 ? mask : 0x00;
514 
515 			c0 = enb_set_reset & 1 ? c0 ^ m0 : val ^ m0;
516 			c1 = enb_set_reset & 2 ? c1 ^ m1 : val ^ m1;
517 			c2 = enb_set_reset & 4 ? c2 ^ m2 : val ^ m2;
518 			c3 = enb_set_reset & 8 ? c3 ^ m3 : val ^ m3;
519 			break;
520 		}
521 		break;
522 	case 1:
523 		/* write mode 1 */
524 		break;
525 	case 2:
526 		/* write mode 2 */
527 		mask = sc->vga_gc.gc_bit_mask;
528 
529 		switch (sc->vga_gc.gc_op) {
530 		case 0x00:		/* replace */
531 			m0 = (val & 1 ? 0xff : 0x00) & mask;
532 			m1 = (val & 2 ? 0xff : 0x00) & mask;
533 			m2 = (val & 4 ? 0xff : 0x00) & mask;
534 			m3 = (val & 8 ? 0xff : 0x00) & mask;
535 
536 			c0 &= ~mask;
537 			c1 &= ~mask;
538 			c2 &= ~mask;
539 			c3 &= ~mask;
540 
541 			c0 |= m0;
542 			c1 |= m1;
543 			c2 |= m2;
544 			c3 |= m3;
545 			break;
546 		case 0x08:		/* AND */
547 			m0 = (val & 1 ? 0xff : 0x00) | ~mask;
548 			m1 = (val & 2 ? 0xff : 0x00) | ~mask;
549 			m2 = (val & 4 ? 0xff : 0x00) | ~mask;
550 			m3 = (val & 8 ? 0xff : 0x00) | ~mask;
551 
552 			c0 &= m0;
553 			c1 &= m1;
554 			c2 &= m2;
555 			c3 &= m3;
556 			break;
557 		case 0x10:		/* OR */
558 			m0 = (val & 1 ? 0xff : 0x00) & mask;
559 			m1 = (val & 2 ? 0xff : 0x00) & mask;
560 			m2 = (val & 4 ? 0xff : 0x00) & mask;
561 			m3 = (val & 8 ? 0xff : 0x00) & mask;
562 
563 			c0 |= m0;
564 			c1 |= m1;
565 			c2 |= m2;
566 			c3 |= m3;
567 			break;
568 		case 0x18:		/* XOR */
569 			m0 = (val & 1 ? 0xff : 0x00) & mask;
570 			m1 = (val & 2 ? 0xff : 0x00) & mask;
571 			m2 = (val & 4 ? 0xff : 0x00) & mask;
572 			m3 = (val & 8 ? 0xff : 0x00) & mask;
573 
574 			c0 ^= m0;
575 			c1 ^= m1;
576 			c2 ^= m2;
577 			c3 ^= m3;
578 			break;
579 		}
580 		break;
581 	case 3:
582 		/* write mode 3 */
583 		mask = sc->vga_gc.gc_bit_mask & val;
584 
585 		val = (val >> sc->vga_gc.gc_rotate) |
586 		    (val << (8 - sc->vga_gc.gc_rotate));
587 
588 		switch (sc->vga_gc.gc_op) {
589 		case 0x00:		/* replace */
590 			m0 = (set_reset & 1 ? 0xff : 0x00) & mask;
591 			m1 = (set_reset & 2 ? 0xff : 0x00) & mask;
592 			m2 = (set_reset & 4 ? 0xff : 0x00) & mask;
593 			m3 = (set_reset & 8 ? 0xff : 0x00) & mask;
594 
595 			c0 &= ~mask;
596 			c1 &= ~mask;
597 			c2 &= ~mask;
598 			c3 &= ~mask;
599 
600 			c0 |= m0;
601 			c1 |= m1;
602 			c2 |= m2;
603 			c3 |= m3;
604 			break;
605 		case 0x08:		/* AND */
606 			m0 = (set_reset & 1 ? 0xff : 0x00) | ~mask;
607 			m1 = (set_reset & 2 ? 0xff : 0x00) | ~mask;
608 			m2 = (set_reset & 4 ? 0xff : 0x00) | ~mask;
609 			m3 = (set_reset & 8 ? 0xff : 0x00) | ~mask;
610 
611 			c0 &= m0;
612 			c1 &= m1;
613 			c2 &= m2;
614 			c3 &= m3;
615 			break;
616 		case 0x10:		/* OR */
617 			m0 = (set_reset & 1 ? 0xff : 0x00) & mask;
618 			m1 = (set_reset & 2 ? 0xff : 0x00) & mask;
619 			m2 = (set_reset & 4 ? 0xff : 0x00) & mask;
620 			m3 = (set_reset & 8 ? 0xff : 0x00) & mask;
621 
622 			c0 |= m0;
623 			c1 |= m1;
624 			c2 |= m2;
625 			c3 |= m3;
626 			break;
627 		case 0x18:		/* XOR */
628 			m0 = (set_reset & 1 ? 0xff : 0x00) & mask;
629 			m1 = (set_reset & 2 ? 0xff : 0x00) & mask;
630 			m2 = (set_reset & 4 ? 0xff : 0x00) & mask;
631 			m3 = (set_reset & 8 ? 0xff : 0x00) & mask;
632 
633 			c0 ^= m0;
634 			c1 ^= m1;
635 			c2 ^= m2;
636 			c3 ^= m3;
637 			break;
638 		}
639 		break;
640 	}
641 
642 	if (sc->vga_gc.gc_mode_oe) {
643 		if (offset & 1) {
644 			offset &= ~1;
645 			if (sc->vga_seq.seq_map_mask & 2)
646 				sc->vga_ram[offset + 1*64*KB] = c1;
647 			if (sc->vga_seq.seq_map_mask & 8)
648 				sc->vga_ram[offset + 3*64*KB] = c3;
649 		} else {
650 			if (sc->vga_seq.seq_map_mask & 1)
651 				sc->vga_ram[offset + 0*64*KB] = c0;
652 			if (sc->vga_seq.seq_map_mask & 4)
653 				sc->vga_ram[offset + 2*64*KB] = c2;
654 		}
655 	} else {
656 		if (sc->vga_seq.seq_map_mask & 1)
657 			sc->vga_ram[offset + 0*64*KB] = c0;
658 		if (sc->vga_seq.seq_map_mask & 2)
659 			sc->vga_ram[offset + 1*64*KB] = c1;
660 		if (sc->vga_seq.seq_map_mask & 4)
661 			sc->vga_ram[offset + 2*64*KB] = c2;
662 		if (sc->vga_seq.seq_map_mask & 8)
663 			sc->vga_ram[offset + 3*64*KB] = c3;
664 	}
665 }
666 
667 static int
vga_mem_handler(struct vmctx * ctx,int vcpu,int dir,uint64_t addr,int size,uint64_t * val,void * arg1,long arg2)668 vga_mem_handler(struct vmctx *ctx, int vcpu, int dir, uint64_t addr,
669 		int size, uint64_t *val, void *arg1, long arg2)
670 {
671 	if (dir == MEM_F_WRITE) {
672 		switch (size) {
673 		case 1:
674 			vga_mem_wr_handler(ctx, addr, *val, arg1);
675 			break;
676 		case 2:
677 			vga_mem_wr_handler(ctx, addr, *val, arg1);
678 			vga_mem_wr_handler(ctx, addr + 1, *val >> 8, arg1);
679 			break;
680 		case 4:
681 			vga_mem_wr_handler(ctx, addr, *val, arg1);
682 			vga_mem_wr_handler(ctx, addr + 1, *val >> 8, arg1);
683 			vga_mem_wr_handler(ctx, addr + 2, *val >> 16, arg1);
684 			vga_mem_wr_handler(ctx, addr + 3, *val >> 24, arg1);
685 			break;
686 		case 8:
687 			vga_mem_wr_handler(ctx, addr, *val, arg1);
688 			vga_mem_wr_handler(ctx, addr + 1, *val >> 8, arg1);
689 			vga_mem_wr_handler(ctx, addr + 2, *val >> 16, arg1);
690 			vga_mem_wr_handler(ctx, addr + 3, *val >> 24, arg1);
691 			vga_mem_wr_handler(ctx, addr + 4, *val >> 32, arg1);
692 			vga_mem_wr_handler(ctx, addr + 5, *val >> 40, arg1);
693 			vga_mem_wr_handler(ctx, addr + 6, *val >> 48, arg1);
694 			vga_mem_wr_handler(ctx, addr + 7, *val >> 56, arg1);
695 			break;
696 		}
697 	} else {
698 		switch (size) {
699 		case 1:
700 			*val = vga_mem_rd_handler(ctx, addr, arg1);
701 			break;
702 		case 2:
703 			*val = vga_mem_rd_handler(ctx, addr, arg1);
704 			*val |= vga_mem_rd_handler(ctx, addr + 1, arg1) << 8;
705 			break;
706 		case 4:
707 			*val = vga_mem_rd_handler(ctx, addr, arg1);
708 			*val |= vga_mem_rd_handler(ctx, addr + 1, arg1) << 8;
709 			*val |= vga_mem_rd_handler(ctx, addr + 2, arg1) << 16;
710 			*val |= vga_mem_rd_handler(ctx, addr + 3, arg1) << 24;
711 			break;
712 		case 8:
713 			*val = vga_mem_rd_handler(ctx, addr, arg1);
714 			*val |= vga_mem_rd_handler(ctx, addr + 1, arg1) << 8;
715 			*val |= vga_mem_rd_handler(ctx, addr + 2, arg1) << 16;
716 			*val |= vga_mem_rd_handler(ctx, addr + 3, arg1) << 24;
717 			*val |= vga_mem_rd_handler(ctx, addr + 4, arg1) << 32;
718 			*val |= vga_mem_rd_handler(ctx, addr + 5, arg1) << 40;
719 			*val |= vga_mem_rd_handler(ctx, addr + 6, arg1) << 48;
720 			*val |= vga_mem_rd_handler(ctx, addr + 7, arg1) << 56;
721 			break;
722 		}
723 	}
724 
725 	return (0);
726 }
727 
728 static int
vga_port_in_handler(struct vmctx * ctx,int in,int port,int bytes,uint8_t * val,void * arg)729 vga_port_in_handler(struct vmctx *ctx, int in, int port, int bytes,
730 		    uint8_t *val, void *arg)
731 {
732 	struct vga_softc *sc = arg;
733 
734 	switch (port) {
735 	case CRTC_IDX_MONO_PORT:
736 	case CRTC_IDX_COLOR_PORT:
737 		*val = sc->vga_crtc.crtc_index;
738 		break;
739 	case CRTC_DATA_MONO_PORT:
740 	case CRTC_DATA_COLOR_PORT:
741 		switch (sc->vga_crtc.crtc_index) {
742 		case CRTC_HORIZ_TOTAL:
743 			*val = sc->vga_crtc.crtc_horiz_total;
744 			break;
745 		case CRTC_HORIZ_DISP_END:
746 			*val = sc->vga_crtc.crtc_horiz_disp_end;
747 			break;
748 		case CRTC_START_HORIZ_BLANK:
749 			*val = sc->vga_crtc.crtc_start_horiz_blank;
750 			break;
751 		case CRTC_END_HORIZ_BLANK:
752 			*val = sc->vga_crtc.crtc_end_horiz_blank;
753 			break;
754 		case CRTC_START_HORIZ_RETRACE:
755 			*val = sc->vga_crtc.crtc_start_horiz_retrace;
756 			break;
757 		case CRTC_END_HORIZ_RETRACE:
758 			*val = sc->vga_crtc.crtc_end_horiz_retrace;
759 			break;
760 		case CRTC_VERT_TOTAL:
761 			*val = sc->vga_crtc.crtc_vert_total;
762 			break;
763 		case CRTC_OVERFLOW:
764 			*val = sc->vga_crtc.crtc_overflow;
765 			break;
766 		case CRTC_PRESET_ROW_SCAN:
767 			*val = sc->vga_crtc.crtc_present_row_scan;
768 			break;
769 		case CRTC_MAX_SCAN_LINE:
770 			*val = sc->vga_crtc.crtc_max_scan_line;
771 			break;
772 		case CRTC_CURSOR_START:
773 			*val = sc->vga_crtc.crtc_cursor_start;
774 			break;
775 		case CRTC_CURSOR_END:
776 			*val = sc->vga_crtc.crtc_cursor_end;
777 			break;
778 		case CRTC_START_ADDR_HIGH:
779 			*val = sc->vga_crtc.crtc_start_addr_high;
780 			break;
781 		case CRTC_START_ADDR_LOW:
782 			*val = sc->vga_crtc.crtc_start_addr_low;
783 			break;
784 		case CRTC_CURSOR_LOC_HIGH:
785 			*val = sc->vga_crtc.crtc_cursor_loc_high;
786 			break;
787 		case CRTC_CURSOR_LOC_LOW:
788 			*val = sc->vga_crtc.crtc_cursor_loc_low;
789 			break;
790 		case CRTC_VERT_RETRACE_START:
791 			*val = sc->vga_crtc.crtc_vert_retrace_start;
792 			break;
793 		case CRTC_VERT_RETRACE_END:
794 			*val = sc->vga_crtc.crtc_vert_retrace_end;
795 			break;
796 		case CRTC_VERT_DISP_END:
797 			*val = sc->vga_crtc.crtc_vert_disp_end;
798 			break;
799 		case CRTC_OFFSET:
800 			*val = sc->vga_crtc.crtc_offset;
801 			break;
802 		case CRTC_UNDERLINE_LOC:
803 			*val = sc->vga_crtc.crtc_underline_loc;
804 			break;
805 		case CRTC_START_VERT_BLANK:
806 			*val = sc->vga_crtc.crtc_start_vert_blank;
807 			break;
808 		case CRTC_END_VERT_BLANK:
809 			*val = sc->vga_crtc.crtc_end_vert_blank;
810 			break;
811 		case CRTC_MODE_CONTROL:
812 			*val = sc->vga_crtc.crtc_mode_ctrl;
813 			break;
814 		case CRTC_LINE_COMPARE:
815 			*val = sc->vga_crtc.crtc_line_compare;
816 			break;
817 		default:
818 			//printf("XXX VGA CRTC: inb 0x%04x at index %d\n", port, sc->vga_crtc.crtc_index);
819 			assert(0);
820 			break;
821 		}
822 		break;
823 	case ATC_IDX_PORT:
824 		*val = sc->vga_atc.atc_index;
825 		break;
826 	case ATC_DATA_PORT:
827 		switch (sc->vga_atc.atc_index) {
828 		case ATC_PALETTE0 ... ATC_PALETTE15:
829 			*val = sc->vga_atc.atc_palette[sc->vga_atc.atc_index];
830 			break;
831 		case ATC_MODE_CONTROL:
832 			*val = sc->vga_atc.atc_mode;
833 			break;
834 		case ATC_OVERSCAN_COLOR:
835 			*val = sc->vga_atc.atc_overscan_color;
836 			break;
837 		case ATC_COLOR_PLANE_ENABLE:
838 			*val = sc->vga_atc.atc_color_plane_enb;
839 			break;
840 		case ATC_HORIZ_PIXEL_PANNING:
841 			*val = sc->vga_atc.atc_horiz_pixel_panning;
842 			break;
843 		case ATC_COLOR_SELECT:
844 			*val = sc->vga_atc.atc_color_select;
845 			break;
846 		default:
847 			//printf("XXX VGA ATC inb 0x%04x at index %d\n", port , sc->vga_atc.atc_index);
848 			assert(0);
849 			break;
850 		}
851 		break;
852 	case SEQ_IDX_PORT:
853 		*val = sc->vga_seq.seq_index;
854 		break;
855 	case SEQ_DATA_PORT:
856 		switch (sc->vga_seq.seq_index) {
857 		case SEQ_RESET:
858 			*val = sc->vga_seq.seq_reset;
859 			break;
860 		case SEQ_CLOCKING_MODE:
861 			*val = sc->vga_seq.seq_clock_mode;
862 			break;
863 		case SEQ_MAP_MASK:
864 			*val = sc->vga_seq.seq_map_mask;
865 			break;
866 		case SEQ_CHAR_MAP_SELECT:
867 			*val = sc->vga_seq.seq_cmap_sel;
868 			break;
869 		case SEQ_MEMORY_MODE:
870 			*val = sc->vga_seq.seq_mm;
871 			break;
872 		default:
873 			//printf("XXX VGA SEQ: inb 0x%04x at index %d\n", port, sc->vga_seq.seq_index);
874 			assert(0);
875 			break;
876 		}
877 		break;
878 	case DAC_DATA_PORT:
879 		*val = sc->vga_dac.dac_palette[3 * sc->vga_dac.dac_rd_index +
880 					       sc->vga_dac.dac_rd_subindex];
881 		sc->vga_dac.dac_rd_subindex++;
882 		if (sc->vga_dac.dac_rd_subindex == 3) {
883 			sc->vga_dac.dac_rd_index++;
884 			sc->vga_dac.dac_rd_subindex = 0;
885 		}
886 		break;
887 	case GC_IDX_PORT:
888 		*val = sc->vga_gc.gc_index;
889 		break;
890 	case GC_DATA_PORT:
891 		switch (sc->vga_gc.gc_index) {
892 		case GC_SET_RESET:
893 			*val = sc->vga_gc.gc_set_reset;
894 			break;
895 		case GC_ENABLE_SET_RESET:
896 			*val = sc->vga_gc.gc_enb_set_reset;
897 			break;
898 		case GC_COLOR_COMPARE:
899 			*val = sc->vga_gc.gc_color_compare;
900 			break;
901 		case GC_DATA_ROTATE:
902 			*val = sc->vga_gc.gc_rotate;
903 			break;
904 		case GC_READ_MAP_SELECT:
905 			*val = sc->vga_gc.gc_read_map_sel;
906 			break;
907 		case GC_MODE:
908 			*val = sc->vga_gc.gc_mode;
909 			break;
910 		case GC_MISCELLANEOUS:
911 			*val = sc->vga_gc.gc_misc;
912 			break;
913 		case GC_COLOR_DONT_CARE:
914 			*val = sc->vga_gc.gc_color_dont_care;
915 			break;
916 		case GC_BIT_MASK:
917 			*val = sc->vga_gc.gc_bit_mask;
918 			break;
919 		default:
920 			//printf("XXX VGA GC: inb 0x%04x at index %d\n", port, sc->vga_crtc.crtc_index);
921 			assert(0);
922 			break;
923 		}
924 		break;
925 	case GEN_MISC_OUTPUT_PORT:
926 		*val = sc->vga_misc;
927 		break;
928 	case GEN_INPUT_STS0_PORT:
929 		assert(0);
930 		break;
931 	case GEN_INPUT_STS1_MONO_PORT:
932 	case GEN_INPUT_STS1_COLOR_PORT:
933 		sc->vga_atc.atc_flipflop = 0;
934 #ifdef __FreeBSD__
935 		sc->vga_sts1 = GEN_IS1_VR | GEN_IS1_DE;
936 		//sc->vga_sts1 ^= (GEN_IS1_VR | GEN_IS1_DE);
937 #else
938 		/*
939 		 * During the bhyve bring-up process, a guest image was failing
940 		 * to successfully boot.  It appeared to be spinning, waiting
941 		 * for this value to be toggled.  Until it can be ruled out
942 		 * that this is unnecessary (and documentation seems to
943 		 * indicate that it should be present),  the toggle should
944 		 * remain.
945 		 */
946 		sc->vga_sts1 ^= (GEN_IS1_VR | GEN_IS1_DE);
947 #endif
948 		*val = sc->vga_sts1;
949 		break;
950 	case GEN_FEATURE_CTRL_PORT:
951 		// OpenBSD calls this with bytes = 1
952 		//assert(0);
953 		*val = 0;
954 		break;
955 	case 0x3c3:
956 		*val = 0;
957 		break;
958 	default:
959 		printf("XXX vga_port_in_handler() unhandled port 0x%x\n", port);
960 		//assert(0);
961 		return (-1);
962 	}
963 
964 	return (0);
965 }
966 
967 static int
vga_port_out_handler(struct vmctx * ctx,int in,int port,int bytes,uint8_t val,void * arg)968 vga_port_out_handler(struct vmctx *ctx, int in, int port, int bytes,
969 		     uint8_t val, void *arg)
970 {
971 	struct vga_softc *sc = arg;
972 
973 	switch (port) {
974 	case CRTC_IDX_MONO_PORT:
975 	case CRTC_IDX_COLOR_PORT:
976 		sc->vga_crtc.crtc_index = val;
977 		break;
978 	case CRTC_DATA_MONO_PORT:
979 	case CRTC_DATA_COLOR_PORT:
980 		switch (sc->vga_crtc.crtc_index) {
981 		case CRTC_HORIZ_TOTAL:
982 			sc->vga_crtc.crtc_horiz_total = val;
983 			break;
984 		case CRTC_HORIZ_DISP_END:
985 			sc->vga_crtc.crtc_horiz_disp_end = val;
986 			break;
987 		case CRTC_START_HORIZ_BLANK:
988 			sc->vga_crtc.crtc_start_horiz_blank = val;
989 			break;
990 		case CRTC_END_HORIZ_BLANK:
991 			sc->vga_crtc.crtc_end_horiz_blank = val;
992 			break;
993 		case CRTC_START_HORIZ_RETRACE:
994 			sc->vga_crtc.crtc_start_horiz_retrace = val;
995 			break;
996 		case CRTC_END_HORIZ_RETRACE:
997 			sc->vga_crtc.crtc_end_horiz_retrace = val;
998 			break;
999 		case CRTC_VERT_TOTAL:
1000 			sc->vga_crtc.crtc_vert_total = val;
1001 			break;
1002 		case CRTC_OVERFLOW:
1003 			sc->vga_crtc.crtc_overflow = val;
1004 			break;
1005 		case CRTC_PRESET_ROW_SCAN:
1006 			sc->vga_crtc.crtc_present_row_scan = val;
1007 			break;
1008 		case CRTC_MAX_SCAN_LINE:
1009 			sc->vga_crtc.crtc_max_scan_line = val;
1010 			break;
1011 		case CRTC_CURSOR_START:
1012 			sc->vga_crtc.crtc_cursor_start = val;
1013 			sc->vga_crtc.crtc_cursor_on = (val & CRTC_CS_CO) == 0;
1014 			break;
1015 		case CRTC_CURSOR_END:
1016 			sc->vga_crtc.crtc_cursor_end = val;
1017 			break;
1018 		case CRTC_START_ADDR_HIGH:
1019 			sc->vga_crtc.crtc_start_addr_high = val;
1020 			sc->vga_crtc.crtc_start_addr &= 0x00ff;
1021 			sc->vga_crtc.crtc_start_addr |= (val << 8);
1022 			break;
1023 		case CRTC_START_ADDR_LOW:
1024 			sc->vga_crtc.crtc_start_addr_low = val;
1025 			sc->vga_crtc.crtc_start_addr &= 0xff00;
1026 			sc->vga_crtc.crtc_start_addr |= (val & 0xff);
1027 			break;
1028 		case CRTC_CURSOR_LOC_HIGH:
1029 			sc->vga_crtc.crtc_cursor_loc_high = val;
1030 			sc->vga_crtc.crtc_cursor_loc &= 0x00ff;
1031 			sc->vga_crtc.crtc_cursor_loc |= (val << 8);
1032 			break;
1033 		case CRTC_CURSOR_LOC_LOW:
1034 			sc->vga_crtc.crtc_cursor_loc_low = val;
1035 			sc->vga_crtc.crtc_cursor_loc &= 0xff00;
1036 			sc->vga_crtc.crtc_cursor_loc |= (val & 0xff);
1037 			break;
1038 		case CRTC_VERT_RETRACE_START:
1039 			sc->vga_crtc.crtc_vert_retrace_start = val;
1040 			break;
1041 		case CRTC_VERT_RETRACE_END:
1042 			sc->vga_crtc.crtc_vert_retrace_end = val;
1043 			break;
1044 		case CRTC_VERT_DISP_END:
1045 			sc->vga_crtc.crtc_vert_disp_end = val;
1046 			break;
1047 		case CRTC_OFFSET:
1048 			sc->vga_crtc.crtc_offset = val;
1049 			break;
1050 		case CRTC_UNDERLINE_LOC:
1051 			sc->vga_crtc.crtc_underline_loc = val;
1052 			break;
1053 		case CRTC_START_VERT_BLANK:
1054 			sc->vga_crtc.crtc_start_vert_blank = val;
1055 			break;
1056 		case CRTC_END_VERT_BLANK:
1057 			sc->vga_crtc.crtc_end_vert_blank = val;
1058 			break;
1059 		case CRTC_MODE_CONTROL:
1060 			sc->vga_crtc.crtc_mode_ctrl = val;
1061 			break;
1062 		case CRTC_LINE_COMPARE:
1063 			sc->vga_crtc.crtc_line_compare = val;
1064 			break;
1065 		default:
1066 			//printf("XXX VGA CRTC: outb 0x%04x, 0x%02x at index %d\n", port, val, sc->vga_crtc.crtc_index);
1067 			assert(0);
1068 			break;
1069 		}
1070 		break;
1071 	case ATC_IDX_PORT:
1072 		if (sc->vga_atc.atc_flipflop == 0) {
1073 			if (sc->vga_atc.atc_index & 0x20)
1074 				assert(0);
1075 			sc->vga_atc.atc_index = val & ATC_IDX_MASK;
1076 		} else {
1077 			switch (sc->vga_atc.atc_index) {
1078 			case ATC_PALETTE0 ... ATC_PALETTE15:
1079 				sc->vga_atc.atc_palette[sc->vga_atc.atc_index] = val & 0x3f;
1080 				break;
1081 			case ATC_MODE_CONTROL:
1082 				sc->vga_atc.atc_mode = val;
1083 				break;
1084 			case ATC_OVERSCAN_COLOR:
1085 				sc->vga_atc.atc_overscan_color = val;
1086 				break;
1087 			case ATC_COLOR_PLANE_ENABLE:
1088 				sc->vga_atc.atc_color_plane_enb = val;
1089 				break;
1090 			case ATC_HORIZ_PIXEL_PANNING:
1091 				sc->vga_atc.atc_horiz_pixel_panning = val;
1092 				break;
1093 			case ATC_COLOR_SELECT:
1094 				sc->vga_atc.atc_color_select = val;
1095 				sc->vga_atc.atc_color_select_45 =
1096 					(val & ATC_CS_C45) << 4;
1097 				sc->vga_atc.atc_color_select_67 =
1098 					((val & ATC_CS_C67) >> 2) << 6;
1099 				break;
1100 			default:
1101 				//printf("XXX VGA ATC: outb 0x%04x, 0x%02x at index %d\n", port, val, sc->vga_atc.atc_index);
1102 				assert(0);
1103 				break;
1104 			}
1105 		}
1106 		sc->vga_atc.atc_flipflop ^= 1;
1107 		break;
1108 	case ATC_DATA_PORT:
1109 		break;
1110 	case SEQ_IDX_PORT:
1111 		sc->vga_seq.seq_index = val & 0x1f;
1112 		break;
1113 	case SEQ_DATA_PORT:
1114 		switch (sc->vga_seq.seq_index) {
1115 		case SEQ_RESET:
1116 			sc->vga_seq.seq_reset = val;
1117 			break;
1118 		case SEQ_CLOCKING_MODE:
1119 			sc->vga_seq.seq_clock_mode = val;
1120 			sc->vga_seq.seq_cm_dots = (val & SEQ_CM_89) ? 8 : 9;
1121 			break;
1122 		case SEQ_MAP_MASK:
1123 			sc->vga_seq.seq_map_mask = val;
1124 			break;
1125 		case SEQ_CHAR_MAP_SELECT:
1126 			sc->vga_seq.seq_cmap_sel = val;
1127 
1128 			sc->vga_seq.seq_cmap_pri_off = ((((val & SEQ_CMS_SA) >> SEQ_CMS_SA_SHIFT) * 2) + ((val & SEQ_CMS_SAH) >> SEQ_CMS_SAH_SHIFT)) * 8 * KB;
1129 			sc->vga_seq.seq_cmap_sec_off = ((((val & SEQ_CMS_SB) >> SEQ_CMS_SB_SHIFT) * 2) + ((val & SEQ_CMS_SBH) >> SEQ_CMS_SBH_SHIFT)) * 8 * KB;
1130 			break;
1131 		case SEQ_MEMORY_MODE:
1132 			sc->vga_seq.seq_mm = val;
1133 			/* Windows queries Chain4 */
1134 			//assert((sc->vga_seq.seq_mm & SEQ_MM_C4) == 0);
1135 			break;
1136 		default:
1137 			//printf("XXX VGA SEQ: outb 0x%04x, 0x%02x at index %d\n", port, val, sc->vga_seq.seq_index);
1138 			assert(0);
1139 			break;
1140 		}
1141 		break;
1142 	case DAC_MASK:
1143 		break;
1144 	case DAC_IDX_RD_PORT:
1145 		sc->vga_dac.dac_rd_index = val;
1146 		sc->vga_dac.dac_rd_subindex = 0;
1147 		break;
1148 	case DAC_IDX_WR_PORT:
1149 		sc->vga_dac.dac_wr_index = val;
1150 		sc->vga_dac.dac_wr_subindex = 0;
1151 		break;
1152 	case DAC_DATA_PORT:
1153 		sc->vga_dac.dac_palette[3 * sc->vga_dac.dac_wr_index +
1154 					sc->vga_dac.dac_wr_subindex] = val;
1155 		sc->vga_dac.dac_wr_subindex++;
1156 		if (sc->vga_dac.dac_wr_subindex == 3) {
1157 			sc->vga_dac.dac_palette_rgb[sc->vga_dac.dac_wr_index] =
1158 				((((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 0] << 2) |
1159 				   ((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 0] & 0x1) << 1) |
1160 				   (sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 0] & 0x1)) << 16) |
1161 				 (((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 1] << 2) |
1162 				   ((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 1] & 0x1) << 1) |
1163 				   (sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 1] & 0x1)) << 8) |
1164 				 (((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 2] << 2) |
1165 				   ((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 2] & 0x1) << 1) |
1166 				   (sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 2] & 0x1)) << 0));
1167 
1168 			sc->vga_dac.dac_wr_index++;
1169 			sc->vga_dac.dac_wr_subindex = 0;
1170 		}
1171 		break;
1172 	case GC_IDX_PORT:
1173 		sc->vga_gc.gc_index = val;
1174 		break;
1175 	case GC_DATA_PORT:
1176 		switch (sc->vga_gc.gc_index) {
1177 		case GC_SET_RESET:
1178 			sc->vga_gc.gc_set_reset = val;
1179 			break;
1180 		case GC_ENABLE_SET_RESET:
1181 			sc->vga_gc.gc_enb_set_reset = val;
1182 			break;
1183 		case GC_COLOR_COMPARE:
1184 			sc->vga_gc.gc_color_compare = val;
1185 			break;
1186 		case GC_DATA_ROTATE:
1187 			sc->vga_gc.gc_rotate = val;
1188 			sc->vga_gc.gc_op = (val >> 3) & 0x3;
1189 			break;
1190 		case GC_READ_MAP_SELECT:
1191 			sc->vga_gc.gc_read_map_sel = val;
1192 			break;
1193 		case GC_MODE:
1194 			sc->vga_gc.gc_mode = val;
1195 			sc->vga_gc.gc_mode_c4 = (val & GC_MODE_C4) != 0;
1196 			assert(!sc->vga_gc.gc_mode_c4);
1197 			sc->vga_gc.gc_mode_oe = (val & GC_MODE_OE) != 0;
1198 			sc->vga_gc.gc_mode_rm = (val >> 3) & 0x1;
1199 			sc->vga_gc.gc_mode_wm = val & 0x3;
1200 
1201 			if (sc->gc_image)
1202 				sc->gc_image->vgamode = 1;
1203 			break;
1204 		case GC_MISCELLANEOUS:
1205 			sc->vga_gc.gc_misc = val;
1206 			sc->vga_gc.gc_misc_gm = val & GC_MISC_GM;
1207 			sc->vga_gc.gc_misc_mm = (val & GC_MISC_MM) >>
1208 			    GC_MISC_MM_SHIFT;
1209 			break;
1210 		case GC_COLOR_DONT_CARE:
1211 			sc->vga_gc.gc_color_dont_care = val;
1212 			break;
1213 		case GC_BIT_MASK:
1214 			sc->vga_gc.gc_bit_mask = val;
1215 			break;
1216 		default:
1217 			//printf("XXX VGA GC: outb 0x%04x, 0x%02x at index %d\n", port, val, sc->vga_gc.gc_index);
1218 			assert(0);
1219 			break;
1220 		}
1221 		break;
1222 	case GEN_INPUT_STS0_PORT:
1223 		/* write to Miscellaneous Output Register */
1224 		sc->vga_misc = val;
1225 		break;
1226 	case GEN_INPUT_STS1_MONO_PORT:
1227 	case GEN_INPUT_STS1_COLOR_PORT:
1228 		/* write to Feature Control Register */
1229 		break;
1230 //	case 0x3c3:
1231 //		break;
1232 	default:
1233 		printf("XXX vga_port_out_handler() unhandled port 0x%x, val 0x%x\n", port, val);
1234 		//assert(0);
1235 		return (-1);
1236 	}
1237 	return (0);
1238 }
1239 
1240 static int
vga_port_handler(struct vmctx * ctx,int vcpu,int in,int port,int bytes,uint32_t * eax,void * arg)1241 vga_port_handler(struct vmctx *ctx, int vcpu, int in, int port, int bytes,
1242 		 uint32_t *eax, void *arg)
1243 {
1244 	uint8_t val;
1245 	int error;
1246 
1247 	switch (bytes) {
1248 	case 1:
1249 		if (in) {
1250 			*eax &= ~0xff;
1251 			error = vga_port_in_handler(ctx, in, port, 1,
1252 						    &val, arg);
1253 			if (!error) {
1254 				*eax |= val & 0xff;
1255 			}
1256 		} else {
1257 			val = *eax & 0xff;
1258 			error = vga_port_out_handler(ctx, in, port, 1,
1259 						     val, arg);
1260 		}
1261 		break;
1262 	case 2:
1263 		if (in) {
1264 			*eax &= ~0xffff;
1265 			error = vga_port_in_handler(ctx, in, port, 1,
1266 						    &val, arg);
1267 			if (!error) {
1268 				*eax |= val & 0xff;
1269 			}
1270 			error = vga_port_in_handler(ctx, in, port + 1, 1,
1271 						    &val, arg);
1272 			if (!error) {
1273 				*eax |= (val & 0xff) << 8;
1274 			}
1275 		} else {
1276 			val = *eax & 0xff;
1277 			error = vga_port_out_handler(ctx, in, port, 1,
1278 						     val, arg);
1279 			val = (*eax >> 8) & 0xff;
1280 			error =vga_port_out_handler(ctx, in, port + 1, 1,
1281 						    val, arg);
1282 		}
1283 		break;
1284 	default:
1285 		assert(0);
1286 		return (-1);
1287 	}
1288 
1289 	return (error);
1290 }
1291 
1292 void *
vga_init(int io_only)1293 vga_init(int io_only)
1294 {
1295 	struct inout_port iop;
1296 	struct vga_softc *sc;
1297 	int port, error;
1298 
1299 	sc = calloc(1, sizeof(struct vga_softc));
1300 
1301 	bzero(&iop, sizeof(struct inout_port));
1302 	iop.name = "VGA";
1303 	for (port = VGA_IOPORT_START; port <= VGA_IOPORT_END; port++) {
1304 		iop.port = port;
1305 		iop.size = 1;
1306 		iop.flags = IOPORT_F_INOUT;
1307 		iop.handler = vga_port_handler;
1308 		iop.arg = sc;
1309 
1310 		error = register_inout(&iop);
1311 		assert(error == 0);
1312 	}
1313 
1314 	sc->gc_image = console_get_image();
1315 
1316 	/* only handle io ports; vga graphics is disabled */
1317 	if (io_only)
1318 		return(sc);
1319 
1320 	sc->mr.name = "VGA memory";
1321 	sc->mr.flags = MEM_F_RW;
1322 	sc->mr.base = 640 * KB;
1323 	sc->mr.size = 128 * KB;
1324 	sc->mr.handler = vga_mem_handler;
1325 	sc->mr.arg1 = sc;
1326 	error = register_mem_fallback(&sc->mr);
1327 	assert(error == 0);
1328 
1329 	sc->vga_ram = malloc(256 * KB);
1330 	memset(sc->vga_ram, 0, 256 * KB);
1331 
1332 	{
1333 		static uint8_t palette[] = {
1334 			0x00,0x00,0x00, 0x00,0x00,0x2a, 0x00,0x2a,0x00, 0x00,0x2a,0x2a,
1335 			0x2a,0x00,0x00, 0x2a,0x00,0x2a, 0x2a,0x2a,0x00, 0x2a,0x2a,0x2a,
1336 			0x00,0x00,0x15, 0x00,0x00,0x3f, 0x00,0x2a,0x15, 0x00,0x2a,0x3f,
1337 			0x2a,0x00,0x15, 0x2a,0x00,0x3f, 0x2a,0x2a,0x15, 0x2a,0x2a,0x3f,
1338 		};
1339 		int i;
1340 
1341 		memcpy(sc->vga_dac.dac_palette, palette, 16 * 3 * sizeof (uint8_t));
1342 		for (i = 0; i < 16; i++) {
1343 			sc->vga_dac.dac_palette_rgb[i] =
1344 				((((sc->vga_dac.dac_palette[3*i + 0] << 2) |
1345 				   ((sc->vga_dac.dac_palette[3*i + 0] & 0x1) << 1) |
1346 				   (sc->vga_dac.dac_palette[3*i + 0] & 0x1)) << 16) |
1347 				 (((sc->vga_dac.dac_palette[3*i + 1] << 2) |
1348 				   ((sc->vga_dac.dac_palette[3*i + 1] & 0x1) << 1) |
1349 				   (sc->vga_dac.dac_palette[3*i + 1] & 0x1)) << 8) |
1350 				 (((sc->vga_dac.dac_palette[3*i + 2] << 2) |
1351 				   ((sc->vga_dac.dac_palette[3*i + 2] & 0x1) << 1) |
1352 				   (sc->vga_dac.dac_palette[3*i + 2] & 0x1)) << 0));
1353 		}
1354 	}
1355 
1356 	return (sc);
1357 }
1358