qemu/roms/seabios/vgasrc/vgafb.c

   1 // Code for manipulating VGA framebuffers.
   2 //
   3 // Copyright (C) 2009-2014  Kevin O'Connor <kevin@koconnor.net>
   4 // Copyright (C) 2001-2008 the LGPL VGABios developers Team
   5 //
   6 // This file may be distributed under the terms of the GNU LGPLv3 license.
   7
   8 #include "biosvar.h" // GET_BDA
   9 #include "byteorder.h" // cpu_to_be16
  10 #include "output.h" // dprintf
  11 #include "stdvga.h" // stdvga_planar4_plane
  12 #include "string.h" // memset_far
  13 #include "vgabios.h" // vgafb_scroll
  14 #include "vgahw.h" // vgahw_get_linelength
  15
  16 static inline void
  17 memmove_stride(u16 seg, void *dst, void *src, int copylen, int stride, int lines)
  18 {
  19     if (src < dst) {
  20         dst += stride * (lines - 1);
  21         src += stride * (lines - 1);
  22         stride = -stride;
  23     }
  24     for (; lines; lines--, dst+=stride, src+=stride)
  25         memcpy_far(seg, dst, seg, src, copylen);
  26 }
  27
  28 static inline void
  29 memset_stride(u16 seg, void *dst, u8 val, int setlen, int stride, int lines)
  30 {
  31     for (; lines; lines--, dst+=stride)
  32         memset_far(seg, dst, val, setlen);
  33 }
  34
  35 static inline void
  36 memset16_stride(u16 seg, void *dst, u16 val, int setlen, int stride, int lines)
  37 {
  38     for (; lines; lines--, dst+=stride)
  39         memset16_far(seg, dst, val, setlen);
  40 }
  41
  42
  43 /****************************************************************
  44  * Basic stdvga graphic manipulation
  45  ****************************************************************/
  46
  47 static void
  48 gfx_planar(struct gfx_op *op)
  49 {
  50     if (!CONFIG_VGA_STDVGA_PORTS)
  51         return;
  52     void *dest_far = (void*)(op->y * op->linelength + op->x / 8);
  53     int plane;
  54     switch (op->op) {
  55     default:
  56     case GO_READ8:
  57         memset(op->pixels, 0, sizeof(op->pixels));
  58         for (plane = 0; plane < 4; plane++) {
  59             stdvga_planar4_plane(plane);
  60             u8 data = GET_FARVAR(SEG_GRAPH, *(u8*)dest_far);
  61             int pixel;
  62             for (pixel=0; pixel<8; pixel++)
  63                 op->pixels[pixel] |= ((data>>(7-pixel)) & 1) << plane;
  64         }
  65         break;
  66     case GO_WRITE8:
  67         for (plane = 0; plane<4; plane++) {
  68             stdvga_planar4_plane(plane);
  69             u8 data = 0;
  70             int pixel;
  71             for (pixel=0; pixel<8; pixel++)
  72                 data |= ((op->pixels[pixel]>>plane) & 1) << (7-pixel);
  73             SET_FARVAR(SEG_GRAPH, *(u8*)dest_far, data);
  74         }
  75         break;
  76     case GO_MEMSET:
  77         for (plane = 0; plane < 4; plane++) {
  78             stdvga_planar4_plane(plane);
  79             u8 data = (op->pixels[0] & (1<<plane)) ? 0xff : 0x00;
  80             memset_stride(SEG_GRAPH, dest_far, data
  81                           , op->xlen / 8, op->linelength, op->ylen);
  82         }
  83         break;
  84     case GO_MEMMOVE: ;
  85         void *src_far = (void*)(op->srcy * op->linelength + op->x / 8);
  86         for (plane = 0; plane < 4; plane++) {
  87             stdvga_planar4_plane(plane);
  88             memmove_stride(SEG_GRAPH, dest_far, src_far
  89                            , op->xlen / 8, op->linelength, op->ylen);
  90         }
  91         break;
  92     }
  93     stdvga_planar4_plane(-1);
  94 }
  95
  96 static void
  97 gfx_cga(struct gfx_op *op)
  98 {
  99     int bpp = GET_GLOBAL(op->vmode_g->depth);
 100     void *dest_far = (void*)(op->y / 2 * op->linelength + op->x / 8 * bpp);
 101     switch (op->op) {
 102     default:
 103     case GO_READ8:
 104         if (op->y & 1)
 105             dest_far += 0x2000;
 106         if (bpp == 1) {
 107             u8 data = GET_FARVAR(SEG_CTEXT, *(u8*)dest_far);
 108             int pixel;
 109             for (pixel=0; pixel<8; pixel++)
 110                 op->pixels[pixel] = (data >> (7-pixel)) & 1;
 111         } else {
 112             u16 data = GET_FARVAR(SEG_CTEXT, *(u16*)dest_far);
 113             data = be16_to_cpu(data);
 114             int pixel;
 115             for (pixel=0; pixel<8; pixel++)
 116                 op->pixels[pixel] = (data >> ((7-pixel)*2)) & 3;
 117         }
 118         break;
 119     case GO_WRITE8:
 120         if (op->y & 1)
 121             dest_far += 0x2000;
 122         if (bpp == 1) {
 123             u8 data = 0;
 124             int pixel;
 125             for (pixel=0; pixel<8; pixel++)
 126                 data |= (op->pixels[pixel] & 1) << (7-pixel);
 127             SET_FARVAR(SEG_CTEXT, *(u8*)dest_far, data);
 128         } else {
 129             u16 data = 0;
 130             int pixel;
 131             for (pixel=0; pixel<8; pixel++)
 132                 data |= (op->pixels[pixel] & 3) << ((7-pixel) * 2);
 133             data = cpu_to_be16(data);
 134             SET_FARVAR(SEG_CTEXT, *(u16*)dest_far, data);
 135         }
 136         break;
 137     case GO_MEMSET: ;
 138         u8 data = op->pixels[0];
 139         if (bpp == 1)
 140             data = (data&1) | ((data&1)<<1);
 141         data &= 3;
 142         data |= (data<<2) | (data<<4) | (data<<6);
 143         memset_stride(SEG_CTEXT, dest_far, data
 144                       , op->xlen / 8 * bpp, op->linelength, op->ylen / 2);
 145         memset_stride(SEG_CTEXT, dest_far + 0x2000, data
 146                       , op->xlen / 8 * bpp, op->linelength, op->ylen / 2);
 147         break;
 148     case GO_MEMMOVE: ;
 149         void *src_far = (void*)(op->srcy / 2 * op->linelength + op->x / 8 * bpp);
 150         memmove_stride(SEG_CTEXT, dest_far, src_far
 151                        , op->xlen / 8 * bpp, op->linelength, op->ylen / 2);
 152         memmove_stride(SEG_CTEXT, dest_far + 0x2000, src_far + 0x2000
 153                        , op->xlen / 8 * bpp, op->linelength, op->ylen / 2);
 154         break;
 155     }
 156 }
 157
 158 static void
 159 gfx_packed(struct gfx_op *op)
 160 {
 161     void *dest_far = (void*)(op->y * op->linelength + op->x);
 162     switch (op->op) {
 163     default:
 164     case GO_READ8:
 165         memcpy_far(GET_SEG(SS), op->pixels, SEG_GRAPH, dest_far, 8);
 166         break;
 167     case GO_WRITE8:
 168         memcpy_far(SEG_GRAPH, dest_far, GET_SEG(SS), op->pixels, 8);
 169         break;
 170     case GO_MEMSET:
 171         memset_stride(SEG_GRAPH, dest_far, op->pixels[0]
 172                       , op->xlen, op->linelength, op->ylen);
 173         break;
 174     case GO_MEMMOVE: ;
 175         void *src_far = (void*)(op->srcy * op->linelength + op->x);
 176         memmove_stride(SEG_GRAPH, dest_far, src_far
 177                        , op->xlen, op->linelength, op->ylen);
 178         break;
 179     }
 180 }
 181
 182
 183 /****************************************************************
 184  * Direct framebuffers in high mem
 185  ****************************************************************/
 186
 187 // Use int 1587 call to copy memory to/from the framebuffer.
 188 static void
 189 memcpy_high(void *dest, void *src, u32 len)
 190 {
 191     u64 gdt[6];
 192     gdt[2] = GDT_DATA | GDT_LIMIT(0xfffff) | GDT_BASE((u32)src);
 193     gdt[3] = GDT_DATA | GDT_LIMIT(0xfffff) | GDT_BASE((u32)dest);
 194
 195     // Call int 1587 to copy data.
 196     len/=2;
 197     u32 flags;
 198     u32 eax = 0x8700;
 199     u32 si = (u32)&gdt;
 200     SET_SEG(ES, GET_SEG(SS));
 201     asm volatile(
 202         "stc\n"
 203         "int $0x15\n"
 204         "cli\n"
 205         "cld\n"
 206         "pushfl\n"
 207         "popl %0\n"
 208         : "=r" (flags), "+a" (eax), "+S" (si), "+c" (len)
 209         : : "cc", "memory");
 210 }
 211
 212 static void
 213 memmove_stride_high(void *dst, void *src, int copylen, int stride, int lines)
 214 {
 215     if (src < dst) {
 216         dst += stride * (lines - 1);
 217         src += stride * (lines - 1);
 218         stride = -stride;
 219     }
 220     for (; lines; lines--, dst+=stride, src+=stride)
 221         memcpy_high(dst, src, copylen);
 222 }
 223
 224 // Map a CGA color to a "direct" mode rgb value.
 225 static u32
 226 get_color(int depth, u8 attr)
 227 {
 228     int rbits, gbits, bbits;
 229     switch (depth) {
 230     case 15: rbits=5; gbits=5; bbits=5; break;
 231     case 16: rbits=5; gbits=6; bbits=5; break;
 232     default:
 233     case 24: rbits=8; gbits=8; bbits=8; break;
 234     }
 235     int h = (attr&8) ? 1 : 0;
 236     int r = (attr&4) ? 2 : 0, g = (attr&2) ? 2 : 0, b = (attr&1) ? 2 : 0;
 237     if ((attr & 0xf) == 6)
 238         g = 1;
 239     int rv = DIV_ROUND_CLOSEST(((1<<rbits) - 1) * (r + h), 3);
 240     int gv = DIV_ROUND_CLOSEST(((1<<gbits) - 1) * (g + h), 3);
 241     int bv = DIV_ROUND_CLOSEST(((1<<bbits) - 1) * (b + h), 3);
 242     return (rv << (gbits+bbits)) + (gv << bbits) + bv;
 243 }
 244
 245 // Find the closest attribute for a given framebuffer color
 246 static u8
 247 reverse_color(int depth, u32 color)
 248 {
 249     int rbits, gbits, bbits;
 250     switch (depth) {
 251     case 15: rbits=5; gbits=5; bbits=5; break;
 252     case 16: rbits=5; gbits=6; bbits=5; break;
 253     default:
 254     case 24: rbits=8; gbits=8; bbits=8; break;
 255     }
 256     int rv = (color >> (gbits+bbits)) & ((1<<rbits)-1);
 257     int gv = (color >> bbits) & ((1<<gbits)-1);
 258     int bv = color & ((1<<bbits)-1);
 259     int r = DIV_ROUND_CLOSEST(rv * 3, (1<<rbits) - 1);
 260     int g = DIV_ROUND_CLOSEST(gv * 3, (1<<gbits) - 1);
 261     int b = DIV_ROUND_CLOSEST(bv * 3, (1<<bbits) - 1);
 262     int h = r && g && b && (r != 2 || g != 2 || b != 2);
 263     return (h ? 8 : 0) | ((r-h) ? 4 : 0) | ((g-h) ? 2 : 0) | ((b-h) ? 1 : 0);
 264 }
 265
 266 static void
 267 gfx_direct(struct gfx_op *op)
 268 {
 269     void *fb = (void*)GET_GLOBAL(VBE_framebuffer);
 270     if (!fb)
 271         return;
 272     int depth = GET_GLOBAL(op->vmode_g->depth);
 273     int bypp = DIV_ROUND_UP(depth, 8);
 274     void *dest_far = (fb + op->displaystart + op->y * op->linelength
 275                       + op->x * bypp);
 276     switch (op->op) {
 277     default:
 278     case GO_READ8: {
 279         u8 data[64];
 280         memcpy_high(MAKE_FLATPTR(GET_SEG(SS), data), dest_far, bypp * 8);
 281         int i;
 282         for (i=0; i<8; i++)
 283             op->pixels[i] = reverse_color(depth, *(u32*)&data[i*bypp]);
 284         break;
 285     }
 286     case GO_WRITE8: {
 287         u8 data[64];
 288         int i;
 289         for (i=0; i<8; i++)
 290             *(u32*)&data[i*bypp] = get_color(depth, op->pixels[i]);
 291         memcpy_high(dest_far, MAKE_FLATPTR(GET_SEG(SS), data), bypp * 8);
 292         break;
 293     }
 294     case GO_MEMSET: {
 295         u32 color = get_color(depth, op->pixels[0]);
 296         u8 data[64];
 297         int i;
 298         for (i=0; i<8; i++)
 299             *(u32*)&data[i*bypp] = color;
 300         memcpy_high(dest_far, MAKE_FLATPTR(GET_SEG(SS), data), bypp * 8);
 301         memcpy_high(dest_far + bypp * 8, dest_far, op->xlen * bypp - bypp * 8);
 302         for (i=1; i < op->ylen; i++)
 303             memcpy_high(dest_far + op->linelength * i
 304                         , dest_far, op->xlen * bypp);
 305         break;
 306     }
 307     case GO_MEMMOVE: ;
 308         void *src_far = (fb + op->displaystart + op->srcy * op->linelength
 309                          + op->x * bypp);
 310         memmove_stride_high(dest_far, src_far
 311                             , op->xlen * bypp, op->linelength, op->ylen);
 312         break;
 313     }
 314 }
 315
 316
 317 /****************************************************************
 318  * Gfx interface
 319  ****************************************************************/
 320
 321 // Prepare a struct gfx_op for use.
 322 void
 323 init_gfx_op(struct gfx_op *op, struct vgamode_s *vmode_g)
 324 {
 325     memset(op, 0, sizeof(*op));
 326     op->vmode_g = vmode_g;
 327     op->linelength = vgahw_get_linelength(vmode_g);
 328     op->displaystart = vgahw_get_displaystart(vmode_g);
 329 }
 330
 331 // Issue a graphics operation.
 332 void
 333 handle_gfx_op(struct gfx_op *op)
 334 {
 335     switch (GET_GLOBAL(op->vmode_g->memmodel)) {
 336     case MM_PLANAR:
 337         gfx_planar(op);
 338         break;
 339     case MM_CGA:
 340         gfx_cga(op);
 341         break;
 342     case MM_PACKED:
 343         gfx_packed(op);
 344         break;
 345     case MM_DIRECT:
 346         gfx_direct(op);
 347         break;
 348     default:
 349         break;
 350     }
 351 }
 352
 353 // Move characters when in graphics mode.
 354 static void
 355 gfx_move_chars(struct vgamode_s *vmode_g, struct cursorpos dest
 356                , struct cursorpos src, struct cursorpos movesize)
 357 {
 358     struct gfx_op op;
 359     init_gfx_op(&op, vmode_g);
 360     op.x = dest.x * 8;
 361     op.xlen = movesize.x * 8;
 362     int cheight = GET_BDA(char_height);
 363     op.y = dest.y * cheight;
 364     op.ylen = movesize.y * cheight;
 365     op.srcy = src.y * cheight;
 366     op.op = GO_MEMMOVE;
 367     handle_gfx_op(&op);
 368 }
 369
 370 // Clear area of screen in graphics mode.
 371 static void
 372 gfx_clear_chars(struct vgamode_s *vmode_g, struct cursorpos dest
 373                 , struct carattr ca, struct cursorpos clearsize)
 374 {
 375     struct gfx_op op;
 376     init_gfx_op(&op, vmode_g);
 377     op.x = dest.x * 8;
 378     op.xlen = clearsize.x * 8;
 379     int cheight = GET_BDA(char_height);
 380     op.y = dest.y * cheight;
 381     op.ylen = clearsize.y * cheight;
 382     op.pixels[0] = ca.attr;
 383     if (vga_emulate_text())
 384         op.pixels[0] = ca.attr >> 4;
 385     op.op = GO_MEMSET;
 386     handle_gfx_op(&op);
 387 }
 388
 389 // Return the font for a given character
 390 struct segoff_s
 391 get_font_data(u8 c)
 392 {
 393     int char_height = GET_BDA(char_height);
 394     struct segoff_s font;
 395     if (char_height == 8 && c >= 128) {
 396         font = GET_IVT(0x1f);
 397         c -= 128;
 398     } else {
 399         font = GET_IVT(0x43);
 400     }
 401     font.offset += c * char_height;
 402     return font;
 403 }
 404
 405 // Write a character to the screen in graphics mode.
 406 static void
 407 gfx_write_char(struct vgamode_s *vmode_g
 408                 , struct cursorpos cp, struct carattr ca)
 409 {
 410     if (cp.x >= GET_BDA(video_cols))
 411         return;
 412
 413     struct segoff_s font = get_font_data(ca.car);
 414     struct gfx_op op;
 415     init_gfx_op(&op, vmode_g);
 416     op.x = cp.x * 8;
 417     int cheight = GET_BDA(char_height);
 418     op.y = cp.y * cheight;
 419     u8 fgattr = ca.attr, bgattr = 0x00;
 420     int usexor = 0;
 421     if (vga_emulate_text()) {
 422         if (ca.use_attr) {
 423             bgattr = fgattr >> 4;
 424             fgattr = fgattr & 0x0f;
 425         } else {
 426             // Read bottom right pixel of the cell to guess bg color
 427             op.op = GO_READ8;
 428             op.y += cheight-1;
 429             handle_gfx_op(&op);
 430             op.y -= cheight-1;
 431             bgattr = op.pixels[7];
 432             fgattr = bgattr ^ 0x7;
 433         }
 434     } else if (fgattr & 0x80 && GET_GLOBAL(vmode_g->depth) < 8) {
 435         usexor = 1;
 436         fgattr &= 0x7f;
 437     }
 438     int i;
 439     for (i = 0; i < cheight; i++, op.y++) {
 440         u8 fontline = GET_FARVAR(font.seg, *(u8*)(font.offset+i));
 441         if (usexor) {
 442             op.op = GO_READ8;
 443             handle_gfx_op(&op);
 444             int j;
 445             for (j = 0; j < 8; j++)
 446                 op.pixels[j] ^= (fontline & (0x80>>j)) ? fgattr : 0x00;
 447         } else {
 448             int j;
 449             for (j = 0; j < 8; j++)
 450                 op.pixels[j] = (fontline & (0x80>>j)) ? fgattr : bgattr;
 451         }
 452         op.op = GO_WRITE8;
 453         handle_gfx_op(&op);
 454     }
 455 }
 456
 457 // Read a character from the screen in graphics mode.
 458 static struct carattr
 459 gfx_read_char(struct vgamode_s *vmode_g, struct cursorpos cp)
 460 {
 461     u8 lines[16];
 462     int cheight = GET_BDA(char_height);
 463     if (cp.x >= GET_BDA(video_cols) || cheight > ARRAY_SIZE(lines))
 464         goto fail;
 465
 466     // Read cell from screen
 467     struct gfx_op op;
 468     init_gfx_op(&op, vmode_g);
 469     op.op = GO_READ8;
 470     op.x = cp.x * 8;
 471     op.y = cp.y * cheight;
 472     int car = 0;
 473     u8 fgattr = 0x00, bgattr = 0x00;
 474     if (vga_emulate_text()) {
 475         // Read bottom right pixel of the cell to guess bg color
 476         op.y += cheight-1;
 477         handle_gfx_op(&op);
 478         op.y -= cheight-1;
 479         bgattr = op.pixels[7];
 480         fgattr = bgattr ^ 0x7;
 481         // Report space character for blank cells (skip null character check)
 482         car = 1;
 483     }
 484     u8 i, j;
 485     for (i=0; i<cheight; i++, op.y++) {
 486         u8 line = 0;
 487         handle_gfx_op(&op);
 488         for (j=0; j<8; j++)
 489             if (op.pixels[j] != bgattr) {
 490                 line |= 0x80 >> j;
 491                 fgattr = op.pixels[j];
 492             }
 493         lines[i] = line;
 494     }
 495
 496     // Determine font
 497     for (; car<256; car++) {
 498         struct segoff_s font = get_font_data(car);
 499         if (memcmp_far(GET_SEG(SS), lines
 500                        , font.seg, (void*)(font.offset+0), cheight) == 0)
 501             return (struct carattr){car, fgattr | (bgattr << 4), 0};
 502     }
 503 fail:
 504     return (struct carattr){0, 0, 0};
 505 }
 506
 507 // Draw/undraw a cursor on the framebuffer by xor'ing the cursor cell
 508 void
 509 gfx_set_swcursor(struct vgamode_s *vmode_g, int enable, struct cursorpos cp)
 510 {
 511     u16 cursor_type = get_cursor_shape();
 512     u8 start = cursor_type >> 8, end = cursor_type & 0xff;
 513     struct gfx_op op;
 514     init_gfx_op(&op, vmode_g);
 515     op.x = cp.x * 8;
 516     int cheight = GET_BDA(char_height);
 517     op.y = cp.y * cheight + start;
 518
 519     int i;
 520     for (i = start; i < cheight && i <= end; i++, op.y++) {
 521         op.op = GO_READ8;
 522         handle_gfx_op(&op);
 523         int j;
 524         for (j = 0; j < 8; j++)
 525             op.pixels[j] ^= 0x07;
 526         op.op = GO_WRITE8;
 527         handle_gfx_op(&op);
 528     }
 529 }
 530
 531 // Set the pixel at the given position.
 532 void
 533 vgafb_write_pixel(u8 color, u16 x, u16 y)
 534 {
 535     struct vgamode_s *vmode_g = get_current_mode();
 536     if (!vmode_g)
 537         return;
 538     vgafb_set_swcursor(0);
 539
 540     struct gfx_op op;
 541     init_gfx_op(&op, vmode_g);
 542     op.x = ALIGN_DOWN(x, 8);
 543     op.y = y;
 544     op.op = GO_READ8;
 545     handle_gfx_op(&op);
 546
 547     int usexor = color & 0x80 && GET_GLOBAL(vmode_g->depth) < 8;
 548     if (usexor)
 549         op.pixels[x & 0x07] ^= color & 0x7f;
 550     else
 551         op.pixels[x & 0x07] = color;
 552     op.op = GO_WRITE8;
 553     handle_gfx_op(&op);
 554 }
 555
 556 // Return the pixel at the given position.
 557 u8
 558 vgafb_read_pixel(u16 x, u16 y)
 559 {
 560     struct vgamode_s *vmode_g = get_current_mode();
 561     if (!vmode_g)
 562         return 0;
 563     vgafb_set_swcursor(0);
 564
 565     struct gfx_op op;
 566     init_gfx_op(&op, vmode_g);
 567     op.x = ALIGN_DOWN(x, 8);
 568     op.y = y;
 569     op.op = GO_READ8;
 570     handle_gfx_op(&op);
 571
 572     return op.pixels[x & 0x07];
 573 }
 574
 575
 576 /****************************************************************
 577  * Text ops
 578  ****************************************************************/
 579
 580 // Return the fb offset for the given character address when in text mode.
 581 void *
 582 text_address(struct cursorpos cp)
 583 {
 584     int stride = GET_BDA(video_cols) * 2;
 585     u32 pageoffset = GET_BDA(video_pagesize) * cp.page;
 586     return (void*)pageoffset + cp.y * stride + cp.x * 2;
 587 }
 588
 589 // Move characters on screen.
 590 void
 591 vgafb_move_chars(struct cursorpos dest
 592                  , struct cursorpos src, struct cursorpos movesize)
 593 {
 594     struct vgamode_s *vmode_g = get_current_mode();
 595     if (!vmode_g)
 596         return;
 597     vgafb_set_swcursor(0);
 598
 599     if (GET_GLOBAL(vmode_g->memmodel) != MM_TEXT) {
 600         gfx_move_chars(vmode_g, dest, src, movesize);
 601         return;
 602     }
 603
 604     int stride = GET_BDA(video_cols) * 2;
 605     memmove_stride(GET_GLOBAL(vmode_g->sstart)
 606                    , text_address(dest), text_address(src)
 607                    , movesize.x * 2, stride, movesize.y);
 608 }
 609
 610 // Clear area of screen.
 611 void
 612 vgafb_clear_chars(struct cursorpos dest
 613                   , struct carattr ca, struct cursorpos clearsize)
 614 {
 615     struct vgamode_s *vmode_g = get_current_mode();
 616     if (!vmode_g)
 617         return;
 618     vgafb_set_swcursor(0);
 619
 620     if (GET_GLOBAL(vmode_g->memmodel) != MM_TEXT) {
 621         gfx_clear_chars(vmode_g, dest, ca, clearsize);
 622         return;
 623     }
 624
 625     int stride = GET_BDA(video_cols) * 2;
 626     u16 attr = ((ca.use_attr ? ca.attr : 0x07) << 8) | ca.car;
 627     memset16_stride(GET_GLOBAL(vmode_g->sstart), text_address(dest), attr
 628                     , clearsize.x * 2, stride, clearsize.y);
 629 }
 630
 631 // Write a character to the screen.
 632 void
 633 vgafb_write_char(struct cursorpos cp, struct carattr ca)
 634 {
 635     struct vgamode_s *vmode_g = get_current_mode();
 636     if (!vmode_g)
 637         return;
 638     vgafb_set_swcursor(0);
 639
 640     if (GET_GLOBAL(vmode_g->memmodel) != MM_TEXT) {
 641         gfx_write_char(vmode_g, cp, ca);
 642         return;
 643     }
 644
 645     void *dest_far = text_address(cp);
 646     if (ca.use_attr) {
 647         u16 dummy = (ca.attr << 8) | ca.car;
 648         SET_FARVAR(GET_GLOBAL(vmode_g->sstart), *(u16*)dest_far, dummy);
 649     } else {
 650         SET_FARVAR(GET_GLOBAL(vmode_g->sstart), *(u8*)dest_far, ca.car);
 651     }
 652 }
 653
 654 // Return the character at the given position on the screen.
 655 struct carattr
 656 vgafb_read_char(struct cursorpos cp)
 657 {
 658     struct vgamode_s *vmode_g = get_current_mode();
 659     if (!vmode_g)
 660         return (struct carattr){0, 0, 0};
 661     vgafb_set_swcursor(0);
 662
 663     if (GET_GLOBAL(vmode_g->memmodel) != MM_TEXT)
 664         return gfx_read_char(vmode_g, cp);
 665
 666     u16 *dest_far = text_address(cp);
 667     u16 v = GET_FARVAR(GET_GLOBAL(vmode_g->sstart), *dest_far);
 668     return (struct carattr){v, v>>8, 0};
 669 }
 670
 671 // Draw/undraw a cursor on the screen
 672 void
 673 vgafb_set_swcursor(int enable)
 674 {
 675     if (!vga_emulate_text())
 676         return;
 677     u8 flags = GET_BDA_EXT(flags);
 678     if (!!(flags & BF_SWCURSOR) == enable)
 679         // Already in requested mode.
 680         return;
 681     struct vgamode_s *vmode_g = get_current_mode();
 682     if (!vmode_g)
 683         return;
 684     struct cursorpos cp = get_cursor_pos(0xff);
 685     if (cp.x >= GET_BDA(video_cols) || cp.y > GET_BDA(video_rows)
 686         || GET_BDA(cursor_type) >= 0x2000)
 687         // Cursor not visible
 688         return;
 689
 690     SET_BDA_EXT(flags, (flags & ~BF_SWCURSOR) | (enable ? BF_SWCURSOR : 0));
 691
 692     if (GET_GLOBAL(vmode_g->memmodel) != MM_TEXT) {
 693         gfx_set_swcursor(vmode_g, enable, cp);
 694         return;
 695     }
 696
 697     // In text mode, swap foreground and background attributes for cursor
 698     void *dest_far = text_address(cp) + 1;
 699     u8 attr = GET_FARVAR(GET_GLOBAL(vmode_g->sstart), *(u8*)dest_far);
 700     attr = (attr >> 4) | (attr << 4);
 701     SET_FARVAR(GET_GLOBAL(vmode_g->sstart), *(u8*)dest_far, attr);
 702 }