lapb_subr.c 7.54 KB
Newer Older
Linus Torvalds's avatar
Linus Torvalds committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
/*
 *	LAPB release 002
 *
 *	This code REQUIRES 2.1.15 or higher/ NET3.038
 *
 *	This module:
 *		This module is free software; you can redistribute it and/or
 *		modify it under the terms of the GNU General Public License
 *		as published by the Free Software Foundation; either version
 *		2 of the License, or (at your option) any later version.
 *
 *	History
 *	LAPB 001	Jonathan Naylor	Started Coding
 */

Joe Perches's avatar
Joe Perches committed
16 17
#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt

Linus Torvalds's avatar
Linus Torvalds committed
18 19 20 21 22 23 24 25 26 27 28
#include <linux/errno.h>
#include <linux/types.h>
#include <linux/socket.h>
#include <linux/in.h>
#include <linux/kernel.h>
#include <linux/timer.h>
#include <linux/string.h>
#include <linux/sockios.h>
#include <linux/net.h>
#include <linux/inet.h>
#include <linux/skbuff.h>
29
#include <linux/slab.h>
Linus Torvalds's avatar
Linus Torvalds committed
30
#include <net/sock.h>
31
#include <linux/uaccess.h>
Linus Torvalds's avatar
Linus Torvalds committed
32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62
#include <linux/fcntl.h>
#include <linux/mm.h>
#include <linux/interrupt.h>
#include <net/lapb.h>

/*
 *	This routine purges all the queues of frames.
 */
void lapb_clear_queues(struct lapb_cb *lapb)
{
	skb_queue_purge(&lapb->write_queue);
	skb_queue_purge(&lapb->ack_queue);
}

/*
 * This routine purges the input queue of those frames that have been
 * acknowledged. This replaces the boxes labelled "V(a) <- N(r)" on the
 * SDL diagram.
 */
void lapb_frames_acked(struct lapb_cb *lapb, unsigned short nr)
{
	struct sk_buff *skb;
	int modulus;

	modulus = (lapb->mode & LAPB_EXTENDED) ? LAPB_EMODULUS : LAPB_SMODULUS;

	/*
	 * Remove all the ack-ed frames from the ack queue.
	 */
	if (lapb->va != nr)
		while (skb_peek(&lapb->ack_queue) && lapb->va != nr) {
63
			skb = skb_dequeue(&lapb->ack_queue);
Linus Torvalds's avatar
Linus Torvalds committed
64 65 66 67 68 69 70
			kfree_skb(skb);
			lapb->va = (lapb->va + 1) % modulus;
		}
}

void lapb_requeue_frames(struct lapb_cb *lapb)
{
71
	struct sk_buff *skb, *skb_prev = NULL;
Linus Torvalds's avatar
Linus Torvalds committed
72 73 74 75 76 77 78 79 80 81

	/*
	 * Requeue all the un-ack-ed frames on the output queue to be picked
	 * up by lapb_kick called from the timer. This arrangement handles the
	 * possibility of an empty output queue.
	 */
	while ((skb = skb_dequeue(&lapb->ack_queue)) != NULL) {
		if (!skb_prev)
			skb_queue_head(&lapb->write_queue, skb);
		else
David S. Miller's avatar
David S. Miller committed
82
			skb_append(skb_prev, skb, &lapb->write_queue);
Linus Torvalds's avatar
Linus Torvalds committed
83 84 85 86 87 88 89 90 91 92 93 94
		skb_prev = skb;
	}
}

/*
 *	Validate that the value of nr is between va and vs. Return true or
 *	false for testing.
 */
int lapb_validate_nr(struct lapb_cb *lapb, unsigned short nr)
{
	unsigned short vc = lapb->va;
	int modulus;
95

Linus Torvalds's avatar
Linus Torvalds committed
96 97 98 99 100 101 102
	modulus = (lapb->mode & LAPB_EXTENDED) ? LAPB_EMODULUS : LAPB_SMODULUS;

	while (vc != lapb->vs) {
		if (nr == vc)
			return 1;
		vc = (vc + 1) % modulus;
	}
103

Linus Torvalds's avatar
Linus Torvalds committed
104 105 106 107 108 109 110 111 112 113 114 115
	return nr == lapb->vs;
}

