fxsdk/fxlink/protocol.c

142 lines
3.9 KiB
C
Raw Normal View History

#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;
}