diff --git a/qcomlt_dbg.c b/qcomlt_dbg.c index bda7e61..64cfc2c 100644 --- a/qcomlt_dbg.c +++ b/qcomlt_dbg.c @@ -44,10 +44,25 @@ #include "cdba-server.h" #include "device.h" +#include "status.h" + +enum qcomlt_parse_state { + STATE_, + STATE_num, + STATE_num_m, + STATE_num_mV, + STATE_num_mV_num, + STATE_num_mV_num_m, + STATE_err, +}; struct qcomlt_dbg { int fd; struct termios orig_tios; + + enum qcomlt_parse_state parse_state; + unsigned long mv; + unsigned long ma; }; static void *qcomlt_dbg_open(struct device *dev) @@ -100,9 +115,124 @@ static void qcomlt_dbg_key(struct device *dev, int key, bool asserted) } } +static int qcomlt_dbg_ctrl_data(int fd, void *data) +{ + struct qcomlt_dbg *dbg = data; + struct status_value dc[] = { + { + .unit = STATUS_MV, + }, + { + .unit = STATUS_MA, + }, + {} + }; + char buf[64]; + ssize_t i; + ssize_t n; + char ch; + + n = read(fd, buf, sizeof(buf)); + if (n < 0) + return n; + + for (i = 0; i < n; i++) { + ch = buf[i]; + + /* + * The control data consists of a stream in the format: + * mV mA + * + * The stream might be split in arbitrary ways across reads, so + * a parser is used instead of sscanf(). + * In the initial state any non-digits are ignored, if a parse + * error occurs thereafter all characters until 'A' are + * dropped, with the result that any unexpected control + * messages are ignored. + */ + switch (dbg->parse_state) { + case STATE_: + if (isdigit(ch)) { + dbg->mv = ch - '0'; + dbg->parse_state = STATE_num; + } + break; + case STATE_num: + if (isdigit(ch)) { + dbg->mv *= 10; + dbg->mv += ch - '0'; + } else if (ch == 'm') { + dbg->parse_state = STATE_num_m; + } else { + dbg->parse_state = STATE_err; + } + break; + case STATE_num_m: + if (ch == 'V') + dbg->parse_state = STATE_num_mV; + else + dbg->parse_state = STATE_err; + break; + case STATE_num_mV: + if (isdigit(ch)) { + dbg->ma = ch - '0'; + dbg->parse_state = STATE_num_mV_num; + } else if (!isspace(ch)) { + dbg->parse_state = STATE_err; + } + break; + case STATE_num_mV_num: + if (isdigit(ch)) { + dbg->ma *= 10; + dbg->ma += ch - '0'; + } else if (ch == 'm') { + dbg->parse_state = STATE_num_mV_num_m; + } else { + dbg->parse_state = STATE_err; + } + break; + case STATE_num_mV_num_m: + if (ch == 'A') { + /* Parser found a match, report it */ + dc[0].value = dbg->mv; + dc[1].value = dbg->ma; + + status_send_values("dc", dc); + } else { + dbg->parse_state = STATE_err; + } + break; + case STATE_err: + if (ch == 'A') + dbg->parse_state = STATE_; + break; + } + } + + return 0; +} + +static void qcomlt_dbg_request_status(void *data) +{ + struct qcomlt_dbg *dbg = data; + + write(dbg->fd, "s", 1); + + watch_timer_add(200, qcomlt_dbg_request_status, dbg); +} + +static void qcomlt_dbg_status_enable(struct device *dev) +{ + struct qcomlt_dbg *dbg = dev->cdb; + + watch_add_readfd(dbg->fd, qcomlt_dbg_ctrl_data, dbg); + watch_timer_add(200, qcomlt_dbg_request_status, dbg); +} + const struct control_ops qcomlt_dbg_ops = { .open = qcomlt_dbg_open, .power = qcomlt_dbg_power, .usb = qcomlt_dbg_usb, .key = qcomlt_dbg_key, + .status_enable = qcomlt_dbg_status_enable, };