OLD | NEW |
(Empty) | |
| 1 /* qmi.c - QMI protocol implementation |
| 2 * Copyright (c) 2010, Code Aurora Forum. All rights reserved. |
| 3 |
| 4 * This program is free software; you can redistribute it and/or modify |
| 5 * it under the terms of the GNU General Public License version 2 and |
| 6 * only version 2 as published by the Free Software Foundation. |
| 7 |
| 8 * This program is distributed in the hope that it will be useful, |
| 9 * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 11 * GNU General Public License for more details. |
| 12 |
| 13 * You should have received a copy of the GNU General Public License |
| 14 * along with this program; if not, write to the Free Software |
| 15 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA |
| 16 * 02110-1301, USA. |
| 17 */ |
| 18 |
| 19 #include "qmi.h" |
| 20 |
| 21 #include <linux/slab.h> |
| 22 |
| 23 struct qmux { |
| 24 u8 tf; /* always 1 */ |
| 25 u16 len; |
| 26 u8 ctrl; |
| 27 u8 service; |
| 28 u8 qmicid; |
| 29 } __attribute__((__packed__)); |
| 30 |
| 31 struct getcid_req { |
| 32 struct qmux header; |
| 33 u8 req; |
| 34 u8 tid; |
| 35 u16 msgid; |
| 36 u16 tlvsize; |
| 37 u8 service; |
| 38 u16 size; |
| 39 u8 qmisvc; |
| 40 } __attribute__((__packed__)); |
| 41 |
| 42 struct releasecid_req { |
| 43 struct qmux header; |
| 44 u8 req; |
| 45 u8 tid; |
| 46 u16 msgid; |
| 47 u16 tlvsize; |
| 48 u8 rlscid; |
| 49 u16 size; |
| 50 u16 cid; |
| 51 } __attribute__((__packed__)); |
| 52 |
| 53 struct ready_req { |
| 54 struct qmux header; |
| 55 u8 req; |
| 56 u8 tid; |
| 57 u16 msgid; |
| 58 u16 tlvsize; |
| 59 } __attribute__((__packed__)); |
| 60 |
| 61 struct seteventreport_req { |
| 62 struct qmux header; |
| 63 u8 req; |
| 64 u16 tid; |
| 65 u16 msgid; |
| 66 u16 tlvsize; |
| 67 u8 reportchanrate; |
| 68 u16 size; |
| 69 u8 period; |
| 70 u32 mask; |
| 71 } __attribute__((__packed__)); |
| 72 |
| 73 struct getpkgsrvcstatus_req { |
| 74 struct qmux header; |
| 75 u8 req; |
| 76 u16 tid; |
| 77 u16 msgid; |
| 78 u16 tlvsize; |
| 79 } __attribute__((__packed__)); |
| 80 |
| 81 struct getmeid_req { |
| 82 struct qmux header; |
| 83 u8 req; |
| 84 u16 tid; |
| 85 u16 msgid; |
| 86 u16 tlvsize; |
| 87 } __attribute__((__packed__)); |
| 88 |
| 89 const size_t qmux_size = sizeof(struct qmux); |
| 90 |
| 91 void *qmictl_new_getcid(u8 tid, u8 svctype, size_t *size) |
| 92 { |
| 93 struct getcid_req *req = kmalloc(sizeof(*req), GFP_KERNEL); |
| 94 if (!req) |
| 95 return NULL; |
| 96 req->req = 0x00; |
| 97 req->tid = tid; |
| 98 req->msgid = 0x0022; |
| 99 req->tlvsize = 0x0004; |
| 100 req->service = 0x01; |
| 101 req->size = 0x0001; |
| 102 req->qmisvc = svctype; |
| 103 *size = sizeof(*req); |
| 104 return req; |
| 105 } |
| 106 |
| 107 void *qmictl_new_releasecid(u8 tid, u16 cid, size_t *size) |
| 108 { |
| 109 struct releasecid_req *req = kmalloc(sizeof(*req), GFP_KERNEL); |
| 110 if (!req) |
| 111 return NULL; |
| 112 req->req = 0x00; |
| 113 req->tid = tid; |
| 114 req->msgid = 0x0023; |
| 115 req->tlvsize = 0x05; |
| 116 req->rlscid = 0x01; |
| 117 req->size = 0x0002; |
| 118 req->cid = cid; |
| 119 *size = sizeof(*req); |
| 120 return req; |
| 121 } |
| 122 |
| 123 void *qmictl_new_ready(u8 tid, size_t *size) |
| 124 { |
| 125 struct ready_req *req = kmalloc(sizeof(*req), GFP_KERNEL); |
| 126 if (!req) |
| 127 return NULL; |
| 128 req->req = 0x00; |
| 129 req->tid = tid; |
| 130 req->msgid = 0x21; |
| 131 req->tlvsize = 0; |
| 132 *size = sizeof(*req); |
| 133 return req; |
| 134 } |
| 135 |
| 136 void *qmiwds_new_seteventreport(u8 tid, size_t *size) |
| 137 { |
| 138 struct seteventreport_req *req = kmalloc(sizeof(*req), GFP_KERNEL); |
| 139 req->req = 0x00; |
| 140 req->tid = tid; |
| 141 req->msgid = 0x0001; |
| 142 req->tlvsize = 0x0008; |
| 143 req->reportchanrate = 0x11; |
| 144 req->size = 0x0005; |
| 145 req->period = 0x01; |
| 146 req->mask = 0x000000ff; |
| 147 *size = sizeof(*req); |
| 148 return req; |
| 149 } |
| 150 |
| 151 void *qmiwds_new_getpkgsrvcstatus(u8 tid, size_t *size) |
| 152 { |
| 153 struct getpkgsrvcstatus_req *req = kmalloc(sizeof(*req), GFP_KERNEL); |
| 154 if (!req) |
| 155 return NULL; |
| 156 req->req = 0x00; |
| 157 req->tid = tid; |
| 158 req->msgid = 0x22; |
| 159 req->tlvsize = 0x0000; |
| 160 *size = sizeof(*req); |
| 161 return req; |
| 162 } |
| 163 |
| 164 void *qmidms_new_getmeid(u8 tid, size_t *size) |
| 165 { |
| 166 struct getmeid_req *req = kmalloc(sizeof(*req), GFP_KERNEL); |
| 167 if (!req) |
| 168 return NULL; |
| 169 req->req = 0x00; |
| 170 req->tid = tid; |
| 171 req->msgid = 0x25; |
| 172 req->tlvsize = 0x0000; |
| 173 *size = sizeof(*req); |
| 174 return req; |
| 175 } |
| 176 |
| 177 int qmux_parse(u16 *cid, void *buf, size_t size) |
| 178 { |
| 179 struct qmux *qmux = buf; |
| 180 |
| 181 if (!buf || size < 12) |
| 182 return -ENOMEM; |
| 183 |
| 184 if (qmux->tf != 1 || qmux->len != size - 1 || qmux->ctrl != 0x80) |
| 185 return -EINVAL; |
| 186 |
| 187 *cid = (qmux->qmicid << 8) + qmux->service; |
| 188 return sizeof(*qmux); |
| 189 } |
| 190 |
| 191 int qmux_fill(u16 cid, void *buf, size_t size) |
| 192 { |
| 193 struct qmux *qmux = buf; |
| 194 |
| 195 if (!buf || size < sizeof(*qmux)) |
| 196 return -ENOMEM; |
| 197 |
| 198 qmux->tf = 1; |
| 199 qmux->len = size - 1; |
| 200 qmux->ctrl = 0; |
| 201 qmux->service = cid & 0xff; |
| 202 qmux->qmicid = cid >> 8; |
| 203 return 0; |
| 204 } |
| 205 |
| 206 static u16 tlv_get(void *msg, u16 msgsize, u8 type, void *buf, u16 bufsize) |
| 207 { |
| 208 u16 pos; |
| 209 u16 msize = 0; |
| 210 |
| 211 if (!msg || !buf) |
| 212 return -ENOMEM; |
| 213 |
| 214 for (pos = 4; pos + 3 < msgsize; pos += msize + 3) { |
| 215 msize = *(u16 *)(msg + pos + 1); |
| 216 if (*(u8 *)(msg + pos) == type) { |
| 217 if (bufsize < msize) |
| 218 return -ENOMEM; |
| 219 |
| 220 memcpy(buf, msg + pos + 3, msize); |
| 221 return msize; |
| 222 } |
| 223 } |
| 224 |
| 225 return -ENOMSG; |
| 226 } |
| 227 |
| 228 int qmi_msgisvalid(void *msg, u16 size) |
| 229 { |
| 230 char tlv[4]; |
| 231 |
| 232 if (tlv_get(msg, size, 2, &tlv[0], 4) == 4) { |
| 233 if (*(u16 *)&tlv[0] != 0) |
| 234 return *(u16 *)&tlv[2]; |
| 235 else |
| 236 return 0; |
| 237 } |
| 238 return -ENOMSG; |
| 239 } |
| 240 |
| 241 int qmi_msgid(void *msg, u16 size) |
| 242 { |
| 243 return size < 2 ? -ENODATA : *(u16 *)msg; |
| 244 } |
| 245 |
| 246 int qmictl_alloccid_resp(void *buf, u16 size, u16 *cid) |
| 247 { |
| 248 int result; |
| 249 u8 offset = sizeof(struct qmux) + 2; |
| 250 |
| 251 if (!buf || size < offset) |
| 252 return -ENOMEM; |
| 253 |
| 254 buf = buf + offset; |
| 255 size -= offset; |
| 256 |
| 257 result = qmi_msgid(buf, size); |
| 258 if (result != 0x22) |
| 259 return -EFAULT; |
| 260 |
| 261 result = qmi_msgisvalid(buf, size); |
| 262 if (result != 0) |
| 263 return -EFAULT; |
| 264 |
| 265 result = tlv_get(buf, size, 0x01, cid, 2); |
| 266 if (result != 2) |
| 267 return -EFAULT; |
| 268 |
| 269 return 0; |
| 270 } |
| 271 |
| 272 int qmictl_freecid_resp(void *buf, u16 size) |
| 273 { |
| 274 int result; |
| 275 u8 offset = sizeof(struct qmux) + 2; |
| 276 |
| 277 if (!buf || size < offset) |
| 278 return -ENOMEM; |
| 279 |
| 280 buf = buf + offset; |
| 281 size -= offset; |
| 282 |
| 283 result = qmi_msgid(buf, size); |
| 284 if (result != 0x23) |
| 285 return -EFAULT; |
| 286 |
| 287 result = qmi_msgisvalid(buf, size); |
| 288 if (result != 0) |
| 289 return -EFAULT; |
| 290 |
| 291 return 0; |
| 292 } |
| 293 |
| 294 int qmiwds_event_resp(void *buf, u16 size, struct qmiwds_stats *stats) |
| 295 { |
| 296 int result; |
| 297 u8 status[2]; |
| 298 |
| 299 u8 offset = sizeof(struct qmux) + 3; |
| 300 |
| 301 if (!buf || size < offset || !stats) |
| 302 return -ENOMEM; |
| 303 |
| 304 buf = buf + offset; |
| 305 size -= offset; |
| 306 |
| 307 result = qmi_msgid(buf, size); |
| 308 if (result == 0x01) { |
| 309 tlv_get(buf, size, 0x10, &stats->txok, 4); |
| 310 tlv_get(buf, size, 0x11, &stats->rxok, 4); |
| 311 tlv_get(buf, size, 0x12, &stats->txerr, 4); |
| 312 tlv_get(buf, size, 0x13, &stats->rxerr, 4); |
| 313 tlv_get(buf, size, 0x14, &stats->txofl, 4); |
| 314 tlv_get(buf, size, 0x15, &stats->rxofl, 4); |
| 315 tlv_get(buf, size, 0x19, &stats->txbytesok, 8); |
| 316 tlv_get(buf, size, 0x1A, &stats->rxbytesok, 8); |
| 317 } else if (result == 0x22) { |
| 318 result = tlv_get(buf, size, 0x01, &status[0], 2); |
| 319 if (result >= 1) |
| 320 stats->linkstate = status[0] == 0x02; |
| 321 if (result == 2) |
| 322 stats->reconfigure = status[1] == 0x01; |
| 323 |
| 324 if (result < 0) |
| 325 return result; |
| 326 } else { |
| 327 return -EFAULT; |
| 328 } |
| 329 |
| 330 return 0; |
| 331 } |
| 332 |
| 333 int qmidms_meid_resp(void *buf, u16 size, char *meid, int meidsize) |
| 334 { |
| 335 int result; |
| 336 |
| 337 u8 offset = sizeof(struct qmux) + 3; |
| 338 |
| 339 if (!buf || size < offset || meidsize < 14) |
| 340 return -ENOMEM; |
| 341 |
| 342 buf = buf + offset; |
| 343 size -= offset; |
| 344 |
| 345 result = qmi_msgid(buf, size); |
| 346 if (result != 0x25) |
| 347 return -EFAULT; |
| 348 |
| 349 result = qmi_msgisvalid(buf, size); |
| 350 if (result) |
| 351 return -EFAULT; |
| 352 |
| 353 result = tlv_get(buf, size, 0x12, meid, 14); |
| 354 if (result != 14) |
| 355 return -EFAULT; |
| 356 |
| 357 return 0; |
| 358 } |
OLD | NEW |