/* Copyright (C) 2011 bg */ #include "ast_config.h" #include /* u_int16_t u_int8_t */ #include /* DIR */ #include /* NULL */ #include /* strlen() */ #include /* stat() */ #include "pdiscovery.h" /* pdiscovery_lookup() */ #include "mutils.h" /* ITEMS_OF() */ #include "ringbuffer.h" /* struct ringbuffer */ #include "at_queue.h" /* write_all() */ #include "at_read.h" /* at_wait() at_read() at_read_result_iov() at_read_result_classification() */ #include "chan_dongle.h" /* opentty() closetty() */ #include "manager.h" /* manager_event_message_raw() */ /* static const char sys_bus_usb_drivers_usb[] = "/sys/bus/usb/drivers/usb"; */ static const char sys_bus_usb_devices[] = "/sys/bus/usb/devices"; /* timeout for port readering milliseconds */ #define PDISCOVERY_TIMEOUT 500 struct pdiscovery_device { u_int16_t vendor_id; u_int16_t product_id; u_int8_t interfaces[INTERFACE_TYPE_NUMBERS]; }; struct pdiscovery_request { const char * name; const char * imei; const char * imsi; }; struct pdiscovery_cache_item { AST_LIST_ENTRY (pdiscovery_cache_item) entry; struct timeval validtill; int status; struct pdiscovery_result res; }; struct discovery_cache { AST_RWLIST_HEAD (, pdiscovery_cache_item) items; }; #define BUILD_NAME(d1, d2, d1len, d2len, out) \ d2len = strlen(d2); \ out = alloca(d1len + 1 + d2len + 1); \ memcpy(out, d1, d1len); \ out[d1len] = '/'; \ memcpy(out + d1len + 1, d2, d2len); \ d2len += d1len + 1; \ out[d2len] = 0; static const struct pdiscovery_device device_ids[] = { { 0x12d1, 0x1001, { 2, 1, /* 0 */ } }, /* E1550 and generic */ // { 0x12d1, 0x1465, { 2, 1, /* 0 */ } }, /* K3520 */ { 0x12d1, 0x140c, { 3, 2, /* 0 */ } }, /* E17xx */ { 0x12d1, 0x14ac, { 4, 3, /* 0 */ } }, /* E153Du-1 : thanks mghadam */ { 0x12d1, 0x1436, { 4, 3, /* 0 */ } }, /* E1750 */ { 0x12d1, 0x1506, { 1, 2, /* 0 */ } }, /* E171 firmware 21.x : thanks Sergey Ivanov */ { 0x2c7c, 0x0125, { 1, 4, /* 0 */ } }, /* Dongle EC25-A LTE modem */ }; static struct discovery_cache cache; #/* return non-0 if all ports matched */ static int ports_match(const struct pdiscovery_ports * p1, const struct pdiscovery_ports * p2) { unsigned i; for(i = 0; i < ITEMS_OF(p1->ports); i++) { if(!p1->ports[i] || ! p2->ports[i] || strcmp(p1->ports[i], p2->ports[i]) != 0) return 0; } return i; } #/* */ static int ports_copy(struct pdiscovery_ports * dst, const struct pdiscovery_ports * src) { unsigned i; for(i = 0; i < ITEMS_OF(dst->ports); i++) if(src->ports[i] != NULL) { dst->ports[i] = ast_strdup(src->ports[i]); if(dst->ports[i] == NULL) return 0; } return i; } #/* */ static void ports_free(struct pdiscovery_ports * ports) { unsigned i; for(i = 0; i < ITEMS_OF(ports->ports); i++) if(ports->ports[i] != NULL) { ast_free(ports->ports[i]); ports->ports[i] = NULL; } } #/* */ static void info_free(struct pdiscovery_result * res) { if(res->imsi) { ast_free(res->imsi); res->imsi = NULL; } if(res->imei) { ast_free(res->imei); res->imei = NULL; } } #/* */ static void info_copy(struct pdiscovery_result * dst, const struct pdiscovery_result * src) { if(src->imei) dst->imei = ast_strdup(src->imei); if(src->imsi) dst->imsi = ast_strdup(src->imsi); } #/* */ static void result_free(struct pdiscovery_result * res) { ports_free(&res->ports); info_free(res); } #/* */ static void cache_item_free(struct pdiscovery_cache_item * item) { if(item) { result_free(&item->res); ast_free(item); } } #/* */ static void cache_item_update(struct pdiscovery_cache_item * item, const struct pdiscovery_result * res, int status) { info_free(&item->res); info_copy(&item->res, res); item->status = status; item->validtill = ast_tvnow(); item->validtill.tv_sec += CONF_GLOBAL(discovery_interval); } #/* */ static struct pdiscovery_cache_item * cache_item_create(const struct pdiscovery_result * res, int status) { struct pdiscovery_cache_item * item = ast_calloc(1, sizeof(*item)); if(item) { if(ports_copy(&item->res.ports, &res->ports)) { cache_item_update(item, res, status); } else { cache_item_free(item); item = NULL; } } return item; } #/* */ static struct pdiscovery_cache_item * cache_search(struct discovery_cache * cache, const struct pdiscovery_result * res) { struct pdiscovery_cache_item * found = NULL; struct pdiscovery_cache_item * item; struct timeval now = ast_tvnow(); AST_RWLIST_WRLOCK(&cache->items); AST_LIST_TRAVERSE_SAFE_BEGIN(&cache->items, item, entry) { if(ast_tvcmp(now, item->validtill) < 0) { if(ports_match(&item->res.ports, &res->ports)) { found = item; break; } } else { // remove expired item AST_LIST_REMOVE_CURRENT(entry); cache_item_free(item); } } AST_LIST_TRAVERSE_SAFE_END; AST_RWLIST_UNLOCK(&cache->items); return found; } #/* */ static int cache_lookup(struct discovery_cache * cache, const struct pdiscovery_request * req, struct pdiscovery_result * res, int * failed) { int found = 0; struct pdiscovery_cache_item * item = cache_search(cache, res); if(item) { res->imei = item->res.imei ? ast_strdup(item->res.imei) : NULL; res->imsi = item->res.imsi ? ast_strdup(item->res.imsi) : NULL; found = item->status || ((req->imei || item->res.imei) && (req->imsi || item->res.imsi)); if(found) { *failed = item->status; } } return found; } #/* */ static void cache_update(struct discovery_cache * cache, const struct pdiscovery_result * res, int status) { struct pdiscovery_cache_item * item = cache_search(cache, res); if(item) { cache_item_update(item, res, status); } else { item = cache_item_create(res, status); AST_LIST_INSERT_TAIL(&cache->items, item, entry); } } #/* */ static void cache_init(struct discovery_cache * cache) { /* TODO: place lock init when locking becomes required */ AST_RWLIST_HEAD_INIT(&cache->items); } #/* */ static void cache_fini(struct discovery_cache * cache) { struct pdiscovery_cache_item * item; AST_RWLIST_WRLOCK(&cache->items); AST_LIST_TRAVERSE_SAFE_BEGIN(&cache->items, item, entry) { AST_LIST_REMOVE_CURRENT(entry); cache_item_free(item); } AST_LIST_TRAVERSE_SAFE_END; AST_RWLIST_UNLOCK(&cache->items); AST_RWLIST_HEAD_DESTROY(&cache->items); } #/* */ static const struct pdiscovery_cache_item * cache_first_readlock(struct discovery_cache * cache) { AST_RWLIST_RDLOCK(&cache->items); return AST_RWLIST_FIRST(&cache->items); } #/* */ static void cache_unlock(struct discovery_cache * cache) { AST_RWLIST_UNLOCK(&cache->items); } #/* */ static int pdiscovery_get_id(const char * name, int len, const char * filename, unsigned * integer) { int len2; char * name2; int assign = 0; BUILD_NAME(name, filename, len, len2, name2); FILE * file = fopen(name2, "r"); if(file) { assign = fscanf(file, "%x", integer); fclose(file); } return assign; } #/* */ static int pdiscovery_is_port(const char * name, int len) { int len2; char * name2; struct stat statb; BUILD_NAME(name, "port_number", len, len2, name2); return stat(name2, &statb) == 0 && S_ISREG(statb.st_mode); } #/* */ static char * pdiscovery_port(const char * name, int len, const char * subdir) { int len2, len3; char * name2, * name3; struct stat statb; char * port = NULL; BUILD_NAME(name, subdir, len, len2, name2); if(stat(name2, &statb) == 0 && S_ISDIR(statb.st_mode) && pdiscovery_is_port(name2, len2)) { // ast_debug(4, "[%s discovery] found port %s\n", devname, dentry->d_name); BUILD_NAME("/dev", subdir, 4, len3, name3); port = ast_strdup(name3); } return port; } #/* */ static char * pdiscovery_port_name(const char * name, int len) { char * port = NULL; struct dirent * dentry; DIR * dir = opendir(name); if(dir) { while((dentry = readdir(dir)) != NULL) { if(strcmp(dentry->d_name, ".") != 0 && strcmp(dentry->d_name, "..") != 0) { port = pdiscovery_port(name, len, dentry->d_name); if(port) break; } } closedir(dir); } return port; } #/* */ static char * pdiscovery_interface(const char * name, int len, unsigned * interface) { char * port = NULL; if(pdiscovery_get_id(name, len, "bInterfaceNumber", interface) == 1) { // ast_debug(4, "[%s discovery] bInterfaceNumber %02x\n", devname, *interface); port = pdiscovery_port_name(name, len); } return port; } #/* */ static char * pdiscovery_find_port(const char * name, int len, const char * subdir, unsigned * interface) { int len2; char * name2; struct stat statb; char * port = NULL; BUILD_NAME(name, subdir, len, len2, name2); if(stat(name2, &statb) == 0 && S_ISDIR(statb.st_mode)) { port = pdiscovery_interface(name2, len2, interface); } return port; } #/* */ static int pdiscovery_interfaces(const char * devname, const char * name, int len, const struct pdiscovery_device * device, struct pdiscovery_ports * ports) { unsigned interface; unsigned idx; int found = 0; struct dirent * dentry; char * port; DIR * dir = opendir(name); if(dir) { while((dentry = readdir(dir)) != NULL) { if(strchr(dentry->d_name, ':')) { port = pdiscovery_find_port(name, len, dentry->d_name, &interface); if(port) { ast_debug(4, "[%s discovery] found InterfaceNumber %02x port %s\n", devname, interface, port); for(idx = 0; idx < (int)ITEMS_OF(device->interfaces); idx++) { if(device->interfaces[idx] == interface) { if(ports->ports[idx] == NULL) { ports->ports[idx] = port; if(++found == INTERFACE_TYPE_NUMBERS) break; } else { ast_debug(4, "[%s discovery] port %s for bInterfaceNumber %02x already exists new is %s\n", devname, ports->ports[idx], interface, port); // FIXME } } } } } } closedir(dir); } return found; } #/* */ static const struct pdiscovery_device * pdiscovery_lookup_ids(const char * devname, const char * name, int len) { unsigned vid; unsigned pid; unsigned idx; if(pdiscovery_get_id(name, len, "idVendor", &vid) == 1 && pdiscovery_get_id(name, len, "idProduct", &pid) == 1) { ast_debug(4, "[%s discovery] found %s is idVendor %04x idProduct %04x\n", devname, name, vid, pid); for(idx = 0; idx < ITEMS_OF(device_ids); idx++) { if(device_ids[idx].vendor_id == vid && device_ids[idx].product_id == pid) { return &device_ids[idx]; } } } return NULL; } #/* 0D 0A IMEI: <15 digits> 0D 0A */ static char * pdiscovery_handle_ati(const char * devname, char * str) { static const char IMEI[] = "\r\nIMEI:"; char * imei = strstr(str, IMEI); if(imei) { imei += STRLEN(IMEI); while(imei[0] == ' ') imei++; str = imei; while(str[0] >= '0' && str[0] <= '9') str++; if((str - imei) == IMEI_SIZE && str[0] == '\r' && str[1] == '\n') { str[0] = 0; imei = ast_strdup(imei); str[0] = '\r'; ast_debug(4, "[%s discovery] found IMEI %s\n", devname, imei); return imei; } } return NULL; } #/* 0D 0A 15 digits 0D 0A */ static char * pdiscovery_handle_cimi(const char * devname, char * str) { char * imsi = NULL; enum states { STATE_BEGIN, STATE_CR1, STATE_LF1, STATE_DIGITS, STATE_CR2, } state; for(state = STATE_BEGIN; *str; ++str) { switch(state) { case STATE_BEGIN: if(*str == '\r') state++; break; case STATE_CR1: if(*str == '\n') state++; else state = STATE_BEGIN; break; case STATE_LF1: if(*str >= '0' && *str <= '9') { state++; imsi = str; } else if(*str == '\r') state = STATE_CR1; else state = STATE_BEGIN; break; case STATE_DIGITS: if(*str >= '0' && *str <= '9') ; else if(*str == '\r') { if((str - imsi) == IMSI_SIZE) state++; else state = STATE_CR1; } else state = STATE_BEGIN; break; case STATE_CR2: if(*str == '\n') { str[-1] = 0; imsi = ast_strdup(imsi); str[-1] = '\r'; ast_debug(4, "[%s discovery] found IMSI %s\n", devname, imsi); return imsi; } /* fall through */ default: state = STATE_BEGIN; } } return NULL; } #/* return non-zero on done with command */ static int pdiscovery_handle_response(const struct pdiscovery_request * req, const struct iovec iov[2], int iovcnt, struct pdiscovery_result * res) { int done = 0; char * str; char sym; size_t len = iov[0].iov_len + iov[1].iov_len; if(len > 0) { len--; if(iovcnt == 2) { str = alloca(len + 1); memcpy(str, iov[0].iov_base, iov[0].iov_len); memcpy(str + iov[0].iov_len, iov[1].iov_base, iov[1].iov_len); } else { str = iov[0].iov_base; } sym = str[len]; str[len] = 0; ast_debug(4, "[%s discovery] < %s\n", req->name, str); done = strstr(str, "OK") != NULL || strstr(str, "ERROR") != NULL; if(req->imei && res->imei == NULL) res->imei = pdiscovery_handle_ati(req->name, str); if(req->imsi && res->imsi == NULL) res->imsi = pdiscovery_handle_cimi(req->name, str); /* restore tail of string for collect data in buffer */ str[len] = sym; } return done; } #/* return zero on sucess */ static int pdiscovery_do_cmd(const struct pdiscovery_request * req, int fd, const char * name, const char * cmd, unsigned length, struct pdiscovery_result * res) { int timeout; char buf[1024 + 1]; struct ringbuffer rb; struct iovec iov[2]; int iovcnt; size_t wrote; ast_debug(4, "[%s discovery] use %s for IMEI/IMSI discovery\n", req->name, name); clean_read_data(req->name, fd); wrote = write_all(fd, cmd, length); if(wrote == length) { timeout = PDISCOVERY_TIMEOUT; rb_init(&rb, buf, sizeof(buf) - 1); while(timeout > 0 && at_wait(fd, &timeout) != 0) { iovcnt = at_read(fd, name, &rb); if(iovcnt > 0) { iovcnt = rb_read_all_iov(&rb, iov); if(pdiscovery_handle_response(req, iov, iovcnt, res)) return 0; } else { snprintf(buf, sizeof(buf), "Read Failed\r\nErrorCode: %d", errno); manager_event_message_raw("DonglePortFail", name, buf); ast_log (LOG_ERROR, "[%s discovery] read from %s failed: %s\n", req->name, name, strerror(errno)); return -1; } } manager_event_message_raw("DonglePortFail", name, "Response Failed"); ast_log (LOG_ERROR, "[%s discovery] failed to get valid response from %s in %d msec\n", req->name, name, PDISCOVERY_TIMEOUT); } else { snprintf(buf, sizeof(buf), "Write Failed\r\nErrorCode: %d", errno); manager_event_message_raw("DonglePortFail", name, buf); ast_log (LOG_ERROR, "[%s discovery] write to %s failed: %s\n", req->name, name, strerror(errno)); } return 1; } #/* return non-zero on fail */ static int pdiscovery_get_info(const char * port, const struct pdiscovery_request * req, struct pdiscovery_result * res) { static const struct { const char * cmd; unsigned length; } cmds[] = { { "AT+CIMI\r", 8 }, /* IMSI */ { "ATI\r", 4 }, /* IMEI */ { "ATI; +CIMI\r" , 11 }, /* IMSI + IMEI */ }; static const int want_map[2][2] = { { 2, 0 }, // want_imei = 0 { 1, 2 } // want_imei = 1 }; int fail = 1; char * lock_file; int fd = opentty(port, &lock_file); if(fd >= 0) { unsigned want_imei = req->imei && res->imei == NULL; // 1 && 0 unsigned want_imsi = req->imsi && res->imsi == NULL; // 1 && 1 unsigned cmd = want_map[want_imei][want_imsi]; /* clean queue first ? */ fail = pdiscovery_do_cmd(req, fd, port, cmds[cmd].cmd, cmds[cmd].length, res); closetty(fd, &lock_file); } return fail; } #/* return non-zero on fail */ static int pdiscovery_get_info_cached(const char * port, const struct pdiscovery_request * req, struct pdiscovery_result * res) { int fail = 1; /* may add info also if !found */ int found = cache_lookup(&cache, req, res, &fail); if(!found) { fail = pdiscovery_get_info(port, req, res); cache_update(&cache, res, fail); } else { ast_debug(4, "[%s discovery] %s use cached IMEI %s IMSI %s failed %d\n", req->name, port, S_OR(res->imei, ""), S_OR(res->imsi, ""), fail); } return fail; } #/* return zero on success */ static int pdiscovery_read_info(const struct pdiscovery_request * req, struct pdiscovery_result * res) { char * dlock; int fail = 1; // const char * cport = res->ports.ports[INTERFACE_TYPE_COM]; const char * dport = res->ports.ports[INTERFACE_TYPE_DATA]; // if(cport && strcmp(cport, dport) != 0) { int pid = lock_try(dport, &dlock); if(pid == 0) { fail = pdiscovery_get_info_cached(dport, req, res); closetty(-1, &dlock); } else { ast_debug(4, "[%s discovery] %s already used by process %d, skipped\n", req->name, dport, pid); // ast_log (LOG_WARNING, "[%s discovery] %s already used by process %d\n", devname, dport, pid); } // } else { // fail = pdiscovery_get_info_cached(dport, req, res); // } return fail; } #/* */ static int pdiscovery_check_req(const struct pdiscovery_request * req, struct pdiscovery_result * res) { int match = 0; if(pdiscovery_read_info(req, res) == 0) { match = ((req->imei == 0) || (res->imei && strcmp(req->imei, res->imei) == 0)) && ((req->imsi == 0) || (res->imsi && strcmp(req->imsi, res->imsi) == 0)); ast_debug(4, "[%s discovery] %smatched IMEI=%s/%s IMSI=%s/%s\n", req->name, match ? "" : "un" , S_OR(req->imei, "") , S_OR(res->imei, ""), S_OR(req->imsi, ""), S_OR(res->imsi, "") ); } return match; } #/* */ static int pdiscovery_check_device(const char * name, int len, const char * subdir, const struct pdiscovery_request * req, struct pdiscovery_result * res) { int len2; char * name2; const struct pdiscovery_device * device; int found = 0; BUILD_NAME(name, subdir, len, len2, name2); device = pdiscovery_lookup_ids(req->name, name2, len2); if(device) { // ast_debug(4, "[%s discovery] should ports <-> interfaces map for %04x:%04x modem=%02x voice=%02x data=%02x\n", ast_debug(4, "[%s discovery] should ports <-> interfaces map for %04x:%04x voice=%02x data=%02x\n", req->name, device->vendor_id, device->product_id, // device->interfaces[INTERFACE_TYPE_COM], device->interfaces[INTERFACE_TYPE_VOICE], device->interfaces[INTERFACE_TYPE_DATA] ); pdiscovery_interfaces(req->name, name2, len2, device, &res->ports); /* check mandatory ports */ if(res->ports.ports[INTERFACE_TYPE_DATA] && res->ports.ports[INTERFACE_TYPE_VOICE]) { found = pdiscovery_check_req(req, res); } } if(!found) result_free(res); return found; } #/* */ static int pdiscovery_request_do(const char * name, int len, const struct pdiscovery_request * req, struct pdiscovery_result * res) { int found = 0; struct dirent * dentry; DIR * dir = opendir(name); if(dir) { while((dentry = readdir(dir)) != NULL) { if(strcmp(dentry->d_name, ".") != 0 && strcmp(dentry->d_name, "..") != 0 && strstr(dentry->d_name, "usb") != dentry->d_name) { ast_debug(4, "[%s discovery] checking %s/%s\n", req->name, name, dentry->d_name); found = pdiscovery_check_device(name, len, dentry->d_name, req, res); if(found) break; } } closedir(dir); } return found; } #/* */ EXPORT_DEF void pdiscovery_init() { cache_init(&cache); } #/* */ EXPORT_DEF void pdiscovery_fini() { cache_fini(&cache); } #/* */ EXPORT_DEF int pdiscovery_lookup(const char * devname, const char * imei, const char * imsi, char ** dport, char ** aport) { int found; struct pdiscovery_result res; const struct pdiscovery_request req = { devname, ((imei && imei[0]) ? imei : NULL), ((imsi && imsi[0]) ? imsi : NULL), }; memset(&res, 0, sizeof(res)); found = pdiscovery_request_do(sys_bus_usb_devices, STRLEN(sys_bus_usb_devices), &req, &res); if(found) { *dport = ast_strdup(res.ports.ports[INTERFACE_TYPE_DATA]); *aport = ast_strdup(res.ports.ports[INTERFACE_TYPE_VOICE]); } result_free(&res); return found; } #/* */ EXPORT_DEF const struct pdiscovery_result * pdiscovery_list_begin(const struct pdiscovery_cache_item ** opaque) { const struct pdiscovery_cache_item * item; struct pdiscovery_result res; const struct pdiscovery_request req = { "list", "ANY", "ANY", }; memset(&res, 0, sizeof(res)); pdiscovery_request_do(sys_bus_usb_devices, STRLEN(sys_bus_usb_devices), &req, &res); result_free(&res); *opaque = item = cache_first_readlock(&cache); return item != NULL ? &item->res : NULL; } #/* */ EXPORT_DECL const struct pdiscovery_result * pdiscovery_list_next(const struct pdiscovery_cache_item ** opaque) { const struct pdiscovery_cache_item * item = AST_RWLIST_NEXT(*opaque, entry); *opaque = item; return item != NULL ? &item->res : NULL; } #/* */ EXPORT_DECL void pdiscovery_list_end() { cache_unlock(&cache); }