From 028df7942f5ac593e116aac0d192d8a8201e8093 Mon Sep 17 00:00:00 2001 From: laforge Date: Sat, 2 Jun 2007 13:45:46 +0000 Subject: Add 'modem alive' detection. We inquire every five minutes if the modem is still alive. git-svn-id: http://svn.openmoko.org/trunk/src/target/gsm@2130 99fdad57-331a-0410-800a-d7fa5415bdb3 --- src/gsmd/Makefile.am | 2 +- src/gsmd/gsmd.c | 137 +++++++++++++++++++++++++++++++- src/gsmd/timer.c | 217 +++++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 352 insertions(+), 4 deletions(-) create mode 100644 src/gsmd/timer.c (limited to 'src/gsmd') diff --git a/src/gsmd/Makefile.am b/src/gsmd/Makefile.am index f874fca..49d96e3 100644 --- a/src/gsmd/Makefile.am +++ b/src/gsmd/Makefile.am @@ -6,7 +6,7 @@ sbin_PROGRAMS = gsmd gsmd_CFLAGS = -D PLUGINDIR=\"$(plugindir)\" gsmd_SOURCES = gsmd.c atcmd.c select.c machine.c vendor.c unsolicited.c log.c \ - usock.c talloc.c operator_cache.c ext_response.c + usock.c talloc.c timer.c operator_cache.c ext_response.c gsmd_LDADD = -ldl gsmd_LDFLAGS = -Wl,--export-dynamic diff --git a/src/gsmd/gsmd.c b/src/gsmd/gsmd.c index 3105bee..65ce6f8 100644 --- a/src/gsmd/gsmd.c +++ b/src/gsmd/gsmd.c @@ -44,9 +44,111 @@ #include #include +#define GSMD_ALIVECMD "ATE0" +#define GSMD_ALIVE_INTERVAL 5*60 +#define GSMD_ALIVE_TIMEOUT 30 + +/* alive checking */ + +struct gsmd_alive_priv { + struct gsmd *gsmd; + int alive_responded; +}; + +static int gsmd_alive_cb(struct gsmd_atcmd *cmd, void *ctx, char *resp) +{ + struct gsmd_alive_priv *alp = ctx; + + DEBUGP("alp=%p - `%s' returned `%s'\n", alp, cmd->buf, resp); + if (!strcmp(resp, "OK")) { + DEBUGP("`%s' returned `%s': OK\n", cmd->buf, resp); + alp->alive_responded = 1; + } + return 0; +} + +static void alive_tmr_cb(struct gsmd_timer *tmr, void *data) +{ + struct gsmd_alive_priv *alp = data; + + DEBUGP("alp=%p gsmd_alive timer expired\n", alp); + + if (alp->alive_responded == 0) { + DEBUGP("modem dead!\n"); + exit(3); + } else + DEBUGP("modem alive!\n"); + + /* FIXME: update some global state */ + + gsmd_timer_free(tmr); + talloc_free(alp); +} + +static int gsmd_modem_alive(struct gsmd *gsmd) +{ + struct gsmd_atcmd *cmd; + struct gsmd_alive_priv *alp; + struct timeval tv; + + alp = talloc(gsmd_tallocs, struct gsmd_alive_priv); + if (!alp) + return -ENOMEM; + + alp->gsmd = gsmd; + alp->alive_responded = 0; + + tv.tv_sec = GSMD_ALIVE_TIMEOUT; + cmd = atcmd_fill(GSMD_ALIVECMD, strlen(GSMD_ALIVECMD)+1, + &gsmd_alive_cb, alp, 0); + if (!cmd) { + talloc_free(alp); + return -ENOMEM; + } + + tv.tv_usec = 0; + gsmd_timer_create(&tv, &alive_tmr_cb, alp); + + return atcmd_submit(gsmd, cmd); +} + +static void alive_interval_tmr_cb(struct gsmd_timer *tmr, void *data) +{ + struct gsmd *gsmd = data; + + DEBUGP("interval expired, starting next alive inquiry\n"); + + /* start a new alive check iteration */ + gsmd_modem_alive(gsmd); + + /* re-add the timer for the next interval */ + tmr->expires.tv_sec = GSMD_ALIVE_INTERVAL; + tmr->expires.tv_usec = 0; + + gsmd_timer_register(tmr); +} + +static int gmsd_alive_start(struct gsmd *gsmd) +{ + struct timeval tv; + + tv.tv_sec = GSMD_ALIVE_INTERVAL; + tv.tv_usec = 0; + + if (!gsmd_timer_create(&tv, &alive_interval_tmr_cb, gsmd)) + return -1; + + gsmd_modem_alive(gsmd); + + return 0; +} + + +/* initial startup code */ + static int gsmd_test_atcb(struct gsmd_atcmd *cmd, void *ctx, char *resp) { - printf("`%s' returned `%s'\n", cmd->buf, resp); + DEBUGP("`%s' returned `%s'\n", cmd->buf, resp); return 0; } @@ -60,7 +162,7 @@ int gsmd_simplecmd(struct gsmd *gsmd, char *cmdtxt) return atcmd_submit(gsmd, cmd); } -int gsmd_initsettings(struct gsmd *gsmd) +static int gsmd_initsettings2(struct gsmd *gsmd) { int rc; @@ -92,6 +194,28 @@ int gsmd_initsettings(struct gsmd *gsmd) return rc; } +/* we submit the first atcmd and wait synchronously for a valid response */ +static int firstcmd_atcb(struct gsmd_atcmd *cmd, void *ctx, char *resp) +{ + struct gsmd *gsmd = ctx; + DEBUGP("`%s' returned `%s'\n", cmd->buf, resp); + if (strcmp(resp, "OK")) { + fprintf(stderr, "response '%s' to initial command invalid", resp); + exit(1); + } + return gsmd_initsettings2(gsmd); +} + +int gsmd_initsettings(struct gsmd *gsmd) +{ + struct gsmd_atcmd *cmd; + cmd = atcmd_fill("ATE0V1", strlen("ATE0V1")+1, &firstcmd_atcb, gsmd, 0); + if (!cmd) + return -ENOMEM; + + return atcmd_submit(gsmd, cmd); +} + struct bdrt { int bps; u_int32_t b; @@ -195,6 +319,8 @@ static void sig_handler(int signr) break; case SIGUSR1: talloc_report_full(gsmd_tallocs, stderr); + case SIGALRM: + gsmd_timer_check_n_run(); break; } } @@ -215,6 +341,7 @@ int main(int argc, char **argv) signal(SIGINT, sig_handler); signal(SIGSEGV, sig_handler); signal(SIGUSR1, sig_handler); + signal(SIGALRM, sig_handler); gsmd_tallocs = talloc_named_const(NULL, 1, "GSMD"); @@ -285,6 +412,8 @@ int main(int argc, char **argv) exit(1); } + gsmd_timer_init(); + if (gsmd_machine_plugin_init(&g, machine_name, vendor_name) < 0) { fprintf(stderr, "no machine plugins found\n"); exit(1); @@ -326,6 +455,8 @@ int main(int argc, char **argv) if (g.interpreter_ready) gsmd_initsettings(&g); + + gmsd_alive_start(&g); gsmd_opname_init(&g); @@ -335,7 +466,7 @@ int main(int argc, char **argv) continue; if (ret < 0) { - if (errno == -EINTR) + if (errno == EINTR) continue; else { DEBUGP("select returned error (%s)\n", diff --git a/src/gsmd/timer.c b/src/gsmd/timer.c new file mode 100644 index 0000000..5200690 --- /dev/null +++ b/src/gsmd/timer.c @@ -0,0 +1,217 @@ +/* gsmd timer code + * + * (C) 2000-2005 by Harald Welte + * (C) 2007 by OpenMoko, Inc. + * Written by Harald Welte + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + +#include +#include +#include +#include +#include + +#include + +#include "gsmd.h" + +#include +#include + +static LLIST_HEAD(gsmd_timers); +static void *__tmr_ctx; + +static void tv_normalize(struct timeval *out) +{ + out->tv_sec += (out->tv_usec / 1000000); + out->tv_usec = (out->tv_usec % 1000000); +} + +/* subtract two struct timevals */ +static int tv_sub(struct timeval *res, const struct timeval *from, + const struct timeval *sub) +{ + res->tv_sec = from->tv_sec - sub->tv_sec; + res->tv_usec = from->tv_usec - sub->tv_usec; + + while (res->tv_usec < 0) { + res->tv_sec -= 1; + res->tv_usec += 1000000; + } + + return 0; +} + +static int tv_add(struct timeval *res, const struct timeval *a1, + const struct timeval *a2) +{ + unsigned int carry; + + res->tv_sec = a1->tv_sec + a2->tv_sec; + res->tv_usec = a1->tv_usec + a2->tv_usec; + + tv_normalize(res); +} + +static int tv_later(const struct timeval *expires, const struct timeval *now) +{ + if (expires->tv_sec < now->tv_sec) + return 0; + else if (expires->tv_sec > now->tv_sec) + return 1; + else /* if (expires->tv_sec == now->tv_sec */ { + if (expires->tv_usec >= now->tv_usec) + return 1; + } + + return 0; +} + +static int tv_smaller(const struct timeval *t1, const struct timeval *t2) +{ + return tv_later(t2, t1); +} + +static int calc_next_expiration(void) +{ + struct gsmd_timer *cur; + struct timeval min, now, diff; + struct itimerval iti; + int ret; + + gettimeofday(&now, NULL); + +retry: + if (llist_empty(&gsmd_timers)) + return 0; + + llist_for_each_entry(cur, &gsmd_timers, list) { + if (gsmd_timers.next == &cur->list) + min = cur->expires; + + if (tv_smaller(&cur->expires, &min)) + min = cur->expires; + } + + if (tv_sub(&diff, &min, &now) < 0) { + /* FIXME: run expired timer callbacks */ + /* we cannot run timers from here since we might be + * called from register_timer() within check_n_run() */ + + /* FIXME: restart with next minimum timer */ + goto retry; + } + + /* re-set kernel timer */ + memset(&iti, 0, sizeof(iti)); + memcpy(&iti.it_value, &diff, sizeof(iti.it_value)); + ret = setitimer(ITIMER_REAL, &iti, NULL); + if (ret < 0) + return ret; + + return 0; +} + +void gsmd_timer_check_n_run(void) +{ + struct gsmd_timer *cur, *cur2; + struct timeval now; + + if (gettimeofday(&now, NULL) < 0) + return; + + llist_for_each_entry_safe(cur, cur2, &gsmd_timers, list) { + if (tv_later(&now, &cur->expires)) { + /* fist delete it from the list of timers */ + llist_del(&cur->list); + /* then call. called function can re-add it */ + (cur->cb)(cur, cur->data); + } + } + + calc_next_expiration(); +} + +int gsmd_timer_init(void) +{ + __tmr_ctx = talloc_named_const(gsmd_tallocs, 1, "timers"); + + return 0; +} + +struct gsmd_timer *gsmd_timer_alloc(void) +{ + struct gsmd_timer *tmr; + + tmr = talloc_size(__tmr_ctx, sizeof(*tmr)); + return tmr; +} + +int gsmd_timer_register(struct gsmd_timer *timer) +{ + int ret; + struct timeval tv; + + ret = gettimeofday(&tv, NULL); + if (ret < 0) + return ret; + + /* convert expiration time into absoulte time */ + timer->expires.tv_sec += tv.tv_sec; + timer->expires.tv_usec += tv.tv_usec; + + llist_add_tail(&timer->list, &gsmd_timers); + + /* re-calculate next expiration */ + calc_next_expiration(); + + return 0; +} + +struct gsmd_timer *gsmd_timer_create(struct timeval *expires, + void (*cb)(struct gsmd_timer *tmr, + void *data), + void *data) +{ + struct gsmd_timer *tmr = gsmd_timer_alloc(); + int rc; + + if (!tmr) + return NULL; + + memcpy(&tmr->expires, expires, sizeof(tmr->expires)); + tmr->cb = cb; + tmr->data = data; + + rc = gsmd_timer_register(tmr); + if (rc < 0) { + talloc_free(tmr); + return NULL; + } + + return tmr; +} + +void gsmd_timer_unregister(struct gsmd_timer *timer) +{ + llist_del(&timer->list); + + /* re-calculate next expiration */ + calc_next_expiration(); +} -- cgit v1.2.3