diff --git a/callouts/77-nm-probe-modem-capabilities.rules b/callouts/77-nm-probe-modem-capabilities.rules index e1d702ed34..261a5351d3 100644 --- a/callouts/77-nm-probe-modem-capabilities.rules +++ b/callouts/77-nm-probe-modem-capabilities.rules @@ -4,7 +4,7 @@ ACTION!="add|change", GOTO="nm_modem_probe_end" SUBSYSTEM!="tty", GOTO="nm_modem_probe_end" KERNEL!="tty*", GOTO="nm_modem_probe_end" -DRIVERS=="option|sierra|hso|serial_cs|cdc_acm", IMPORT{program}="nm-modem-probe --delay 2000 --export $tempnode" +DRIVERS=="option|sierra|hso|serial_cs|cdc_acm", IMPORT{program}="nm-modem-probe --delay 3000 --export $tempnode" LABEL="nm_modem_probe_end" diff --git a/callouts/nm-modem-probe.c b/callouts/nm-modem-probe.c index c7d5b11574..fd77089d93 100644 --- a/callouts/nm-modem-probe.c +++ b/callouts/nm-modem-probe.c @@ -281,22 +281,88 @@ parse_gmm (const char *buf) return gsm ? MODEM_CAP_GSM : 0; } -static int modem_probe_caps(int fd) +static int +g_timeval_subtract (GTimeVal *result, GTimeVal *x, GTimeVal *y) +{ + int nsec; + + /* Perform the carry for the later subtraction by updating y. */ + if (x->tv_usec < y->tv_usec) { + nsec = (y->tv_usec - x->tv_usec) / G_USEC_PER_SEC + 1; + y->tv_usec -= G_USEC_PER_SEC * nsec; + y->tv_sec += nsec; + } + if (x->tv_usec - y->tv_usec > G_USEC_PER_SEC) { + nsec = (x->tv_usec - y->tv_usec) / G_USEC_PER_SEC; + y->tv_usec += G_USEC_PER_SEC * nsec; + y->tv_sec -= nsec; + } + + /* Compute the time remaining to wait. + tv_usec is certainly positive. */ + result->tv_sec = x->tv_sec - y->tv_sec; + result->tv_usec = x->tv_usec - y->tv_usec; + + /* Return 1 if result is negative. */ + return x->tv_sec < y->tv_sec; +} + +static int modem_probe_caps(int fd, glong timeout_ms) { const char *gcap_responses[] = { GCAP_TAG, NULL }; const char *terminators[] = { "OK", "ERROR", "ERR", NULL }; char *reply = NULL; - int idx, term_idx, ret = 0; + int idx = -1, term_idx = -1, ret = 0; + gboolean try_ati = FALSE; + GTimeVal start, end; + gboolean send_success; - if (!modem_send_command (fd, "AT+GCAP\r\n")) - return -1; + /* If a delay was specified, start a bit later */ + if (timeout_ms > 500) { + g_usleep (500000); + timeout_ms -= 500; + } - idx = modem_wait_reply (fd, 3, gcap_responses, terminators, &term_idx, &reply); - if (0 == term_idx && 0 == idx) { - /* Success */ - verbose ("GCAP response: %s", reply); - ret = parse_gcap (reply); - } else if (1 == term_idx || 2 == term_idx) { + /* Standard response timeout case */ + timeout_ms += 3000; + + while (timeout_ms > 0) { + GTimeVal diff; + + g_get_current_time (&start); + + idx = term_idx = 0; + send_success = modem_send_command (fd, "AT+GCAP\r\n"); + if (send_success) + idx = modem_wait_reply (fd, 2, gcap_responses, terminators, &term_idx, &reply); + + g_get_current_time (&end); + g_timeval_subtract (&diff, &end, &start); + timeout_ms -= (diff.tv_sec * 1000) + (diff.tv_usec / 1000); + + if (!send_success) + continue; + + if (0 == term_idx && 0 == idx) { + /* Success */ + verbose ("GCAP response: %s", reply); + ret = parse_gcap (reply); + break; + } else if (0 == term_idx && -1 == idx) { + /* Just returned "OK" but no GCAP (Sierra) */ + try_ati = TRUE; + break; + } else if (1 == term_idx || 2 == term_idx) { + try_ati = TRUE; + } else + verbose ("timed out waiting for GCAP reply (idx %d, term_idx %d)", idx, term_idx); + g_free (reply); + reply = NULL; + g_usleep (100000); + timeout_ms -= 100; + } + + if (!ret && try_ati) { const char *ati_responses[] = { GCAP_TAG, NULL }; /* Many cards (ex Sierra 860 & 875) won't accept AT+GCAP but @@ -314,8 +380,7 @@ static int modem_probe_caps(int fd) ret = parse_gcap (reply); } } - } else - verbose ("timed out waiting for GCAP reply (idx %d, term_idx %d)", idx, term_idx); + } g_free (reply); reply = NULL; @@ -441,11 +506,6 @@ main(int argc, char *argv[]) delay_ms = (guint32) tmp; } - if (delay_ms) { - verbose ("waiting %ums before probing", delay_ms); - g_usleep (delay_ms * 1000); - } - fd = open (device, O_RDWR | O_EXCL | O_NONBLOCK); if (fd < 0) { g_printerr ("open(%s) failed: %d\n", device, errno); @@ -470,7 +530,7 @@ main(int argc, char *argv[]) attrs.c_cflag |= (B9600 | CS8 | CREAD | PARENB); tcsetattr (fd, TCSANOW, &attrs); - caps = modem_probe_caps (fd); + caps = modem_probe_caps (fd, delay_ms); tcsetattr (fd, TCSANOW, &orig); if (caps < 0) {