Add CMD_BLIT and CMD_FILL

src/common.h

 #define CMD_PIXEL 0
+#define CMD_BLIT 1
+#define CMD_FILL 2
 
 struct pixel {
 	int x, y;
 	uint8_t bright;
 } __attribute__((packed));
 
+struct blit {
+	int x, y;
+	int width, height;
+	uint8_t data[];
+} __attribute__((packed));
+
+struct fill {
+	int x, y;
+	int width, height;
+	uint8_t bright;
+} __attribute__((packed));
+
 struct packet {
 	uint8_t cmd;
 	union {
 		struct pixel pixel;
+		struct blit blit;
+		struct fill fill;
 	};
 } __attribute__((packed));

src/mensactrl.c

@@ -47,15 +47,24 @@ struct mensa_fb {
 	ssize_t size;
 };
 
-static void blit_area(struct mensa_fb *mensafb, const int col, const int row,
+int verbose = 0;
+
+static int is_inbounds(struct mensa_fb *mensafb, int x, int y, int width, int height)
+{
+    if (y < 0 || height < 0 || y + height > mensafb->y_res ||
+		    x < 0 || width < 0 || x + width > mensafb->x_res)
+	    return 0;
+    return 1;
+}
+
+static int blit_area(struct mensa_fb *mensafb, const int col, const int row,
 		const int width, const int height)
 {
     int r, c;
     int vmpos, vpos, hmpos, hpos, pos;
 
-    if (row < 0 || height < 0 || row + height > mensafb->y_res ||
-		    col < 0 || width < 0 || col + width > mensafb->x_res)
-	    return;
+    if (!is_inbounds(mensafb, col, row, width, height))
+	    return -1;
 
     for (r = row; r < row + height; r++) {
 	for (c = col; c < col + width; c++) {
@@ -88,16 +97,15 @@ static void blit_area(struct mensa_fb *mensafb, const int col, const int row,
 	    }
 	}
     }
+    return 0;
 }
 
 void setPixel(struct mensa_fb *mensafb, int col, int row, uint8_t bright)
 {
 	int idx = row * mensafb->x_res + col;
 
-	if (col < 0 || col >= mensafb->x_res)
-		return;
-	if (row < 0 || row >= mensafb->y_res)
-		return;
+	if (!is_inbounds(mensafb, col, row, 1, 1))
+	    return;
 
 	mensafb->inputfb[idx] = bright;
 }
@@ -140,15 +148,88 @@ static struct mensa_fb *setup_fb(const char *devname, int hmodules, int vmodules
 	return mensafb;
 }
 
-void handlePixel(struct mensa_fb *mensafb, struct pixel *pix) {
+int copyArea(struct mensa_fb *mensafb, int x, int y, int w, int h, uint8_t *data, size_t len)
+{
+	int row;
+	if (!is_inbounds(mensafb, x, y, w, h)) {
+		if (verbose)
+			printf("Geometry overflow: %ix%i+%i+%i\n", x, y, w, h);
+		return -1;
+	}
+
+	if (len != (size_t)w*h) {
+		if (verbose)
+			printf("Blit size mismatch: want %i, have %i\n", w*h, len);
+		return -1;
+	}
+
+	for (row = y; row < y + h; row++) {
+		int idx = row * mensafb->x_res + x;
+		memcpy(&mensafb->inputfb[idx], &data[row*w], w);
+	}
+	return 0;
+}
+
+int handlePixel(struct mensa_fb *mensafb, struct pixel *pix, size_t len) {
+	if (len != sizeof(struct pixel))
+		return -1;
+
+	if (!is_inbounds(mensafb, pix->x, pix->y, 1, 1)) {
+		return -1;
+	}
+
 	setPixel(mensafb, pix->x, pix->y, pix->bright);
-	blit_area(mensafb, pix->x, pix->y, 1, 1);
+	return blit_area(mensafb, pix->x, pix->y, 1, 1);
+}
+
+int handleBlit(struct mensa_fb *mensafb, struct blit *blit, size_t len)
+{
+	int rc;
+
+	if (len < sizeof(struct blit))
+		return -1;
+
+	if (!is_inbounds(mensafb, blit->x, blit->y, blit->width, blit->height)) {
+		return -1;
+	}
+
+	rc = copyArea(mensafb, blit->x, blit->y, blit->width, blit->height, blit->data, len - sizeof(struct blit));
+	if (rc < 0)
+		return rc;
+
+	return blit_area(mensafb, blit->x, blit->y, blit->width, blit->height);
+}
+
+int handleFill(struct mensa_fb *mensafb, struct fill *fill, size_t len) {
+	int row;
+
+	if (len != sizeof(struct fill))
+		return -1;
+
+	if (!is_inbounds(mensafb, fill->x, fill->y, fill->width, fill->height)) {
+		return -1;
+	}
+
+	for (row = fill->y; row < fill->y + fill->height; row++) {
+		int idx = row * mensafb->x_res + fill->x;
+		memset(&mensafb->inputfb[idx], fill->bright, fill->width);
+	}
+
+	return blit_area(mensafb, fill->x, fill->y, fill->width, fill->height);
 }
 
-void handleCommand(struct mensa_fb *mensafb, struct packet *p) {
+void handleCommand(struct mensa_fb *mensafb, struct packet *p, size_t len) {
+	int rc = 0;
 	switch(p->cmd) {
-		case CMD_PIXEL: handlePixel(mensafb, &p->pixel); break;
+		case CMD_PIXEL: rc = handlePixel(mensafb, &p->pixel, len - 1); break;
+		case CMD_BLIT: rc = handleBlit(mensafb, &p->blit, len - 1); break;
+		case CMD_FILL: rc = handleFill(mensafb, &p->fill, len - 1); break;
+	default:
+	       if (verbose)
+		       printf("Unknown command: 0x%02x\n", p->cmd);
 	}
+	if ((rc < 0) && (verbose))
+		printf("Error handling command 0x%02x\n", p->cmd);
 }
 
 int main(int argc, char *argv[]) {
@@ -173,7 +254,7 @@ int main(int argc, char *argv[]) {
 			zmq_msg_recv (&message, responder, 0);
 
 			if(zmq_msg_size(&message)) {
-				handleCommand(mensafb, (struct packet *)zmq_msg_data(&message));
+				handleCommand(mensafb, (struct packet *)zmq_msg_data(&message), zmq_msg_size(&message));
 			}
 
 			if (!zmq_msg_more(&message)) {