/*
 *	This routine is the centralised routine for parsing the control
 *	information for the different frame formats.
 */
int lapb_decode(struct lapb_cb *lapb, struct sk_buff *skb,
		struct lapb_frame *frame)
{
	frame->type = LAPB_ILLEGAL;

116
	lapb_dbg(2, "(%p) S%d RX %3ph\n", lapb->dev, lapb->state, skb->data);
Linus Torvalds's avatar
Linus Torvalds committed
117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148

	/* We always need to look at 2 bytes, sometimes we need
	 * to look at 3 and those cases are handled below.
	 */
	if (!pskb_may_pull(skb, 2))
		return -1;

	if (lapb->mode & LAPB_MLP) {
		if (lapb->mode & LAPB_DCE) {
			if (skb->data[0] == LAPB_ADDR_D)
				frame->cr = LAPB_COMMAND;
			if (skb->data[0] == LAPB_ADDR_C)
				frame->cr = LAPB_RESPONSE;
		} else {
			if (skb->data[0] == LAPB_ADDR_C)
				frame->cr = LAPB_COMMAND;
			if (skb->data[0] == LAPB_ADDR_D)
				frame->cr = LAPB_RESPONSE;
		}
	} else {
		if (lapb->mode & LAPB_DCE) {
			if (skb->data[0] == LAPB_ADDR_B)
				frame->cr = LAPB_COMMAND;
			if (skb->data[0] == LAPB_ADDR_A)
				frame->cr = LAPB_RESPONSE;
		} else {
			if (skb->data[0] == LAPB_ADDR_A)
				frame->cr = LAPB_COMMAND;
			if (skb->data[0] == LAPB_ADDR_B)
				frame->cr = LAPB_RESPONSE;
		}
	}
149

Linus Torvalds's avatar
Linus Torvalds committed
150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219
	skb_pull(skb, 1);

	if (lapb->mode & LAPB_EXTENDED) {
		if (!(skb->data[0] & LAPB_S)) {
			if (!pskb_may_pull(skb, 2))
				return -1;
			/*
			 * I frame - carries NR/NS/PF
			 */
			frame->type       = LAPB_I;
			frame->ns         = (skb->data[0] >> 1) & 0x7F;
			frame->nr         = (skb->data[1] >> 1) & 0x7F;
			frame->pf         = skb->data[1] & LAPB_EPF;
			frame->control[0] = skb->data[0];
			frame->control[1] = skb->data[1];
			skb_pull(skb, 2);
		} else if ((skb->data[0] & LAPB_U) == 1) {
			if (!pskb_may_pull(skb, 2))
				return -1;
			/*
			 * S frame - take out PF/NR
			 */
			frame->type       = skb->data[0] & 0x0F;
			frame->nr         = (skb->data[1] >> 1) & 0x7F;
			frame->pf         = skb->data[1] & LAPB_EPF;
			frame->control[0] = skb->data[0];
			frame->control[1] = skb->data[1];
			skb_pull(skb, 2);
		} else if ((skb->data[0] & LAPB_U) == 3) {
			/*
			 * U frame - take out PF
			 */
			frame->type       = skb->data[0] & ~LAPB_SPF;
			frame->pf         = skb->data[0] & LAPB_SPF;
			frame->control[0] = skb->data[0];
			frame->control[1] = 0x00;
			skb_pull(skb, 1);
		}
	} else {
		if (!(skb->data[0] & LAPB_S)) {
			/*
			 * I frame - carries NR/NS/PF
			 */
			frame->type = LAPB_I;
			frame->ns   = (skb->data[0] >> 1) & 0x07;
			frame->nr   = (skb->data[0] >> 5) & 0x07;
			frame->pf   = skb->data[0] & LAPB_SPF;
		} else if ((skb->data[0] & LAPB_U) == 1) {
			/*
			 * S frame - take out PF/NR
			 */
			frame->type = skb->data[0] & 0x0F;
			frame->nr   = (skb->data[0] >> 5) & 0x07;
			frame->pf   = skb->data[0] & LAPB_SPF;
		} else if ((skb->data[0] & LAPB_U) == 3) {
			/*
			 * U frame - take out PF
			 */
			frame->type = skb->data[0] & ~LAPB_SPF;
			frame->pf   = skb->data[0] & LAPB_SPF;
		}

		frame->control[0] = skb->data[0];

		skb_pull(skb, 1);
	}

