From d2b6da5122e2a62af21d995569d6905bd7892d91 Mon Sep 17 00:00:00 2001 From: Lephenixnoir Date: Tue, 25 May 2021 20:46:27 +0200 Subject: [PATCH] fxlink: add interactive mode to exchange data with gint --- CMakeLists.txt | 7 +- fxlink/fxlink.h | 3 + fxlink/interactive.c | 232 +++++++++++++++++++++++++++++++++++++++++++ fxlink/main.c | 79 +++------------ fxlink/png.c | 42 ++++++++ fxlink/png.h | 14 +++ fxlink/protocol.c | 141 ++++++++++++++++++++++++++ fxlink/protocol.h | 71 +++++++++++++ 8 files changed, 521 insertions(+), 68 deletions(-) create mode 100644 fxlink/interactive.c create mode 100644 fxlink/png.c create mode 100644 fxlink/png.h create mode 100644 fxlink/protocol.c create mode 100644 fxlink/protocol.h diff --git a/CMakeLists.txt b/CMakeLists.txt index eed7691..f03691d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,9 +34,10 @@ add_custom_target(fxsdk ALL DEPENDS "${BIN}/fxsdk.sh") # fxlink configure_file(fxlink/config.h.in "${BIN}/include/fxlink/config.h") -add_executable(fxlink fxlink/usb.c fxlink/filter.c fxlink/main.c - fxlink/properties.c fxlink/ud2.c fxlink/util.c) -target_link_libraries(fxlink PkgConfig::libusb) # PkgConfig::libudev +add_executable(fxlink fxlink/usb.c fxlink/filter.c fxlink/interactive.c + fxlink/main.c fxlink/png.c fxlink/properties.c fxlink/ud2.c fxlink/util.c + fxlink/protocol.c) +target_link_libraries(fxlink PkgConfig::libpng PkgConfig::libusb) # PkgConfig::libudev target_include_directories(fxlink PRIVATE "${BIN}/include/fxlink") if(NOT FXLINK_DISABLE_UDISKS2) target_link_libraries(fxlink PkgConfig::udisks2) diff --git a/fxlink/fxlink.h b/fxlink/fxlink.h index e08d727..1d00369 100644 --- a/fxlink/fxlink.h +++ b/fxlink/fxlink.h @@ -18,4 +18,7 @@ int main_blocks(filter_t *filter, delay_t *delay); /* Main function for -s */ int main_send(filter_t *filter, delay_t *delay, char **files); +/* Main function for -i */ +int main_interactive(filter_t *filter,delay_t *delay,libusb_context *context); + #endif /* FXLINK_FXLINK_H */ diff --git a/fxlink/interactive.c b/fxlink/interactive.c new file mode 100644 index 0000000..9a17ac3 --- /dev/null +++ b/fxlink/interactive.c @@ -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 +#include +#include +#include +#include +#include +#include + +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; +} diff --git a/fxlink/main.c b/fxlink/main.c index c2e8adc..b5a5f27 100644 --- a/fxlink/main.c +++ b/fxlink/main.c @@ -12,8 +12,6 @@ #include #include -/* Main functions for each mdoe */ -int main_list(filter_t *filter, delay_t *delay, libusb_context *context); int main_test(libusb_device *device, libusb_context *context); static const char *help_string = @@ -31,7 +29,7 @@ static const char *help_string = " -l, --list List detected calculators on the USB ports (libusb)\n" " -b, --blocks List detected Mass Storage filesystems (udisks2)\n" " -s, --send Send a file to a Mass Storage calculator (udisks2)\n" -" --test Communication tests by Lephe (libusb) [WIP!]\n" +" -i, --interactive Interactive messaging with a gint add-in (libusb)\n" "\n" "General options:\n" " -w DELAY Wait up to this many seconds for a calculator to\n" @@ -67,18 +65,18 @@ int main(int argc, char **argv) // Command-line argument parsing //--- - enum { TEST=1, LIBUSB_LOG }; + enum { LIBUSB_LOG=1 }; const struct option longs[] = { - { "help", no_argument, NULL, 'h' }, - { "list", no_argument, NULL, 'l' }, - { "blocks", no_argument, NULL, 'b' }, - { "send", no_argument, NULL, 's' }, - { "test", no_argument, NULL, TEST }, - { "libusb-log", required_argument, NULL, LIBUSB_LOG }, + { "help", no_argument, NULL, 'h' }, + { "list", no_argument, NULL, 'l' }, + { "blocks", no_argument, NULL, 'b' }, + { "send", no_argument, NULL, 's' }, + { "interactive", no_argument, NULL, 'i' }, + { "libusb-log", required_argument, NULL, LIBUSB_LOG }, }; while(option >= 0 && option != '?') - switch((option = getopt_long(argc, argv, "hlbsf:w::", longs, NULL))) + switch((option = getopt_long(argc, argv, "hlbsif:w::", longs, NULL))) { case 'h': fprintf(stderr, help_string, argv[0]); @@ -86,7 +84,7 @@ int main(int argc, char **argv) case 'l': case 'b': case 's': - case TEST: + case 'i': mode = option; break; case LIBUSB_LOG: @@ -141,7 +139,7 @@ int main(int argc, char **argv) libusb_context *context = NULL; /* Initialize libusb for corresponding modes */ - if(mode == 'l' || mode == TEST) { + if(mode == 'l' || mode == 'i') { if((rc = libusb_init(&context))) return libusb_err(rc, "error initializing libusb"); libusb_set_option(context, LIBUSB_OPTION_LOG_LEVEL, loglevel); @@ -168,18 +166,8 @@ int main(int argc, char **argv) rc = err("this fxlink was built without UDisks2; -s is disabled"); #endif } - else if(mode == TEST) { - libusb_device *dev = NULL; - int rc = usb_unique_wait(filter, &delay, context, &dev); - - if(rc == FILTER_NONE) - printf("No device found.\n"); - else if(rc == FILTER_MULTIPLE) - printf("Multiple devices found, ambiguous!\n"); - else if(rc == FILTER_UNIQUE) { - rc = main_test(dev, context); - libusb_unref_device(dev); - } + else if(mode == 'i') { + rc = main_interactive(filter, &delay, context); } if(context) libusb_exit(context); @@ -240,48 +228,9 @@ int main_list(filter_t *filter, delay_t *delay, libusb_context *context) } //--- -// WIP tests +// libudev tests; work but not useful yet //--- -int main_test(libusb_device *dev, libusb_context *context) -{ - libusb_device_handle *dh; - int rc; - (void)context; - - if((rc = libusb_open(dev, &dh))) - return libusb_err(rc, "cannot open device %s", usb_id(dev)); - - /* When possible detach any existing driver */ - libusb_set_auto_detach_kernel_driver(dh, true); - - if((rc = libusb_claim_interface(dh, 0))) { - libusb_close(dh); - return libusb_err(rc, "cannot claim interface on %s", usb_id(dev)); - } - - uint8_t buffer[2048]; - int transferred = -1; - - while(1) - { - rc = libusb_bulk_transfer(dh, 0x81, buffer, 2048, &transferred, 500); - - if((rc == 0 || rc == LIBUSB_ERROR_TIMEOUT) && transferred > 0) - fwrite(buffer, 1, transferred, stdout); - if(rc) - rc=libusb_err(rc,"cannot perform bulk transfer on %s",usb_id(dev)); - - fprintf(stderr, "Transferred: %d\n", transferred); - if(rc || transferred == 0) break; - } - - libusb_release_interface(dh, 0); - libusb_close(dh); - return rc; -} - -/* libudev tests, work but not useful yet */ #if 0 #include int main_udev_test(libusb_device *dev) diff --git a/fxlink/png.c b/fxlink/png.c new file mode 100644 index 0000000..89f9453 --- /dev/null +++ b/fxlink/png.c @@ -0,0 +1,42 @@ +#include "png.h" +#include "util.h" +#include + +/* 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; +} diff --git a/fxlink/png.h b/fxlink/png.h new file mode 100644 index 0000000..acc76a7 --- /dev/null +++ b/fxlink/png.h @@ -0,0 +1,14 @@ +//--- +// fxlink:png - Tools to output PNG images with libpng +//--- + +#ifndef FXLINK_PNG_H +#define FXLINK_PNG_H + +#include + +/* 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 */ diff --git a/fxlink/protocol.c b/fxlink/protocol.c new file mode 100644 index 0000000..d67b5d5 --- /dev/null +++ b/fxlink/protocol.c @@ -0,0 +1,141 @@ +#include "protocol.h" +#include "util.h" + +#include +#include +#include +#include + +//--- +// 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; +} diff --git a/fxlink/protocol.h b/fxlink/protocol.h new file mode 100644 index 0000000..9595e3e --- /dev/null +++ b/fxlink/protocol.h @@ -0,0 +1,71 @@ +//--- +// fxlink:protocol - Custom fxlink protocol +//--- + +#ifndef FXLINK_PROTOCOL_H +#define FXLINK_PROTOCOL_H + +#include +#include + +/* 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 */