vgasrc/clext.c

   1 //  QEMU Cirrus CLGD 54xx VGABIOS Extension.
   2 //
   3 // Copyright (C) 2009  Kevin O'Connor <kevin@koconnor.net>
   4 //  Copyright (c) 2004 Makoto Suzuki (suzu)
   5 //
   6 // This file may be distributed under the terms of the GNU LGPLv3 license.
   7
   8 #include "clext.h" // clext_init
   9 #include "vgabios.h" // VBE_VENDOR_STRING
  10 #include "biosvar.h" // GET_GLOBAL
  11 #include "util.h" // dprintf
  12 #include "bregs.h" // struct bregs
  13 #include "vbe.h" // struct vbe_info
  14 #include "stdvga.h" // VGAREG_SEQU_ADDRESS
  15 #include "pci.h" // pci_config_readl
  16 #include "pci_regs.h" // PCI_BASE_ADDRESS_0
  17
  18
  19 /****************************************************************
  20  * tables
  21  ****************************************************************/
  22
  23 /* VGA */
  24 static u16 cseq_vga[] VAR16 = {0x0007,0xffff};
  25 static u16 cgraph_vga[] VAR16 = {0x0009,0x000a,0x000b,0xffff};
  26 static u16 ccrtc_vga[] VAR16 = {0x001a,0x001b,0x001d,0xffff};
  27
  28 /* extensions */
  29 static u16 cgraph_svgacolor[] VAR16 = {
  30     0x0000,0x0001,0x0002,0x0003,0x0004,0x4005,0x0506,0x0f07,0xff08,
  31     0x0009,0x000a,0x000b,
  32     0xffff
  33 };
  34 /* 640x480x8 */
  35 static u16 cseq_640x480x8[] VAR16 = {
  36     0x0300,0x2101,0x0f02,0x0003,0x0e04,0x1107,
  37     0x580b,0x580c,0x580d,0x580e,
  38     0x0412,0x0013,0x2017,
  39     0x331b,0x331c,0x331d,0x331e,
  40     0xffff
  41 };
  42 static u16 ccrtc_640x480x8[] VAR16 = {
  43     0x2c11,
  44     0x5f00,0x4f01,0x4f02,0x8003,0x5204,0x1e05,0x0b06,0x3e07,
  45     0x4009,0x000c,0x000d,
  46     0xea10,0xdf12,0x5013,0x4014,0xdf15,0x0b16,0xc317,0xff18,
  47     0x001a,0x221b,0x001d,
  48     0xffff
  49 };
  50 /* 640x480x16 */
  51 static u16 cseq_640x480x16[] VAR16 = {
  52     0x0300,0x2101,0x0f02,0x0003,0x0e04,0x1707,
  53     0x580b,0x580c,0x580d,0x580e,
  54     0x0412,0x0013,0x2017,
  55     0x331b,0x331c,0x331d,0x331e,
  56     0xffff
  57 };
  58 static u16 ccrtc_640x480x16[] VAR16 = {
  59     0x2c11,
  60     0x5f00,0x4f01,0x4f02,0x8003,0x5204,0x1e05,0x0b06,0x3e07,
  61     0x4009,0x000c,0x000d,
  62     0xea10,0xdf12,0xa013,0x4014,0xdf15,0x0b16,0xc317,0xff18,
  63     0x001a,0x221b,0x001d,
  64     0xffff
  65 };
  66 /* 640x480x24 */
  67 static u16 cseq_640x480x24[] VAR16 = {
  68     0x0300,0x2101,0x0f02,0x0003,0x0e04,0x1507,
  69     0x580b,0x580c,0x580d,0x580e,
  70     0x0412,0x0013,0x2017,
  71     0x331b,0x331c,0x331d,0x331e,
  72     0xffff
  73 };
  74 static u16 ccrtc_640x480x24[] VAR16 = {
  75     0x2c11,
  76     0x5f00,0x4f01,0x4f02,0x8003,0x5204,0x1e05,0x0b06,0x3e07,
  77     0x4009,0x000c,0x000d,
  78     0xea10,0xdf12,0xf013,0x4014,0xdf15,0x0b16,0xc317,0xff18,
  79     0x001a,0x221b,0x001d,
  80     0xffff
  81 };
  82 /* 800x600x8 */
  83 static u16 cseq_800x600x8[] VAR16 = {
  84     0x0300,0x2101,0x0f02,0x0003,0x0e04,0x1107,
  85     0x230b,0x230c,0x230d,0x230e,
  86     0x0412,0x0013,0x2017,
  87     0x141b,0x141c,0x141d,0x141e,
  88     0xffff
  89 };
  90 static u16 ccrtc_800x600x8[] VAR16 = {
  91     0x2311,0x7d00,0x6301,0x6302,0x8003,0x6b04,0x1a05,0x9806,0xf007,
  92     0x6009,0x000c,0x000d,
  93     0x7d10,0x5712,0x6413,0x4014,0x5715,0x9816,0xc317,0xff18,
  94     0x001a,0x221b,0x001d,
  95     0xffff
  96 };
  97 /* 800x600x16 */
  98 static u16 cseq_800x600x16[] VAR16 = {
  99     0x0300,0x2101,0x0f02,0x0003,0x0e04,0x1707,
 100     0x230b,0x230c,0x230d,0x230e,
 101     0x0412,0x0013,0x2017,
 102     0x141b,0x141c,0x141d,0x141e,
 103     0xffff
 104 };
 105 static u16 ccrtc_800x600x16[] VAR16 = {
 106     0x2311,0x7d00,0x6301,0x6302,0x8003,0x6b04,0x1a05,0x9806,0xf007,
 107     0x6009,0x000c,0x000d,
 108     0x7d10,0x5712,0xc813,0x4014,0x5715,0x9816,0xc317,0xff18,
 109     0x001a,0x221b,0x001d,
 110     0xffff
 111 };
 112 /* 800x600x24 */
 113 static u16 cseq_800x600x24[] VAR16 = {
 114     0x0300,0x2101,0x0f02,0x0003,0x0e04,0x1507,
 115     0x230b,0x230c,0x230d,0x230e,
 116     0x0412,0x0013,0x2017,
 117     0x141b,0x141c,0x141d,0x141e,
 118     0xffff
 119 };
 120 static u16 ccrtc_800x600x24[] VAR16 = {
 121     0x2311,0x7d00,0x6301,0x6302,0x8003,0x6b04,0x1a05,0x9806,0xf007,
 122     0x6009,0x000c,0x000d,
 123     0x7d10,0x5712,0x2c13,0x4014,0x5715,0x9816,0xc317,0xff18,
 124     0x001a,0x321b,0x001d,
 125     0xffff
 126 };
 127 /* 1024x768x8 */
 128 static u16 cseq_1024x768x8[] VAR16 = {
 129     0x0300,0x2101,0x0f02,0x0003,0x0e04,0x1107,
 130     0x760b,0x760c,0x760d,0x760e,
 131     0x0412,0x0013,0x2017,
 132     0x341b,0x341c,0x341d,0x341e,
 133     0xffff
 134 };
 135 static u16 ccrtc_1024x768x8[] VAR16 = {
 136     0x2911,0xa300,0x7f01,0x7f02,0x8603,0x8304,0x9405,0x2406,0xf507,
 137     0x6009,0x000c,0x000d,
 138     0x0310,0xff12,0x8013,0x4014,0xff15,0x2416,0xc317,0xff18,
 139     0x001a,0x221b,0x001d,
 140     0xffff
 141 };
 142 /* 1024x768x16 */
 143 static u16 cseq_1024x768x16[] VAR16 = {
 144     0x0300,0x2101,0x0f02,0x0003,0x0e04,0x1707,
 145     0x760b,0x760c,0x760d,0x760e,
 146     0x0412,0x0013,0x2017,
 147     0x341b,0x341c,0x341d,0x341e,
 148     0xffff
 149 };
 150 static u16 ccrtc_1024x768x16[] VAR16 = {
 151     0x2911,0xa300,0x7f01,0x7f02,0x8603,0x8304,0x9405,0x2406,0xf507,
 152     0x6009,0x000c,0x000d,
 153     0x0310,0xff12,0x0013,0x4014,0xff15,0x2416,0xc317,0xff18,
 154     0x001a,0x321b,0x001d,
 155     0xffff
 156 };
 157 /* 1024x768x24 */
 158 static u16 cseq_1024x768x24[] VAR16 = {
 159     0x0300,0x2101,0x0f02,0x0003,0x0e04,0x1507,
 160     0x760b,0x760c,0x760d,0x760e,
 161     0x0412,0x0013,0x2017,
 162     0x341b,0x341c,0x341d,0x341e,
 163     0xffff
 164 };
 165 static u16 ccrtc_1024x768x24[] VAR16 = {
 166     0x2911,0xa300,0x7f01,0x7f02,0x8603,0x8304,0x9405,0x2406,0xf507,
 167     0x6009,0x000c,0x000d,
 168     0x0310,0xff12,0x8013,0x4014,0xff15,0x2416,0xc317,0xff18,
 169     0x001a,0x321b,0x001d,
 170     0xffff
 171 };
 172 /* 1280x1024x8 */
 173 static u16 cseq_1280x1024x8[] VAR16 = {
 174     0x0300,0x2101,0x0f02,0x0003,0x0e04,0x1107,
 175     0x760b,0x760c,0x760d,0x760e,
 176     0x0412,0x0013,0x2017,
 177     0x341b,0x341c,0x341d,0x341e,
 178     0xffff
 179 };
 180 static u16 ccrtc_1280x1024x8[] VAR16 = {
 181     0x2911,0xc300,0x9f01,0x9f02,0x8603,0x8304,0x9405,0x2406,0xf707,
 182     0x6009,0x000c,0x000d,
 183     0x0310,0xff12,0xa013,0x4014,0xff15,0x2416,0xc317,0xff18,
 184     0x001a,0x221b,0x001d,
 185     0xffff
 186 };
 187 /* 1280x1024x16 */
 188 static u16 cseq_1280x1024x16[] VAR16 = {
 189     0x0300,0x2101,0x0f02,0x0003,0x0e04,0x1707,
 190     0x760b,0x760c,0x760d,0x760e,
 191     0x0412,0x0013,0x2017,
 192     0x341b,0x341c,0x341d,0x341e,
 193     0xffff
 194 };
 195 static u16 ccrtc_1280x1024x16[] VAR16 = {
 196     0x2911,0xc300,0x9f01,0x9f02,0x8603,0x8304,0x9405,0x2406,0xf707,
 197     0x6009,0x000c,0x000d,
 198     0x0310,0xff12,0x4013,0x4014,0xff15,0x2416,0xc317,0xff18,
 199     0x001a,0x321b,0x001d,
 200     0xffff
 201 };
 202
 203 /* 1600x1200x8 */
 204 static u16 cseq_1600x1200x8[] VAR16 = {
 205     0x0300,0x2101,0x0f02,0x0003,0x0e04,0x1107,
 206     0x760b,0x760c,0x760d,0x760e,
 207     0x0412,0x0013,0x2017,
 208     0x341b,0x341c,0x341d,0x341e,
 209     0xffff
 210 };
 211 static u16 ccrtc_1600x1200x8[] VAR16 = {
 212     0x2911,0xc300,0x9f01,0x9f02,0x8603,0x8304,0x9405,0x2406,0xf707,
 213     0x6009,0x000c,0x000d,
 214     0x0310,0xff12,0xc813,0x4014,0xff15,0x2416,0xc317,0xff18,
 215     0x001a,0x221b,0x001d,
 216     0xffff
 217 };
 218
 219 struct cirrus_mode_s {
 220     u16 mode;
 221     struct vgamode_s info;
 222
 223     u16 hidden_dac; /* 0x3c6 */
 224     u16 *seq; /* 0x3c4 */
 225     u16 *graph; /* 0x3ce */
 226     u16 *crtc; /* 0x3d4 */
 227 };
 228
 229 static struct cirrus_mode_s cirrus_modes[] VAR16 = {
 230     {0x5f,{MM_PACKED,640,480,8},0x00,
 231      cseq_640x480x8,cgraph_svgacolor,ccrtc_640x480x8},
 232     {0x64,{MM_DIRECT,640,480,16},0xe1,
 233      cseq_640x480x16,cgraph_svgacolor,ccrtc_640x480x16},
 234     {0x66,{MM_DIRECT,640,480,15},0xf0,
 235      cseq_640x480x16,cgraph_svgacolor,ccrtc_640x480x16},
 236     {0x71,{MM_DIRECT,640,480,24},0xe5,
 237      cseq_640x480x24,cgraph_svgacolor,ccrtc_640x480x24},
 238
 239     {0x5c,{MM_PACKED,800,600,8},0x00,
 240      cseq_800x600x8,cgraph_svgacolor,ccrtc_800x600x8},
 241     {0x65,{MM_DIRECT,800,600,16},0xe1,
 242      cseq_800x600x16,cgraph_svgacolor,ccrtc_800x600x16},
 243     {0x67,{MM_DIRECT,800,600,15},0xf0,
 244      cseq_800x600x16,cgraph_svgacolor,ccrtc_800x600x16},
 245
 246     {0x60,{MM_PACKED,1024,768,8},0x00,
 247      cseq_1024x768x8,cgraph_svgacolor,ccrtc_1024x768x8},
 248     {0x74,{MM_DIRECT,1024,768,16},0xe1,
 249      cseq_1024x768x16,cgraph_svgacolor,ccrtc_1024x768x16},
 250     {0x68,{MM_DIRECT,1024,768,15},0xf0,
 251      cseq_1024x768x16,cgraph_svgacolor,ccrtc_1024x768x16},
 252
 253     {0x78,{MM_DIRECT,800,600,24},0xe5,
 254      cseq_800x600x24,cgraph_svgacolor,ccrtc_800x600x24},
 255     {0x79,{MM_DIRECT,1024,768,24},0xe5,
 256      cseq_1024x768x24,cgraph_svgacolor,ccrtc_1024x768x24},
 257
 258     {0x6d,{MM_PACKED,1280,1024,8},0x00,
 259      cseq_1280x1024x8,cgraph_svgacolor,ccrtc_1280x1024x8},
 260     {0x69,{MM_DIRECT,1280,1024,15},0xf0,
 261      cseq_1280x1024x16,cgraph_svgacolor,ccrtc_1280x1024x16},
 262     {0x75,{MM_DIRECT,1280,1024,16},0xe1,
 263      cseq_1280x1024x16,cgraph_svgacolor,ccrtc_1280x1024x16},
 264
 265     {0x7b,{MM_PACKED,1600,1200,8},0x00,
 266      cseq_1600x1200x8,cgraph_svgacolor,ccrtc_1600x1200x8},
 267 };
 268
 269 static struct cirrus_mode_s mode_switchback VAR16 =
 270     {0xfe,{0xff},0,cseq_vga,cgraph_vga,ccrtc_vga};
 271
 272 static struct {
 273     u16 vesamode, mode;
 274 } cirrus_vesa_modelist[] VAR16 = {
 275     // 640x480x8
 276     { 0x101, 0x5f },
 277     // 640x480x15
 278     { 0x110, 0x66 },
 279     // 640x480x16
 280     { 0x111, 0x64 },
 281     // 640x480x24
 282     { 0x112, 0x71 },
 283     // 800x600x8
 284     { 0x103, 0x5c },
 285     // 800x600x15
 286     { 0x113, 0x67 },
 287     // 800x600x16
 288     { 0x114, 0x65 },
 289     // 800x600x24
 290     { 0x115, 0x78 },
 291     // 1024x768x8
 292     { 0x105, 0x60 },
 293     // 1024x768x15
 294     { 0x116, 0x68 },
 295     // 1024x768x16
 296     { 0x117, 0x74 },
 297     // 1024x768x24
 298     { 0x118, 0x79 },
 299     // 1280x1024x8
 300     { 0x107, 0x6d },
 301     // 1280x1024x15
 302     { 0x119, 0x69 },
 303     // 1280x1024x16
 304     { 0x11a, 0x75 },
 305 };
 306
 307
 308 /****************************************************************
 309  * helper functions
 310  ****************************************************************/
 311
 312 static u16
 313 cirrus_vesamode_to_mode(u16 vesamode)
 314 {
 315     int i;
 316     for (i=0; i<ARRAY_SIZE(cirrus_vesa_modelist); i++)
 317         if (GET_GLOBAL(cirrus_vesa_modelist[i].vesamode) == vesamode)
 318             return GET_GLOBAL(cirrus_vesa_modelist[i].mode);
 319     return 0;
 320 }
 321
 322 static struct cirrus_mode_s *
 323 cirrus_get_modeentry(int mode)
 324 {
 325     int transmode = cirrus_vesamode_to_mode(mode);
 326     if (transmode)
 327         mode = transmode;
 328     struct cirrus_mode_s *table_g = cirrus_modes;
 329     while (table_g < &cirrus_modes[ARRAY_SIZE(cirrus_modes)]) {
 330         u16 tmode = GET_GLOBAL(table_g->mode);
 331         if (tmode == mode)
 332             return table_g;
 333         table_g++;
 334     }
 335     return NULL;
 336 }
 337
 338 struct vgamode_s *
 339 clext_find_mode(int mode)
 340 {
 341     struct cirrus_mode_s *table_g = cirrus_get_modeentry(mode);
 342     if (table_g)
 343         return &table_g->info;
 344     return stdvga_find_mode(mode);
 345 }
 346
 347 static void
 348 cirrus_switch_mode_setregs(u16 *data, u16 port)
 349 {
 350     for (;;) {
 351         u16 val = GET_GLOBAL(*data);
 352         if (val == 0xffff)
 353             return;
 354         outw(val, port);
 355         data++;
 356     }
 357 }
 358
 359 static void
 360 cirrus_switch_mode(struct cirrus_mode_s *table)
 361 {
 362     // Unlock cirrus special
 363     stdvga_sequ_write(0x06, 0x12);
 364     cirrus_switch_mode_setregs(GET_GLOBAL(table->seq), VGAREG_SEQU_ADDRESS);
 365     cirrus_switch_mode_setregs(GET_GLOBAL(table->graph), VGAREG_GRDC_ADDRESS);
 366     cirrus_switch_mode_setregs(GET_GLOBAL(table->crtc), stdvga_get_crtc());
 367
 368     stdvga_pelmask_write(0x00);
 369     stdvga_pelmask_read();
 370     stdvga_pelmask_read();
 371     stdvga_pelmask_read();
 372     stdvga_pelmask_read();
 373     stdvga_pelmask_write(GET_GLOBAL(table->hidden_dac));
 374     stdvga_pelmask_write(0xff);
 375
 376     u8 memmodel = GET_GLOBAL(table->info.memmodel);
 377     u8 on = 0;
 378     if (memmodel == MM_PLANAR)
 379         on = 0x41;
 380     else if (memmodel != MM_TEXT)
 381         on = 0x01;
 382     stdvga_attr_mask(0x10, 0x01, on);
 383 }
 384
 385 static u8
 386 cirrus_get_memsize(void)
 387 {
 388     // get DRAM band width
 389     u8 v = stdvga_sequ_read(0x0f);
 390     u8 x = (v >> 3) & 0x03;
 391     if (x == 0x03 && v & 0x80)
 392         // 4MB
 393         return 0x40;
 394     return 0x04 << x;
 395 }
 396
 397 static void
 398 cirrus_enable_16k_granularity(void)
 399 {
 400     stdvga_grdc_mask(0x0b, 0x00, 0x20);
 401 }
 402
 403 static void
 404 cirrus_clear_vram(u16 param)
 405 {
 406     cirrus_enable_16k_granularity();
 407     u8 count = cirrus_get_memsize() * 4;
 408     u8 i;
 409     for (i=0; i<count; i++) {
 410         stdvga_grdc_write(0x09, i);
 411         memset16_far(SEG_GRAPH, 0, param, 16 * 1024);
 412     }
 413     stdvga_grdc_write(0x09, 0x00);
 414 }
 415
 416 int
 417 clext_set_mode(int mode, int flags)
 418 {
 419     dprintf(1, "cirrus mode %x\n", mode);
 420     SET_BDA(vbe_mode, 0);
 421     struct cirrus_mode_s *table_g = cirrus_get_modeentry(mode);
 422     if (table_g) {
 423         cirrus_switch_mode(table_g);
 424         if (!(flags & MF_LINEARFB))
 425             cirrus_enable_16k_granularity();
 426         if (!(flags & MF_NOCLEARMEM))
 427             cirrus_clear_vram(0);
 428         SET_BDA(video_mode, mode);
 429         SET_BDA(vbe_mode, mode | flags);
 430         return 0;
 431     }
 432     cirrus_switch_mode(&mode_switchback);
 433     dprintf(1, "cirrus mode switch regular\n");
 434     return stdvga_set_mode(mode, flags);
 435 }
 436
 437 static int
 438 cirrus_check(void)
 439 {
 440     stdvga_sequ_write(0x06, 0x92);
 441     return stdvga_sequ_read(0x06) == 0x12;
 442 }
 443
 444
 445 /****************************************************************
 446  * extbios
 447  ****************************************************************/
 448
 449 static void
 450 clext_101280(struct bregs *regs)
 451 {
 452     u8 v = stdvga_crtc_read(stdvga_get_crtc(), 0x27);
 453     if (v == 0xa0)
 454         // 5430
 455         regs->ax = 0x0032;
 456     else if (v == 0xb8)
 457         // 5446
 458         regs->ax = 0x0039;
 459     else
 460         regs->ax = 0x00ff;
 461     regs->bx = 0x00;
 462     return;
 463 }
 464
 465 static void
 466 clext_101281(struct bregs *regs)
 467 {
 468     // XXX
 469     regs->ax = 0x0100;
 470 }
 471
 472 static void
 473 clext_101282(struct bregs *regs)
 474 {
 475     regs->al = stdvga_crtc_read(stdvga_get_crtc(), 0x27) & 0x03;
 476     regs->ah = 0xAF;
 477 }
 478
 479 static void
 480 clext_101285(struct bregs *regs)
 481 {
 482     regs->al = cirrus_get_memsize();
 483 }
 484
 485 static void
 486 clext_10129a(struct bregs *regs)
 487 {
 488     regs->ax = 0x4060;
 489     regs->cx = 0x1132;
 490 }
 491
 492 extern void a0h_callback(void);
 493 ASM16(
 494     // fatal: not implemented yet
 495     "a0h_callback:"
 496     "cli\n"
 497     "hlt\n"
 498     "retf");
 499
 500 static void
 501 clext_1012a0(struct bregs *regs)
 502 {
 503     struct cirrus_mode_s *table_g = cirrus_get_modeentry(regs->al & 0x7f);
 504     regs->ah = (table_g ? 1 : 0);
 505     regs->si = 0xffff;
 506     regs->di = regs->ds = regs->es = regs->bx = (u32)a0h_callback;
 507 }
 508
 509 static void
 510 clext_1012a1(struct bregs *regs)
 511 {
 512     regs->bx = 0x0e00; // IBM 8512/8513, color
 513 }
 514
 515 static void
 516 clext_1012a2(struct bregs *regs)
 517 {
 518     regs->al = 0x07; // HSync 31.5 - 64.0 kHz
 519 }
 520
 521 static void
 522 clext_1012ae(struct bregs *regs)
 523 {
 524     regs->al = 0x01; // High Refresh 75Hz
 525 }
 526
 527 static void
 528 clext_1012XX(struct bregs *regs)
 529 {
 530     debug_stub(regs);
 531 }
 532
 533 void
 534 clext_1012(struct bregs *regs)
 535 {
 536     switch (regs->bl) {
 537     case 0x80: clext_101280(regs); break;
 538     case 0x81: clext_101281(regs); break;
 539     case 0x82: clext_101282(regs); break;
 540     case 0x85: clext_101285(regs); break;
 541     case 0x9a: clext_10129a(regs); break;
 542     case 0xa0: clext_1012a0(regs); break;
 543     case 0xa1: clext_1012a1(regs); break;
 544     case 0xa2: clext_1012a2(regs); break;
 545     case 0xae: clext_1012ae(regs); break;
 546     default:   clext_1012XX(regs); break;
 547     }
 548 }
 549
 550
 551 /****************************************************************
 552  * vesa calls
 553  ****************************************************************/
 554
 555 void
 556 clext_list_modes(u16 seg, u16 *dest, u16 *last)
 557 {
 558     int i;
 559     for (i=0; i<ARRAY_SIZE(cirrus_vesa_modelist) && dest<last; i++) {
 560         SET_FARVAR(seg, *dest, GET_GLOBAL(cirrus_vesa_modelist[i].vesamode));
 561         dest++;
 562     }
 563     stdvga_list_modes(seg, dest, last);
 564 }
 565
 566 static u8
 567 cirrus_get_bpp_bytes(void)
 568 {
 569     u8 v = stdvga_sequ_read(0x07) & 0x0e;
 570     if (v == 0x06)
 571         v &= 0x02;
 572     v >>= 1;
 573     if (v != 0x04)
 574         v++;
 575     return v;
 576 }
 577
 578 static void
 579 cirrus_set_line_offset(u16 new_line_offset)
 580 {
 581     new_line_offset /= 8;
 582     u16 crtc_addr = stdvga_get_crtc();
 583     stdvga_crtc_write(crtc_addr, 0x13, new_line_offset);
 584     stdvga_crtc_mask(crtc_addr, 0x1b, 0x10, (new_line_offset & 0x100) >> 4);
 585 }
 586
 587 static u16
 588 cirrus_get_line_offset(void)
 589 {
 590     u16 crtc_addr = stdvga_get_crtc();
 591     u8 reg13 = stdvga_crtc_read(crtc_addr, 0x13);
 592     u8 reg1b = stdvga_crtc_read(crtc_addr, 0x1b);
 593     return (((reg1b & 0x10) << 4) + reg13) * 8;
 594 }
 595
 596 static void
 597 cirrus_set_start_addr(u32 addr)
 598 {
 599     u16 crtc_addr = stdvga_get_crtc();
 600     stdvga_crtc_write(crtc_addr, 0x0d, addr);
 601     stdvga_crtc_write(crtc_addr, 0x0c, addr >> 8);
 602     stdvga_crtc_mask(crtc_addr, 0x1d, 0x80, (addr & 0x0800) >> 4);
 603     stdvga_crtc_mask(crtc_addr, 0x1b, 0x0d
 604                      , ((addr & 0x0100) >> 8) | ((addr & 0x0600) >> 7));
 605 }
 606
 607 static u32
 608 cirrus_get_start_addr(void)
 609 {
 610     u16 crtc_addr = stdvga_get_crtc();
 611     u8 b2 = stdvga_crtc_read(crtc_addr, 0x0c);
 612     u8 b1 = stdvga_crtc_read(crtc_addr, 0x0d);
 613     u8 b3 = stdvga_crtc_read(crtc_addr, 0x1b);
 614     u8 b4 = stdvga_crtc_read(crtc_addr, 0x1d);
 615     return (b1 | (b2<<8) | ((b3 & 0x01) << 16) | ((b3 & 0x0c) << 15)
 616             | ((b4 & 0x80) << 12));
 617 }
 618
 619 static void
 620 cirrus_vesa_05h(struct bregs *regs)
 621 {
 622     if (regs->bl > 1)
 623         goto fail;
 624     if (regs->bh == 0) {
 625         // set mempage
 626         if (regs->dx >= 0x100)
 627             goto fail;
 628         stdvga_grdc_write(regs->bl + 9, regs->dx);
 629     } else if (regs->bh == 1) {
 630         // get mempage
 631         regs->dx = stdvga_grdc_read(regs->bl + 9);
 632     } else
 633         goto fail;
 634
 635     regs->ax = 0x004f;
 636     return;
 637 fail:
 638     regs->ax = 0x014f;
 639 }
 640
 641 static void
 642 cirrus_vesa_06h(struct bregs *regs)
 643 {
 644     if (regs->bl > 2) {
 645         regs->ax = 0x0100;
 646         return;
 647     }
 648
 649     if (regs->bl == 0x00) {
 650         cirrus_set_line_offset(cirrus_get_bpp_bytes() * regs->cx);
 651     } else if (regs->bl == 0x02) {
 652         cirrus_set_line_offset(regs->cx);
 653     }
 654
 655     u32 v = cirrus_get_line_offset();
 656     regs->cx = v / cirrus_get_bpp_bytes();
 657     regs->bx = v;
 658     regs->dx = (cirrus_get_memsize() * 64 * 1024) / v;
 659     regs->ax = 0x004f;
 660 }
 661
 662 static void
 663 cirrus_vesa_07h(struct bregs *regs)
 664 {
 665     if (regs->bl == 0x80 || regs->bl == 0x00) {
 666         u32 addr = (cirrus_get_bpp_bytes() * regs->cx
 667                     + cirrus_get_line_offset() * regs->dx);
 668         cirrus_set_start_addr(addr / 4);
 669     } else if (regs->bl == 0x01) {
 670         u32 addr = cirrus_get_start_addr() * 4;
 671         u32 linelength = cirrus_get_line_offset();
 672         regs->dx = addr / linelength;
 673         regs->cx = (addr % linelength) / cirrus_get_bpp_bytes();
 674     } else {
 675         regs->ax = 0x0100;
 676         return;
 677     }
 678
 679     regs->ax = 0x004f;
 680 }
 681
 682 static void
 683 cirrus_vesa_10h(struct bregs *regs)
 684 {
 685     if (regs->bl == 0x00) {
 686         regs->bx = 0x0f30;
 687         regs->ax = 0x004f;
 688         return;
 689     }
 690     if (regs->bl == 0x01) {
 691         SET_BDA(vbe_flag, regs->bh);
 692         regs->ax = 0x004f;
 693         return;
 694     }
 695     if (regs->bl == 0x02) {
 696         regs->bh = GET_BDA(vbe_flag);
 697         regs->ax = 0x004f;
 698         return;
 699     }
 700     regs->ax = 0x014f;
 701 }
 702
 703 static void
 704 cirrus_vesa_not_handled(struct bregs *regs)
 705 {
 706     debug_stub(regs);
 707     regs->ax = 0x014f;
 708 }
 709
 710 void
 711 cirrus_vesa(struct bregs *regs)
 712 {
 713     switch (regs->al) {
 714     case 0x05: cirrus_vesa_05h(regs); break;
 715     case 0x06: cirrus_vesa_06h(regs); break;
 716     case 0x07: cirrus_vesa_07h(regs); break;
 717     case 0x10: cirrus_vesa_10h(regs); break;
 718     default:   cirrus_vesa_not_handled(regs); break;
 719     }
 720 }
 721
 722
 723 /****************************************************************
 724  * init
 725  ****************************************************************/
 726
 727 int
 728 clext_init(void)
 729 {
 730     int ret = stdvga_init();
 731     if (ret)
 732         return ret;
 733
 734     dprintf(1, "cirrus init\n");
 735     if (! cirrus_check())
 736         return -1;
 737     dprintf(1, "cirrus init 2\n");
 738
 739     u32 lfb_addr = 0;
 740     int bdf = GET_GLOBAL(VgaBDF);
 741     if (CONFIG_VGA_PCI && bdf >= 0)
 742         lfb_addr = (pci_config_readl(bdf, PCI_BASE_ADDRESS_0)
 743                     & PCI_BASE_ADDRESS_MEM_MASK);
 744     SET_VGA(VBE_framebuffer, lfb_addr);
 745     u16 totalmem = cirrus_get_memsize();
 746     SET_VGA(VBE_total_memory, totalmem * 64 * 1024);
 747     SET_VGA(VBE_win_granularity, 16);
 748
 749     // memory setup
 750     stdvga_sequ_write(0x0a, stdvga_sequ_read(0x0f) & 0x18);
 751     // set vga mode
 752     stdvga_sequ_write(0x07, 0x00);
 753     // reset bitblt
 754     stdvga_grdc_write(0x31, 0x04);
 755     stdvga_grdc_write(0x31, 0x00);
 756
 757     return 0;
 758 }