code.vuplus.com Git - vuplus_openembedded/blob - recipes/gsm/files/0001-Introduce-ports.patch

   1 From 516d67c679101d1503dbd4c0613bcd6ff1b604e4 Mon Sep 17 00:00:00 2001
   2 From: Andrzej Zaborowski <balrog@zabor.org>
   3 Date: Wed, 19 Sep 2007 14:03:28 +0200
   4 Subject: [PATCH] Introduce ports.
   5
   6 ---
   7  include/gsmd/atcmd.h        |    2 +-
   8  include/gsmd/gsmd.h         |    7 +-
   9  include/gsmd/uart.h         |   28 ++++++
  10  include/gsmd/vendorplugin.h |    4 +-
  11  src/gsmd/Makefile.am        |    2 +-
  12  src/gsmd/atcmd.c            |  177 +++++++++++++++++---------------------
  13  src/gsmd/gsmd.c             |   64 ++------------
  14  src/gsmd/uart.c             |  202 +++++++++++++++++++++++++++++++++++++++++++
  15  8 files changed, 328 insertions(+), 158 deletions(-)
  16  create mode 100644 include/gsmd/uart.h
  17  create mode 100644 src/gsmd/uart.c
  18
  19 diff --git a/include/gsmd/atcmd.h b/include/gsmd/atcmd.h
  20 index 0d6c62a..a1af6a0 100644
  21 --- a/include/gsmd/atcmd.h
  22 +++ b/include/gsmd/atcmd.h
  23 @@ -9,7 +9,7 @@ typedef int atcmd_cb_t(struct gsmd_atcmd *cmd, void *ctx, char *resp);
  24
  25  extern struct gsmd_atcmd *atcmd_fill(const char *cmd, int rlen, atcmd_cb_t *cb, void *ctx, u_int16_t id);
  26  extern int atcmd_submit(struct gsmd *g, struct gsmd_atcmd *cmd);
  27 -extern int atcmd_init(struct gsmd *g, int sockfd);
  28 +extern int atcmd_init(struct gsmd *g, struct gsmd_port *port);
  29  extern void atcmd_drain(int fd);
  30
  31  #endif /* __GSMD__ */
  32 diff --git a/include/gsmd/gsmd.h b/include/gsmd/gsmd.h
  33 index ed334f1..4afdf66 100644
  34 --- a/include/gsmd/gsmd.h
  35 +++ b/include/gsmd/gsmd.h
  36 @@ -10,6 +10,7 @@
  37  #include <gsmd/machineplugin.h>
  38  #include <gsmd/vendorplugin.h>
  39  #include <gsmd/select.h>
  40 +#include <gsmd/uart.h>
  41  #include <gsmd/state.h>
  42
  43  void *gsmd_tallocs;
  44 @@ -52,6 +53,7 @@ enum llparse_state {
  45  #define MLPARSE_BUF_SIZE       65535
  46
  47  struct llparser {
  48 +       struct gsmd_port *port;
  49         enum llparse_state state;
  50         unsigned int len;
  51         unsigned int flags;
  52 @@ -70,7 +72,7 @@ struct gsmd;
  53  struct gsmd {
  54         unsigned int flags;
  55         int interpreter_ready;
  56 -       struct gsmd_fd gfd_uart;
  57 +       struct gsmd_uart uart;
  58         struct gsmd_fd gfd_sock;
  59         struct llparser llp;
  60         struct llist_head users;
  61 @@ -81,9 +83,10 @@ struct gsmd {
  62         struct gsmd_device_state dev_state;
  63
  64         struct llist_head operators;            /* cached list of operator names */
  65 -       unsigned char *mlbuf;           /* ml_parse buffer */
  66 +       char *mlbuf;                    /* ml_parse buffer */
  67         unsigned int mlbuf_len;
  68         int mlunsolicited;
  69 +       int clear_to_send;
  70  };
  71
  72  struct gsmd_user {
  73 diff --git a/include/gsmd/uart.h b/include/gsmd/uart.h
  74 new file mode 100644
  75 index 0000000..a006fa7
  76 --- /dev/null
  77 +++ b/include/gsmd/uart.h
  78 @@ -0,0 +1,28 @@
  79 +#ifndef __GSMD_UART_H
  80 +#define __GSMD_UART_H
  81 +
  82 +#ifdef __GSMD__
  83 +
  84 +struct gsmd_port {
  85 +       int (*write)(struct gsmd_port *port, const char data[], int len);
  86 +       int (*set_break)(struct gsmd_port *port, int state);
  87 +       /* more parameters here */
  88 +       int (*newdata_cb)(void *opaque, const char data[], int len);
  89 +       void *newdata_opaque;
  90 +};
  91 +
  92 +struct gsmd_uart {
  93 +       struct gsmd_port port;
  94 +       struct gsmd_fd gfd;
  95 +       char txfifo[2048];
  96 +       int tx_start;
  97 +       int tx_len;
  98 +};
  99 +
 100 +extern int set_baudrate(int fd, int baudrate, int hwflow);
 101 +extern void uart_drain(int fd);
 102 +extern int uart_init(struct gsmd_uart *uart, int sockfd);
 103 +
 104 +#endif /* __GSMD__ */
 105 +
 106 +#endif
 107 diff --git a/include/gsmd/vendorplugin.h b/include/gsmd/vendorplugin.h
 108 index 1911fef..1c82790 100644
 109 --- a/include/gsmd/vendorplugin.h
 110 +++ b/include/gsmd/vendorplugin.h
 111 @@ -11,8 +11,8 @@ struct gsmd_unsolicit;
 112
 113  struct gsmd_vendor_plugin {
 114         struct llist_head list;
 115 -       unsigned char *name;
 116 -       unsigned char *ext_chars;
 117 +       char *name;
 118 +       char *ext_chars;
 119         unsigned int num_unsolicit;
 120         const struct gsmd_unsolicit *unsolicit;
 121         int (*detect)(struct gsmd *g);
 122 diff --git a/src/gsmd/Makefile.am b/src/gsmd/Makefile.am
 123 index 9ac45ee..110b757 100644
 124 --- a/src/gsmd/Makefile.am
 125 +++ b/src/gsmd/Makefile.am
 126 @@ -13,7 +13,7 @@ sbin_PROGRAMS = gsmd
 127  gsmd_CFLAGS = -D PLUGINDIR=\"$(plugindir)\"
 128  gsmd_SOURCES = gsmd.c atcmd.c select.c machine.c vendor.c unsolicited.c log.c \
 129                usock.c talloc.c timer.c operator_cache.c ext_response.c \
 130 -              sms_cb.c sms_pdu.c
 131 +              sms_cb.c sms_pdu.c uart.c
 132  gsmd_LDADD = -ldl
 133  gsmd_LDFLAGS = -Wl,--export-dynamic
 134
 135 diff --git a/src/gsmd/atcmd.c b/src/gsmd/atcmd.c
 136 index 2ef6a10..27dfa41 100644
 137 --- a/src/gsmd/atcmd.c
 138 +++ b/src/gsmd/atcmd.c
 139 @@ -159,7 +159,8 @@ static int llparse_byte(struct llparser *llp, char byte)
 140         return ret;
 141  }
 142
 143 -static int llparse_string(struct llparser *llp, char *buf, unsigned int len)
 144 +static int llparse_string(struct llparser *llp, const char *buf,
 145 +               unsigned int len)
 146  {
 147         while (len--) {
 148                 int rc = llparse_byte(llp, *(buf++));
 149 @@ -187,6 +188,55 @@ static int llparse_init(struct llparser *llp)
 150         return 0;
 151  }
 152
 153 +/* See if we can now send more commands to the port */
 154 +static void atcmd_wake_queue(struct gsmd *g)
 155 +{
 156 +       int len, rc;
 157 +       char *cr;
 158 +
 159 +       /* write pending commands to UART */
 160 +       while (g->interpreter_ready && g->clear_to_send) {
 161 +               struct gsmd_atcmd *pos, *pos2;
 162 +               llist_for_each_entry_safe(pos, pos2, &g->pending_atcmds, list) {
 163 +                       cr = strchr(pos->cur, '\n');
 164 +                       if (cr)
 165 +                               len = cr - pos->cur;
 166 +                       else
 167 +                               len = pos->buflen;
 168 +                       rc = g->llp.port->write(g->llp.port, pos->cur, len);
 169 +                       if (rc == 0) {
 170 +                               gsmd_log(GSMD_ERROR,
 171 +                                               "write returns 0, aborting\n");
 172 +                               break;
 173 +                       }
 174 +                       if (cr && rc == len)
 175 +                               rc ++;  /* Skip the \n */
 176 +                       pos->buflen -= rc;
 177 +                       pos->cur += rc;
 178 +                       g->llp.port->write(g->llp.port, "\r", 1);
 179 +
 180 +                       if (!pos->buflen) {
 181 +                               /* success: remove from global list of
 182 +                                * to-be-sent atcmds */
 183 +                               llist_del(&pos->list);
 184 +                               /* append to global list of executing atcmds */
 185 +                               llist_add_tail(&pos->list, &g->busy_atcmds);
 186 +
 187 +                               /* we only send one cmd at the moment */
 188 +                               g->clear_to_send = 0;
 189 +                               break;
 190 +                       } else {
 191 +                               /* The write was short or the atcmd has more
 192 +                                * lines to send after a "> ".  */
 193 +                               if (rc < len)
 194 +                                       break;
 195 +                               g->clear_to_send = 0;
 196 +                               break;
 197 +                       }
 198 +               }
 199 +       }
 200 +}
 201 +
 202  /* mid-level parser */
 203
 204  static int parse_final_result(const char *res)
 205 @@ -216,6 +266,7 @@ static int ml_parse(const char *buf, int len, void *ctx)
 206                 g->interpreter_ready = 1;
 207                 gsmd_initsettings(g);
 208                 gmsd_alive_start(g);
 209 +               atcmd_wake_queue(g);
 210                 return 0;
 211         }
 212
 213 @@ -316,6 +367,7 @@ static int ml_parse(const char *buf, int len, void *ctx)
 214                                 } else {
 215                                         DEBUGP("Calling cmd->cb()\n");
 216                                         cmd->resp = g->mlbuf;
 217 +                                       g->mlbuf[g->mlbuf_len] = 0;
 218                                         rc = cmd->cb(cmd, cmd->ctx, cmd->resp);
 219                                         DEBUGP("Clearing mlbuf\n");
 220                                 }
 221 @@ -370,12 +422,15 @@ static int ml_parse(const char *buf, int len, void *ctx)
 222         if (g->mlbuf_len)
 223                 g->mlbuf[g->mlbuf_len ++] = '\n';
 224         DEBUGP("Appending buf to mlbuf\n");
 225 -       if (len > MLPARSE_BUF_SIZE - g->mlbuf_len)
 226 +       if (len > MLPARSE_BUF_SIZE - g->mlbuf_len) {
 227                 len = MLPARSE_BUF_SIZE - g->mlbuf_len;
 228 +               gsmd_log(GSMD_NOTICE, "g->mlbuf overrun\n");
 229 +       }
 230         memcpy(g->mlbuf + g->mlbuf_len, buf, len);
 231         g->mlbuf_len += len;
 232
 233         if (g->mlunsolicited) {
 234 +               g->mlbuf[g->mlbuf_len] = 0;
 235                 rc = unsolicited_parse(g, g->mlbuf, g->mlbuf_len,
 236                                 strchr(g->mlbuf, ':') + 1);
 237                 if (rc == -EAGAIN) {
 238 @@ -422,8 +477,11 @@ final_cb:
 239
 240         /* if we're finished with current commands, but still have pending
 241          * commands: we want to WRITE again */
 242 -       if (llist_empty(&g->busy_atcmds) && !llist_empty(&g->pending_atcmds))
 243 -               g->gfd_uart.when |= GSMD_FD_WRITE;
 244 +       if (llist_empty(&g->busy_atcmds)) {
 245 +               g->clear_to_send = 1;
 246 +               if (!llist_empty(&g->pending_atcmds))
 247 +                       atcmd_wake_queue(g);
 248 +       }
 249
 250         return rc;
 251  }
 252 @@ -433,85 +491,23 @@ static int atcmd_prompt(void *data)
 253  {
 254         struct gsmd *g = data;
 255
 256 -       g->gfd_uart.when |= GSMD_FD_WRITE;
 257 +       g->clear_to_send = 1;
 258 +       atcmd_wake_queue(g);
 259  }
 260
 261  /* callback to be called if [virtual] UART has some data for us */
 262 -static int atcmd_select_cb(int fd, unsigned int what, void *data)
 263 +static int atcmd_newdata_cb(void *opaque, const char data[], int len)
 264  {
 265 -       int len, rc;
 266 -       static char rxbuf[1024];
 267 -       struct gsmd *g = data;
 268 -       char *cr;
 269 -
 270 -       if (what & GSMD_FD_READ) {
 271 -               memset(rxbuf, 0, sizeof(rxbuf));
 272 -               while ((len = read(fd, rxbuf, sizeof(rxbuf)))) {
 273 -                       if (len < 0) {
 274 -                               if (errno == EAGAIN)
 275 -                                       return 0;
 276 -                               gsmd_log(GSMD_NOTICE, "ERROR reading from fd %u: %d (%s)\n", fd, len,
 277 -                                       strerror(errno));
 278 -                                       return len;
 279 -                       }
 280 -                       rc = llparse_string(&g->llp, rxbuf, len);
 281 -                       if (rc < 0) {
 282 -                               gsmd_log(GSMD_ERROR, "ERROR during llparse_string: %d\n", rc);
 283 -                               return rc;
 284 -                       }
 285 -               }
 286 -       }
 287 -
 288 -       /* write pending commands to UART */
 289 -       if ((what & GSMD_FD_WRITE) && g->interpreter_ready) {
 290 -               struct gsmd_atcmd *pos, *pos2;
 291 -               llist_for_each_entry_safe(pos, pos2, &g->pending_atcmds, list) {
 292 -                       cr = strchr(pos->cur, '\n');
 293 -                       if (cr)
 294 -                               len = cr - pos->cur;
 295 -                       else
 296 -                               len = pos->buflen - 1;  /* assuming zero-terminated strings */
 297 -                       rc = write(fd, pos->cur, len);
 298 -                       if (rc == 0) {
 299 -                               gsmd_log(GSMD_ERROR, "write returns 0, aborting\n");
 300 -                               break;
 301 -                       } else if (rc < 0) {
 302 -                               gsmd_log(GSMD_ERROR, "error during write to fd %d: %d\n",
 303 -                                       fd, rc);
 304 -                               return rc;
 305 -                       }
 306 -                       if (!cr || rc == len)
 307 -                               rc ++;  /* Skip the \n or \0 */
 308 -                       pos->buflen -= rc;
 309 -                       pos->cur += rc;
 310 -                       write(fd, "\r", 1);
 311 -
 312 -                       if (!pos->buflen) {
 313 -                               /* success: remove from global list of
 314 -                                * to-be-sent atcmds */
 315 -                               llist_del(&pos->list);
 316 -                               /* append to global list of executing atcmds */
 317 -                               llist_add_tail(&pos->list, &g->busy_atcmds);
 318 -
 319 -                               /* we only send one cmd at the moment */
 320 -                               break;
 321 -                       } else {
 322 -                               /* The write was short or the atcmd has more
 323 -                                * lines to send after a "> ".  */
 324 -                               if (rc < len)
 325 -                                       return 0;
 326 -                               break;
 327 -                       }
 328 -               }
 329 +       struct gsmd *g = opaque;
 330 +       int rc;
 331
 332 -               /* Either pending_atcmds is empty or a command has to wait */
 333 -               g->gfd_uart.when &= ~GSMD_FD_WRITE;
 334 -       }
 335 +       rc = llparse_string(&g->llp, data, len);
 336 +       if (rc < 0)
 337 +               gsmd_log(GSMD_ERROR, "ERROR during llparse_string: %d\n", rc);
 338
 339 -       return 0;
 340 +       return rc;
 341  }
 342
 343 -
 344  struct gsmd_atcmd *atcmd_fill(const char *cmd, int rlen,
 345                               atcmd_cb_t cb, void *ctx, u_int16_t id)
 346  {
 347 @@ -544,36 +540,18 @@ int atcmd_submit(struct gsmd *g, struct gsmd_atcmd *cmd)
 348  {
 349         DEBUGP("submitting command `%s'\n", cmd->buf);
 350
 351 -       if (llist_empty(&g->pending_atcmds))
 352 -               g->gfd_uart.when |= GSMD_FD_WRITE;
 353 +       llist_empty(&g->pending_atcmds);
 354         llist_add_tail(&cmd->list, &g->pending_atcmds);
 355 +       atcmd_wake_queue(g);
 356
 357         return 0;
 358  }
 359
 360 -void atcmd_drain(int fd)
 361 -{
 362 -       int rc;
 363 -       struct termios t;
 364 -       rc = tcflush(fd, TCIOFLUSH);
 365 -       rc = tcgetattr(fd, &t);
 366 -       DEBUGP("c_iflag = 0x%08x, c_oflag = 0x%08x, c_cflag = 0x%08x, c_lflag = 0x%08x\n",
 367 -               t.c_iflag, t.c_oflag, t.c_cflag, t.c_lflag);
 368 -       t.c_iflag = t.c_oflag = 0;
 369 -       cfmakeraw(&t);
 370 -       rc = tcsetattr(fd, TCSANOW, &t);
 371 -}
 372 -
 373  /* init atcmd parser */
 374 -int atcmd_init(struct gsmd *g, int sockfd)
 375 +int atcmd_init(struct gsmd *g, struct gsmd_port *port)
 376  {
 377         __atcmd_ctx = talloc_named_const(gsmd_tallocs, 1, "atcmds");
 378
 379 -       g->gfd_uart.fd = sockfd;
 380 -       g->gfd_uart.when = GSMD_FD_READ;
 381 -       g->gfd_uart.data = g;
 382 -       g->gfd_uart.cb = &atcmd_select_cb;
 383 -
 384         INIT_LLIST_HEAD(&g->pending_atcmds);
 385         INIT_LLIST_HEAD(&g->busy_atcmds);
 386
 387 @@ -581,7 +559,9 @@ int atcmd_init(struct gsmd *g, int sockfd)
 388
 389         g->mlbuf_len = 0;
 390         g->mlunsolicited = 0;
 391 +       g->clear_to_send = 1;
 392
 393 +       g->llp.port = port;
 394         g->llp.cur = g->llp.buf;
 395         g->llp.len = sizeof(g->llp.buf);
 396         g->llp.cb = &ml_parse;
 397 @@ -589,5 +569,8 @@ int atcmd_init(struct gsmd *g, int sockfd)
 398         g->llp.ctx = g;
 399         g->llp.flags = LGSM_ATCMD_F_EXTENDED;
 400
 401 -       return gsmd_register_fd(&g->gfd_uart);
 402 +       port->newdata_opaque = g;
 403 +       port->newdata_cb = atcmd_newdata_cb;
 404 +
 405 +       return 0;
 406  }
 407 diff --git a/src/gsmd/gsmd.c b/src/gsmd/gsmd.c
 408 index 51b4f2c..846bd17 100644
 409 --- a/src/gsmd/gsmd.c
 410 +++ b/src/gsmd/gsmd.c
 411 @@ -26,7 +26,6 @@
 412  #include <string.h>
 413  #include <errno.h>
 414  #include <fcntl.h>
 415 -#include <termios.h>
 416  #include <signal.h>
 417
 418  #define _GNU_SOURCE
 419 @@ -247,56 +246,6 @@ int gsmd_initsettings(struct gsmd *gsmd)
 420         return atcmd_submit(gsmd, cmd);
 421  }
 422
 423 -struct bdrt {
 424 -       int bps;
 425 -       u_int32_t b;
 426 -};
 427 -
 428 -static struct bdrt bdrts[] = {
 429 -       { 0, B0 },
 430 -       { 9600, B9600 },
 431 -       { 19200, B19200 },
 432 -       { 38400, B38400 },
 433 -       { 57600, B57600 },
 434 -       { 115200, B115200 },
 435 -       { 230400, B230400 },
 436 -       { 460800, B460800 },
 437 -       { 921600, B921600 },
 438 -};
 439 -
 440 -static int set_baudrate(int fd, int baudrate, int hwflow)
 441 -{
 442 -       int i;
 443 -       u_int32_t bd = 0;
 444 -       struct termios ti;
 445 -
 446 -       for (i = 0; i < ARRAY_SIZE(bdrts); i++) {
 447 -               if (bdrts[i].bps == baudrate)
 448 -                       bd = bdrts[i].b;
 449 -       }
 450 -       if (bd == 0)
 451 -               return -EINVAL;
 452 -
 453 -       i = tcgetattr(fd, &ti);
 454 -       if (i < 0)
 455 -               return i;
 456 -
 457 -       i = cfsetispeed(&ti, B0);
 458 -       if (i < 0)
 459 -               return i;
 460 -
 461 -       i = cfsetospeed(&ti, bd);
 462 -       if (i < 0)
 463 -               return i;
 464 -
 465 -       if (hwflow)
 466 -               ti.c_cflag |= CRTSCTS;
 467 -       else
 468 -               ti.c_cflag &= ~CRTSCTS;
 469 -
 470 -       return tcsetattr(fd, 0, &ti);
 471 -}
 472 -
 473  static int gsmd_initialize(struct gsmd *g)
 474  {
 475         INIT_LLIST_HEAD(&g->users);
 476 @@ -478,14 +427,19 @@ int main(int argc, char **argv)
 477         if (wait >= 0)
 478                 g.interpreter_ready = !wait;
 479
 480 -       if (atcmd_init(&g, fd) < 0) {
 481 +       if (uart_init(&g.uart, fd) < 0) {
 482                 fprintf(stderr, "can't initialize UART device\n");
 483                 exit(1);
 484         }
 485
 486 -  write(fd, "\r", 1);
 487 -  sleep(1);
 488 -       atcmd_drain(fd);
 489 +       if (atcmd_init(&g, &g.uart.port) < 0) {
 490 +               fprintf(stderr, "can't initialize AT parser\n");
 491 +               exit(1);
 492 +       }
 493 +       write(fd, "\r", 1);
 494 +       sleep(1);
 495 +
 496 +       uart_drain(fd);
 497
 498         if (usock_init(&g) < 0) {
 499                 fprintf(stderr, "can't open unix socket\n");
 500 diff --git a/src/gsmd/uart.c b/src/gsmd/uart.c
 501 new file mode 100644
 502 index 0000000..22a4a5c
 503 --- /dev/null
 504 +++ b/src/gsmd/uart.c
 505 @@ -0,0 +1,202 @@
 506 +/* Wrapper for the physical UART in a struct gsmd_port abstraction.
 507 + *
 508 + * Copyright (C) 2007 Openmoko, Inc.
 509 + * Written by Andrzej Zaborowski <andrew@openedhand.com>
 510 + *
 511 + * This program is free software; you can redistribute it and/or
 512 + * modify it under the terms of the GNU General Public License as
 513 + * published by the Free Software Foundation; either version 2 of
 514 + * the License, or (at your option) any later version.
 515 + *
 516 + * This program is distributed in the hope that it will be useful,
 517 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
 518 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 519 + * GNU General Public License for more details.
 520 + *
 521 + * You should have received a copy of the GNU General Public License
 522 + * along with this program; if not, write to the Free Software
 523 + * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 524 + * MA 02111-1307 USA
 525 + */
 526 +
 527 +#include <string.h>
 528 +#include <fcntl.h>
 529 +#include <termios.h>
 530 +#include <unistd.h>
 531 +#include <errno.h>
 532 +
 533 +#include "gsmd.h"
 534 +
 535 +#include <gsmd/gsmd.h>
 536 +
 537 +void uart_drain(int fd)
 538 +{
 539 +       int rc;
 540 +       struct termios t;
 541 +       rc = tcflush(fd, TCIOFLUSH);
 542 +       rc = tcgetattr(fd, &t);
 543 +       DEBUGP(
 544 +                       "c_iflag = 0x%08x, c_oflag = 0x%08x, "
 545 +                       "c_cflag = 0x%08x, c_lflag = 0x%08x\n",
 546 +                       t.c_iflag, t.c_oflag, t.c_cflag, t.c_lflag);
 547 +       t.c_iflag = t.c_oflag = 0;
 548 +       cfmakeraw(&t);
 549 +       rc = tcsetattr(fd, TCSANOW, &t);
 550 +}
 551 +
 552 +struct bdrt {
 553 +       int bps;
 554 +       u_int32_t b;
 555 +};
 556 +
 557 +static struct bdrt bdrts[] = {
 558 +       { 0, B0 },
 559 +       { 9600, B9600 },
 560 +       { 19200, B19200 },
 561 +       { 38400, B38400 },
 562 +       { 57600, B57600 },
 563 +       { 115200, B115200 },
 564 +       { 230400, B230400 },
 565 +       { 460800, B460800 },
 566 +       { 921600, B921600 },
 567 +};
 568 +
 569 +int set_baudrate(int fd, int baudrate, int hwflow)
 570 +{
 571 +       int i;
 572 +       u_int32_t bd = 0;
 573 +       struct termios ti;
 574 +
 575 +       for (i = 0; i < ARRAY_SIZE(bdrts); i++) {
 576 +               if (bdrts[i].bps == baudrate)
 577 +                       bd = bdrts[i].b;
 578 +       }
 579 +       if (bd == 0)
 580 +               return -EINVAL;
 581 +
 582 +       i = tcgetattr(fd, &ti);
 583 +       if (i < 0)
 584 +               return i;
 585 +
 586 +       i = cfsetispeed(&ti, B0);
 587 +       if (i < 0)
 588 +               return i;
 589 +
 590 +       i = cfsetospeed(&ti, bd);
 591 +       if (i < 0)
 592 +               return i;
 593 +
 594 +       if (hwflow)
 595 +               ti.c_cflag |= CRTSCTS;
 596 +       else
 597 +               ti.c_cflag &= ~CRTSCTS;
 598 +
 599 +       return tcsetattr(fd, 0, &ti);
 600 +}
 601 +
 602 +static int uart_select_cb(int fd, unsigned int what, void *data)
 603 +{
 604 +       struct gsmd_uart *uart = (struct gsmd_uart *) data;
 605 +       static char rxbuf[2048];
 606 +       int rc, len;
 607 +
 608 +       if ((what & GSMD_FD_READ) && uart->port.newdata_cb) {
 609 +               while ((len = read(fd, rxbuf, sizeof(rxbuf)))) {
 610 +                       if (len < 0) {
 611 +                               if (errno == EAGAIN || errno == EINTR)
 612 +                                       return 0;
 613 +                               gsmd_log(GSMD_NOTICE, "ERROR reading from "
 614 +                                               "fd %u: %d (%s)\n", fd, errno,
 615 +                                               strerror(errno));
 616 +                               return -errno;
 617 +                       }
 618 +
 619 +                       rc = uart->port.newdata_cb(
 620 +                                       uart->port.newdata_opaque,
 621 +                                       rxbuf,
 622 +                                       len);
 623 +                       if (rc < 0)
 624 +                               return rc;
 625 +               }
 626 +       }
 627 +
 628 +       /* Write pending data to UART.  */
 629 +       if ((what & GSMD_FD_WRITE) && uart->tx_len) {
 630 +               while (uart->tx_start + uart->tx_len >= sizeof(uart->txfifo)) {
 631 +                       len = sizeof(uart->txfifo) - uart->tx_start;
 632 +                       rc = write(fd, &uart->txfifo[uart->tx_start], len);
 633 +                       if (rc < 0 && errno != EINTR) {
 634 +                               if (errno == EAGAIN)
 635 +                                       return 0;
 636 +                               gsmd_log(GSMD_NOTICE, "ERROR writing "
 637 +                                               "fd %u: %d (%s)\n", fd, errno,
 638 +                                               strerror(errno));
 639 +                               return -errno;
 640 +                       }
 641 +
 642 +                       if (rc > 0) {
 643 +                               uart->tx_start += rc;
 644 +                               uart->tx_len -= rc;
 645 +                       }
 646 +               }
 647 +               uart->tx_start &= sizeof(uart->txfifo) - 1;
 648 +
 649 +               while (uart->tx_len) {
 650 +                       rc = write(fd, &uart->txfifo[uart->tx_start],
 651 +                                       uart->tx_len);
 652 +                       if (rc < 0 && errno != EINTR) {
 653 +                               if (errno == EAGAIN)
 654 +                                       return 0;
 655 +                               gsmd_log(GSMD_NOTICE, "ERROR writing "
 656 +                                               "fd %u: %d (%s)\n", fd, errno,
 657 +                                               strerror(errno));
 658 +                               return -errno;
 659 +                       }
 660 +
 661 +                       if (rc > 0) {
 662 +                               uart->tx_start += rc;
 663 +                               uart->tx_len -= rc;
 664 +                       }
 665 +               }
 666 +
 667 +               /* If we reached here, there's no more data for the moment.  */
 668 +               uart->gfd.when &= ~GSMD_FD_WRITE;
 669 +       }
 670 +
 671 +       return 0;
 672 +}
 673 +
 674 +static int uart_write(struct gsmd_port *port, const char data[], int len)
 675 +{
 676 +       struct gsmd_uart *uart = (struct gsmd_uart *) port;
 677 +       int start = (uart->tx_start + uart->tx_len) &
 678 +               (sizeof(uart->txfifo) - 1);
 679 +       int space = sizeof(uart->txfifo) - start;
 680 +
 681 +       if (uart->tx_len + len > sizeof(uart->txfifo))
 682 +               len = sizeof(uart->txfifo) - uart->tx_len;
 683 +
 684 +       if (len)
 685 +               uart->gfd.when |= GSMD_FD_WRITE;
 686 +
 687 +       if (len > space) {
 688 +               memcpy(uart->txfifo + start, data, space);
 689 +               memcpy(uart->txfifo, data + space, len - space);
 690 +       } else
 691 +               memcpy(uart->txfifo + start, data, len);
 692 +
 693 +       uart->tx_len += len;
 694 +       return len;
 695 +}
 696 +
 697 +int uart_init(struct gsmd_uart *uart, int sockfd)
 698 +{
 699 +       uart->gfd.fd = sockfd;
 700 +       uart->gfd.when = GSMD_FD_READ;
 701 +       uart->gfd.data = uart;
 702 +       uart->gfd.cb = &uart_select_cb;
 703 +
 704 +       uart->port.write = uart_write;
 705 +
 706 +       return gsmd_register_fd(&uart->gfd);
 707 +}
 708 --
 709 1.5.2.1
 710