init
This commit is contained in:
zhongluofeng
@@ -0,0 +1,902 @@
|
||||
/*
|
||||
Copyright 2025 Quectel Wireless Solutions Co.,Ltd
|
||||
|
||||
Quectel hereby grants customers of Quectel a license to use, modify,
|
||||
distribute and publish the Software in binary form provided that
|
||||
customers shall have no right to reverse engineer, reverse assemble,
|
||||
decompile or reduce to source code form any portion of the Software.
|
||||
Under no circumstances may customers modify, demonstrate, use, deliver
|
||||
or disclose any portion of the Software in source code form.
|
||||
*/
|
||||
|
||||
#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 <poll.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/un.h>
|
||||
#include <linux/if.h>
|
||||
#include <dirent.h>
|
||||
#include <signal.h>
|
||||
#include <inttypes.h>
|
||||
#include <linux/socket.h>
|
||||
#include <linux/qrtr.h>
|
||||
|
||||
#define QRTR_PROTO_VER_1 1
|
||||
struct qrtr_hdr_v1 {
|
||||
__le32 version;
|
||||
__le32 type;
|
||||
__le32 src_node_id;
|
||||
__le32 src_port_id;
|
||||
__le32 confirm_rx;
|
||||
__le32 size;
|
||||
__le32 dst_node_id;
|
||||
__le32 dst_port_id;
|
||||
} __attribute__ ((packed));
|
||||
|
||||
#include "qendian.h"
|
||||
#include "qlist.h"
|
||||
#include "QCQMI.h"
|
||||
#include "QCQCTL.h"
|
||||
#include "QCQMUX.h"
|
||||
|
||||
static const char * get_time(void) {
|
||||
static char time_buf[128];
|
||||
struct timeval tv;
|
||||
time_t time;
|
||||
suseconds_t millitm;
|
||||
struct tm *ti;
|
||||
|
||||
gettimeofday (&tv, NULL);
|
||||
|
||||
time= tv.tv_sec;
|
||||
millitm = (tv.tv_usec + 500) / 1000;
|
||||
|
||||
if (millitm == 1000) {
|
||||
++time;
|
||||
millitm = 0;
|
||||
}
|
||||
|
||||
ti = localtime(&time);
|
||||
sprintf(time_buf, "[%02d-%02d_%02d:%02d:%02d:%03d]", ti->tm_mon+1, ti->tm_mday, ti->tm_hour, ti->tm_min, ti->tm_sec, (int)millitm);
|
||||
return time_buf;
|
||||
}
|
||||
|
||||
#define dprintf(fmt, args...) do { fprintf(stdout, "%s " fmt, get_time(), ##args); } 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 align_4(_len) (((_len) + 3) & ~3)
|
||||
|
||||
typedef struct {
|
||||
struct qlistnode qnode;
|
||||
int ClientFd;
|
||||
QCQMIMSG qrtr[0];
|
||||
} QRTR_PROXY_MSG;
|
||||
|
||||
typedef struct {
|
||||
struct qlistnode qnode;
|
||||
uint8_t QMIType;
|
||||
uint8_t ClientId;
|
||||
uint32_t node_id;
|
||||
uint32_t port_id;
|
||||
unsigned AccessTime;
|
||||
} QRTR_PROXY_CLINET;
|
||||
|
||||
typedef struct {
|
||||
struct qlistnode qnode;
|
||||
struct qlistnode client_qnode;
|
||||
int ClientFd;
|
||||
unsigned AccessTime;
|
||||
} QRTR_PROXY_CONNECTION;
|
||||
|
||||
typedef struct {
|
||||
struct qlistnode qnode;
|
||||
uint32_t service;
|
||||
uint32_t version;
|
||||
uint32_t instance;
|
||||
uint32_t node;
|
||||
uint32_t port;
|
||||
|
||||
__le32 src_node_id;
|
||||
__le32 src_port_id;
|
||||
} QRTR_SERVICE;
|
||||
|
||||
static int qrtr_proxy_quit = 0;
|
||||
static pthread_t thread_id = 0;
|
||||
static int cdc_wdm_fd = -1;
|
||||
static int qrtr_proxy_server_fd = -1;
|
||||
static struct qlistnode qrtr_proxy_connection;
|
||||
static struct qlistnode qrtr_server_list;
|
||||
static int verbose_debug = 0;
|
||||
static uint32_t node_modem = 3; //IPQ ~ 3, QCM ~ 0
|
||||
static uint32_t node_myself = 1;
|
||||
|
||||
static QRTR_SERVICE *find_qrtr_service(uint8_t QMIType)
|
||||
{
|
||||
struct qlistnode *node;
|
||||
|
||||
qlist_for_each (node, &qrtr_server_list) {
|
||||
QRTR_SERVICE *srv = qnode_to_item(node, QRTR_SERVICE, qnode);
|
||||
if (srv->service == QMIType)
|
||||
return srv;
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static uint8_t client_bitmap[0xf0];
|
||||
static uint8_t port_bitmap[0xff0];
|
||||
static int alloc_client_id(void) {
|
||||
int id = 1;
|
||||
|
||||
for (id = 1; id < (int)sizeof(client_bitmap); id++) {
|
||||
if (client_bitmap[id] == 0) {
|
||||
client_bitmap[id] = id;
|
||||
return id;
|
||||
}
|
||||
}
|
||||
|
||||
dprintf("NOT find %s()\n", __func__);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void free_client_id(int id) {
|
||||
if (id < (int)sizeof(client_bitmap) && client_bitmap[id] == id) {
|
||||
client_bitmap[id] = 0;
|
||||
return;
|
||||
}
|
||||
dprintf("NOT find %s(id=%d)\n", __func__, id);
|
||||
}
|
||||
|
||||
static int alloc_port_id(void) {
|
||||
int id = 1;
|
||||
|
||||
for (id = 1; id < (int)sizeof(port_bitmap); id++) {
|
||||
if (port_bitmap[id] == 0) {
|
||||
port_bitmap[id] = id;
|
||||
return id;
|
||||
}
|
||||
}
|
||||
|
||||
dprintf("NOT find %s()\n", __func__);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void free_port_id(int id) {
|
||||
if (id < (int)sizeof(port_bitmap) && port_bitmap[id] == id) {
|
||||
port_bitmap[id] = 0;
|
||||
return;
|
||||
}
|
||||
dprintf("NOT find %s(id=%d)\n", __func__, id);
|
||||
}
|
||||
|
||||
static void dump_qrtr(void *buf, size_t len, char flag)
|
||||
{
|
||||
size_t i;
|
||||
static char printf_buf[1024];
|
||||
int cnt = 0, limit=1024;
|
||||
unsigned char *d = (unsigned char *)buf;
|
||||
struct qrtr_hdr_v1 *hdr = (struct qrtr_hdr_v1 *)buf;
|
||||
const char *ctrl_pkt_strings[] = {
|
||||
[QRTR_TYPE_DATA] = "data",
|
||||
[QRTR_TYPE_HELLO] = "hello",
|
||||
[QRTR_TYPE_BYE] = "bye",
|
||||
[QRTR_TYPE_NEW_SERVER] = "new-server",
|
||||
[QRTR_TYPE_DEL_SERVER] = "del-server",
|
||||
[QRTR_TYPE_DEL_CLIENT] = "del-client",
|
||||
[QRTR_TYPE_RESUME_TX] = "resume-tx",
|
||||
[QRTR_TYPE_EXIT] = "exit",
|
||||
[QRTR_TYPE_PING] = "ping",
|
||||
[QRTR_TYPE_NEW_LOOKUP] = "new-lookup",
|
||||
[QRTR_TYPE_DEL_LOOKUP] = "del-lookup",
|
||||
};
|
||||
|
||||
for (i = 0; i < len && i < 64; i++) {
|
||||
if (i%4 == 0)
|
||||
cnt += snprintf(printf_buf+cnt, limit-cnt, " ");
|
||||
cnt += snprintf(printf_buf+cnt, limit-cnt, "%02x", d[i]);
|
||||
}
|
||||
dprintf("%s\n", printf_buf);
|
||||
|
||||
dprintf("%c ver=%d, type=%d(%s), %x,%x -> %x,%x, confirm_rx=%d, size=%u\n",
|
||||
flag,
|
||||
le32toh(hdr->version), le32toh(hdr->type), ctrl_pkt_strings[le32toh(hdr->type)],
|
||||
le32toh(hdr->src_node_id), le32toh(hdr->src_port_id), le32toh(hdr->dst_node_id), le32toh(hdr->dst_port_id),
|
||||
le32toh(hdr->confirm_rx), le32toh(hdr->size));
|
||||
}
|
||||
|
||||
static int send_qmi_to_client(PQCQMIMSG pQMI, int fd) {
|
||||
struct pollfd pollfds[]= {{fd, POLLOUT, 0}};
|
||||
ssize_t ret = 0;
|
||||
ssize_t size = le16toh(pQMI->QMIHdr.Length) + 1;
|
||||
|
||||
do {
|
||||
ret = poll(pollfds, sizeof(pollfds)/sizeof(pollfds[0]), 5000);
|
||||
} while (ret == -1 && errno == EINTR && qrtr_proxy_quit == 0);
|
||||
|
||||
if (pollfds[0].revents & POLLOUT) {
|
||||
ret = write(fd, pQMI, size);
|
||||
}
|
||||
|
||||
return ret == size ? 0 : -1;
|
||||
}
|
||||
|
||||
static int send_qrtr_to_dev(struct qrtr_hdr_v1 *hdr, int fd) {
|
||||
struct pollfd pollfds[]= {{fd, POLLOUT, 0}};
|
||||
ssize_t ret = 0;
|
||||
ssize_t size = align_4(le32toh(hdr->size) + sizeof(*hdr));
|
||||
|
||||
do {
|
||||
ret = poll(pollfds, sizeof(pollfds)/sizeof(pollfds[0]), 5000);
|
||||
} while (ret == -1 && errno == EINTR && qrtr_proxy_quit == 0);
|
||||
|
||||
if (pollfds[0].revents & POLLOUT) {
|
||||
ret = write(fd, hdr, size);
|
||||
}
|
||||
|
||||
return ret == size ? 0 : -1;
|
||||
}
|
||||
|
||||
static int qrtr_node_enqueue(const void *data, size_t len,
|
||||
int type, struct sockaddr_qrtr *from,
|
||||
struct sockaddr_qrtr *to, unsigned int confirm_rx)
|
||||
{
|
||||
int rc = -1;
|
||||
size_t size = sizeof(struct qrtr_hdr_v1) + len;
|
||||
struct qrtr_hdr_v1 *hdr = (struct qrtr_hdr_v1 *)malloc(align_4(size));
|
||||
|
||||
if (hdr) {
|
||||
hdr->version = htole32(QRTR_PROTO_VER_1);
|
||||
hdr->type = htole32(type);
|
||||
hdr->src_node_id = htole32(from->sq_node);
|
||||
hdr->src_port_id = htole32(from->sq_port);
|
||||
hdr->dst_node_id = htole32(to->sq_node);
|
||||
hdr->dst_port_id = htole32(to->sq_port);
|
||||
hdr->size = htole32(len);
|
||||
hdr->confirm_rx = htole32(!!confirm_rx);
|
||||
|
||||
memcpy(hdr + 1, data, len);
|
||||
dump_qrtr(hdr, size, '>');
|
||||
send_qrtr_to_dev(hdr, cdc_wdm_fd);
|
||||
free(hdr);
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
static int send_ctrl_hello(__u32 sq_node, __u32 sq_port)
|
||||
{
|
||||
struct qrtr_ctrl_pkt pkt;
|
||||
int rc;
|
||||
struct sockaddr_qrtr to = {AF_QIPCRTR, sq_node, sq_port};
|
||||
struct sockaddr_qrtr from = {AF_QIPCRTR, node_myself, QRTR_PORT_CTRL};
|
||||
|
||||
memset(&pkt, 0, sizeof(pkt));
|
||||
pkt.cmd = htole32(QRTR_TYPE_HELLO);
|
||||
|
||||
rc = qrtr_node_enqueue(&pkt, sizeof(pkt), QRTR_TYPE_HELLO, &from, &to, 0);
|
||||
if (rc < 0)
|
||||
return rc;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int ctrl_cmd_del_client(__u32 sq_node, __u32 sq_port, uint8_t QMIType)
|
||||
{
|
||||
struct qrtr_ctrl_pkt pkt;
|
||||
int rc;
|
||||
struct sockaddr_qrtr to = {AF_QIPCRTR, QRTR_NODE_BCAST, QRTR_PORT_CTRL};
|
||||
struct sockaddr_qrtr from = {AF_QIPCRTR, sq_node, sq_port};
|
||||
QRTR_SERVICE *srv = find_qrtr_service(QMIType);
|
||||
|
||||
if (srv) {
|
||||
to.sq_node = srv->src_node_id;
|
||||
}
|
||||
|
||||
memset(&pkt, 0, sizeof(pkt));
|
||||
pkt.cmd = htole32(QRTR_TYPE_DEL_CLIENT);
|
||||
pkt.client.node = htole32(sq_node);
|
||||
pkt.client.port = htole32(sq_port);
|
||||
|
||||
rc = qrtr_node_enqueue(&pkt, sizeof(pkt), QRTR_TYPE_DATA, &from, &to, 0);
|
||||
if (rc < 0)
|
||||
return rc;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void handle_server_change(struct qrtr_hdr_v1 *hdr) {
|
||||
struct qrtr_ctrl_pkt *pkt = (struct qrtr_ctrl_pkt *)(hdr + 1);
|
||||
QRTR_SERVICE *s;
|
||||
|
||||
dprintf ("[qrtr] %s server on %u:%u(%u:%u) -> service %u, instance %x\n",
|
||||
QRTR_TYPE_NEW_SERVER == hdr->type ? "add" : "remove",
|
||||
le32toh(pkt->server.node), le32toh(pkt->server.port),
|
||||
le32toh(hdr->src_node_id), le32toh(hdr->src_port_id),
|
||||
le32toh(pkt->server.service), le32toh(pkt->server.instance));
|
||||
|
||||
if (le32toh(pkt->server.node) != node_modem) {
|
||||
return; //we only care modem
|
||||
}
|
||||
|
||||
s = (QRTR_SERVICE *)malloc(sizeof(QRTR_SERVICE));
|
||||
if (!s)
|
||||
return;
|
||||
|
||||
qlist_init(&s->qnode);
|
||||
s->service = le32toh(pkt->server.service);
|
||||
s->version = le32toh(pkt->server.instance) & 0xff;
|
||||
s->instance = le32toh(pkt->server.instance) >> 8;
|
||||
s->node = le32toh(pkt->server.node);
|
||||
s->port = le32toh(pkt->server.port);
|
||||
|
||||
s->src_node_id = le32toh(hdr->src_node_id);
|
||||
s->src_port_id = le32toh(hdr->src_port_id);
|
||||
|
||||
if (QRTR_TYPE_NEW_SERVER == hdr->type) {
|
||||
qlist_add_tail(&qrtr_server_list, &s->qnode);
|
||||
}
|
||||
else if (QRTR_TYPE_DEL_SERVER == hdr->type) {
|
||||
qlist_remove(&s->qnode);
|
||||
}
|
||||
}
|
||||
|
||||
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("bind %s errno: %d (%s)\n", name, errno, strerror(errno));
|
||||
return -1;
|
||||
}
|
||||
|
||||
dprintf("local server: %s sockfd = %d\n", name, sockfd);
|
||||
cfmakenoblock(sockfd);
|
||||
listen(sockfd, 1);
|
||||
|
||||
return sockfd;
|
||||
}
|
||||
|
||||
static uint8_t alloc_qrtr_client_id(QRTR_PROXY_CONNECTION *qrtr_con, uint8_t QMIType) {
|
||||
QRTR_PROXY_CLINET *qrtr_client = (QRTR_PROXY_CLINET *)malloc(sizeof(QRTR_PROXY_CLINET));
|
||||
|
||||
qlist_init(&qrtr_client->qnode);
|
||||
qrtr_client->QMIType = QMIType;
|
||||
qrtr_client->ClientId = alloc_client_id();
|
||||
qrtr_client->node_id = 1;
|
||||
qrtr_client->port_id = alloc_port_id();
|
||||
qrtr_client->AccessTime = 0;
|
||||
|
||||
dprintf("+++ ClientFd=%d QMIType=%d ClientId=%d, node_id=%d, port_id=%d\n",
|
||||
qrtr_con->ClientFd, qrtr_client->QMIType, qrtr_client->ClientId,
|
||||
qrtr_client->node_id, qrtr_client->port_id);
|
||||
qlist_add_tail(&qrtr_con->client_qnode, &qrtr_client->qnode);
|
||||
|
||||
return qrtr_client->ClientId;
|
||||
}
|
||||
|
||||
static void release_qrtr_client_id(QRTR_PROXY_CONNECTION *qrtr_con, uint8_t QMIType, uint8_t ClientId) {
|
||||
struct qlistnode *client_node;
|
||||
int find = 0;
|
||||
|
||||
qlist_for_each (client_node, &qrtr_con->client_qnode) {
|
||||
QRTR_PROXY_CLINET *qrtr_client = qnode_to_item(client_node, QRTR_PROXY_CLINET, qnode);
|
||||
|
||||
if (QMIType == qrtr_client->QMIType && ClientId == qrtr_client->ClientId) {
|
||||
dprintf("--- ClientFd=%d QMIType=%d ClientId=%d, node_id=%d, port_id=%d\n",
|
||||
qrtr_con->ClientFd, qrtr_client->QMIType, qrtr_client->ClientId,
|
||||
qrtr_client->node_id, qrtr_client->port_id);
|
||||
ctrl_cmd_del_client(qrtr_client->node_id, qrtr_client->port_id, qrtr_client->QMIType);
|
||||
free_client_id(qrtr_client->ClientId);
|
||||
free_port_id(qrtr_client->port_id);
|
||||
qlist_remove(&qrtr_client->qnode);
|
||||
free(qrtr_client);
|
||||
find++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!find) {
|
||||
dprintf("NOT find on %s(ClientFd=%d, QMIType=%d, ClientId=%d)\n",
|
||||
__func__, qrtr_con->ClientFd, QMIType, ClientId);
|
||||
}
|
||||
}
|
||||
|
||||
static void accept_qrtr_connection(int serverfd) {
|
||||
int clientfd = -1;
|
||||
unsigned char addr[128];
|
||||
socklen_t alen = sizeof(addr);
|
||||
QRTR_PROXY_CONNECTION *qrtr_con;
|
||||
|
||||
clientfd = accept(serverfd, (struct sockaddr *)addr, &alen);
|
||||
|
||||
qrtr_con = (QRTR_PROXY_CONNECTION *)malloc(sizeof(QRTR_PROXY_CONNECTION));
|
||||
if (qrtr_con) {
|
||||
qlist_init(&qrtr_con->qnode);
|
||||
qlist_init(&qrtr_con->client_qnode);
|
||||
qrtr_con->ClientFd= clientfd;
|
||||
qrtr_con->AccessTime = 0;
|
||||
dprintf("+++ ClientFd=%d\n", qrtr_con->ClientFd);
|
||||
qlist_add_tail(&qrtr_proxy_connection, &qrtr_con->qnode);
|
||||
}
|
||||
|
||||
cfmakenoblock(clientfd);
|
||||
}
|
||||
|
||||
static void cleanup_qrtr_connection(int clientfd) {
|
||||
struct qlistnode *con_node;
|
||||
int find = 0;
|
||||
|
||||
qlist_for_each(con_node, &qrtr_proxy_connection) {
|
||||
QRTR_PROXY_CONNECTION *qrtr_con = qnode_to_item(con_node, QRTR_PROXY_CONNECTION, qnode);
|
||||
|
||||
if (qrtr_con->ClientFd == clientfd) {
|
||||
while (!qlist_empty(&qrtr_con->client_qnode)) {
|
||||
QRTR_PROXY_CLINET *qrtr_client = qnode_to_item(qlist_head(&qrtr_con->client_qnode), QRTR_PROXY_CLINET, qnode);
|
||||
|
||||
release_qrtr_client_id(qrtr_con, qrtr_client->QMIType, qrtr_client->ClientId);
|
||||
}
|
||||
|
||||
dprintf("--- ClientFd=%d\n", qrtr_con->ClientFd);
|
||||
close(qrtr_con->ClientFd);
|
||||
qlist_remove(&qrtr_con->qnode);
|
||||
free(qrtr_con);
|
||||
find = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!find) {
|
||||
dprintf("NOT find on %s(ClientFd=%d)\n", __func__, clientfd);
|
||||
}
|
||||
}
|
||||
|
||||
static void recv_qrtr_from_dev(struct qrtr_hdr_v1 *hdr) {
|
||||
int find = 0;
|
||||
uint32_t type = le32toh(hdr->type);
|
||||
|
||||
if (type == QRTR_TYPE_HELLO) {
|
||||
send_ctrl_hello(le32toh(hdr->src_node_id), le32toh(hdr->src_port_id));
|
||||
find++;
|
||||
}
|
||||
else if (type == QRTR_TYPE_NEW_SERVER || type == QRTR_TYPE_DEL_SERVER) {
|
||||
handle_server_change(hdr);
|
||||
find++;
|
||||
}
|
||||
else if (type == QRTR_TYPE_DATA) {
|
||||
struct qlistnode *con_node, *client_node;
|
||||
|
||||
qlist_for_each(con_node, &qrtr_proxy_connection) {
|
||||
QRTR_PROXY_CONNECTION *qrtr_con = qnode_to_item(con_node, QRTR_PROXY_CONNECTION, qnode);
|
||||
|
||||
qlist_for_each(client_node, &qrtr_con->client_qnode) {
|
||||
QRTR_PROXY_CLINET *qrtr_client = qnode_to_item(client_node, QRTR_PROXY_CLINET, qnode);
|
||||
|
||||
if (qrtr_client->node_id == le32toh(hdr->dst_node_id) && qrtr_client->port_id == le32toh(hdr->dst_port_id)) {
|
||||
PQCQMIMSG pQMI = (PQCQMIMSG)malloc(hdr->size + sizeof(QCQMI_HDR));
|
||||
|
||||
if (pQMI) {
|
||||
pQMI->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI;
|
||||
pQMI->QMIHdr.Length = htole16(hdr->size + sizeof(QCQMI_HDR) - 1);
|
||||
pQMI->QMIHdr.CtlFlags = 0x00;
|
||||
pQMI->QMIHdr.QMIType = qrtr_client->QMIType;
|
||||
pQMI->QMIHdr.ClientId = qrtr_client->ClientId;
|
||||
memcpy(&pQMI->MUXMsg, hdr + 1, hdr->size);
|
||||
send_qmi_to_client(pQMI, qrtr_con->ClientFd);
|
||||
free(pQMI);
|
||||
find++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (hdr->confirm_rx) {
|
||||
struct qrtr_ctrl_pkt pkt;
|
||||
struct sockaddr_qrtr from = {AF_QIPCRTR, le32toh(hdr->dst_node_id), le32toh(hdr->dst_port_id)};
|
||||
struct sockaddr_qrtr to = {AF_QIPCRTR, le32toh(hdr->src_node_id), le32toh(hdr->src_port_id)};
|
||||
|
||||
memset(&pkt, 0, sizeof(pkt));
|
||||
pkt.cmd = htole32(QRTR_TYPE_RESUME_TX);
|
||||
pkt.client.node = hdr->dst_node_id;
|
||||
pkt.client.port = hdr->dst_port_id;
|
||||
|
||||
qrtr_node_enqueue(&pkt, sizeof(pkt), QRTR_TYPE_RESUME_TX, &from, &to, 0);
|
||||
}
|
||||
}
|
||||
else if (type == QRTR_TYPE_RESUME_TX) {
|
||||
}
|
||||
|
||||
if (!find) {
|
||||
dprintf("NOT find on %s()\n", __func__);
|
||||
}
|
||||
}
|
||||
|
||||
static int recv_qmi_from_client(PQCQMIMSG pQMI, int clientfd) {
|
||||
QRTR_PROXY_CONNECTION *qrtr_con;
|
||||
struct qlistnode *con_node, *client_node;
|
||||
int find = 0;
|
||||
|
||||
qlist_for_each(con_node, &qrtr_proxy_connection) {
|
||||
qrtr_con = qnode_to_item(con_node, QRTR_PROXY_CONNECTION, qnode);
|
||||
if (qrtr_con->ClientFd == clientfd)
|
||||
break;
|
||||
qrtr_con = NULL;
|
||||
}
|
||||
|
||||
if (!qrtr_con) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (le16toh(pQMI->QMIHdr.QMIType) == QMUX_TYPE_CTL) {
|
||||
if (pQMI->CTLMsg.QMICTLMsgHdr.QMICTLType == QMICTL_SYNC_REQ) {
|
||||
dprintf("do not allow client send QMICTL_SYNC_REQ\n");
|
||||
return 0;
|
||||
}
|
||||
else if (le16toh(pQMI->CTLMsg.QMICTLMsgHdr.QMICTLType) == QMICTL_GET_CLIENT_ID_REQ) {
|
||||
uint8_t QMIType = pQMI->CTLMsg.GetClientIdReq.QMIType;
|
||||
PQCQMIMSG pRsp = (PQCQMIMSG)malloc(256);
|
||||
|
||||
if (pRsp) {
|
||||
uint8_t ClientId = 0;
|
||||
|
||||
if (find_qrtr_service(QMIType)) {
|
||||
ClientId = alloc_qrtr_client_id(qrtr_con, QMIType);
|
||||
}
|
||||
|
||||
pRsp->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI;
|
||||
pRsp->QMIHdr.Length = htole16(sizeof(pRsp->CTLMsg.GetClientIdRsp) + sizeof(pRsp->QMIHdr) - 1);
|
||||
pRsp->QMIHdr.CtlFlags = 0x00;
|
||||
pRsp->QMIHdr.QMIType = QMUX_TYPE_CTL;
|
||||
pRsp->QMIHdr.ClientId = 0;
|
||||
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.CtlFlags = QMICTL_FLAG_RESPONSE;
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.TransactionId = pQMI->CTLMsg.QMICTLMsgHdr.TransactionId;
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.QMICTLType = pQMI->CTLMsg.QMICTLMsgHdr.QMICTLType;
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.Length = htole16(sizeof(pRsp->CTLMsg.GetClientIdRsp) - sizeof(pRsp->CTLMsg.QMICTLMsgHdr));
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.TLVType = QCTLV_TYPE_RESULT_CODE;
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.TLVLength = htole16(4);
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXResult = htole16(ClientId ? 0 : QMI_RESULT_FAILURE);
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXError = htole16(ClientId ? 0 : QMI_ERR_INTERNAL);
|
||||
pRsp->CTLMsg.GetClientIdRsp.TLV2Type = QCTLV_TYPE_REQUIRED_PARAMETER;
|
||||
pRsp->CTLMsg.GetClientIdRsp.TLV2Length = htole16(2);
|
||||
pRsp->CTLMsg.GetClientIdRsp.QMIType = QMIType;
|
||||
pRsp->CTLMsg.GetClientIdRsp.ClientId = ClientId;
|
||||
|
||||
send_qmi_to_client(pRsp, clientfd);
|
||||
free(pRsp);
|
||||
find++;
|
||||
}
|
||||
}
|
||||
else if (le16toh(pQMI->CTLMsg.QMICTLMsgHdr.QMICTLType) == QMICTL_RELEASE_CLIENT_ID_REQ) {
|
||||
PQCQMIMSG pRsp = (PQCQMIMSG)malloc(256);
|
||||
release_qrtr_client_id(qrtr_con, pQMI->CTLMsg.ReleaseClientIdReq.QMIType, pQMI->CTLMsg.ReleaseClientIdReq.ClientId);
|
||||
|
||||
if (pRsp) {
|
||||
pRsp->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI;
|
||||
pRsp->QMIHdr.Length = htole16(sizeof(pRsp->CTLMsg.ReleaseClientIdRsp) + sizeof(pRsp->QMIHdr) - 1);
|
||||
pRsp->QMIHdr.CtlFlags = 0x00;
|
||||
pRsp->QMIHdr.QMIType = QMUX_TYPE_CTL;
|
||||
pRsp->QMIHdr.ClientId = 0;
|
||||
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.CtlFlags = QMICTL_FLAG_RESPONSE;
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.TransactionId = pQMI->CTLMsg.QMICTLMsgHdr.TransactionId;
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.QMICTLType = pQMI->CTLMsg.QMICTLMsgHdr.QMICTLType;
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.Length = htole16(sizeof(pRsp->CTLMsg.ReleaseClientIdRsp) - sizeof(pRsp->CTLMsg.QMICTLMsgHdr));
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.TLVType = QCTLV_TYPE_RESULT_CODE;
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.TLVLength = htole16(4);
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXResult = htole16(0);
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXError = htole16(0);
|
||||
pRsp->CTLMsg.ReleaseClientIdRsp.TLV2Type = QCTLV_TYPE_REQUIRED_PARAMETER;
|
||||
pRsp->CTLMsg.ReleaseClientIdRsp.TLV2Length = htole16(2);
|
||||
pRsp->CTLMsg.ReleaseClientIdRsp.QMIType = pQMI->CTLMsg.ReleaseClientIdReq.QMIType;
|
||||
pRsp->CTLMsg.ReleaseClientIdRsp.ClientId = pQMI->CTLMsg.ReleaseClientIdReq.ClientId;
|
||||
|
||||
send_qmi_to_client(pRsp, clientfd);
|
||||
free(pRsp);
|
||||
find++;
|
||||
}
|
||||
}
|
||||
else if (le16toh(pQMI->CTLMsg.QMICTLMsgHdr.QMICTLType) == QMICTL_GET_VERSION_REQ) {
|
||||
PQCQMIMSG pRsp = (PQCQMIMSG)malloc(256);
|
||||
|
||||
if (pRsp) {
|
||||
pRsp->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI;
|
||||
pRsp->QMIHdr.Length = htole16(sizeof(pRsp->CTLMsg.GetVersionRsp) + sizeof(pRsp->QMIHdr) - 1);
|
||||
pRsp->QMIHdr.CtlFlags = 0x00;
|
||||
pRsp->QMIHdr.QMIType = QMUX_TYPE_CTL;
|
||||
pRsp->QMIHdr.ClientId = 0;
|
||||
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.CtlFlags = QMICTL_FLAG_RESPONSE;
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.TransactionId = pQMI->CTLMsg.QMICTLMsgHdr.TransactionId;
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.QMICTLType = pQMI->CTLMsg.QMICTLMsgHdr.QMICTLType;
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.Length = htole16(sizeof(pRsp->CTLMsg.GetVersionRsp) - sizeof(pRsp->CTLMsg.QMICTLMsgHdr));
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.TLVType = QCTLV_TYPE_RESULT_CODE;
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.TLVLength = htole16(4);
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXResult = htole16(0);
|
||||
pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXError = htole16(0);
|
||||
pRsp->CTLMsg.GetVersionRsp.TLV2Type = QCTLV_TYPE_REQUIRED_PARAMETER;
|
||||
pRsp->CTLMsg.GetVersionRsp.TLV2Length = htole16(1);
|
||||
pRsp->CTLMsg.GetVersionRsp.NumElements = 0;
|
||||
|
||||
send_qmi_to_client(pRsp, clientfd);
|
||||
free(pRsp);
|
||||
find++;
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
qlist_for_each (client_node, &qrtr_con->client_qnode) {
|
||||
QRTR_PROXY_CLINET *qrtr_client = qnode_to_item(client_node, QRTR_PROXY_CLINET, qnode);
|
||||
|
||||
if (pQMI->QMIHdr.QMIType == qrtr_client->QMIType && pQMI->QMIHdr.ClientId == qrtr_client->ClientId) {
|
||||
QRTR_SERVICE *srv = find_qrtr_service(pQMI->QMIHdr.QMIType);
|
||||
|
||||
if (srv && srv->service) {
|
||||
struct sockaddr_qrtr from = {AF_QIPCRTR, qrtr_client->node_id, qrtr_client->port_id};
|
||||
struct sockaddr_qrtr to = {AF_QIPCRTR, srv->node, srv->port};
|
||||
|
||||
qrtr_node_enqueue(&pQMI->MUXMsg, le16toh(pQMI->QMIHdr.Length) + 1 - sizeof(QCQMI_HDR),
|
||||
QRTR_TYPE_DATA, &from, &to, 0);
|
||||
find++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!find) {
|
||||
dprintf("NOT find on %s()\n", __func__);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int qrtr_proxy_init(void) {
|
||||
unsigned i;
|
||||
int qrtr_sync_done = 0;
|
||||
|
||||
dprintf("%s enter\n", __func__);
|
||||
send_ctrl_hello(QRTR_NODE_BCAST, QRTR_PORT_CTRL);
|
||||
|
||||
for (i = 0; i < 10; i++) {
|
||||
sleep(1);
|
||||
qrtr_sync_done = !qlist_empty(&qrtr_server_list);
|
||||
if (qrtr_sync_done)
|
||||
break;
|
||||
}
|
||||
|
||||
dprintf("%s %s\n", __func__, qrtr_sync_done ? "succful" : "fail");
|
||||
return qrtr_sync_done ? 0 : -1;
|
||||
}
|
||||
|
||||
static void qrtr_start_server(const char* servername) {
|
||||
qrtr_proxy_server_fd = create_local_server(servername);
|
||||
dprintf("qrtr_proxy_server_fd = %d\n", qrtr_proxy_server_fd);
|
||||
if (qrtr_proxy_server_fd == -1) {
|
||||
dprintf("Failed to create %s, errno: %d (%s)\n", servername, errno, strerror(errno));
|
||||
}
|
||||
}
|
||||
|
||||
static void qrtr_close_server(const char* servername) {
|
||||
if (qrtr_proxy_server_fd != -1) {
|
||||
dprintf("%s %s\n", __func__, servername);
|
||||
close(qrtr_proxy_server_fd);
|
||||
qrtr_proxy_server_fd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
static void *qrtr_proxy_loop(void *param)
|
||||
{
|
||||
void *rx_buf;
|
||||
struct qlistnode *con_node;
|
||||
QRTR_PROXY_CONNECTION *qrtr_con;
|
||||
|
||||
(void)param;
|
||||
dprintf("%s enter thread_id %p\n", __func__, (void *)pthread_self());
|
||||
|
||||
rx_buf = malloc(8192);
|
||||
if (!rx_buf)
|
||||
return NULL;
|
||||
|
||||
while (cdc_wdm_fd > 0 && qrtr_proxy_quit == 0) {
|
||||
struct pollfd pollfds[32];
|
||||
int ne, ret, nevents = 0;
|
||||
ssize_t nreads;
|
||||
|
||||
pollfds[nevents].fd = cdc_wdm_fd;
|
||||
pollfds[nevents].events = POLLIN;
|
||||
pollfds[nevents].revents= 0;
|
||||
nevents++;
|
||||
|
||||
if (qrtr_proxy_server_fd > 0) {
|
||||
pollfds[nevents].fd = qrtr_proxy_server_fd;
|
||||
pollfds[nevents].events = POLLIN;
|
||||
pollfds[nevents].revents= 0;
|
||||
nevents++;
|
||||
}
|
||||
|
||||
qlist_for_each(con_node, &qrtr_proxy_connection) {
|
||||
qrtr_con = qnode_to_item(con_node, QRTR_PROXY_CONNECTION, qnode);
|
||||
|
||||
pollfds[nevents].fd = qrtr_con->ClientFd;
|
||||
pollfds[nevents].events = POLLIN;
|
||||
pollfds[nevents].revents= 0;
|
||||
nevents++;
|
||||
|
||||
if (nevents == (sizeof(pollfds)/sizeof(pollfds[0])))
|
||||
break;
|
||||
}
|
||||
|
||||
do {
|
||||
//ret = poll(pollfds, nevents, -1);
|
||||
ret = poll(pollfds, nevents, (qrtr_proxy_server_fd > 0) ? -1 : 200);
|
||||
} while (ret == -1 && errno == EINTR && qrtr_proxy_quit == 0);
|
||||
|
||||
if (ret < 0) {
|
||||
dprintf("%s poll=%d, errno: %d (%s)\n", __func__, ret, errno, strerror(errno));
|
||||
goto qrtr_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 qrtr_proxy_loop_exit;
|
||||
}
|
||||
else if (fd == qrtr_proxy_server_fd) {
|
||||
|
||||
}
|
||||
else {
|
||||
cleanup_qrtr_connection(fd);
|
||||
}
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!(pollfds[ne].revents & POLLIN)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (fd == qrtr_proxy_server_fd) {
|
||||
accept_qrtr_connection(fd);
|
||||
}
|
||||
else if (fd == cdc_wdm_fd) {
|
||||
struct qrtr_hdr_v1 *hdr = (struct qrtr_hdr_v1 *)rx_buf;
|
||||
|
||||
nreads = read(fd, rx_buf, 8192);
|
||||
if (nreads <= 0) {
|
||||
dprintf("%s read=%d errno: %d (%s)\n", __func__, (int)nreads, errno, strerror(errno));
|
||||
goto qrtr_proxy_loop_exit;
|
||||
}
|
||||
else if (nreads != (int)align_4(le32toh(hdr->size) + sizeof(*hdr))) {
|
||||
dprintf("%s nreads=%d, hdr->size = %d\n", __func__, (int)nreads, le32toh(hdr->size));
|
||||
continue;
|
||||
}
|
||||
|
||||
dump_qrtr(hdr, nreads, '<');
|
||||
recv_qrtr_from_dev(hdr);
|
||||
}
|
||||
else {
|
||||
PQCQMIMSG pQMI = (PQCQMIMSG)rx_buf;
|
||||
|
||||
nreads = read(fd, rx_buf, 8192);
|
||||
if (nreads <= 0) {
|
||||
dprintf("%s read=%d errno: %d (%s)", __func__, (int)nreads, errno, strerror(errno));
|
||||
cleanup_qrtr_connection(fd);
|
||||
break;
|
||||
}
|
||||
else 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_from_client(pQMI, fd);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
qrtr_proxy_loop_exit:
|
||||
while (!qlist_empty(&qrtr_proxy_connection)) {
|
||||
QRTR_PROXY_CONNECTION *qrtr_con = qnode_to_item(qlist_head(&qrtr_proxy_connection), QRTR_PROXY_CONNECTION, qnode);
|
||||
|
||||
cleanup_qrtr_connection(qrtr_con->ClientFd);
|
||||
}
|
||||
|
||||
dprintf("%s exit, thread_id %p\n", __func__, (void *)pthread_self());
|
||||
free(rx_buf);
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static void usage(void) {
|
||||
dprintf(" -d <device_name> A valid qrtr device\n"
|
||||
" default /dev/mhi_IPCR, but mhi_IPCR may be invalid\n"
|
||||
" -i <netcard_name> netcard name\n"
|
||||
" -v Will show all details\n");
|
||||
}
|
||||
|
||||
static void sig_action(int sig) {
|
||||
if (qrtr_proxy_quit == 0) {
|
||||
qrtr_proxy_quit = 1;
|
||||
if (thread_id)
|
||||
pthread_kill(thread_id, sig);
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
int opt;
|
||||
char cdc_wdm[32+1] = "/dev/mhi_IPCR";
|
||||
char servername[64] = {0};
|
||||
|
||||
signal(SIGINT, sig_action);
|
||||
signal(SIGTERM, sig_action);
|
||||
|
||||
optind = 1;
|
||||
while ( -1 != (opt = getopt(argc, argv, "d:i:vh"))) {
|
||||
switch (opt) {
|
||||
case 'd':
|
||||
strcpy(cdc_wdm, optarg);
|
||||
break;
|
||||
case 'v':
|
||||
verbose_debug = 1;
|
||||
break;
|
||||
default:
|
||||
usage();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
sprintf(servername, "quectel-qrtr-proxy%c", cdc_wdm[strlen(cdc_wdm)-1]);
|
||||
dprintf("Will use cdc-wdm='%s', proxy='%s'\n", cdc_wdm, servername);
|
||||
|
||||
while (qrtr_proxy_quit == 0) {
|
||||
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)\n", cdc_wdm, errno, strerror(errno));
|
||||
sleep(5);
|
||||
continue;
|
||||
}
|
||||
cfmakenoblock(cdc_wdm_fd);
|
||||
qlist_init(&qrtr_proxy_connection);
|
||||
qlist_init(&qrtr_server_list);
|
||||
pthread_create(&thread_id, NULL, qrtr_proxy_loop, NULL);
|
||||
|
||||
if (qrtr_proxy_init() == 0) {
|
||||
qrtr_start_server(servername);
|
||||
pthread_join(thread_id, NULL);
|
||||
qrtr_close_server(servername);
|
||||
}
|
||||
else {
|
||||
pthread_cancel(thread_id);
|
||||
pthread_join(thread_id, NULL);
|
||||
}
|
||||
|
||||
close(cdc_wdm_fd);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user