parameters.c 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589
  1. /*********************************************************************
  2. *
  3. * Filename: parameters.c
  4. * Version: 1.0
  5. * Description: A more general way to handle (pi,pl,pv) parameters
  6. * Status: Experimental.
  7. * Author: Dag Brattli <dagb@cs.uit.no>
  8. * Created at: Mon Jun 7 10:25:11 1999
  9. * Modified at: Sun Jan 30 14:08:39 2000
  10. * Modified by: Dag Brattli <dagb@cs.uit.no>
  11. *
  12. * Copyright (c) 1999-2000 Dag Brattli, All Rights Reserved.
  13. *
  14. * This program is free software; you can redistribute it and/or
  15. * modify it under the terms of the GNU General Public License as
  16. * published by the Free Software Foundation; either version 2 of
  17. * the License, or (at your option) any later version.
  18. *
  19. * This program is distributed in the hope that it will be useful,
  20. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  21. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  22. * GNU General Public License for more details.
  23. *
  24. * You should have received a copy of the GNU General Public License
  25. * along with this program; if not, write to the Free Software
  26. * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  27. * MA 02111-1307 USA
  28. *
  29. ********************************************************************/
  30. #include <linux/types.h>
  31. #include <linux/module.h>
  32. #include <asm/unaligned.h>
  33. #include <asm/byteorder.h>
  34. #include <net/irda/irda.h>
  35. #include <net/irda/parameters.h>
  36. static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
  37. PV_TYPE type, PI_HANDLER func);
  38. static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
  39. PV_TYPE type, PI_HANDLER func);
  40. static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
  41. PV_TYPE type, PI_HANDLER func);
  42. static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi,
  43. PV_TYPE type, PI_HANDLER func);
  44. static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi,
  45. PV_TYPE type, PI_HANDLER func);
  46. static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi,
  47. PV_TYPE type, PI_HANDLER func);
  48. static int irda_param_unpack(__u8 *buf, char *fmt, ...);
  49. /* Parameter value call table. Must match PV_TYPE */
  50. static PV_HANDLER pv_extract_table[] = {
  51. irda_extract_integer, /* Handler for any length integers */
  52. irda_extract_integer, /* Handler for 8 bits integers */
  53. irda_extract_integer, /* Handler for 16 bits integers */
  54. irda_extract_string, /* Handler for strings */
  55. irda_extract_integer, /* Handler for 32 bits integers */
  56. irda_extract_octseq, /* Handler for octet sequences */
  57. irda_extract_no_value /* Handler for no value parameters */
  58. };
  59. static PV_HANDLER pv_insert_table[] = {
  60. irda_insert_integer, /* Handler for any length integers */
  61. irda_insert_integer, /* Handler for 8 bits integers */
  62. irda_insert_integer, /* Handler for 16 bits integers */
  63. NULL, /* Handler for strings */
  64. irda_insert_integer, /* Handler for 32 bits integers */
  65. NULL, /* Handler for octet sequences */
  66. irda_insert_no_value /* Handler for no value parameters */
  67. };
  68. /*
  69. * Function irda_insert_no_value (self, buf, len, pi, type, func)
  70. */
  71. static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi,
  72. PV_TYPE type, PI_HANDLER func)
  73. {
  74. irda_param_t p;
  75. int ret;
  76. p.pi = pi;
  77. p.pl = 0;
  78. /* Call handler for this parameter */
  79. ret = (*func)(self, &p, PV_GET);
  80. /* Extract values anyway, since handler may need them */
  81. irda_param_pack(buf, "bb", p.pi, p.pl);
  82. if (ret < 0)
  83. return ret;
  84. return 2; /* Inserted pl+2 bytes */
  85. }
  86. /*
  87. * Function irda_extract_no_value (self, buf, len, type, func)
  88. *
  89. * Extracts a parameter without a pv field (pl=0)
  90. *
  91. */
  92. static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi,
  93. PV_TYPE type, PI_HANDLER func)
  94. {
  95. irda_param_t p;
  96. int ret;
  97. /* Extract values anyway, since handler may need them */
  98. irda_param_unpack(buf, "bb", &p.pi, &p.pl);
  99. /* Call handler for this parameter */
  100. ret = (*func)(self, &p, PV_PUT);
  101. if (ret < 0)
  102. return ret;
  103. return 2; /* Extracted pl+2 bytes */
  104. }
  105. /*
  106. * Function irda_insert_integer (self, buf, len, pi, type, func)
  107. */
  108. static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi,
  109. PV_TYPE type, PI_HANDLER func)
  110. {
  111. irda_param_t p;
  112. int n = 0;
  113. int err;
  114. p.pi = pi; /* In case handler needs to know */
  115. p.pl = type & PV_MASK; /* The integer type codes the lenght as well */
  116. p.pv.i = 0; /* Clear value */
  117. /* Call handler for this parameter */
  118. err = (*func)(self, &p, PV_GET);
  119. if (err < 0)
  120. return err;
  121. /*
  122. * If parameter lenght is still 0, then (1) this is an any length
  123. * integer, and (2) the handler function does not care which length
  124. * we choose to use, so we pick the one the gives the fewest bytes.
  125. */
  126. if (p.pl == 0) {
  127. if (p.pv.i < 0xff) {
  128. IRDA_DEBUG(2, "%s(), using 1 byte\n", __FUNCTION__);
  129. p.pl = 1;
  130. } else if (p.pv.i < 0xffff) {
  131. IRDA_DEBUG(2, "%s(), using 2 bytes\n", __FUNCTION__);
  132. p.pl = 2;
  133. } else {
  134. IRDA_DEBUG(2, "%s(), using 4 bytes\n", __FUNCTION__);
  135. p.pl = 4; /* Default length */
  136. }
  137. }
  138. /* Check if buffer is long enough for insertion */
  139. if (len < (2+p.pl)) {
  140. IRDA_WARNING("%s: buffer to short for insertion!\n",
  141. __FUNCTION__);
  142. return -1;
  143. }
  144. IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__,
  145. p.pi, p.pl, p.pv.i);
  146. switch (p.pl) {
  147. case 1:
  148. n += irda_param_pack(buf, "bbb", p.pi, p.pl, (__u8) p.pv.i);
  149. break;
  150. case 2:
  151. if (type & PV_BIG_ENDIAN)
  152. p.pv.i = cpu_to_be16((__u16) p.pv.i);
  153. else
  154. p.pv.i = cpu_to_le16((__u16) p.pv.i);
  155. n += irda_param_pack(buf, "bbs", p.pi, p.pl, (__u16) p.pv.i);
  156. break;
  157. case 4:
  158. if (type & PV_BIG_ENDIAN)
  159. cpu_to_be32s(&p.pv.i);
  160. else
  161. cpu_to_le32s(&p.pv.i);
  162. n += irda_param_pack(buf, "bbi", p.pi, p.pl, p.pv.i);
  163. break;
  164. default:
  165. IRDA_WARNING("%s: length %d not supported\n",
  166. __FUNCTION__, p.pl);
  167. /* Skip parameter */
  168. return -1;
  169. }
  170. return p.pl+2; /* Inserted pl+2 bytes */
  171. }
  172. /*
  173. * Function irda_extract integer (self, buf, len, pi, type, func)
  174. *
  175. * Extract a possibly variable length integer from buffer, and call
  176. * handler for processing of the parameter
  177. */
  178. static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
  179. PV_TYPE type, PI_HANDLER func)
  180. {
  181. irda_param_t p;
  182. int n = 0;
  183. int extract_len; /* Real lenght we extract */
  184. int err;
  185. p.pi = pi; /* In case handler needs to know */
  186. p.pl = buf[1]; /* Extract lenght of value */
  187. p.pv.i = 0; /* Clear value */
  188. extract_len = p.pl; /* Default : extract all */
  189. /* Check if buffer is long enough for parsing */
  190. if (len < (2+p.pl)) {
  191. IRDA_WARNING("%s: buffer to short for parsing! "
  192. "Need %d bytes, but len is only %d\n",
  193. __FUNCTION__, p.pl, len);
  194. return -1;
  195. }
  196. /*
  197. * Check that the integer length is what we expect it to be. If the
  198. * handler want a 16 bits integer then a 32 bits is not good enough
  199. * PV_INTEGER means that the handler is flexible.
  200. */
  201. if (((type & PV_MASK) != PV_INTEGER) && ((type & PV_MASK) != p.pl)) {
  202. IRDA_ERROR("%s: invalid parameter length! "
  203. "Expected %d bytes, but value had %d bytes!\n",
  204. __FUNCTION__, type & PV_MASK, p.pl);
  205. /* Most parameters are bit/byte fields or little endian,
  206. * so it's ok to only extract a subset of it (the subset
  207. * that the handler expect). This is necessary, as some
  208. * broken implementations seems to add extra undefined bits.
  209. * If the parameter is shorter than we expect or is big
  210. * endian, we can't play those tricks. Jean II */
  211. if((p.pl < (type & PV_MASK)) || (type & PV_BIG_ENDIAN)) {
  212. /* Skip parameter */
  213. return p.pl+2;
  214. } else {
  215. /* Extract subset of it, fallthrough */
  216. extract_len = type & PV_MASK;
  217. }
  218. }
  219. switch (extract_len) {
  220. case 1:
  221. n += irda_param_unpack(buf+2, "b", &p.pv.i);
  222. break;
  223. case 2:
  224. n += irda_param_unpack(buf+2, "s", &p.pv.i);
  225. if (type & PV_BIG_ENDIAN)
  226. p.pv.i = be16_to_cpu((__u16) p.pv.i);
  227. else
  228. p.pv.i = le16_to_cpu((__u16) p.pv.i);
  229. break;
  230. case 4:
  231. n += irda_param_unpack(buf+2, "i", &p.pv.i);
  232. if (type & PV_BIG_ENDIAN)
  233. be32_to_cpus(&p.pv.i);
  234. else
  235. le32_to_cpus(&p.pv.i);
  236. break;
  237. default:
  238. IRDA_WARNING("%s: length %d not supported\n",
  239. __FUNCTION__, p.pl);
  240. /* Skip parameter */
  241. return p.pl+2;
  242. }
  243. IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__,
  244. p.pi, p.pl, p.pv.i);
  245. /* Call handler for this parameter */
  246. err = (*func)(self, &p, PV_PUT);
  247. if (err < 0)
  248. return err;
  249. return p.pl+2; /* Extracted pl+2 bytes */
  250. }
  251. /*
  252. * Function irda_extract_string (self, buf, len, type, func)
  253. */
  254. static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
  255. PV_TYPE type, PI_HANDLER func)
  256. {
  257. char str[33];
  258. irda_param_t p;
  259. int err;
  260. IRDA_DEBUG(2, "%s()\n", __FUNCTION__);
  261. p.pi = pi; /* In case handler needs to know */
  262. p.pl = buf[1]; /* Extract lenght of value */
  263. IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d\n", __FUNCTION__,
  264. p.pi, p.pl);
  265. /* Check if buffer is long enough for parsing */
  266. if (len < (2+p.pl)) {
  267. IRDA_WARNING("%s: buffer to short for parsing! "
  268. "Need %d bytes, but len is only %d\n",
  269. __FUNCTION__, p.pl, len);
  270. return -1;
  271. }
  272. /* Should be safe to copy string like this since we have already
  273. * checked that the buffer is long enough */
  274. strncpy(str, buf+2, p.pl);
  275. IRDA_DEBUG(2, "%s(), str=0x%02x 0x%02x\n", __FUNCTION__,
  276. (__u8) str[0], (__u8) str[1]);
  277. /* Null terminate string */
  278. str[p.pl+1] = '\0';
  279. p.pv.c = str; /* Handler will need to take a copy */
  280. /* Call handler for this parameter */
  281. err = (*func)(self, &p, PV_PUT);
  282. if (err < 0)
  283. return err;
  284. return p.pl+2; /* Extracted pl+2 bytes */
  285. }
  286. /*
  287. * Function irda_extract_octseq (self, buf, len, type, func)
  288. */
  289. static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
  290. PV_TYPE type, PI_HANDLER func)
  291. {
  292. irda_param_t p;
  293. p.pi = pi; /* In case handler needs to know */
  294. p.pl = buf[1]; /* Extract lenght of value */
  295. /* Check if buffer is long enough for parsing */
  296. if (len < (2+p.pl)) {
  297. IRDA_WARNING("%s: buffer to short for parsing! "
  298. "Need %d bytes, but len is only %d\n",
  299. __FUNCTION__, p.pl, len);
  300. return -1;
  301. }
  302. IRDA_DEBUG(0, "%s(), not impl\n", __FUNCTION__);
  303. return p.pl+2; /* Extracted pl+2 bytes */
  304. }
  305. /*
  306. * Function irda_param_pack (skb, fmt, ...)
  307. *
  308. * Format:
  309. * 'i' = 32 bits integer
  310. * 's' = string
  311. *
  312. */
  313. int irda_param_pack(__u8 *buf, char *fmt, ...)
  314. {
  315. irda_pv_t arg;
  316. va_list args;
  317. char *p;
  318. int n = 0;
  319. va_start(args, fmt);
  320. for (p = fmt; *p != '\0'; p++) {
  321. switch (*p) {
  322. case 'b': /* 8 bits unsigned byte */
  323. buf[n++] = (__u8)va_arg(args, int);
  324. break;
  325. case 's': /* 16 bits unsigned short */
  326. arg.i = (__u16)va_arg(args, int);
  327. put_unaligned((__u16)arg.i, (__u16 *)(buf+n)); n+=2;
  328. break;
  329. case 'i': /* 32 bits unsigned integer */
  330. arg.i = va_arg(args, __u32);
  331. put_unaligned(arg.i, (__u32 *)(buf+n)); n+=4;
  332. break;
  333. #if 0
  334. case 'c': /* \0 terminated string */
  335. arg.c = va_arg(args, char *);
  336. strcpy(buf+n, arg.c);
  337. n += strlen(arg.c) + 1;
  338. break;
  339. #endif
  340. default:
  341. va_end(args);
  342. return -1;
  343. }
  344. }
  345. va_end(args);
  346. return 0;
  347. }
  348. EXPORT_SYMBOL(irda_param_pack);
  349. /*
  350. * Function irda_param_unpack (skb, fmt, ...)
  351. */
  352. static int irda_param_unpack(__u8 *buf, char *fmt, ...)
  353. {
  354. irda_pv_t arg;
  355. va_list args;
  356. char *p;
  357. int n = 0;
  358. va_start(args, fmt);
  359. for (p = fmt; *p != '\0'; p++) {
  360. switch (*p) {
  361. case 'b': /* 8 bits byte */
  362. arg.ip = va_arg(args, __u32 *);
  363. *arg.ip = buf[n++];
  364. break;
  365. case 's': /* 16 bits short */
  366. arg.ip = va_arg(args, __u32 *);
  367. *arg.ip = get_unaligned((__u16 *)(buf+n)); n+=2;
  368. break;
  369. case 'i': /* 32 bits unsigned integer */
  370. arg.ip = va_arg(args, __u32 *);
  371. *arg.ip = get_unaligned((__u32 *)(buf+n)); n+=4;
  372. break;
  373. #if 0
  374. case 'c': /* \0 terminated string */
  375. arg.c = va_arg(args, char *);
  376. strcpy(arg.c, buf+n);
  377. n += strlen(arg.c) + 1;
  378. break;
  379. #endif
  380. default:
  381. va_end(args);
  382. return -1;
  383. }
  384. }
  385. va_end(args);
  386. return 0;
  387. }
  388. /*
  389. * Function irda_param_insert (self, pi, buf, len, info)
  390. *
  391. * Insert the specified parameter (pi) into buffer. Returns number of
  392. * bytes inserted
  393. */
  394. int irda_param_insert(void *self, __u8 pi, __u8 *buf, int len,
  395. pi_param_info_t *info)
  396. {
  397. pi_minor_info_t *pi_minor_info;
  398. __u8 pi_minor;
  399. __u8 pi_major;
  400. int type;
  401. int ret = -1;
  402. int n = 0;
  403. IRDA_ASSERT(buf != NULL, return ret;);
  404. IRDA_ASSERT(info != 0, return ret;);
  405. pi_minor = pi & info->pi_mask;
  406. pi_major = pi >> info->pi_major_offset;
  407. /* Check if the identifier value (pi) is valid */
  408. if ((pi_major > info->len-1) ||
  409. (pi_minor > info->tables[pi_major].len-1))
  410. {
  411. IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n",
  412. __FUNCTION__, pi);
  413. /* Skip this parameter */
  414. return -1;
  415. }
  416. /* Lookup the info on how to parse this parameter */
  417. pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
  418. /* Find expected data type for this parameter identifier (pi)*/
  419. type = pi_minor_info->type;
  420. /* Check if handler has been implemented */
  421. if (!pi_minor_info->func) {
  422. IRDA_MESSAGE("%s: no handler for pi=%#x\n", __FUNCTION__, pi);
  423. /* Skip this parameter */
  424. return -1;
  425. }
  426. /* Insert parameter value */
  427. ret = (*pv_insert_table[type & PV_MASK])(self, buf+n, len, pi, type,
  428. pi_minor_info->func);
  429. return ret;
  430. }
  431. EXPORT_SYMBOL(irda_param_insert);
  432. /*
  433. * Function irda_param_extract (self, buf, len, info)
  434. *
  435. * Parse all parameters. If len is correct, then everything should be
  436. * safe. Returns the number of bytes that was parsed
  437. *
  438. */
  439. static int irda_param_extract(void *self, __u8 *buf, int len,
  440. pi_param_info_t *info)
  441. {
  442. pi_minor_info_t *pi_minor_info;
  443. __u8 pi_minor;
  444. __u8 pi_major;
  445. int type;
  446. int ret = -1;
  447. int n = 0;
  448. IRDA_ASSERT(buf != NULL, return ret;);
  449. IRDA_ASSERT(info != 0, return ret;);
  450. pi_minor = buf[n] & info->pi_mask;
  451. pi_major = buf[n] >> info->pi_major_offset;
  452. /* Check if the identifier value (pi) is valid */
  453. if ((pi_major > info->len-1) ||
  454. (pi_minor > info->tables[pi_major].len-1))
  455. {
  456. IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n",
  457. __FUNCTION__, buf[0]);
  458. /* Skip this parameter */
  459. return 2 + buf[n + 1]; /* Continue */
  460. }
  461. /* Lookup the info on how to parse this parameter */
  462. pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
  463. /* Find expected data type for this parameter identifier (pi)*/
  464. type = pi_minor_info->type;
  465. IRDA_DEBUG(3, "%s(), pi=[%d,%d], type=%d\n", __FUNCTION__,
  466. pi_major, pi_minor, type);
  467. /* Check if handler has been implemented */
  468. if (!pi_minor_info->func) {
  469. IRDA_MESSAGE("%s: no handler for pi=%#x\n",
  470. __FUNCTION__, buf[n]);
  471. /* Skip this parameter */
  472. return 2 + buf[n + 1]; /* Continue */
  473. }
  474. /* Parse parameter value */
  475. ret = (*pv_extract_table[type & PV_MASK])(self, buf+n, len, buf[n],
  476. type, pi_minor_info->func);
  477. return ret;
  478. }
  479. /*
  480. * Function irda_param_extract_all (self, buf, len, info)
  481. *
  482. * Parse all parameters. If len is correct, then everything should be
  483. * safe. Returns the number of bytes that was parsed
  484. *
  485. */
  486. int irda_param_extract_all(void *self, __u8 *buf, int len,
  487. pi_param_info_t *info)
  488. {
  489. int ret = -1;
  490. int n = 0;
  491. IRDA_ASSERT(buf != NULL, return ret;);
  492. IRDA_ASSERT(info != 0, return ret;);
  493. /*
  494. * Parse all parameters. Each parameter must be at least two bytes
  495. * long or else there is no point in trying to parse it
  496. */
  497. while (len > 2) {
  498. ret = irda_param_extract(self, buf+n, len, info);
  499. if (ret < 0)
  500. return ret;
  501. n += ret;
  502. len -= ret;
  503. }
  504. return n;
  505. }
  506. EXPORT_SYMBOL(irda_param_extract_all);