| Message ID | 20240227033523.123542-1-steve.schrock@getcruise.com (mailing list archive) |
|---|---|
| State | Superseded, archived |
| Headers | show |
| Series | qmimodem: First QRTR commit: service discovery | expand |
Hi Steve, > This change allows the developer to force the /gobi_0 modem to be initialized > by passing --enable-qrtr to configure. Enabling QRTR will disable the existing > QMUX implementation. This is only temporary -- discovery of the QRTR > modem will be implemented later. please send these as RFC patches since they can not be applied upstream, not even temporary. > QRTR service discovery works by sending QRTR_TYPE_NEW_LOOKUP to > the special socket of type AF_QIPCRTR. Then the services are received > one-by-one. Soon they will be stored and made available for use by the > qmi client. If possible modifications to drivers/ and plugins/ should be split into separate patches. > Since the QRTR implementation does not implement shutdown, I modified > gobi to handle the failure to avoid lengthy timeouts. Just create a new plugin. The Gobi plugin is really the wrong place for that. If I remember this correctly, then Gobi was a set of cards talking some of the original versions of QMUX+QMI. > --- > Makefile.am | 5 + > configure.ac | 9 ++ > drivers/qmimodem/qmi.c | 231 +++++++++++++++++++++++++++++++++++++++++ > drivers/qmimodem/qmi.h | 1 + > plugins/gobi.c | 40 ++++++- > 5 files changed, 285 insertions(+), 1 deletion(-) > > diff --git a/Makefile.am b/Makefile.am > index 85bebae9..afec6997 100644 > --- a/Makefile.am > +++ b/Makefile.am > @@ -386,6 +386,11 @@ builtin_sources += $(qmi_sources) \ > drivers/qmimodem/call-forwarding.c > > builtin_sources += plugins/gobi.c > + > +if QMIMODEM_QRTR > +builtin_modules += gobi > +endif > + If the QMIMODEM can support QRTR then just enable a new plugins/qrtr.c along with the plugins/gobi.c, but don’t intermix it please. So far we just enabled the drivers support and with that it enabled the plugins. Doing this more fine grained creates a complexity that I rather not have right now. > endif > > if MBIMMODEM > diff --git a/configure.ac b/configure.ac > index 9fa6531a..14b37eef 100644 > --- a/configure.ac > +++ b/configure.ac > @@ -227,6 +227,15 @@ AC_ARG_ENABLE(qmimodem, AS_HELP_STRING([--disable-qmimodem], > [enable_qmimodem=${enableval}]) > AM_CONDITIONAL(QMIMODEM, test "${enable_qmimodem}" != "no") > > +AC_ARG_ENABLE(qrtr, AS_HELP_STRING([--enable_qrtr], > + [enable Qualcomm QRTR modem support (WIP)]), > + [enable_qrtr=${enableval}], > + [enable_qrtr=no]) > +AM_CONDITIONAL(QMIMODEM_QRTR, test "${enable_qrtr}" != "no") > +if (test "${enable_qrtr}" == "yes"); then > + AC_DEFINE(QMIMODEM_QRTR, [], [forces Qualcomm QRTR instead of QMUX]) > +fi > + > AC_ARG_ENABLE(mbimmodem, AS_HELP_STRING([--disable-mbimmodem], > [disable MBIM modem support]), > [enable_mbimmodem=${enableval}]) > diff --git a/drivers/qmimodem/qmi.c b/drivers/qmimodem/qmi.c > index 35751d7c..48f9ce10 100644 > --- a/drivers/qmimodem/qmi.c > +++ b/drivers/qmimodem/qmi.c > @@ -33,6 +33,8 @@ > #include <stdlib.h> > #include <string.h> > #include <linux/limits.h> > +#include <linux/qrtr.h> > +#include <sys/socket.h> > > #include <ell/ell.h> > > @@ -1874,6 +1876,235 @@ struct qmi_device *qmi_device_new_qmux(const char *device) > return &qmux->super; > } > > +struct qmi_device_qrtr { > + struct qmi_device super; > + struct discover_data *discover_data; > +}; > + > +static void __debug_ctrl_request(const struct qrtr_ctrl_pkt *packet, > + qmi_debug_func_t function, > + void *user_data) > +{ > + char strbuf[72 + 16], *str; > + const char *type; > + > + if (!function) > + return; > + > + str = strbuf; > + str += sprintf(str, " %s", > + __service_type_to_string(QMI_SERVICE_CONTROL)); > + > + type = "_pkt"; > + > + str += sprintf(str, "%s cmd=%d", type, > + L_LE32_TO_CPU(packet->cmd)); > + > + function(strbuf, user_data); > +} > + > +static void handle_control_packet(struct qmi_device_qrtr *qrtr, > + const struct qrtr_ctrl_pkt *packet) > +{ > + struct qmi_device *device = &qrtr->super; > + uint32_t cmd; > + uint32_t type; > + uint32_t instance; > + uint32_t version; > + uint32_t node; > + uint32_t port; > + > + __debug_ctrl_request(packet, device->debug_func, > + device->debug_data); > + > + cmd = L_LE32_TO_CPU(packet->cmd); > + if (cmd != QRTR_TYPE_NEW_SERVER) { > + DBG("Unknown command: %d", cmd); > + return; > + } > + > + if (!packet->server.service && !packet->server.instance && > + !packet->server.node && !packet->server.port) { > + DBG("Initial service discovery has completed"); > + > + if (qrtr->discover_data->func) > + qrtr->discover_data->func(qrtr->discover_data->user_data); > + > + discover_data_free(qrtr->discover_data); > + qrtr->discover_data = NULL; > + > + return; > + } > + > + type = L_LE32_TO_CPU(packet->server.service); > + version = L_LE32_TO_CPU(packet->server.instance) & 0xff; > + instance = L_LE32_TO_CPU(packet->server.instance) >> 8; > + > + node = L_LE32_TO_CPU(packet->server.node); > + port = L_LE32_TO_CPU(packet->server.port); > + > + if (cmd == QRTR_TYPE_NEW_SERVER) { > + DBG("New server: Type: %d Version: %d Instance: %d Node: %d Port: %d", > + type, version, instance, node, port); > + } > +} > + > +static void handle_packet(struct qmi_device_qrtr *qrtr, uint32_t sending_port, > + const void *buf, ssize_t len) > +{ > + const struct qrtr_ctrl_pkt *packet = buf; > + > + if (sending_port != QRTR_PORT_CTRL) { > + DBG("Receive of service data is not implemented"); > + return; > + } > + > + if ((unsigned long) len < sizeof(*packet)) { > + DBG("qrtr control packet is too small"); > + return; > + } > + > + handle_control_packet(qrtr, packet); > +} > + > +static bool received_qrtr_data(struct l_io *io, void *user_data) > +{ > + struct qmi_device_qrtr *qrtr = user_data; > + struct sockaddr_qrtr addr; > + unsigned char buf[2048]; > + ssize_t bytes_read; > + socklen_t addr_size; > + > + addr_size = sizeof(addr); > + bytes_read = recvfrom(l_io_get_fd(qrtr->super.io), buf, sizeof(buf), 0, > + (struct sockaddr *) &addr, &addr_size); > + DBG("Received %ld bytes from Node: %d Port: 0x%x", bytes_read, > + addr.sq_node, addr.sq_port); > + > + if (bytes_read < 0) > + return true; > + > + l_util_hexdump(true, buf, bytes_read, qrtr->super.debug_func, > + qrtr->super.debug_data); > + > + handle_packet(qrtr, addr.sq_port, buf, bytes_read); > + > + return true; > +} > + > +static int qmi_device_qrtr_discover(struct qmi_device *device, > + qmi_discover_func_t func, > + void *user_data, > + qmi_destroy_func_t destroy) > +{ > + struct qmi_device_qrtr *qrtr = > + l_container_of(device, struct qmi_device_qrtr, super); > + struct discover_data *data; > + struct qrtr_ctrl_pkt packet; > + struct sockaddr_qrtr addr; > + socklen_t addr_len; > + int rc; > + ssize_t bytes_written; > + int fd; > + > + __debug_device(device, "device %p discover", device); > + > + if (qrtr->discover_data) > + return -EINPROGRESS; > + > + data = l_new(struct discover_data, 1); > + > + data->super.destroy = discover_data_free; > + data->device = device; > + data->func = func; > + data->user_data = user_data; > + data->destroy = destroy; > + > + fd = l_io_get_fd(device->io); > + > + /* > + * The control node is configured by the system. Use getsockname to > + * get its value. > + */ > + addr_len = sizeof(addr); > + rc = getsockname(fd, (struct sockaddr *) &addr, &addr_len); > + if (rc) { > + DBG("getsockname failed: %s", strerror(errno)); > + goto error; > + } > + > + if (addr.sq_family != AF_QIPCRTR || addr_len != sizeof(addr)) { > + DBG("Unexpected sockaddr from getsockname. family: %d size: %d", > + addr.sq_family, addr_len); > + rc = -EIO; > + goto error; > + } > + > + addr.sq_port = QRTR_PORT_CTRL; > + memset(&packet, 0, sizeof(packet)); > + packet.cmd = L_CPU_TO_LE32(QRTR_TYPE_NEW_LOOKUP); > + > + bytes_written = sendto(fd, &packet, > + sizeof(packet), 0, > + (struct sockaddr *) &addr, addr_len); > + if (bytes_written < 0) { > + DBG("Failure sending data: %s", strerror(errno)); > + rc = -errno; > + goto error; > + } > + > + l_util_hexdump(false, &packet, bytes_written, > + device->debug_func, device->debug_data); > + > + qrtr->discover_data = data; > + > + return 0; > + > +error: > + l_free(data); > + > + return rc; > +} > + > +static void qmi_device_qrtr_destroy(struct qmi_device *device) > +{ > + struct qmi_device_qrtr *qrtr = > + l_container_of(device, struct qmi_device_qrtr, super); > + > + l_free(qrtr); > +} > + > +static const struct qmi_device_ops qrtr_ops = { > + .write = NULL, > + .discover = qmi_device_qrtr_discover, > + .client_create = NULL, > + .client_release = NULL, > + .shutdown = NULL, > + .destroy = qmi_device_qrtr_destroy, > +}; > + > +struct qmi_device *qmi_device_new_qrtr(void) > +{ > + struct qmi_device_qrtr *qrtr; > + int fd; > + > + fd = socket(AF_QIPCRTR, SOCK_DGRAM, 0); > + if (fd < 0) > + return NULL; > + > + qrtr = l_new(struct qmi_device_qrtr, 1); > + > + if (qmi_device_init(&qrtr->super, fd, &qrtr_ops) < 0) { > + close(fd); > + l_free(qrtr); > + return NULL; > + } > + > + l_io_set_read_handler(qrtr->super.io, received_qrtr_data, qrtr, NULL); > + > + return &qrtr->super; > +} > + > struct qmi_param *qmi_param_new(void) > { > struct qmi_param *param; > diff --git a/drivers/qmimodem/qmi.h b/drivers/qmimodem/qmi.h > index 6a3d3415..506fed6b 100644 > --- a/drivers/qmimodem/qmi.h > +++ b/drivers/qmimodem/qmi.h > @@ -101,6 +101,7 @@ bool qmi_device_set_expected_data_format(struct qmi_device *device, > enum qmi_device_expected_data_format format); > > struct qmi_device *qmi_device_new_qmux(const char *device); > +struct qmi_device *qmi_device_new_qrtr(void); > > struct qmi_param; > > diff --git a/plugins/gobi.c b/plugins/gobi.c > index 99a7d556..cf78ad58 100644 > --- a/plugins/gobi.c > +++ b/plugins/gobi.c > @@ -105,6 +105,7 @@ static int gobi_probe(struct ofono_modem *modem) > if (!data) > return -ENOMEM; > > +#ifndef QMIMODEM_QRTR > kernel_driver = ofono_modem_get_string(modem, "KernelDriver"); > DBG("kernel_driver: %s", kernel_driver); > > @@ -117,6 +118,9 @@ static int gobi_probe(struct ofono_modem *modem) > ofono_modem_get_string(modem, "NetworkInterface"), > sizeof(data->main_net_name)); > DBG("net: %s (%d)", data->main_net_name, data->main_net_ifindex); > +#else > + (void) kernel_driver; > +#endif #ifdef in the code are the super last resort if there is no other way to solve things. It would create a testing nightmare for no good reason. And tricks like "(void) variable" are not acceptable. That just hides serious warnings that should have been avoided in the first place. Regards Marcel
Thank you for the review explanations, Marcel. I understand that these were some ugly changes while awaiting the QRTR modem discovery, hopefully in udevng if possible. For now I will submit the changes that are entirely contained in qmi. This code will not be reachable without applying the extra changes in my earlier patch. Regards, Steve On Tue, Feb 27, 2024 at 12:29 AM Marcel Holtmann <marcel@holtmann.org> wrote: > > Hi Steve, > > > This change allows the developer to force the /gobi_0 modem to be initialized > > by passing --enable-qrtr to configure. Enabling QRTR will disable the existing > > QMUX implementation. This is only temporary -- discovery of the QRTR > > modem will be implemented later. > > please send these as RFC patches since they can not be applied upstream, not even temporary. > > > QRTR service discovery works by sending QRTR_TYPE_NEW_LOOKUP to > > the special socket of type AF_QIPCRTR. Then the services are received > > one-by-one. Soon they will be stored and made available for use by the > > qmi client. > > If possible modifications to drivers/ and plugins/ should be split into separate patches. > > > Since the QRTR implementation does not implement shutdown, I modified > > gobi to handle the failure to avoid lengthy timeouts. > > Just create a new plugin. The Gobi plugin is really the wrong place for that. If I remember this correctly, then Gobi was a set of cards talking some of the original versions of QMUX+QMI. > > > --- > > Makefile.am | 5 + > > configure.ac | 9 ++ > > drivers/qmimodem/qmi.c | 231 +++++++++++++++++++++++++++++++++++++++++ > > drivers/qmimodem/qmi.h | 1 + > > plugins/gobi.c | 40 ++++++- > > 5 files changed, 285 insertions(+), 1 deletion(-) > > > > diff --git a/Makefile.am b/Makefile.am > > index 85bebae9..afec6997 100644 > > --- a/Makefile.am > > +++ b/Makefile.am > > @@ -386,6 +386,11 @@ builtin_sources += $(qmi_sources) \ > > drivers/qmimodem/call-forwarding.c > > > > builtin_sources += plugins/gobi.c > > + > > +if QMIMODEM_QRTR > > +builtin_modules += gobi > > +endif > > + > > If the QMIMODEM can support QRTR then just enable a new plugins/qrtr.c along with the plugins/gobi.c, but don’t intermix it please. So far we just enabled the drivers support and with that it enabled the plugins. Doing this more fine grained creates a complexity that I rather not have right now. > > > endif > > > > if MBIMMODEM > > diff --git a/configure.ac b/configure.ac > > index 9fa6531a..14b37eef 100644 > > --- a/configure.ac > > +++ b/configure.ac > > @@ -227,6 +227,15 @@ AC_ARG_ENABLE(qmimodem, AS_HELP_STRING([--disable-qmimodem], > > [enable_qmimodem=${enableval}]) > > AM_CONDITIONAL(QMIMODEM, test "${enable_qmimodem}" != "no") > > > > +AC_ARG_ENABLE(qrtr, AS_HELP_STRING([--enable_qrtr], > > + [enable Qualcomm QRTR modem support (WIP)]), > > + [enable_qrtr=${enableval}], > > + [enable_qrtr=no]) > > +AM_CONDITIONAL(QMIMODEM_QRTR, test "${enable_qrtr}" != "no") > > +if (test "${enable_qrtr}" == "yes"); then > > + AC_DEFINE(QMIMODEM_QRTR, [], [forces Qualcomm QRTR instead of QMUX]) > > +fi > > + > > AC_ARG_ENABLE(mbimmodem, AS_HELP_STRING([--disable-mbimmodem], > > [disable MBIM modem support]), > > [enable_mbimmodem=${enableval}]) > > diff --git a/drivers/qmimodem/qmi.c b/drivers/qmimodem/qmi.c > > index 35751d7c..48f9ce10 100644 > > --- a/drivers/qmimodem/qmi.c > > +++ b/drivers/qmimodem/qmi.c > > @@ -33,6 +33,8 @@ > > #include <stdlib.h> > > #include <string.h> > > #include <linux/limits.h> > > +#include <linux/qrtr.h> > > +#include <sys/socket.h> > > > > #include <ell/ell.h> > > > > @@ -1874,6 +1876,235 @@ struct qmi_device *qmi_device_new_qmux(const char *device) > > return &qmux->super; > > } > > > > +struct qmi_device_qrtr { > > + struct qmi_device super; > > + struct discover_data *discover_data; > > +}; > > + > > +static void __debug_ctrl_request(const struct qrtr_ctrl_pkt *packet, > > + qmi_debug_func_t function, > > + void *user_data) > > +{ > > + char strbuf[72 + 16], *str; > > + const char *type; > > + > > + if (!function) > > + return; > > + > > + str = strbuf; > > + str += sprintf(str, " %s", > > + __service_type_to_string(QMI_SERVICE_CONTROL)); > > + > > + type = "_pkt"; > > + > > + str += sprintf(str, "%s cmd=%d", type, > > + L_LE32_TO_CPU(packet->cmd)); > > + > > + function(strbuf, user_data); > > +} > > + > > +static void handle_control_packet(struct qmi_device_qrtr *qrtr, > > + const struct qrtr_ctrl_pkt *packet) > > +{ > > + struct qmi_device *device = &qrtr->super; > > + uint32_t cmd; > > + uint32_t type; > > + uint32_t instance; > > + uint32_t version; > > + uint32_t node; > > + uint32_t port; > > + > > + __debug_ctrl_request(packet, device->debug_func, > > + device->debug_data); > > + > > + cmd = L_LE32_TO_CPU(packet->cmd); > > + if (cmd != QRTR_TYPE_NEW_SERVER) { > > + DBG("Unknown command: %d", cmd); > > + return; > > + } > > + > > + if (!packet->server.service && !packet->server.instance && > > + !packet->server.node && !packet->server.port) { > > + DBG("Initial service discovery has completed"); > > + > > + if (qrtr->discover_data->func) > > + qrtr->discover_data->func(qrtr->discover_data->user_data); > > + > > + discover_data_free(qrtr->discover_data); > > + qrtr->discover_data = NULL; > > + > > + return; > > + } > > + > > + type = L_LE32_TO_CPU(packet->server.service); > > + version = L_LE32_TO_CPU(packet->server.instance) & 0xff; > > + instance = L_LE32_TO_CPU(packet->server.instance) >> 8; > > + > > + node = L_LE32_TO_CPU(packet->server.node); > > + port = L_LE32_TO_CPU(packet->server.port); > > + > > + if (cmd == QRTR_TYPE_NEW_SERVER) { > > + DBG("New server: Type: %d Version: %d Instance: %d Node: %d Port: %d", > > + type, version, instance, node, port); > > + } > > +} > > + > > +static void handle_packet(struct qmi_device_qrtr *qrtr, uint32_t sending_port, > > + const void *buf, ssize_t len) > > +{ > > + const struct qrtr_ctrl_pkt *packet = buf; > > + > > + if (sending_port != QRTR_PORT_CTRL) { > > + DBG("Receive of service data is not implemented"); > > + return; > > + } > > + > > + if ((unsigned long) len < sizeof(*packet)) { > > + DBG("qrtr control packet is too small"); > > + return; > > + } > > + > > + handle_control_packet(qrtr, packet); > > +} > > + > > +static bool received_qrtr_data(struct l_io *io, void *user_data) > > +{ > > + struct qmi_device_qrtr *qrtr = user_data; > > + struct sockaddr_qrtr addr; > > + unsigned char buf[2048]; > > + ssize_t bytes_read; > > + socklen_t addr_size; > > + > > + addr_size = sizeof(addr); > > + bytes_read = recvfrom(l_io_get_fd(qrtr->super.io), buf, sizeof(buf), 0, > > + (struct sockaddr *) &addr, &addr_size); > > + DBG("Received %ld bytes from Node: %d Port: 0x%x", bytes_read, > > + addr.sq_node, addr.sq_port); > > + > > + if (bytes_read < 0) > > + return true; > > + > > + l_util_hexdump(true, buf, bytes_read, qrtr->super.debug_func, > > + qrtr->super.debug_data); > > + > > + handle_packet(qrtr, addr.sq_port, buf, bytes_read); > > + > > + return true; > > +} > > + > > +static int qmi_device_qrtr_discover(struct qmi_device *device, > > + qmi_discover_func_t func, > > + void *user_data, > > + qmi_destroy_func_t destroy) > > +{ > > + struct qmi_device_qrtr *qrtr = > > + l_container_of(device, struct qmi_device_qrtr, super); > > + struct discover_data *data; > > + struct qrtr_ctrl_pkt packet; > > + struct sockaddr_qrtr addr; > > + socklen_t addr_len; > > + int rc; > > + ssize_t bytes_written; > > + int fd; > > + > > + __debug_device(device, "device %p discover", device); > > + > > + if (qrtr->discover_data) > > + return -EINPROGRESS; > > + > > + data = l_new(struct discover_data, 1); > > + > > + data->super.destroy = discover_data_free; > > + data->device = device; > > + data->func = func; > > + data->user_data = user_data; > > + data->destroy = destroy; > > + > > + fd = l_io_get_fd(device->io); > > + > > + /* > > + * The control node is configured by the system. Use getsockname to > > + * get its value. > > + */ > > + addr_len = sizeof(addr); > > + rc = getsockname(fd, (struct sockaddr *) &addr, &addr_len); > > + if (rc) { > > + DBG("getsockname failed: %s", strerror(errno)); > > + goto error; > > + } > > + > > + if (addr.sq_family != AF_QIPCRTR || addr_len != sizeof(addr)) { > > + DBG("Unexpected sockaddr from getsockname. family: %d size: %d", > > + addr.sq_family, addr_len); > > + rc = -EIO; > > + goto error; > > + } > > + > > + addr.sq_port = QRTR_PORT_CTRL; > > + memset(&packet, 0, sizeof(packet)); > > + packet.cmd = L_CPU_TO_LE32(QRTR_TYPE_NEW_LOOKUP); > > + > > + bytes_written = sendto(fd, &packet, > > + sizeof(packet), 0, > > + (struct sockaddr *) &addr, addr_len); > > + if (bytes_written < 0) { > > + DBG("Failure sending data: %s", strerror(errno)); > > + rc = -errno; > > + goto error; > > + } > > + > > + l_util_hexdump(false, &packet, bytes_written, > > + device->debug_func, device->debug_data); > > + > > + qrtr->discover_data = data; > > + > > + return 0; > > + > > +error: > > + l_free(data); > > + > > + return rc; > > +} > > + > > +static void qmi_device_qrtr_destroy(struct qmi_device *device) > > +{ > > + struct qmi_device_qrtr *qrtr = > > + l_container_of(device, struct qmi_device_qrtr, super); > > + > > + l_free(qrtr); > > +} > > + > > +static const struct qmi_device_ops qrtr_ops = { > > + .write = NULL, > > + .discover = qmi_device_qrtr_discover, > > + .client_create = NULL, > > + .client_release = NULL, > > + .shutdown = NULL, > > + .destroy = qmi_device_qrtr_destroy, > > +}; > > + > > +struct qmi_device *qmi_device_new_qrtr(void) > > +{ > > + struct qmi_device_qrtr *qrtr; > > + int fd; > > + > > + fd = socket(AF_QIPCRTR, SOCK_DGRAM, 0); > > + if (fd < 0) > > + return NULL; > > + > > + qrtr = l_new(struct qmi_device_qrtr, 1); > > + > > + if (qmi_device_init(&qrtr->super, fd, &qrtr_ops) < 0) { > > + close(fd); > > + l_free(qrtr); > > + return NULL; > > + } > > + > > + l_io_set_read_handler(qrtr->super.io, received_qrtr_data, qrtr, NULL); > > + > > + return &qrtr->super; > > +} > > + > > struct qmi_param *qmi_param_new(void) > > { > > struct qmi_param *param; > > diff --git a/drivers/qmimodem/qmi.h b/drivers/qmimodem/qmi.h > > index 6a3d3415..506fed6b 100644 > > --- a/drivers/qmimodem/qmi.h > > +++ b/drivers/qmimodem/qmi.h > > @@ -101,6 +101,7 @@ bool qmi_device_set_expected_data_format(struct qmi_device *device, > > enum qmi_device_expected_data_format format); > > > > struct qmi_device *qmi_device_new_qmux(const char *device); > > +struct qmi_device *qmi_device_new_qrtr(void); > > > > struct qmi_param; > > > > diff --git a/plugins/gobi.c b/plugins/gobi.c > > index 99a7d556..cf78ad58 100644 > > --- a/plugins/gobi.c > > +++ b/plugins/gobi.c > > @@ -105,6 +105,7 @@ static int gobi_probe(struct ofono_modem *modem) > > if (!data) > > return -ENOMEM; > > > > +#ifndef QMIMODEM_QRTR > > kernel_driver = ofono_modem_get_string(modem, "KernelDriver"); > > DBG("kernel_driver: %s", kernel_driver); > > > > @@ -117,6 +118,9 @@ static int gobi_probe(struct ofono_modem *modem) > > ofono_modem_get_string(modem, "NetworkInterface"), > > sizeof(data->main_net_name)); > > DBG("net: %s (%d)", data->main_net_name, data->main_net_ifindex); > > +#else > > + (void) kernel_driver; > > +#endif > > #ifdef in the code are the super last resort if there is no other way to solve things. It would create a testing nightmare for no good reason. And tricks like "(void) variable" are not acceptable. That just hides serious warnings that should have been avoided in the first place. > > Regards > > Marcel >
diff --git a/Makefile.am b/Makefile.am index 85bebae9..afec6997 100644 --- a/Makefile.am +++ b/Makefile.am @@ -386,6 +386,11 @@ builtin_sources += $(qmi_sources) \ drivers/qmimodem/call-forwarding.c builtin_sources += plugins/gobi.c + +if QMIMODEM_QRTR +builtin_modules += gobi +endif + endif if MBIMMODEM diff --git a/configure.ac b/configure.ac index 9fa6531a..14b37eef 100644 --- a/configure.ac +++ b/configure.ac @@ -227,6 +227,15 @@ AC_ARG_ENABLE(qmimodem, AS_HELP_STRING([--disable-qmimodem], [enable_qmimodem=${enableval}]) AM_CONDITIONAL(QMIMODEM, test "${enable_qmimodem}" != "no") +AC_ARG_ENABLE(qrtr, AS_HELP_STRING([--enable_qrtr], + [enable Qualcomm QRTR modem support (WIP)]), + [enable_qrtr=${enableval}], + [enable_qrtr=no]) +AM_CONDITIONAL(QMIMODEM_QRTR, test "${enable_qrtr}" != "no") +if (test "${enable_qrtr}" == "yes"); then + AC_DEFINE(QMIMODEM_QRTR, [], [forces Qualcomm QRTR instead of QMUX]) +fi + AC_ARG_ENABLE(mbimmodem, AS_HELP_STRING([--disable-mbimmodem], [disable MBIM modem support]), [enable_mbimmodem=${enableval}]) diff --git a/drivers/qmimodem/qmi.c b/drivers/qmimodem/qmi.c index 35751d7c..48f9ce10 100644 --- a/drivers/qmimodem/qmi.c +++ b/drivers/qmimodem/qmi.c @@ -33,6 +33,8 @@ #include <stdlib.h> #include <string.h> #include <linux/limits.h> +#include <linux/qrtr.h> +#include <sys/socket.h> #include <ell/ell.h> @@ -1874,6 +1876,235 @@ struct qmi_device *qmi_device_new_qmux(const char *device) return &qmux->super; } +struct qmi_device_qrtr { + struct qmi_device super; + struct discover_data *discover_data; +}; + +static void __debug_ctrl_request(const struct qrtr_ctrl_pkt *packet, + qmi_debug_func_t function, + void *user_data) +{ + char strbuf[72 + 16], *str; + const char *type; + + if (!function) + return; + + str = strbuf; + str += sprintf(str, " %s", + __service_type_to_string(QMI_SERVICE_CONTROL)); + + type = "_pkt"; + + str += sprintf(str, "%s cmd=%d", type, + L_LE32_TO_CPU(packet->cmd)); + + function(strbuf, user_data); +} + +static void handle_control_packet(struct qmi_device_qrtr *qrtr, + const struct qrtr_ctrl_pkt *packet) +{ + struct qmi_device *device = &qrtr->super; + uint32_t cmd; + uint32_t type; + uint32_t instance; + uint32_t version; + uint32_t node; + uint32_t port; + + __debug_ctrl_request(packet, device->debug_func, + device->debug_data); + + cmd = L_LE32_TO_CPU(packet->cmd); + if (cmd != QRTR_TYPE_NEW_SERVER) { + DBG("Unknown command: %d", cmd); + return; + } + + if (!packet->server.service && !packet->server.instance && + !packet->server.node && !packet->server.port) { + DBG("Initial service discovery has completed"); + + if (qrtr->discover_data->func) + qrtr->discover_data->func(qrtr->discover_data->user_data); + + discover_data_free(qrtr->discover_data); + qrtr->discover_data = NULL; + + return; + } + + type = L_LE32_TO_CPU(packet->server.service); + version = L_LE32_TO_CPU(packet->server.instance) & 0xff; + instance = L_LE32_TO_CPU(packet->server.instance) >> 8; + + node = L_LE32_TO_CPU(packet->server.node); + port = L_LE32_TO_CPU(packet->server.port); + + if (cmd == QRTR_TYPE_NEW_SERVER) { + DBG("New server: Type: %d Version: %d Instance: %d Node: %d Port: %d", + type, version, instance, node, port); + } +} + +static void handle_packet(struct qmi_device_qrtr *qrtr, uint32_t sending_port, + const void *buf, ssize_t len) +{ + const struct qrtr_ctrl_pkt *packet = buf; + + if (sending_port != QRTR_PORT_CTRL) { + DBG("Receive of service data is not implemented"); + return; + } + + if ((unsigned long) len < sizeof(*packet)) { + DBG("qrtr control packet is too small"); + return; + } + + handle_control_packet(qrtr, packet); +} + +static bool received_qrtr_data(struct l_io *io, void *user_data) +{ + struct qmi_device_qrtr *qrtr = user_data; + struct sockaddr_qrtr addr; + unsigned char buf[2048]; + ssize_t bytes_read; + socklen_t addr_size; + + addr_size = sizeof(addr); + bytes_read = recvfrom(l_io_get_fd(qrtr->super.io), buf, sizeof(buf), 0, + (struct sockaddr *) &addr, &addr_size); + DBG("Received %ld bytes from Node: %d Port: 0x%x", bytes_read, + addr.sq_node, addr.sq_port); + + if (bytes_read < 0) + return true; + + l_util_hexdump(true, buf, bytes_read, qrtr->super.debug_func, + qrtr->super.debug_data); + + handle_packet(qrtr, addr.sq_port, buf, bytes_read); + + return true; +} + +static int qmi_device_qrtr_discover(struct qmi_device *device, + qmi_discover_func_t func, + void *user_data, + qmi_destroy_func_t destroy) +{ + struct qmi_device_qrtr *qrtr = + l_container_of(device, struct qmi_device_qrtr, super); + struct discover_data *data; + struct qrtr_ctrl_pkt packet; + struct sockaddr_qrtr addr; + socklen_t addr_len; + int rc; + ssize_t bytes_written; + int fd; + + __debug_device(device, "device %p discover", device); + + if (qrtr->discover_data) + return -EINPROGRESS; + + data = l_new(struct discover_data, 1); + + data->super.destroy = discover_data_free; + data->device = device; + data->func = func; + data->user_data = user_data; + data->destroy = destroy; + + fd = l_io_get_fd(device->io); + + /* + * The control node is configured by the system. Use getsockname to + * get its value. + */ + addr_len = sizeof(addr); + rc = getsockname(fd, (struct sockaddr *) &addr, &addr_len); + if (rc) { + DBG("getsockname failed: %s", strerror(errno)); + goto error; + } + + if (addr.sq_family != AF_QIPCRTR || addr_len != sizeof(addr)) { + DBG("Unexpected sockaddr from getsockname. family: %d size: %d", + addr.sq_family, addr_len); + rc = -EIO; + goto error; + } + + addr.sq_port = QRTR_PORT_CTRL; + memset(&packet, 0, sizeof(packet)); + packet.cmd = L_CPU_TO_LE32(QRTR_TYPE_NEW_LOOKUP); + + bytes_written = sendto(fd, &packet, + sizeof(packet), 0, + (struct sockaddr *) &addr, addr_len); + if (bytes_written < 0) { + DBG("Failure sending data: %s", strerror(errno)); + rc = -errno; + goto error; + } + + l_util_hexdump(false, &packet, bytes_written, + device->debug_func, device->debug_data); + + qrtr->discover_data = data; + + return 0; + +error: + l_free(data); + + return rc; +} + +static void qmi_device_qrtr_destroy(struct qmi_device *device) +{ + struct qmi_device_qrtr *qrtr = + l_container_of(device, struct qmi_device_qrtr, super); + + l_free(qrtr); +} + +static const struct qmi_device_ops qrtr_ops = { + .write = NULL, + .discover = qmi_device_qrtr_discover, + .client_create = NULL, + .client_release = NULL, + .shutdown = NULL, + .destroy = qmi_device_qrtr_destroy, +}; + +struct qmi_device *qmi_device_new_qrtr(void) +{ + struct qmi_device_qrtr *qrtr; + int fd; + + fd = socket(AF_QIPCRTR, SOCK_DGRAM, 0); + if (fd < 0) + return NULL; + + qrtr = l_new(struct qmi_device_qrtr, 1); + + if (qmi_device_init(&qrtr->super, fd, &qrtr_ops) < 0) { + close(fd); + l_free(qrtr); + return NULL; + } + + l_io_set_read_handler(qrtr->super.io, received_qrtr_data, qrtr, NULL); + + return &qrtr->super; +} + struct qmi_param *qmi_param_new(void) { struct qmi_param *param; diff --git a/drivers/qmimodem/qmi.h b/drivers/qmimodem/qmi.h index 6a3d3415..506fed6b 100644 --- a/drivers/qmimodem/qmi.h +++ b/drivers/qmimodem/qmi.h @@ -101,6 +101,7 @@ bool qmi_device_set_expected_data_format(struct qmi_device *device, enum qmi_device_expected_data_format format); struct qmi_device *qmi_device_new_qmux(const char *device); +struct qmi_device *qmi_device_new_qrtr(void); struct qmi_param; diff --git a/plugins/gobi.c b/plugins/gobi.c index 99a7d556..cf78ad58 100644 --- a/plugins/gobi.c +++ b/plugins/gobi.c @@ -105,6 +105,7 @@ static int gobi_probe(struct ofono_modem *modem) if (!data) return -ENOMEM; +#ifndef QMIMODEM_QRTR kernel_driver = ofono_modem_get_string(modem, "KernelDriver"); DBG("kernel_driver: %s", kernel_driver); @@ -117,6 +118,9 @@ static int gobi_probe(struct ofono_modem *modem) ofono_modem_get_string(modem, "NetworkInterface"), sizeof(data->main_net_name)); DBG("net: %s (%d)", data->main_net_name, data->main_net_ifindex); +#else + (void) kernel_driver; +#endif ofono_modem_set_data(modem, data); @@ -175,7 +179,10 @@ static void shutdown_device(struct ofono_modem *modem) cleanup_services(data); - qmi_device_shutdown(data->device, shutdown_cb, modem, NULL); + if (qmi_device_shutdown(data->device, shutdown_cb, modem, NULL)) { + DBG("qmi_device_shutdown failed, calling shutdown_cb directly"); + shutdown_cb(modem); + } } static void power_reset_cb(struct qmi_result *result, void *user_data) @@ -426,11 +433,17 @@ static int gobi_enable(struct ofono_modem *modem) DBG("%p", modem); +#ifdef QMIMODEM_QRTR + (void) device; + data->device = qmi_device_new_qrtr(); +#else device = ofono_modem_get_string(modem, "Device"); if (!device) return -EINVAL; data->device = qmi_device_new_qmux(device); +#endif + if (!data->device) return -EIO; @@ -766,3 +779,28 @@ static struct ofono_modem_driver gobi_driver = { }; OFONO_MODEM_DRIVER_BUILTIN(gobi, &gobi_driver) + +#ifdef QMIMODEM_QRTR +static struct ofono_modem *qrtr_modem; + +static int gobi_init(void) +{ + DBG(""); + + qrtr_modem = ofono_modem_create(NULL, "gobi"); + ofono_modem_register(qrtr_modem); + + return 0; +} + +static void gobi_exit(void) +{ + DBG(""); + + ofono_modem_remove(qrtr_modem); + qrtr_modem = NULL; +} + +OFONO_PLUGIN_DEFINE(gobi, "Qualcomm Gobi modem driver", VERSION, + OFONO_PLUGIN_PRIORITY_DEFAULT, gobi_init, gobi_exit) +#endif