diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..a0b0bc1 --- /dev/null +++ b/Makefile @@ -0,0 +1,17 @@ +CDBA := bad + +.PHONY: all + +all: $(CDBA) + +CFLAGS := -Wall -g -O2 +LDFLAGS := -ludev + +CDBA_SRCS := bad.c cdb_assist.c circ_buf.c device.c fastboot.c +CDBA_OBJS := $(CDBA_SRCS:.c=.o) + +$(CDBA): $(CDBA_OBJS) + $(CC) $(LDFLAGS) -o $@ $^ + +clean: + rm -f $(CDBA_OBJS) diff --git a/bad.c b/bad.c new file mode 100644 index 0000000..0c967e1 --- /dev/null +++ b/bad.c @@ -0,0 +1,231 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "bad.h" +#include "circ_buf.h" +#include "device.h" +#include "fastboot.h" +#include "list.h" + +struct device *selected_device; + +static void fastboot_opened(struct fastboot *fb, void *data) +{ + const uint8_t one = 1; + struct msg *msg; + + msg = alloca(sizeof(*msg) + 1); + msg->type = MSG_FASTBOOT_PRESENT; + msg->len = 1; + memcpy(msg->data, &one, 1); + + write(STDOUT_FILENO, msg, sizeof(*msg) + 1); +} + +static void fastboot_info(struct fastboot *fb, const void *buf, size_t len) +{ + fprintf(stderr, "%s\n", (char *)buf); +} + +static void fastboot_disconnect(void *data) +{ + const uint8_t zero = 0; + struct msg *msg; + + msg = alloca(sizeof(*msg) + 1); + msg->type = MSG_FASTBOOT_PRESENT; + msg->len = 1; + memcpy(msg->data, &zero, 1); + + write(STDOUT_FILENO, msg, sizeof(*msg) + 1); +} + +static struct fastboot_ops fastboot_ops = { + .opened = fastboot_opened, + .disconnect = fastboot_disconnect, + .info = fastboot_info, +}; + +static void msg_select_board(const void *param) +{ + struct msg reply = { MSG_SELECT_BOARD, 0 }; + + selected_device = device_open(param, &fastboot_ops); + if (!selected_device) + fprintf(stderr, "failed to open %s\n", (const char *)param); + + write(STDOUT_FILENO, &reply, sizeof(reply)); +} + +static void *fastboot_payload; +static size_t fastboot_size; + +static void msg_fastboot_download(const void *data, size_t len) +{ + struct msg reply = { MSG_FASTBOOT_DOWNLOAD, }; + size_t new_size = fastboot_size + len; + void *newp; + + newp = realloc(fastboot_payload, new_size); + if (!newp) + err(1, "failed too expant fastboot scratch area"); + + memcpy(newp + fastboot_size, data, len); + + fastboot_payload = newp; + fastboot_size = new_size; + + if (!len) { + device_boot(selected_device, fastboot_payload, fastboot_size); + + write(STDOUT_FILENO, &reply, sizeof(reply)); + free(fastboot_payload); + fastboot_payload = NULL; + fastboot_size = 0; + } +} + +static int handle_stdin(int fd, void *buf) +{ + static struct circ_buf recv_buf = { 0 }; + struct msg *msg; + struct msg hdr; + size_t n; + int ret; + + ret = circ_fill(STDIN_FILENO, &recv_buf); + if (ret < 0 && errno != EAGAIN) { + fprintf(stderr, "read %d\n", ret); + return -1; + } + + for (;;) { + n = circ_peak(&recv_buf, &hdr, sizeof(hdr)); + if (n != sizeof(hdr)) + return 0; + + if (CIRC_AVAIL(&recv_buf) < sizeof(*msg) + hdr.len) + return 0; + + msg = malloc(sizeof(*msg) + hdr.len); + circ_read(&recv_buf, msg, sizeof(*msg) + hdr.len); + + switch (msg->type) { + case MSG_CONSOLE: + device_write(selected_device, msg->data, msg->len); + break; + case MSG_FASTBOOT_PRESENT: + break; + case MSG_SELECT_BOARD: + msg_select_board(msg->data); + break; + case MSG_HARDRESET: + // fprintf(stderr, "hard reset\n"); + break; + case MSG_POWER_ON: + device_power_on(selected_device); + break; + case MSG_POWER_OFF: + device_power_off(selected_device); + break; + case MSG_FASTBOOT_DOWNLOAD: + msg_fastboot_download(msg->data, msg->len); + break; + case MSG_FASTBOOT_BOOT: + // fprintf(stderr, "fastboot boot\n"); + break; + case MSG_STATUS_UPDATE: + device_print_status(selected_device); + break; + case MSG_VBUS_ON: + device_vbus(selected_device, true); + break; + case MSG_VBUS_OFF: + device_vbus(selected_device, false); + break; + default: + fprintf(stderr, "unk %d len %d\n", msg->type, msg->len); + exit(1); + } + + free(msg); + } + + return 0; +} + +struct watch { + struct list_head node; + + int fd; + int (*cb)(int, void*); + void *data; +}; + +static struct list_head read_watches = LIST_INIT(read_watches); + +void watch_add_readfd(int fd, int (*cb)(int, void*), void *data) +{ + struct watch *w; + + w = calloc(1, sizeof(*w)); + w->fd = fd; + w->cb = cb; + w->data = data; + + list_add(&read_watches, &w->node); +} + +int main(int argc, char **argv) +{ + struct watch *w; + fd_set rfds; + int flags; + int nfds; + int ret; + + watch_add_readfd(STDIN_FILENO, handle_stdin, NULL); + + flags = fcntl(STDIN_FILENO, F_GETFL, 0); + fcntl(STDIN_FILENO, F_SETFL, flags | O_NONBLOCK); + + for (;;) { + nfds = 0; + + list_for_each_entry(w, &read_watches, node) { + nfds = MAX(nfds, w->fd); + FD_SET(w->fd, &rfds); + } + + if (!FD_ISSET(STDIN_FILENO, &rfds)) { + fprintf(stderr, "rfds is trash!\n"); + goto done; + } + + ret = select(nfds + 1, &rfds, NULL, NULL, NULL); + if (ret < 0) + continue; + + list_for_each_entry(w, &read_watches, node) { + if (FD_ISSET(w->fd, &rfds)) { + ret = w->cb(w->fd, w->data); + if (ret < 0) { + fprintf(stderr, "cb returned %d\n", ret); + goto done; + } + } + } + } + +done: + + device_power_off(selected_device); + + return 0; +} diff --git a/bad.h b/bad.h new file mode 100644 index 0000000..49b4afe --- /dev/null +++ b/bad.h @@ -0,0 +1,39 @@ +#ifndef __BAD_H__ +#define __BAD_H__ + +#include +#include + +#define __packed __attribute__((packed)) + +#define MIN(x, y) ((x) < (y) ? (x) : (y)) +#define MAX(x, y) ((x) > (y) ? (x) : (y)) + +struct msg { + uint8_t type; + uint16_t len; + uint8_t data[]; +} __packed; + +enum { + MSG_SELECT_BOARD = 1, + MSG_CONSOLE, + MSG_HARDRESET, + MSG_POWER_ON, + MSG_POWER_OFF, + MSG_FASTBOOT_PRESENT, + MSG_FASTBOOT_DOWNLOAD, + MSG_FASTBOOT_BOOT, + MSG_STATUS_UPDATE, + MSG_VBUS_ON, + MSG_VBUS_OFF, + MSG_FASTBOOT_REBOOT, +}; + +void watch_add_readfd(int fd, int (*cb)(int, void*), void *data); +int watch_add_quit(int (*cb)(int, void*), void *data); +int watch_add_timer(void (*cb)(void*), void *data, unsigned interval, bool repeat); +void watch_quit(void); +int watch_run(void); + +#endif diff --git a/cdb_assist.c b/cdb_assist.c new file mode 100644 index 0000000..45cad00 --- /dev/null +++ b/cdb_assist.c @@ -0,0 +1,513 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "bad.h" + +struct cdb_assist { + char serial[9]; + + char control_uart[20]; + char target_uart[20]; + + int control_tty; + int target_tty; + + struct termios control_tios; + struct termios target_tios; + + struct cdb_assist *next; + + /* parser */ + unsigned state; + unsigned num[2]; + char key[32]; + unsigned key_idx; + bool voltage; + + /* state */ + unsigned current_actual; + unsigned current_set; + unsigned voltage_actual; + unsigned voltage_set; + + bool vbat; + bool btn[3]; + bool vbus; + unsigned vref; +}; + +static int readat(int dir, const char *name, char *buf, size_t len) +{ + ssize_t n; + int fd; + int ret = 0; + + fd = openat(dir, name, O_RDONLY); + if (fd < 0) + return fd; + + n = read(fd, buf, len - 1); + if (n < 0) { + warn("failed to read %s", name); + ret = -EINVAL; + goto close_fd; + } + buf[n] = '\0'; + + buf[strcspn(buf, "\n")] = '\0'; + +close_fd: + close(fd); + return ret; +} + +static struct cdb_assist *enumerate_cdb_assists() +{ + struct cdb_assist *cdb; + struct cdb_assist *all = NULL; + struct cdb_assist *last = NULL; + struct dirent *de; + char interface[30]; + char serial[9]; + DIR *dir; + int tty; + int fd; + int ret; + + tty = open("/sys/class/tty", O_DIRECTORY); + if (tty < 0) + err(1, "failed to open /sys/class/tty"); + + dir = fdopendir(tty); + if (!dir) + err(1, "failed to opendir /sys/class/tty"); + + while ((de = readdir(dir)) != NULL) { + if (strncmp(de->d_name, "ttyACM", 6) != 0) + continue; + + fd = openat(tty, de->d_name, O_DIRECTORY); + if (fd < 0) + continue; + + ret = readat(fd, "../../interface", interface, sizeof(interface)); + if (ret < 0) + goto close_fd; + + ret = readat(fd, "../../../serial", serial, sizeof(serial)); + if (ret < 0) + goto close_fd; + + for (cdb = all; cdb; cdb = cdb->next) { + if (strcmp(cdb->serial, serial) == 0) + break; + } + + if (!cdb) { + cdb = calloc(1, sizeof(*cdb)); + + strcpy(cdb->serial, serial); + + if (last) { + last->next = cdb; + last = cdb; + } else { + last = cdb; + all = cdb; + } + } + + if (strcmp(interface, "Control UART") == 0) + strcpy(cdb->control_uart, de->d_name); + else if (strcmp(interface, "Target UART") == 0) + strcpy(cdb->target_uart, de->d_name); + else + errx(1, "tty is neither control nor target\n"); + +close_fd: + close(fd); + } + + closedir(dir); + close(tty); + + return all; +} + +static struct cdb_assist *cdb_assist_find(const char *serial) +{ + struct cdb_assist *cdb; + struct cdb_assist *all; + + all = enumerate_cdb_assists(); + + for (cdb = all; cdb; cdb = cdb->next) { + if (strcmp(cdb->serial, serial) == 0) + return cdb; + } + + return NULL; +} + +static int tty_open(const char *tty, struct termios *old) +{ + struct termios tios; + char buf[80] = "/dev/"; + int ret; + int fd; + + strcat(buf, tty); + fd = open(buf, O_RDWR | O_NOCTTY | O_EXCL); + if (fd < 0) + err(1, "unable to open \"%s\"", tty); + + ret = tcgetattr(fd, old); + if (ret < 0) + err(1, "unable to retrieve \"%s\" tios", tty); + + memset(&tios, 0, sizeof(tios)); + tios.c_cflag = B115200 | CRTSCTS | CS8 | CLOCAL | CREAD; + tios.c_iflag = IGNPAR; + tios.c_oflag = 0; + + tcflush(fd, TCIFLUSH); + + ret = tcsetattr(fd, TCSANOW, &tios); + if (ret < 0) + err(1, "unable to update \"%s\" tios", tty); + + return fd; +} + +enum { + STATE_, + STATE_key, + STATE_key_bool, + STATE_key_value, + STATE_key_o, + STATE_key_of, + STATE_key_num, + STATE_key_num_m, + STATE_num, + STATE_num_m, + STATE_num_mX, + STATE_num_mX_, + STATE_num_num_m, +}; + +static void cdb_parser_bool(struct cdb_assist *cdb, const char *key, bool set) +{ + static const char *sz_keys[] = { "vbat", "btn1", "btn2", "btn3", "vbus" }; + int i; + + for (i = 0; i < 5; i++) + if (strcmp(key, sz_keys[i]) == 0) + break; + + switch (i) { + case 0: + cdb->vbat = set; + break; + case 1: + case 2: + case 3: + cdb->btn[i-1] = set; + break; + case 4: + cdb->vbus = set; + break; + } +} +static void cdb_parser_current(struct cdb_assist *cdb, unsigned set, unsigned actual) +{ + cdb->current_actual = actual; + cdb->current_set = set; +} + +static void cdb_parser_voltage(struct cdb_assist *cdb, unsigned set, unsigned actual) +{ + cdb->voltage_actual = actual; + cdb->voltage_set = set; +} + +static void cdb_parser_vref(struct cdb_assist *cdb, unsigned vref) +{ + cdb->vref = vref; +} + +static void cdb_parser_push(struct cdb_assist *cdb, char ch) +{ + switch (cdb->state) { + case STATE_: + if (isdigit(ch)) { + cdb->num[0] = ch - '0'; + cdb->state = STATE_num; + } else if (isalpha(ch)) { + cdb->key[0] = ch; + cdb->key_idx = 1; + cdb->state = STATE_key; + } + break; + case STATE_num: + if (isdigit(ch)) { + cdb->num[0] *= 10; + cdb->num[0] += ch - '0'; + } else if (ch == 'm') { + cdb->state = STATE_num_m; + } else { + cdb->state = STATE_; + } + break; + case STATE_num_m: + if (ch == 'v') { + cdb->voltage = true; + cdb->state = STATE_num_mX; + } else if (ch == 'a') { + cdb->voltage = false; + cdb->state = STATE_num_mX; + } else { + cdb->state = STATE_; + } + break; + case STATE_num_mX: + if (ch == '/') { + cdb->num[1] = 0; + cdb->state = STATE_num_mX_; + } else { + cdb->state = STATE_; + } + break; + case STATE_num_mX_: + if (isdigit(ch)) { + cdb->num[1] *= 10; + cdb->num[1] += ch - '0'; + } else if (ch == 'm') { + cdb->state = STATE_num_num_m; + } else { + cdb->state = STATE_; + } + break; + case STATE_num_num_m: + if (ch == 'v' && cdb->voltage) + cdb_parser_voltage(cdb, cdb->num[0], cdb->num[1]); + else if (ch == 'a' && !cdb->voltage) + cdb_parser_current(cdb, cdb->num[0], cdb->num[1]); + + cdb->state = STATE_; + break; + case STATE_key: + if (isalnum(ch)) { + cdb->key[cdb->key_idx++] = ch; + } else if (ch == ':') { + cdb->key[cdb->key_idx] = '\0'; + cdb->state = STATE_key_bool; + } else if (ch == '=') { + cdb->key[cdb->key_idx] = '\0'; + cdb->state = STATE_key_value; + } else { + cdb->state = STATE_; + } + break; + case STATE_key_bool: + if (ch == 'o') + cdb->state = STATE_key_o; + else + cdb->state = STATE_; + break; + case STATE_key_o: + if (ch == 'f') { + cdb->state = STATE_key_of; + } else if (ch == 'n') { + cdb_parser_bool(cdb, cdb->key, true); + cdb->state = STATE_; + } else { + cdb->state = STATE_; + } + break; + case STATE_key_of: + if (ch == 'f') + cdb_parser_bool(cdb, cdb->key, false); + cdb->state = STATE_; + break; + case STATE_key_value: + if (isdigit(ch)) { + cdb->num[0] = ch - '0'; + cdb->state = STATE_key_num; + } else { + cdb->state = STATE_; + } + break; + case STATE_key_num: + if (isdigit(ch)) { + cdb->num[0] *= 10; + cdb->num[0] += ch - '0'; + } else if (ch == 'm') { + cdb->state = STATE_key_num_m; + } else { + cdb->state = STATE_; + } + break; + case STATE_key_num_m: + if (ch == 'v') + cdb_parser_vref(cdb, cdb->num[0]); + cdb->state = STATE_; + break; + } +} + +static int cdb_assist_ctrl_data(int fd, void *data) +{ + struct cdb_assist *cdb = data; + char buf[10]; + ssize_t n; + ssize_t k; + + n = read(fd, buf, sizeof(buf) - 1); + if (n < 0) + return n; + + for (k = 0; k < n; k++) + cdb_parser_push(cdb, tolower(buf[k])); + + return 0; +} + +static int cdb_assist_target_data(int fd, void *data) +{ + struct msg hdr; + char buf[128]; + ssize_t n; + + n = read(fd, buf, sizeof(buf)); + if (n < 0) + return n; + + hdr.type = MSG_CONSOLE; + hdr.len = n; + write(STDOUT_FILENO, &hdr, sizeof(hdr)); + write(STDOUT_FILENO, buf, n); + + return 0; +} + +static int cdb_ctrl_write(struct cdb_assist *cdb, const char *buf, size_t len) +{ + return write(cdb->control_tty, buf, len); +} + +struct cdb_assist *cdb_assist_open(const char *serial) +{ + struct cdb_assist *cdb; + int ret; + + cdb = cdb_assist_find(serial); + if (!cdb) { + fprintf(stderr, "unable to find cdb assist with serial %s\n", serial); + return NULL; + } + + cdb->control_tty = tty_open(cdb->control_uart, &cdb->control_tios); + if (cdb->control_tty < 0) + return NULL; + + cdb->target_tty = tty_open(cdb->target_uart, &cdb->target_tios); + if (cdb->target_tty < 0) + return NULL; + + watch_add_readfd(cdb->control_tty, cdb_assist_ctrl_data, cdb); + watch_add_readfd(cdb->target_tty, cdb_assist_target_data, cdb); + + ret = cdb_ctrl_write(cdb, "vpabc", 5); + if (ret < 0) + return NULL; + + return cdb; +} + +void cdb_assist_close(struct cdb_assist *cdb) +{ + int ret; + + tcflush(cdb->control_tty, TCIFLUSH); + + ret = tcsetattr(cdb->target_tty, TCSAFLUSH, &cdb->target_tios); + if (ret < 0) + warn("unable to restore tios of \"%s\"", cdb->target_uart); + + close(cdb->control_tty); + close(cdb->target_tty); +} + +void cdb_power(struct cdb_assist *cdb, bool on) +{ + const char cmd[] = "pP"; + cdb_ctrl_write(cdb, &cmd[on], 1); +} + +void cdb_vbus(struct cdb_assist *cdb, bool on) +{ + const char cmd[] = "vV"; + cdb_ctrl_write(cdb, &cmd[on], 1); +} + +void cdb_gpio(struct cdb_assist *cdb, int gpio, bool on) +{ + const char *cmd[] = { "aA", "bB", "cC" }; + cdb_ctrl_write(cdb, &cmd[gpio][on], 1); +} + +int cdb_target_write(struct cdb_assist *cdb, const void *buf, size_t len) +{ + return write(cdb->target_tty, buf, len); +} + +void cdb_target_break(struct cdb_assist *cdb) +{ + tcsendbreak(cdb->target_tty, 0); +} + +int cdb_vref(struct cdb_assist *cdb) +{ + return cdb->vref; +} + +void cdb_assist_print_status(struct cdb_assist *cdb) +{ + struct msg hdr; + char buf[128]; + int n; + + n = sprintf(buf, "%dmV %dmA%s%s%s%s%s ref: %dmV", + cdb->voltage_set, + cdb->current_actual, + cdb->vbat ? " vbat" : "", + cdb->vbus ? " vbus" : "", + cdb->btn[0] ? " btn1" : "", + cdb->btn[1] ? " btn2" : "", + cdb->btn[2] ? " btn3" : "", + cdb->vref); + + hdr.type = MSG_STATUS_UPDATE; + hdr.len = n; + write(STDOUT_FILENO, &hdr, sizeof(hdr)); + write(STDOUT_FILENO, buf, n); +} + +void cdb_set_voltage(struct cdb_assist *cdb, unsigned mV) +{ + char buf[20]; + int n; + + n = sprintf(buf, "u%d\r\n", mV); + cdb_ctrl_write(cdb, buf, n); +} diff --git a/cdb_assist.h b/cdb_assist.h new file mode 100644 index 0000000..f45614a --- /dev/null +++ b/cdb_assist.h @@ -0,0 +1,20 @@ +#ifndef __CDB_ASSIST_H__ +#define __CDB_ASSIST_H__ + +#include + +struct cdb_assist; + +struct cdb_assist *cdb_assist_open(const char *serial); +void cdb_assist_close(struct cdb_assist *cdb); + +void cdb_power(struct cdb_assist *cdb, bool on); +void cdb_vbus(struct cdb_assist *cdb, bool on); +void cdb_gpio(struct cdb_assist *cdb, int gpio, bool on); +int cdb_target_write(struct cdb_assist *cdb, const void *buf, size_t len); +void cdb_target_break(struct cdb_assist *cdb); +unsigned int cdb_vref(struct cdb_assist *cdb); +void cdb_assist_print_status(struct cdb_assist *cdb); +void cdb_set_voltage(struct cdb_assist *cdb, unsigned mV); + +#endif diff --git a/circ_buf.c b/circ_buf.c new file mode 100644 index 0000000..ab39f7a --- /dev/null +++ b/circ_buf.c @@ -0,0 +1,105 @@ +/* + * Copyright (c) 2018, Linaro Ltd. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +#include "circ_buf.h" + +/** + * circ_fill() - read data into circular buffer + * @fd: non-blocking file descriptor to read + * @circ: circ_buf object to write to + * + * Return: 0 if fifo is full or fd depleted, negative errno on failure + */ +ssize_t circ_fill(int fd, struct circ_buf *circ) +{ + size_t space; + size_t count = 0; + ssize_t n = 0; + + do { + space = CIRC_SPACE_TO_END(circ); + if (!space) { + errno = EAGAIN; + return -1; + } + + n = read(fd, circ->buf + circ->head, space); + if (n == 0) { + errno = EPIPE; + return -1; + } else if (n < 0) + return -1; + + count += n; + + circ->head = (circ->head + n) & (CIRC_BUF_SIZE - 1); + } while (n != space); + + return 0; +} + +size_t circ_peak(struct circ_buf *circ, void *buf, size_t len) +{ + size_t tail = circ->tail; + char *p = buf; + + while (len--) { + if (tail == circ->head) + return 0; + + *p++ = circ->buf[tail]; + + tail = (tail + 1) & (CIRC_BUF_SIZE - 1); + } + + return (void*)p - buf; +} + +size_t circ_read(struct circ_buf *circ, void *buf, size_t len) +{ + char *p = buf; + + while (len--) { + if (circ->tail == circ->head) + return 0; + + *p++ = circ->buf[circ->tail]; + + circ->tail = (circ->tail + 1) & (CIRC_BUF_SIZE - 1); + } + + return (void*)p - buf; +} diff --git a/circ_buf.h b/circ_buf.h new file mode 100644 index 0000000..a169d04 --- /dev/null +++ b/circ_buf.h @@ -0,0 +1,57 @@ +/* + * Copyright (c) 2018, Linaro Ltd. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef __CIRC_BUF_H__ +#define __CIRC_BUF_H__ + +#include + +#ifndef MIN +#define MIN(x, y) ((x) < (y) ? (x) : (y)) +#endif + +#define CIRC_BUF_SIZE 16384 + +struct circ_buf { + char buf[CIRC_BUF_SIZE]; + size_t head; + size_t tail; +}; + +#define CIRC_AVAIL(circ) (((circ)->head - (circ)->tail) & (CIRC_BUF_SIZE - 1)) +#define CIRC_SPACE(circ) (((circ)->tail - (circ)->head - 1) & (CIRC_BUF_SIZE - 1)) + +#define CIRC_SPACE_TO_END(circ) MIN(CIRC_SPACE(circ), CIRC_BUF_SIZE - (circ)->head) + +ssize_t circ_fill(int fd, struct circ_buf *circ); +size_t circ_peak(struct circ_buf *circ, void *buf, size_t len); +size_t circ_read(struct circ_buf *circ, void *buf, size_t len); + +#endif diff --git a/device.c b/device.c new file mode 100644 index 0000000..6ef24dc --- /dev/null +++ b/device.c @@ -0,0 +1,151 @@ +#include + +#include +#include +#include +#include +#include + +#include "cdb_assist.h" +#include "device.h" +#include "fastboot.h" + +#define ARRAY_SIZE(x) ((sizeof(x)/sizeof((x)[0]))) + +static void device_fastboot_boot(struct device *device); +static void device_fastboot_flash_reboot(struct device *device); + +struct device { + char *board; + char *cdb_serial; + char *name; + char *serial; + unsigned voltage; + bool tickle_mmc; + bool pshold_shutdown; + struct fastboot *fastboot; + + void (*boot)(struct device *); + + struct cdb_assist *cdb; +}; + +static struct device devices[] = { +}; + +struct device *device_open(const char *board, + struct fastboot_ops *fastboot_ops) +{ + struct device *device = NULL; + int i; + + for (i = 0; i < ARRAY_SIZE(devices); i++) { + if (strcmp(devices[i].board, board) == 0) { + device = &devices[i]; + break; + } + } + + if (!device) + return NULL; + + device->cdb = cdb_assist_open(device->cdb_serial); + if (!device->cdb) + errx(1, "failed to open cdb assist"); + + cdb_set_voltage(device->cdb, device->voltage); + + device->fastboot = fastboot_open(device->serial, fastboot_ops, NULL); + + return device; +} + +int device_power_on(struct device *device) +{ + if (!device) + return 0; + + cdb_power(device->cdb, true); + cdb_gpio(device->cdb, 0, true); + usleep(500000); + cdb_gpio(device->cdb, 0, false); + + return 0; +} + +int device_enter_fastboot(struct device *device) +{ + cdb_gpio(device->cdb, 1, true); + cdb_vbus(device->cdb, true); + cdb_power(device->cdb, true); + + return 0; +} + +int device_power_off(struct device *device) +{ + if (!device) + return 0; + + cdb_vbus(device->cdb, false); + cdb_power(device->cdb, false); + + if (device->pshold_shutdown) { + cdb_gpio(device->cdb, 2, true); + sleep(2); + cdb_gpio(device->cdb, 2, false); + } + + return 0; +} + +void device_print_status(struct device *device) +{ + cdb_assist_print_status(device->cdb); +} + +void device_vbus(struct device *device, bool enable) +{ + cdb_vbus(device->cdb, enable); +} + +void device_trigger_fastboot(struct device *device, bool enable) +{ + cdb_gpio(device->cdb, 1, enable); +} + +int device_write(struct device *device, const void *buf, size_t len) +{ + if (!device) + return 0; + + return cdb_target_write(device->cdb, buf, len); +} + +void device_break(struct device *device) +{ + cdb_target_break(device->cdb); +} + +const char *device_get_serial(struct device *device) +{ + return device->serial; +} + +static void device_fastboot_boot(struct device *device) +{ + fastboot_boot(device->fastboot); +} + +static void device_fastboot_flash_reboot(struct device *device) +{ +// fastboot_flash(fb, "boot"); +// fastboot_reboot(fb); +} + +void device_boot(struct device *device, const void *data, size_t len) +{ + fastboot_download(device->fastboot, data, len); + + device->boot(device); +} diff --git a/device.h b/device.h new file mode 100644 index 0000000..08f8f8b --- /dev/null +++ b/device.h @@ -0,0 +1,25 @@ +#ifndef __DEVICE_H__ +#define __DEVICE_H__ + +struct device; +struct cdb_assist; +struct fastboot; +struct fastboot_ops; + +struct device *device_open(const char *board, struct fastboot_ops *fastboot_ops); +int device_power_on(struct device *device); +int device_power_off(struct device *device); + +int device_enter_fastboot(struct device *device); + +void device_print_status(struct device *device); +void device_vbus(struct device *device, bool enable); +int device_write(struct device *device, const void *buf, size_t len); +void device_break(struct device *device); + +void device_set_bootimg(struct device *device, int fd); +const char *device_get_serial(struct device *device); +void device_trigger_fastboot(struct device *device, bool enable); +void device_boot(struct device *device, const void *data, size_t len); + +#endif diff --git a/fastboot.c b/fastboot.c new file mode 100644 index 0000000..6dcf397 --- /dev/null +++ b/fastboot.c @@ -0,0 +1,418 @@ +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "bad.h" +#include "fastboot.h" + +#define MAX_USBFS_BULK_SIZE (16*1024) + +struct fastboot { + const char *serial; + + int fd; + unsigned ep_in; + unsigned ep_out; + + const char *dev_path; + + void *data; + + struct fastboot_ops *ops; + + int state; + + struct udev_monitor *mon; +}; + +enum { + FASTBOOT_STATE_START, + FASTBOOT_STATE_OPENED, + FASTBOOT_STATE_CLOSED, +}; + +static int fastboot_read(struct fastboot *fb, char *buf, size_t len) +{ + struct usbdevfs_bulktransfer bulk = {0}; + char status[65]; + int n; + + for (;;) { + bulk.ep = fb->ep_in; + bulk.len = 64; + bulk.data = status; + bulk.timeout = 1000; + + n = ioctl(fb->fd, USBDEVFS_BULK, &bulk); + if (n < 0) { + warn("failed to receive usb bulk transfer"); + return -ENXIO; + } + + status[n] = '\0'; + + if (n < 4) { + warn("malformed response from fastboot"); + return -1; + } + + if (strncmp(status, "INFO", 4) == 0) { + fb->ops->info(fb, status + 4, n - 4); + } else if (strncmp(status, "OKAY", 4) == 0) { + if (buf) { + strncpy(buf, status + 4, len); + buf[len - 1] = '\0'; + } + return n - 4; + } else if (strncmp(status, "FAIL", 4) == 0) { + fb->ops->tty_write(status + 4, n - 4, true); + return -ENXIO; + } else if (strncmp(status, "DATA", 4) == 0) { + return strtol(status + 4, NULL, 16); + } + } + + return 0; +} + +static int fastboot_write(struct fastboot *fb, const void *data, size_t len) +{ + struct usbdevfs_bulktransfer bulk = {0}; + size_t count = 0; + int n; + + do { + bulk.ep = fb->ep_out; + bulk.len = MIN(len, MAX_USBFS_BULK_SIZE); + bulk.data = (void*)data; + bulk.timeout = 1000; + + n = ioctl(fb->fd, USBDEVFS_BULK, &bulk); + if (n < 0) { + warn("failed to send usb bulk transfer"); + return -1; + } + + data += n; + len -= n; + count += n; + } while (len > 0); + + return count; +} + +static int parse_usb_desc(int usbfd, unsigned *ep_in, unsigned *ep_out) +{ + const struct usb_interface_descriptor *ifc; + const struct usb_endpoint_descriptor *ept; + const struct usb_device_descriptor *dev; + const struct usb_config_descriptor *cfg; + const struct usb_descriptor_header *hdr; + unsigned type; + unsigned out; + unsigned in; + unsigned k; + unsigned l; + ssize_t n; + void *ptr; + void *end; + char desc[1024]; + int ret; + int id; + + n = read(usbfd, desc, sizeof(desc)); + if (n < 0) + return n; + + ptr = (void*)desc; + end = ptr + n; + + dev = ptr; + ptr += dev->bLength; + if (ptr >= end || dev->bDescriptorType != USB_DT_DEVICE) + return -EINVAL; + + cfg = ptr; + ptr += cfg->bLength; + if (ptr >= end || cfg->bDescriptorType != USB_DT_CONFIG) + return -EINVAL; + + for (k = 0; k < cfg->bNumInterfaces; k++) { + if (ptr >= end) + return -EINVAL; + + do { + ifc = ptr; + if (ifc->bLength < USB_DT_INTERFACE_SIZE) + return -EINVAL; + + ptr += ifc->bLength; + } while (ptr < end && ifc->bDescriptorType != USB_DT_INTERFACE); + + in = -1; + out = -1; + + for (l = 0; l < ifc->bNumEndpoints; l++) { + if (ptr >= end) + return -EINVAL; + + do { + ept = ptr; + if (ept->bLength < USB_DT_ENDPOINT_SIZE) + return -EINVAL; + + ptr += ept->bLength; + } while (ptr < end && ept->bDescriptorType != USB_DT_ENDPOINT); + + type = ept->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK; + if (type != USB_ENDPOINT_XFER_BULK) + continue; + + if (ept->bEndpointAddress & USB_DIR_IN) + in = ept->bEndpointAddress; + else + out = ept->bEndpointAddress; + + if (ptr >= end) + break; + + hdr = ptr; + if (hdr->bDescriptorType == USB_DT_SS_ENDPOINT_COMP) + ptr += USB_DT_SS_EP_COMP_SIZE; + } + + if (ifc->bInterfaceClass != 0xff) + continue; + + if (ifc->bInterfaceSubClass != 0x42) + continue; + + if (ifc->bInterfaceProtocol != 0x03) + continue; + + id = ifc->bInterfaceNumber; + ret = ioctl(usbfd, USBDEVFS_CLAIMINTERFACE, &id); + if (ret < 0) { + warn("failed to claim interface"); + continue; + } + + *ep_in = in; + *ep_out = out; + + return 0; + } + + return -ENOENT; +} + +static int handle_udev_event(int fd, void *data) +{ + struct fastboot *fastboot = data; + struct udev_device* dev; + const char *dev_path; + const char *dev_node; + const char *action; + const char *serial; + unsigned ep_out; + unsigned ep_in; + int usbfd; + int ret; + + dev = udev_monitor_receive_device(fastboot->mon); + + action = udev_device_get_action(dev); + dev_node = udev_device_get_devnode(dev); + dev_path = udev_device_get_devpath(dev); + // vid = udev_device_get_sysattr_value(dev, "idVendor"); + // pid = udev_device_get_sysattr_value(dev, "idProduct"); + serial = udev_device_get_sysattr_value(dev, "serial"); + + if (!action) + goto unref_dev; + + if (!strcmp(action, "add")) { + if (!serial || strcmp(serial, fastboot->serial)) + goto unref_dev; + + usbfd = open(dev_node, O_RDWR); + if (usbfd < 0) + goto unref_dev; + + ret = parse_usb_desc(usbfd, &ep_in, &ep_out); + if (ret < 0) { + close(usbfd); + goto unref_dev; + } + + fastboot->ep_in = ep_in; + fastboot->ep_out = ep_out; + fastboot->fd = usbfd; + fastboot->dev_path = strdup(dev_path); + + fastboot->state = FASTBOOT_STATE_OPENED; + + if (fastboot->ops && fastboot->ops->opened) + fastboot->ops->opened(fastboot, fastboot->data); + } else if (!strcmp(action, "remove")) { + if (strcmp(dev_path, fastboot->dev_path)) + goto unref_dev; + + close(fastboot->fd); + fastboot->fd = -1; + fastboot->dev_path = NULL; + + if (fastboot->ops && fastboot->ops->disconnect) + fastboot->ops->disconnect(fastboot->data); + + fastboot->state = FASTBOOT_STATE_CLOSED; + } + +unref_dev: + udev_device_unref(dev); + + return 0; +} + +struct fastboot *fastboot_open(const char *serial, struct fastboot_ops *ops, void *data) +{ + struct fastboot *fb; + struct udev* udev; + int fd; + + udev = udev_new(); + if (!udev) + err(1, "udev_new() failed"); + + fb = calloc(1, sizeof(struct fastboot)); + if (!fb) + err(1, "failed to allocate fastboot structure"); + + fb->serial = serial; + fb->ops = ops; + fb->data = data; + + fb->state = FASTBOOT_STATE_START; + + fb->mon = udev_monitor_new_from_netlink(udev, "udev"); + udev_monitor_filter_add_match_subsystem_devtype(fb->mon, "usb", NULL); + udev_monitor_enable_receiving(fb->mon); + + fd = udev_monitor_get_fd(fb->mon); + + watch_add_readfd(fd, handle_udev_event, fb); + + return fb; +} + +int fastboot_getvar(struct fastboot *fb, const char *var, char *buf, size_t len) +{ + char cmd[128]; + int n; + + n = snprintf(cmd, sizeof(cmd), "getvar:%s", var); + fastboot_write(fb, cmd, n); + + return fastboot_read(fb, buf, len); +} + +int fastboot_download(struct fastboot *fb, const void *data, size_t len) +{ + size_t xfer; + ssize_t n; + size_t offset = 0; + void *buf; + char cmd[32]; + int ret = 0; + + buf = malloc(MAX_USBFS_BULK_SIZE); + if (!buf) + err(1, "failed to allocate usb scratch buffer"); + + n = sprintf(cmd, "download:%08x", (unsigned int)len); + fastboot_write(fb, cmd, n); + + n = fastboot_read(fb, buf, MAX_USBFS_BULK_SIZE); + if (n < 0) + errx(1, "remote rejected download request"); + + while (len > 0) { + xfer = MIN(len, MAX_USBFS_BULK_SIZE); + + ret = fastboot_write(fb, data + offset, xfer); + if (ret < 0) + goto out; + + offset += xfer; + len -= xfer; + } + + ret = fastboot_read(fb, NULL, 0); + +out: + free(buf); + return ret; +} + +int fastboot_boot(struct fastboot *fb) +{ + char buf[80]; + int n; + + fastboot_write(fb, "boot", 4); + + n = fastboot_read(fb, buf, sizeof(buf)); + if (n >= 0) + printf("%s\n", buf); + + return 0; +} + +int fastboot_erase(struct fastboot *fb, const char *partition) +{ + char buf[80]; + int n; + + n = sprintf(buf, "erase:%s", partition); + fastboot_write(fb, buf, n); + + fastboot_read(fb, buf, sizeof(buf)); + + return 0; +} + +int fastboot_flash(struct fastboot *fb, const char *partition) +{ + char buf[80]; + int n; + + n = sprintf(buf, "flash:%s", partition); + fastboot_write(fb, buf, n); + + fastboot_read(fb, buf, sizeof(buf)); + + return 0; +} + +int fastboot_reboot(struct fastboot *fb) +{ + char buf[80]; + + fastboot_write(fb, "reboot", 6); + + fastboot_read(fb, buf, sizeof(buf)); + + return 0; +} diff --git a/fastboot.h b/fastboot.h new file mode 100644 index 0000000..27ecbd7 --- /dev/null +++ b/fastboot.h @@ -0,0 +1,21 @@ +#ifndef __FASTBOOT_H__ +#define __FASTBOOT_H__ + +struct fastboot; + +struct fastboot_ops { + void (*opened)(struct fastboot *, void *); + void (*disconnect)(void *); + void (*info)(struct fastboot *, const void *, size_t); + ssize_t (*tty_write)(const void *, size_t, bool); +}; + +struct fastboot *fastboot_open(const char *serial, struct fastboot_ops *ops, void *); +int fastboot_getvar(struct fastboot *fb, const char *var, char *buf, size_t len); +int fastboot_download(struct fastboot *fb, const void *data, size_t len); +int fastboot_boot(struct fastboot *fb); +int fastboot_erase(struct fastboot *fb, const char *partition); +int fastboot_flash(struct fastboot *fb, const char *partition); +int fastboot_reboot(struct fastboot *fb); + +#endif diff --git a/list.h b/list.h new file mode 100644 index 0000000..263316c --- /dev/null +++ b/list.h @@ -0,0 +1,102 @@ +/* + * Copyright (c) 2016, Linaro Ltd. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef __LIST_H__ +#define __LIST_H__ + +#include +#include + +#define container_of(ptr, type, member) ({ \ + const typeof(((type *)0)->member)*__mptr = (ptr); \ + (type *)((char *)__mptr - offsetof(type, member)); \ + }) + +struct list_head { + struct list_head *prev; + struct list_head *next; +}; + +#define LIST_INIT(list) { &(list), &(list) } + +static inline void list_init(struct list_head *list) +{ + list->prev = list->next = list; +} + +static inline bool list_empty(struct list_head *list) +{ + return list->next == list; +} + +static inline void list_add(struct list_head *list, struct list_head *item) +{ + struct list_head *prev = list->prev; + + item->next = list; + item->prev = prev; + + prev->next = list->prev = item; +} + +static inline void list_del(struct list_head *item) +{ + item->prev->next = item->next; + item->next->prev = item->prev; +} + +#define list_for_each(item, list) \ + for (item = (list)->next; item != list; item = item->next) + +#define list_for_each_safe(item, next, list) \ + for (item = (list)->next, next = item->next; item != list; item = next, next = item->next) + +#define list_entry(item, type, member) \ + container_of(item, type, member) + +#define list_entry_first(list, type, member) \ + container_of((list)->next, type, member) + +#define list_entry_next(item, member) \ + container_of((item)->member.next, typeof(*(item)), member) + +#define list_for_each_entry(item, list, member) \ + for (item = list_entry_first(list, typeof(*(item)), member); \ + &item->member != list; \ + item = list_entry_next(item, member)) + +#define list_for_each_entry_safe(item, next, list, member) \ + for (item = list_entry_first(list, typeof(*(item)), member), \ + next = list_entry_next(item, member); \ + &item->member != list; \ + item = next, \ + next = list_entry_next(item, member)) \ + +#endif