diff options
| -rw-r--r-- | Makefile.am | 4 | ||||
| -rw-r--r-- | drivers/isimodem/debug.c | 7 | ||||
| -rw-r--r-- | drivers/isimodem/isimodem.c | 284 | ||||
| -rw-r--r-- | drivers/isimodem/isimodem.h | 2 | ||||
| -rw-r--r-- | drivers/isimodem/mtc.h | 17 | ||||
| -rw-r--r-- | gisi/modem.h | 4 | ||||
| -rw-r--r-- | gisi/netlink.c | 270 | ||||
| -rw-r--r-- | gisi/netlink.h | 21 | ||||
| -rw-r--r-- | plugins/usbpnmodem.c | 98 |
9 files changed, 485 insertions, 222 deletions
diff --git a/Makefile.am b/Makefile.am index b6a47b05..062e464d 100644 --- a/Makefile.am +++ b/Makefile.am @@ -118,6 +118,10 @@ builtin_sources += $(gisi_sources) \ drivers/isimodem/gprs.c \ drivers/isimodem/gprs-context.c \ drivers/isimodem/gpds.h + +builtin_modules += usbpnmodem +builtin_sources += plugins/usbpnmodem.c + endif if ATMODEM diff --git a/drivers/isimodem/debug.c b/drivers/isimodem/debug.c index a2c2ef60..45313b4d 100644 --- a/drivers/isimodem/debug.c +++ b/drivers/isimodem/debug.c @@ -109,12 +109,18 @@ const char *mtc_isi_cause_name(enum mtc_isi_cause value) const char *mtc_message_id_name(enum mtc_message_id value) { switch (value) { + _(MTC_STATE_REQ); _(MTC_STATE_QUERY_REQ); _(MTC_POWER_OFF_REQ); _(MTC_POWER_ON_REQ); + _(MTC_STARTUP_SYNQ_REQ); + _(MTC_SHUTDOWN_SYNC_REQ); + _(MTC_STATE_RESP); _(MTC_STATE_QUERY_RESP); _(MTC_POWER_OFF_RESP); _(MTC_POWER_ON_RESP); + _(MTC_STARTUP_SYNQ_RESP); + _(MTC_SHUTDOWN_SYNC_RESP); _(MTC_STATE_INFO_IND); _(MTC_COMMON_MESSAGE); } @@ -140,6 +146,7 @@ const char *mtc_modem_state_name(enum mtc_modem_state value) _(MTC_DISK_WIPE); _(MTC_SW_RESET); _(MTC_CMT_ONLY_MODE); + _(MTC_STATE_NONE); } return "MTC_<UNKNOWN>"; } diff --git a/drivers/isimodem/isimodem.c b/drivers/isimodem/isimodem.c index 931ce7dc..e6e0bc15 100644 --- a/drivers/isimodem/isimodem.c +++ b/drivers/isimodem/isimodem.c @@ -60,27 +60,44 @@ struct isi_data { struct ofono_modem *modem; + char const *ifname; GIsiModem *idx; GIsiClient *client; + GPhonetNetlink *link; + unsigned interval; + int reported; + int mtc_state; + int iface_up; }; -static GPhonetNetlink *link = NULL; -static GSList *g_modems = NULL; - -static struct isi_data *find_modem_by_idx(GSList *modems, GIsiModem *idx) +static void report_powered(struct isi_data *isi, ofono_bool_t powered) { - GSList *m = NULL; - - for (m = g_modems; m; m = m->next) { - struct isi_data *isi = m->data; + if (powered != isi->reported) + ofono_modem_set_powered(isi->modem, isi->reported = powered); +} - if (isi->idx == idx) - return isi; +static void set_power_by_mtc_state(struct isi_data *isi, int state) +{ + switch (isi->mtc_state = state) + { + case MTC_STATE_NONE: + case MTC_POWER_OFF: + case MTC_CHARGING: + case MTC_SELFTEST_FAIL: + report_powered(isi, 0); + break; + + case MTC_RF_INACTIVE: + case MTC_NORMAL: + report_powered(isi, 1); + break; + + default: + report_powered(isi, 1); } - return NULL; } -static void mtc_state_cb(GIsiClient *client, const void *restrict data, +static void mtc_state_ind_cb(GIsiClient *client, const void *restrict data, size_t len, uint16_t object, void *opaque) { const unsigned char *msg = data; @@ -94,12 +111,54 @@ static void mtc_state_cb(GIsiClient *client, const void *restrict data, if (len < 3 || msg[0] != MTC_STATE_INFO_IND) return; + if (msg[2] == MTC_START) { + DBG("target modem state: %s (0x%02X)", + mtc_modem_state_name(msg[1]), msg[1]); + } else if (msg[2] == MTC_READY) { + DBG("current modem state: %s (0x%02X)", + mtc_modem_state_name(msg[1]), msg[1]); + set_power_by_mtc_state(isi, msg[1]); + } +} + +static bool mtc_poll_query_cb(GIsiClient *client, const void *restrict data, + size_t len, uint16_t object, void *opaque) +{ + const unsigned char *msg = data; + struct isi_data *isi = opaque; + + if (!msg) { + const unsigned char req[] = { + MTC_STATE_QUERY_REQ, 0x00, 0x00 + }; + + if (!isi-> iface_up) + return true; + + isi->interval *= 2; + if (isi->interval >= 20) + isi->interval = 20; + + g_isi_request_make(client, req, sizeof(req), + isi->interval, + mtc_poll_query_cb, opaque); + + return true; + } + + if (len < 3 || msg[0] != MTC_STATE_QUERY_RESP) + return false; + + g_isi_subscribe(client, MTC_STATE_INFO_IND, mtc_state_ind_cb, opaque); + DBG("current modem state: %s (0x%02X)", mtc_modem_state_name(msg[1]), msg[1]); DBG("target modem state: %s (0x%02X)", mtc_modem_state_name(msg[2]), msg[2]); - ofono_modem_set_powered(isi->modem, msg[1] != MTC_POWER_OFF); + set_power_by_mtc_state(isi, msg[1]); + + return true; } static bool mtc_query_cb(GIsiClient *client, const void *restrict data, @@ -121,7 +180,7 @@ static bool mtc_query_cb(GIsiClient *client, const void *restrict data, DBG("target modem state: %s (0x%02X)", mtc_modem_state_name(msg[2]), msg[2]); - ofono_modem_set_powered(isi->modem, msg[1] != MTC_POWER_OFF); + set_power_by_mtc_state(isi, msg[1]); return true; } @@ -129,13 +188,20 @@ static bool mtc_query_cb(GIsiClient *client, const void *restrict data, static void reachable_cb(GIsiClient *client, bool alive, uint16_t object, void *opaque) { + struct isi_data *isi = opaque; + const unsigned char msg[] = { MTC_STATE_QUERY_REQ, 0x00, 0x00 /* Filler */ }; if (!alive) { - DBG("Unable to bootstrap mtc driver"); + DBG("MTC client: %s", strerror(-g_isi_client_error(client))); + + if (isi->iface_up) + g_isi_request_make(client, msg, sizeof(msg), + isi->interval = MTC_TIMEOUT, + mtc_poll_query_cb, opaque); return; } @@ -144,156 +210,97 @@ static void reachable_cb(GIsiClient *client, bool alive, uint16_t object, g_isi_version_major(client), g_isi_version_minor(client)); - g_isi_subscribe(client, MTC_STATE_INFO_IND, mtc_state_cb, opaque); + g_isi_subscribe(client, MTC_STATE_INFO_IND, mtc_state_ind_cb, opaque); g_isi_request_make(client, msg, sizeof(msg), MTC_TIMEOUT, mtc_query_cb, opaque); } -static void netlink_status_cb(bool up, uint8_t addr, GIsiModem *idx, +static void phonet_status_cb(GIsiModem *idx, + GPhonetLinkState st, + char const *ifname, void *data) { - struct isi_data *isi = find_modem_by_idx(g_modems, idx); - - DBG("PhoNet is %s, addr=0x%02x, idx=%p", - up ? "up" : "down", addr, idx); - - if (up) { - - if (isi) { - - DBG("Modem already registered: (0x%02x)", - g_isi_modem_index(idx)); - return; - } - - isi = g_new0(struct isi_data, 1); - if (!isi) - return; - - isi->idx = idx; - isi->modem = ofono_modem_create(NULL, "isimodem"); - if (!isi->modem) { - g_free(isi); - return; - } - - g_modems = g_slist_prepend(g_modems, isi); - ofono_modem_set_data(isi->modem, isi); - ofono_modem_register(isi->modem); - - DBG("Done regging modem"); + struct ofono_modem *modem = data; + struct isi_data *isi = ofono_modem_get_data(modem); - } else { - if (!isi) { - DBG("Unknown modem: (0x%02x)", - g_isi_modem_index(idx)); - return; - } + DBG("Link %s (%u) is %s", + isi->ifname, g_isi_modem_index(isi->idx), + st == PN_LINK_REMOVED ? "removed" : + st == PN_LINK_DOWN ? "down" : "up"); - g_modems = g_slist_remove(g_modems, isi); - g_isi_client_destroy(isi->client); + isi->iface_up = st == PN_LINK_UP; - DBG("Now removing modem"); - ofono_modem_remove(isi->modem); - g_free(isi); - isi = NULL; - } + if (st == PN_LINK_UP) + g_isi_verify(isi->client, reachable_cb, isi); + else if (st == PN_LINK_DOWN) + set_power_by_mtc_state(isi, MTC_STATE_NONE); + else if (st == PN_LINK_REMOVED) + ofono_modem_remove(modem); } -static bool mtc_power_on_cb(GIsiClient *client, const void *restrict data, - size_t len, uint16_t object, void *opaque) +static int isi_modem_probe(struct ofono_modem *modem) { - const unsigned char *msg = data; - struct isi_data *isi = opaque; - - if (!msg) { - DBG("ISI client error: %d", g_isi_client_error(client)); - return true; - } - - if (len < 2 || msg[0] != MTC_POWER_ON_RESP) - return false; - - if (msg[1] == MTC_OK) - ofono_modem_set_powered(isi->modem, TRUE); + struct isi_data *isi; + char const *ifname = ofono_modem_get_string(modem, "Interface"); + GIsiModem *idx; + GPhonetNetlink *link; - return true; -} + if (ifname == NULL) + return -EINVAL; -static bool mtc_power_off_cb(GIsiClient *client, const void *restrict data, - size_t len, uint16_t object, void *opaque) -{ - const unsigned char *msg = data; - struct isi_data *isi = opaque; + DBG("(%p) with %s", modem, ifname); - if (!msg) { - DBG("ISI client error: %d", g_isi_client_error(client)); - return true; + idx = g_isi_modem_by_name(ifname); + if (idx == NULL) { + DBG("Interface=%s: %s", ifname, strerror(errno)); + return -errno; } - if (len < 2 || msg[0] != MTC_POWER_OFF_RESP) - return false; + link = g_pn_netlink_by_name(ifname); + if (link) { + DBG("%s: %s", ifname, strerror(EBUSY)); + return -EBUSY; + } - if (msg[1] == MTC_OK) - ofono_modem_set_powered(isi->modem, FALSE); + link = g_pn_netlink_start(idx, phonet_status_cb, modem); + if (!link) { + DBG("%s: %s", ifname, strerror(errno)); + return -errno; + } - return true; -} + isi = g_new0(struct isi_data, 1); + if (isi == NULL) + return -ENOMEM; -static int isi_modem_probe(struct ofono_modem *modem) -{ - struct isi_data *isi = ofono_modem_get_data(modem); + ofono_modem_set_data(isi->modem = modem, isi); + isi->idx = idx; + isi->ifname = ifname; + isi->link = link; isi->client = g_isi_client_create(isi->idx, PN_MTC); - if (!isi->client) - return -ENOMEM; - - g_isi_verify(isi->client, reachable_cb, isi); return 0; } static void isi_modem_remove(struct ofono_modem *modem) { - DBG(""); -} - -static int isi_modem_enable(struct ofono_modem *modem) -{ - struct isi_data *isi = ofono_modem_get_data(modem); - - const unsigned char msg[] = { - MTC_POWER_ON_REQ, - 0x00, 0x00 /* Filler */ - }; - - if (!g_isi_request_make(isi->client, msg, sizeof(msg), MTC_TIMEOUT, - mtc_power_on_cb, isi)) - return -EINVAL; - - return -EINPROGRESS; -} - -static int isi_modem_disable(struct ofono_modem *modem) -{ struct isi_data *isi = ofono_modem_get_data(modem); - const unsigned char msg[] = { - MTC_POWER_OFF_REQ, - 0x00, 0x00 /* Filler */ - }; + DBG("(%p) with %s", modem, isi ? isi->ifname : NULL); - if (!g_isi_request_make(isi->client, msg, sizeof(msg), MTC_TIMEOUT, - mtc_power_off_cb, isi)) - return -EINVAL; + if (isi == NULL) + return; - return -EINPROGRESS; + g_isi_client_destroy(isi->client); + g_free(isi); } static void isi_modem_pre_sim(struct ofono_modem *modem) { struct isi_data *isi = ofono_modem_get_data(modem); + DBG("(%p) with %s", modem, isi->ifname); + ofono_sim_create(isi->modem, 0, "isimodem", isi->idx); ofono_devinfo_create(isi->modem, 0, "isimodem", isi->idx); ofono_voicecall_create(isi->modem, 0, "isimodem", isi->idx); @@ -305,6 +312,8 @@ static void isi_modem_post_sim(struct ofono_modem *modem) struct ofono_gprs *gprs; struct ofono_gprs_context *gc; + DBG("(%p) with %s", modem, isi->ifname); + ofono_phonebook_create(isi->modem, 0, "isimodem", isi->idx); ofono_netreg_create(isi->modem, 0, "isimodem", isi->idx); ofono_sms_create(isi->modem, 0, "isimodem", isi->idx); @@ -329,16 +338,12 @@ static struct ofono_modem_driver driver = { .name = "isimodem", .probe = isi_modem_probe, .remove = isi_modem_remove, - .enable = isi_modem_enable, - .disable = isi_modem_disable, .pre_sim = isi_modem_pre_sim, .post_sim = isi_modem_post_sim, }; static int isimodem_init(void) { - link = g_pn_netlink_start(netlink_status_cb, NULL); - isi_devinfo_init(); isi_phonebook_init(); isi_netreg_init(); @@ -363,23 +368,6 @@ static int isimodem_init(void) static void isimodem_exit(void) { - GSList *m; - - for (m = g_modems; m; m = m->next) { - struct isi_data *isi = m->data; - - ofono_modem_remove(isi->modem); - g_free(isi); - } - - g_slist_free(g_modems); - g_modems = NULL; - - if (link) { - g_pn_netlink_stop(link); - link = NULL; - } - ofono_modem_driver_unregister(&driver); isi_devinfo_exit(); diff --git a/drivers/isimodem/isimodem.h b/drivers/isimodem/isimodem.h index d58bc920..57abcb2a 100644 --- a/drivers/isimodem/isimodem.h +++ b/drivers/isimodem/isimodem.h @@ -19,6 +19,8 @@ * */ +extern struct ofono_modem *isi_modem_by_interface(char const *ifname); + extern void isi_phonebook_init(); extern void isi_phonebook_exit(); diff --git a/drivers/isimodem/mtc.h b/drivers/isimodem/mtc.h index 59aaaf9f..b9004db4 100644 --- a/drivers/isimodem/mtc.h +++ b/drivers/isimodem/mtc.h @@ -42,13 +42,27 @@ enum mtc_isi_cause { MTC_RESET_REQUIRED = 0x17 }; +enum mtc_isi_action { + MTC_START = 0x03, + MTC_READY = 0x04, + MTC_NOS_READY = 0x0C, + MTC_SOS_START = 0x11, + MTC_SOS_READY = 0x12, +}; + enum mtc_message_id { + MTC_STATE_REQ = 0x01, MTC_STATE_QUERY_REQ = 0x02, MTC_POWER_OFF_REQ = 0x03, MTC_POWER_ON_REQ = 0x04, + MTC_STARTUP_SYNQ_REQ = 0x0B, + MTC_SHUTDOWN_SYNC_REQ = 0x12, + MTC_STATE_RESP = 0x64, MTC_STATE_QUERY_RESP = 0x65, MTC_POWER_OFF_RESP = 0x66, MTC_POWER_ON_RESP = 0x67, + MTC_STARTUP_SYNQ_RESP = 0x6E, + MTC_SHUTDOWN_SYNC_RESP = 0x75, MTC_STATE_INFO_IND = 0xC0, MTC_COMMON_MESSAGE = 0xF0 }; @@ -69,7 +83,8 @@ enum mtc_modem_state { MTC_DISCHARGING = 0x0C, MTC_DISK_WIPE = 0x0D, MTC_SW_RESET = 0x0E, - MTC_CMT_ONLY_MODE = 0xFF + MTC_CMT_ONLY_MODE = 0xFF, + MTC_STATE_NONE = -1, /* Used only internally */ }; #ifdef __cplusplus diff --git a/gisi/modem.h b/gisi/modem.h index 9443bce2..7c314e4e 100644 --- a/gisi/modem.h +++ b/gisi/modem.h @@ -18,6 +18,8 @@ #ifndef GISI_MODEM_H #define GISI_MODEM_H +#include <stdint.h> + typedef struct _GIsiModem GIsiModem; static inline unsigned g_isi_modem_index(GIsiModem *m) @@ -25,6 +27,8 @@ static inline unsigned g_isi_modem_index(GIsiModem *m) return (uintptr_t)m; } +GIsiModem *g_isi_modem_by_name(char const *name); + typedef void (*GIsiDebugFunc) (const void *restrict data, size_t len, void *opaque); diff --git a/gisi/netlink.c b/gisi/netlink.c index 210ec5c1..9396b665 100644 --- a/gisi/netlink.c +++ b/gisi/netlink.c @@ -38,24 +38,93 @@ #endif #include "phonet.h" #include <linux/rtnetlink.h> +#include <linux/if.h> #include <sys/ioctl.h> -#include <net/if.h> #include <errno.h> #include <glib.h> #include "netlink.h" +#include "modem.h" + +#ifndef ARPHRD_PHONET +#define ARPHRD_PHONET (820) +#endif + +/* + * GCC -Wcast-align does not like rtlink alignment macros, + * fixed macros by Andrzej Zaborowski <balrogg@gmail.com>. + */ +#undef IFA_RTA +#define IFA_RTA(r) ((struct rtattr*)(void*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct ifaddrmsg)))) + +#undef IFLA_RTA +#define IFLA_RTA(r) ((struct rtattr*)(void *)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct ifinfomsg)))) + +#undef NLMSG_NEXT +#define NLMSG_NEXT(nlh,len) ((len) -= NLMSG_ALIGN((nlh)->nlmsg_len), \ + (struct nlmsghdr*)(void*)(((char*)(nlh)) + NLMSG_ALIGN((nlh)->nlmsg_len))) + +#undef RTA_NEXT +#define RTA_NEXT(rta,attrlen) ((attrlen) -= RTA_ALIGN((rta)->rta_len), \ + (struct rtattr*)(void*)(((char*)(rta)) + RTA_ALIGN((rta)->rta_len))) + struct _GPhonetNetlink { GPhonetNetlinkFunc callback; void *opaque; guint watch; + unsigned interface; }; +/* if_nametoindex is in #include <net/if.h>, + but it is not compatible with <linux/if.h> */ + +extern unsigned if_nametoindex (char const *name); + +GIsiModem *g_isi_modem_by_name(char const *name) +{ + unsigned index = if_nametoindex(name); + + if (errno == 0) + errno = ENODEV; + + return (GIsiModem *)(void *)index; +} + static inline GIsiModem *make_modem(unsigned idx) { return (void *)(uintptr_t)idx; } +static GSList *netlink_list = NULL; + +GPhonetNetlink *g_pn_netlink_by_modem(GIsiModem *idx) +{ + GSList *m; + unsigned index = g_isi_modem_index(idx); + + for (m = netlink_list; m; m = m->next) { + GPhonetNetlink *self = m->data; + + if (index == self->interface) + return self; + } + + return NULL; +} + +GPhonetNetlink *g_pn_netlink_by_name(char const *ifname) +{ + if (ifname == NULL) { + return g_pn_netlink_by_modem(make_modem(0)); + } else { + unsigned index = if_nametoindex(ifname); + if (index == 0) + return NULL; + return g_pn_netlink_by_modem(make_modem(index)); + } +} + static void bring_up(unsigned ifindex) { struct ifreq req = { .ifr_ifindex = ifindex, }; @@ -70,16 +139,81 @@ error: close(fd); } +static void g_pn_nl_addr(GPhonetNetlink *self, struct nlmsghdr *nlh) +{ + int len; + uint8_t local = 0xff; + uint8_t remote = 0xff; + + const struct ifaddrmsg *ifa; + const struct rtattr *rta; + + ifa = NLMSG_DATA(nlh); + len = IFA_PAYLOAD(nlh); + + /* If Phonet is absent, kernel transmits other families... */ + if (ifa->ifa_family != AF_PHONET) + return; + if (ifa->ifa_index != self->interface) + return; + + for (rta = IFA_RTA(ifa); RTA_OK(rta, len); rta = RTA_NEXT(rta, len)) { + if (rta->rta_type == IFA_LOCAL) + local = *(uint8_t *)RTA_DATA(rta); + else if (rta->rta_type == IFA_ADDRESS) + remote = *(uint8_t *)RTA_DATA(rta); + } +} + +static void g_pn_nl_link(GPhonetNetlink *self, struct nlmsghdr *nlh) +{ + const struct ifinfomsg *ifi; + const struct rtattr *rta; + int len; + const char *ifname = NULL; + GIsiModem *idx = NULL; + GPhonetLinkState st; + + ifi = NLMSG_DATA(nlh); + len = IFA_PAYLOAD(nlh); + + if (ifi->ifi_type != ARPHRD_PHONET) + return; + + if (self->interface != 0 && self->interface != (unsigned)ifi->ifi_index) + return; + + idx = make_modem(ifi->ifi_index); + +#define UP (IFF_UP | IFF_LOWER_UP | IFF_RUNNING) + + if (nlh->nlmsg_type == RTM_DELLINK) + st = PN_LINK_REMOVED; + else if ((ifi->ifi_flags & UP) != UP) + st = PN_LINK_DOWN; + else + st = PN_LINK_UP; + + for (rta = IFLA_RTA(ifi); RTA_OK(rta, len); + rta = RTA_NEXT(rta, len)) { + if (rta->rta_type == IFLA_IFNAME) + ifname = RTA_DATA(rta); + } + + if (ifname && idx) + self->callback(idx, st, ifname, self->opaque); +} + + /* Parser Netlink messages */ static gboolean g_pn_nl_process(GIOChannel *channel, GIOCondition cond, gpointer data) { struct { struct nlmsghdr nlh; - struct rtmsg rtm; - char buf[1024]; - } req; - struct iovec iov = { &req, sizeof(req), }; + char buf[16384]; + } resp; + struct iovec iov = { &resp, (sizeof resp), }; struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1, }; ssize_t ret; struct nlmsghdr *nlh; @@ -90,19 +224,20 @@ static gboolean g_pn_nl_process(GIOChannel *channel, GIOCondition cond, return FALSE; ret = recvmsg(fd, &msg, 0); - if (ret == -1 || (msg.msg_flags & MSG_TRUNC)) + if (ret == -1) return TRUE; - for (nlh = (struct nlmsghdr *)&req; NLMSG_OK(nlh, (size_t)ret); - nlh = NLMSG_NEXT(nlh, ret)) { - const struct ifaddrmsg *ifa; - const struct rtattr *rta; - int len; - bool up; - uint8_t addr = 0; + if (msg.msg_flags & MSG_TRUNC) { + g_critical("Netlink message of %zu bytes truncated at %zu", + ret, (sizeof resp)); + return TRUE; + } + for (nlh = &resp.nlh; NLMSG_OK(nlh, (size_t)ret); + nlh = NLMSG_NEXT(nlh, ret)) { if (nlh->nlmsg_type == NLMSG_DONE) break; + switch (nlh->nlmsg_type) { case NLMSG_ERROR: { const struct nlmsgerr *err; @@ -111,85 +246,74 @@ static gboolean g_pn_nl_process(GIOChannel *channel, GIOCondition cond, return FALSE; } case RTM_NEWADDR: - up = true; - break; case RTM_DELADDR: - up = false; + g_pn_nl_addr(self, nlh); + break; + case RTM_NEWLINK: + case RTM_DELLINK: + g_pn_nl_link(self, nlh); break; default: continue; } - /* We have a route message */ - ifa = NLMSG_DATA(nlh); - len = IFA_PAYLOAD(nlh); - - /* If Phonet is absent, kernel transmits other families... */ - if (ifa->ifa_family != AF_PHONET) - continue; - for (rta = IFA_RTA(ifa); RTA_OK(rta, len); - rta = RTA_NEXT(rta, len)) - if (rta->rta_type == IFA_LOCAL) - memcpy(&addr, RTA_DATA(rta), 1); - if (up) - bring_up(ifa->ifa_index); - self->callback(up, addr, - make_modem(ifa->ifa_index), self->opaque); } return TRUE; } -/* Dump current Phonet address table */ -static int g_pn_netlink_query(int fd) +/* Dump current links */ +static int g_pn_netlink_getlink(int fd) { struct { struct nlmsghdr nlh; - struct rtmsg rtm; - } req; + struct ifinfomsg ifi; + } req = { + .nlh = { + .nlmsg_type = RTM_GETLINK, + .nlmsg_len = sizeof req, + .nlmsg_flags = NLM_F_REQUEST | NLM_F_ROOT | NLM_F_MATCH, + .nlmsg_pid = getpid(), + }, + .ifi = { + .ifi_family = AF_UNSPEC, + .ifi_type = ARPHRD_PHONET, + .ifi_change = 0xffFFffFF, + } + }; + struct sockaddr_nl addr = { .nl_family = AF_NETLINK, }; - req.nlh.nlmsg_type = RTM_GETADDR; - req.nlh.nlmsg_len = NLMSG_LENGTH(sizeof(req.rtm)); - req.nlh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ROOT; - req.nlh.nlmsg_seq = 0; - req.nlh.nlmsg_pid = getpid(); - - req.rtm.rtm_family = AF_PHONET; - req.rtm.rtm_dst_len = 6; - req.rtm.rtm_src_len = 0; - req.rtm.rtm_tos = 0; - - req.rtm.rtm_table = RT_TABLE_MAIN; - req.rtm.rtm_protocol = RTPROT_STATIC; - req.rtm.rtm_scope = RT_SCOPE_UNIVERSE; - req.rtm.rtm_type = RTN_UNICAST; - req.rtm.rtm_flags = 0; - - if (sendto(fd, &req, req.nlh.nlmsg_len, 0, - (struct sockaddr *)&addr, sizeof(addr)) == -1) - return -1; - return 0; + return sendto(fd, &req, (sizeof req), 0, + (struct sockaddr *)&addr, sizeof(addr)); } -GPhonetNetlink *g_pn_netlink_start(GPhonetNetlinkFunc cb, void *opaque) +GPhonetNetlink *g_pn_netlink_start(GIsiModem *idx, + GPhonetNetlinkFunc callback, + void *data) { GIOChannel *chan; GPhonetNetlink *self; - unsigned group = RTNLGRP_PHONET_IFADDR; int fd; - - self = malloc(sizeof(*self)); - if (self == NULL) - return NULL; + unsigned group = RTNLGRP_LINK; + unsigned interface = g_isi_modem_index(idx); fd = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE); if (fd == -1) + return NULL; + + self = calloc(1, sizeof(*self)); + if (self == NULL) goto error; fcntl(fd, F_SETFL, O_NONBLOCK | fcntl(fd, F_GETFL)); + if (setsockopt(fd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, - &group, sizeof(group))) + &group, sizeof(group))) goto error; - g_pn_netlink_query(fd); + + if (interface) + bring_up(interface); + + g_pn_netlink_getlink(fd); chan = g_io_channel_unix_new(fd); if (chan == NULL) @@ -198,22 +322,28 @@ GPhonetNetlink *g_pn_netlink_start(GPhonetNetlinkFunc cb, void *opaque) g_io_channel_set_encoding(chan, NULL, NULL); g_io_channel_set_buffered(chan, FALSE); - self->callback = cb; - self->opaque = opaque; + self->callback = callback; + self->opaque = data; + self->interface = interface; self->watch = g_io_add_watch(chan, G_IO_IN|G_IO_ERR|G_IO_HUP, g_pn_nl_process, self); g_io_channel_unref(chan); + + netlink_list = g_slist_prepend(netlink_list, self); + return self; error: - if (fd != -1) - close(fd); + close(fd); free(self); return NULL; } void g_pn_netlink_stop(GPhonetNetlink *self) { - g_source_remove(self->watch); - free(self); + if (self) { + netlink_list = g_slist_remove(netlink_list, self); + g_source_remove(self->watch); + free(self); + } } diff --git a/gisi/netlink.h b/gisi/netlink.h index 9e555a93..dfade5b8 100644 --- a/gisi/netlink.h +++ b/gisi/netlink.h @@ -35,10 +35,25 @@ extern "C" { struct _GPhonetNetlink; typedef struct _GPhonetNetlink GPhonetNetlink; -typedef void (*GPhonetNetlinkFunc)(bool up, uint8_t addr, GIsiModem *idx, - void *data); +typedef enum { + PN_LINK_REMOVED, + PN_LINK_DOWN, + PN_LINK_UP +} GPhonetLinkState; + +typedef void (*GPhonetNetlinkFunc)(GIsiModem *idx, + GPhonetLinkState st, + char const *iface, + void *data); + +GPhonetNetlink *g_pn_netlink_by_name(char const *ifname); + +GPhonetNetlink *g_pn_netlink_by_modem(GIsiModem *idx); + +GPhonetNetlink *g_pn_netlink_start(GIsiModem *idx, + GPhonetNetlinkFunc callback, + void *data); -GPhonetNetlink *g_pn_netlink_start(GPhonetNetlinkFunc func, void *data); void g_pn_netlink_stop(GPhonetNetlink *self); #ifdef __cplusplus diff --git a/plugins/usbpnmodem.c b/plugins/usbpnmodem.c new file mode 100644 index 00000000..5f06e9d5 --- /dev/null +++ b/plugins/usbpnmodem.c @@ -0,0 +1,98 @@ +/* + * This file is part of oFono - Open Source Telephony + * + * Copyright (C) 2009 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 + +#include <stdio.h> +#include <errno.h> +#include <stdlib.h> +#include <string.h> +#include <glib.h> + +#include <gisi/netlink.h> +#include <gisi/client.h> + +#define OFONO_API_SUBJECT_TO_CHANGE +#include <ofono/plugin.h> +#include <ofono/log.h> +#include <ofono/modem.h> + +static GPhonetNetlink *link = NULL; + +/* + * Add or remove isimodems + * when usbpn* phonet interfaces are added/removed + */ +static void usbpn_status_cb(GIsiModem *idx, + GPhonetLinkState st, + char const ifname[], + void *data) +{ + struct ofono_modem *modem; + + DBG("Phonet link %s (%u) is %s", + ifname, g_isi_modem_index(idx), + st == PN_LINK_REMOVED ? "removed" : + st == PN_LINK_DOWN ? "down" : "up"); + + /* Expect phonet interface name usbpn<idx> */ + if (strncmp(ifname, "usbpn", 5) || + ifname[5 + strspn(ifname + 5, "0123456789")]) + return; + + if (st == PN_LINK_REMOVED) + return; + + link = g_pn_netlink_by_name(ifname); + if (link) { + DBG("Modem for interface %s already exists", ifname); + return; + } + + modem = ofono_modem_create(NULL, "isimodem"); + if (!modem) + return; + + ofono_modem_set_string(modem, "Interface", ifname); + + if (ofono_modem_register(modem) == 0) + DBG("Done regging modem %s", ofono_modem_get_path(modem)); + else + ofono_modem_remove(modem); +} + +static int usbpn_init(void) +{ + link = g_pn_netlink_start(NULL, usbpn_status_cb, NULL); + return 0; +} + +static void usbpn_exit(void) +{ + g_pn_netlink_stop(link); + link = NULL; +} + +OFONO_PLUGIN_DEFINE(usbpnmodem, "Hotplug driver for USB Phonet modems", VERSION, + OFONO_PLUGIN_PRIORITY_DEFAULT, + usbpn_init, usbpn_exit) |