init
This commit is contained in:
zhongluofeng
@@ -0,0 +1,502 @@
|
||||
/*
|
||||
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 <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <ctype.h>
|
||||
#include <fcntl.h>
|
||||
#include <stddef.h>
|
||||
#include <pthread.h>
|
||||
#include <errno.h>
|
||||
#include <time.h>
|
||||
#include <signal.h>
|
||||
#include <getopt.h>
|
||||
#include <poll.h>
|
||||
#include <sys/time.h>
|
||||
#include <endian.h>
|
||||
#include <time.h>
|
||||
#include <sys/types.h>
|
||||
#include <limits.h>
|
||||
#include <inttypes.h>
|
||||
#include <sys/socket.h>
|
||||
#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 <endian.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
#include "qlist.h"
|
||||
#include "QMIThread.h"
|
||||
#include "atchannel.h"
|
||||
#include "at_tok.h"
|
||||
|
||||
#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 safe_free(__x) do { if (__x) { free((void *)__x); __x = NULL;}} while(0)
|
||||
#define safe_at_response_free(__x) { if (__x) { at_response_free(__x); __x = NULL;}}
|
||||
|
||||
#define at_response_error(err, p_response) \
|
||||
(err \
|
||||
|| p_response == NULL \
|
||||
|| p_response->finalResponse == NULL \
|
||||
|| p_response->success == 0)
|
||||
|
||||
typedef struct {
|
||||
struct qlistnode qnode;
|
||||
int ClientFd;
|
||||
unsigned AccessTime;
|
||||
} ATC_PROXY_CONNECTION;
|
||||
|
||||
static int atc_proxy_quit = 0;
|
||||
static pthread_t thread_id = 0;
|
||||
static int atc_dev_fd = -1;
|
||||
static int atc_proxy_server_fd = -1;
|
||||
static struct qlistnode atc_proxy_connection;
|
||||
static int verbose_debug = 0;
|
||||
static int modem_reset_flag = 0;
|
||||
static uint8_t atc_buf[4096];
|
||||
static int asr_style_atc = 0;
|
||||
extern int asprintf(char **s, const char *fmt, ...);
|
||||
static ATC_PROXY_CONNECTION *current_client_fd = NULL;
|
||||
|
||||
static void dump_atc(uint8_t *pATC, int fd,int size, const char flag)
|
||||
{
|
||||
if (verbose_debug) {
|
||||
printf("%c %d:\n", flag, fd);
|
||||
printf("%.*s\n", size, pATC);
|
||||
}
|
||||
}
|
||||
|
||||
static int send_atc_to_client(int clientFd, uint8_t *pATC, int size) {
|
||||
struct pollfd pollfds[]= {{clientFd, POLLOUT, 0}};
|
||||
ssize_t ret = 0;
|
||||
|
||||
do {
|
||||
ret = poll(pollfds, sizeof(pollfds)/sizeof(pollfds[0]), 5000);
|
||||
} while (ret == -1 && errno == EINTR && atc_proxy_quit == 0);
|
||||
|
||||
if (pollfds[0].revents & POLLOUT) {
|
||||
ret = write(clientFd, pATC, size);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static void onUnsolicited (const char *s, const char *sms_pdu)
|
||||
{
|
||||
struct qlistnode *con_node;
|
||||
int ret;
|
||||
char buf[1024];
|
||||
|
||||
if(s) {
|
||||
strcpy(buf, s);
|
||||
strcat(buf, "\r\n");
|
||||
}
|
||||
if(sms_pdu) {
|
||||
strcat(buf, sms_pdu);
|
||||
strcat(buf, "\r\n");
|
||||
}
|
||||
|
||||
if(current_client_fd) {
|
||||
ATC_PROXY_CONNECTION *atc_con = current_client_fd;
|
||||
ret = send_atc_to_client(atc_con->ClientFd, (uint8_t *)buf, strlen(buf));
|
||||
if(ret < 0) {
|
||||
close(atc_con->ClientFd);
|
||||
qlist_remove(&atc_con->qnode);
|
||||
free(atc_con);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
qlist_for_each(con_node, &atc_proxy_connection) {
|
||||
ATC_PROXY_CONNECTION *atc_con = qnode_to_item(con_node, ATC_PROXY_CONNECTION, qnode);
|
||||
if(atc_con && atc_con->ClientFd > 0) {
|
||||
ret = send_atc_to_client(atc_con->ClientFd, (uint8_t *)buf, strlen(buf));
|
||||
if(ret < 0) {
|
||||
close(atc_con->ClientFd);
|
||||
con_node = con_node->prev;
|
||||
qlist_remove(&atc_con->qnode);
|
||||
free(atc_con);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void onTimeout(void) {
|
||||
dprintf("%s", __func__);
|
||||
//TODO
|
||||
}
|
||||
|
||||
static void onClose(void) {
|
||||
dprintf("%s", __func__);
|
||||
}
|
||||
|
||||
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 void accept_atc_connection(int serverfd) {
|
||||
int clientfd = -1;
|
||||
unsigned char addr[128];
|
||||
socklen_t alen = sizeof(addr);
|
||||
ATC_PROXY_CONNECTION *atc_con;
|
||||
|
||||
clientfd = accept(serverfd, (struct sockaddr *)addr, &alen);
|
||||
|
||||
atc_con = (ATC_PROXY_CONNECTION *)malloc(sizeof(ATC_PROXY_CONNECTION));
|
||||
if (atc_con) {
|
||||
qlist_init(&atc_con->qnode);
|
||||
atc_con->ClientFd= clientfd;
|
||||
atc_con->AccessTime = 0;
|
||||
dprintf("+++ ClientFd=%d\n", atc_con->ClientFd);
|
||||
qlist_add_tail(&atc_proxy_connection, &atc_con->qnode);
|
||||
}
|
||||
|
||||
cfmakenoblock(clientfd);
|
||||
}
|
||||
|
||||
static void cleanup_atc_connection(int clientfd) {
|
||||
struct qlistnode *con_node;
|
||||
|
||||
qlist_for_each(con_node, &atc_proxy_connection) {
|
||||
ATC_PROXY_CONNECTION *atc_con = qnode_to_item(con_node, ATC_PROXY_CONNECTION, qnode);
|
||||
if (atc_con->ClientFd == clientfd) {
|
||||
dprintf("--- ClientFd=%d\n", atc_con->ClientFd);
|
||||
close(atc_con->ClientFd);
|
||||
qlist_remove(&atc_con->qnode);
|
||||
free(atc_con);
|
||||
if (current_client_fd == atc_con)
|
||||
current_client_fd = NULL;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int atc_proxy_init(void) {
|
||||
int err;
|
||||
char *cmd;
|
||||
ATResponse *p_response = NULL;
|
||||
|
||||
err = at_handshake();
|
||||
if (err) {
|
||||
dprintf("handshake fail, TODO ... ");
|
||||
goto exit;
|
||||
}
|
||||
|
||||
at_send_command_singleline("AT+QCFG=\"usbnet\"", "+QCFG:", NULL);
|
||||
at_send_command_multiline("AT+QNETDEVCTL=?", "+QNETDEVCTL:", NULL);
|
||||
at_send_command("AT+CGREG=2", NULL); //GPRS Network Registration Status
|
||||
at_send_command("AT+CEREG=2", NULL); //EPS Network Registration Status
|
||||
at_send_command("AT+C5GREG=2", NULL); //5GS Network Registration Status
|
||||
|
||||
at_send_command_singleline("AT+QNETDEVSTATUS=?", "+QNETDEVSTATUS:", &p_response);
|
||||
if (at_response_error(err, p_response))
|
||||
asr_style_atc = 1; //EC200T/EC100Y do not support this AT, but RG801/RG500U support
|
||||
|
||||
safe_at_response_free(p_response);
|
||||
|
||||
err = at_send_command_singleline("AT+QCFG=\"NAT\"", "+QCFG:", &p_response);
|
||||
if (!at_response_error(err, p_response)) {
|
||||
int old_nat, new_nat = asr_style_atc ? 1 : 0;
|
||||
|
||||
err = at_tok_scanf(p_response->p_intermediates->line, "%s%d", NULL, &old_nat);
|
||||
if (err == 2 && old_nat != new_nat) {
|
||||
safe_at_response_free(p_response);
|
||||
asprintf(&cmd, "AT+QCFG=\"NAT\",%d", new_nat);
|
||||
err = at_send_command(cmd, &p_response);
|
||||
safe_free(cmd);
|
||||
if (!at_response_error(err, p_response)) {
|
||||
err = at_send_command("at+cfun=1,1",NULL);
|
||||
}
|
||||
safe_at_response_free(p_response);
|
||||
}
|
||||
err = 0;
|
||||
}
|
||||
safe_at_response_free(p_response);
|
||||
|
||||
exit:
|
||||
return err;
|
||||
}
|
||||
|
||||
static void atc_start_server(const char* servername) {
|
||||
atc_proxy_server_fd = create_local_server(servername);
|
||||
dprintf("atc_proxy_server_fd = %d\n", atc_proxy_server_fd);
|
||||
if (atc_proxy_server_fd == -1) {
|
||||
dprintf("Failed to create %s, errno: %d (%s)\n", servername, errno, strerror(errno));
|
||||
}
|
||||
}
|
||||
|
||||
static void atc_close_server(const char* servername) {
|
||||
if (atc_proxy_server_fd != -1) {
|
||||
dprintf("%s %s close server\n", __func__, servername);
|
||||
close(atc_proxy_server_fd);
|
||||
atc_proxy_server_fd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
static void *atc_proxy_loop(void *param)
|
||||
{
|
||||
uint8_t *pATC = atc_buf;
|
||||
struct qlistnode *con_node;
|
||||
ATC_PROXY_CONNECTION *atc_con;
|
||||
|
||||
(void)param;
|
||||
dprintf("%s enter thread_id %p\n", __func__, (void *)pthread_self());
|
||||
|
||||
qlist_init(&atc_proxy_connection);
|
||||
while (atc_dev_fd > 0 && atc_proxy_quit == 0) {
|
||||
struct pollfd pollfds[2+64];
|
||||
int ne, ret, nevents = 0;
|
||||
ssize_t nreads;
|
||||
|
||||
pollfds[nevents].fd = atc_dev_fd;
|
||||
pollfds[nevents].events = POLLIN;
|
||||
pollfds[nevents].revents= 0;
|
||||
nevents++;
|
||||
|
||||
if (atc_proxy_server_fd > 0) {
|
||||
pollfds[nevents].fd = atc_proxy_server_fd;
|
||||
pollfds[nevents].events = POLLIN;
|
||||
pollfds[nevents].revents= 0;
|
||||
nevents++;
|
||||
}
|
||||
|
||||
qlist_for_each(con_node, &atc_proxy_connection) {
|
||||
atc_con = qnode_to_item(con_node, ATC_PROXY_CONNECTION, qnode);
|
||||
|
||||
pollfds[nevents].fd = atc_con->ClientFd;
|
||||
pollfds[nevents].events = POLLIN;
|
||||
pollfds[nevents].revents= 0;
|
||||
nevents++;
|
||||
|
||||
if (nevents == (sizeof(pollfds)/sizeof(pollfds[0])))
|
||||
break;
|
||||
}
|
||||
|
||||
do {
|
||||
ret = poll(pollfds, nevents, (atc_proxy_server_fd > 0) ? -1 : 200);
|
||||
} while (ret == -1 && errno == EINTR && atc_proxy_quit == 0);
|
||||
|
||||
if (ret < 0) {
|
||||
dprintf("%s poll=%d, errno: %d (%s)\n", __func__, ret, errno, strerror(errno));
|
||||
goto atc_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 == atc_dev_fd) {
|
||||
goto atc_proxy_loop_exit;
|
||||
} else if(fd == atc_proxy_server_fd) {
|
||||
|
||||
} else {
|
||||
cleanup_atc_connection(fd);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!(pollfds[ne].revents & POLLIN)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (fd == atc_proxy_server_fd) {
|
||||
accept_atc_connection(fd);
|
||||
}
|
||||
else if (fd == atc_dev_fd) {
|
||||
usleep(10*1000); //let atchannel.c read at response.
|
||||
if (modem_reset_flag)
|
||||
goto atc_proxy_loop_exit;
|
||||
}
|
||||
else {
|
||||
memset(atc_buf, 0x0, sizeof(atc_buf));
|
||||
nreads = read(fd, pATC, sizeof(atc_buf));
|
||||
if (nreads <= 0) {
|
||||
dprintf("%s read=%d errno: %d (%s)", __func__, (int)nreads, errno, strerror(errno));
|
||||
cleanup_atc_connection(fd);
|
||||
break;
|
||||
}
|
||||
|
||||
dump_atc(pATC, fd, nreads, 'r');
|
||||
qlist_for_each(con_node, &atc_proxy_connection) {
|
||||
atc_con = qnode_to_item(con_node, ATC_PROXY_CONNECTION, qnode);
|
||||
if (atc_con->ClientFd == pollfds[nevents].fd) {
|
||||
current_client_fd = atc_con;
|
||||
break;
|
||||
}
|
||||
}
|
||||
at_send_command ((const char *)pATC, NULL);
|
||||
current_client_fd = NULL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
atc_proxy_loop_exit:
|
||||
at_close();
|
||||
while (!qlist_empty(&atc_proxy_connection)) {
|
||||
ATC_PROXY_CONNECTION *atc_con = qnode_to_item(qlist_head(&atc_proxy_connection), ATC_PROXY_CONNECTION, qnode);
|
||||
cleanup_atc_connection(atc_con->ClientFd);
|
||||
}
|
||||
dprintf("%s exit, thread_id %p\n", __func__, (void *)pthread_self());
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static void usage(void) {
|
||||
dprintf(" -d <device_name> A valid atc device\n"
|
||||
" default /dev/ttyUSB2, but /dev/ttyUSB2 may be invalid\n"
|
||||
" -i <netcard_name> netcard name\n"
|
||||
" -v Will show all details\n");
|
||||
}
|
||||
|
||||
static void sig_action(int sig) {
|
||||
if (atc_proxy_quit == 0) {
|
||||
atc_proxy_quit = 1;
|
||||
if (thread_id)
|
||||
pthread_kill(thread_id, sig);
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
int opt;
|
||||
char atc_dev[32+1] = "/dev/ttyUSB2";
|
||||
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(atc_dev, optarg);
|
||||
break;
|
||||
case 'v':
|
||||
verbose_debug = 1;
|
||||
break;
|
||||
default:
|
||||
usage();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (access(atc_dev, R_OK | W_OK)) {
|
||||
dprintf("Fail to access %s, errno: %d (%s). break\n", atc_dev, errno, strerror(errno));
|
||||
return -1;
|
||||
}
|
||||
|
||||
sprintf(servername, "quectel-atc-proxy%c", atc_dev[strlen(atc_dev) - 1]);
|
||||
dprintf("Will use atc-dev='%s', proxy='%s'\n", atc_dev, servername);
|
||||
|
||||
while (atc_proxy_quit == 0) {
|
||||
if (access(atc_dev, R_OK | W_OK)) {
|
||||
dprintf("Fail to access %s, errno: %d (%s). continue\n", atc_dev, errno, strerror(errno));
|
||||
// wait device
|
||||
sleep(3);
|
||||
continue;
|
||||
}
|
||||
|
||||
atc_dev_fd = open(atc_dev, O_RDWR | O_NONBLOCK | O_NOCTTY);
|
||||
if (atc_dev_fd == -1) {
|
||||
dprintf("Failed to open %s, errno: %d (%s). break\n", atc_dev, errno, strerror(errno));
|
||||
return -1;
|
||||
}
|
||||
cfmakenoblock(atc_dev_fd);
|
||||
if (at_open(atc_dev_fd, onUnsolicited, 1)) {
|
||||
close(atc_dev_fd);
|
||||
atc_dev_fd = -1;
|
||||
}
|
||||
at_set_on_timeout(onTimeout);
|
||||
at_set_on_reader_closed(onClose);
|
||||
|
||||
/* no atc_proxy_loop lives, create one */
|
||||
pthread_create(&thread_id, NULL, atc_proxy_loop, NULL);
|
||||
/* try to redo init if failed, init function must be successfully */
|
||||
while (atc_proxy_init() != 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(atc_dev, F_OK|R_OK|W_OK))
|
||||
break;
|
||||
}
|
||||
retry_times = 0;
|
||||
atc_start_server(servername);
|
||||
if (atc_proxy_server_fd == -1)
|
||||
pthread_cancel(thread_id);
|
||||
pthread_join(thread_id, NULL);
|
||||
|
||||
/* close local server at last */
|
||||
atc_close_server(servername);
|
||||
close(atc_dev_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;
|
||||
}
|
||||
Reference in New Issue
Block a user