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