2010-10-14 12:29:58 +00:00
|
|
|
/*
|
2010-04-21 07:47:16 +00:00
|
|
|
*
|
2010-10-14 12:29:58 +00:00
|
|
|
* oFono - Open Source Telephony
|
2010-04-21 07:47:16 +00:00
|
|
|
*
|
2011-10-10 20:40:08 +00:00
|
|
|
* Copyright (C) 2009-2010 Nokia Corporation and/or its subsidiary(-ies).
|
2010-10-14 12:29:58 +00:00
|
|
|
*
|
|
|
|
* This program is free software; you can redistribute it and/or modify
|
|
|
|
* it under the terms of the GNU General Public License version 2 as
|
|
|
|
* published by the Free Software Foundation.
|
|
|
|
*
|
|
|
|
* This program is distributed in the hope that it will be useful,
|
|
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
* GNU General Public License for more details.
|
|
|
|
*
|
|
|
|
* You should have received a copy of the GNU General Public License
|
|
|
|
* along with this program; if not, write to the Free Software
|
|
|
|
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
2010-04-21 07:47:16 +00:00
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
|
|
#include <config.h>
|
|
|
|
#endif
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
#include <stdint.h>
|
|
|
|
#include <string.h>
|
|
|
|
#include <sys/types.h>
|
|
|
|
#include <sys/socket.h>
|
|
|
|
#include <sys/ioctl.h>
|
|
|
|
#include <sys/uio.h>
|
2010-04-21 07:47:16 +00:00
|
|
|
#include <net/if.h>
|
2010-11-14 16:23:32 +00:00
|
|
|
#include <errno.h>
|
|
|
|
#include <glib.h>
|
2010-04-21 07:47:16 +00:00
|
|
|
|
2019-08-12 22:53:18 +00:00
|
|
|
#pragma GCC diagnostic ignored "-Wpragmas"
|
2019-08-12 17:38:06 +00:00
|
|
|
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
#include "message.h"
|
|
|
|
#include "common.h"
|
2010-04-21 07:47:16 +00:00
|
|
|
#include "modem.h"
|
2010-11-14 16:23:32 +00:00
|
|
|
#include "socket.h"
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
#define ISIDBG(m, fmt, ...) \
|
|
|
|
if ((m) != NULL && (m)->debug != NULL) \
|
|
|
|
m->debug("gisi: "fmt, ##__VA_ARGS__);
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
struct _GIsiServiceMux {
|
|
|
|
GIsiModem *modem;
|
|
|
|
GSList *pending;
|
2010-11-15 16:03:07 +00:00
|
|
|
GIsiVersion version;
|
|
|
|
uint8_t resource;
|
2010-11-14 16:23:32 +00:00
|
|
|
uint8_t last_utid;
|
2010-11-29 09:36:11 +00:00
|
|
|
uint16_t object;
|
2010-11-14 16:23:32 +00:00
|
|
|
unsigned subscriptions;
|
|
|
|
unsigned registrations;
|
|
|
|
gboolean reachable;
|
|
|
|
gboolean version_pending;
|
|
|
|
};
|
|
|
|
typedef struct _GIsiServiceMux GIsiServiceMux;
|
|
|
|
|
|
|
|
struct _GIsiModem {
|
|
|
|
unsigned index;
|
2011-02-07 07:49:27 +00:00
|
|
|
uint8_t device;
|
2010-11-14 16:23:32 +00:00
|
|
|
GHashTable *services;
|
2010-11-15 16:03:07 +00:00
|
|
|
gboolean subs_source;
|
2010-11-16 15:06:11 +00:00
|
|
|
int req_fd;
|
|
|
|
int ind_fd;
|
|
|
|
guint req_watch;
|
|
|
|
guint ind_watch;
|
2010-12-03 07:55:04 +00:00
|
|
|
GIsiDebugFunc debug;
|
|
|
|
GIsiNotifyFunc trace;
|
2010-11-25 14:52:10 +00:00
|
|
|
void *opaque;
|
2011-02-04 12:49:29 +00:00
|
|
|
unsigned long flags;
|
2010-11-14 16:23:32 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
struct _GIsiPending {
|
|
|
|
enum GIsiMessageType type;
|
|
|
|
GIsiServiceMux *service;
|
2011-01-18 21:27:08 +00:00
|
|
|
gpointer owner;
|
2010-11-14 16:23:32 +00:00
|
|
|
guint timeout;
|
|
|
|
GIsiNotifyFunc notify;
|
|
|
|
GDestroyNotify destroy;
|
|
|
|
void *data;
|
|
|
|
uint8_t utid;
|
|
|
|
uint8_t msgid;
|
|
|
|
};
|
|
|
|
|
|
|
|
static GIsiServiceMux *service_get(GIsiModem *modem, uint8_t resource)
|
|
|
|
{
|
|
|
|
GIsiServiceMux *mux;
|
2010-11-15 16:03:07 +00:00
|
|
|
int key = resource;
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-15 16:03:07 +00:00
|
|
|
mux = g_hash_table_lookup(modem->services, GINT_TO_POINTER(key));
|
2010-12-03 07:53:32 +00:00
|
|
|
if (mux != NULL)
|
2010-11-14 16:23:32 +00:00
|
|
|
return mux;
|
|
|
|
|
|
|
|
mux = g_try_new0(GIsiServiceMux, 1);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (mux == NULL)
|
2010-11-14 16:23:32 +00:00
|
|
|
return NULL;
|
|
|
|
|
2010-11-15 16:03:07 +00:00
|
|
|
g_hash_table_insert(modem->services, GINT_TO_POINTER(key), mux);
|
2010-11-14 16:23:32 +00:00
|
|
|
|
|
|
|
mux->modem = modem;
|
|
|
|
mux->resource = resource;
|
|
|
|
mux->version.major = -1;
|
|
|
|
mux->version.minor = -1;
|
|
|
|
mux->reachable = FALSE;
|
|
|
|
mux->version_pending = FALSE;
|
2010-04-21 07:47:16 +00:00
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
return mux;
|
|
|
|
}
|
|
|
|
|
|
|
|
static gint utid_equal(gconstpointer a, gconstpointer b)
|
2010-04-21 07:47:16 +00:00
|
|
|
{
|
2010-11-14 16:23:32 +00:00
|
|
|
const GIsiPending *pa = a;
|
|
|
|
const GIsiPending *pb = b;
|
|
|
|
|
|
|
|
return pa->utid - pb->utid;
|
|
|
|
}
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
static const char *pend_type_to_str(enum GIsiMessageType type)
|
|
|
|
{
|
|
|
|
switch (type) {
|
|
|
|
case GISI_MESSAGE_TYPE_REQ:
|
|
|
|
return "REQ";
|
|
|
|
case GISI_MESSAGE_TYPE_IND:
|
|
|
|
return "IND";
|
|
|
|
case GISI_MESSAGE_TYPE_NTF:
|
|
|
|
return "NTF";
|
|
|
|
case GISI_MESSAGE_TYPE_RESP:
|
|
|
|
return "RESP";
|
|
|
|
case GISI_MESSAGE_TYPE_COMMON:
|
|
|
|
return "COMMON";
|
|
|
|
}
|
|
|
|
return "UNKNOWN";
|
|
|
|
}
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
static void pending_dispatch(GIsiPending *pend, GIsiMessage *msg)
|
|
|
|
{
|
2010-12-03 07:55:04 +00:00
|
|
|
GIsiModem *modem;
|
|
|
|
|
|
|
|
if (pend->notify == NULL)
|
2010-11-14 16:23:32 +00:00
|
|
|
return;
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
modem = pend->service->modem;
|
|
|
|
|
|
|
|
ISIDBG(modem, "%s %s to %p [res=0x%02X, id=0x%02X, utid=0x%02X]",
|
|
|
|
g_isi_msg_strerror(msg), pend_type_to_str(pend->type), pend,
|
2010-12-22 15:47:11 +00:00
|
|
|
g_isi_msg_resource(msg), g_isi_msg_id(msg),
|
|
|
|
g_isi_msg_utid(msg));
|
2010-12-03 07:55:04 +00:00
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
pend->notify(msg, pend->data);
|
|
|
|
}
|
|
|
|
|
2011-01-12 19:24:09 +00:00
|
|
|
static void pending_remove_and_dispatch(GIsiPending *op, GIsiMessage *msg)
|
|
|
|
{
|
|
|
|
GIsiModem *modem;
|
|
|
|
|
|
|
|
op->service->pending = g_slist_remove(op->service->pending, op);
|
|
|
|
|
|
|
|
if (op->notify == NULL || msg == NULL)
|
|
|
|
goto destroy;
|
|
|
|
|
|
|
|
modem = op->service->modem;
|
|
|
|
|
|
|
|
ISIDBG(modem, "%s %s to %p [res=0x%02X, id=0x%02X, utid=0x%02X]",
|
2011-04-27 12:45:14 +00:00
|
|
|
g_isi_msg_error(msg) ? g_isi_msg_strerror(msg) : "normal",
|
|
|
|
pend_type_to_str(op->type), op,
|
2011-01-12 19:24:09 +00:00
|
|
|
g_isi_msg_resource(msg), g_isi_msg_id(msg),
|
|
|
|
g_isi_msg_utid(msg));
|
|
|
|
|
|
|
|
op->notify(msg, op->data);
|
|
|
|
|
|
|
|
destroy:
|
|
|
|
if (op->timeout > 0)
|
|
|
|
g_source_remove(op->timeout);
|
|
|
|
|
|
|
|
if (op->destroy != NULL)
|
|
|
|
op->destroy(op->data);
|
|
|
|
|
|
|
|
g_free(op);
|
|
|
|
}
|
|
|
|
|
2010-11-16 15:06:11 +00:00
|
|
|
static void service_dispatch(GIsiServiceMux *mux, GIsiMessage *msg,
|
|
|
|
gboolean is_indication)
|
2010-11-14 16:23:32 +00:00
|
|
|
{
|
|
|
|
uint8_t msgid = g_isi_msg_id(msg);
|
|
|
|
uint8_t utid = g_isi_msg_utid(msg);
|
|
|
|
|
|
|
|
GSList *l = mux->pending;
|
|
|
|
|
2010-12-03 07:53:32 +00:00
|
|
|
while (l != NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
GSList *next = l->next;
|
|
|
|
GIsiPending *pend = l->data;
|
|
|
|
|
|
|
|
/*
|
|
|
|
* REQs, NTFs and INDs are dispatched on message ID. While
|
|
|
|
* INDs have the unique transaction ID set to zero, NTFs
|
|
|
|
* typically mirror the UTID of the request that set up the
|
|
|
|
* session, and REQs can naturally have any transaction ID.
|
|
|
|
*
|
|
|
|
* RESPs are dispatched on unique transaction ID, explicitly
|
|
|
|
* ignoring the msgid. A RESP also completes a transaction,
|
|
|
|
* so it needs to be removed after being notified of.
|
|
|
|
*
|
2010-11-15 16:03:07 +00:00
|
|
|
* Version query responses are dispatched in a similar fashion
|
2010-11-14 16:23:32 +00:00
|
|
|
* as RESPs, but based on the pending type and the message ID.
|
|
|
|
* Some of these may be synthesized, but nevertheless need to
|
|
|
|
* be removed.
|
|
|
|
*/
|
2010-12-22 15:47:11 +00:00
|
|
|
if (pend->type < GISI_MESSAGE_TYPE_RESP
|
|
|
|
&& pend->msgid == msgid) {
|
2010-11-14 16:23:32 +00:00
|
|
|
|
|
|
|
pending_dispatch(pend, msg);
|
|
|
|
|
|
|
|
} else if (pend->type == GISI_MESSAGE_TYPE_RESP &&
|
2010-11-16 15:06:11 +00:00
|
|
|
!is_indication && pend->utid == utid) {
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2011-01-12 19:24:09 +00:00
|
|
|
pending_remove_and_dispatch(pend, msg);
|
2010-11-14 16:23:32 +00:00
|
|
|
break;
|
2010-04-21 07:47:16 +00:00
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
} else if (pend->type == GISI_MESSAGE_TYPE_COMMON &&
|
|
|
|
msgid == COMMON_MESSAGE &&
|
|
|
|
pend->msgid == COMM_ISI_VERSION_GET_REQ) {
|
|
|
|
|
2011-01-12 19:24:09 +00:00
|
|
|
pending_remove_and_dispatch(pend, msg);
|
2010-11-14 16:23:32 +00:00
|
|
|
}
|
2011-01-12 19:24:09 +00:00
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
l = next;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
static void common_message_decode(GIsiServiceMux *mux, GIsiMessage *msg)
|
|
|
|
{
|
|
|
|
uint8_t code;
|
|
|
|
uint8_t major;
|
|
|
|
uint8_t minor;
|
|
|
|
|
|
|
|
if (!g_isi_msg_data_get_byte(msg, 0, &code))
|
|
|
|
return;
|
|
|
|
|
|
|
|
switch (code) {
|
2010-11-29 09:36:11 +00:00
|
|
|
case COMM_ISA_ENTITY_NOT_REACHABLE_RESP:
|
|
|
|
mux->reachable = FALSE;
|
|
|
|
msg->error = ENOENT;
|
|
|
|
break;
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
case COMM_ISI_VERSION_GET_RESP:
|
|
|
|
|
|
|
|
if (g_isi_msg_data_get_byte(msg, 1, &major) &&
|
|
|
|
g_isi_msg_data_get_byte(msg, 2, &minor)) {
|
|
|
|
mux->version.major = major;
|
|
|
|
mux->version.minor = minor;
|
|
|
|
}
|
2010-11-29 09:36:11 +00:00
|
|
|
/* fall through */
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2011-03-17 10:52:56 +00:00
|
|
|
default:
|
2010-11-14 16:23:32 +00:00
|
|
|
/*
|
|
|
|
* PN_SIM doesn't support ISI version, but sends a
|
2011-03-17 10:52:56 +00:00
|
|
|
* garbage message as a response. Work around this
|
|
|
|
* modem wart.
|
2010-11-14 16:23:32 +00:00
|
|
|
*/
|
2010-11-29 09:36:11 +00:00
|
|
|
mux->object = g_isi_msg_object(msg);
|
2010-11-14 16:23:32 +00:00
|
|
|
mux->version_pending = FALSE;
|
2010-11-29 09:36:11 +00:00
|
|
|
mux->reachable = TRUE;
|
2010-11-14 16:23:32 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
msg->version = &mux->version;
|
|
|
|
}
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
static void firewall_notify_handle(GIsiModem *modem, GIsiMessage *msg)
|
|
|
|
{
|
|
|
|
uint8_t id;
|
|
|
|
|
|
|
|
if (!g_isi_msg_data_get_byte(msg, 0, &id))
|
|
|
|
return;
|
|
|
|
|
|
|
|
ISIDBG(modem, "firewall blocked message 0x%02X", id);
|
|
|
|
}
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
static gboolean isi_callback(GIOChannel *channel, GIOCondition cond,
|
|
|
|
gpointer data)
|
|
|
|
{
|
|
|
|
GIsiModem *modem = data;
|
|
|
|
int len;
|
2010-11-16 15:06:11 +00:00
|
|
|
int fd;
|
2010-11-14 16:23:32 +00:00
|
|
|
|
|
|
|
if (cond & (G_IO_NVAL|G_IO_HUP)) {
|
2010-12-03 07:55:04 +00:00
|
|
|
ISIDBG(modem, "Unexpected event on PhoNet channel %p", channel);
|
2010-11-14 16:23:32 +00:00
|
|
|
return FALSE;
|
|
|
|
}
|
|
|
|
|
2010-11-16 15:06:11 +00:00
|
|
|
fd = g_io_channel_unix_get_fd(channel);
|
2010-11-14 16:23:32 +00:00
|
|
|
len = g_isi_phonet_peek_length(channel);
|
|
|
|
|
|
|
|
if (len > 0) {
|
|
|
|
struct sockaddr_pn addr;
|
|
|
|
uint32_t buf[(len + 3) / 4];
|
|
|
|
|
|
|
|
GIsiServiceMux *mux;
|
|
|
|
GIsiMessage msg;
|
|
|
|
unsigned key;
|
|
|
|
|
|
|
|
len = g_isi_phonet_read(channel, buf, len, &addr);
|
|
|
|
if (len < 2)
|
|
|
|
return TRUE;
|
|
|
|
|
|
|
|
msg.addr = &addr;
|
|
|
|
msg.error = 0;
|
|
|
|
msg.data = buf;
|
|
|
|
msg.len = len;
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
if (modem->trace != NULL)
|
|
|
|
modem->trace(&msg, NULL);
|
2010-11-14 16:23:32 +00:00
|
|
|
|
|
|
|
key = addr.spn_resource;
|
2010-12-24 00:24:34 +00:00
|
|
|
mux = g_hash_table_lookup(modem->services,
|
|
|
|
GINT_TO_POINTER(key));
|
2010-12-03 07:55:04 +00:00
|
|
|
if (mux == NULL) {
|
|
|
|
/*
|
|
|
|
* Unfortunately, the FW report has the wrong
|
|
|
|
* resource ID in the N900 modem.
|
|
|
|
*/
|
|
|
|
if (key == PN_FIREWALL)
|
|
|
|
firewall_notify_handle(modem, &msg);
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
return TRUE;
|
2010-12-03 07:55:04 +00:00
|
|
|
}
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-12-23 04:39:39 +00:00
|
|
|
msg.version = &mux->version;
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
if (g_isi_msg_id(&msg) == COMMON_MESSAGE)
|
|
|
|
common_message_decode(mux, &msg);
|
|
|
|
|
2010-11-16 15:06:11 +00:00
|
|
|
service_dispatch(mux, &msg, fd == modem->ind_fd);
|
2010-11-14 16:23:32 +00:00
|
|
|
}
|
|
|
|
return TRUE;
|
|
|
|
}
|
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
static gboolean modem_subs_update(gpointer data)
|
2010-11-14 16:23:32 +00:00
|
|
|
{
|
2010-11-23 14:07:00 +00:00
|
|
|
GHashTableIter iter;
|
|
|
|
gpointer keyptr, value;
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
GIsiModem *modem = data;
|
2011-02-04 12:58:04 +00:00
|
|
|
gboolean legacy = modem->flags & GISI_MODEM_FLAG_USE_LEGACY_SUBSCRIBE;
|
2011-02-04 12:54:41 +00:00
|
|
|
struct sockaddr_pn commgr = {
|
|
|
|
.spn_family = AF_PHONET,
|
|
|
|
.spn_resource = PN_COMMGR,
|
|
|
|
.spn_dev = modem->device,
|
|
|
|
};
|
2011-02-04 12:58:04 +00:00
|
|
|
uint8_t msg[4 + 1024] = {
|
|
|
|
0, /* UTID */
|
|
|
|
legacy ? PNS_SUBSCRIBED_RESOURCES_IND :
|
|
|
|
PNS_SUBSCRIBED_RESOURCES_EXTEND_IND,
|
|
|
|
0, /* Count */
|
|
|
|
0, /* Filler */
|
2010-11-23 14:07:00 +00:00
|
|
|
};
|
|
|
|
uint8_t count = 0;
|
2011-02-04 12:58:04 +00:00
|
|
|
size_t len;
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
modem->subs_source = 0;
|
2010-11-16 15:06:11 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
g_hash_table_iter_init(&iter, modem->services);
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
while (g_hash_table_iter_next(&iter, &keyptr, &value)) {
|
|
|
|
GIsiServiceMux *mux = value;
|
2010-11-16 15:06:11 +00:00
|
|
|
|
2011-02-04 12:58:04 +00:00
|
|
|
if (mux->subscriptions == 0)
|
|
|
|
continue;
|
|
|
|
|
|
|
|
if (legacy)
|
2010-11-23 14:07:00 +00:00
|
|
|
msg[3 + count] = mux->resource;
|
2011-02-04 12:58:04 +00:00
|
|
|
else
|
|
|
|
/* Resource field is 32bit and Little-endian */
|
|
|
|
msg[4 + count * 4 + 3] = mux->resource;
|
|
|
|
|
|
|
|
count++;
|
2010-11-23 14:07:00 +00:00
|
|
|
}
|
2011-02-04 12:58:04 +00:00
|
|
|
|
|
|
|
len = legacy ? 3 + count : 4 + count * 4;
|
2010-11-23 14:07:00 +00:00
|
|
|
msg[2] = count;
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2011-02-04 12:58:04 +00:00
|
|
|
sendto(modem->ind_fd, msg, len, MSG_NOSIGNAL, (void *) &commgr,
|
2010-11-23 14:07:00 +00:00
|
|
|
sizeof(commgr));
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
return FALSE;
|
2010-11-14 16:23:32 +00:00
|
|
|
}
|
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
static void modem_subs_update_when_idle(GIsiModem *modem)
|
2010-11-14 16:23:32 +00:00
|
|
|
{
|
2010-11-23 14:07:00 +00:00
|
|
|
if (modem->subs_source > 0)
|
|
|
|
return;
|
|
|
|
|
|
|
|
modem->subs_source = g_idle_add(modem_subs_update, modem);
|
2010-11-14 16:23:32 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void service_name_register(GIsiServiceMux *mux)
|
|
|
|
{
|
2011-02-04 12:54:41 +00:00
|
|
|
struct sockaddr_pn namesrv = {
|
|
|
|
.spn_family = AF_PHONET,
|
|
|
|
.spn_resource = PN_NAMESERVICE,
|
|
|
|
.spn_dev = mux->modem->device,
|
|
|
|
};
|
2010-11-14 16:23:32 +00:00
|
|
|
uint8_t msg[] = {
|
|
|
|
0, PNS_NAME_ADD_REQ, 0, 0,
|
|
|
|
0, 0, 0, mux->resource, /* 32-bit Big-Endian name */
|
|
|
|
0, 0, /* device/object */
|
|
|
|
0, 0, /* filler */
|
|
|
|
};
|
|
|
|
uint16_t object = 0;
|
|
|
|
|
2010-11-16 15:06:11 +00:00
|
|
|
if (ioctl(mux->modem->req_fd, SIOCPNGETOBJECT, &object) < 0) {
|
2010-12-03 07:55:04 +00:00
|
|
|
ISIDBG(mux->modem, "ioctl(SIOCPNGETOBJECT): %s",
|
|
|
|
strerror(errno));
|
2010-11-14 16:23:32 +00:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Fill in the object ID */
|
|
|
|
msg[8] = object >> 8;
|
|
|
|
msg[9] = object & 0xFF;
|
|
|
|
|
2010-11-16 15:06:11 +00:00
|
|
|
sendto(mux->modem->req_fd, msg, sizeof(msg), MSG_NOSIGNAL,
|
2011-02-04 12:55:15 +00:00
|
|
|
(void *) &namesrv, sizeof(namesrv));
|
2010-11-14 16:23:32 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void service_name_deregister(GIsiServiceMux *mux)
|
|
|
|
{
|
2011-02-04 12:54:41 +00:00
|
|
|
struct sockaddr_pn namesrv = {
|
|
|
|
.spn_family = AF_PHONET,
|
|
|
|
.spn_resource = PN_NAMESERVICE,
|
|
|
|
.spn_dev = mux->modem->device,
|
|
|
|
};
|
2010-11-14 16:23:32 +00:00
|
|
|
const uint8_t msg[] = {
|
|
|
|
0, PNS_NAME_REMOVE_REQ, 0, 0,
|
|
|
|
0, 0, 0, mux->resource,
|
|
|
|
};
|
|
|
|
|
2010-11-16 15:06:11 +00:00
|
|
|
sendto(mux->modem->req_fd, msg, sizeof(msg), MSG_NOSIGNAL,
|
2011-02-04 12:55:15 +00:00
|
|
|
(void *) &namesrv, sizeof(namesrv));
|
2010-11-14 16:23:32 +00:00
|
|
|
}
|
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
static void pending_destroy(gpointer value, gpointer user)
|
2010-11-14 16:23:32 +00:00
|
|
|
{
|
2010-11-23 14:07:00 +00:00
|
|
|
GIsiPending *op = value;
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-12-03 07:53:32 +00:00
|
|
|
if (op == NULL)
|
2010-11-23 14:07:00 +00:00
|
|
|
return;
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
if (op->timeout > 0)
|
|
|
|
g_source_remove(op->timeout);
|
2010-11-15 16:03:07 +00:00
|
|
|
|
2010-12-03 07:53:32 +00:00
|
|
|
if (op->destroy != NULL)
|
2010-11-23 14:07:00 +00:00
|
|
|
op->destroy(op->data);
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
g_free(op);
|
|
|
|
}
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
static void service_finalize(gpointer value)
|
|
|
|
{
|
|
|
|
GIsiServiceMux *mux = value;
|
|
|
|
GIsiModem *modem = mux->modem;
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
if (mux->subscriptions > 0)
|
|
|
|
modem_subs_update_when_idle(modem);
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
if (mux->registrations > 0)
|
|
|
|
service_name_deregister(mux);
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
g_slist_foreach(mux->pending, pending_destroy, NULL);
|
|
|
|
g_slist_free(mux->pending);
|
|
|
|
g_free(mux);
|
|
|
|
}
|
2010-11-15 16:03:07 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
GIsiModem *g_isi_modem_create(unsigned index)
|
2010-11-15 16:03:07 +00:00
|
|
|
{
|
2010-11-23 14:07:00 +00:00
|
|
|
GIsiModem *modem;
|
|
|
|
GIOChannel *inds;
|
|
|
|
GIOChannel *reqs;
|
2010-11-15 16:03:07 +00:00
|
|
|
|
2010-11-23 14:07:00 +00:00
|
|
|
if (index == 0) {
|
|
|
|
errno = ENODEV;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
modem = g_try_new0(GIsiModem, 1);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (modem == NULL) {
|
2010-11-23 14:07:00 +00:00
|
|
|
errno = ENOMEM;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
inds = g_isi_phonet_new(index);
|
|
|
|
reqs = g_isi_phonet_new(index);
|
|
|
|
|
2010-12-03 07:53:32 +00:00
|
|
|
if (inds == NULL || reqs == NULL) {
|
2010-11-23 14:07:00 +00:00
|
|
|
g_free(modem);
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
modem->req_fd = g_io_channel_unix_get_fd(reqs);
|
|
|
|
modem->req_watch = g_io_add_watch(reqs,
|
|
|
|
G_IO_IN|G_IO_ERR|G_IO_HUP|G_IO_NVAL,
|
|
|
|
isi_callback, modem);
|
|
|
|
modem->ind_fd = g_io_channel_unix_get_fd(inds);
|
|
|
|
modem->ind_watch = g_io_add_watch(inds,
|
|
|
|
G_IO_IN|G_IO_ERR|G_IO_HUP|G_IO_NVAL,
|
|
|
|
isi_callback, modem);
|
|
|
|
|
|
|
|
g_io_channel_unref(reqs);
|
|
|
|
g_io_channel_unref(inds);
|
|
|
|
|
|
|
|
modem->index = index;
|
|
|
|
modem->services = g_hash_table_new_full(g_direct_hash, NULL,
|
|
|
|
NULL, service_finalize);
|
|
|
|
|
|
|
|
return modem;
|
|
|
|
}
|
|
|
|
|
|
|
|
GIsiModem *g_isi_modem_create_by_name(const char *name)
|
|
|
|
{
|
|
|
|
return g_isi_modem_create(if_nametoindex(name));
|
2010-11-15 16:03:07 +00:00
|
|
|
}
|
|
|
|
|
2010-11-25 14:52:10 +00:00
|
|
|
void *g_isi_modem_set_userdata(GIsiModem *modem, void *data)
|
|
|
|
{
|
2011-02-07 07:44:50 +00:00
|
|
|
void *old;
|
2010-11-25 14:52:10 +00:00
|
|
|
|
2011-02-07 07:44:50 +00:00
|
|
|
if (modem == NULL)
|
|
|
|
return NULL;
|
|
|
|
|
|
|
|
old = modem->opaque;
|
2010-11-25 14:52:10 +00:00
|
|
|
modem->opaque = data;
|
2011-02-07 07:44:50 +00:00
|
|
|
|
2010-11-25 14:52:10 +00:00
|
|
|
return old;
|
|
|
|
}
|
|
|
|
|
|
|
|
void *g_isi_modem_get_userdata(GIsiModem *modem)
|
|
|
|
{
|
2011-02-07 07:44:50 +00:00
|
|
|
if (modem == NULL)
|
|
|
|
return NULL;
|
|
|
|
|
2010-11-25 14:52:10 +00:00
|
|
|
return modem->opaque;
|
|
|
|
}
|
|
|
|
|
2011-02-04 12:49:29 +00:00
|
|
|
unsigned long g_isi_modem_flags(GIsiModem *modem)
|
|
|
|
{
|
|
|
|
if (modem == NULL)
|
|
|
|
return 0;
|
|
|
|
|
|
|
|
return modem->flags;
|
|
|
|
}
|
|
|
|
|
|
|
|
void g_isi_modem_set_flags(GIsiModem *modem, unsigned long flags)
|
|
|
|
{
|
|
|
|
if (modem == NULL)
|
|
|
|
return;
|
|
|
|
|
|
|
|
modem->flags = flags;
|
|
|
|
}
|
|
|
|
|
2011-02-07 07:49:27 +00:00
|
|
|
uint8_t g_isi_modem_device(GIsiModem *modem)
|
|
|
|
{
|
|
|
|
if (modem == NULL)
|
|
|
|
return 0;
|
|
|
|
|
|
|
|
return modem->device;
|
|
|
|
}
|
|
|
|
|
|
|
|
int g_isi_modem_set_device(GIsiModem *modem, uint8_t remote)
|
|
|
|
{
|
|
|
|
if (modem == NULL)
|
|
|
|
return -EINVAL;
|
|
|
|
|
|
|
|
if (remote != PN_DEV_HOST && remote != PN_DEV_MODEM)
|
|
|
|
return -EINVAL;
|
|
|
|
|
|
|
|
modem->device = remote;
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
static uint8_t service_next_utid(GIsiServiceMux *mux)
|
|
|
|
{
|
|
|
|
if (mux->last_utid == 0x00 || mux->last_utid == 0xFF)
|
|
|
|
return 1;
|
|
|
|
|
|
|
|
return mux->last_utid + 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
static void service_subs_incr(GIsiServiceMux *mux)
|
|
|
|
{
|
|
|
|
GIsiModem *modem = mux->modem;
|
|
|
|
|
|
|
|
mux->subscriptions++;
|
|
|
|
|
2010-11-15 16:03:07 +00:00
|
|
|
if (mux->subscriptions == 1)
|
|
|
|
modem_subs_update_when_idle(modem);
|
2010-11-14 16:23:32 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void service_subs_decr(GIsiServiceMux *mux)
|
|
|
|
{
|
|
|
|
GIsiModem *modem = mux->modem;
|
|
|
|
|
|
|
|
if (mux->subscriptions == 0)
|
|
|
|
return;
|
|
|
|
|
|
|
|
mux->subscriptions--;
|
|
|
|
|
2010-11-15 16:03:07 +00:00
|
|
|
if (mux->subscriptions == 0)
|
|
|
|
modem_subs_update_when_idle(modem);
|
2010-11-14 16:23:32 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void service_regs_incr(GIsiServiceMux *mux)
|
|
|
|
{
|
|
|
|
mux->registrations++;
|
|
|
|
|
|
|
|
if (mux->registrations == 1)
|
|
|
|
service_name_register(mux);
|
|
|
|
}
|
|
|
|
|
|
|
|
static void service_regs_decr(GIsiServiceMux *mux)
|
|
|
|
{
|
|
|
|
if (mux->registrations == 0)
|
|
|
|
return;
|
|
|
|
|
|
|
|
mux->registrations--;
|
|
|
|
|
|
|
|
if (mux->registrations == 0)
|
|
|
|
service_name_deregister(mux);
|
|
|
|
}
|
|
|
|
|
|
|
|
void g_isi_modem_destroy(GIsiModem *modem)
|
|
|
|
{
|
2010-12-03 07:53:32 +00:00
|
|
|
if (modem == NULL)
|
2010-11-14 16:23:32 +00:00
|
|
|
return;
|
|
|
|
|
2010-11-15 16:03:07 +00:00
|
|
|
g_hash_table_remove_all(modem->services);
|
|
|
|
|
|
|
|
if (modem->subs_source > 0) {
|
|
|
|
g_source_remove(modem->subs_source);
|
|
|
|
modem_subs_update(modem);
|
|
|
|
}
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
g_hash_table_unref(modem->services);
|
2010-11-16 15:06:11 +00:00
|
|
|
|
2010-12-03 07:53:32 +00:00
|
|
|
if (modem->ind_watch > 0)
|
2010-11-16 15:06:11 +00:00
|
|
|
g_source_remove(modem->ind_watch);
|
|
|
|
|
2010-12-03 07:53:32 +00:00
|
|
|
if (modem->req_watch > 0)
|
2010-11-16 15:06:11 +00:00
|
|
|
g_source_remove(modem->req_watch);
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
g_free(modem);
|
|
|
|
}
|
|
|
|
|
|
|
|
unsigned g_isi_modem_index(GIsiModem *modem)
|
|
|
|
{
|
2010-12-03 07:53:32 +00:00
|
|
|
return modem != NULL ? modem->index : 0;
|
2010-11-14 16:23:32 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
GIsiPending *g_isi_request_send(GIsiModem *modem, uint8_t resource,
|
|
|
|
const void *__restrict buf, size_t len,
|
|
|
|
unsigned timeout, GIsiNotifyFunc notify,
|
|
|
|
void *data, GDestroyNotify destroy)
|
|
|
|
{
|
|
|
|
struct sockaddr_pn dst = {
|
|
|
|
.spn_family = AF_PHONET,
|
|
|
|
.spn_resource = resource,
|
2011-02-04 12:54:41 +00:00
|
|
|
.spn_dev = modem->device,
|
2010-11-14 16:23:32 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
return g_isi_request_sendto(modem, &dst, buf, len, timeout, notify,
|
|
|
|
data, destroy);
|
|
|
|
};
|
|
|
|
|
|
|
|
GIsiPending *g_isi_request_vsend(GIsiModem *modem, uint8_t resource,
|
|
|
|
const struct iovec *__restrict iov,
|
|
|
|
size_t iovlen, unsigned timeout,
|
|
|
|
GIsiNotifyFunc notify, void *data,
|
|
|
|
GDestroyNotify destroy)
|
|
|
|
{
|
|
|
|
struct sockaddr_pn dst = {
|
|
|
|
.spn_family = AF_PHONET,
|
|
|
|
.spn_resource = resource,
|
2011-02-04 12:54:41 +00:00
|
|
|
.spn_dev = modem->device,
|
2010-11-14 16:23:32 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
return g_isi_request_vsendto(modem, &dst, iov, iovlen, timeout, notify,
|
|
|
|
data, destroy);
|
|
|
|
}
|
|
|
|
|
|
|
|
GIsiPending *g_isi_request_sendto(GIsiModem *modem, struct sockaddr_pn *dst,
|
|
|
|
const void *__restrict buf, size_t len,
|
|
|
|
unsigned timeout, GIsiNotifyFunc notify,
|
|
|
|
void *data, GDestroyNotify destroy)
|
|
|
|
{
|
|
|
|
const struct iovec iov = {
|
|
|
|
.iov_base = (void *)buf,
|
|
|
|
.iov_len = len,
|
|
|
|
};
|
|
|
|
|
|
|
|
return g_isi_request_vsendto(modem, dst, &iov, 1, timeout, notify, data,
|
|
|
|
destroy);
|
|
|
|
}
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
static void vtrace(struct sockaddr_pn *dst,
|
2010-11-14 16:23:32 +00:00
|
|
|
const struct iovec *__restrict iov, size_t iovlen,
|
2010-12-03 07:55:04 +00:00
|
|
|
size_t total_len, GIsiNotifyFunc trace)
|
2010-11-14 16:23:32 +00:00
|
|
|
{
|
|
|
|
uint8_t buffer[total_len];
|
|
|
|
uint8_t *ptr = buffer;
|
|
|
|
GIsiMessage msg = {
|
|
|
|
.addr = dst,
|
|
|
|
.data = (const void *)buffer,
|
|
|
|
.len = total_len,
|
|
|
|
};
|
|
|
|
size_t i;
|
|
|
|
|
|
|
|
for (i = 0; i < iovlen; i++) {
|
|
|
|
memcpy(ptr, iov[i].iov_base, iov[i].iov_len);
|
|
|
|
ptr += iov[i].iov_len;
|
|
|
|
}
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
trace(&msg, NULL);
|
2010-11-14 16:23:32 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static gboolean resp_timeout(gpointer data)
|
|
|
|
{
|
2011-01-12 19:24:09 +00:00
|
|
|
GIsiPending *op = data;
|
2010-11-14 16:23:32 +00:00
|
|
|
GIsiMessage msg = {
|
|
|
|
.error = ETIMEDOUT,
|
|
|
|
};
|
|
|
|
|
2011-01-12 19:24:09 +00:00
|
|
|
op->timeout = 0;
|
|
|
|
|
|
|
|
pending_remove_and_dispatch(op, &msg);
|
2010-11-14 16:23:32 +00:00
|
|
|
|
|
|
|
return FALSE;
|
|
|
|
}
|
|
|
|
|
|
|
|
GIsiPending *g_isi_request_vsendto(GIsiModem *modem, struct sockaddr_pn *dst,
|
|
|
|
const struct iovec *__restrict iov,
|
|
|
|
size_t iovlen, unsigned timeout,
|
|
|
|
GIsiNotifyFunc notify, void *data,
|
|
|
|
GDestroyNotify destroy)
|
|
|
|
{
|
|
|
|
struct iovec _iov[1 + iovlen];
|
|
|
|
struct msghdr msg = {
|
|
|
|
.msg_name = (void *)dst,
|
|
|
|
.msg_namelen = sizeof(struct sockaddr_pn),
|
|
|
|
.msg_iov = _iov,
|
|
|
|
.msg_iovlen = 1 + iovlen,
|
|
|
|
.msg_control = NULL,
|
|
|
|
.msg_controllen = 0,
|
|
|
|
.msg_flags = 0,
|
|
|
|
};
|
|
|
|
ssize_t ret;
|
|
|
|
size_t i, len;
|
|
|
|
|
|
|
|
GIsiServiceMux *mux;
|
|
|
|
GIsiPending *resp;
|
|
|
|
|
2010-12-03 07:53:32 +00:00
|
|
|
if (modem == NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
errno = EINVAL;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
mux = service_get(modem, dst->spn_resource);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (mux == NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
errno = ENOMEM;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
resp = g_try_new0(GIsiPending, 1);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (resp == NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
errno = ENOMEM;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
resp->type = GISI_MESSAGE_TYPE_RESP;
|
|
|
|
resp->utid = service_next_utid(mux);
|
|
|
|
resp->service = mux;
|
|
|
|
resp->notify = notify;
|
|
|
|
resp->destroy = destroy;
|
|
|
|
resp->data = data;
|
|
|
|
|
|
|
|
if (g_slist_find_custom(mux->pending, resp, utid_equal)) {
|
|
|
|
/*
|
|
|
|
* FIXME: perhaps retry with randomized access after
|
|
|
|
* initial miss. Although if the rate at which
|
|
|
|
* requests are sent is so high that the unique
|
|
|
|
* transaction ID wraps, it's likely there is
|
|
|
|
* something wrong and we might as well fail here.
|
|
|
|
*/
|
2010-12-03 07:55:04 +00:00
|
|
|
ISIDBG(modem, "ERROR: UTID wrapped, modem busy");
|
2010-11-14 16:23:32 +00:00
|
|
|
errno = EBUSY;
|
|
|
|
goto error;
|
|
|
|
}
|
|
|
|
|
|
|
|
_iov[0].iov_base = &resp->utid;
|
|
|
|
_iov[0].iov_len = 1;
|
|
|
|
|
|
|
|
for (i = 0, len = 1; i < iovlen; i++) {
|
|
|
|
_iov[1 + i] = iov[i];
|
|
|
|
len += iov[i].iov_len;
|
|
|
|
}
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
if (modem->trace != NULL)
|
|
|
|
vtrace(dst, _iov, 1 + iovlen, len, modem->trace);
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-16 15:06:11 +00:00
|
|
|
ret = sendmsg(modem->req_fd, &msg, MSG_NOSIGNAL);
|
2010-11-14 16:23:32 +00:00
|
|
|
if (ret == -1)
|
|
|
|
goto error;
|
|
|
|
|
|
|
|
if (ret != (ssize_t)len) {
|
|
|
|
errno = EMSGSIZE;
|
|
|
|
goto error;
|
|
|
|
}
|
|
|
|
|
|
|
|
mux->pending = g_slist_prepend(mux->pending, resp);
|
|
|
|
|
2010-12-03 07:53:32 +00:00
|
|
|
if (timeout > 0)
|
2010-12-22 15:47:11 +00:00
|
|
|
resp->timeout = g_timeout_add_seconds(timeout, resp_timeout,
|
|
|
|
resp);
|
2010-11-14 16:23:32 +00:00
|
|
|
|
|
|
|
mux->last_utid = resp->utid;
|
|
|
|
return resp;
|
|
|
|
|
|
|
|
error:
|
|
|
|
g_free(resp);
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
uint8_t g_isi_request_utid(GIsiPending *resp)
|
|
|
|
{
|
2010-12-03 07:53:32 +00:00
|
|
|
return resp != NULL ? resp->utid : 0;
|
2010-11-14 16:23:32 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
void g_isi_pending_remove(GIsiPending *op)
|
|
|
|
{
|
2010-12-03 07:53:32 +00:00
|
|
|
if (op == NULL)
|
2010-11-14 16:23:32 +00:00
|
|
|
return;
|
|
|
|
|
|
|
|
if (op->type == GISI_MESSAGE_TYPE_IND)
|
|
|
|
service_subs_decr(op->service);
|
|
|
|
|
|
|
|
if (op->type == GISI_MESSAGE_TYPE_REQ)
|
|
|
|
service_regs_decr(op->service);
|
|
|
|
|
2010-12-03 07:53:32 +00:00
|
|
|
if (op->type == GISI_MESSAGE_TYPE_RESP && op->notify != NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
GIsiMessage msg = {
|
|
|
|
.error = ESHUTDOWN,
|
|
|
|
};
|
2011-01-12 19:24:09 +00:00
|
|
|
|
|
|
|
pending_remove_and_dispatch(op, &msg);
|
|
|
|
return;
|
2010-11-14 16:23:32 +00:00
|
|
|
}
|
|
|
|
|
2011-01-12 19:24:09 +00:00
|
|
|
op->service->pending = g_slist_remove(op->service->pending, op);
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
pending_destroy(op, NULL);
|
|
|
|
}
|
|
|
|
|
2011-01-18 21:27:08 +00:00
|
|
|
static void foreach_destroy(GIsiPending *op)
|
|
|
|
{
|
|
|
|
if (op->type == GISI_MESSAGE_TYPE_IND)
|
|
|
|
service_subs_decr(op->service);
|
|
|
|
|
|
|
|
if (op->type == GISI_MESSAGE_TYPE_REQ)
|
|
|
|
service_regs_decr(op->service);
|
|
|
|
|
|
|
|
if (op->type == GISI_MESSAGE_TYPE_RESP && op->notify != NULL) {
|
|
|
|
GIsiMessage msg = {
|
|
|
|
.error = ESHUTDOWN,
|
|
|
|
};
|
|
|
|
|
|
|
|
pending_dispatch(op, &msg);
|
|
|
|
}
|
|
|
|
|
|
|
|
pending_destroy(op, NULL);
|
|
|
|
}
|
|
|
|
|
|
|
|
void g_isi_pending_set_owner(GIsiPending *op, gpointer owner)
|
|
|
|
{
|
|
|
|
if (op == NULL)
|
|
|
|
return;
|
|
|
|
|
|
|
|
op->owner = owner;
|
|
|
|
}
|
|
|
|
|
|
|
|
void g_isi_remove_pending_by_owner(GIsiModem *modem, uint8_t resource,
|
|
|
|
gpointer owner)
|
|
|
|
{
|
|
|
|
GIsiServiceMux *mux;
|
|
|
|
GSList *l;
|
|
|
|
GSList *next;
|
|
|
|
GIsiPending *op;
|
|
|
|
GSList *owned = NULL;
|
|
|
|
|
|
|
|
mux = service_get(modem, resource);
|
|
|
|
if (mux == NULL)
|
|
|
|
return;
|
|
|
|
|
|
|
|
for (l = mux->pending; l != NULL; l = next) {
|
|
|
|
next = l->next;
|
|
|
|
op = l->data;
|
|
|
|
|
|
|
|
if (op->owner != owner)
|
|
|
|
continue;
|
|
|
|
|
|
|
|
mux->pending = g_slist_remove_link(mux->pending, l);
|
|
|
|
|
|
|
|
l->next = owned;
|
|
|
|
owned = l;
|
|
|
|
}
|
|
|
|
|
|
|
|
for (l = owned; l != NULL; l = l->next) {
|
|
|
|
op = l->data;
|
|
|
|
|
|
|
|
foreach_destroy(op);
|
|
|
|
}
|
|
|
|
|
|
|
|
g_slist_free(owned);
|
|
|
|
}
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
GIsiPending *g_isi_ntf_subscribe(GIsiModem *modem, uint8_t resource,
|
|
|
|
uint8_t msgid, GIsiNotifyFunc notify,
|
|
|
|
void *data, GDestroyNotify destroy)
|
|
|
|
{
|
|
|
|
GIsiServiceMux *mux;
|
|
|
|
GIsiPending *ntf;
|
|
|
|
|
|
|
|
mux = service_get(modem, resource);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (mux == NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
errno = ENOMEM;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
ntf = g_try_new0(GIsiPending, 1);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (ntf == NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
errno = ENOMEM;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
ntf->type = GISI_MESSAGE_TYPE_NTF;
|
|
|
|
ntf->service = mux;
|
|
|
|
ntf->notify = notify;
|
|
|
|
ntf->data = data;
|
|
|
|
ntf->destroy = destroy;
|
|
|
|
ntf->msgid = msgid;
|
|
|
|
|
|
|
|
mux->pending = g_slist_append(mux->pending, ntf);
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
ISIDBG(modem, "Subscribed to %s (%p) [res=0x%02X, id=0x%02X]",
|
|
|
|
pend_type_to_str(ntf->type), ntf, resource, msgid);
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
return ntf;
|
|
|
|
}
|
|
|
|
|
|
|
|
GIsiPending *g_isi_service_bind(GIsiModem *modem, uint8_t resource,
|
|
|
|
uint8_t msgid, GIsiNotifyFunc notify,
|
|
|
|
void *data, GDestroyNotify destroy)
|
|
|
|
{
|
|
|
|
GIsiServiceMux *mux;
|
|
|
|
GIsiPending *srv;
|
|
|
|
|
|
|
|
mux = service_get(modem, resource);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (mux == NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
errno = ENOMEM;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
srv = g_try_new0(GIsiPending, 1);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (srv == NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
errno = ENOMEM;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
srv->type = GISI_MESSAGE_TYPE_REQ;
|
|
|
|
srv->service = mux;
|
|
|
|
srv->notify = notify;
|
|
|
|
srv->data = data;
|
|
|
|
srv->destroy = destroy;
|
|
|
|
srv->msgid = msgid;
|
|
|
|
|
|
|
|
mux->pending = g_slist_append(mux->pending, srv);
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
ISIDBG(modem, "Bound service for %s (%p) [res=0x%02X, id=0x%02X]",
|
|
|
|
pend_type_to_str(srv->type), srv, resource, msgid);
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
service_regs_incr(mux);
|
|
|
|
|
|
|
|
return srv;
|
|
|
|
}
|
|
|
|
|
|
|
|
GIsiPending *g_isi_ind_subscribe(GIsiModem *modem, uint8_t resource,
|
|
|
|
uint8_t msgid, GIsiNotifyFunc notify,
|
|
|
|
void *data, GDestroyNotify destroy)
|
|
|
|
{
|
|
|
|
GIsiServiceMux *mux;
|
|
|
|
GIsiPending *ind;
|
|
|
|
|
|
|
|
mux = service_get(modem, resource);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (mux == NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
errno = ENOMEM;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
ind = g_try_new0(GIsiPending, 1);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (ind == NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
errno = ENOMEM;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
ind->type = GISI_MESSAGE_TYPE_IND;
|
|
|
|
ind->service = mux;
|
|
|
|
ind->notify = notify;
|
|
|
|
ind->data = data;
|
|
|
|
ind->destroy = destroy;
|
|
|
|
ind->msgid = msgid;
|
|
|
|
|
|
|
|
mux->pending = g_slist_append(mux->pending, ind);
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
ISIDBG(modem, "Subscribed for %s (%p) [res=0x%02X, id=0x%02X]",
|
|
|
|
pend_type_to_str(ind->type), ind, resource, msgid);
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
service_subs_incr(mux);
|
|
|
|
|
|
|
|
return ind;
|
|
|
|
}
|
|
|
|
|
|
|
|
int g_isi_response_send(GIsiModem *modem, const GIsiMessage *req,
|
|
|
|
const void *__restrict buf, size_t len)
|
|
|
|
{
|
|
|
|
const struct iovec iov = {
|
|
|
|
.iov_base = (void *)buf,
|
|
|
|
.iov_len = len,
|
|
|
|
};
|
|
|
|
|
|
|
|
return g_isi_response_vsend(modem, req, &iov, 1);
|
|
|
|
}
|
|
|
|
|
|
|
|
int g_isi_response_vsend(GIsiModem *modem, const GIsiMessage *req,
|
|
|
|
const struct iovec *__restrict iov,
|
|
|
|
size_t iovlen)
|
|
|
|
{
|
|
|
|
struct iovec _iov[1 + iovlen];
|
|
|
|
uint8_t utid;
|
|
|
|
size_t i;
|
|
|
|
|
|
|
|
utid = g_isi_msg_utid(req);
|
|
|
|
|
|
|
|
_iov[0].iov_base = &utid;
|
|
|
|
_iov[0].iov_len = 1;
|
|
|
|
|
|
|
|
for (i = 0; i < iovlen; i++)
|
|
|
|
_iov[1 + i] = iov[i];
|
|
|
|
|
|
|
|
return g_isi_modem_vsendto(modem, req->addr, _iov, 1 + iovlen);
|
|
|
|
}
|
|
|
|
|
2010-12-31 12:48:59 +00:00
|
|
|
int g_isi_modem_send(GIsiModem *modem, uint8_t resource,
|
|
|
|
const void *__restrict buf, size_t len)
|
|
|
|
{
|
|
|
|
struct sockaddr_pn dst = {
|
|
|
|
.spn_family = AF_PHONET,
|
|
|
|
.spn_resource = resource,
|
2011-02-04 12:54:41 +00:00
|
|
|
.spn_dev = modem->device,
|
2010-12-31 12:48:59 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
return g_isi_modem_sendto(modem, &dst, buf, len);
|
|
|
|
}
|
|
|
|
|
|
|
|
int g_isi_modem_vsend(GIsiModem *modem, uint8_t resource,
|
|
|
|
const struct iovec *__restrict iov,
|
|
|
|
size_t iovlen)
|
|
|
|
{
|
|
|
|
struct sockaddr_pn dst = {
|
|
|
|
.spn_family = AF_PHONET,
|
|
|
|
.spn_resource = resource,
|
2011-02-04 12:54:41 +00:00
|
|
|
.spn_dev = modem->device,
|
2010-12-31 12:48:59 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
return g_isi_modem_vsendto(modem, &dst, iov, iovlen);
|
|
|
|
}
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
int g_isi_modem_sendto(GIsiModem *modem, struct sockaddr_pn *dst,
|
|
|
|
const void *__restrict buf, size_t len)
|
|
|
|
{
|
|
|
|
const struct iovec iov = {
|
|
|
|
.iov_base = (void *)buf,
|
|
|
|
.iov_len = len,
|
|
|
|
};
|
|
|
|
|
|
|
|
return g_isi_modem_vsendto(modem, dst, &iov, 1);
|
|
|
|
}
|
|
|
|
|
|
|
|
int g_isi_modem_vsendto(GIsiModem *modem, struct sockaddr_pn *dst,
|
|
|
|
const struct iovec *__restrict iov,
|
|
|
|
size_t iovlen)
|
|
|
|
{
|
|
|
|
struct msghdr msg = {
|
|
|
|
.msg_name = (void *)dst,
|
|
|
|
.msg_namelen = sizeof(struct sockaddr_pn),
|
|
|
|
.msg_iov = (struct iovec *)iov,
|
|
|
|
.msg_iovlen = iovlen,
|
|
|
|
.msg_control = NULL,
|
|
|
|
.msg_controllen = 0,
|
|
|
|
.msg_flags = 0,
|
|
|
|
};
|
|
|
|
ssize_t ret;
|
|
|
|
size_t i, len;
|
|
|
|
GIsiServiceMux *mux;
|
|
|
|
|
2010-12-03 07:53:32 +00:00
|
|
|
if (modem == NULL)
|
2010-11-14 16:23:32 +00:00
|
|
|
return -EINVAL;
|
|
|
|
|
|
|
|
mux = service_get(modem, dst->spn_resource);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (mux == NULL)
|
2010-11-14 16:23:32 +00:00
|
|
|
return -ENOMEM;
|
|
|
|
|
|
|
|
for (i = 0, len = 0; i < iovlen; i++)
|
|
|
|
len += iov[i].iov_len;
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
if (modem->trace != NULL)
|
|
|
|
vtrace(dst, iov, iovlen, len, modem->trace);
|
2010-11-14 16:23:32 +00:00
|
|
|
|
2010-11-16 15:06:11 +00:00
|
|
|
ret = sendmsg(modem->req_fd, &msg, MSG_NOSIGNAL);
|
2010-11-14 16:23:32 +00:00
|
|
|
if (ret == -1)
|
|
|
|
return -errno;
|
|
|
|
|
|
|
|
if (ret != (ssize_t)len)
|
|
|
|
return -EMSGSIZE;
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
void g_isi_modem_set_trace(GIsiModem *modem, GIsiNotifyFunc trace)
|
|
|
|
{
|
|
|
|
if (modem == NULL)
|
|
|
|
return;
|
|
|
|
|
|
|
|
modem->trace = trace;
|
|
|
|
}
|
|
|
|
|
|
|
|
void g_isi_modem_set_debug(GIsiModem *modem, GIsiDebugFunc debug)
|
2010-11-14 16:23:32 +00:00
|
|
|
{
|
2010-12-03 07:55:04 +00:00
|
|
|
if (modem == NULL)
|
2010-11-14 16:23:32 +00:00
|
|
|
return;
|
|
|
|
|
|
|
|
modem->debug = debug;
|
|
|
|
}
|
|
|
|
|
|
|
|
static int version_get_send(GIsiModem *modem, GIsiPending *ping)
|
|
|
|
{
|
|
|
|
GIsiServiceMux *mux = ping->service;
|
2011-02-04 12:54:41 +00:00
|
|
|
struct sockaddr_pn dst = {
|
2010-11-14 16:23:32 +00:00
|
|
|
.spn_family = AF_PHONET,
|
|
|
|
.spn_resource = mux->resource,
|
2011-02-04 12:54:41 +00:00
|
|
|
.spn_dev = modem->device,
|
2010-11-14 16:23:32 +00:00
|
|
|
};
|
|
|
|
uint8_t msg[] = {
|
|
|
|
ping->utid, /* UTID */
|
|
|
|
COMMON_MESSAGE,
|
|
|
|
COMM_ISI_VERSION_GET_REQ,
|
|
|
|
0, /* Filler */
|
|
|
|
};
|
|
|
|
ssize_t ret;
|
|
|
|
|
|
|
|
if (g_slist_find_custom(mux->pending, ping, utid_equal))
|
|
|
|
return -EBUSY;
|
|
|
|
|
2010-11-16 15:06:11 +00:00
|
|
|
ret = sendto(modem->req_fd, msg, sizeof(msg), MSG_NOSIGNAL,
|
|
|
|
(void *)&dst, sizeof(dst));
|
2010-11-14 16:23:32 +00:00
|
|
|
|
|
|
|
if (ret == -1)
|
|
|
|
return -errno;
|
|
|
|
|
|
|
|
if (ret != (ssize_t)sizeof(msg))
|
|
|
|
return -EMSGSIZE;
|
|
|
|
|
|
|
|
mux->last_utid = ping->utid;
|
|
|
|
mux->version_pending = TRUE;
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
static gboolean reachable_notify(gpointer data)
|
|
|
|
{
|
|
|
|
GIsiPending *pong = data;
|
|
|
|
GIsiServiceMux *mux = pong->service;
|
|
|
|
|
|
|
|
struct sockaddr_pn addr = {
|
|
|
|
.spn_resource = mux->resource,
|
2010-11-29 09:36:11 +00:00
|
|
|
.spn_dev = mux->object >> 8,
|
|
|
|
.spn_obj = mux->object & 0xff,
|
2010-11-14 16:23:32 +00:00
|
|
|
};
|
|
|
|
GIsiMessage msg = {
|
|
|
|
.version = &mux->version,
|
|
|
|
.addr = &addr,
|
|
|
|
};
|
|
|
|
|
2011-01-12 19:24:09 +00:00
|
|
|
pending_remove_and_dispatch(pong, &msg);
|
2010-11-14 16:23:32 +00:00
|
|
|
|
|
|
|
return FALSE;
|
|
|
|
}
|
|
|
|
|
|
|
|
GIsiPending *g_isi_resource_ping(GIsiModem *modem, uint8_t resource,
|
|
|
|
GIsiNotifyFunc notify, void *data,
|
|
|
|
GDestroyNotify destroy)
|
|
|
|
{
|
|
|
|
GIsiServiceMux *mux;
|
|
|
|
GIsiPending *ping;
|
|
|
|
int ret;
|
|
|
|
|
|
|
|
mux = service_get(modem, resource);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (mux == NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
errno = ENOMEM;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
ping = g_try_new0(GIsiPending, 1);
|
2010-12-03 07:53:32 +00:00
|
|
|
if (ping == NULL) {
|
2010-11-14 16:23:32 +00:00
|
|
|
errno = ENOMEM;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
ping->type = GISI_MESSAGE_TYPE_COMMON;
|
|
|
|
ping->utid = service_next_utid(mux);
|
|
|
|
ping->service = mux;
|
|
|
|
ping->notify = notify;
|
|
|
|
ping->data = data;
|
|
|
|
ping->destroy = destroy;
|
|
|
|
ping->msgid = COMM_ISI_VERSION_GET_REQ;
|
|
|
|
|
|
|
|
if (mux->reachable) {
|
|
|
|
g_idle_add(reachable_notify, ping);
|
|
|
|
return ping;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!mux->version_pending) {
|
|
|
|
ret = version_get_send(modem, ping);
|
|
|
|
if (ret < 0) {
|
|
|
|
g_free(ping);
|
|
|
|
errno = ret;
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
mux->last_utid = ping->utid;
|
|
|
|
}
|
|
|
|
|
|
|
|
ping->timeout = g_timeout_add_seconds(COMMON_TIMEOUT, resp_timeout,
|
|
|
|
ping);
|
|
|
|
mux->pending = g_slist_prepend(mux->pending, ping);
|
|
|
|
mux->version_pending = TRUE;
|
2010-04-21 07:47:16 +00:00
|
|
|
|
2010-12-03 07:55:04 +00:00
|
|
|
ISIDBG(modem, "Ping sent %s (%p) [res=0x%02X]",
|
|
|
|
pend_type_to_str(ping->type), ping, resource);
|
|
|
|
|
2010-11-14 16:23:32 +00:00
|
|
|
return ping;
|
2010-04-21 07:47:16 +00:00
|
|
|
}
|