1 2 #include <stddef.h> 3 #include <fcntl.h> 4 #include <talloc.h> 5 6 #include <libubox/list.h> 7 #include <libubox/utils.h> 8 9 #include "qmi-enums.h" 10 #include "qmi-enums-private.h" 11 #include "qmi-message.h" 12 #include "qmi-struct.h" 13 14 #include "ctrl.h" 15 #include "uqmid.h" 16 #include "logging.h" 17 #include "services.h" 18 #include "modem.h" 19 #include "gsmtap_util.h" 20 21 /* FIXME: decide dump_packet */ 22 #define dump_packet(str, buf, len) 23 24 static void 25 __qmi_request_complete(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) 26 { 27 void *tlv_buf; 28 int tlv_len; 29 30 if (!req->pending) 31 return; 32 33 req->pending = false; 34 req->complete = true; 35 list_del(&req->list); 36 37 if (msg) { 38 tlv_buf = qmi_msg_get_tlv_buf(msg, &tlv_len); 39 req->ret = qmi_check_message_status(tlv_buf, tlv_len); 40 } else { 41 req->ret = QMI_ERROR_CANCELLED; 42 } 43 44 if (req->cb && msg) 45 req->cb(service, req, msg); 46 47 talloc_free(req); 48 /* frees msg as well because of tree */ 49 } 50 51 static void 52 qmi_process_msg(struct qmi_dev *qmi, struct qmi_msg *msg) 53 { 54 struct qmi_service *service; 55 struct qmi_request *req; 56 uint16_t tid; 57 bool resp, ind; 58 59 if (msg->qmux.service == QMI_SERVICE_CTL) { 60 modem_log(qmi->modem, LOGL_DEBUG, "Process message from srv %d msg %04x flag: %02x tid: %02x", 61 msg->qmux.service, le16_to_cpu(msg->ctl.message), msg->flags, msg->ctl.transaction); 62 tid = msg->ctl.transaction; 63 ind = msg->flags & QMI_CTL_FLAG_INDICATION; 64 resp = msg->flags & QMI_CTL_FLAG_RESPONSE; 65 if (!ind && !resp) { 66 /* TODO: error_log("Invalid message received") */ 67 return; 68 } 69 } else { 70 modem_log(qmi->modem, LOGL_DEBUG, "Process message from srv %d msg %04x flag: %02x tid: %04x", 71 msg->qmux.service, le16_to_cpu(msg->svc.message), msg->flags, msg->svc.transaction); 72 tid = le16_to_cpu(msg->svc.transaction); 73 ind = msg->flags & QMI_SERVICE_FLAG_INDICATION; 74 resp = msg->flags & QMI_SERVICE_FLAG_RESPONSE; 75 if (!ind && !resp) { 76 /* TODO: error_log("Invalid message received") */ 77 return; 78 } 79 } 80 81 service = uqmi_service_find(qmi, msg->qmux.service); 82 if (!service) { 83 /* error_log("Couldn't find a service for incoming message") */ 84 return; 85 } 86 87 /* Hopefully an indication *and* response isn't possible */ 88 if (ind) { 89 uqmi_service_handle_indication(service, msg); 90 } 91 92 if (resp) { 93 list_for_each_entry(req, &service->reqs, list) { 94 if (req->tid != tid) 95 continue; 96 97 __qmi_request_complete(service, req, msg); 98 return; 99 } 100 } 101 102 /* error_log("Couldn't find a tid for incoming message") */ 103 } 104 105 static void qmi_notify_read(struct ustream *us, int bytes) 106 { 107 struct qmi_dev *qmi = container_of(us, struct qmi_dev, sf.stream); 108 struct qmi_msg *msg; 109 char *buf; 110 int len, msg_len; 111 112 113 while (1) { 114 buf = ustream_get_read_buf(us, &len); 115 if (!buf || !len) 116 return; 117 118 /* FIXME: check partial reads */ 119 /* FIXME: implement mbim */ 120 121 dump_packet("Received packet", buf, len); 122 if (len < offsetof(struct qmi_msg, flags)) 123 return; 124 msg = (struct qmi_msg *) buf; 125 msg_len = le16_to_cpu(msg->qmux.len) + 1; 126 127 if (len < msg_len) 128 return; 129 130 gsmtap_send(qmi->modem, msg, msg_len); 131 qmi_process_msg(qmi, msg); 132 ustream_consume(us, msg_len); 133 } 134 } 135 136 static void qmi_notify_state(struct ustream *us) 137 { 138 struct qmi_dev *qmi = container_of(us, struct qmi_dev, sf.stream); 139 140 if (us->eof || us->write_error) { 141 modem_log(qmi->modem, LOGL_ERROR, "Modem connection died! Closing modem."); 142 } else { 143 modem_log(qmi->modem, LOGL_ERROR, "Unknown modem fd state change eof: %d write_error: %d. Closing modem anyways", 144 us->eof, us->write_error); 145 } 146 147 /* errors! */ 148 if (qmi->state != QMI_STOPPING) 149 qmi->state = QMI_ERROR; 150 151 if (qmi->error_cb) 152 qmi->error_cb(qmi, qmi->error_cb_data); 153 } 154 155 struct qmi_dev *qmi_device_open(struct modem *modem, const char *path) 156 { 157 struct qmi_dev *qmi; 158 struct ustream *us; 159 int fd; 160 161 /* assert(qmi) */ 162 163 fd = open(path, O_RDWR | O_EXCL | O_NONBLOCK | O_NOCTTY); 164 if (fd < 0) 165 return NULL; 166 167 qmi = talloc_zero(modem, struct qmi_dev); 168 us = &qmi->sf.stream; 169 170 us->notify_state = qmi_notify_state; 171 us->notify_read = qmi_notify_read; 172 ustream_fd_init(&qmi->sf, fd); 173 INIT_LIST_HEAD(&qmi->services); 174 qmi->modem = modem; 175 176 uqmi_ctrl_generate(qmi); 177 178 return qmi; 179 } 180 181 /* timer callback to give service the time to shut down */ 182 static void qmi_device_close_cb(struct uloop_timeout *timeout) 183 { 184 struct qmi_service *service, *tmp; 185 struct qmi_dev *qmi = container_of(timeout, struct qmi_dev, shutdown); 186 187 modem_log(qmi->modem, LOGL_INFO, "Closing qmi device"); 188 uqmi_service_close(qmi->ctrl); 189 list_for_each_entry_safe(service, tmp, &qmi->services, list) { 190 list_del(&service->list); 191 talloc_free(service); 192 } 193 qmi->ctrl = NULL; 194 195 ustream_free(&qmi->sf.stream); 196 close(qmi->sf.fd.fd); 197 198 if (qmi->closing_cb) 199 qmi->closing_cb(qmi, qmi->closing_cb_data); 200 } 201 202 /* called by the service when the QMI modem release the client id */ 203 void qmi_device_service_closed(struct qmi_dev *qmi) 204 { 205 if (qmi->state != QMI_STOPPING) 206 return; 207 /* only ctrl left, use schedule to decouple it from req and break a free(req) loop */ 208 if (qmi->services.next == qmi->services.prev && qmi->services.prev == &qmi->ctrl->list) 209 uloop_timeout_set(&qmi->shutdown, 0); 210 } 211 212 void qmi_device_close(struct qmi_dev *qmi, int timeout_ms) 213 { 214 struct qmi_service *service, *tmp; 215 bool error = qmi->state == QMI_ERROR; 216 217 if (qmi->state == QMI_STOPPING) 218 return; 219 220 qmi->state = QMI_STOPPING; 221 if (!error) { 222 list_for_each_entry_safe(service, tmp, &qmi->services, list) { 223 /* CTL service is required to close the others. The pending request will be cleared in _cb */ 224 if (service->service == QMI_SERVICE_CTL) 225 continue; 226 227 uqmi_service_close(service); 228 } 229 } 230 231 /* should we allow to close all services at once or should we close it slowly? one-by-one? */ 232 if (timeout_ms <= 0 || error) { 233 qmi_device_close_cb(&qmi->shutdown); 234 } else { 235 qmi->shutdown.cb = qmi_device_close_cb; 236 uloop_timeout_set(&qmi->shutdown, timeout_ms); 237 } 238 } 239
This page was automatically generated by LXR 0.3.1. • OpenWrt