Patchwork sh: sm501: Add hardware cursor feature

login
register
mail settings
Submitter Shin-ichiro KAWASAKI
Date Jan. 1, 2010, 6:59 a.m.
Message ID <4B3D9D5B.6080501@juno.dti.ne.jp>
Download mbox | patch
Permalink /patch/41984/
State New
Headers show

Comments

Shin-ichiro KAWASAKI - Jan. 1, 2010, 6:59 a.m.
This patch adds hardware cursor feature to SM501 graphics chip emulation,
to make the graphic console more useful for QEMU SH4 users.


Signed-off-by: Shin-ichiro KAWASAKI <kawasaki@juno.dti.ne.jp>

 hw/sm501.c          |  154 +++++++++++++++++++++++++++++++++++++++++++++++----
 hw/sm501_template.h |   42 ++++++++++++++
 2 files changed, 185 insertions(+), 11 deletions(-)
Aurelien Jarno - Jan. 14, 2010, 3:18 p.m.
On Fri, Jan 01, 2010 at 03:59:39PM +0900, Shin-ichiro KAWASAKI wrote:
> This patch adds hardware cursor feature to SM501 graphics chip emulation,
> to make the graphic console more useful for QEMU SH4 users.
> 
> 
> Signed-off-by: Shin-ichiro KAWASAKI <kawasaki@juno.dti.ne.jp>

Thanks, applied.

