parent
30befdd2cf
commit
d2b6da5122
GPG Key ID:
1BBA026E13FC0495
@ -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