| /* |
| * |
| * oFono - Open Source Telephony |
| * |
| * Copyright (C) 2009-2010 Nokia Corporation and/or its subsidiary(-ies). |
| * |
| * 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 |
| * |
| */ |
| |
| #ifdef HAVE_CONFIG_H |
| #include <config.h> |
| #endif |
| |
| #define _GNU_SOURCE |
| #include <stdint.h> |
| #include <string.h> |
| #include <sys/types.h> |
| #include <sys/socket.h> |
| #include <sys/ioctl.h> |
| #include <sys/uio.h> |
| #include <net/if.h> |
| #include <errno.h> |
| #include <glib.h> |
| |
| #include "message.h" |
| #include "common.h" |
| #include "modem.h" |
| #include "socket.h" |
| |
| #define ISIDBG(m, fmt, ...) \ |
| if ((m) != NULL && (m)->debug != NULL) \ |
| m->debug("gisi: "fmt, ##__VA_ARGS__); |
| |
| struct _GIsiServiceMux { |
| GIsiModem *modem; |
| GSList *pending; |
| GIsiVersion version; |
| uint8_t resource; |
| uint8_t last_utid; |
| uint16_t object; |
| unsigned subscriptions; |
| unsigned registrations; |
| gboolean reachable; |
| gboolean version_pending; |
| }; |
| typedef struct _GIsiServiceMux GIsiServiceMux; |
| |
| struct _GIsiModem { |
| unsigned index; |
| uint8_t device; |
| GHashTable *services; |
| gboolean subs_source; |
| int req_fd; |
| int ind_fd; |
| guint req_watch; |
| guint ind_watch; |
| GIsiDebugFunc debug; |
| GIsiNotifyFunc trace; |
| void *opaque; |
| unsigned long flags; |
| }; |
| |
| struct _GIsiPending { |
| enum GIsiMessageType type; |
| GIsiServiceMux *service; |
| gpointer owner; |
| 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; |
| int key = resource; |
| |
| mux = g_hash_table_lookup(modem->services, GINT_TO_POINTER(key)); |
| if (mux != NULL) |
| return mux; |
| |
| mux = g_try_new0(GIsiServiceMux, 1); |
| if (mux == NULL) |
| return NULL; |
| |
| g_hash_table_insert(modem->services, GINT_TO_POINTER(key), mux); |
| |
| mux->modem = modem; |
| mux->resource = resource; |
| mux->version.major = -1; |
| mux->version.minor = -1; |
| mux->reachable = FALSE; |
| mux->version_pending = FALSE; |
| |
| return mux; |
| } |
| |
| static gint utid_equal(gconstpointer a, gconstpointer b) |
| { |
| const GIsiPending *pa = a; |
| const GIsiPending *pb = b; |
| |
| return pa->utid - pb->utid; |
| } |
| |
| 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"; |
| } |
| |
| static void pending_dispatch(GIsiPending *pend, GIsiMessage *msg) |
| { |
| GIsiModem *modem; |
| |
| if (pend->notify == NULL) |
| return; |
| |
| 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, |
| g_isi_msg_resource(msg), g_isi_msg_id(msg), |
| g_isi_msg_utid(msg)); |
| |
| pend->notify(msg, pend->data); |
| } |
| |
| 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]", |
| g_isi_msg_error(msg) ? g_isi_msg_strerror(msg) : "normal", |
| pend_type_to_str(op->type), op, |
| 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); |
| } |
| |
| static void service_dispatch(GIsiServiceMux *mux, GIsiMessage *msg, |
| gboolean is_indication) |
| { |
| uint8_t msgid = g_isi_msg_id(msg); |
| uint8_t utid = g_isi_msg_utid(msg); |
| |
| GSList *l = mux->pending; |
| |
| while (l != NULL) { |
| 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. |
| * |
| * Version query responses are dispatched in a similar fashion |
| * as RESPs, but based on the pending type and the message ID. |
| * Some of these may be synthesized, but nevertheless need to |
| * be removed. |
| */ |
| if (pend->type < GISI_MESSAGE_TYPE_RESP |
| && pend->msgid == msgid) { |
| |
| pending_dispatch(pend, msg); |
| |
| } else if (pend->type == GISI_MESSAGE_TYPE_RESP && |
| !is_indication && pend->utid == utid) { |
| |
| pending_remove_and_dispatch(pend, msg); |
| break; |
| |
| } else if (pend->type == GISI_MESSAGE_TYPE_COMMON && |
| msgid == COMMON_MESSAGE && |
| pend->msgid == COMM_ISI_VERSION_GET_REQ) { |
| |
| pending_remove_and_dispatch(pend, msg); |
| } |
| |
| 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) { |
| case COMM_ISA_ENTITY_NOT_REACHABLE_RESP: |
| mux->reachable = FALSE; |
| msg->error = ENOENT; |
| break; |
| |
| 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; |
| } |
| /* fall through */ |
| |
| default: |
| /* |
| * PN_SIM doesn't support ISI version, but sends a |
| * garbage message as a response. Work around this |
| * modem wart. |
| */ |
| mux->object = g_isi_msg_object(msg); |
| mux->version_pending = FALSE; |
| mux->reachable = TRUE; |
| break; |
| } |
| msg->version = &mux->version; |
| } |
| |
| 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); |
| } |
| |
| static gboolean isi_callback(GIOChannel *channel, GIOCondition cond, |
| gpointer data) |
| { |
| GIsiModem *modem = data; |
| int len; |
| int fd; |
| |
| if (cond & (G_IO_NVAL|G_IO_HUP)) { |
| ISIDBG(modem, "Unexpected event on PhoNet channel %p", channel); |
| return FALSE; |
| } |
| |
| fd = g_io_channel_unix_get_fd(channel); |
| 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; |
| |
| if (modem->trace != NULL) |
| modem->trace(&msg, NULL); |
| |
| key = addr.spn_resource; |
| mux = g_hash_table_lookup(modem->services, |
| GINT_TO_POINTER(key)); |
| 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); |
| |
| return TRUE; |
| } |
| |
| msg.version = &mux->version; |
| |
| if (g_isi_msg_id(&msg) == COMMON_MESSAGE) |
| common_message_decode(mux, &msg); |
| |
| service_dispatch(mux, &msg, fd == modem->ind_fd); |
| } |
| return TRUE; |
| } |
| |
| static gboolean modem_subs_update(gpointer data) |
| { |
| GHashTableIter iter; |
| gpointer keyptr, value; |
| |
| GIsiModem *modem = data; |
| gboolean legacy = modem->flags & GISI_MODEM_FLAG_USE_LEGACY_SUBSCRIBE; |
| struct sockaddr_pn commgr = { |
| .spn_family = AF_PHONET, |
| .spn_resource = PN_COMMGR, |
| .spn_dev = modem->device, |
| }; |
| uint8_t msg[4 + 1024] = { |
| 0, /* UTID */ |
| legacy ? PNS_SUBSCRIBED_RESOURCES_IND : |
| PNS_SUBSCRIBED_RESOURCES_EXTEND_IND, |
| 0, /* Count */ |
| 0, /* Filler */ |
| }; |
| uint8_t count = 0; |
| size_t len; |
| |
| modem->subs_source = 0; |
| |
| g_hash_table_iter_init(&iter, modem->services); |
| |
| while (g_hash_table_iter_next(&iter, &keyptr, &value)) { |
| GIsiServiceMux *mux = value; |
| |
| if (mux->subscriptions == 0) |
| continue; |
| |
| if (legacy) |
| msg[3 + count] = mux->resource; |
| else |
| /* Resource field is 32bit and Little-endian */ |
| msg[4 + count * 4 + 3] = mux->resource; |
| |
| count++; |
| } |
| |
| len = legacy ? 3 + count : 4 + count * 4; |
| msg[2] = count; |
| |
| sendto(modem->ind_fd, msg, len, MSG_NOSIGNAL, (void *) &commgr, |
| sizeof(commgr)); |
| |
| return FALSE; |
| } |
| |
| static void modem_subs_update_when_idle(GIsiModem *modem) |
| { |
| if (modem->subs_source > 0) |
| return; |
| |
| modem->subs_source = g_idle_add(modem_subs_update, modem); |
| } |
| |
| static void service_name_register(GIsiServiceMux *mux) |
| { |
| struct sockaddr_pn namesrv = { |
| .spn_family = AF_PHONET, |
| .spn_resource = PN_NAMESERVICE, |
| .spn_dev = mux->modem->device, |
| }; |
| 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; |
| |
| if (ioctl(mux->modem->req_fd, SIOCPNGETOBJECT, &object) < 0) { |
| ISIDBG(mux->modem, "ioctl(SIOCPNGETOBJECT): %s", |
| strerror(errno)); |
| return; |
| } |
| |
| /* Fill in the object ID */ |
| msg[8] = object >> 8; |
| msg[9] = object & 0xFF; |
| |
| sendto(mux->modem->req_fd, msg, sizeof(msg), MSG_NOSIGNAL, |
| (void *) &namesrv, sizeof(namesrv)); |
| } |
| |
| static void service_name_deregister(GIsiServiceMux *mux) |
| { |
| struct sockaddr_pn namesrv = { |
| .spn_family = AF_PHONET, |
| .spn_resource = PN_NAMESERVICE, |
| .spn_dev = mux->modem->device, |
| }; |
| const uint8_t msg[] = { |
| 0, PNS_NAME_REMOVE_REQ, 0, 0, |
| 0, 0, 0, mux->resource, |
| }; |
| |
| sendto(mux->modem->req_fd, msg, sizeof(msg), MSG_NOSIGNAL, |
| (void *) &namesrv, sizeof(namesrv)); |
| } |
| |
| static void pending_destroy(gpointer value, gpointer user) |
| { |
| GIsiPending *op = value; |
| |
| if (op == NULL) |
| return; |
| |
| if (op->timeout > 0) |
| g_source_remove(op->timeout); |
| |
| if (op->destroy != NULL) |
| op->destroy(op->data); |
| |
| g_free(op); |
| } |
| |
| static void service_finalize(gpointer value) |
| { |
| GIsiServiceMux *mux = value; |
| GIsiModem *modem = mux->modem; |
| |
| if (mux->subscriptions > 0) |
| modem_subs_update_when_idle(modem); |
| |
| if (mux->registrations > 0) |
| service_name_deregister(mux); |
| |
| g_slist_foreach(mux->pending, pending_destroy, NULL); |
| g_slist_free(mux->pending); |
| g_free(mux); |
| } |
| |
| GIsiModem *g_isi_modem_create(unsigned index) |
| { |
| GIsiModem *modem; |
| GIOChannel *inds; |
| GIOChannel *reqs; |
| |
| if (index == 0) { |
| errno = ENODEV; |
| return NULL; |
| } |
| |
| modem = g_try_new0(GIsiModem, 1); |
| if (modem == NULL) { |
| errno = ENOMEM; |
| return NULL; |
| } |
| |
| inds = g_isi_phonet_new(index); |
| reqs = g_isi_phonet_new(index); |
| |
| if (inds == NULL || reqs == NULL) { |
| 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)); |
| } |
| |
| void *g_isi_modem_set_userdata(GIsiModem *modem, void *data) |
| { |
| void *old; |
| |
| if (modem == NULL) |
| return NULL; |
| |
| old = modem->opaque; |
| modem->opaque = data; |
| |
| return old; |
| } |
| |
| void *g_isi_modem_get_userdata(GIsiModem *modem) |
| { |
| if (modem == NULL) |
| return NULL; |
| |
| return modem->opaque; |
| } |
| |
| 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; |
| } |
| |
| 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; |
| } |
| |
| 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++; |
| |
| if (mux->subscriptions == 1) |
| modem_subs_update_when_idle(modem); |
| } |
| |
| static void service_subs_decr(GIsiServiceMux *mux) |
| { |
| GIsiModem *modem = mux->modem; |
| |
| if (mux->subscriptions == 0) |
| return; |
| |
| mux->subscriptions--; |
| |
| if (mux->subscriptions == 0) |
| modem_subs_update_when_idle(modem); |
| } |
| |
| 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) |
| { |
| if (modem == NULL) |
| return; |
| |
| g_hash_table_remove_all(modem->services); |
| |
| if (modem->subs_source > 0) { |
| g_source_remove(modem->subs_source); |
| modem_subs_update(modem); |
| } |
| |
| g_hash_table_unref(modem->services); |
| |
| if (modem->ind_watch > 0) |
| g_source_remove(modem->ind_watch); |
| |
| if (modem->req_watch > 0) |
| g_source_remove(modem->req_watch); |
| |
| g_free(modem); |
| } |
| |
| unsigned g_isi_modem_index(GIsiModem *modem) |
| { |
| return modem != NULL ? modem->index : 0; |
| } |
| |
| 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, |
| .spn_dev = modem->device, |
| }; |
| |
| 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, |
| .spn_dev = modem->device, |
| }; |
| |
| 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); |
| } |
| |
| static void vtrace(struct sockaddr_pn *dst, |
| const struct iovec *__restrict iov, size_t iovlen, |
| size_t total_len, GIsiNotifyFunc trace) |
| { |
| 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; |
| } |
| |
| trace(&msg, NULL); |
| } |
| |
| static gboolean resp_timeout(gpointer data) |
| { |
| GIsiPending *op = data; |
| GIsiMessage msg = { |
| .error = ETIMEDOUT, |
| }; |
| |
| op->timeout = 0; |
| |
| pending_remove_and_dispatch(op, &msg); |
| |
| 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; |
| |
| if (modem == NULL) { |
| errno = EINVAL; |
| return NULL; |
| } |
| |
| mux = service_get(modem, dst->spn_resource); |
| if (mux == NULL) { |
| errno = ENOMEM; |
| return NULL; |
| } |
| |
| resp = g_try_new0(GIsiPending, 1); |
| if (resp == NULL) { |
| 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. |
| */ |
| ISIDBG(modem, "ERROR: UTID wrapped, modem busy"); |
| 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; |
| } |
| |
| if (modem->trace != NULL) |
| vtrace(dst, _iov, 1 + iovlen, len, modem->trace); |
| |
| ret = sendmsg(modem->req_fd, &msg, MSG_NOSIGNAL); |
| if (ret == -1) |
| goto error; |
| |
| if (ret != (ssize_t)len) { |
| errno = EMSGSIZE; |
| goto error; |
| } |
| |
| mux->pending = g_slist_prepend(mux->pending, resp); |
| |
| if (timeout > 0) |
| resp->timeout = g_timeout_add_seconds(timeout, resp_timeout, |
| resp); |
| |
| mux->last_utid = resp->utid; |
| return resp; |
| |
| error: |
| g_free(resp); |
| return NULL; |
| } |
| |
| uint8_t g_isi_request_utid(GIsiPending *resp) |
| { |
| return resp != NULL ? resp->utid : 0; |
| } |
| |
| void g_isi_pending_remove(GIsiPending *op) |
| { |
| if (op == NULL) |
| 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); |
| |
| if (op->type == GISI_MESSAGE_TYPE_RESP && op->notify != NULL) { |
| GIsiMessage msg = { |
| .error = ESHUTDOWN, |
| }; |
| |
| pending_remove_and_dispatch(op, &msg); |
| return; |
| } |
| |
| op->service->pending = g_slist_remove(op->service->pending, op); |
| |
| pending_destroy(op, NULL); |
| } |
| |
| 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); |
| } |
| |
| 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); |
| if (mux == NULL) { |
| errno = ENOMEM; |
| return NULL; |
| } |
| |
| ntf = g_try_new0(GIsiPending, 1); |
| if (ntf == NULL) { |
| 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); |
| |
| ISIDBG(modem, "Subscribed to %s (%p) [res=0x%02X, id=0x%02X]", |
| pend_type_to_str(ntf->type), ntf, resource, msgid); |
| |
| 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); |
| if (mux == NULL) { |
| errno = ENOMEM; |
| return NULL; |
| } |
| |
| srv = g_try_new0(GIsiPending, 1); |
| if (srv == NULL) { |
| 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); |
| |
| ISIDBG(modem, "Bound service for %s (%p) [res=0x%02X, id=0x%02X]", |
| pend_type_to_str(srv->type), srv, resource, msgid); |
| |
| 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); |
| if (mux == NULL) { |
| errno = ENOMEM; |
| return NULL; |
| } |
| |
| ind = g_try_new0(GIsiPending, 1); |
| if (ind == NULL) { |
| 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); |
| |
| ISIDBG(modem, "Subscribed for %s (%p) [res=0x%02X, id=0x%02X]", |
| pend_type_to_str(ind->type), ind, resource, msgid); |
| |
| 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); |
| } |
| |
| 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, |
| .spn_dev = modem->device, |
| }; |
| |
| 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, |
| .spn_dev = modem->device, |
| }; |
| |
| return g_isi_modem_vsendto(modem, &dst, iov, iovlen); |
| } |
| |
| 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; |
| |
| if (modem == NULL) |
| return -EINVAL; |
| |
| mux = service_get(modem, dst->spn_resource); |
| if (mux == NULL) |
| return -ENOMEM; |
| |
| for (i = 0, len = 0; i < iovlen; i++) |
| len += iov[i].iov_len; |
| |
| if (modem->trace != NULL) |
| vtrace(dst, iov, iovlen, len, modem->trace); |
| |
| ret = sendmsg(modem->req_fd, &msg, MSG_NOSIGNAL); |
| if (ret == -1) |
| return -errno; |
| |
| if (ret != (ssize_t)len) |
| return -EMSGSIZE; |
| |
| return 0; |
| } |
| |
| 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) |
| { |
| if (modem == NULL) |
| return; |
| |
| modem->debug = debug; |
| } |
| |
| static int version_get_send(GIsiModem *modem, GIsiPending *ping) |
| { |
| GIsiServiceMux *mux = ping->service; |
| struct sockaddr_pn dst = { |
| .spn_family = AF_PHONET, |
| .spn_resource = mux->resource, |
| .spn_dev = modem->device, |
| }; |
| 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; |
| |
| ret = sendto(modem->req_fd, msg, sizeof(msg), MSG_NOSIGNAL, |
| (void *)&dst, sizeof(dst)); |
| |
| 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, |
| .spn_dev = mux->object >> 8, |
| .spn_obj = mux->object & 0xff, |
| }; |
| GIsiMessage msg = { |
| .version = &mux->version, |
| .addr = &addr, |
| }; |
| |
| pending_remove_and_dispatch(pong, &msg); |
| |
| 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); |
| if (mux == NULL) { |
| errno = ENOMEM; |
| return NULL; |
| } |
| |
| ping = g_try_new0(GIsiPending, 1); |
| if (ping == NULL) { |
| 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; |
| |
| ISIDBG(modem, "Ping sent %s (%p) [res=0x%02X]", |
| pend_type_to_str(ping->type), ping, resource); |
| |
| return ping; |
| } |