dchvs/eg25-g
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
480df7a9f4
eg25-g/quectel-CM/quectel-qmi-proxy.c
dchvs 480df7a9f4 Add driver eg25-g sources
2021-01-21 22:34:57 -06:00

1132 lines
39 KiB
C
Executable File
Raw Blame History

#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <stdarg.h>
#include <stddef.h>
#include <fcntl.h>
#include <pthread.h>
#include <sys/epoll.h>
#include <sys/poll.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <linux/un.h>
typedef unsigned char uint8_t;
typedef unsigned short uint16_t;
typedef unsigned int uint32_t;
#define cpu_to_le16(x) (x)
#define cpu_to_le32(x) (x)
#define le16_to_cpu(x) (x)
#define le32_to_cpu(x) (x)
#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)
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;
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;
} __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 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 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;
}
static int pthread_cond_timeout_np(pthread_cond_t *cond, pthread_mutex_t * mutex, unsigned msecs) {
if (msecs != 0) {
struct timespec ts;
setTimespecRelative(&ts, msecs);
return pthread_cond_timedwait(cond, mutex, &ts);
} 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 = le16_to_cpu(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 < 16; 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 = le16_to_cpu(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 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 (pQMI->CTLMsg.QMICTLMsgHdrRsp.QMICTLType == QMICTL_GET_CLIENT_ID_RESP)
get_client_id(qmi_con, &pQMI->CTLMsg.GetClientIdRsp);
else if (pQMI->CTLMsg.QMICTLMsgHdrRsp.QMICTLType == QMICTL_RELEASE_CLIENT_ID_RESP)
release_client_id(qmi_con, &pQMI->CTLMsg.ReleaseClientIdRsp);
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, errno: %d (%s)", __func__, ret, errno, strerror(errno));
}
s_pCtlReq = NULL;
pthread_mutex_unlock(&s_ctlmutex);
return ret;
}
PQCQMUX_TLV qmi_find_tlv (PQCQMIMSG pQMI, uint8_t TLVType) {
int Length = 0;
while (Length < le16_to_cpu(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 += (le16_to_cpu((pTLV->Length)) + sizeof(QCQMUX_TLV));
}
return NULL;
}
static int qmi_proxy_init(void) {
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;
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 = 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 = QMICTL_GET_VERSION_REQ;
pQMI->CTLMsg.GetVersionReq.Length = 0x0004;
pQMI->CTLMsg.GetVersionReq.TLVType= QCTLV_TYPE_REQUIRED_PARAMETER;
pQMI->CTLMsg.GetVersionReq.TLVLength = 0x0001;
pQMI->CTLMsg.GetVersionReq.QMUXTypes = QMUX_TYPE_ALL;
pQMI->QMIHdr.Length = 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) {
goto qmi_proxy_init_fail;
}
for (NumElements = 0; NumElements < pRsp->CTLMsg.GetVersionRsp.NumElements; NumElements++) {
dprintf("QMUXType = %02x Version = %d.%d\n",
pRsp->CTLMsg.GetVersionRsp.TypeVersion[NumElements].QMUXType,
pRsp->CTLMsg.GetVersionRsp.TypeVersion[NumElements].MajorVersion,
pRsp->CTLMsg.GetVersionRsp.TypeVersion[NumElements].MinorVersion);
}
}
free(pRsp);
pQMI->CTLMsg.GetClientIdReq.TransactionId = TransactionId++;
pQMI->CTLMsg.GetClientIdReq.QMICTLType = QMICTL_GET_CLIENT_ID_REQ;
pQMI->CTLMsg.GetClientIdReq.Length = 0x0004;
pQMI->CTLMsg.GetClientIdReq.TLVType= QCTLV_TYPE_REQUIRED_PARAMETER;
pQMI->CTLMsg.GetClientIdReq.TLVLength = 0x0001;
pQMI->CTLMsg.GetClientIdReq.QMIType = QMUX_TYPE_WDS_ADMIN;
pQMI->QMIHdr.Length = 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) {
goto qmi_proxy_init_fail;
}
WDAClientId = pRsp->CTLMsg.GetClientIdRsp.ClientId;
dprintf("WDAClientId = %d\n", WDAClientId);
}
free(pRsp);
rx_urb_size = 16 * 1024; //must same as rx_urb_size defined in GobiNet&qmi_wwan driver
ep_type = 0x02;
iface_id = 0x04;
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 = TransactionId++;
pQMI->MUXMsg.SetDataFormatReq.Type = QMIWDS_ADMIN_SET_DATA_FORMAT_REQ;
pQMI->MUXMsg.SetDataFormatReq.Length = 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 = cpu_to_le16(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 = cpu_to_le16(4);
pQMI->MUXMsg.SetDataFormatReq.UnderlyingLinkLayerProtocolTlv.Value = cpu_to_le32(0x02); /* Set Ethernet mode */
//Uplink (UL) data aggregation protocol to be used for uplink data transfer.
pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationProtocolTlv.TLVType = 0x12;
pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationProtocolTlv.TLVLength = cpu_to_le16(4);
pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationProtocolTlv.Value = cpu_to_le32(0x05); //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 = cpu_to_le16(4);
pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationProtocolTlv.Value = cpu_to_le32(0x05); //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 = cpu_to_le16(4);
pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxDatagramsTlv.Value = cpu_to_le32((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 = cpu_to_le16(4);
pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxSizeTlv.Value = cpu_to_le32(rx_urb_size);
//Peripheral End Point ID
pQMI->MUXMsg.SetDataFormatReq.epTlv.TLVType = 0x17;
pQMI->MUXMsg.SetDataFormatReq.epTlv.TLVLength = cpu_to_le16(8);
pQMI->MUXMsg.SetDataFormatReq.epTlv.ep_type = cpu_to_le32(ep_type); // DATA_EP_TYPE_BAM_DMUX
pQMI->MUXMsg.SetDataFormatReq.epTlv.iface_id = cpu_to_le32(iface_id);
pQMI->QMIHdr.Length = 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) {
goto qmi_proxy_init_fail;
}
pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x11);
if (pFormat)
dprintf("link_prot %d\n", pFormat->Value);
pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x12);
if (pFormat)
dprintf("ul_data_aggregation_protoco %d\n", pFormat->Value);
pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x13);
if (pFormat)
dprintf("dl_data_aggregation_protoco %d\n", pFormat->Value);
pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x15);
if (pFormat)
dprintf("dl_data_aggregation_max_datagrams %d\n", pFormat->Value);
pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x16);
if (pFormat)
dprintf("dl_data_aggregation_max_size %d\n", pFormat->Value);
pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x17);
if (pFormat)
dprintf("ul_data_aggregation_max_datagrams %d\n", pFormat->Value);
pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x18);
if (pFormat)
dprintf("ul_data_aggregation_max_size %d\n", pFormat->Value);
}
free(pRsp);
qmi_proxy_server_fd = create_local_server("quectel-qmi-proxy");
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));
goto qmi_proxy_init_fail;
}
dprintf("%s exit\n", __func__);
return 0;
qmi_proxy_init_fail:
dprintf("%s exit\n", __func__);
return -1;
}
static void *qmi_proxy_loop(void *param)
{
static uint8_t qmi_buf[2048];
PQCQMIMSG pQMI = (PQCQMIMSG)qmi_buf;
struct qlistnode *con_node;
QMI_PROXY_CONNECTION *qmi_con;
dprintf("%s enter\n", __func__);
qlist_init(&qmi_proxy_connection);
qlist_init(&qmi_proxy_ctl_msg);
while (cdc_wdm_fd > 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;
}
dprintf("poll ");
for (ne = 0; ne < nevents; ne++) {
dprintf("%d ", pollfds[ne].fd);
}
dprintf("\n");
do {
//ret = poll(pollfds, nevents, -1);
ret = poll(pollfds, nevents, (qmi_proxy_server_fd > 0) ? -1 : 200);
} while (ret < 0 && errno == EINTR);
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 < 16; 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 != ((pQMI->QMIHdr.Length) + 1)) {
dprintf("%s nreads=%d, pQCQMI->QMIHdr.Length = %d\n", __func__, (int)nreads, (pQMI->QMIHdr.Length));
continue;
}
recv_qmi(pQMI, nreads);
}
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 != ((pQMI->QMIHdr.Length) + 1)) {
dprintf("%s nreads=%d, pQCQMI->QMIHdr.Length = %d\n", __func__, (int)nreads, (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\n", __func__);
return NULL;
}
int main(int argc, char *argv[]) {
int opt;
const char *cdc_wdm = "/dev/cdc-wdm0";
pthread_t thread_id;
optind = 1;
while ( -1 != (opt = getopt(argc, argv, "d:v"))) {
switch (opt) {
case 'd':
cdc_wdm = optarg;
break;
case 'v':
verbose_debug = 1;
break;
default:
break;
}
}
while(1) {
if (access("/dev/cdc-wdm0", F_OK)) {
sleep(2);
continue;
}
cdc_wdm_fd = open(cdc_wdm, O_RDWR | O_NONBLOCK | O_NOCTTY);
if (cdc_wdm_fd == -1) {
dprintf("%s Failed to open %s, errno: %d (%s)\n", __func__, cdc_wdm, errno, strerror(errno));
return -1;
}
cfmakenoblock(cdc_wdm_fd);
if (!pthread_create(&thread_id, NULL, qmi_proxy_loop, NULL)) {
if (!qmi_proxy_init()) {
pthread_join(thread_id, NULL);
close(qmi_proxy_server_fd);
qmi_proxy_server_fd = -1;
}
}
}
return 0;
}
Reference in New Issue View Git Blame Copy Permalink