	return 0;
}

220
/*
Linus Torvalds's avatar
Linus Torvalds committed
221
 *	This routine is called when the HDLC layer internally  generates a
222
 *	command or  response  for  the remote machine ( eg. RR, UA etc. ).
Linus Torvalds's avatar
Linus Torvalds committed
223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258
 *	Only supervisory or unnumbered frames are processed, FRMRs are handled
 *	by lapb_transmit_frmr below.
 */
void lapb_send_control(struct lapb_cb *lapb, int frametype,
		       int poll_bit, int type)
{
	struct sk_buff *skb;
	unsigned char  *dptr;

	if ((skb = alloc_skb(LAPB_HEADER_LEN + 3, GFP_ATOMIC)) == NULL)
		return;

	skb_reserve(skb, LAPB_HEADER_LEN + 1);

	if (lapb->mode & LAPB_EXTENDED) {
		if ((frametype & LAPB_U) == LAPB_U) {
			dptr   = skb_put(skb, 1);
			*dptr  = frametype;
			*dptr |= poll_bit ? LAPB_SPF : 0;
		} else {
			dptr     = skb_put(skb, 2);
			dptr[0]  = frametype;
			dptr[1]  = (lapb->vr << 1);
			dptr[1] |= poll_bit ? LAPB_EPF : 0;
		}
	} else {
		dptr   = skb_put(skb, 1);
		*dptr  = frametype;
		*dptr |= poll_bit ? LAPB_SPF : 0;
		if ((frametype & LAPB_U) == LAPB_S)	/* S frames carry NR */
			*dptr |= (lapb->vr << 5);
	}

	lapb_transmit_buffer(lapb, skb, type);
}

259
/*
Linus Torvalds's avatar
Linus Torvalds committed
260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284
 *	This routine generates FRMRs based on information previously stored in
 *	the LAPB control block.
 */
void lapb_transmit_frmr(struct lapb_cb *lapb)
{
	struct sk_buff *skb;
	unsigned char  *dptr;

	if ((skb = alloc_skb(LAPB_HEADER_LEN + 7, GFP_ATOMIC)) == NULL)
		return;

	skb_reserve(skb, LAPB_HEADER_LEN + 1);

	if (lapb->mode & LAPB_EXTENDED) {
		dptr    = skb_put(skb, 6);
		*dptr++ = LAPB_FRMR;
		*dptr++ = lapb->frmr_data.control[0];
		*dptr++ = lapb->frmr_data.control[1];
		*dptr++ = (lapb->vs << 1) & 0xFE;
		*dptr   = (lapb->vr << 1) & 0xFE;
		if (lapb->frmr_data.cr == LAPB_RESPONSE)
			*dptr |= 0x01;
		dptr++;
		*dptr++ = lapb->frmr_type;

285
		lapb_dbg(1, "(%p) S%d TX FRMR %5ph\n",
Joe Perches's avatar
Joe Perches committed
286
			 lapb->dev, lapb->state,
287
			 &skb->data[1]);
Linus Torvalds's avatar
Linus Torvalds committed
288 289 290 291 292 293 294 295 296 297 298
	} else {
		dptr    = skb_put(skb, 4);
		*dptr++ = LAPB_FRMR;
		*dptr++ = lapb->frmr_data.control[0];
		*dptr   = (lapb->vs << 1) & 0x0E;
		*dptr  |= (lapb->vr << 5) & 0xE0;
		if (lapb->frmr_data.cr == LAPB_RESPONSE)
			*dptr |= 0x10;
		dptr++;
		*dptr++ = lapb->frmr_type;

299 300
		lapb_dbg(1, "(%p) S%d TX FRMR %3ph\n",
			 lapb->dev, lapb->state, &skb->data[1]);
Linus Torvalds's avatar
Linus Torvalds committed
301 302 303 304
	}

	lapb_transmit_buffer(lapb, skb, LAPB_RESPONSE);
}