GPG Key ID:
1BBA026E13FC0495
8 changed files with 521 additions and 68 deletions
@ -0,0 +1,232 @@
|
||||
#include "config.h" |
||||
#include "fxlink.h" |
||||
#include "util.h" |
||||
#include "properties.h" |
||||
#include "filter.h" |
||||
#include "protocol.h" |
||||
#include "usb.h" |
||||
#include "png.h" |
||||
|
||||
#include <libusb.h> |
||||
#include <string.h> |
||||
#include <stdlib.h> |
||||
#include <stdio.h> |
||||
#include <unistd.h> |
||||
#include <time.h> |
||||
#include <png.h> |
||||
|
||||
static char *output_file(char const *path,char const *type,char const *suffix) |
||||
{ |
||||
char *filename = NULL; |
||||
int counter = 1; |
||||
|
||||
time_t time_raw; |
||||
struct tm time_bd; |
||||
time(&time_raw); |
||||
localtime_r(&time_raw, &time_bd); |
||||
|
||||
while(1) { |
||||
asprintf(&filename, "%s/fxlink-%.16s-%04d.%02d.%02d-%02dh%02d-%d.%s", |
||||
path, type, time_bd.tm_year + 1900, time_bd.tm_mon, |
||||
time_bd.tm_mday, time_bd.tm_hour, time_bd.tm_min, counter, suffix); |
||||
if(!filename) continue; |
||||
|
||||
/* Try to find a name for a file that doesn't exist */ |
||||
if(access(filename, F_OK) == -1) break; |
||||
|
||||
free(filename); |
||||
counter++; |
||||
} |
||||
|
||||
return filename; |
||||
} |
||||
|
||||
static bool message_new(message_t *msg, usb_fxlink_header_t const *h) |
||||
{ |
||||
int version_major = (h->version >> 8) & 0xff; |
||||
int version_minor = (h->version) & 0xff; |
||||
|
||||
fprintf(stderr, "New message (v%d.%d): application '%.16s', type '%.16s', " |
||||
"size %d bytes\n", version_major, version_minor, h->application, |
||||
h->type, h->size); |
||||
|
||||
msg->output = malloc(h->size); |
||||
if(!msg->output) { |
||||
err("cannot allocate memory for message of %d bytes", h->size); |
||||
return false; |
||||
} |
||||
|
||||
msg->header = *h; |
||||
msg->size_read = 0; |
||||
msg->valid = true; |
||||
return true; |
||||
} |
||||
|
||||
static void message_finish(message_t *msg) |
||||
{ |
||||
char const *path = "."; |
||||
|
||||
if(!strncmp(msg->header.application, "fxlink", 16)) { |
||||
if(!strncmp(msg->header.type, "image", 16)) { |
||||
usb_fxlink_image_t *img = (void *)msg->output; |
||||
char *filename = output_file(path, msg->header.type, "png"); |
||||
|
||||
uint8_t **row_pointers = fxlink_protocol_decode_image(msg); |
||||
fxlink_png_save(row_pointers, img->width, img->height, filename); |
||||
|
||||
printf("Saved image (%dx%d, format=%d) to '%s'\n", |
||||
img->width, img->height, img->pixel_format, filename); |
||||
free(row_pointers); |
||||
free(filename); |
||||
return; |
||||
} |
||||
|
||||
if(!strncmp(msg->header.type, "text", 16)) { |
||||
printf("------------------\n"); |
||||
fwrite(msg->output, 1, msg->header.size, stdout); |
||||
if(msg->output[msg->header.size - 1] != '\n') printf("\n"); |
||||
printf("------------------\n"); |
||||
return; |
||||
} |
||||
} |
||||
|
||||
/* Default to saving to a blob */ |
||||
char *filename = output_file(path, "blob", "bin"); |
||||
FILE *fp = fopen(filename, "wb"); |
||||
if(!fp) { |
||||
err("could not save to '%s': %m", filename); |
||||
return; |
||||
} |
||||
|
||||
fwrite(msg->output, 1, msg->header.size, fp); |
||||
fclose(fp); |
||||
fprintf(stderr, "Saved as blob to '%s'\n", filename); |
||||
free(filename); |
||||
} |
||||
|
||||
static void message_output(message_t *msg, void *buffer, int size) |
||||
{ |
||||
int data_left = msg->header.size - msg->size_read; |
||||
|
||||
printf("Got %d bytes of message data!\n", size); |
||||
|
||||
if(size > data_left) { |
||||
err("Too much data in message, dropping %d bytes", size - data_left); |
||||
size = data_left; |
||||
} |
||||
|
||||
memcpy(msg->output + msg->size_read, buffer, size); |
||||
|
||||
msg->size_read += size; |
||||
if(msg->size_read >= msg->header.size) { |
||||
fprintf(stderr, "Successfully read %d bytes\n", msg->size_read); |
||||
message_finish(msg); |
||||
msg->valid = false; |
||||
} |
||||
} |
||||
|
||||
int main_interactive(filter_t *filter, delay_t *delay, libusb_context *context) |
||||
{ |
||||
libusb_device *dev = NULL; |
||||
libusb_device_handle *dh = NULL; |
||||
|
||||
/* Wait for a device to be connected */ |
||||
filter_clean_libusb(filter); |
||||
int rc = usb_unique_wait(filter, delay, context, &dev); |
||||
|
||||
if(rc == FILTER_NONE) { |
||||
printf("No device found.\n"); |
||||
return 1; |
||||
} |
||||
else if(rc == FILTER_MULTIPLE) { |
||||
printf("Multiple devices found, ambiguous!\n"); |
||||
return 1; |
||||
} |
||||
|
||||
if((rc = libusb_open(dev, &dh))) { |
||||
rc = libusb_err(rc, "cannot open device %s", usb_id(dev)); |
||||
goto end; |
||||
} |
||||
|
||||
/* Don't detach kernel drivers to avoid breaking the Mass Storage
|
||||
communications if fxlink is ever started while the native LINK |
||||
application is running! */ |
||||
libusb_set_auto_detach_kernel_driver(dh, false); |
||||
|
||||
if((rc = libusb_claim_interface(dh, 0))) { |
||||
rc = libusb_err(rc, "cannot claim interface on %s", usb_id(dev)); |
||||
goto end; |
||||
} |
||||
|
||||
printf("Connected to %s, starting test.\n", usb_id(dev)); |
||||
|
||||
/* This buffer is used to receive messages; if the header is not complete
|
||||
it is left in the buffer, hence the extra room */ |
||||
__attribute__((aligned(4))) |
||||
static uint8_t buffer[2048 + sizeof(usb_fxlink_header_t)] = { 0 }; |
||||
/* Amount of data in the buffer */ |
||||
int buffer_size = 0; |
||||
|
||||
/* Current message */ |
||||
message_t msg = { 0 }; |
||||
|
||||
while(1) |
||||
{ |
||||
int transferred = -1; |
||||
rc = libusb_bulk_transfer(dh, 0x81, buffer + buffer_size, 2048, |
||||
&transferred, 2000); |
||||
|
||||
if(rc == LIBUSB_ERROR_NO_DEVICE) { |
||||
printf("Disconnected, leaving.\n"); |
||||
break; |
||||
} |
||||
else if(rc && rc != LIBUSB_ERROR_TIMEOUT) { |
||||
rc = libusb_err(rc, "bulk transfer failed on %s", usb_id(dev)); |
||||
continue; |
||||
} |
||||
if(transferred <= 0) continue; |
||||
|
||||
buffer_size += transferred; |
||||
|
||||
/* If there is an unfinished message, continue working on it */ |
||||
if(msg.valid) { |
||||
message_output(&msg, buffer, buffer_size); |
||||
buffer_size = 0; |
||||
} |
||||
|
||||
/* If the header is not yet fully transmitted, wait */ |
||||
usb_fxlink_header_t *h = (void *)buffer; |
||||
if(buffer_size < (int)sizeof *h) continue; |
||||
|
||||
/* Handle a new message */ |
||||
if(h->version == 0x00000100) { |
||||
int data_size = buffer_size - sizeof *h; |
||||
|
||||
if(!message_new(&msg, h)) |
||||
printf("dropping %d bytes\n", data_size); |
||||
else |
||||
message_output(&msg, buffer + sizeof *h, data_size); |
||||
|
||||
buffer_size = 0; |
||||
continue; |
||||
} |
||||
else { |
||||
err("invalid header, dropping %d bytes", transferred); |
||||
buffer_size = 0; |
||||
} |
||||
} |
||||
|
||||
/* Save last unfinished message */ |
||||
if(buffer_size > 0) { |
||||
printf("%d bytes not collected dropped\n", buffer_size); |
||||
} |
||||
rc = 0; |
||||
|
||||
end: |
||||
if(dh) { |
||||
libusb_release_interface(dh, 0); |
||||
libusb_close(dh); |
||||
} |
||||
if(dev) libusb_unref_device(dev); |
||||
return rc; |
||||
} |
||||
@ -0,0 +1,42 @@
|
||||
#include "png.h" |
||||
#include "util.h" |
||||
#include <stdio.h> |
||||
|
||||
/* fxlink_png_save(): Save a bitmap into a PNG file */ |
||||
int fxlink_png_save(png_byte **row_pointers, int width, int height, |
||||
char const *path) |
||||
{ |
||||
png_struct *png = png_create_write_struct(PNG_LIBPNG_VER_STRING, |
||||
NULL, NULL, NULL); |
||||
if(!png) |
||||
return err("failed to write PNG: png_create_write_struct"); |
||||
|
||||
png_infop info = png_create_info_struct(png); |
||||
if(!info) |
||||
return err("failed to write PNG: png_create_info_struct"); |
||||
|
||||
FILE *fp = fopen(path, "wb"); |
||||
if(!fp) { |
||||
png_destroy_write_struct(&png, &info); |
||||
return err("failed to open '%s' to write PNG: %m", path); |
||||
} |
||||
|
||||
if(setjmp(png_jmpbuf(png))) { |
||||
fclose(fp); |
||||
png_destroy_write_struct(&png, &info); |
||||
return 1; |
||||
} |
||||
|
||||
png_init_io(png, fp); |
||||
png_set_IHDR(png, info, |
||||
width, height, 8, |
||||
PNG_COLOR_TYPE_RGB, PNG_INTERLACE_NONE, |
||||
PNG_COMPRESSION_TYPE_DEFAULT, PNG_FILTER_TYPE_DEFAULT); |
||||
|
||||
png_write_info(png, info); |
||||
png_write_image(png, row_pointers); |
||||
png_write_end(png, NULL); |
||||
png_destroy_write_struct(&png, &info); |
||||
fclose(fp); |
||||
return 0; |
||||
} |
||||
@ -0,0 +1,14 @@
|
||||
//---
|
||||
// fxlink:png - Tools to output PNG images with libpng
|
||||
//---
|
||||
|
||||
#ifndef FXLINK_PNG_H |
||||
#define FXLINK_PNG_H |
||||
|
||||
#include <png.h> |
||||
|
||||
/* fxlink_png_save(): Save a bitmap into a PNG file */ |
||||
int fxlink_png_save(png_byte **row_pointers, int width, int height, |
||||
char const *path); |
||||
|
||||
#endif /* FXLINK_PNG_H */ |
||||
@ -0,0 +1,141 @@
|
||||
#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; |
||||
} |
||||
@ -0,0 +1,71 @@
|
||||
//---
|
||||
// fxlink:protocol - Custom fxlink protocol
|
||||
//---
|
||||
|
||||
#ifndef FXLINK_PROTOCOL_H |
||||
#define FXLINK_PROTOCOL_H |
||||
|
||||
#include <stdint.h> |
||||
#include <stdbool.h> |
||||
|
||||
/* See the gint source for details on the protocol */ |
||||
typedef struct |
||||
{ |
||||
uint32_t version; |
||||
uint32_t size; |
||||
uint32_t transfer_size; |
||||
|
||||
char application[16]; |
||||
char type[16]; |
||||
|
||||
} usb_fxlink_header_t; |
||||
|
||||
/* Subheader for the fxlink built-in "image" type */ |
||||
typedef struct |
||||
{ |
||||
uint32_t width; |
||||
uint32_t height; |
||||
int pixel_format; |
||||
|
||||
} usb_fxlink_image_t; |
||||
|
||||
/* Pixel formats */ |
||||
typedef enum |
||||
{ |
||||
/* Image is an array of *big-endian* uint16_t with RGB565 format */ |
||||
USB_FXLINK_IMAGE_RGB565 = 0, |
||||
/* Image is an array of bits in mono format */ |
||||
USB_FXLINK_IMAGE_MONO, |
||||
/* Image is two consecutive mono arrays, one for light, one for dark */ |
||||
USB_FXLINK_IMAGE_GRAY, |
||||
|
||||
} usb_fxlink_image_format_t; |
||||
|
||||
//---
|
||||
// Utilities in this implementation
|
||||
//---
|
||||
|
||||
/* Message currently being transferred */ |
||||
typedef struct |
||||
{ |
||||
usb_fxlink_header_t header; |
||||
/* Valid when we are reading a message */ |
||||
bool valid; |
||||
/* Data already read in this message */ |
||||
uint32_t size_read; |
||||
/* Data buffer */ |
||||
char *output; |
||||
|
||||
} message_t; |
||||
|
||||
/* fxlink_protocol_decode_image(): Decode an image into RGB888 format
|
||||
|
||||
This function decodes the message into an RGB888 image and returns an array |
||||
of row pointers with the image data (free the array and each element of the |
||||
array after use). |
||||
|
||||
If there are not enough bytes in the input, it pads with zeros, and if there |
||||
are extra bytes, they are dropped; in both cases a warning is printed. */ |
||||
uint8_t **fxlink_protocol_decode_image(message_t *msg); |
||||
|
||||
#endif /* FXLINK_PROTOCOL_H */ |
||||
Loading…
Reference in new issue