#include <stdio.h>#include <stdlib.h>#include <string.h>#include <vector>#include <memory>#include <string>
#include <unistd.h>#include <fcntl.h>#include <termios.h>#include <sys/signal.h>
#include <libusb-1.0/libusb.h>
#define VENDOR 0x0483#define PRODUCT 0x5741
#define START_ADDRESS 0x10040000
#define STDIN_BUF_SIZE 256#define RX_BUF_SIZE 16384
// in milliseconds#define CONFIG_TIMEOUT 100#define FLASH_TIMEOUT 200
#define GET_LINE_CODING(dummy) libusb_control_transfer(dev, 0xa1, 0x21, 0, 0, (u8*)dummy, 7, 100)#define SET_LINE_CODING(line_coding) libusb_control_transfer(dev, 0x21, 0x20, 0, 0, (u8*)line_coding, 7, 100)#define SET_CTRL_LINE_STATE(state) libusb_control_transfer(dev, 0x21, 0x22, state, 0, nullptr, 0, 100)
typedef unsigned char u8;typedef unsigned int u32;
static const u8 api_line_coding[] = {0x64, 0, 0, 0, 0, 0, 8};static const u8 transfer_line_coding[] = {0, 8, 7, 0, 0, 0, 8};static const u8 serial_line_coding[] = {0, 0xc2, 1, 0, 0, 0, 8};
std::string filename;
static struct termios old_term = {0};static int old_fl = 0;static struct termios new_term = {0};static int new_fl = 0;
static int reattach_drivers = 0;
static libusb_device_handle *dev = nullptr;static struct timeval short_timeout = {0, 20 * 1000};
static u8 tx_buf[1024];
static bool can_free_read_tx = false;static bool should_print = false;static bool waiting_for_output = false;
static void handle_read_tx(libusb_transfer *tx) { if (tx->status == LIBUSB_TRANSFER_CANCELLED) can_free_read_tx = true;}
static void handle_serial_tx(libusb_transfer *tx) { if (tx->status == LIBUSB_TRANSFER_COMPLETED) { waiting_for_output = false; if (should_print && tx->endpoint == 0x83 && tx->actual_length > 0) write(STDOUT_FILENO, tx->buffer, tx->actual_length); }}
u8 bytes_xored(u8 *buf, int size) { u8 x = 0; for (int i = 0; i < size; i++) x ^= buf[i]; return x;}
struct timeval from_ms(int total_ms) { int secs = total_ms / 1000; int ms = total_ms % 1000; return (struct timeval) {secs, ms * 1000};}
libusb_transfer *start_configuration(const u8 *line_coding, u8 *buffer, int size, libusb_transfer_cb_fn callback, int timeout) { u8 dummy[7];
libusb_transfer *read_tx = libusb_alloc_transfer(0);
libusb_fill_bulk_transfer(read_tx, dev, 0x83, buffer, size, callback, nullptr, timeout);
// no timeout for the interrupt transfer. this is so that it can act as a kind of signalling mechanism //libusb_fill_interrupt_transfer(int_tx, dev, 0x82, &tx_buf[512], 512, handle_int_transfer, nullptr, 0);
libusb_submit_transfer(read_tx); //libusb_submit_transfer(int_tx);
GET_LINE_CODING(dummy); GET_LINE_CODING(dummy); GET_LINE_CODING(dummy); GET_LINE_CODING(dummy);
SET_LINE_CODING(line_coding); GET_LINE_CODING(dummy); SET_CTRL_LINE_STATE(3); SET_LINE_CODING(line_coding); GET_LINE_CODING(dummy);
usleep(100 * 1000); return read_tx;}
void end_configuration(libusb_transfer *read_tx) { can_free_read_tx = false; libusb_cancel_transfer(read_tx);
struct timeval timeout = from_ms(read_tx->timeout); libusb_handle_events_timeout(nullptr, &timeout);
if (can_free_read_tx) libusb_free_transfer(read_tx);
SET_CTRL_LINE_STATE(2);}
bool submit_command(libusb_transfer *read_tx, u8 *buf, int size) { int tx = 0; libusb_bulk_transfer(dev, 3, buf, size, &tx, 100);
struct timeval timeout = from_ms(read_tx->timeout); bool success = libusb_handle_events_timeout(nullptr, &timeout) == LIBUSB_SUCCESS;
u8 temp; libusb_bulk_transfer(dev, 0x83, &temp, 0, &tx, 100);
libusb_submit_transfer(read_tx);
return success;}
void submit_command(libusb_transfer *read_tx, std::string str) { submit_command(read_tx, (u8*)str.data(), str.size());}
std::vector<u8> reload_file() { std::vector<u8> file; const char *fname = filename.c_str();
FILE *f = fopen(fname, "rb"); if (!f) { printf("Could not open \"%s\"", fname); return file; }
fseek(f, 0, SEEK_END); int sz = ftell(f); rewind(f);
if (sz < 1) { printf("File \"%s\" could not be read\n", fname); return file; }
file.resize(sz); fread(file.data(), 1, sz, f); fclose(f);
return file;}
void flash_image() { puts("\nFlashing...");
std::vector<u8> file = reload_file(); if (!file.size()) return;
libusb_transfer *init_read_tx = start_configuration(api_line_coding, &tx_buf[0], 512, handle_read_tx, CONFIG_TIMEOUT); submit_command(init_read_tx, "\nswitchDtmInterface 0\n"); submit_command(init_read_tx, "\nBlueBootloader\n"); end_configuration(init_read_tx);
libusb_transfer *flash_read_tx = start_configuration(transfer_line_coding, &tx_buf[0], 512, handle_read_tx, FLASH_TIMEOUT);
u8 cmd1 = 0x7f; if (!submit_command(flash_read_tx, &cmd1, 1)) { end_configuration(flash_read_tx); puts("Failed to set up flash transfer (retrying may work)\n"); return; }
u8 cmd2[] = {0x02, 0xfd}; submit_command(flash_read_tx, cmd2, 2);
int left = file.size(); int chunks = 0; int offset = 0;
u8 chunk_buf[258];
while (left > 0) { if (chunks % 8 == 0) { int section = chunks / 8; int sec_h = (section >> 8) & 0xff; int sec_l = section & 0xff;
u8 cmd3[] = {0x43, 0xbc}; submit_command(flash_read_tx, cmd3, 2); u8 cmd4[] = {(u8)sec_h, (u8)sec_l, (u8)(sec_h ^ sec_l)}; submit_command(flash_read_tx, cmd4, 3); }
u32 addr = START_ADDRESS + (chunks * 0x100);
u8 cmd5[] = {0x31, 0xce}; submit_command(flash_read_tx, cmd5, 2); u8 cmd6[] = {(u8)(addr >> 24), (u8)(addr >> 16), (u8)(addr >> 8), (u8)addr, bytes_xored((u8*)&addr, sizeof(u32))}; submit_command(flash_read_tx, cmd6, 5);
int tx = 256; if (tx > left) tx = left;
chunk_buf[0] = (tx - 1) & 0xff; memcpy(&chunk_buf[1], file.data() + offset, tx); chunk_buf[tx+1] = bytes_xored(&chunk_buf[0], tx + 1); // we include the size byte in the range to xor
submit_command(flash_read_tx, chunk_buf, tx + 2);
chunks++; offset += tx; left -= tx; }
end_configuration(flash_read_tx);
libusb_transfer *finish_read_tx = start_configuration(api_line_coding, &tx_buf[0], 512, handle_read_tx, CONFIG_TIMEOUT); submit_command(finish_read_tx, "\nBlueReset 1\n"); submit_command(finish_read_tx, "\nBlueReset 0\n"); end_configuration(finish_read_tx);
puts("Done! Entering console... (hit Ctrl+R to flash again)\n");}
void close_libusb() { if (reattach_drivers & 1) libusb_attach_kernel_driver(dev, 0); if (reattach_drivers & 2) libusb_attach_kernel_driver(dev, 1);
libusb_close(dev); libusb_exit(nullptr);}
void restore_terminal() { tcsetattr(STDIN_FILENO, TCSADRAIN, &old_term); fcntl(STDIN_FILENO, F_SETFL, old_fl);}
static void signal_handler(int signum) { printf("Exiting\n"); close_libusb(); restore_terminal(); exit(0);}
int main(int argc, char **argv) { signal(SIGINT, signal_handler); tcgetattr(STDIN_FILENO, &old_term); old_fl = fcntl(STDIN_FILENO, F_GETFL);
puts("BlueNRG2 flasher");
if (argc < 2) { puts("Requires file to flash"); return 1; }
filename = std::string(argv[1]);
libusb_init(nullptr); dev = libusb_open_device_with_vid_pid(NULL, VENDOR, PRODUCT); if (!dev) { puts("Could not open BlueNRG2 dev kit"); return 4; }
if (libusb_kernel_driver_active(dev, 0)) { libusb_detach_kernel_driver(dev, 0); reattach_drivers |= 1; } if (libusb_kernel_driver_active(dev, 1)) { libusb_detach_kernel_driver(dev, 1); reattach_drivers |= 2; }
new_term = old_term; new_term.c_lflag &= ~ICANON; new_term.c_lflag &= ~ECHO; new_term.c_cc[VMIN] = 1; new_term.c_cc[VTIME] = 0; tcsetattr(STDIN_FILENO, TCSANOW, &new_term); new_fl = old_fl | O_NONBLOCK; fcntl(STDIN_FILENO, F_SETFL, new_fl);
char stdin_buffer[STDIN_BUF_SIZE]; auto rx_buffer = std::make_unique<char[]>(RX_BUF_SIZE);
should_print = false; while (true) { flash_image(); libusb_transfer *serial_tx = start_configuration(serial_line_coding, (u8*)rx_buffer.get(), RX_BUF_SIZE, handle_serial_tx, 0); should_print = true;
while (true) { int input_len = read(STDIN_FILENO, stdin_buffer, STDIN_BUF_SIZE);
if (input_len > 0) { char cmd = stdin_buffer[0];
if (cmd == 0x12) { // Ctrl+R if (waiting_for_output) libusb_cancel_transfer(serial_tx); break; }
int temp = 0; libusb_bulk_transfer(dev, 3, (u8*)stdin_buffer, input_len, &temp, 100); }
if (!waiting_for_output) { waiting_for_output = true; libusb_submit_transfer(serial_tx); }
libusb_handle_events_timeout(nullptr, &short_timeout); }
should_print = false; end_configuration(serial_tx); }
close_libusb(); restore_terminal();
return 0;}