/****************************************************************************** @file quectel-qmi-proxy.c @brief The qmi proxy. DESCRIPTION Connectivity Management Tool for USB network adapter of Quectel wireless cellular modules. INITIALIZATION AND SEQUENCING REQUIREMENTS None. --------------------------------------------------------------------------- Copyright (c) 2016 - 2020 Quectel Wireless Solution, Co., Ltd. All Rights Reserved. Quectel Wireless Solution Proprietary and Confidential. --------------------------------------------------------------------------- ******************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifndef MIN #define MIN(a, b) ((a) < (b)? (a): (b)) #endif #ifndef htole32 #if __BYTE_ORDER == __LITTLE_ENDIAN #define htole16(x) (uint16_t)(x) #define le16toh(x) (uint16_t)(x) #define letoh16(x) (uint16_t)(x) #define htole32(x) (uint32_t)(x) #define le32toh(x) (uint32_t)(x) #define letoh32(x) (uint32_t)(x) #define htole64(x) (uint64_t)(x) #define le64toh(x) (uint64_t)(x) #define letoh64(x) (uint64_t)(x) #else static __inline uint16_t __bswap16(uint16_t __x) { return (__x<<8) | (__x>>8); } static __inline uint32_t __bswap32(uint32_t __x) { return (__x>>24) | (__x>>8&0xff00) | (__x<<8&0xff0000) | (__x<<24); } static __inline uint64_t __bswap64(uint64_t __x) { return (__bswap32(__x)+0ULL<<32) | (__bswap32(__x>>32)); } #define htole16(x) __bswap16(x) #define le16toh(x) __bswap16(x) #define letoh16(x) __bswap16(x) #define htole32(x) __bswap32(x) #define le32toh(x) __bswap32(x) #define letoh32(x) __bswap32(x) #define htole64(x) __bswap64(x) #define le64toh(x) __bswap64(x) #define letoh64(x) __bswap64(x) #endif #endif #define dprintf(fmt, arg...) do { printf(fmt, ##arg); } while(0) #define SYSCHECK(c) do{if((c)<0) {dprintf("%s %d error: '%s' (code: %d)\n", __func__, __LINE__, strerror(errno), errno); return -1;}}while(0) #define cfmakenoblock(fd) do{fcntl(fd, F_SETFL, fcntl(fd,F_GETFL) | O_NONBLOCK);}while(0) #define qmidev_is_pciemhi(_qmichannel) (strncmp(_qmichannel, "/dev/mhi_", strlen("/dev/mhi_")) == 0) typedef struct _QCQMI_HDR { uint8_t IFType; uint16_t Length; uint8_t CtlFlags; // reserved uint8_t QMIType; uint8_t ClientId; } __attribute__ ((packed)) QCQMI_HDR, *PQCQMI_HDR; typedef struct _QMICTL_SYNC_REQ_MSG { uint8_t CtlFlags; // QMICTL_FLAG_REQUEST uint8_t TransactionId; uint16_t QMICTLType; // QMICTL_CTL_SYNC_REQ uint16_t Length; // 0 } __attribute__ ((packed)) QMICTL_SYNC_REQ_MSG, *PQMICTL_SYNC_REQ_MSG; typedef struct _QMICTL_SYNC_RESP_MSG { uint8_t CtlFlags; // QMICTL_FLAG_RESPONSE uint8_t TransactionId; uint16_t QMICTLType; // QMICTL_CTL_SYNC_RESP uint16_t Length; uint8_t TLVType; // QCTLV_TYPE_RESULT_CODE uint16_t TLVLength; // 0x0004 uint16_t QMIResult; uint16_t QMIError; } __attribute__ ((packed)) QMICTL_SYNC_RESP_MSG, *PQMICTL_SYNC_RESP_MSG; typedef struct _QMICTL_SYNC_IND_MSG { uint8_t CtlFlags; // QMICTL_FLAG_INDICATION uint8_t TransactionId; uint16_t QMICTLType; // QMICTL_REVOKE_CLIENT_ID_IND uint16_t Length; } __attribute__ ((packed)) QMICTL_SYNC_IND_MSG, *PQMICTL_SYNC_IND_MSG; typedef struct _QMICTL_GET_CLIENT_ID_REQ_MSG { uint8_t CtlFlags; // QMICTL_FLAG_REQUEST uint8_t TransactionId; uint16_t QMICTLType; // QMICTL_GET_CLIENT_ID_REQ uint16_t Length; uint8_t TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER uint16_t TLVLength; // 1 uint8_t QMIType; // QMUX type } __attribute__ ((packed)) QMICTL_GET_CLIENT_ID_REQ_MSG, *PQMICTL_GET_CLIENT_ID_REQ_MSG; typedef struct _QMICTL_GET_CLIENT_ID_RESP_MSG { uint8_t CtlFlags; // QMICTL_FLAG_RESPONSE uint8_t TransactionId; uint16_t QMICTLType; // QMICTL_GET_CLIENT_ID_RESP uint16_t Length; uint8_t TLVType; // QCTLV_TYPE_RESULT_CODE uint16_t TLVLength; // 0x0004 uint16_t QMIResult; // result code uint16_t QMIError; // error code uint8_t TLV2Type; // QCTLV_TYPE_REQUIRED_PARAMETER uint16_t TLV2Length; // 2 uint8_t QMIType; uint8_t ClientId; } __attribute__ ((packed)) QMICTL_GET_CLIENT_ID_RESP_MSG, *PQMICTL_GET_CLIENT_ID_RESP_MSG; typedef struct _QMICTL_RELEASE_CLIENT_ID_REQ_MSG { uint8_t CtlFlags; // QMICTL_FLAG_REQUEST uint8_t TransactionId; uint16_t QMICTLType; // QMICTL_RELEASE_CLIENT_ID_REQ uint16_t Length; uint8_t TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER uint16_t TLVLength; // 0x0002 uint8_t QMIType; uint8_t ClientId; } __attribute__ ((packed)) QMICTL_RELEASE_CLIENT_ID_REQ_MSG, *PQMICTL_RELEASE_CLIENT_ID_REQ_MSG; typedef struct _QMICTL_RELEASE_CLIENT_ID_RESP_MSG { uint8_t CtlFlags; // QMICTL_FLAG_RESPONSE uint8_t TransactionId; uint16_t QMICTLType; // QMICTL_RELEASE_CLIENT_ID_RESP uint16_t Length; uint8_t TLVType; // QCTLV_TYPE_RESULT_CODE uint16_t TLVLength; // 0x0004 uint16_t QMIResult; // result code uint16_t QMIError; // error code uint8_t TLV2Type; // QCTLV_TYPE_REQUIRED_PARAMETER uint16_t TLV2Length; // 2 uint8_t QMIType; uint8_t ClientId; } __attribute__ ((packed)) QMICTL_RELEASE_CLIENT_ID_RESP_MSG, *PQMICTL_RELEASE_CLIENT_ID_RESP_MSG; // QMICTL Control Flags #define QMICTL_CTL_FLAG_CMD 0x00 #define QMICTL_CTL_FLAG_RSP 0x01 #define QMICTL_CTL_FLAG_IND 0x02 typedef struct _QCQMICTL_MSG_HDR { uint8_t CtlFlags; // 00-cmd, 01-rsp, 10-ind uint8_t TransactionId; uint16_t QMICTLType; uint16_t Length; } __attribute__ ((packed)) QCQMICTL_MSG_HDR, *PQCQMICTL_MSG_HDR; #define QCQMICTL_MSG_HDR_SIZE sizeof(QCQMICTL_MSG_HDR) typedef struct _QCQMICTL_MSG_HDR_RESP { uint8_t CtlFlags; // 00-cmd, 01-rsp, 10-ind uint8_t TransactionId; uint16_t QMICTLType; uint16_t Length; uint8_t TLVType; // 0x02 - result code uint16_t TLVLength; // 4 uint16_t QMUXResult; // QMI_RESULT_SUCCESS // QMI_RESULT_FAILURE uint16_t QMUXError; // QMI_ERR_INVALID_ARG // QMI_ERR_NO_MEMORY // QMI_ERR_INTERNAL // QMI_ERR_FAULT } __attribute__ ((packed)) QCQMICTL_MSG_HDR_RESP, *PQCQMICTL_MSG_HDR_RESP; typedef struct _QMICTL_GET_VERSION_REQ_MSG { uint8_t CtlFlags; // QMICTL_FLAG_REQUEST uint8_t TransactionId; uint16_t QMICTLType; // QMICTL_GET_VERSION_REQ uint16_t Length; // 0 uint8_t TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER uint16_t TLVLength; // var uint8_t QMUXTypes; // List of one byte QMUX_TYPE values // 0xFF returns a list of versions for all // QMUX_TYPEs implemented on the device } __attribute__ ((packed)) QMICTL_GET_VERSION_REQ_MSG, *PQMICTL_GET_VERSION_REQ_MSG; typedef struct _QMUX_TYPE_VERSION_STRUCT { uint8_t QMUXType; uint16_t MajorVersion; uint16_t MinorVersion; } __attribute__ ((packed)) QMUX_TYPE_VERSION_STRUCT, *PQMUX_TYPE_VERSION_STRUCT; typedef struct _QMICTL_GET_VERSION_RESP_MSG { uint8_t CtlFlags; // QMICTL_FLAG_RESPONSE uint8_t TransactionId; uint16_t QMICTLType; // QMICTL_GET_VERSION_RESP uint16_t Length; uint8_t TLVType; // QCTLV_TYPE_RESULT_CODE uint16_t TLVLength; // 0x0004 uint16_t QMIResult; uint16_t QMIError; uint8_t TLV2Type; // QCTLV_TYPE_REQUIRED_PARAMETER uint16_t TLV2Length; // var uint8_t NumElements; // Num of QMUX_TYPE_VERSION_STRUCT QMUX_TYPE_VERSION_STRUCT TypeVersion[0]; } __attribute__ ((packed)) QMICTL_GET_VERSION_RESP_MSG, *PQMICTL_GET_VERSION_RESP_MSG; typedef struct _QMICTL_MSG { union { // Message Header QCQMICTL_MSG_HDR QMICTLMsgHdr; QCQMICTL_MSG_HDR_RESP QMICTLMsgHdrRsp; // QMICTL Message //QMICTL_SET_INSTANCE_ID_REQ_MSG SetInstanceIdReq; //QMICTL_SET_INSTANCE_ID_RESP_MSG SetInstanceIdRsp; QMICTL_GET_VERSION_REQ_MSG GetVersionReq; QMICTL_GET_VERSION_RESP_MSG GetVersionRsp; QMICTL_GET_CLIENT_ID_REQ_MSG GetClientIdReq; QMICTL_GET_CLIENT_ID_RESP_MSG GetClientIdRsp; //QMICTL_RELEASE_CLIENT_ID_REQ_MSG ReleaseClientIdReq; QMICTL_RELEASE_CLIENT_ID_RESP_MSG ReleaseClientIdRsp; //QMICTL_REVOKE_CLIENT_ID_IND_MSG RevokeClientIdInd; //QMICTL_INVALID_CLIENT_ID_IND_MSG InvalidClientIdInd; //QMICTL_SET_DATA_FORMAT_REQ_MSG SetDataFormatReq; //QMICTL_SET_DATA_FORMAT_RESP_MSG SetDataFormatRsp; QMICTL_SYNC_REQ_MSG SyncReq; QMICTL_SYNC_RESP_MSG SyncRsp; QMICTL_SYNC_IND_MSG SyncInd; }; } __attribute__ ((packed)) QMICTL_MSG, *PQMICTL_MSG; typedef struct _QCQMUX_MSG_HDR { uint8_t CtlFlags; // 0: single QMUX Msg; 1: uint16_t TransactionId; uint16_t Type; uint16_t Length; uint8_t payload[0]; } __attribute__ ((packed)) QCQMUX_MSG_HDR, *PQCQMUX_MSG_HDR; typedef struct _QCQMUX_MSG_HDR_RESP { uint8_t CtlFlags; // 0: single QMUX Msg; 1: uint16_t TransactionId; uint16_t Type; uint16_t Length; uint8_t TLVType; // 0x02 - result code uint16_t TLVLength; // 4 uint16_t QMUXResult; // QMI_RESULT_SUCCESS // QMI_RESULT_FAILURE uint16_t QMUXError; // QMI_ERR_INVALID_ARG // QMI_ERR_NO_MEMORY // QMI_ERR_INTERNAL // QMI_ERR_FAULT } __attribute__ ((packed)) QCQMUX_MSG_HDR_RESP, *PQCQMUX_MSG_HDR_RESP; typedef struct _QMIWDS_ADMIN_SET_DATA_FORMAT { uint16_t Type; // QMUX type 0x0000 uint16_t Length; } __attribute__ ((packed)) QMIWDS_ADMIN_SET_DATA_FORMAT, *PQMIWDS_ADMIN_SET_DATA_FORMAT; typedef struct _QMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS { uint8_t TLVType; uint16_t TLVLength; uint8_t QOSSetting; } __attribute__ ((packed)) QMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS, *PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS; typedef struct _QMIWDS_ADMIN_SET_DATA_FORMAT_TLV { uint8_t TLVType; uint16_t TLVLength; uint32_t Value; } __attribute__ ((packed)) QMIWDS_ADMIN_SET_DATA_FORMAT_TLV, *PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV; typedef struct _QMIWDS_ENDPOINT_TLV { uint8_t TLVType; uint16_t TLVLength; uint32_t ep_type; uint32_t iface_id; } __attribute__ ((packed)) QMIWDS_ENDPOINT_TLV, *PQMIWDS_ENDPOINT_TLV; #define QUECTEL_UL_DATA_AGG typedef uint32_t UINT; typedef struct { UINT size; UINT rx_urb_size; UINT ep_type; UINT iface_id; UINT MuxId; UINT ul_data_aggregation_max_datagrams; //0x17 UINT ul_data_aggregation_max_size ;//0x18 UINT dl_minimum_padding; //0x1A } QMAP_SETTING; typedef struct { unsigned int size; unsigned int rx_urb_size; unsigned int ep_type; unsigned int iface_id; unsigned int qmap_mode; unsigned int qmap_version; unsigned int dl_minimum_padding; char ifname[8][16]; unsigned char mux_id[8]; } RMNET_INFO; typedef struct _QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG { uint8_t CtlFlags; // 0: single QMUX Msg; 1: uint16_t TransactionId; uint16_t Type; uint16_t Length; QMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS QosDataFormatTlv; QMIWDS_ADMIN_SET_DATA_FORMAT_TLV UnderlyingLinkLayerProtocolTlv; QMIWDS_ADMIN_SET_DATA_FORMAT_TLV UplinkDataAggregationProtocolTlv; QMIWDS_ADMIN_SET_DATA_FORMAT_TLV DownlinkDataAggregationProtocolTlv; QMIWDS_ADMIN_SET_DATA_FORMAT_TLV DownlinkDataAggregationMaxDatagramsTlv; QMIWDS_ADMIN_SET_DATA_FORMAT_TLV DownlinkDataAggregationMaxSizeTlv; QMIWDS_ENDPOINT_TLV epTlv; #ifdef QUECTEL_UL_DATA_AGG QMIWDS_ADMIN_SET_DATA_FORMAT_TLV DlMinimumPassingTlv; QMIWDS_ADMIN_SET_DATA_FORMAT_TLV UplinkDataAggregationMaxDatagramsTlv; QMIWDS_ADMIN_SET_DATA_FORMAT_TLV UplinkDataAggregationMaxSizeTlv; #endif } __attribute__ ((packed)) QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG, *PQMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG; typedef struct _QCQMUX_TLV { uint8_t Type; uint16_t Length; uint8_t Value[0]; } __attribute__ ((packed)) QCQMUX_TLV, *PQCQMUX_TLV; typedef struct _QMUX_MSG { union { // Message Header QCQMUX_MSG_HDR QMUXMsgHdr; QCQMUX_MSG_HDR_RESP QMUXMsgHdrResp; QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG SetDataFormatReq; }; } __attribute__ ((packed)) QMUX_MSG, *PQMUX_MSG; typedef struct _QCQMIMSG { QCQMI_HDR QMIHdr; union { QMICTL_MSG CTLMsg; QMUX_MSG MUXMsg; }; } __attribute__ ((packed)) QCQMIMSG, *PQCQMIMSG; // QMUX Message Definitions -- QMI SDU #define QMUX_CTL_FLAG_SINGLE_MSG 0x00 #define QMUX_CTL_FLAG_COMPOUND_MSG 0x01 #define QMUX_CTL_FLAG_TYPE_CMD 0x00 #define QMUX_CTL_FLAG_TYPE_RSP 0x02 #define QMUX_CTL_FLAG_TYPE_IND 0x04 #define QMUX_CTL_FLAG_MASK_COMPOUND 0x01 #define QMUX_CTL_FLAG_MASK_TYPE 0x06 // 00-cmd, 01-rsp, 10-ind #define USB_CTL_MSG_TYPE_QMI 0x01 #define QMICTL_FLAG_REQUEST 0x00 #define QMICTL_FLAG_RESPONSE 0x01 #define QMICTL_FLAG_INDICATION 0x02 // QMICTL Type #define QMICTL_SET_INSTANCE_ID_REQ 0x0020 #define QMICTL_SET_INSTANCE_ID_RESP 0x0020 #define QMICTL_GET_VERSION_REQ 0x0021 #define QMICTL_GET_VERSION_RESP 0x0021 #define QMICTL_GET_CLIENT_ID_REQ 0x0022 #define QMICTL_GET_CLIENT_ID_RESP 0x0022 #define QMICTL_RELEASE_CLIENT_ID_REQ 0x0023 #define QMICTL_RELEASE_CLIENT_ID_RESP 0x0023 #define QMICTL_REVOKE_CLIENT_ID_IND 0x0024 #define QMICTL_INVALID_CLIENT_ID_IND 0x0025 #define QMICTL_SET_DATA_FORMAT_REQ 0x0026 #define QMICTL_SET_DATA_FORMAT_RESP 0x0026 #define QMICTL_SYNC_REQ 0x0027 #define QMICTL_SYNC_RESP 0x0027 #define QMICTL_SYNC_IND 0x0027 #define QCTLV_TYPE_REQUIRED_PARAMETER 0x01 // Define QMI Type typedef enum _QMI_SERVICE_TYPE { QMUX_TYPE_CTL = 0x00, QMUX_TYPE_WDS = 0x01, QMUX_TYPE_DMS = 0x02, QMUX_TYPE_NAS = 0x03, QMUX_TYPE_QOS = 0x04, QMUX_TYPE_WMS = 0x05, QMUX_TYPE_PDS = 0x06, QMUX_TYPE_UIM = 0x0B, QMUX_TYPE_WDS_IPV6 = 0x11, QMUX_TYPE_WDS_ADMIN = 0x1A, QMUX_TYPE_MAX = 0xFF, QMUX_TYPE_ALL = 0xFF } QMI_SERVICE_TYPE; #define QMIWDS_ADMIN_SET_DATA_FORMAT_REQ 0x0020 #define QMIWDS_ADMIN_SET_DATA_FORMAT_RESP 0x0020 struct qlistnode { struct qlistnode *next; struct qlistnode *prev; }; #define qnode_to_item(node, container, member) \ (container *) (((char*) (node)) - offsetof(container, member)) #define qlist_for_each(node, list) \ for (node = (list)->next; node != (list); node = node->next) #define qlist_empty(list) ((list) == (list)->next) #define qlist_head(list) ((list)->next) #define qlist_tail(list) ((list)->prev) typedef struct { struct qlistnode qnode; uint8_t ClientFd; QCQMIMSG qmi[0]; } QMI_PROXY_MSG; typedef struct { struct qlistnode qnode; uint8_t QMIType; uint8_t ClientId; unsigned AccessTime; } QMI_PROXY_CLINET; typedef struct { struct qlistnode qnode; struct qlistnode client_qnode; uint8_t ClientFd; unsigned AccessTime; } QMI_PROXY_CONNECTION; static void qlist_init(struct qlistnode *node) { node->next = node; node->prev = node; } static void qlist_add_tail(struct qlistnode *head, struct qlistnode *item) { item->next = head; item->prev = head->prev; head->prev->next = item; head->prev = item; } static void qlist_remove(struct qlistnode *item) { item->next->prev = item->prev; item->prev->next = item->next; } static int qmi_proxy_quit = 0; static pthread_t thread_id = 0; static int cdc_wdm_fd = -1; static int qmi_proxy_server_fd = -1; static struct qlistnode qmi_proxy_connection; static struct qlistnode qmi_proxy_ctl_msg; static PQCQMIMSG s_pCtlReq; static PQCQMIMSG s_pCtlRsq; static pthread_mutex_t s_ctlmutex = PTHREAD_MUTEX_INITIALIZER; static pthread_cond_t s_ctlcond = PTHREAD_COND_INITIALIZER; static void ql_get_driver_rmnet_info(char *ifname, RMNET_INFO *rmnet_info) { int ifc_ctl_sock; struct ifreq ifr; int rc; int request = 0x89F3; unsigned char data[512]; memset(rmnet_info, 0x00, sizeof(*rmnet_info)); ifc_ctl_sock = socket(AF_INET, SOCK_DGRAM, 0); if (ifc_ctl_sock <= 0) { dprintf("socket() failed: %s\n", strerror(errno)); return; } memset(&ifr, 0, sizeof(struct ifreq)); strncpy(ifr.ifr_name, ifname, IFNAMSIZ); ifr.ifr_name[IFNAMSIZ - 1] = 0; ifr.ifr_ifru.ifru_data = (void *)data; rc = ioctl(ifc_ctl_sock, request, &ifr); if (rc < 0) { dprintf("ioctl(0x%x, rmnet_info) failed: %s, rc=%d", request, strerror(errno), rc); } else { memcpy(rmnet_info, data, sizeof(*rmnet_info)); } close(ifc_ctl_sock); } static void ql_set_driver_qmap_setting(char *ifname, QMAP_SETTING *qmap_settings) { int ifc_ctl_sock; struct ifreq ifr; int rc; int request = 0x89F2; ifc_ctl_sock = socket(AF_INET, SOCK_DGRAM, 0); if (ifc_ctl_sock <= 0) { dprintf("socket() failed: %s\n", strerror(errno)); return; } memset(&ifr, 0, sizeof(struct ifreq)); strncpy(ifr.ifr_name, ifname, IFNAMSIZ); ifr.ifr_name[IFNAMSIZ - 1] = 0; ifr.ifr_ifru.ifru_data = (void *)qmap_settings; rc = ioctl(ifc_ctl_sock, request, &ifr); if (rc < 0) { dprintf("ioctl(0x%x, qmap_settings) failed: %s, rc=%d\n", request, strerror(errno), rc); } close(ifc_ctl_sock); } static void setTimespecRelative(struct timespec *p_ts, long long msec) { struct timeval tv; gettimeofday(&tv, (struct timezone *) NULL); /* what's really funny about this is that I know pthread_cond_timedwait just turns around and makes this a relative time again */ p_ts->tv_sec = tv.tv_sec + (msec / 1000); p_ts->tv_nsec = (tv.tv_usec + (msec % 1000) * 1000L ) * 1000L; if (p_ts->tv_nsec >= 1000000000UL) { p_ts->tv_sec += 1; p_ts->tv_nsec -= 1000000000UL; } } static int pthread_cond_timeout_np(pthread_cond_t *cond, pthread_mutex_t * mutex, unsigned msecs) { if (msecs != 0) { unsigned i; unsigned t = msecs/4; int ret = 0; if (t == 0) t = 1; for (i = 0; i < msecs; i += t) { struct timespec ts; setTimespecRelative(&ts, t); ret = pthread_cond_timedwait(cond, mutex, &ts); //to advoid system time change if (ret != ETIMEDOUT) { if(ret) dprintf("ret=%d, msecs=%u, t=%u", ret, msecs, t); break; } } return ret; } else { return pthread_cond_wait(cond, mutex); } } static int create_local_server(const char *name) { int sockfd = -1; int reuse_addr = 1; struct sockaddr_un sockaddr; socklen_t alen; /*Create server socket*/ SYSCHECK(sockfd = socket(AF_LOCAL, SOCK_STREAM, 0)); memset(&sockaddr, 0, sizeof(sockaddr)); sockaddr.sun_family = AF_LOCAL; sockaddr.sun_path[0] = 0; memcpy(sockaddr.sun_path + 1, name, strlen(name) ); alen = strlen(name) + offsetof(struct sockaddr_un, sun_path) + 1; SYSCHECK(setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &reuse_addr,sizeof(reuse_addr))); if(bind(sockfd, (struct sockaddr *)&sockaddr, alen) < 0) { close(sockfd); dprintf("%s bind %s errno: %d (%s)\n", __func__, name, errno, strerror(errno)); return -1; } dprintf("local server: %s sockfd = %d\n", name, sockfd); cfmakenoblock(sockfd); listen(sockfd, 1); return sockfd; } static void accept_qmi_connection(int serverfd) { int clientfd = -1; unsigned char addr[128]; socklen_t alen = sizeof(addr); QMI_PROXY_CONNECTION *qmi_con; clientfd = accept(serverfd, (struct sockaddr *)addr, &alen); qmi_con = (QMI_PROXY_CONNECTION *)malloc(sizeof(QMI_PROXY_CONNECTION)); if (qmi_con) { qlist_init(&qmi_con->qnode); qlist_init(&qmi_con->client_qnode); qmi_con->ClientFd= clientfd; qmi_con->AccessTime = 0; dprintf("+++ ClientFd=%d\n", qmi_con->ClientFd); qlist_add_tail(&qmi_proxy_connection, &qmi_con->qnode); } cfmakenoblock(clientfd); } static void cleanup_qmi_connection(int clientfd) { struct qlistnode *con_node, *qmi_node; qlist_for_each(con_node, &qmi_proxy_connection) { QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode); if (qmi_con->ClientFd == clientfd) { while (!qlist_empty(&qmi_con->client_qnode)) { QMI_PROXY_CLINET *qmi_client = qnode_to_item(qlist_head(&qmi_con->client_qnode), QMI_PROXY_CLINET, qnode); dprintf("xxx ClientFd=%d QMIType=%d ClientId=%d\n", qmi_con->ClientFd, qmi_client->QMIType, qmi_client->ClientId); qlist_remove(&qmi_client->qnode); free(qmi_client); } qlist_for_each(qmi_node, &qmi_proxy_ctl_msg) { QMI_PROXY_MSG *qmi_msg = qnode_to_item(qmi_node, QMI_PROXY_MSG, qnode); if (qmi_msg->ClientFd == qmi_con->ClientFd) { qlist_remove(&qmi_msg->qnode); free(qmi_msg); break; } } dprintf("--- ClientFd=%d\n", qmi_con->ClientFd); close(qmi_con->ClientFd); qlist_remove(&qmi_con->qnode); free(qmi_con); break; } } } static void get_client_id(QMI_PROXY_CONNECTION *qmi_con, PQMICTL_GET_CLIENT_ID_RESP_MSG pClient) { if (pClient->QMIResult == 0 && pClient->QMIError == 0) { QMI_PROXY_CLINET *qmi_client = (QMI_PROXY_CLINET *)malloc(sizeof(QMI_PROXY_CLINET)); qlist_init(&qmi_client->qnode); qmi_client->QMIType = pClient->QMIType; qmi_client->ClientId = pClient->ClientId; qmi_client->AccessTime = 0; dprintf("+++ ClientFd=%d QMIType=%d ClientId=%d\n", qmi_con->ClientFd, qmi_client->QMIType, qmi_client->ClientId); qlist_add_tail(&qmi_con->client_qnode, &qmi_client->qnode); } } static void release_client_id(QMI_PROXY_CONNECTION *qmi_con, PQMICTL_RELEASE_CLIENT_ID_RESP_MSG pClient) { struct qlistnode *client_node; if (pClient->QMIResult == 0 && pClient->QMIError == 0) { qlist_for_each (client_node, &qmi_con->client_qnode) { QMI_PROXY_CLINET *qmi_client = qnode_to_item(client_node, QMI_PROXY_CLINET, qnode); if (pClient->QMIType == qmi_client->QMIType && pClient->ClientId == qmi_client->ClientId) { dprintf("--- ClientFd=%d QMIType=%d ClientId=%d\n", qmi_con->ClientFd, qmi_client->QMIType, qmi_client->ClientId); qlist_remove(&qmi_client->qnode); free(qmi_client); break; } } } } static int verbose_debug = 0; static int send_qmi_to_cdc_wdm(PQCQMIMSG pQMI) { struct pollfd pollfds[]= {{cdc_wdm_fd, POLLOUT, 0}}; ssize_t ret = 0; do { ret = poll(pollfds, sizeof(pollfds)/sizeof(pollfds[0]), 5000); } while ((ret < 0) && (errno == EINTR)); if (pollfds[0].revents & POLLOUT) { ssize_t size = le16toh(pQMI->QMIHdr.Length) + 1; ret = write(cdc_wdm_fd, pQMI, size); if (verbose_debug) { ssize_t i; printf("w %d %zd: ", cdc_wdm_fd, ret); for (i = 0; i < size; i++) printf("%02x ", ((uint8_t *)pQMI)[i]); printf("\n"); } } return ret; } static int send_qmi_to_client(PQCQMIMSG pQMI, int clientFd) { struct pollfd pollfds[]= {{clientFd, POLLOUT, 0}}; ssize_t ret = 0; do { ret = poll(pollfds, sizeof(pollfds)/sizeof(pollfds[0]), 5000); } while ((ret < 0) && (errno == EINTR)); if (pollfds[0].revents & POLLOUT) { ssize_t size = le16toh(pQMI->QMIHdr.Length) + 1; ret = write(clientFd, pQMI, size); if (verbose_debug) { ssize_t i; printf("w %d %zd: ", clientFd, ret); for (i = 0; i < 16; i++) printf("%02x ", ((uint8_t *)pQMI)[i]); printf("\n"); } } return ret; } static int modem_reset_flag = 0; static void recv_qmi(PQCQMIMSG pQMI, unsigned size) { struct qlistnode *con_node, *client_node; if (qmi_proxy_server_fd <= 0) { pthread_mutex_lock(&s_ctlmutex); if (s_pCtlReq != NULL) { if (pQMI->QMIHdr.QMIType == QMUX_TYPE_CTL && s_pCtlReq->CTLMsg.QMICTLMsgHdrRsp.TransactionId == pQMI->CTLMsg.QMICTLMsgHdrRsp.TransactionId) { s_pCtlRsq = malloc(size); memcpy(s_pCtlRsq, pQMI, size); pthread_cond_signal(&s_ctlcond); } else if (pQMI->QMIHdr.QMIType != QMUX_TYPE_CTL && s_pCtlReq->MUXMsg.QMUXMsgHdr.TransactionId == pQMI->MUXMsg.QMUXMsgHdr.TransactionId) { s_pCtlRsq = malloc(size); memcpy(s_pCtlRsq, pQMI, size); pthread_cond_signal(&s_ctlcond); } } pthread_mutex_unlock(&s_ctlmutex); } else if (pQMI->QMIHdr.QMIType == QMUX_TYPE_CTL) { if (pQMI->CTLMsg.QMICTLMsgHdr.CtlFlags == QMICTL_CTL_FLAG_RSP) { if (!qlist_empty(&qmi_proxy_ctl_msg)) { QMI_PROXY_MSG *qmi_msg = qnode_to_item(qlist_head(&qmi_proxy_ctl_msg), QMI_PROXY_MSG, qnode); qlist_for_each(con_node, &qmi_proxy_connection) { QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode); if (qmi_con->ClientFd == qmi_msg->ClientFd) { send_qmi_to_client(pQMI, qmi_msg->ClientFd); if (le16toh(pQMI->CTLMsg.QMICTLMsgHdrRsp.QMICTLType) == QMICTL_GET_CLIENT_ID_RESP) get_client_id(qmi_con, &pQMI->CTLMsg.GetClientIdRsp); else if ((le16toh(pQMI->CTLMsg.QMICTLMsgHdrRsp.QMICTLType) == QMICTL_RELEASE_CLIENT_ID_RESP) || (le16toh(pQMI->CTLMsg.QMICTLMsgHdrRsp.QMICTLType) == QMICTL_REVOKE_CLIENT_ID_IND)) { release_client_id(qmi_con, &pQMI->CTLMsg.ReleaseClientIdRsp); if (le16toh(pQMI->CTLMsg.QMICTLMsgHdrRsp.QMICTLType) == QMICTL_REVOKE_CLIENT_ID_IND) modem_reset_flag = 1; } else { } } } qlist_remove(&qmi_msg->qnode); free(qmi_msg); } } if (!qlist_empty(&qmi_proxy_ctl_msg)) { QMI_PROXY_MSG *qmi_msg = qnode_to_item(qlist_head(&qmi_proxy_ctl_msg), QMI_PROXY_MSG, qnode); qlist_for_each(con_node, &qmi_proxy_connection) { QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode); if (qmi_con->ClientFd == qmi_msg->ClientFd) { send_qmi_to_cdc_wdm(qmi_msg->qmi); } } } } else { qlist_for_each(con_node, &qmi_proxy_connection) { QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode); qlist_for_each(client_node, &qmi_con->client_qnode) { QMI_PROXY_CLINET *qmi_client = qnode_to_item(client_node, QMI_PROXY_CLINET, qnode); if (pQMI->QMIHdr.QMIType == qmi_client->QMIType) { if (pQMI->QMIHdr.ClientId == 0 || pQMI->QMIHdr.ClientId == qmi_client->ClientId) { send_qmi_to_client(pQMI, qmi_con->ClientFd); } } } } } } static int send_qmi(PQCQMIMSG pQMI, unsigned size, int clientfd) { if (qmi_proxy_server_fd <= 0) { send_qmi_to_cdc_wdm(pQMI); } else if (pQMI->QMIHdr.QMIType == QMUX_TYPE_CTL) { QMI_PROXY_MSG *qmi_msg; if (qlist_empty(&qmi_proxy_ctl_msg)) send_qmi_to_cdc_wdm(pQMI); qmi_msg = malloc(sizeof(QMI_PROXY_MSG) + size); qlist_init(&qmi_msg->qnode); qmi_msg->ClientFd = clientfd; memcpy(qmi_msg->qmi, pQMI, size); qlist_add_tail(&qmi_proxy_ctl_msg, &qmi_msg->qnode); } else { send_qmi_to_cdc_wdm(pQMI); } return 0; } static int send_qmi_timeout(PQCQMIMSG pRequest, PQCQMIMSG *ppResponse, unsigned mseconds) { int ret; pthread_mutex_lock(&s_ctlmutex); s_pCtlReq = pRequest; s_pCtlRsq = NULL; if (ppResponse) *ppResponse = NULL; send_qmi_to_cdc_wdm(pRequest); ret = pthread_cond_timeout_np(&s_ctlcond, &s_ctlmutex, mseconds); if (!ret) { if (s_pCtlRsq && ppResponse) { *ppResponse = s_pCtlRsq; } else if (s_pCtlRsq) { free(s_pCtlRsq); } } else { dprintf("%s ret=%d\n", __func__, ret); } s_pCtlReq = NULL; pthread_mutex_unlock(&s_ctlmutex); return ret; } static PQCQMUX_TLV qmi_find_tlv (PQCQMIMSG pQMI, uint8_t TLVType) { int Length = 0; while (Length < le16toh(pQMI->MUXMsg.QMUXMsgHdr.Length)) { PQCQMUX_TLV pTLV = (PQCQMUX_TLV)(&pQMI->MUXMsg.QMUXMsgHdr.payload[Length]); //dprintf("TLV {%02x, %04x}\n", pTLV->Type, pTLV->Length); if (pTLV->Type == TLVType) { return pTLV; } Length += (le16toh((pTLV->Length)) + sizeof(QCQMUX_TLV)); } return NULL; } static int qmi_proxy_init(char *qmi_device, char *ifname) { unsigned i; int ret; QCQMIMSG _QMI; PQCQMIMSG pQMI = &_QMI; PQCQMIMSG pRsp; uint8_t TransactionId = 0xC1; uint8_t WDAClientId = 0; unsigned rx_urb_size = 0; unsigned ep_type, iface_id; unsigned qmap_version; QMAP_SETTING qmap_settings = {0}; RMNET_INFO rmnet_info; dprintf("%s enter\n", __func__); pQMI->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI; pQMI->QMIHdr.CtlFlags = 0x00; pQMI->QMIHdr.QMIType = QMUX_TYPE_CTL; pQMI->QMIHdr.ClientId= 0x00; pQMI->CTLMsg.QMICTLMsgHdr.CtlFlags = QMICTL_FLAG_REQUEST; for (i = 0; i < 10; i++) { pQMI->CTLMsg.SyncReq.TransactionId = TransactionId++; pQMI->CTLMsg.SyncReq.QMICTLType = QMICTL_SYNC_REQ; pQMI->CTLMsg.SyncReq.Length = 0; pQMI->QMIHdr.Length = htole16(le16toh(pQMI->CTLMsg.QMICTLMsgHdr.Length) + sizeof(QCQMI_HDR) + sizeof(QCQMICTL_MSG_HDR) - 1); ret = send_qmi_timeout(pQMI, NULL, 1000); if (!ret) break; } if (ret) goto qmi_proxy_init_fail; pQMI->CTLMsg.GetVersionReq.TransactionId = TransactionId++; pQMI->CTLMsg.GetVersionReq.QMICTLType = htole16(QMICTL_GET_VERSION_REQ); pQMI->CTLMsg.GetVersionReq.Length = htole16(0x0004); pQMI->CTLMsg.GetVersionReq.TLVType= QCTLV_TYPE_REQUIRED_PARAMETER; pQMI->CTLMsg.GetVersionReq.TLVLength = htole16(0x0001); pQMI->CTLMsg.GetVersionReq.QMUXTypes = QMUX_TYPE_ALL; pQMI->QMIHdr.Length = htole16(le16toh(pQMI->CTLMsg.QMICTLMsgHdr.Length) + sizeof(QCQMI_HDR) + sizeof(QCQMICTL_MSG_HDR) - 1); ret = send_qmi_timeout(pQMI, &pRsp, 3000); if (ret || (pRsp == NULL)) goto qmi_proxy_init_fail; if (pRsp) { uint8_t NumElements = 0; if (pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXResult ||pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXError) { dprintf("QMICTL_GET_VERSION_REQ QMUXResult=%d, QMUXError=%d\n", le16toh(pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXResult), le16toh(pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXError)); goto qmi_proxy_init_fail; } for (NumElements = 0; NumElements < pRsp->CTLMsg.GetVersionRsp.NumElements; NumElements++) { if (verbose_debug) dprintf("QMUXType = %02x Version = %d.%d\n", pRsp->CTLMsg.GetVersionRsp.TypeVersion[NumElements].QMUXType, le16toh(pRsp->CTLMsg.GetVersionRsp.TypeVersion[NumElements].MajorVersion), le16toh(pRsp->CTLMsg.GetVersionRsp.TypeVersion[NumElements].MinorVersion)); } } free(pRsp); pQMI->CTLMsg.GetClientIdReq.TransactionId = TransactionId++; pQMI->CTLMsg.GetClientIdReq.QMICTLType = htole16(QMICTL_GET_CLIENT_ID_REQ); pQMI->CTLMsg.GetClientIdReq.Length = htole16(0x0004); pQMI->CTLMsg.GetClientIdReq.TLVType= QCTLV_TYPE_REQUIRED_PARAMETER; pQMI->CTLMsg.GetClientIdReq.TLVLength = htole16(0x0001); pQMI->CTLMsg.GetClientIdReq.QMIType = QMUX_TYPE_WDS_ADMIN; pQMI->QMIHdr.Length = htole16(le16toh(pQMI->CTLMsg.QMICTLMsgHdr.Length) + sizeof(QCQMI_HDR) + sizeof(QCQMICTL_MSG_HDR) - 1); ret = send_qmi_timeout(pQMI, &pRsp, 3000); if (ret || (pRsp == NULL)) goto qmi_proxy_init_fail; if (pRsp) { if (pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXResult ||pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXError) { dprintf("QMICTL_GET_CLIENT_ID_REQ QMUXResult=%d, QMUXError=%d\n", le16toh(pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXResult), le16toh(pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXError)); goto qmi_proxy_init_fail; } WDAClientId = pRsp->CTLMsg.GetClientIdRsp.ClientId; if (verbose_debug) dprintf("WDAClientId = %d\n", WDAClientId); } free(pRsp); rx_urb_size = 16*1024; //must same as rx_urb_size defined in GobiNet&qmi_wwan driver //SDX24&SDX55 support 32KB if(qmidev_is_pciemhi(qmi_device)) ep_type = 0x03; else ep_type = 0x02; iface_id = 0x04; qmap_version = 5; qmap_settings.size = sizeof(qmap_settings); qmap_settings.ul_data_aggregation_max_datagrams = 16; qmap_settings.ul_data_aggregation_max_size = 8*1024; qmap_settings.dl_minimum_padding = 11; //16->11 ql_get_driver_rmnet_info(ifname, &rmnet_info); if (rmnet_info.size) { rx_urb_size = rmnet_info.rx_urb_size; ep_type = rmnet_info.ep_type; iface_id = rmnet_info.iface_id; qmap_version = rmnet_info.qmap_version; qmap_settings.dl_minimum_padding = rmnet_info.dl_minimum_padding; } qmap_settings.dl_minimum_padding = 0; //no effect when register to real netowrk pQMI->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI; pQMI->QMIHdr.CtlFlags = 0x00; pQMI->QMIHdr.QMIType = QMUX_TYPE_WDS_ADMIN; pQMI->QMIHdr.ClientId= WDAClientId; pQMI->MUXMsg.QMUXMsgHdr.CtlFlags = QMICTL_FLAG_REQUEST; pQMI->MUXMsg.QMUXMsgHdr.TransactionId = htole16(TransactionId++); pQMI->MUXMsg.SetDataFormatReq.Type = htole16(QMIWDS_ADMIN_SET_DATA_FORMAT_REQ); pQMI->MUXMsg.SetDataFormatReq.Length = htole16(sizeof(QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG) - sizeof(QCQMUX_MSG_HDR)); //Indicates whether the Quality of Service(QOS) data format is used by the client. pQMI->MUXMsg.SetDataFormatReq.QosDataFormatTlv.TLVType = 0x10; pQMI->MUXMsg.SetDataFormatReq.QosDataFormatTlv.TLVLength = htole16(0x0001); pQMI->MUXMsg.SetDataFormatReq.QosDataFormatTlv.QOSSetting = 0; /* no-QOS header */ //Underlying Link Layer Protocol pQMI->MUXMsg.SetDataFormatReq.UnderlyingLinkLayerProtocolTlv.TLVType = 0x11; pQMI->MUXMsg.SetDataFormatReq.UnderlyingLinkLayerProtocolTlv.TLVLength = htole16(4); pQMI->MUXMsg.SetDataFormatReq.UnderlyingLinkLayerProtocolTlv.Value = htole32(0x02); /* Set IP mode */ //Uplink (UL) data aggregation protocol to be used for uplink data transfer. pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationProtocolTlv.TLVType = 0x12; pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationProtocolTlv.TLVLength = htole16(4); pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationProtocolTlv.Value = htole32(qmap_version); //UL QMAP is enabled //Downlink (DL) data aggregation protocol to be used for downlink data transfer pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationProtocolTlv.TLVType = 0x13; pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationProtocolTlv.TLVLength = htole16(4); pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationProtocolTlv.Value = htole32(qmap_version); //UL QMAP is enabled //Maximum number of datagrams in a single aggregated packet on downlink pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxDatagramsTlv.TLVType = 0x15; pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxDatagramsTlv.TLVLength = htole16(4); pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxDatagramsTlv.Value = htole32((rx_urb_size>2048)?(rx_urb_size/1024):1); //Maximum size in bytes of a single aggregated packet allowed on downlink pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxSizeTlv.TLVType = 0x16; pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxSizeTlv.TLVLength = htole16(4); pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxSizeTlv.Value = htole32(rx_urb_size); //Peripheral End Point ID pQMI->MUXMsg.SetDataFormatReq.epTlv.TLVType = 0x17; pQMI->MUXMsg.SetDataFormatReq.epTlv.TLVLength = htole16(8); pQMI->MUXMsg.SetDataFormatReq.epTlv.ep_type = htole32(ep_type); // DATA_EP_TYPE_BAM_DMUX pQMI->MUXMsg.SetDataFormatReq.epTlv.iface_id = htole32(iface_id); #ifdef QUECTEL_UL_DATA_AGG //Maximum number of datagrams in a single aggregated packet on uplink pQMI->MUXMsg.SetDataFormatReq.DlMinimumPassingTlv.TLVType = 0x19; pQMI->MUXMsg.SetDataFormatReq.DlMinimumPassingTlv.TLVLength = htole16(4); pQMI->MUXMsg.SetDataFormatReq.DlMinimumPassingTlv.Value = htole32(qmap_settings.dl_minimum_padding); //Maximum number of datagrams in a single aggregated packet on uplink pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationMaxDatagramsTlv.TLVType = 0x1B; pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationMaxDatagramsTlv.TLVLength = htole16(4); pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationMaxDatagramsTlv.Value = htole32(qmap_settings.ul_data_aggregation_max_datagrams); //Maximum size in bytes of a single aggregated packet allowed on downlink pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationMaxSizeTlv.TLVType = 0x1C; pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationMaxSizeTlv.TLVLength = htole16(4); pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationMaxSizeTlv.Value = htole32(qmap_settings.ul_data_aggregation_max_size); #endif pQMI->QMIHdr.Length = htole16(le16toh(pQMI->MUXMsg.QMUXMsgHdr.Length) + sizeof(QCQMI_HDR) + sizeof(QCQMUX_MSG_HDR) - 1); ret = send_qmi_timeout(pQMI, &pRsp, 3000); if (ret || (pRsp == NULL)) goto qmi_proxy_init_fail; if (pRsp) { PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV pFormat; if (pRsp->MUXMsg.QMUXMsgHdrResp.QMUXResult || pRsp->MUXMsg.QMUXMsgHdrResp.QMUXError) { dprintf("QMIWDS_ADMIN_SET_DATA_FORMAT_REQ QMUXResult=%d, QMUXError=%d\n", le16toh(pRsp->MUXMsg.QMUXMsgHdrResp.QMUXResult), le16toh(pRsp->MUXMsg.QMUXMsgHdrResp.QMUXError)); goto qmi_proxy_init_fail; } pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x11); if (pFormat) dprintf("link_prot %d\n", le32toh(pFormat->Value)); pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x12); if (pFormat) dprintf("ul_data_aggregation_protocol %d\n", le32toh(pFormat->Value)); pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x13); if (pFormat) dprintf("dl_data_aggregation_protocol %d\n", le32toh(pFormat->Value)); pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x15); if (pFormat) dprintf("dl_data_aggregation_max_datagrams %d\n", le32toh(pFormat->Value)); pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x16); if (pFormat) { dprintf("dl_data_aggregation_max_size %d\n", le32toh(pFormat->Value)); rx_urb_size = le32toh(pFormat->Value); } pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x17); if (pFormat) { qmap_settings.ul_data_aggregation_max_datagrams = MIN(qmap_settings.ul_data_aggregation_max_datagrams, le32toh(pFormat->Value)); dprintf("ul_data_aggregation_max_datagrams %d\n", qmap_settings.ul_data_aggregation_max_datagrams); } pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x18); if (pFormat) { qmap_settings.ul_data_aggregation_max_size = MIN(qmap_settings.ul_data_aggregation_max_size, le32toh(pFormat->Value)); dprintf("ul_data_aggregation_max_size %d\n", qmap_settings.ul_data_aggregation_max_size); } if (qmap_settings.ul_data_aggregation_max_datagrams > 1) { ql_set_driver_qmap_setting(ifname, &qmap_settings); } } free(pRsp); dprintf("%s finished, rx_urb_size is %u\n", __func__, rx_urb_size); return 0; qmi_proxy_init_fail: dprintf("%s failed\n", __func__); return -1; } static void qmi_start_server(const char* servername) { qmi_proxy_server_fd = create_local_server(servername); printf("%s: qmi_proxy_server_fd = %d\n", __func__, qmi_proxy_server_fd); if (qmi_proxy_server_fd == -1) { dprintf("%s Failed to create %s, errno: %d (%s)\n", __func__, "quectel-qmi-proxy", errno, strerror(errno)); } } static void qmi_close_server(const char* servername) { if (qmi_proxy_server_fd != -1) { dprintf("%s %s close server\n", __func__, servername); close(qmi_proxy_server_fd); qmi_proxy_server_fd = -1; } } static uint8_t qmi_buf[2048]; static void *qmi_proxy_loop(void *param) { PQCQMIMSG pQMI = (PQCQMIMSG)qmi_buf; struct qlistnode *con_node; QMI_PROXY_CONNECTION *qmi_con; dprintf("%s enter thread_id %p\n", __func__, (void *)pthread_self()); qlist_init(&qmi_proxy_connection); qlist_init(&qmi_proxy_ctl_msg); while (cdc_wdm_fd > 0 && qmi_proxy_quit == 0) { struct pollfd pollfds[2+64]; int ne, ret, nevents = 0; ssize_t nreads; pollfds[nevents].fd = cdc_wdm_fd; pollfds[nevents].events = POLLIN; pollfds[nevents].revents= 0; nevents++; if (qmi_proxy_server_fd > 0) { pollfds[nevents].fd = qmi_proxy_server_fd; pollfds[nevents].events = POLLIN; pollfds[nevents].revents= 0; nevents++; } qlist_for_each(con_node, &qmi_proxy_connection) { qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode); pollfds[nevents].fd = qmi_con->ClientFd; pollfds[nevents].events = POLLIN; pollfds[nevents].revents= 0; nevents++; if (nevents == (sizeof(pollfds)/sizeof(pollfds[0]))) break; } #if 0 dprintf("poll "); for (ne = 0; ne < nevents; ne++) { dprintf("%d ", pollfds[ne].fd); } dprintf("\n"); #endif do { //ret = poll(pollfds, nevents, -1); ret = poll(pollfds, nevents, (qmi_proxy_server_fd > 0) ? -1 : 200); } while (ret < 0 && errno == EINTR && qmi_proxy_quit == 0); if (ret < 0) { dprintf("%s poll=%d, errno: %d (%s)\n", __func__, ret, errno, strerror(errno)); goto qmi_proxy_loop_exit; } for (ne = 0; ne < nevents; ne++) { int fd = pollfds[ne].fd; short revents = pollfds[ne].revents; if (revents & (POLLERR | POLLHUP | POLLNVAL)) { dprintf("%s poll fd = %d, revents = %04x\n", __func__, fd, revents); if (fd == cdc_wdm_fd) { goto qmi_proxy_loop_exit; } else if(fd == qmi_proxy_server_fd) { } else { cleanup_qmi_connection(fd); } continue; } if (!(pollfds[ne].revents & POLLIN)) { continue; } if (fd == qmi_proxy_server_fd) { accept_qmi_connection(fd); } else if (fd == cdc_wdm_fd) { nreads = read(fd, pQMI, sizeof(qmi_buf)); if (verbose_debug) { ssize_t i; printf("r %d %zd: ", fd, nreads); for (i = 0; i < nreads; i++) printf("%02x ", ((uint8_t *)pQMI)[i]); printf("\n"); } if (nreads <= 0) { dprintf("%s read=%d errno: %d (%s)\n", __func__, (int)nreads, errno, strerror(errno)); goto qmi_proxy_loop_exit; } if (nreads != (le16toh(pQMI->QMIHdr.Length) + 1)) { dprintf("%s nreads=%d, pQCQMI->QMIHdr.Length = %d\n", __func__, (int)nreads, le16toh(pQMI->QMIHdr.Length)); continue; } recv_qmi(pQMI, nreads); if (modem_reset_flag) goto qmi_proxy_loop_exit; } else { nreads = read(fd, pQMI, sizeof(qmi_buf)); if (verbose_debug) { ssize_t i; printf("r %d %zd: ", fd, nreads); for (i = 0; i < 16; i++) printf("%02x ", ((uint8_t *)pQMI)[i]); printf("\n"); } if (nreads <= 0) { dprintf("%s read=%d errno: %d (%s)", __func__, (int)nreads, errno, strerror(errno)); cleanup_qmi_connection(fd); break; } if (nreads != (le16toh(pQMI->QMIHdr.Length) + 1)) { dprintf("%s nreads=%d, pQCQMI->QMIHdr.Length = %d\n", __func__, (int)nreads, le16toh(pQMI->QMIHdr.Length)); continue; } send_qmi(pQMI, nreads, fd); } } } qmi_proxy_loop_exit: while (!qlist_empty(&qmi_proxy_connection)) { QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(qlist_head(&qmi_proxy_connection), QMI_PROXY_CONNECTION, qnode); cleanup_qmi_connection(qmi_con->ClientFd); } dprintf("%s exit, thread_id %p\n", __func__, (void *)pthread_self()); return NULL; } static int dir_get_child(const char *dirname, char *buff, unsigned bufsize) { struct dirent *entptr = NULL; DIR *dirptr = opendir(dirname); if (!dirptr) goto error; while ((entptr = readdir(dirptr))) { if (entptr->d_name[0] == '.') continue; snprintf(buff, bufsize, "%s", entptr->d_name); break; } closedir(dirptr); return 0; error: buff[0] = '\0'; if (dirptr) closedir(dirptr); return -1; } static int mhidevice_detect(char *device_name, char *ifname) { if (!access("/sys/class/net/pcie_mhi0", F_OK)) strcpy(ifname, "pcie_mhi0"); else if (!access("/sys/class/net/rmnet_mhi0", F_OK)) strcpy(ifname, "rmnet_mhi0"); else { goto error; } if (!access("/dev/mhi_QMI0", F_OK)) { strcpy(device_name, "/dev/mhi_QMI0"); } else { goto error; } return 0; error: return -1; } static int qmidevice_detect(char *device_name, char *ifname) { struct dirent* ent = NULL; DIR *pDir; char dir[255] = "/sys/bus/usb/devices"; pDir = opendir(dir); if (pDir) { struct { char subdir[255 * 3]; char qmifile[255]; char ifname[255]; } *pl; char qmidevice[255] = {'\0'}; pl = (typeof(pl)) malloc(sizeof(*pl)); memset(pl, 0x00, sizeof(*pl)); while ((ent = readdir(pDir)) != NULL) { char idVendor[4+1] = {0}; char idProduct[4+1] = {0}; int fd = 0; memset(pl, 0x00, sizeof(*pl)); snprintf(pl->subdir, sizeof(pl->subdir), "%s/%s/idVendor", dir, ent->d_name); fd = open(pl->subdir, O_RDONLY); if (fd > 0) { read(fd, idVendor, 4); close(fd); } snprintf(pl->subdir, sizeof(pl->subdir), "%s/%s/idProduct", dir, ent->d_name); fd = open(pl->subdir, O_RDONLY); if (fd > 0) { read(fd, idProduct, 4); close(fd); } if (strncasecmp(idVendor, "05c6", 4) && strncasecmp(idVendor, "2c7c", 4)) continue; dprintf("Find %s/%s idVendor=%s idProduct=%s\n", dir, ent->d_name, idVendor, idProduct); snprintf(pl->subdir, sizeof(pl->subdir), "%s/%s:1.4/usbmisc", dir, ent->d_name); if (access(pl->subdir, R_OK)) { snprintf(pl->subdir, sizeof(pl->subdir), "%s/%s:1.4/usb", dir, ent->d_name); if (access(pl->subdir, R_OK)) { dprintf("no GobiQMI/usbmic/usb found in %s/%s:1.4\n", dir, ent->d_name); continue; } } dir_get_child(pl->subdir, pl->qmifile, sizeof(pl->qmifile)); snprintf(qmidevice, sizeof(qmidevice), "/dev/%.*s", 100, pl->qmifile); strcpy(device_name, qmidevice); snprintf(pl->subdir, sizeof(pl->subdir), "%s/%s:1.4/net", dir, ent->d_name); dir_get_child(pl->subdir, pl->ifname, sizeof(pl->ifname)); strcpy(ifname, pl->ifname); } closedir(pDir); free(pl); if (device_name[0] == '\0' || ifname[0] == '\0') { goto error; } return 0; } error: return -1; } static void usage(void) { dprintf(" -d A valid qmi device\n" " default /dev/cdc-wdm0, but cdc-wdm0 may be invalid\n" " -i netcard name\n" " -v Will show all details\n"); } static void sig_action(int sig) { if (qmi_proxy_quit == 0) { qmi_proxy_quit = 1; if (thread_id) pthread_kill(thread_id, sig); } } int main(int argc, char *argv[]) { int opt; char cdc_wdm[32+1] = {'\0'}; char ifname[32+1] = {'\0'}; int retry_times = 0; char servername[64] = {0}; optind = 1; signal(SIGINT, sig_action); while ( -1 != (opt = getopt(argc, argv, "d:i:vh"))) { switch (opt) { case 'd': strcpy(cdc_wdm, optarg); break; case 'v': verbose_debug = 1; break; case 'i': strcpy(ifname, optarg); break; default: usage(); return 0; } } if (cdc_wdm[0] == '\0' || ifname[0] == '\0') { if(qmidevice_detect(cdc_wdm, ifname) && mhidevice_detect(cdc_wdm, ifname)) { dprintf("network interface '%s' or qmidev '%s' is not exist\n", ifname, cdc_wdm); return -1; } } if (cdc_wdm[0] == '\0' || ifname[0] == '\0') { dprintf("network interface '%s' or qmidev '%s' is not exist\n", ifname, cdc_wdm); return -1; } sprintf(servername, "quectel-qmi-proxy%d", cdc_wdm[strlen(cdc_wdm)-1]-'0'); if (access(cdc_wdm, R_OK | W_OK)) { dprintf("Fail to access %s, errno: %d (%s). break\n", cdc_wdm, errno, strerror(errno)); return -1; } while (qmi_proxy_quit == 0) { if (access(cdc_wdm, R_OK | W_OK)) { dprintf("Fail to access %s, errno: %d (%s). continue\n", cdc_wdm, errno, strerror(errno)); // wait device sleep(3); continue; } dprintf("Will use cdc-wdm='%s', ifname='%s'\n", cdc_wdm, ifname); cdc_wdm_fd = open(cdc_wdm, O_RDWR | O_NONBLOCK | O_NOCTTY); if (cdc_wdm_fd == -1) { dprintf("Failed to open %s, errno: %d (%s). break\n", cdc_wdm, errno, strerror(errno)); return -1; } cfmakenoblock(cdc_wdm_fd); /* no qmi_proxy_loop lives, create one */ pthread_create(&thread_id, NULL, qmi_proxy_loop, NULL); /* try to redo init if failed, init function must be successfully */ while (qmi_proxy_init(cdc_wdm, ifname) != 0) { if (retry_times < 5) { dprintf("fail to init proxy, try again in 2 seconds.\n"); sleep(2); retry_times++; } else { dprintf("has failed too much times, restart the modem and have a try...\n"); break; } /* break loop if modem is detached */ if (access(cdc_wdm, F_OK|R_OK|W_OK)) break; } retry_times = 0; qmi_start_server(servername); pthread_join(thread_id, NULL); /* close local server at last */ qmi_close_server(servername); close(cdc_wdm_fd); /* DO RESTART IN 20s IF MODEM RESET ITSELF */ if (modem_reset_flag) { unsigned int time_to_wait = 20; while (time_to_wait) { time_to_wait = sleep(time_to_wait); } modem_reset_flag = 0; } } return 0; }