lapb_subr.c 7.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * LAPB release 002
  4. *
  5. * This code REQUIRES 2.1.15 or higher/ NET3.038
  6. *
  7. * History
  8. * LAPB 001 Jonathan Naylor Started Coding
  9. */
  10. #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  11. #include <linux/errno.h>
  12. #include <linux/types.h>
  13. #include <linux/socket.h>
  14. #include <linux/in.h>
  15. #include <linux/kernel.h>
  16. #include <linux/timer.h>
  17. #include <linux/string.h>
  18. #include <linux/sockios.h>
  19. #include <linux/net.h>
  20. #include <linux/inet.h>
  21. #include <linux/skbuff.h>
  22. #include <linux/slab.h>
  23. #include <net/sock.h>
  24. #include <linux/uaccess.h>
  25. #include <linux/fcntl.h>
  26. #include <linux/mm.h>
  27. #include <linux/interrupt.h>
  28. #include <net/lapb.h>
  29. /*
  30. * This routine purges all the queues of frames.
  31. */
  32. void lapb_clear_queues(struct lapb_cb *lapb)
  33. {
  34. skb_queue_purge(&lapb->write_queue);
  35. skb_queue_purge(&lapb->ack_queue);
  36. }
  37. /*
  38. * This routine purges the input queue of those frames that have been
  39. * acknowledged. This replaces the boxes labelled "V(a) <- N(r)" on the
  40. * SDL diagram.
  41. */
  42. void lapb_frames_acked(struct lapb_cb *lapb, unsigned short nr)
  43. {
  44. struct sk_buff *skb;
  45. int modulus;
  46. modulus = (lapb->mode & LAPB_EXTENDED) ? LAPB_EMODULUS : LAPB_SMODULUS;
  47. /*
  48. * Remove all the ack-ed frames from the ack queue.
  49. */
  50. if (lapb->va != nr)
  51. while (skb_peek(&lapb->ack_queue) && lapb->va != nr) {
  52. skb = skb_dequeue(&lapb->ack_queue);
  53. kfree_skb(skb);
  54. lapb->va = (lapb->va + 1) % modulus;
  55. }
  56. }
  57. void lapb_requeue_frames(struct lapb_cb *lapb)
  58. {
  59. struct sk_buff *skb, *skb_prev = NULL;
  60. /*
  61. * Requeue all the un-ack-ed frames on the output queue to be picked
  62. * up by lapb_kick called from the timer. This arrangement handles the
  63. * possibility of an empty output queue.
  64. */
  65. while ((skb = skb_dequeue(&lapb->ack_queue)) != NULL) {
  66. if (!skb_prev)
  67. skb_queue_head(&lapb->write_queue, skb);
  68. else
  69. skb_append(skb_prev, skb, &lapb->write_queue);
  70. skb_prev = skb;
  71. }
  72. }
  73. /*
  74. * Validate that the value of nr is between va and vs. Return true or
  75. * false for testing.
  76. */
  77. int lapb_validate_nr(struct lapb_cb *lapb, unsigned short nr)
  78. {
  79. unsigned short vc = lapb->va;
  80. int modulus;
  81. modulus = (lapb->mode & LAPB_EXTENDED) ? LAPB_EMODULUS : LAPB_SMODULUS;
  82. while (vc != lapb->vs) {
  83. if (nr == vc)
  84. return 1;
  85. vc = (vc + 1) % modulus;
  86. }
  87. return nr == lapb->vs;
  88. }
  89. /*
  90. * This routine is the centralised routine for parsing the control
  91. * information for the different frame formats.
  92. */
  93. int lapb_decode(struct lapb_cb *lapb, struct sk_buff *skb,
  94. struct lapb_frame *frame)
  95. {
  96. frame->type = LAPB_ILLEGAL;
  97. lapb_dbg(2, "(%p) S%d RX %3ph\n", lapb->dev, lapb->state, skb->data);
  98. /* We always need to look at 2 bytes, sometimes we need
  99. * to look at 3 and those cases are handled below.
  100. */
  101. if (!pskb_may_pull(skb, 2))
  102. return -1;
  103. if (lapb->mode & LAPB_MLP) {
  104. if (lapb->mode & LAPB_DCE) {
  105. if (skb->data[0] == LAPB_ADDR_D)
  106. frame->cr = LAPB_COMMAND;
  107. if (skb->data[0] == LAPB_ADDR_C)
  108. frame->cr = LAPB_RESPONSE;
  109. } else {
  110. if (skb->data[0] == LAPB_ADDR_C)
  111. frame->cr = LAPB_COMMAND;
  112. if (skb->data[0] == LAPB_ADDR_D)
  113. frame->cr = LAPB_RESPONSE;
  114. }
  115. } else {
  116. if (lapb->mode & LAPB_DCE) {
  117. if (skb->data[0] == LAPB_ADDR_B)
  118. frame->cr = LAPB_COMMAND;
  119. if (skb->data[0] == LAPB_ADDR_A)
  120. frame->cr = LAPB_RESPONSE;
  121. } else {
  122. if (skb->data[0] == LAPB_ADDR_A)
  123. frame->cr = LAPB_COMMAND;
  124. if (skb->data[0] == LAPB_ADDR_B)
  125. frame->cr = LAPB_RESPONSE;
  126. }
  127. }
  128. skb_pull(skb, 1);
  129. if (lapb->mode & LAPB_EXTENDED) {
  130. if (!(skb->data[0] & LAPB_S)) {
  131. if (!pskb_may_pull(skb, 2))
  132. return -1;
  133. /*
  134. * I frame - carries NR/NS/PF
  135. */
  136. frame->type = LAPB_I;
  137. frame->ns = (skb->data[0] >> 1) & 0x7F;
  138. frame->nr = (skb->data[1] >> 1) & 0x7F;
  139. frame->pf = skb->data[1] & LAPB_EPF;
  140. frame->control[0] = skb->data[0];
  141. frame->control[1] = skb->data[1];
  142. skb_pull(skb, 2);
  143. } else if ((skb->data[0] & LAPB_U) == 1) {
  144. if (!pskb_may_pull(skb, 2))
  145. return -1;
  146. /*
  147. * S frame - take out PF/NR
  148. */
  149. frame->type = skb->data[0] & 0x0F;
  150. frame->nr = (skb->data[1] >> 1) & 0x7F;
  151. frame->pf = skb->data[1] & LAPB_EPF;
  152. frame->control[0] = skb->data[0];
  153. frame->control[1] = skb->data[1];
  154. skb_pull(skb, 2);
  155. } else if ((skb->data[0] & LAPB_U) == 3) {
  156. /*
  157. * U frame - take out PF
  158. */
  159. frame->type = skb->data[0] & ~LAPB_SPF;
  160. frame->pf = skb->data[0] & LAPB_SPF;
  161. frame->control[0] = skb->data[0];
  162. frame->control[1] = 0x00;
  163. skb_pull(skb, 1);
  164. }
  165. } else {
  166. if (!(skb->data[0] & LAPB_S)) {
  167. /*
  168. * I frame - carries NR/NS/PF
  169. */
  170. frame->type = LAPB_I;
  171. frame->ns = (skb->data[0] >> 1) & 0x07;
  172. frame->nr = (skb->data[0] >> 5) & 0x07;
  173. frame->pf = skb->data[0] & LAPB_SPF;
  174. } else if ((skb->data[0] & LAPB_U) == 1) {
  175. /*
  176. * S frame - take out PF/NR
  177. */
  178. frame->type = skb->data[0] & 0x0F;
  179. frame->nr = (skb->data[0] >> 5) & 0x07;
  180. frame->pf = skb->data[0] & LAPB_SPF;
  181. } else if ((skb->data[0] & LAPB_U) == 3) {
  182. /*
  183. * U frame - take out PF
  184. */
  185. frame->type = skb->data[0] & ~LAPB_SPF;
  186. frame->pf = skb->data[0] & LAPB_SPF;
  187. }
  188. frame->control[0] = skb->data[0];
  189. skb_pull(skb, 1);
  190. }
  191. return 0;
  192. }
  193. /*
  194. * This routine is called when the HDLC layer internally generates a
  195. * command or response for the remote machine ( eg. RR, UA etc. ).
  196. * Only supervisory or unnumbered frames are processed, FRMRs are handled
  197. * by lapb_transmit_frmr below.
  198. */
  199. void lapb_send_control(struct lapb_cb *lapb, int frametype,
  200. int poll_bit, int type)
  201. {
  202. struct sk_buff *skb;
  203. unsigned char *dptr;
  204. if ((skb = alloc_skb(LAPB_HEADER_LEN + 3, GFP_ATOMIC)) == NULL)
  205. return;
  206. skb_reserve(skb, LAPB_HEADER_LEN + 1);
  207. if (lapb->mode & LAPB_EXTENDED) {
  208. if ((frametype & LAPB_U) == LAPB_U) {
  209. dptr = skb_put(skb, 1);
  210. *dptr = frametype;
  211. *dptr |= poll_bit ? LAPB_SPF : 0;
  212. } else {
  213. dptr = skb_put(skb, 2);
  214. dptr[0] = frametype;
  215. dptr[1] = (lapb->vr << 1);
  216. dptr[1] |= poll_bit ? LAPB_EPF : 0;
  217. }
  218. } else {
  219. dptr = skb_put(skb, 1);
  220. *dptr = frametype;
  221. *dptr |= poll_bit ? LAPB_SPF : 0;
  222. if ((frametype & LAPB_U) == LAPB_S) /* S frames carry NR */
  223. *dptr |= (lapb->vr << 5);
  224. }
  225. lapb_transmit_buffer(lapb, skb, type);
  226. }
  227. /*
  228. * This routine generates FRMRs based on information previously stored in
  229. * the LAPB control block.
  230. */
  231. void lapb_transmit_frmr(struct lapb_cb *lapb)
  232. {
  233. struct sk_buff *skb;
  234. unsigned char *dptr;
  235. if ((skb = alloc_skb(LAPB_HEADER_LEN + 7, GFP_ATOMIC)) == NULL)
  236. return;
  237. skb_reserve(skb, LAPB_HEADER_LEN + 1);
  238. if (lapb->mode & LAPB_EXTENDED) {
  239. dptr = skb_put(skb, 6);
  240. *dptr++ = LAPB_FRMR;
  241. *dptr++ = lapb->frmr_data.control[0];
  242. *dptr++ = lapb->frmr_data.control[1];
  243. *dptr++ = (lapb->vs << 1) & 0xFE;
  244. *dptr = (lapb->vr << 1) & 0xFE;
  245. if (lapb->frmr_data.cr == LAPB_RESPONSE)
  246. *dptr |= 0x01;
  247. dptr++;
  248. *dptr++ = lapb->frmr_type;
  249. lapb_dbg(1, "(%p) S%d TX FRMR %5ph\n",
  250. lapb->dev, lapb->state,
  251. &skb->data[1]);
  252. } else {
  253. dptr = skb_put(skb, 4);
  254. *dptr++ = LAPB_FRMR;
  255. *dptr++ = lapb->frmr_data.control[0];
  256. *dptr = (lapb->vs << 1) & 0x0E;
  257. *dptr |= (lapb->vr << 5) & 0xE0;
  258. if (lapb->frmr_data.cr == LAPB_RESPONSE)
  259. *dptr |= 0x10;
  260. dptr++;
  261. *dptr++ = lapb->frmr_type;
  262. lapb_dbg(1, "(%p) S%d TX FRMR %3ph\n",
  263. lapb->dev, lapb->state, &skb->data[1]);
  264. }
  265. lapb_transmit_buffer(lapb, skb, LAPB_RESPONSE);
  266. }