2 files changed, 75 insertions, 11 deletions

diff --git a/payloads/libpayload/drivers/video/graphics.c b/payloads/libpayload/drivers/video/graphics.c
index b52bd99103..21f520c290 100644
--- a/payloads/libpayload/drivers/video/graphics.c
+++ b/payloads/libpayload/drivers/video/graphics.c
@@ -61,17 +61,53 @@ static const struct vector vzero = {
 	.y = 0,
 };
 
+struct color_transformation {
+	uint8_t base;
+	int16_t scale;
+};
+
+struct color_mapping {
+	struct color_transformation red;
+	struct color_transformation green;
+	struct color_transformation blue;
+	int enabled;
+};
+
+static struct color_mapping color_map;
+
+static inline void set_color_trans(struct color_transformation *trans,
+				   uint8_t bg_color, uint8_t fg_color)
+{
+	trans->base = bg_color;
+	trans->scale = fg_color - bg_color;
+}
+
+int set_color_map(const struct rgb_color *background,
+		  const struct rgb_color *foreground)
+{
+	if (background == NULL || foreground == NULL)
+		return CBGFX_ERROR_INVALID_PARAMETER;
+
+	set_color_trans(&color_map.red, background->red, foreground->red);
+	set_color_trans(&color_map.green, background->green,
+			foreground->green);
+	set_color_trans(&color_map.blue, background->blue, foreground->blue);
+	color_map.enabled = 1;
+
+	return CBGFX_SUCCESS;
+}
+
+void clear_color_map(void)
+{
+	color_map.enabled = 0;
+}
+
 struct blend_value {
 	uint8_t alpha;
 	struct rgb_color rgb;
 };
 
-static struct blend_value blend = {
-	.alpha = 0,
-	.rgb.red = 0,
-	.rgb.green = 0,
-	.rgb.blue = 0,
-};
+static struct blend_value blend;
 
 int set_blend(const struct rgb_color *rgb, uint8_t alpha)
 {
@@ -185,6 +221,15 @@ static int within_box(const struct vector *v, const struct rect *bound)
 		return -1;
 }
 
+/* Helper function that applies color_map to the color. */
+static inline uint8_t apply_map(uint8_t color,
+				const struct color_transformation *trans)
+{
+	if (!color_map.enabled)
+		return color;
+	return trans->base + trans->scale * color / UINT8_MAX;
+}
+
 /*
  * Helper function that applies color and opacity from blend struct
  * into the color.
@@ -203,13 +248,16 @@ static inline uint32_t calculate_color(const struct rgb_color *rgb,
 {
 	uint32_t color = 0;
 
-	color |= (apply_blend(rgb->red, blend.rgb.red)
+	color |= (apply_blend(apply_map(rgb->red, &color_map.red),
+			      blend.rgb.red)
 		  >> (8 - fbinfo->red_mask_size))
 		 << fbinfo->red_mask_pos;
-	color |= (apply_blend(rgb->green, blend.rgb.green)
+	color |= (apply_blend(apply_map(rgb->green, &color_map.green),
+			      blend.rgb.green)
 		  >> (8 - fbinfo->green_mask_size))
 		 << fbinfo->green_mask_pos;
-	color |= (apply_blend(rgb->blue, blend.rgb.blue)
+	color |= (apply_blend(apply_map(rgb->blue, &color_map.blue),
+			      blend.rgb.blue)
 		  >> (8 - fbinfo->blue_mask_size))
 		 << fbinfo->blue_mask_pos;
 	if (invert)
diff --git a/payloads/libpayload/include/cbgfx.h b/payloads/libpayload/include/cbgfx.h
index f2883b0c43..85b61a7b10 100644
--- a/payloads/libpayload/include/cbgfx.h
+++ b/payloads/libpayload/include/cbgfx.h
@@ -228,6 +228,24 @@ int draw_bitmap_direct(const void *bitmap, size_t size,
 int get_bitmap_dimension(const void *bitmap, size_t sz, struct scale *dim_rel);
 
 /**
+ * Setup color mappings of background and foreground colors. Black and white
+ * pixels will be mapped to the background and foreground colors, respectively.
+ * Call clear_color_map() to disabled color mapping.
+ *
+ * @param[in] background	Background color.
+ * @param[in] foreground	Foreground color.
+ *
+ * @return CBGFX_* error codes
+ */
+int set_color_map(const struct rgb_color *background,
+		  const struct rgb_color *foreground);
+
+/**
+ * Clear color mappings.
+ */
+void clear_color_map(void);
+
+/**
  * Setup alpha and rgb values for alpha blending.  When alpha is != 0,
  * this enables a translucent layer of color (defined by rgb) to be
  * blended at a given translucency (alpha) to all things drawn.  Call
@@ -244,8 +262,6 @@ int set_blend(const struct rgb_color *rgb, uint8_t alpha);
 
 /**
  * Clear alpha and rgb values, thus disabling any alpha blending.
- *
- * @return CBGFX_* error codes
  */
 void clear_blend(void);