1 /* similar to dev.c but self container */ 2 3 #include <stdint.h> 4 5 #include <talloc.h> 6 #include <errno.h> 7 8 #include "qmi-enums.h" 9 #include "qmi-message.h" 10 11 #include "ctrl.h" 12 #include "logging.h" 13 #include "modem.h" 14 #include "services.h" 15 #include "uqmid.h" 16 17 #include "gsmtap_util.h" 18 19 #ifdef DEBUG_PACKET 20 static void dump_packet(const char *prefix, void *ptr, int len) 21 { 22 unsigned char *data = ptr; 23 int i; 24 25 fprintf(stderr, "%s:", prefix); 26 for (i = 0; i < len; i++) 27 fprintf(stderr, " %02x", data[i]); 28 fprintf(stderr, "\n"); 29 } 30 #else 31 static void dump_packet(const char *prefix, void *ptr, int len) 32 { 33 } 34 #endif 35 36 struct qmi_service * 37 uqmi_service_find(struct qmi_dev *qmi, int service_id) 38 { 39 struct qmi_service *service; 40 list_for_each_entry(service, &qmi->services, list) { 41 if (service->service == service_id) 42 return service; 43 } 44 45 return NULL; 46 } 47 48 struct qmi_service * 49 uqmi_service_create(struct qmi_dev *qmi, int service_id) 50 { 51 struct qmi_service *service = talloc_zero(qmi, struct qmi_service); 52 53 service->service = service_id; 54 service->qmi = qmi; 55 service->client_id = -1; 56 57 list_add(&service->list, &qmi->services); 58 INIT_LIST_HEAD(&service->indications); 59 INIT_LIST_HEAD(&service->reqs); 60 61 return service; 62 } 63 64 struct qmi_service * 65 uqmi_service_find_or_init(struct qmi_dev *qmi, int service_id) 66 { 67 struct qmi_service *service = uqmi_service_find(qmi, service_id); 68 69 if (service) 70 return service; 71 72 return uqmi_service_create(qmi, service_id); 73 } 74 75 int 76 uqmi_service_get_next_tid(struct qmi_service *service) 77 { 78 service->tid++; 79 /* CTL only has 8 bit tid */ 80 if (service->service == QMI_SERVICE_CTL && service->tid >= (1 << 8)) 81 service->tid = 1; 82 else if (!service->tid) 83 service->tid = 1; 84 85 return service->tid; 86 } 87 88 static int 89 _service_send_request(struct qmi_service *service, struct qmi_request *req) 90 { 91 struct qmi_msg *msg = req->msg; 92 int len = qmi_complete_request_message(msg); 93 uint16_t tid = uqmi_service_get_next_tid(service); 94 95 if (req->service->service == QMI_SERVICE_CTL) { 96 msg->ctl.transaction = tid; 97 } else { 98 /* FIXME: check if service state is ready for this */ 99 msg->svc.transaction = cpu_to_le16(tid); 100 msg->qmux.client = service->client_id; 101 } 102 103 req->ret = -1; 104 req->tid = tid; 105 req->pending = true; 106 107 /* FIXME: fix mbim support */ 108 109 if (service->service == QMI_SERVICE_CTL) 110 modem_log(service->qmi->modem, LOGL_DEBUG, "Transmit message to srv %d msg %04x flag: %02x tid: %02x", 111 msg->qmux.service, le16_to_cpu(msg->ctl.message), msg->flags, le16_to_cpu(msg->ctl.transaction)); 112 else 113 modem_log(service->qmi->modem, LOGL_DEBUG, "Transmit message to srv %d msg %04x flag: %02x tid: %04x", 114 msg->qmux.service, le16_to_cpu(msg->svc.message), msg->flags, le16_to_cpu(msg->svc.transaction)); 115 116 dump_packet("Send packet", msg, len); 117 gsmtap_send(service->qmi->modem, msg, len); 118 ustream_write(&service->qmi->sf.stream, (void *) msg, len, false); 119 120 return 0; 121 } 122 123 /* send out all pending request */ 124 static int 125 uqmi_service_send_request(struct qmi_service *service) 126 { 127 struct qmi_request *req; 128 list_for_each_entry(req, &service->reqs, list) { 129 if (!req->pending && !req->complete) 130 _service_send_request(service, req); 131 } 132 133 return 0; 134 } 135 136 void 137 uqmi_service_get_client_id_cb(struct qmi_service *service, uint16_t client_id) 138 { 139 service->client_id = client_id; 140 service->state = SERVICE_READY; 141 142 service_log(service, LOGL_INFO, "Assigned client id %d. Service READY", client_id); 143 uqmi_service_send_request(service); 144 } 145 146 /* get a client id via ctrl service */ 147 static int 148 uqmi_service_get_client_id(struct qmi_service *service) 149 { 150 if (service->service == QMI_SERVICE_CTL) { 151 fprintf(stderr, "Foo!!!"); 152 return -EINVAL; 153 } 154 155 service_log(service, LOGL_INFO, "Request client id"); 156 157 service->state = SERVICE_WAIT_CID; 158 uqmi_ctrl_request_clientid(service); 159 160 return 0; 161 } 162 163 int 164 uqmi_service_send_simple(struct qmi_service *service, 165 int(*encoder)(struct qmi_msg *msg), 166 request_cb cb, void *cb_data) 167 { 168 struct qmi_request *req = talloc_zero(service, struct qmi_request); 169 struct qmi_msg *msg = talloc_zero_size(req, 1024); 170 171 req->msg = msg; 172 req->cb = cb; 173 req->cb_data = cb_data; 174 175 int ret = encoder(msg); 176 if (ret) { 177 cb(service, req, NULL); 178 talloc_free(req); 179 modem_log(service->qmi->modem, LOGL_ERROR, "Failed to encode in send_simple"); 180 return -1; 181 } 182 183 return uqmi_service_send_msg(service, req); 184 } 185 186 int 187 uqmi_service_send_msg(struct qmi_service *service, struct qmi_request *req) 188 { 189 req->pending = false; 190 req->complete = false; 191 req->service = service; 192 193 list_add(&req->list, &service->reqs); 194 if (service->state == SERVICE_IDLE) 195 return uqmi_service_get_client_id(service); 196 else 197 return uqmi_service_send_request(service); 198 } 199 200 int 201 uqmi_service_send_msg2(struct qmi_dev *qmi, struct qmi_request *req, int service_id) 202 { 203 struct qmi_service *service = uqmi_service_find_or_init(qmi, service_id); 204 205 if (!service) { 206 /* FIXME: error_log("Couldn't find/create service for id %d", service_id) */ 207 return -EINVAL; 208 } 209 210 req->pending = false; 211 req->complete = false; 212 req->service = service; 213 214 list_add(&req->list, &service->reqs); 215 if (service->state == SERVICE_IDLE) 216 return uqmi_service_get_client_id(service); 217 else 218 return uqmi_service_send_request(service); 219 } 220 221 /* called when the call id returns */ 222 void uqmi_service_close_cb(struct qmi_service *service) 223 { 224 struct qmi_dev *qmi = service->qmi; 225 service_log(service, LOGL_INFO, "Released service."); 226 227 list_del(&service->list); 228 talloc_free(service); 229 qmi_device_service_closed(qmi); 230 } 231 232 void uqmi_service_close(struct qmi_service *service) 233 { 234 if (service->service == QMI_SERVICE_CTL) 235 return; 236 237 service_log(service, LOGL_INFO, "Closing service %d (cid %d)", service->service, service->client_id); 238 239 /* FIXME: SERVICE_WAIT_CID might/will leak a CID */ 240 if (service->state != SERVICE_READY) { 241 uqmi_service_close_cb(service); 242 return; 243 } 244 245 /* Control service is special */ 246 uqmi_ctrl_release_clientid(service); 247 } 248 249 int uqmi_service_register_indication(struct qmi_service *service, uint16_t qmi_ind, indication_cb cb, void *cb_data) 250 { 251 struct qmi_indication *indication; 252 253 indication = talloc_zero(service, struct qmi_indication); 254 if (!indication) 255 return 1; 256 257 indication->cb = cb; 258 indication->cb_data = cb_data; 259 indication->qmi_ind = qmi_ind; 260 list_add(&indication->list, &service->indications); 261 262 return 0; 263 } 264 265 int uqmi_service_remove_indication(struct qmi_service *service, uint16_t qmi_ind, indication_cb cb, void *cb_data) 266 { 267 struct qmi_indication *indication, *tmp; 268 269 list_for_each_entry_safe(indication, tmp, &service->indications, list) { 270 if (qmi_ind != indication->qmi_ind) 271 continue; 272 273 if (indication->cb != cb) 274 continue; 275 276 if (indication->cb_data != cb_data) 277 continue; 278 279 list_del(&indication->list); 280 } 281 282 return 0; 283 } 284 285 int uqmi_service_handle_indication(struct qmi_service *service, struct qmi_msg *msg) 286 { 287 uint16_t qmi_ind; 288 bool handled = false; 289 struct qmi_indication *indication, *tmp; 290 291 if (msg->qmux.service == QMI_SERVICE_CTL) 292 qmi_ind = msg->ctl.message; 293 else 294 qmi_ind = msg->svc.message; 295 296 297 list_for_each_entry_safe(indication, tmp, &service->indications, list) { 298 if (qmi_ind != indication->qmi_ind) 299 continue; 300 301 if (indication->cb) { 302 indication->cb(service, msg, indication->cb_data); 303 handled = true; 304 } 305 } 306 307 return handled ? 0 : 1; 308 } 309
This page was automatically generated by LXR 0.3.1. • OpenWrt