>  hw/sm501.c          |  154 +++++++++++++++++++++++++++++++++++++++++++++++----
>  hw/sm501_template.h |   42 ++++++++++++++
>  2 files changed, 185 insertions(+), 11 deletions(-)
> 
> diff --git a/hw/sm501.c b/hw/sm501.c
> index 612a8e5..cd1f595 100644
> --- a/hw/sm501.c
> +++ b/hw/sm501.c
> @@ -434,6 +434,8 @@
>  
>  /* end of register definitions */
>  
> +#define SM501_HWC_WIDTH                       (64)
> +#define SM501_HWC_HEIGHT                      (64)
>  
>  /* SM501 local memory size taken from "linux/drivers/mfd/sm501.c" */
>  static const uint32_t sm501_mem_local_size[] = {
> @@ -526,6 +528,95 @@ static uint32_t get_local_mem_size_index(uint32_t size)
>      return index;
>  }
>  
> +/**
> + * Check the availability of hardware cursor.
> + * @param crt  0 for PANEL, 1 for CRT.
> + */
> +static inline int is_hwc_enabled(SM501State *state, int crt)
> +{
> +    uint32_t addr = crt ? state->dc_crt_hwc_addr : state->dc_panel_hwc_addr;
> +    return addr & 0x80000000;
> +}
> +
> +/**
> + * Get the address which holds cursor pattern data.
> + * @param crt  0 for PANEL, 1 for CRT.
> + */
> +static inline uint32_t get_hwc_address(SM501State *state, int crt)
> +{
> +    uint32_t addr = crt ? state->dc_crt_hwc_addr : state->dc_panel_hwc_addr;
> +    return (addr & 0x03FFFFF0)/* >> 4*/;
> +}
> +
> +/**
> + * Get the cursor position in y coordinate.
> + * @param crt  0 for PANEL, 1 for CRT.
> + */
> +static inline uint32_t get_hwc_y(SM501State *state, int crt)
> +{
> +    uint32_t location = crt ? state->dc_crt_hwc_location
> +                            : state->dc_panel_hwc_location;
> +    return (location & 0x07FF0000) >> 16;
> +}
> +
> +/**
> + * Get the cursor position in x coordinate.
> + * @param crt  0 for PANEL, 1 for CRT.
> + */
> +static inline uint32_t get_hwc_x(SM501State *state, int crt)
> +{
> +    uint32_t location = crt ? state->dc_crt_hwc_location
> +                            : state->dc_panel_hwc_location;
> +    return location & 0x000007FF;
> +}
> +
> +/**
> + * Get the cursor position in x coordinate.
> + * @param crt  0 for PANEL, 1 for CRT.
> + * @param index  0, 1, 2 or 3 which specifies color of corsor dot.
> + */
> +static inline uint16_t get_hwc_color(SM501State *state, int crt, int index)
> +{
> +    uint16_t color_reg = 0;
> +    uint16_t color_565 = 0;
> +
> +    if (index == 0) {
> +        return 0;
> +    }
> +
> +    switch (index) {
> +    case 1:
> +    case 2:
> +        color_reg = crt ? state->dc_crt_hwc_color_1_2
> +                        : state->dc_panel_hwc_color_1_2;
> +        break;
> +    case 3:
> +        color_reg = crt ? state->dc_crt_hwc_color_3
> +                        : state->dc_panel_hwc_color_3;
> +        break;
> +    default:
> +        printf("invalid hw cursor color.\n");
> +        assert(0);
> +    }
> +
> +    switch (index) {
> +    case 1:
> +    case 3:
> +        color_565 = (uint16_t)(color_reg & 0xFFFF);
> +        break;
> +    case 2:
> +        color_565 = (uint16_t)((color_reg >> 16) & 0xFFFF);
> +        break;
> +    }
> +    return color_565;
> +}
> +
> +static int within_hwc_y_range(SM501State *state, int y, int crt)
> +{
> +    int hwc_y = get_hwc_y(state, crt);
> +    return (hwc_y <= y && y < hwc_y + SM501_HWC_HEIGHT);
> +}
> +
>  static uint32_t sm501_system_config_read(void *opaque, target_phys_addr_t addr)
>  {
>      SM501State * s = (SM501State *)opaque;
> @@ -736,13 +827,13 @@ static uint32_t sm501_disp_ctrl_read(void *opaque, target_phys_addr_t addr)
>  	ret = s->dc_crt_hwc_addr;
>  	break;
>      case SM501_DC_CRT_HWC_LOC:
> -	ret = s->dc_crt_hwc_addr;
> +	ret = s->dc_crt_hwc_location;
>  	break;
>      case SM501_DC_CRT_HWC_COLOR_1_2:
> -	ret = s->dc_crt_hwc_addr;
> +	ret = s->dc_crt_hwc_color_1_2;
>  	break;
>      case SM501_DC_CRT_HWC_COLOR_3:
> -	ret = s->dc_crt_hwc_addr;
> +	ret = s->dc_crt_hwc_color_3;
>  	break;
>  
>      case SM501_DC_PANEL_PALETTE ... SM501_DC_PANEL_PALETTE + 0x400*3 - 4:
> @@ -809,13 +900,13 @@ static void sm501_disp_ctrl_write(void *opaque,
>  	s->dc_panel_hwc_addr = value & 0x8FFFFFF0;
>  	break;
>      case SM501_DC_PANEL_HWC_LOC:
> -	s->dc_panel_hwc_addr = value & 0x0FFF0FFF;
> +	s->dc_panel_hwc_location = value & 0x0FFF0FFF;
>  	break;
>      case SM501_DC_PANEL_HWC_COLOR_1_2:
> -	s->dc_panel_hwc_addr = value;
> +	s->dc_panel_hwc_color_1_2 = value;
>  	break;
>      case SM501_DC_PANEL_HWC_COLOR_3:
> -	s->dc_panel_hwc_addr = value & 0x0000FFFF;
> +	s->dc_panel_hwc_color_3 = value & 0x0000FFFF;
>  	break;
>  
>      case SM501_DC_CRT_CONTROL:
> @@ -844,13 +935,13 @@ static void sm501_disp_ctrl_write(void *opaque,
>  	s->dc_crt_hwc_addr = value & 0x8FFFFFF0;
>  	break;
>      case SM501_DC_CRT_HWC_LOC:
> -	s->dc_crt_hwc_addr = value & 0x0FFF0FFF;
> +	s->dc_crt_hwc_location = value & 0x0FFF0FFF;
>  	break;
>      case SM501_DC_CRT_HWC_COLOR_1_2:
> -	s->dc_crt_hwc_addr = value;
> +	s->dc_crt_hwc_color_1_2 = value;
>  	break;
>      case SM501_DC_CRT_HWC_COLOR_3:
> -	s->dc_crt_hwc_addr = value & 0x0000FFFF;
> +	s->dc_crt_hwc_color_3 = value & 0x0000FFFF;
>  	break;
>  
>      case SM501_DC_PANEL_PALETTE ... SM501_DC_PANEL_PALETTE + 0x400*3 - 4:
> @@ -883,6 +974,9 @@ static CPUWriteMemoryFunc * const sm501_disp_ctrl_writefn[] = {
>  typedef void draw_line_func(uint8_t *d, const uint8_t *s,
>  			    int width, const uint32_t *pal);
>  
> +typedef void draw_hwc_line_func(SM501State * s, int crt, uint8_t * palette,
> +                                int c_y, uint8_t *d, int width);
> +
>  #define DEPTH 8
>  #include "sm501_template.h"
>  
> @@ -937,6 +1031,16 @@ static draw_line_func * draw_line32_funcs[] = {
>      draw_line32_16bgr,
>  };
>  
> +static draw_hwc_line_func * draw_hwc_line_funcs[] = {
> +    draw_hwc_line_8,
> +    draw_hwc_line_15,
> +    draw_hwc_line_16,
> +    draw_hwc_line_32,
> +    draw_hwc_line_32bgr,
> +    draw_hwc_line_15bgr,
> +    draw_hwc_line_16bgr,
> +};
> +
>  static inline int get_depth_index(DisplayState *s)
>  {
>      switch(ds_get_bits_per_pixel(s)) {
> @@ -966,8 +1070,10 @@ static void sm501_draw_crt(SM501State * s)
>      int dst_bpp = ds_get_bytes_per_pixel(s->ds) + (ds_get_bits_per_pixel(s->ds) % 8 ? 1 : 0);
>      uint32_t * palette = (uint32_t *)&s->dc_palette[SM501_DC_CRT_PALETTE
>  						    - SM501_DC_PANEL_PALETTE];
> +    uint8_t hwc_palette[3 * 3];
>      int ds_depth_index = get_depth_index(s->ds);
>      draw_line_func * draw_line = NULL;
> +    draw_hwc_line_func * draw_hwc_line = NULL;
>      int full_update = 0;
>      int y_start = -1;
>      int page_min = 0x7fffffff;
> @@ -995,6 +1101,22 @@ static void sm501_draw_crt(SM501State * s)
>  	break;
>      }
>  
> +    /* set up to draw hardware cursor */
> +    if (is_hwc_enabled(s, 1)) {
> +        int i;
> +
> +        /* get cursor palette */
> +        for (i = 0; i < 3; i++) {
> +            uint16_t rgb565 = get_hwc_color(s, 1, i + 1);
> +            hwc_palette[i * 3 + 0] = (rgb565 & 0xf800) >> 8; /* red */
> +            hwc_palette[i * 3 + 1] = (rgb565 & 0x07e0) >> 3; /* green */
> +            hwc_palette[i * 3 + 2] = (rgb565 & 0x001f) << 3; /* blue */
> +        }
> +
> +        /* choose cursor draw line function */
> +        draw_hwc_line = draw_hwc_line_funcs[ds_depth_index];
> +    }
> +
>      /* adjust console size */
>      if (s->last_width != width || s->last_height != height) {
>  	qemu_console_resize(s->ds, width, height);
> @@ -1005,7 +1127,8 @@ static void sm501_draw_crt(SM501State * s)
>  
>      /* draw each line according to conditions */
>      for (y = 0; y < height; y++) {
> -	int update = full_update;
> +	int update_hwc = draw_hwc_line ? within_hwc_y_range(s, y, 1) : 0;
> +	int update = full_update || update_hwc;
>  	ram_addr_t page0 = offset & TARGET_PAGE_MASK;
>  	ram_addr_t page1 = (offset + width * src_bpp - 1) & TARGET_PAGE_MASK;
>  	ram_addr_t page;
> @@ -1017,7 +1140,16 @@ static void sm501_draw_crt(SM501State * s)
>  
>  	/* draw line and change status */
>  	if (update) {
> -	    draw_line(&(ds_get_data(s->ds)[y * width * dst_bpp]), src, width, palette);
> +            uint8_t * d = &(ds_get_data(s->ds)[y * width * dst_bpp]);
> +
> +            /* draw graphics layer */
> +            draw_line(d, src, width, palette);
> +
> +            /* draw haredware cursor */
> +            if (update_hwc) {
> +                draw_hwc_line(s, 1, hwc_palette, y - get_hwc_y(s, 1), d, width);
> +            }
> +
>  	    if (y_start < 0)
>  		y_start = y;
>  	    if (page0 < page_min)
> diff --git a/hw/sm501_template.h b/hw/sm501_template.h
> index 1679df7..d1ceef9 100644
> --- a/hw/sm501_template.h
> +++ b/hw/sm501_template.h
> @@ -96,6 +96,48 @@ static void glue(draw_line32_, PIXEL_NAME)(
>      } while (-- width != 0);
>  }
>  
> +/**
> + * Draw hardware cursor image on the given line.
> + */
> +static void glue(draw_hwc_line_, PIXEL_NAME)(SM501State * s, int crt,
> +                         uint8_t * palette, int c_y, uint8_t *d, int width)
> +{
> +    int x, i;
> +    uint8_t bitset = 0;
> +
> +    /* get hardware cursor pattern */
> +    uint32_t cursor_addr = get_hwc_address(s, crt);
> +    assert(0 <= c_y && c_y < SM501_HWC_HEIGHT);
> +    cursor_addr += 64 * c_y / 4;  /* 4 pixels per byte */
> +    cursor_addr += s->base;
> +
> +    /* get cursor position */
> +    x = get_hwc_x(s, crt);
> +    d += x * BPP;
> +
> +    for (i = 0; i < SM501_HWC_WIDTH && x + i < width; i++) {
> +        uint8_t v;
> +
> +        /* get pixel value */
> +        if (i % 4 == 0) {
> +            cpu_physical_memory_rw(cursor_addr, &bitset, 1, 0);
> +            cursor_addr++;
> +        }
> +        v = bitset & 3;
> +        bitset >>= 2;
> +
> +        /* write pixel */
> +        if (v) {
> +            v--;
> +            uint8_t r = palette[v * 3 + 0];
> +            uint8_t g = palette[v * 3 + 1];
> +            uint8_t b = palette[v * 3 + 2];
> +            ((PIXEL_TYPE *) d)[0] = glue(rgb_to_pixel, PIXEL_NAME)(r, g, b);
> +        }
> +        d += BPP;
> +    }
> +}
> +
>  #undef DEPTH
>  #undef BPP
>  #undef PIXEL_TYPE
> 
> 
> 
>

Patch

diff --git a/hw/sm501.c b/hw/sm501.c
index 612a8e5..cd1f595 100644
--- a/hw/sm501.c
+++ b/hw/sm501.c
@@ -434,6 +434,8 @@ 
 
 /* end of register definitions */
 
+#define SM501_HWC_WIDTH                       (64)
+#define SM501_HWC_HEIGHT                      (64)
 
 /* SM501 local memory size taken from "linux/drivers/mfd/sm501.c" */
 static const uint32_t sm501_mem_local_size[] = {
@@ -526,6 +528,95 @@  static uint32_t get_local_mem_size_index(uint32_t size)
     return index;
 }
 
+/**
+ * Check the availability of hardware cursor.
+ * @param crt  0 for PANEL, 1 for CRT.
+ */
+static inline int is_hwc_enabled(SM501State *state, int crt)
+{
+    uint32_t addr = crt ? state->dc_crt_hwc_addr : state->dc_panel_hwc_addr;
+    return addr & 0x80000000;
+}
+
+/**
+ * Get the address which holds cursor pattern data.
+ * @param crt  0 for PANEL, 1 for CRT.
+ */
+static inline uint32_t get_hwc_address(SM501State *state, int crt)
+{
+    uint32_t addr = crt ? state->dc_crt_hwc_addr : state->dc_panel_hwc_addr;
+    return (addr & 0x03FFFFF0)/* >> 4*/;
+}
+
+/**
+ * Get the cursor position in y coordinate.
+ * @param crt  0 for PANEL, 1 for CRT.
+ */
+static inline uint32_t get_hwc_y(SM501State *state, int crt)
+{
+    uint32_t location = crt ? state->dc_crt_hwc_location
+                            : state->dc_panel_hwc_location;
+    return (location & 0x07FF0000) >> 16;
+}
+
+/**
+ * Get the cursor position in x coordinate.
+ * @param crt  0 for PANEL, 1 for CRT.
+ */
+static inline uint32_t get_hwc_x(SM501State *state, int crt)
+{
+    uint32_t location = crt ? state->dc_crt_hwc_location
+                            : state->dc_panel_hwc_location;
+    return location & 0x000007FF;
+}
+
+/**
+ * Get the cursor position in x coordinate.
+ * @param crt  0 for PANEL, 1 for CRT.
+ * @param index  0, 1, 2 or 3 which specifies color of corsor dot.
+ */
+static inline uint16_t get_hwc_color(SM501State *state, int crt, int index)
+{
+    uint16_t color_reg = 0;
+    uint16_t color_565 = 0;
+
+    if (index == 0) {
+        return 0;
+    }
+
+    switch (index) {
+    case 1:
+    case 2:
+        color_reg = crt ? state->dc_crt_hwc_color_1_2
+                        : state->dc_panel_hwc_color_1_2;
+        break;
+    case 3:
+        color_reg = crt ? state->dc_crt_hwc_color_3
+                        : state->dc_panel_hwc_color_3;
+        break;
+    default:
+        printf("invalid hw cursor color.\n");
+        assert(0);
+    }
+
+    switch (index) {
+    case 1:
+    case 3:
+        color_565 = (uint16_t)(color_reg & 0xFFFF);
+        break;
+    case 2:
+        color_565 = (uint16_t)((color_reg >> 16) & 0xFFFF);
+        break;
+    }
+    return color_565;
+}
+
+static int within_hwc_y_range(SM501State *state, int y, int crt)
+{
+    int hwc_y = get_hwc_y(state, crt);
+    return (hwc_y <= y && y < hwc_y + SM501_HWC_HEIGHT);
+}
+
 static uint32_t sm501_system_config_read(void *opaque, target_phys_addr_t addr)
 {
     SM501State * s = (SM501State *)opaque;
@@ -736,13 +827,13 @@  static uint32_t sm501_disp_ctrl_read(void *opaque, target_phys_addr_t addr)
 	ret = s->dc_crt_hwc_addr;
 	break;
     case SM501_DC_CRT_HWC_LOC:
-	ret = s->dc_crt_hwc_addr;
+	ret = s->dc_crt_hwc_location;
 	break;
     case SM501_DC_CRT_HWC_COLOR_1_2:
-	ret = s->dc_crt_hwc_addr;
+	ret = s->dc_crt_hwc_color_1_2;
 	break;
     case SM501_DC_CRT_HWC_COLOR_3:
-	ret = s->dc_crt_hwc_addr;
+	ret = s->dc_crt_hwc_color_3;
 	break;
 
     case SM501_DC_PANEL_PALETTE ... SM501_DC_PANEL_PALETTE + 0x400*3 - 4:
@@ -809,13 +900,13 @@  static void sm501_disp_ctrl_write(void *opaque,
 	s->dc_panel_hwc_addr = value & 0x8FFFFFF0;
 	break;
     case SM501_DC_PANEL_HWC_LOC:
-	s->dc_panel_hwc_addr = value & 0x0FFF0FFF;
+	s->dc_panel_hwc_location = value & 0x0FFF0FFF;
 	break;
     case SM501_DC_PANEL_HWC_COLOR_1_2:
-	s->dc_panel_hwc_addr = value;
+	s->dc_panel_hwc_color_1_2 = value;
 	break;
     case SM501_DC_PANEL_HWC_COLOR_3:
-	s->dc_panel_hwc_addr = value & 0x0000FFFF;
+	s->dc_panel_hwc_color_3 = value & 0x0000FFFF;
 	break;
 
     case SM501_DC_CRT_CONTROL:
@@ -844,13 +935,13 @@  static void sm501_disp_ctrl_write(void *opaque,
 	s->dc_crt_hwc_addr = value & 0x8FFFFFF0;
 	break;
     case SM501_DC_CRT_HWC_LOC:
-	s->dc_crt_hwc_addr = value & 0x0FFF0FFF;
+	s->dc_crt_hwc_location = value & 0x0FFF0FFF;
 	break;
     case SM501_DC_CRT_HWC_COLOR_1_2:
-	s->dc_crt_hwc_addr = value;
+	s->dc_crt_hwc_color_1_2 = value;
 	break;
     case SM501_DC_CRT_HWC_COLOR_3:
-	s->dc_crt_hwc_addr = value & 0x0000FFFF;
+	s->dc_crt_hwc_color_3 = value & 0x0000FFFF;
 	break;
 
     case SM501_DC_PANEL_PALETTE ... SM501_DC_PANEL_PALETTE + 0x400*3 - 4:
@@ -883,6 +974,9 @@  static CPUWriteMemoryFunc * const sm501_disp_ctrl_writefn[] = {
 typedef void draw_line_func(uint8_t *d, const uint8_t *s,
 			    int width, const uint32_t *pal);
 
+typedef void draw_hwc_line_func(SM501State * s, int crt, uint8_t * palette,
+                                int c_y, uint8_t *d, int width);
+
 #define DEPTH 8
 #include "sm501_template.h"
 
@@ -937,6 +1031,16 @@  static draw_line_func * draw_line32_funcs[] = {
     draw_line32_16bgr,
 };
 
+static draw_hwc_line_func * draw_hwc_line_funcs[] = {
+    draw_hwc_line_8,
+    draw_hwc_line_15,
+    draw_hwc_line_16,
+    draw_hwc_line_32,
+    draw_hwc_line_32bgr,
+    draw_hwc_line_15bgr,
+    draw_hwc_line_16bgr,
+};
+
 static inline int get_depth_index(DisplayState *s)
 {
     switch(ds_get_bits_per_pixel(s)) {
@@ -966,8 +1070,10 @@  static void sm501_draw_crt(SM501State * s)
     int dst_bpp = ds_get_bytes_per_pixel(s->ds) + (ds_get_bits_per_pixel(s->ds) % 8 ? 1 : 0);
     uint32_t * palette = (uint32_t *)&s->dc_palette[SM501_DC_CRT_PALETTE
 						    - SM501_DC_PANEL_PALETTE];
+    uint8_t hwc_palette[3 * 3];
     int ds_depth_index = get_depth_index(s->ds);
     draw_line_func * draw_line = NULL;
+    draw_hwc_line_func * draw_hwc_line = NULL;
     int full_update = 0;
     int y_start = -1;
     int page_min = 0x7fffffff;
@@ -995,6 +1101,22 @@  static void sm501_draw_crt(SM501State * s)
 	break;
     }
 
+    /* set up to draw hardware cursor */
+    if (is_hwc_enabled(s, 1)) {
+        int i;
+
+        /* get cursor palette */
+        for (i = 0; i < 3; i++) {
+            uint16_t rgb565 = get_hwc_color(s, 1, i + 1);
+            hwc_palette[i * 3 + 0] = (rgb565 & 0xf800) >> 8; /* red */
+            hwc_palette[i * 3 + 1] = (rgb565 & 0x07e0) >> 3; /* green */
+            hwc_palette[i * 3 + 2] = (rgb565 & 0x001f) << 3; /* blue */
+        }
+
+        /* choose cursor draw line function */
+        draw_hwc_line = draw_hwc_line_funcs[ds_depth_index];
+    }
+
     /* adjust console size */
     if (s->last_width != width || s->last_height != height) {
 	qemu_console_resize(s->ds, width, height);
@@ -1005,7 +1127,8 @@  static void sm501_draw_crt(SM501State * s)
 
     /* draw each line according to conditions */
     for (y = 0; y < height; y++) {
-	int update = full_update;
+	int update_hwc = draw_hwc_line ? within_hwc_y_range(s, y, 1) : 0;
+	int update = full_update || update_hwc;
 	ram_addr_t page0 = offset & TARGET_PAGE_MASK;
 	ram_addr_t page1 = (offset + width * src_bpp - 1) & TARGET_PAGE_MASK;
 	ram_addr_t page;
@@ -1017,7 +1140,16 @@  static void sm501_draw_crt(SM501State * s)
 
 	/* draw line and change status */
 	if (update) {
-	    draw_line(&(ds_get_data(s->ds)[y * width * dst_bpp]), src, width, palette);
+            uint8_t * d = &(ds_get_data(s->ds)[y * width * dst_bpp]);
+
+            /* draw graphics layer */
+            draw_line(d, src, width, palette);
+
+            /* draw haredware cursor */
+            if (update_hwc) {
+                draw_hwc_line(s, 1, hwc_palette, y - get_hwc_y(s, 1), d, width);
+            }
+
 	    if (y_start < 0)
 		y_start = y;
 	    if (page0 < page_min)
diff --git a/hw/sm501_template.h b/hw/sm501_template.h
index 1679df7..d1ceef9 100644
--- a/hw/sm501_template.h
+++ b/hw/sm501_template.h
@@ -96,6 +96,48 @@  static void glue(draw_line32_, PIXEL_NAME)(
     } while (-- width != 0);
 }
 
+/**
+ * Draw hardware cursor image on the given line.
+ */
+static void glue(draw_hwc_line_, PIXEL_NAME)(SM501State * s, int crt,
+                         uint8_t * palette, int c_y, uint8_t *d, int width)
+{
+    int x, i;
+    uint8_t bitset = 0;
+
+    /* get hardware cursor pattern */
+    uint32_t cursor_addr = get_hwc_address(s, crt);
+    assert(0 <= c_y && c_y < SM501_HWC_HEIGHT);
+    cursor_addr += 64 * c_y / 4;  /* 4 pixels per byte */
+    cursor_addr += s->base;
+
+    /* get cursor position */
+    x = get_hwc_x(s, crt);
+    d += x * BPP;
+
+    for (i = 0; i < SM501_HWC_WIDTH && x + i < width; i++) {
+        uint8_t v;
+
+        /* get pixel value */
+        if (i % 4 == 0) {
+            cpu_physical_memory_rw(cursor_addr, &bitset, 1, 0);
+            cursor_addr++;
+        }
+        v = bitset & 3;
+        bitset >>= 2;
+
+        /* write pixel */
+        if (v) {
+            v--;
+            uint8_t r = palette[v * 3 + 0];
+            uint8_t g = palette[v * 3 + 1];
+            uint8_t b = palette[v * 3 + 2];
+            ((PIXEL_TYPE *) d)[0] = glue(rgb_to_pixel, PIXEL_NAME)(r, g, b);
+        }
+        d += BPP;
+    }
+}
+
 #undef DEPTH
 #undef BPP
 #undef PIXEL_TYPE