op-packages/quectel-cm/src/qrtr.c

/*
    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 <stdio.h>
#include <string.h>
#include <termios.h>
#include <stdio.h>
#include <ctype.h>
#include "QMIThread.h"

typedef struct {
    uint32_t service;
    uint32_t version;
    uint32_t instance;
    uint32_t node;
    uint32_t port;
} QrtrService;

#define QRTR_MAX (QMUX_TYPE_WDS_ADMIN + 1)
static QrtrService service_list[QRTR_MAX];
static int qmiclientId[QRTR_MAX];
static int get_client(UCHAR QMIType);
static uint32_t node_modem = 3; //IPQ ~ 3, QCM ~ 0

#ifdef USE_LINUX_MSM_IPC
#include <linux/msm_ipc.h>

struct xport_ipc_router_server_addr {
        uint32_t service;
        uint32_t instance;
        uint32_t node_id;
        uint32_t port_id;
};

union ctl_msg {
        uint32_t cmd;
                struct {
                uint32_t cmd;
                        uint32_t service;
                        uint32_t instance;
                uint32_t node_id;
                uint32_t port_id;
        } srv;
                struct {
                uint32_t cmd;
                uint32_t node_id;
                uint32_t port_id;
        } cli;
        };
#define CTL_CMD_NEW_SERVER          4
#define CTL_CMD_REMOVE_SERVER       5

#define VERSION_MASK 0xff
#define GET_VERSION(x) (x & 0xff)
#define GET_XPORT_SVC_INSTANCE(x) GET_VERSION(x)
#define GET_INSTANCE(x) ((x & 0xff00) >> 8)

static int msm_ipc_socket(const char *name)
{
    int sock;
    int flags;

    sock = socket(AF_MSM_IPC, SOCK_DGRAM, 0);
    if (sock < 0) {
        dbg_time("%s(%s) errno: %d (%s)\n", __func__, name, errno, strerror(errno));
        return -1;
    }

    fcntl(sock, F_SETFD, FD_CLOEXEC);
    flags = fcntl(sock, F_GETFL, 0);
    fcntl(sock, F_SETFL, flags | O_NONBLOCK);

    return sock;
}

static uint32_t xport_lookup
(
 int lookup_sock_fd,
 uint32_t service_id,
 uint32_t version
)
{
  uint32_t num_servers_found = 0;
  uint32_t num_entries_to_fill = 4;
  struct server_lookup_args *lookup_arg;
  int i;

  lookup_arg = (struct server_lookup_args *)malloc(sizeof(*lookup_arg)
      + (num_entries_to_fill * sizeof(struct msm_ipc_server_info)));
  if (!lookup_arg)
  {
    dbg_time("%s: Malloc failed\n", __func__);
    return 0;
  }

  lookup_arg->port_name.service = service_id;
  lookup_arg->port_name.instance = GET_XPORT_SVC_INSTANCE(version);
  lookup_arg->num_entries_in_array = num_entries_to_fill;
  lookup_arg->lookup_mask =  VERSION_MASK;
  lookup_arg->num_entries_found = 0;
  if (ioctl(lookup_sock_fd, IPC_ROUTER_IOCTL_LOOKUP_SERVER, lookup_arg) < 0)
  {
    dbg_time("%s: Lookup failed for %08x: %08x\n", __func__, service_id, version);
    free(lookup_arg);
    return 0;
  }

  dbg_time("%s: num_entries_found %d for type=%d instance=%d", __func__,
    lookup_arg->num_entries_found, service_id, version);
  num_servers_found = 0;
  for (i = 0; ((i < (int)num_entries_to_fill) && (i < lookup_arg->num_entries_found)); i++)
  {
     QrtrService service_info[1];

    if (lookup_arg->srv_info[i].node_id != node_modem)
        continue;
    num_servers_found++;

    service_info[0].service = lookup_arg->srv_info[i].service;
    service_info[0].version = GET_VERSION(lookup_arg->srv_info[i].instance);
    service_info[0].instance = GET_INSTANCE(lookup_arg->srv_info[i].instance);
    service_info[0].node = lookup_arg->srv_info[i].node_id;
    service_info[0].port = lookup_arg->srv_info[i].port_id;

    service_list[service_id] = service_info[0];
    qmiclientId[service_id] = get_client(service_id);
  }

  free(lookup_arg);
  return num_servers_found;
}

static int xport_send(int sock, uint32_t node, uint32_t port, const void *data, unsigned int sz)
{
    struct sockaddr_msm_ipc addr = {};
    int rc;

    addr.family = AF_MSM_IPC;
    addr.address.addrtype = MSM_IPC_ADDR_ID;
    addr.address.addr.port_addr.node_id = node;
    addr.address.addr.port_addr.port_id = port;

    rc = sendto(sock, data, sz, MSG_DONTWAIT, (void *)&addr, sizeof(addr));
    if (rc < 0) {
        dbg_time("xport_send errno: %d (%s)\n", errno, strerror(errno));
        return -1;
    }

    return 0;
}

static int xport_recv(int sock, void *data, unsigned int sz, uint32_t *node, uint32_t *port)
{
    struct sockaddr_msm_ipc addr = {};
    socklen_t addr_size = sizeof(struct sockaddr_msm_ipc);
    int rc;

    rc = recvfrom(sock, data, sz, MSG_DONTWAIT, (void *)&addr, &addr_size);
    if (rc < 0) {
        dbg_time("xport_recv errno: %d (%s)\n", errno, strerror(errno));
    }
    else if (addr.address.addrtype != MSM_IPC_ADDR_ID) {
        dbg_time("xport_recv addrtype is NOT MSM_IPC_ADDR_ID\n");
        rc = -1;
    }

    *node = addr.address.addr.port_addr.node_id;
    *port = addr.address.addr.port_addr.port_id;
    return rc;
}
#define qmi_recv xport_recv

static int xport_ctrl_init(void)
{
    int ctrl_sock;
    int rc;
    uint32_t instance = 1; //modem
    uint32_t version;

    ctrl_sock = msm_ipc_socket("ctrl_port");
    if (ctrl_sock == -1)
        return -1;

    rc = ioctl(ctrl_sock, IPC_ROUTER_IOCTL_GET_VERSION, &version);
    if (rc < 0) {
        dbg_time("%s: failed to get ipc version\n", __func__);
        goto init_close_ctrl_fd;
    }
    dbg_time("%s ipc_version = %d", __func__, version);

    rc = ioctl(ctrl_sock, IPC_ROUTER_IOCTL_BIND_CONTROL_PORT, NULL);
    if (rc < 0) {
        dbg_time("%s: failed to bind as control port\n", __func__);
        goto init_close_ctrl_fd;
    }

    //cat /sys/kernel/debug/msm_ipc_router/dump_servers
    rc = 0;
    rc += xport_lookup(ctrl_sock, QMUX_TYPE_WDS, instance);
    if (service_list[QMUX_TYPE_WDS].port) {
        qmiclientId[QMUX_TYPE_WDS_IPV6] = get_client(QMUX_TYPE_WDS);
    }
    rc += xport_lookup(ctrl_sock, QMUX_TYPE_NAS, instance);
    rc += xport_lookup(ctrl_sock, QMUX_TYPE_UIM, instance);
    rc += xport_lookup(ctrl_sock, QMUX_TYPE_DMS, instance);
    rc += xport_lookup(ctrl_sock, QMUX_TYPE_WDS_ADMIN, instance);

    if (rc == 0) {
        dbg_time("%s: failed to lookup qmi service\n", __func__);
        goto init_close_ctrl_fd;
    }

    return ctrl_sock;

init_close_ctrl_fd:
    close(ctrl_sock);
    return -1;
}

static void handle_ctrl_pkt(int sock) {
    union ctl_msg pkt;
    uint32_t type;
    int rc;

    rc = recvfrom(sock, &pkt, sizeof(pkt), 0, NULL, NULL);
    if (rc < 0)
        return;

    type = le32toh(pkt.cmd);
    if (CTL_CMD_NEW_SERVER == type || CTL_CMD_REMOVE_SERVER == type) {
        QrtrService s;

        s.service = le32toh(pkt.srv.service);
        s.version = le32toh(pkt.srv.instance) & 0xff;
        s.instance = le32toh(pkt.srv.instance) >> 8;
        s.node = le32toh(pkt.srv.node_id);
        s.port = le32toh(pkt.srv.port_id);

        if (debug_qmi)
            dbg_time ("[qrtr] %s  server on %u:%u -> service %u, version %u, instance %u",
                    CTL_CMD_NEW_SERVER == type ? "add" : "remove",
                     s.node, s.port, s.service, s.version, s.instance);

        if (CTL_CMD_NEW_SERVER == type) {
            if (s.service < QRTR_MAX) {
                service_list[s.service] = s;
            }
        }
        else if (CTL_CMD_REMOVE_SERVER == type) {
            if (s.service < QRTR_MAX) {
                memset(&service_list[s.service], 0, sizeof(QrtrService));
            }
        }
    }
}
#else
#include <linux/socket.h>
#include <linux/qrtr.h>
#endif

static int qrtr_socket(void)
{
    struct sockaddr_qrtr sq;
    socklen_t sl = sizeof(sq);
    int sock;
    int rc;

    sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
    if (sock < 0) {
        dbg_time("qrtr_socket errno: %d (%s)\n", errno, strerror(errno));
        return -1;
    }

    rc = getsockname(sock, (void *)&sq, &sl);
    if (rc || sq.sq_family != AF_QIPCRTR || sl != sizeof(sq)) {
        dbg_time("getsockname: %d (%s)\n", errno, strerror(errno));
        close(sock);
        return -1;
    }

    return sock;
}

static int qrtr_send(int sock, uint32_t node, uint32_t port, const void *data, unsigned int sz)
{
    struct sockaddr_qrtr sq;
    int rc;

    sq.sq_family = AF_QIPCRTR;
    sq.sq_node = node;
    sq.sq_port = port;

    rc = sendto(sock, data, sz, MSG_DONTWAIT, (void *)&sq, sizeof(sq));
    if (rc < 0) {
        dbg_time("sendto errno: %d (%s)\n", errno, strerror(errno));
        return -1;
    }

    return 0;
}

static int qrtr_recv(int sock, void *data, unsigned int sz, uint32_t *node, uint32_t *port)
{
    struct sockaddr_qrtr sq;
    socklen_t sl = sizeof(sq);
    int rc;

    rc = recvfrom(sock, data, sz, MSG_DONTWAIT, (void *)&sq, &sl);
    if (rc < 0) {
        dbg_time("qrtr_recv errno: %d (%s)\n", errno, strerror(errno));
    }

    *node  = sq.sq_node;
    *port  = sq.sq_port;
    return rc;
    }
#define qmi_recv qrtr_recv

static int qrtr_ctrl_init(void) {
    int sock;
    int rc;
    struct qrtr_ctrl_pkt pkt;
    struct sockaddr_qrtr sq;
    socklen_t sl = sizeof(sq);

    sock = qrtr_socket();
    if (sock == -1)
        return -1;

    memset(&pkt, 0, sizeof(pkt));
    pkt.cmd = htole32(QRTR_TYPE_NEW_LOOKUP);

    getsockname(sock, (void *)&sq, &sl);
    rc = qrtr_send(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt));
    if (rc == -1) {
        dbg_time("qrtr_send errno: %d (%s)\n", errno, strerror(errno));
        close(sock);
        return -1;
    }

    return sock;
}

static void handle_server_change(uint32_t type, struct qrtr_ctrl_pkt *ppkt) {
    struct qrtr_ctrl_pkt pkt = *ppkt;
        QrtrService s;

        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);

        if (debug_qmi)
            dbg_time ("[qrtr] %s  server on %u:%u -> service %u, version %u, instance %u",
                    QRTR_TYPE_NEW_SERVER == type ? "add" : "remove",
                     s.node, s.port, s.service, s.version, s.instance);

        if (s.node != node_modem)
            return; //we only care modem

        if (QRTR_TYPE_NEW_SERVER == type) {
            if (s.service < QRTR_MAX) {
                service_list[s.service] = s;
            }
        }
        else if (QRTR_TYPE_DEL_SERVER == type) {
            if (s.service < QRTR_MAX) {
                memset(&service_list[s.service], 0, sizeof(QrtrService));
            }
        }
    }

static void handle_ctrl_pkt(int sock) {
    struct qrtr_ctrl_pkt pkt;
    struct sockaddr_qrtr sq;
    socklen_t sl = sizeof(sq);
    uint32_t type;
    int rc;

    rc = recvfrom(sock, &pkt, sizeof(pkt), 0, (void *)&sq, &sl);
    if (rc < 0)
        return;

    type = le32toh(pkt.cmd);
    if (debug_qmi)
        dbg_time("type %u, node %u, sq.port %x, len: %d", type, sq.sq_node, sq.sq_port, rc);

    if (sq.sq_port != QRTR_PORT_CTRL)
        return;

    if (QRTR_TYPE_NEW_SERVER == type || QRTR_TYPE_DEL_SERVER == type) {
        handle_server_change(type, &pkt);
    }
}

static int get_client(UCHAR QMIType) {
    int ClientId;
    QrtrService *s = &service_list[QMIType];

    if (!s ->service) {
        dbg_time("%s service: %d for QMIType: %d", __func__, s ->service, QMIType);
        return -ENODEV;
    }

#ifdef USE_LINUX_MSM_IPC
    ClientId = msm_ipc_socket("xport");
#else
    ClientId = qrtr_socket();
#endif
    if (ClientId == -1) {
        return 0;
    }

    switch (QMIType) {
        case QMUX_TYPE_WDS: dbg_time("Get clientWDS = %d", ClientId); break;
        case QMUX_TYPE_DMS: dbg_time("Get clientDMS = %d", ClientId); break;
        case QMUX_TYPE_NAS: dbg_time("Get clientNAS = %d", ClientId); break;
        case QMUX_TYPE_QOS: dbg_time("Get clientQOS = %d", ClientId); break;
        case QMUX_TYPE_WMS: dbg_time("Get clientWMS = %d", ClientId); break;
        case QMUX_TYPE_PDS: dbg_time("Get clientPDS = %d", ClientId); break;
        case QMUX_TYPE_UIM: dbg_time("Get clientUIM = %d", ClientId); break;
        case QMUX_TYPE_WDS_ADMIN: dbg_time("Get clientWDA = %d", ClientId);
        break;
        default: break;
    }

    return ClientId;
}

static void handle_alloc_client(PROFILE_T *profile) {
    int srv_list[] = {QMUX_TYPE_WDS, QMUX_TYPE_NAS, QMUX_TYPE_UIM, QMUX_TYPE_DMS, QMUX_TYPE_WDS_ADMIN};
    size_t i = 0, srv_ready = 0;
    static int report = -1;

    if (report != -1)
        return;

    for(i = 0; i < sizeof(srv_list)/sizeof(srv_list[0]); i++) {
        int srv = srv_list[i];

        if (service_list[srv].service)
            srv_ready++;
        else
            continue;

        if (qmiclientId[srv] == 0) {
            qmiclientId[srv] = get_client(srv);

            if (qmiclientId[srv] != 0) {
                if (srv == QMUX_TYPE_WDS) {
                    qmiclientId[QMUX_TYPE_WDS_IPV6] = get_client(QMUX_TYPE_WDS);
                }
                else if (srv == QMUX_TYPE_WDS_ADMIN) {
                     profile->wda_client = qmiclientId[QMUX_TYPE_WDS_ADMIN];
                }
            }
        }
    }

    if (srv_ready == sizeof(srv_list)/sizeof(srv_list[0])) {
        if (qmiclientId[QMUX_TYPE_WDS]) {
            qmidevice_send_event_to_main(RIL_INDICATE_DEVICE_CONNECTED);
        } else {
            qmidevice_send_event_to_main(RIL_INDICATE_DEVICE_DISCONNECTED);
        }
        report = 1;
    }
}

static int qmi_send(PQCQMIMSG pRequest) {
    uint8_t QMIType = pRequest->QMIHdr.QMIType;
    int sock;
    QrtrService *s = &service_list[QMIType == QMUX_TYPE_WDS_IPV6 ? QMUX_TYPE_WDS: QMIType];
    sock = qmiclientId[QMIType];

    pRequest->QMIHdr.ClientId = 0xaa;
    if (!s ->service || !sock) {
        dbg_time("%s service: %d, sock: %d for QMIType: %d", __func__, s ->service, sock, QMIType);
        return -ENODEV;
    }

#ifdef USE_LINUX_MSM_IPC
    return xport_send(sock, s->node, s->port, &pRequest->MUXMsg,
            le16_to_cpu(pRequest->QMIHdr.Length) + 1 - sizeof(QCQMI_HDR));
#else
    return qrtr_send(sock, s->node, s->port, &pRequest->MUXMsg,
            le16_to_cpu(pRequest->QMIHdr.Length) + 1 - sizeof(QCQMI_HDR));
#endif
}

static int qmi_deinit(void) {
    unsigned int i;

    for (i = 0; i < sizeof(qmiclientId)/sizeof(qmiclientId[0]); i++)
    {
        if (qmiclientId[i] != 0)
        {
            close(qmiclientId[i]);
            qmiclientId[i] = 0;
        }
    }

    return 0;
}

static void * qmi_read(void *pData) {
    PROFILE_T *profile = (PROFILE_T *)pData;
    int ctrl_sock;
    int wait_for_request_quit = 0;   

#ifdef USE_LINUX_MSM_IPC
    ctrl_sock = xport_ctrl_init();
    if (ctrl_sock != -1)
        qmidevice_send_event_to_main(RIL_INDICATE_DEVICE_CONNECTED);
#else
    ctrl_sock = qrtr_ctrl_init();
#endif

    if (ctrl_sock == -1)
        goto _quit;

    while (1) {
        struct pollfd pollfds[16] = {{qmidevice_control_fd[1], POLLIN, 0}, {ctrl_sock, POLLIN, 0}};
        int ne, ret, nevents = 2;
        unsigned int i;

        for (i = 0; i < sizeof(qmiclientId)/sizeof(qmiclientId[0]); i++)
        {
            if (qmiclientId[i] != 0)
            {
                pollfds[nevents].fd = qmiclientId[i];
                pollfds[nevents].events = POLLIN;
                pollfds[nevents].revents = 0;
                nevents++;
            }
        }

        do {
            ret = poll(pollfds, nevents, wait_for_request_quit ? 1000 : -1);
         } while ((ret < 0) && (errno == EINTR));

	if (ret == 0 && wait_for_request_quit) {
            QmiThreadRecvQMI(NULL); //main thread may pending on QmiThreadSendQMI()
            continue;
	}

        if (ret <= 0) {
            dbg_time("%s poll=%d, errno: %d (%s)", __func__, ret, errno, strerror(errno));
            break;
        }

        for (ne = 0; ne < nevents; ne++) {
            int fd = pollfds[ne].fd;
            short revents = pollfds[ne].revents;

            if (revents & (POLLERR | POLLHUP | POLLNVAL)) {
                dbg_time("%s poll err/hup/inval", __func__);
                dbg_time("epoll fd = %d, events = 0x%04x", fd, revents);
                if (fd == qmidevice_control_fd[1]) {
                } else {
                }
                if (revents & (POLLERR | POLLHUP | POLLNVAL))
                    goto _quit;
            }

            if ((revents & POLLIN) == 0)
                continue;

            if (fd == qmidevice_control_fd[1]) {
                int triger_event;
                if (read(fd, &triger_event, sizeof(triger_event)) == sizeof(triger_event)) {
                    //DBG("triger_event = 0x%x", triger_event);
                    switch (triger_event) {
                        case RIL_REQUEST_QUIT:
                            goto _quit;
                        break;
                        case SIG_EVENT_STOP:
                            wait_for_request_quit = 1;   
                        break;
                        default:
                        break;
                    }
                }
            }
            else  if (fd == ctrl_sock) {
                handle_ctrl_pkt(ctrl_sock);
                handle_alloc_client(profile);
            }
            else
            {
                PQCQMIMSG pResponse = (PQCQMIMSG)cm_recv_buf;
                int rc;
                uint32_t sq_node = 0;
                uint32_t sq_port = 0;

                rc = qmi_recv(fd, &pResponse->MUXMsg, sizeof(cm_recv_buf) - sizeof(QCQMI_HDR), &sq_node, &sq_port);
                if (debug_qmi)
                    dbg_time("fd %d, node %u, port %x, len: %d", fd, sq_node, sq_port, rc);

                if (rc <= 0)
                {
                    dbg_time("%s read=%d errno: %d (%s)",  __func__, rc, errno, strerror(errno));
                    break;
                }

                for (i = 0; i < sizeof(qmiclientId)/sizeof(qmiclientId[0]); i++)
                {
                    if (qmiclientId[i] == fd)
                    {
                        pResponse->QMIHdr.QMIType = i;

                        if (service_list[i].node != sq_node || service_list[i].port != sq_port) {
                            continue;
                        }
                    }
                }

                pResponse->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI;
                pResponse->QMIHdr.Length = cpu_to_le16(rc + sizeof(QCQMI_HDR)  - 1);
                pResponse->QMIHdr.CtlFlags = 0x00;
                pResponse->QMIHdr.ClientId = 0xaa;

                QmiThreadRecvQMI(pResponse);
            }
        }
    }

_quit:
    qmi_deinit();
    close(ctrl_sock);
    qmidevice_send_event_to_main(RIL_INDICATE_DEVICE_DISCONNECTED);
    QmiThreadRecvQMI(NULL); //main thread may pending on QmiThreadSendQMI()
    dbg_time("%s exit", __func__);
    pthread_exit(NULL);
    return NULL;
}

const struct qmi_device_ops qrtr_qmidev_ops = {
	.deinit = qmi_deinit,
	.send = qmi_send,
	.read = qmi_read,
};