2 files changed, 11 insertions, 2 deletions

diff --git a/firmware/drivers/lcd.c b/firmware/drivers/lcd.c
index 056aca0769..d38b1daec5 100644
--- a/firmware/drivers/lcd.c
+++ b/firmware/drivers/lcd.c
@@ -910,9 +910,17 @@ void lcd_fillrect (int x, int y, int nx, int ny)
 /* Invert a rectangular area at (x, y), size (nx, ny) */
 void lcd_invertrect (int x, int y, int nx, int ny)
 {
-    int i;
+    int i,j;
+
+    if (x>LCD_WIDTH) return;
+    if (y>LCD_HEIGHT) return;
+
+    if (x+nx>LCD_WIDTH) nx=LCD_WIDTH-x;
+    if (y+ny>LCD_HEIGHT) ny=LCD_HEIGHT-y;
+
     for (i = 0; i < nx; i++)
-        lcd_bitmap (ones, x+i, y, 1, ny, false);
+        for (j = 0; j < ny; j++)
+            INVERT_PIXEL((x+i),(y+j));
 }
 
 void lcd_drawline( int x1, int y1, int x2, int y2 )
diff --git a/firmware/drivers/lcd.h b/firmware/drivers/lcd.h
index a2d392cbd5..21e1275284 100644
--- a/firmware/drivers/lcd.h
+++ b/firmware/drivers/lcd.h
@@ -79,6 +79,7 @@ extern void lcd_double_height (bool on);
 
 #define DRAW_PIXEL(x,y) lcd_framebuffer[x][y/8] |= (1<<(y&7))
 #define CLEAR_PIXEL(x,y) lcd_framebuffer[x][y/8] &= ~(1<<(y&7))
+#define INVERT_PIXEL(x,y) lcd_framebuffer[x][y/8] ^= (1<<(y&7))
 
 /*
  * Memory copy of display bitmap