142 lines
3.9 KiB
C
142 lines
3.9 KiB
C
#include "protocol.h"
|
|
#include "util.h"
|
|
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <endian.h>
|
|
|
|
//---
|
|
// Image format
|
|
//---
|
|
|
|
static int img_bytes_per_row(int format, int width)
|
|
{
|
|
if(format == USB_FXLINK_IMAGE_RGB565)
|
|
return 2 * width;
|
|
if(format == USB_FXLINK_IMAGE_MONO)
|
|
return (width + 7) >> 3;
|
|
if(format == USB_FXLINK_IMAGE_GRAY)
|
|
return 2 * ((width + 7) >> 3);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void decode_rgb565(void *pixels, int width, int height, int size,
|
|
uint8_t **row_pointers)
|
|
{
|
|
int bpr = img_bytes_per_row(USB_FXLINK_IMAGE_RGB565, width);
|
|
|
|
for(int y = 0; y < height; y++) {
|
|
void *row = pixels + y * bpr;
|
|
|
|
for(int x = 0; x < width; x++) {
|
|
/* Don't read past the read buffer if the image is incomplete */
|
|
void *input = row + 2 * x;
|
|
uint16_t color = 0;
|
|
if(input - pixels + 2 <= size) color = *(uint16_t *)input;
|
|
|
|
color = be16toh(color);
|
|
|
|
row_pointers[y][3*x+0] = (color >> 11) << 3;
|
|
row_pointers[y][3*x+1] = ((color >> 5) & 0x3f) << 2;
|
|
row_pointers[y][3*x+2] = (color & 0x1f) << 3;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void decode_mono(void *pixels, int width, int height, int size,
|
|
uint8_t **row_pointers)
|
|
{
|
|
int bpr = img_bytes_per_row(USB_FXLINK_IMAGE_MONO, width);
|
|
|
|
for(int y = 0; y < height; y++) {
|
|
void *row = pixels + y * bpr;
|
|
|
|
for(int x = 0; x < width; x++) {
|
|
/* Don't read past the read buffer if the image is incomplete */
|
|
void *input = row + (x >> 3);
|
|
int byte = 0;
|
|
if(input - pixels + 1 <= size) byte = *(uint8_t *)input;
|
|
int color = (byte & (0x80 >> (x & 7))) ? 0 : 255;
|
|
|
|
row_pointers[y][3*x+0] = color;
|
|
row_pointers[y][3*x+1] = color;
|
|
row_pointers[y][3*x+2] = color;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void decode_gray(void *pixels, int width, int height, int size,
|
|
uint8_t **row_pointers)
|
|
{
|
|
int bpr = img_bytes_per_row(USB_FXLINK_IMAGE_MONO, width);
|
|
|
|
for(int k = 0; k < 2 * height; k++) {
|
|
void *row = pixels + k * bpr;
|
|
int y = k % height;
|
|
|
|
for(int x = 0; x < width; x++) {
|
|
/* Don't read past the read buffer if the image is incomplete */
|
|
void *input = row + (x >> 3);
|
|
int byte = 0;
|
|
if(input - pixels + 1 <= size) byte = *(uint8_t *)input;
|
|
|
|
int color = (byte & (0x80 >> (x & 7)));
|
|
/* Everything is inverted */
|
|
if(!color) color = (k >= height) ? 0xaa : 0x55;
|
|
else color = 0x00;
|
|
|
|
row_pointers[y][3*x+0] += color;
|
|
row_pointers[y][3*x+1] += color;
|
|
row_pointers[y][3*x+2] += color;
|
|
}
|
|
}
|
|
}
|
|
|
|
uint8_t **fxlink_protocol_decode_image(message_t *msg)
|
|
{
|
|
usb_fxlink_image_t *img = (void *)msg->output;
|
|
void *pixels = msg->output + sizeof *img;
|
|
|
|
/* Compute the amount of data for the specified image format and size */
|
|
int bytes_per_row = img_bytes_per_row(img->pixel_format, img->width);
|
|
int expected_size = img->height * bytes_per_row;
|
|
|
|
/* Check that the correct amount of data was sent */
|
|
int size = msg->size_read - sizeof *img;
|
|
if(size < expected_size)
|
|
printf("warning: got %d bytes but needed %d, image will be "
|
|
"incomplete\n", size, expected_size);
|
|
if(size > expected_size)
|
|
printf("warning: got %d bytes but needed %d for image, dropping extra"
|
|
"\n", size, expected_size);
|
|
|
|
/* Allocate row pointers */
|
|
uint8_t **row_pointers = malloc(img->height*sizeof *row_pointers);
|
|
if(!row_pointers) {
|
|
err("failed to write allocate memory to decode image");
|
|
return NULL;
|
|
}
|
|
|
|
for(size_t y = 0; y < img->height; y++) {
|
|
row_pointers[y] = calloc(img->width, 3);
|
|
if(!row_pointers[y]) {
|
|
err("failed to write allocate memory to decode image");
|
|
for(size_t i = 0 ; i < y; i++) free(row_pointers[i]);
|
|
free(row_pointers);
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
/* Decode image */
|
|
if(img->pixel_format == USB_FXLINK_IMAGE_RGB565)
|
|
decode_rgb565(pixels, img->width, img->height, size, row_pointers);
|
|
if(img->pixel_format == USB_FXLINK_IMAGE_MONO)
|
|
decode_mono(pixels, img->width, img->height, size, row_pointers);
|
|
if(img->pixel_format == USB_FXLINK_IMAGE_GRAY)
|
|
decode_gray(pixels, img->width, img->height, size, row_pointers);
|
|
|
|
return row_pointers;
|
|
}
|