gisi/modem.c

   1 /*
   2  *
   3  *  oFono - Open Source Telephony
   4  *
   5  *  Copyright (C) 2009-2010 Nokia Corporation and/or its subsidiary(-ies).
   6  *
   7  *  This program is free software; you can redistribute it and/or modify
   8  *  it under the terms of the GNU General Public License version 2 as
   9  *  published by the Free Software Foundation.
  10  *
  11  *  This program is distributed in the hope that it will be useful,
  12  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  *  GNU General Public License for more details.
  15  *
  16  *  You should have received a copy of the GNU General Public License
  17  *  along with this program; if not, write to the Free Software
  18  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  *
  20  */
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include <config.h>
  24 #endif
  25
  26 #define _GNU_SOURCE
  27 #include <stdint.h>
  28 #include <string.h>
  29 #include <sys/types.h>
  30 #include <sys/socket.h>
  31 #include <sys/ioctl.h>
  32 #include <sys/uio.h>
  33 #include <net/if.h>
  34 #include <errno.h>
  35 #include <glib.h>
  36
  37 #include "message.h"
  38 #include "common.h"
  39 #include "modem.h"
  40 #include "socket.h"
  41
  42 #define ISIDBG(m, fmt, ...)                             \
  43         if ((m) != NULL && (m)->debug != NULL)          \
  44                 m->debug("gisi: "fmt, ##__VA_ARGS__);
  45
  46 struct _GIsiServiceMux {
  47         GIsiModem *modem;
  48         GSList *pending;
  49         GIsiVersion version;
  50         uint8_t resource;
  51         uint8_t last_utid;
  52         uint16_t object;
  53         unsigned subscriptions;
  54         unsigned registrations;
  55         gboolean reachable;
  56         gboolean version_pending;
  57 };
  58 typedef struct _GIsiServiceMux GIsiServiceMux;
  59
  60 struct _GIsiModem {
  61         unsigned index;
  62         GHashTable *services;
  63         gboolean subs_source;
  64         int req_fd;
  65         int ind_fd;
  66         guint req_watch;
  67         guint ind_watch;
  68         GIsiDebugFunc debug;
  69         GIsiNotifyFunc trace;
  70         void *opaque;
  71 };
  72
  73 struct _GIsiPending {
  74         enum GIsiMessageType type;
  75         GIsiServiceMux *service;
  76         guint timeout;
  77         GIsiNotifyFunc notify;
  78         GDestroyNotify destroy;
  79         void *data;
  80         uint8_t utid;
  81         uint8_t msgid;
  82 };
  83
  84 static const struct sockaddr_pn namesrv = {
  85         .spn_family = AF_PHONET,
  86         .spn_resource = PN_NAMESERVICE,
  87 };
  88
  89 static const struct sockaddr_pn commgr = {
  90         .spn_family = AF_PHONET,
  91         .spn_resource = PN_COMMGR,
  92 };
  93
  94 static GIsiServiceMux *service_get(GIsiModem *modem, uint8_t resource)
  95 {
  96         GIsiServiceMux *mux;
  97         int key = resource;
  98
  99         mux = g_hash_table_lookup(modem->services, GINT_TO_POINTER(key));
 100         if (mux != NULL)
 101                 return mux;
 102
 103         mux = g_try_new0(GIsiServiceMux, 1);
 104         if (mux == NULL)
 105                 return NULL;
 106
 107         g_hash_table_insert(modem->services, GINT_TO_POINTER(key), mux);
 108
 109         mux->modem = modem;
 110         mux->resource = resource;
 111         mux->version.major = -1;
 112         mux->version.minor = -1;
 113         mux->reachable = FALSE;
 114         mux->version_pending = FALSE;
 115
 116         return mux;
 117 }
 118
 119 static gint utid_equal(gconstpointer a, gconstpointer b)
 120 {
 121         const GIsiPending *pa = a;
 122         const GIsiPending *pb = b;
 123
 124         return pa->utid - pb->utid;
 125 }
 126
 127 static const char *pend_type_to_str(enum GIsiMessageType type)
 128 {
 129         switch (type) {
 130         case GISI_MESSAGE_TYPE_REQ:
 131                 return "REQ";
 132         case GISI_MESSAGE_TYPE_IND:
 133                 return "IND";
 134         case GISI_MESSAGE_TYPE_NTF:
 135                 return "NTF";
 136         case GISI_MESSAGE_TYPE_RESP:
 137                 return "RESP";
 138         case GISI_MESSAGE_TYPE_COMMON:
 139                 return "COMMON";
 140         }
 141         return "UNKNOWN";
 142 }
 143
 144 static void pending_dispatch(GIsiPending *pend, GIsiMessage *msg)
 145 {
 146         GIsiModem *modem;
 147
 148         if (pend->notify == NULL)
 149                 return;
 150
 151         modem = pend->service->modem;
 152
 153         ISIDBG(modem, "%s %s to %p [res=0x%02X, id=0x%02X, utid=0x%02X]",
 154                 g_isi_msg_strerror(msg), pend_type_to_str(pend->type), pend,
 155                 g_isi_msg_resource(msg), g_isi_msg_id(msg),
 156                 g_isi_msg_utid(msg));
 157
 158         pend->notify(msg, pend->data);
 159 }
 160
 161 static void service_dispatch(GIsiServiceMux *mux, GIsiMessage *msg,
 162                                 gboolean is_indication)
 163 {
 164         uint8_t msgid = g_isi_msg_id(msg);
 165         uint8_t utid = g_isi_msg_utid(msg);
 166
 167         GSList *l = mux->pending;
 168
 169         while (l != NULL) {
 170                 GSList *next = l->next;
 171                 GIsiPending *pend = l->data;
 172                 msg->private = pend;
 173
 174                 /*
 175                  * REQs, NTFs and INDs are dispatched on message ID.  While
 176                  * INDs have the unique transaction ID set to zero, NTFs
 177                  * typically mirror the UTID of the request that set up the
 178                  * session, and REQs can naturally have any transaction ID.
 179                  *
 180                  * RESPs are dispatched on unique transaction ID, explicitly
 181                  * ignoring the msgid.  A RESP also completes a transaction,
 182                  * so it needs to be removed after being notified of.
 183                  *
 184                  * Version query responses are dispatched in a similar fashion
 185                  * as RESPs, but based on the pending type and the message ID.
 186                  * Some of these may be synthesized, but nevertheless need to
 187                  * be removed.
 188                  */
 189                 if (pend->type < GISI_MESSAGE_TYPE_RESP
 190                                 && pend->msgid == msgid) {
 191
 192                         pending_dispatch(pend, msg);
 193
 194                 } else if (pend->type == GISI_MESSAGE_TYPE_RESP &&
 195                                 !is_indication && pend->utid == utid) {
 196
 197                         pending_dispatch(pend, msg);
 198                         pend->notify = NULL;
 199
 200                         g_isi_pending_remove(pend);
 201                         break;
 202
 203                 } else if (pend->type == GISI_MESSAGE_TYPE_COMMON &&
 204                                 msgid == COMMON_MESSAGE &&
 205                                 pend->msgid == COMM_ISI_VERSION_GET_REQ) {
 206
 207                         pending_dispatch(pend, msg);
 208                         pend->notify = NULL;
 209
 210                         g_isi_pending_remove(pend);
 211
 212                 }
 213                 l = next;
 214         }
 215 }
 216
 217 static void common_message_decode(GIsiServiceMux *mux, GIsiMessage *msg)
 218 {
 219         uint8_t code;
 220         uint8_t major;
 221         uint8_t minor;
 222
 223         if (!g_isi_msg_data_get_byte(msg, 0, &code))
 224                 return;
 225
 226         switch (code) {
 227         case COMM_ISA_ENTITY_NOT_REACHABLE_RESP:
 228                 mux->reachable = FALSE;
 229                 msg->error = ENOENT;
 230                 break;
 231
 232         case COMM_ISI_VERSION_GET_RESP:
 233
 234                 if (g_isi_msg_data_get_byte(msg, 1, &major) &&
 235                                 g_isi_msg_data_get_byte(msg, 2, &minor)) {
 236                         mux->version.major = major;
 237                         mux->version.minor = minor;
 238                 }
 239                 /* fall through */
 240
 241         case 0x00:
 242                 /*
 243                  * PN_SIM doesn't support ISI version, but sends a
 244                  * 0x00 message as a response.  Work around this modem
 245                  * wart.
 246                  */
 247                 mux->object = g_isi_msg_object(msg);
 248                 mux->version_pending = FALSE;
 249                 mux->reachable = TRUE;
 250                 break;
 251         }
 252         msg->version = &mux->version;
 253 }
 254
 255 static void firewall_notify_handle(GIsiModem *modem, GIsiMessage *msg)
 256 {
 257         uint8_t id;
 258
 259         if (!g_isi_msg_data_get_byte(msg, 0, &id))
 260                 return;
 261
 262         ISIDBG(modem, "firewall blocked message 0x%02X", id);
 263 }
 264
 265 static gboolean isi_callback(GIOChannel *channel, GIOCondition cond,
 266                                 gpointer data)
 267 {
 268         GIsiModem *modem = data;
 269         int len;
 270         int fd;
 271
 272         if (cond & (G_IO_NVAL|G_IO_HUP)) {
 273                 ISIDBG(modem, "Unexpected event on PhoNet channel %p", channel);
 274                 return FALSE;
 275         }
 276
 277         fd = g_io_channel_unix_get_fd(channel);
 278         len = g_isi_phonet_peek_length(channel);
 279
 280         if (len > 0) {
 281                 struct sockaddr_pn addr;
 282                 uint32_t buf[(len + 3) / 4];
 283
 284                 GIsiServiceMux *mux;
 285                 GIsiMessage msg;
 286                 unsigned key;
 287
 288                 len = g_isi_phonet_read(channel, buf, len, &addr);
 289                 if (len < 2)
 290                         return TRUE;
 291
 292                 msg.addr = &addr;
 293                 msg.error = 0;
 294                 msg.data = buf;
 295                 msg.len = len;
 296
 297                 if (modem->trace != NULL)
 298                         modem->trace(&msg, NULL);
 299
 300                 key = addr.spn_resource;
 301                 mux = g_hash_table_lookup(modem->services, GINT_TO_POINTER(key));
 302                 if (mux == NULL) {
 303                         /*
 304                          * Unfortunately, the FW report has the wrong
 305                          * resource ID in the N900 modem.
 306                          */
 307                         if (key == PN_FIREWALL)
 308                                 firewall_notify_handle(modem, &msg);
 309
 310                         return TRUE;
 311                 }
 312
 313                 msg.version = &mux->version;
 314
 315                 if (g_isi_msg_id(&msg) == COMMON_MESSAGE)
 316                         common_message_decode(mux, &msg);
 317
 318                 service_dispatch(mux, &msg, fd == modem->ind_fd);
 319         }
 320         return TRUE;
 321 }
 322
 323 static gboolean modem_subs_update(gpointer data)
 324 {
 325         GHashTableIter iter;
 326         gpointer keyptr, value;
 327
 328         GIsiModem *modem = data;
 329         uint8_t msg[3 + 256] = {
 330                 0, PNS_SUBSCRIBED_RESOURCES_IND,
 331                 0,
 332         };
 333         uint8_t count = 0;
 334
 335         modem->subs_source = 0;
 336
 337         g_hash_table_iter_init(&iter, modem->services);
 338
 339         while (g_hash_table_iter_next(&iter, &keyptr, &value)) {
 340                 GIsiServiceMux *mux = value;
 341
 342                 if (mux->subscriptions > 0) {
 343                         msg[3 + count] = mux->resource;
 344                         count++;
 345                 }
 346         }
 347         msg[2] = count;
 348
 349         sendto(modem->ind_fd, msg, 3 + msg[2], MSG_NOSIGNAL, (void *)&commgr,
 350                 sizeof(commgr));
 351
 352         return FALSE;
 353 }
 354
 355 static void modem_subs_update_when_idle(GIsiModem *modem)
 356 {
 357         if (modem->subs_source > 0)
 358                 return;
 359
 360         modem->subs_source = g_idle_add(modem_subs_update, modem);
 361 }
 362
 363 static void service_name_register(GIsiServiceMux *mux)
 364 {
 365         uint8_t msg[] = {
 366                 0, PNS_NAME_ADD_REQ, 0, 0,
 367                 0, 0, 0, mux->resource, /* 32-bit Big-Endian name */
 368                 0, 0,                   /* device/object */
 369                 0, 0,                   /* filler */
 370         };
 371         uint16_t object = 0;
 372
 373         if (ioctl(mux->modem->req_fd, SIOCPNGETOBJECT, &object) < 0) {
 374                 ISIDBG(mux->modem, "ioctl(SIOCPNGETOBJECT): %s",
 375                         strerror(errno));
 376                 return;
 377         }
 378
 379         /* Fill in the object ID */
 380         msg[8] = object >> 8;
 381         msg[9] = object & 0xFF;
 382
 383         sendto(mux->modem->req_fd, msg, sizeof(msg), MSG_NOSIGNAL,
 384                 (void *)&namesrv, sizeof(namesrv));
 385 }
 386
 387 static void service_name_deregister(GIsiServiceMux *mux)
 388 {
 389         const uint8_t msg[] = {
 390                 0, PNS_NAME_REMOVE_REQ, 0, 0,
 391                 0, 0, 0, mux->resource,
 392         };
 393
 394         sendto(mux->modem->req_fd, msg, sizeof(msg), MSG_NOSIGNAL,
 395                 (void *)&namesrv, sizeof(namesrv));
 396 }
 397
 398 static void pending_destroy(gpointer value, gpointer user)
 399 {
 400         GIsiPending *op = value;
 401
 402         if (op == NULL)
 403                 return;
 404
 405         if (op->timeout > 0)
 406                 g_source_remove(op->timeout);
 407
 408         if (op->destroy != NULL)
 409                 op->destroy(op->data);
 410
 411         g_free(op);
 412 }
 413
 414 static void service_finalize(gpointer value)
 415 {
 416         GIsiServiceMux *mux = value;
 417         GIsiModem *modem = mux->modem;
 418
 419         if (mux->subscriptions > 0)
 420                 modem_subs_update_when_idle(modem);
 421
 422         if (mux->registrations > 0)
 423                 service_name_deregister(mux);
 424
 425         g_slist_foreach(mux->pending, pending_destroy, NULL);
 426         g_slist_free(mux->pending);
 427         g_free(mux);
 428 }
 429
 430 GIsiModem *g_isi_modem_create(unsigned index)
 431 {
 432         GIsiModem *modem;
 433         GIOChannel *inds;
 434         GIOChannel *reqs;
 435
 436         if (index == 0) {
 437                 errno = ENODEV;
 438                 return NULL;
 439         }
 440
 441         modem = g_try_new0(GIsiModem, 1);
 442         if (modem == NULL) {
 443                 errno = ENOMEM;
 444                 return NULL;
 445         }
 446
 447         inds = g_isi_phonet_new(index);
 448         reqs = g_isi_phonet_new(index);
 449
 450         if (inds == NULL || reqs == NULL) {
 451                 g_free(modem);
 452                 return NULL;
 453         }
 454
 455         modem->req_fd = g_io_channel_unix_get_fd(reqs);
 456         modem->req_watch = g_io_add_watch(reqs,
 457                                         G_IO_IN|G_IO_ERR|G_IO_HUP|G_IO_NVAL,
 458                                         isi_callback, modem);
 459         modem->ind_fd = g_io_channel_unix_get_fd(inds);
 460         modem->ind_watch = g_io_add_watch(inds,
 461                                         G_IO_IN|G_IO_ERR|G_IO_HUP|G_IO_NVAL,
 462                                         isi_callback, modem);
 463
 464         g_io_channel_unref(reqs);
 465         g_io_channel_unref(inds);
 466
 467         modem->index = index;
 468         modem->services = g_hash_table_new_full(g_direct_hash, NULL,
 469                                                 NULL, service_finalize);
 470
 471         return modem;
 472 }
 473
 474 GIsiModem *g_isi_modem_create_by_name(const char *name)
 475 {
 476         return g_isi_modem_create(if_nametoindex(name));
 477 }
 478
 479 void *g_isi_modem_set_userdata(GIsiModem *modem, void *data)
 480 {
 481         void *old = modem->opaque;
 482
 483         modem->opaque = data;
 484         return old;
 485 }
 486
 487 void *g_isi_modem_get_userdata(GIsiModem *modem)
 488 {
 489         return modem->opaque;
 490 }
 491
 492 static uint8_t service_next_utid(GIsiServiceMux *mux)
 493 {
 494         if (mux->last_utid == 0x00 || mux->last_utid == 0xFF)
 495                 return 1;
 496
 497         return mux->last_utid + 1;
 498 }
 499
 500 static void service_subs_incr(GIsiServiceMux *mux)
 501 {
 502         GIsiModem *modem = mux->modem;
 503
 504         mux->subscriptions++;
 505
 506         if (mux->subscriptions == 1)
 507                 modem_subs_update_when_idle(modem);
 508 }
 509
 510 static void service_subs_decr(GIsiServiceMux *mux)
 511 {
 512         GIsiModem *modem = mux->modem;
 513
 514         if (mux->subscriptions == 0)
 515                 return;
 516
 517         mux->subscriptions--;
 518
 519         if (mux->subscriptions == 0)
 520                 modem_subs_update_when_idle(modem);
 521 }
 522
 523 static void service_regs_incr(GIsiServiceMux *mux)
 524 {
 525         mux->registrations++;
 526
 527         if (mux->registrations == 1)
 528                 service_name_register(mux);
 529 }
 530
 531 static void service_regs_decr(GIsiServiceMux *mux)
 532 {
 533         if (mux->registrations == 0)
 534                 return;
 535
 536         mux->registrations--;
 537
 538         if (mux->registrations == 0)
 539                 service_name_deregister(mux);
 540 }
 541
 542 void g_isi_modem_destroy(GIsiModem *modem)
 543 {
 544         if (modem == NULL)
 545                 return;
 546
 547         g_hash_table_remove_all(modem->services);
 548
 549         if (modem->subs_source > 0) {
 550                 g_source_remove(modem->subs_source);
 551                 modem_subs_update(modem);
 552         }
 553
 554         g_hash_table_unref(modem->services);
 555
 556         if (modem->ind_watch > 0)
 557                 g_source_remove(modem->ind_watch);
 558
 559         if (modem->req_watch > 0)
 560                 g_source_remove(modem->req_watch);
 561
 562         g_free(modem);
 563 }
 564
 565 unsigned g_isi_modem_index(GIsiModem *modem)
 566 {
 567         return modem != NULL ? modem->index : 0;
 568 }
 569
 570 GIsiPending *g_isi_request_send(GIsiModem *modem, uint8_t resource,
 571                                         const void *__restrict buf, size_t len,
 572                                         unsigned timeout, GIsiNotifyFunc notify,
 573                                         void *data, GDestroyNotify destroy)
 574 {
 575         struct sockaddr_pn dst = {
 576                 .spn_family = AF_PHONET,
 577                 .spn_resource = resource,
 578         };
 579
 580         return g_isi_request_sendto(modem, &dst, buf, len, timeout, notify,
 581                                         data, destroy);
 582 };
 583
 584 GIsiPending *g_isi_request_vsend(GIsiModem *modem, uint8_t resource,
 585                                         const struct iovec *__restrict iov,
 586                                         size_t iovlen, unsigned timeout,
 587                                         GIsiNotifyFunc notify, void *data,
 588                                         GDestroyNotify destroy)
 589 {
 590         struct sockaddr_pn dst = {
 591                 .spn_family = AF_PHONET,
 592                 .spn_resource = resource,
 593         };
 594
 595         return g_isi_request_vsendto(modem, &dst, iov, iovlen, timeout, notify,
 596                                         data, destroy);
 597 }
 598
 599 GIsiPending *g_isi_request_sendto(GIsiModem *modem, struct sockaddr_pn *dst,
 600                                         const void *__restrict buf, size_t len,
 601                                         unsigned timeout, GIsiNotifyFunc notify,
 602                                         void *data, GDestroyNotify destroy)
 603 {
 604         const struct iovec iov = {
 605                 .iov_base = (void *)buf,
 606                 .iov_len = len,
 607         };
 608
 609         return g_isi_request_vsendto(modem, dst, &iov, 1, timeout, notify, data,
 610                                         destroy);
 611 }
 612
 613 static void vtrace(struct sockaddr_pn *dst,
 614                         const struct iovec *__restrict iov, size_t iovlen,
 615                         size_t total_len, GIsiNotifyFunc trace)
 616 {
 617         uint8_t buffer[total_len];
 618         uint8_t *ptr = buffer;
 619         GIsiMessage msg = {
 620                 .addr = dst,
 621                 .data = (const void *)buffer,
 622                 .len = total_len,
 623         };
 624         size_t i;
 625
 626         for (i = 0; i < iovlen; i++) {
 627                 memcpy(ptr, iov[i].iov_base, iov[i].iov_len);
 628                 ptr += iov[i].iov_len;
 629         }
 630
 631         trace(&msg, NULL);
 632 }
 633
 634 static gboolean resp_timeout(gpointer data)
 635 {
 636         GIsiPending *resp = data;
 637         GIsiMessage msg = {
 638                 .error = ETIMEDOUT,
 639                 .private = resp,
 640         };
 641
 642         pending_dispatch(resp, &msg);
 643         resp->notify = NULL;
 644
 645         g_isi_pending_remove(resp);
 646         return FALSE;
 647 }
 648
 649 GIsiPending *g_isi_request_vsendto(GIsiModem *modem, struct sockaddr_pn *dst,
 650                                         const struct iovec *__restrict iov,
 651                                         size_t iovlen, unsigned timeout,
 652                                         GIsiNotifyFunc notify, void *data,
 653                                         GDestroyNotify destroy)
 654 {
 655         struct iovec _iov[1 + iovlen];
 656         struct msghdr msg = {
 657                 .msg_name = (void *)dst,
 658                 .msg_namelen = sizeof(struct sockaddr_pn),
 659                 .msg_iov = _iov,
 660                 .msg_iovlen = 1 + iovlen,
 661                 .msg_control = NULL,
 662                 .msg_controllen = 0,
 663                 .msg_flags = 0,
 664         };
 665         ssize_t ret;
 666         size_t i, len;
 667
 668         GIsiServiceMux *mux;
 669         GIsiPending *resp;
 670
 671         if (modem == NULL) {
 672                 errno = EINVAL;
 673                 return NULL;
 674         }
 675
 676         mux = service_get(modem, dst->spn_resource);
 677         if (mux == NULL) {
 678                 errno = ENOMEM;
 679                 return NULL;
 680         }
 681
 682         resp = g_try_new0(GIsiPending, 1);
 683         if (resp == NULL) {
 684                 errno = ENOMEM;
 685                 return NULL;
 686         }
 687
 688         resp->type = GISI_MESSAGE_TYPE_RESP;
 689         resp->utid = service_next_utid(mux);
 690         resp->service = mux;
 691         resp->notify = notify;
 692         resp->destroy = destroy;
 693         resp->data = data;
 694
 695         if (g_slist_find_custom(mux->pending, resp, utid_equal)) {
 696                 /*
 697                  * FIXME: perhaps retry with randomized access after
 698                  * initial miss. Although if the rate at which
 699                  * requests are sent is so high that the unique
 700                  * transaction ID wraps, it's likely there is
 701                  * something wrong and we might as well fail here.
 702                  */
 703                 ISIDBG(modem, "ERROR: UTID wrapped, modem busy");
 704                 errno = EBUSY;
 705                 goto error;
 706         }
 707
 708         _iov[0].iov_base = &resp->utid;
 709         _iov[0].iov_len = 1;
 710
 711         for (i = 0, len = 1; i < iovlen; i++) {
 712                 _iov[1 + i] = iov[i];
 713                 len += iov[i].iov_len;
 714         }
 715
 716         if (modem->trace != NULL)
 717                 vtrace(dst, _iov, 1 + iovlen, len, modem->trace);
 718
 719         ret = sendmsg(modem->req_fd, &msg, MSG_NOSIGNAL);
 720         if (ret == -1)
 721                 goto error;
 722
 723         if (ret != (ssize_t)len) {
 724                 errno = EMSGSIZE;
 725                 goto error;
 726         }
 727
 728         mux->pending = g_slist_prepend(mux->pending, resp);
 729
 730         if (timeout > 0)
 731                 resp->timeout = g_timeout_add_seconds(timeout, resp_timeout,
 732                                                         resp);
 733
 734         mux->last_utid = resp->utid;
 735         return resp;
 736
 737 error:
 738         g_free(resp);
 739         return NULL;
 740 }
 741
 742 uint8_t g_isi_request_utid(GIsiPending *resp)
 743 {
 744         return resp != NULL ? resp->utid : 0;
 745 }
 746
 747 GIsiPending *g_isi_pending_from_msg(const GIsiMessage *msg)
 748 {
 749         return msg != NULL ? msg->private : NULL;
 750 }
 751
 752 void g_isi_pending_remove(GIsiPending *op)
 753 {
 754         if (op == NULL)
 755                 return;
 756
 757         op->service->pending = g_slist_remove(op->service->pending, op);
 758
 759         if (op->type == GISI_MESSAGE_TYPE_IND)
 760                 service_subs_decr(op->service);
 761
 762         if (op->type == GISI_MESSAGE_TYPE_REQ)
 763                 service_regs_decr(op->service);
 764
 765         if (op->type == GISI_MESSAGE_TYPE_RESP && op->notify != NULL) {
 766                 GIsiMessage msg = {
 767                         .error = ESHUTDOWN,
 768                         .private = op,
 769                 };
 770                 op->notify(&msg, op->data);
 771                 op->notify = NULL;
 772         }
 773
 774         pending_destroy(op, NULL);
 775 }
 776
 777 GIsiPending *g_isi_ntf_subscribe(GIsiModem *modem, uint8_t resource,
 778                                         uint8_t msgid, GIsiNotifyFunc notify,
 779                                         void *data, GDestroyNotify destroy)
 780 {
 781         GIsiServiceMux *mux;
 782         GIsiPending *ntf;
 783
 784         mux = service_get(modem, resource);
 785         if (mux == NULL) {
 786                 errno = ENOMEM;
 787                 return NULL;
 788         }
 789
 790         ntf = g_try_new0(GIsiPending, 1);
 791         if (ntf == NULL) {
 792                 errno = ENOMEM;
 793                 return NULL;
 794         }
 795
 796         ntf->type = GISI_MESSAGE_TYPE_NTF;
 797         ntf->service = mux;
 798         ntf->notify = notify;
 799         ntf->data = data;
 800         ntf->destroy = destroy;
 801         ntf->msgid = msgid;
 802
 803         mux->pending = g_slist_append(mux->pending, ntf);
 804
 805         ISIDBG(modem, "Subscribed to %s (%p) [res=0x%02X, id=0x%02X]",
 806                 pend_type_to_str(ntf->type), ntf, resource, msgid);
 807
 808         return ntf;
 809 }
 810
 811 GIsiPending *g_isi_service_bind(GIsiModem *modem, uint8_t resource,
 812                                 uint8_t msgid, GIsiNotifyFunc notify,
 813                                 void *data, GDestroyNotify destroy)
 814 {
 815         GIsiServiceMux *mux;
 816         GIsiPending *srv;
 817
 818         mux = service_get(modem, resource);
 819         if (mux == NULL) {
 820                 errno = ENOMEM;
 821                 return NULL;
 822         }
 823
 824         srv = g_try_new0(GIsiPending, 1);
 825         if (srv == NULL) {
 826                 errno = ENOMEM;
 827                 return NULL;
 828         }
 829
 830         srv->type = GISI_MESSAGE_TYPE_REQ;
 831         srv->service = mux;
 832         srv->notify = notify;
 833         srv->data = data;
 834         srv->destroy = destroy;
 835         srv->msgid = msgid;
 836
 837         mux->pending = g_slist_append(mux->pending, srv);
 838
 839         ISIDBG(modem, "Bound service for %s (%p) [res=0x%02X, id=0x%02X]",
 840                 pend_type_to_str(srv->type), srv, resource, msgid);
 841
 842         service_regs_incr(mux);
 843
 844         return srv;
 845 }
 846
 847 GIsiPending *g_isi_ind_subscribe(GIsiModem *modem, uint8_t resource,
 848                                         uint8_t msgid, GIsiNotifyFunc notify,
 849                                         void *data, GDestroyNotify destroy)
 850 {
 851         GIsiServiceMux *mux;
 852         GIsiPending *ind;
 853
 854         mux = service_get(modem, resource);
 855         if (mux == NULL) {
 856                 errno = ENOMEM;
 857                 return NULL;
 858         }
 859
 860         ind = g_try_new0(GIsiPending, 1);
 861         if (ind == NULL) {
 862                 errno = ENOMEM;
 863                 return NULL;
 864         }
 865
 866         ind->type = GISI_MESSAGE_TYPE_IND;
 867         ind->service = mux;
 868         ind->notify = notify;
 869         ind->data = data;
 870         ind->destroy = destroy;
 871         ind->msgid = msgid;
 872
 873         mux->pending = g_slist_append(mux->pending, ind);
 874
 875         ISIDBG(modem, "Subscribed for %s (%p) [res=0x%02X, id=0x%02X]",
 876                 pend_type_to_str(ind->type), ind, resource, msgid);
 877
 878         service_subs_incr(mux);
 879
 880         return ind;
 881 }
 882
 883 int g_isi_response_send(GIsiModem *modem, const GIsiMessage *req,
 884                                 const void *__restrict buf, size_t len)
 885 {
 886         const struct iovec iov = {
 887                 .iov_base = (void *)buf,
 888                 .iov_len = len,
 889         };
 890
 891         return g_isi_response_vsend(modem, req, &iov, 1);
 892 }
 893
 894 int g_isi_response_vsend(GIsiModem *modem, const GIsiMessage *req,
 895                                 const struct iovec *__restrict iov,
 896                                 size_t iovlen)
 897 {
 898         struct iovec _iov[1 + iovlen];
 899         uint8_t utid;
 900         size_t i;
 901
 902         utid = g_isi_msg_utid(req);
 903
 904         _iov[0].iov_base = &utid;
 905         _iov[0].iov_len = 1;
 906
 907         for (i = 0; i < iovlen; i++)
 908                 _iov[1 + i] = iov[i];
 909
 910         return g_isi_modem_vsendto(modem, req->addr, _iov, 1 + iovlen);
 911 }
 912
 913 int g_isi_modem_sendto(GIsiModem *modem, struct sockaddr_pn *dst,
 914                         const void *__restrict buf, size_t len)
 915 {
 916         const struct iovec iov = {
 917                 .iov_base = (void *)buf,
 918                 .iov_len = len,
 919         };
 920
 921         return g_isi_modem_vsendto(modem, dst, &iov, 1);
 922 }
 923
 924 int g_isi_modem_vsendto(GIsiModem *modem, struct sockaddr_pn *dst,
 925                                 const struct iovec *__restrict iov,
 926                                 size_t iovlen)
 927 {
 928         struct msghdr msg = {
 929                 .msg_name = (void *)dst,
 930                 .msg_namelen = sizeof(struct sockaddr_pn),
 931                 .msg_iov = (struct iovec *)iov,
 932                 .msg_iovlen = iovlen,
 933                 .msg_control = NULL,
 934                 .msg_controllen = 0,
 935                 .msg_flags = 0,
 936         };
 937         ssize_t ret;
 938         size_t i, len;
 939         GIsiServiceMux *mux;
 940
 941         if (modem == NULL)
 942                 return -EINVAL;
 943
 944         mux = service_get(modem, dst->spn_resource);
 945         if (mux == NULL)
 946                 return -ENOMEM;
 947
 948         for (i = 0, len = 0; i < iovlen; i++)
 949                 len += iov[i].iov_len;
 950
 951         if (modem->trace != NULL)
 952                 vtrace(dst, iov, iovlen, len, modem->trace);
 953
 954         ret = sendmsg(modem->req_fd, &msg, MSG_NOSIGNAL);
 955         if (ret == -1)
 956                 return -errno;
 957
 958         if (ret != (ssize_t)len)
 959                 return -EMSGSIZE;
 960
 961         return 0;
 962 }
 963
 964 void g_isi_modem_set_trace(GIsiModem *modem, GIsiNotifyFunc trace)
 965 {
 966         if (modem == NULL)
 967                 return;
 968
 969         modem->trace = trace;
 970 }
 971
 972 void g_isi_modem_set_debug(GIsiModem *modem, GIsiDebugFunc debug)
 973 {
 974         if (modem == NULL)
 975                 return;
 976
 977         modem->debug = debug;
 978 }
 979
 980 static int version_get_send(GIsiModem *modem, GIsiPending *ping)
 981 {
 982         GIsiServiceMux *mux = ping->service;
 983         const struct sockaddr_pn dst = {
 984                 .spn_family = AF_PHONET,
 985                 .spn_resource = mux->resource,
 986         };
 987         uint8_t msg[] = {
 988                 ping->utid,     /* UTID */
 989                 COMMON_MESSAGE,
 990                 COMM_ISI_VERSION_GET_REQ,
 991                 0,              /* Filler */
 992         };
 993         ssize_t ret;
 994
 995         if (g_slist_find_custom(mux->pending, ping, utid_equal))
 996                 return -EBUSY;
 997
 998         ret = sendto(modem->req_fd, msg, sizeof(msg), MSG_NOSIGNAL,
 999                         (void *)&dst, sizeof(dst));
1000
1001         if (ret == -1)
1002                 return -errno;
1003
1004         if (ret != (ssize_t)sizeof(msg))
1005                 return -EMSGSIZE;
1006
1007         mux->last_utid = ping->utid;
1008         mux->version_pending = TRUE;
1009         return 0;
1010 }
1011
1012 static gboolean reachable_notify(gpointer data)
1013 {
1014         GIsiPending *pong = data;
1015         GIsiServiceMux *mux = pong->service;
1016
1017         struct sockaddr_pn addr = {
1018                 .spn_resource = mux->resource,
1019                 .spn_dev = mux->object >> 8,
1020                 .spn_obj = mux->object & 0xff,
1021         };
1022         GIsiMessage msg = {
1023                 .version = &mux->version,
1024                 .private = pong,
1025                 .addr = &addr,
1026         };
1027
1028         pending_dispatch(pong, &msg);
1029         pong->notify = NULL;
1030
1031         g_isi_pending_remove(pong);
1032         return FALSE;
1033 }
1034
1035 GIsiPending *g_isi_resource_ping(GIsiModem *modem, uint8_t resource,
1036                                         GIsiNotifyFunc notify, void *data,
1037                                         GDestroyNotify destroy)
1038 {
1039         GIsiServiceMux *mux;
1040         GIsiPending *ping;
1041         int ret;
1042
1043         mux = service_get(modem, resource);
1044         if (mux == NULL) {
1045                 errno = ENOMEM;
1046                 return NULL;
1047         }
1048
1049         ping = g_try_new0(GIsiPending, 1);
1050         if (ping == NULL) {
1051                 errno = ENOMEM;
1052                 return NULL;
1053         }
1054
1055         ping->type = GISI_MESSAGE_TYPE_COMMON;
1056         ping->utid = service_next_utid(mux);
1057         ping->service = mux;
1058         ping->notify = notify;
1059         ping->data = data;
1060         ping->destroy = destroy;
1061         ping->msgid = COMM_ISI_VERSION_GET_REQ;
1062
1063         if (mux->reachable) {
1064                 g_idle_add(reachable_notify, ping);
1065                 return ping;
1066         }
1067
1068         if (!mux->version_pending) {
1069                 ret = version_get_send(modem, ping);
1070                 if (ret < 0) {
1071                         g_free(ping);
1072                         errno = ret;
1073                         return NULL;
1074                 }
1075                 mux->last_utid = ping->utid;
1076         }
1077
1078         ping->timeout = g_timeout_add_seconds(COMMON_TIMEOUT, resp_timeout,
1079                                                 ping);
1080         mux->pending = g_slist_prepend(mux->pending, ping);
1081         mux->version_pending = TRUE;
1082
1083         ISIDBG(modem, "Ping sent %s (%p) [res=0x%02X]",
1084                 pend_type_to_str(ping->type), ping, resource);
1085
1086         return ping;
1087 }